JP2010223684A - Positioning device for moving objects - Google Patents

Abstract

To provide a positioning device for a moving body capable of outputting a more accurate velocity vector of the moving body.
A position calculating means for calculating the position of a moving body using satellite radio waves, a speed calculating means for calculating a speed vector of the moving body by satellite radio waves or inertial navigation, and a road are expressed by a plurality of links. Storage means for storing map data; projection means for outputting a projection velocity vector obtained by projecting the velocity vector calculated by the velocity vector calculation means onto a link that is estimated to be a moving body among the links; and the projection Projection availability determination means for determining whether or not the projection speed vector output by the means can be adopted, and when the projection speed vector is judged to be adopted by the projection availability judgment means, the projection speed vector is A moving body positioning apparatus, characterized in that it outputs as a velocity vector of the moving body.
[Selection] Figure 2

Description

  The present invention relates to a positioning device for a moving body that calculates the position and speed of the moving body using satellite radio waves.

  Conventionally, in GNSS (Global Navigation Satellite System; including GPS (Global Positioning System), Galileo, Glonass, etc.), an invention for an apparatus for calculating a velocity vector of a moving body based on a change amount of a carrier frequency has been disclosed. (For example, refer to Patent Document 1). The velocity vector of the moving body can specify the azimuth (traveling direction) of the moving body according to its direction, and the position change of the moving body can also be calculated using the integrated value. Further, the velocity vector can be used in combination with single positioning or INS (Inertial Navigation System) when specifying the position of the moving body.

JP 2009-25046 A

  However, the speed vector calculated in the GNSS has a problem that accuracy and variation vary depending on the environment. As a result, inconveniences such as an increase in accumulation error when combined with inertial navigation or the like may occur. FIG. 1 is a diagram showing how such inconvenience occurs.

  The accuracy of the velocity vector calculated in the GNSS is mainly determined by (A) a positioning error based on radio waves transmitted from each GNSS satellite, and (B) the number and positional relationship of the captured GNSS satellites. In particular, in an urban area, the positioning error (A) occurs due to the influence of multipath (incorrect estimation of the distance between a satellite and a moving object due to reflection of radio waves by a building or the like). Also, the number of captured GNSS satellites in (B) is reduced by shielding between the satellites by buildings and suspensions.

  If the accuracy of the velocity vector estimated in GNSS decreases, the accuracy of estimation of the posture of the moving body decreases. When the number of captured GNSS satellites is small in an urban area or the like, the position is estimated by INS, but the error of the posture of the moving body continues to accumulate.

  The present invention has been made to solve such problems, and it is a main object of the present invention to provide a positioning device for a moving body that can output a more accurate velocity vector of the moving body.

In order to achieve the above object, one embodiment of the present invention provides:
Position calculating means for calculating the position of the moving body using satellite radio waves;
Speed calculating means for calculating a speed vector of the moving body by satellite radio waves or inertial navigation;
Storage means for storing map data in which a road is represented by a plurality of links;
Projection means for outputting a projection speed vector obtained by projecting the speed vector calculated by the speed vector calculation means onto a link estimated to have a moving body among the plurality of links;
Projection propriety determining means for determining whether or not to adopt a projection velocity vector output by the projecting means,
When it is determined by the projection availability determination means that the projection speed vector should be adopted, the projection speed vector is output as a speed vector of the moving body,
This is a mobile positioning device.

  According to this aspect of the present invention, a more accurate velocity vector of a moving body can be output.

In one embodiment of the present invention,
A velocity vector integrating means for calculating the position change of the moving body by integrating the velocity vectors of the moving body output based on the determination result of the projection propriety determining means;
Which is more reliable, the position of the moving body calculated by the position calculating means and the position of the moving body obtained by taking into account the position change of the moving body calculated by the speed vector integrating means? Reliability determination means for determining
The position of the moving body that is determined to be highly reliable may be output by the reliability determining means.

  In this way, the more reliable one of the position of the moving body calculated by independent positioning and the position of the moving body calculated by integrating the velocity vectors is output, so the influence of multipath etc. is suppressed. can do.

In one embodiment of the present invention,
Inertial navigation positioning calculation means for calculating the position and velocity of the moving body by inertial navigation;
A Kalman filter for inputting a difference between the position and speed of the moving body calculated by the inertial navigation positioning calculation means and the position and speed of the moving body calculated by satellite radio waves,
The inertial navigation positioning calculation means may perform correction according to the output of the Kalman filter.

  In this way, more accurate inertial navigation positioning can be performed.

  ADVANTAGE OF THE INVENTION According to this invention, the positioning apparatus for moving bodies which can output the more accurate velocity vector of a moving body can be provided.

It is a figure which shows a mode that the inconvenience that the accumulation | storage error at the time of combining with inertial navigation etc. arises that the precision and dispersion | variation of the velocity vector calculated in GNSS change. 1 is a system configuration example of a mobile positioning apparatus 1 according to a first embodiment of the present invention. It is a figure which shows a mode that the speed vector V of a vehicle is projected on the link W, and the projection speed vector V # is obtained. It is a figure which shows a mode that an azimuth | direction error can be reduced by using the projection velocity vector V #. It is a flowchart which shows the flow of the characteristic process performed with the positioning apparatus 1 for mobile bodies which concerns on 1st Example of this invention. It is an example of a system configuration | structure of the positioning apparatus 2 for moving bodies which concerns on 2nd Example of this invention. It is a flowchart which shows the flow of the characteristic process performed by the positioning apparatus 2 for mobile bodies which concerns on 2nd Example of this invention. It is an example of a system configuration | structure of the positioning apparatus 3 for moving bodies which concerns on 3rd Example of this invention.

  DESCRIPTION OF EMBODIMENTS Hereinafter, embodiments for carrying out the present invention will be described with reference to the accompanying drawings.

<First embodiment>
Hereinafter, a mobile positioning apparatus 1 according to a first embodiment of the present invention will be described with reference to the drawings. The mobile positioning device 1 is a device applied to GNSS. GNSS is a positioning system in which a positioning device mounted on a moving body uses a signal from a satellite to measure the position of the moving body. A positioning system using satellites such as GPS (Global Positioning System), Galileo, Glonass, etc. Including. In the following description, GPS will be described as a basic configuration, but the present invention is not limited to GPS and can be widely applied to any GNSS. The moving body may be a vehicle, a motorcycle, a railway, a ship, an aircraft, a hawk lift, a robot, an information terminal such as a mobile phone that moves as a person moves.

  In the following description, an example of GNSS will be described as applied to GPS. Further, it is assumed that the moving body is a vehicle.

  GPS satellites always broadcast navigation messages (satellite signals) toward the earth. The navigation message includes satellite orbit information (ephemeris and almanac) regarding the corresponding GPS satellite, clock correction value, and ionospheric correction coefficient. The navigation message is spread by the C / A code, is carried on the L1 wave (frequency: 1575.42 MHz), and is constantly broadcast toward the earth. The L1 wave is a combined wave of a Sin wave modulated with a C / A code and a Cos wave modulated with a P code (Precision Code), and is orthogonally modulated. The C / A code and the P code are pseudo noise codes, and are code strings in which -1 and 1 are irregularly arranged periodically.

  FIG. 2 is a system configuration example of the mobile positioning apparatus 1 according to the first embodiment of the present invention. The mobile positioning device 1 is a functional block realized by executing a program by a GPS antenna 10 and a microcomputer (not shown) as main components (however, it may be a circuit configuration). The signal reception unit 20, the position calculation unit 22, the velocity calculation unit 24, the projection propriety determination unit 26, the projection unit 28, the map matching unit 30, and a map database stored in a storage device (not shown) (denoted as map DB in the figure) 32).

  The positioning signal receiving unit 20 performs C / A code synchronization on the signal received by the GPS antenna 10 using the internally generated replica C / A code, and extracts a navigation message. There are a wide variety of C / A code synchronization methods, and any appropriate method may be employed. For example, it may be a method of tracking the code phase at which the correlation value of the replica C / A code with respect to the received C / A code peaks using a DLL (Delay-Locked Loop).

  The positioning signal receiving unit 20 calculates a pseudo distance ρ between the GPS satellite and the vehicle (more precisely, the mobile positioning device 1). The pseudorange ρ includes a clock error (clock bias) and an error due to a change in radio wave propagation speed. The pseudo distance ρ is calculated by the following equation (1), for example. In the equation, N corresponds to the number of bits of the C / A code between the GPS satellite and the vehicle, and is calculated based on the phase of the replica C / A code and the receiver clock inside the mobile unit positioning device 1. . The numerical value 300 is derived from the fact that the C / A code has a 1-bit length of 1 μs and a length corresponding to 1 bit of about 300 m (1 μs × light speed).

  ρ = N × 300 (1)

  In addition, the positioning signal receiving unit 20 has a function of measuring the carrier wave phase of the satellite signal, and measures the Doppler frequency change amount Δf of the Doppler shifted received carrier wave using an internally generated replica carrier. The Doppler frequency change amount Δf is measured as a difference (= fr−fc) between the replica carrier frequency fr and the known carrier frequency fc (1575.42 MHz). Such a function may be realized by a phase-locked loop (PLL) that calculates a carrier correlation value using a replica carrier and tracks a received carrier.

  The position calculation unit 22 first calculates the position of the GPS satellite (hereinafter referred to as “satellite position”). Specifically, based on the satellite orbit information of the navigation message and the current time, the current position (Xi, Yi, Zi) of the GPS satellite i in the world coordinate system is calculated. (Indicating that it is the i-th satellite) GPS satellite motion is limited to a certain plane (orbital plane) including the center of gravity of the earth, and the orbit is an elliptical motion with the center of gravity of the earth as one focal point. The position of the GPS satellite in the plane can be calculated. The position (Xi, Yi, Zi) of the GPS satellite i in the world coordinate system is determined by considering that the orbital plane of the GPS satellite i and the equatorial plane of the world coordinate system are in a rotational relationship. Can be obtained by three-dimensionally rotating coordinate transformation.

  Then, the position of the vehicle (Xu, Yu, Zu) is measured based on the positions of the plurality of GPS satellites and the corresponding observation pseudorange ρ. The position of the vehicle is derived by the principle of triangulation based on the position obtained for the three GPS satellites and the observation pseudorange ρ. Since the observation pseudorange ρ includes a clock error as described above, the clock error component is removed using the observation pseudorange ρ and the satellite position obtained for the fourth GPS satellite.

  Note that the calculation function of the position calculation unit 22 is not limited to such single positioning, but may be interference positioning (a method in which received data at a fixed station installed at a known point is used in combination). In the case of interference positioning, the position of the vehicle is measured using the single phase difference, double phase difference, etc. of the observation pseudo distance ρ obtained by the fixed station and the vehicle as described above.

  The speed calculation unit 24 calculates the vehicle speed vector V based on the Doppler frequency change amount Δf and the speed vector Vi of the GPS satellite. Specifically, first, based on the Doppler frequency change amount Δf, a relative velocity vector (V−V1) between the GPS satellite i and the vehicle is calculated using, for example, the following equation (2).

Δf = fc × {(V−V1) · (Xu−Xi, Yu−Yi, Zu−Zi) / √ {(Xu−Xi) ² + (Yu−Yi) ² + (Zu−Zi) ² } 2)

  Then, a difference vector between the GPS satellite i and the vehicle relative velocity vector (V-V1) and the GPS satellite i velocity vector Vi is calculated as the vehicle velocity vector V. The velocity vector Vi of the GPS satellite i can be obtained by subtracting the previous satellite position from the current satellite position and dividing the result by the positioning calculation cycle.

  The projection availability determination unit 26 determines whether or not the projection velocity vector calculated by the projection unit 28 can be adopted.

  First, the projection unit 28 will be described. Projection unit 28 projects vehicle speed vector V onto a link determined to be traveling along the vehicle among a plurality of links included in map database 32, and calculates projection speed vector V #. Information (particularly, the direction of the link) regarding the link determined to be traveling along the vehicle is provided from the map matching unit 30. The map matching unit 30 determines which position on the map database 32 from the map database 32 in which roads are represented by nodes and links and information from various sensors (GPS, INS sensors, wheel speed sensors, steering angle sensors, etc.). Identifying what you are driving.

  FIG. 3 is a diagram showing a state in which the vehicle velocity vector V is projected onto the link W to obtain a projection velocity vector V #. By using such a projection velocity vector V #, it is possible to reduce the azimuth error. FIG. 4 is a diagram showing such a state.

The projection availability determination unit 26 determines that the vehicle is traveling along a specific link included in the map database 32 when, for example, all or a part of the following conditions are satisfied, and accordingly, the projection speed vector V # Is determined to be adopted. The link curvature may be included in the map database 32 or may be estimated from several nodes before and after.
(1) The map matching unit 30 matches a specific link.
(2) The vehicle speed is equal to or higher than a predetermined value.
(3) The link length is greater than or equal to a certain value.
(4) The distance from the end point of the link is greater than a certain value.
(5) The curvature corresponding to the link is below a certain value.

  On the other hand, when it is not determined that the projection velocity vector V # should be adopted, the velocity vector V output from the velocity calculation unit 24 may be used, or the velocity vector grasped by the map matching unit 30 is used. Also good.

  FIG. 5 is a flowchart showing a flow of characteristic processing executed by the mobile positioning apparatus 1 according to the first embodiment of the present invention.

  First, the position calculation unit 22 calculates the vehicle position (Xu, Yu, Zu), and the speed calculation unit 24 calculates the vehicle speed vector V (S100).

  Next, the projection availability determination unit 26 determines whether or not the projection velocity vector V # can be adopted as described above (S102).

  When it is determined that the projection velocity vector V # should be adopted, the projection unit 28 performs projection, and the vehicle position (Xu, Yu, Zu) calculated by the position calculation unit 22 and the projection unit 28 output the result. The projection speed vector V # is finally output (S104).

  On the other hand, if it is determined that the projection speed vector V # should not be adopted, the vehicle position (Xu, Yu, Zu) calculated by the position calculation unit 22 and the vehicle speed vector calculated by the speed calculation unit 24 are used. V is finally output (S106).

  According to the mobile positioning apparatus 1 of the present embodiment described above, when it is determined that the vehicle is traveling along the link, the projection speed vector V # output by the projection unit 28 is finally output. Therefore, a more accurate vehicle speed vector can be output.

<Second embodiment>
Hereinafter, a mobile positioning device 2 according to a second embodiment of the present invention will be described with reference to the drawings.

  FIG. 6 is a system configuration example of the mobile positioning apparatus 2 according to the second embodiment of the present invention. The mobile positioning device 2 includes a speed vector integrating unit 40 and a reliability determining unit 42 in addition to the configuration (only the reference numerals are shown in the figure) of the mobile positioning device 1 according to the first embodiment. Have.

  For example, the velocity vector integration unit 40 multiplies the velocity vector V or the projection velocity vector V # finally output by the mobile positioning device 1 of the first embodiment by the sample interval, and the position of the vehicle at the sampling time. Changes (ΔX, ΔY, ΔZ) are calculated. And while performing such a process repeatedly, the position (Xu (n-1), Yu (n-1), Yu (n-1), vehicle position finally output by the mobile positioning device 1 of the first embodiment last time. Zu (n-1)) is added to the position change of the vehicle to calculate the current vehicle position (Xu *, Yu *, Zu *) (see the following equation (3)).

  (Xu *, Yu *, Zu *) = (Xu (n−1) + ΔX, Yu (n−1) + ΔY, Zu (n−1) + ΔZ) (3)

  Here, instead of using the velocity vector V or the projected velocity vector V # as it is, a velocity average obtained by assigning an appropriate weight to the velocity vectors in a plurality of past samples may be used.

  The reliability determination unit 42 is a vehicle position obtained by taking into account the vehicle position (Xu, Yu, Zu) calculated by the position calculation unit 32 and the vehicle position change calculated by the speed vector integration unit 40. Whether (Xu *, Yu *, Zu *) is more reliable is determined.

Here, the position and velocity vector calculated by GNSS have the following characteristics. (A) The position is more susceptible to multipath as the vehicle speed is lower. (B) The velocity vector collision is less susceptible to multipath than the position. Therefore, the reliability determination unit 42, for example, satisfies the vehicle position (Xu) obtained by taking into account the change in the position of the moving object calculated by the speed vector integration unit 40 when all or part of the following conditions are satisfied. *, Yu *, Zu *) are determined to be highly reliable.
(1) The vehicle speed is below a certain value.
(2) The number of satellites used to calculate the velocity vector is greater than or equal to a certain value.
(3) The residual sum of squares in the calculation of the velocity vector is less than a certain value.
(4) The DOP (Dilution of Precision) of the satellite group used for calculating the velocity vector is below a certain value.

  FIG. 7 is a flowchart showing a flow of characteristic processing executed by the mobile positioning apparatus 2 according to the second embodiment of the present invention. Since S100 to S106 are the same as in the first embodiment, detailed description thereof is omitted.

  When the vehicle position (Xu, Yu, Zu) calculated by the position calculation unit 22 and the projection velocity vector V # or the velocity vector V are output (S100 to 106), the vehicle calculated by the position calculation unit 32 is output. And the position of the vehicle (Xu *, Yu *, Zu *) obtained by taking into account the change in the position of the vehicle calculated by the speed vector integrating unit 40. It is determined whether the property is high (S108).

  When it is determined that the reliability of the vehicle position (Xu, Yu, Zu) calculated by the position calculation unit 32 is high, the position (Xu, Yu, Zu) of the vehicle calculated by the position calculation unit 32 is (S110).

  On the other hand, if it is determined that the vehicle position (Xu *, Yu *, Zu *) obtained by taking into account the position change of the moving body calculated by the speed vector integration unit 40 is highly reliable, the speed vector integration is performed. The vehicle position (Xu *, Yu *, Zu *) obtained by taking into account the vehicle position change calculated by the unit 40 is output as the vehicle position (S112).

  In S110 or S112, the vehicle speed vector is also output.

  According to the mobile positioning device 2 of the present embodiment described above, the more reliable one of the position of the vehicle calculated by single positioning or the like and the position of the vehicle calculated by accumulating the speed vectors are selected. Since it outputs, the influence of multipath etc. can be suppressed.

<Third embodiment>
Hereinafter, a mobile positioning device 3 according to a third embodiment of the present invention will be described with reference to the drawings.

  FIG. 8 is a system configuration example of the mobile positioning device 3 according to the third embodiment of the present invention. The mobile positioning device 3 includes an inertial sensor 50, an INS positioning calculation unit 52, a Kalman filter 54, in addition to the configuration (only the reference numerals are shown in the figure) of the mobile positioning device 1 according to the first embodiment. And a wheel speed sensor 56.

  The inertial sensor 50 is, for example, an acceleration sensor (G sensor) or a yaw rate sensor.

  The INS positioning calculation unit 52 calculates the position (Xui, Yui, Zui) of the vehicle by inertial navigation. There are various methods for positioning the vehicle position by inertial navigation, and any method may be used. For example, the vehicle position is obtained by integrating the output value of the acceleration sensor twice by performing posture conversion, gravity correction, and Coriolis force correction, and adding the moving distance obtained by the two-time integration to the previous value of the vehicle position. May be derived. The vehicle position and vehicle speed (INS positioning result) calculated by the INS positioning calculation unit 52 are the vehicle position and speed (GPS positioning result) according to the configuration of the mobile positioning device 1 described above for each positioning cycle. The difference value is taken, and the difference value is input to the Kalman filter 54 to determine various correction amounts. Further, the difference from the speed detected by the wheel speed sensor 56 may also be input to the Kalman filter 54.

  The Kalman filter 54 estimates various correction amounts η so as to be the most probable value based on the reliability of the INS positioning result and the GPS positioning result. The state equation in the Kalman filter 54 is set as the following equation (4). In the equation, η (tn) represents a state variable at time t = tn. U (tn-1) and w (tn-1) are known input and disturbance (system noise: normal white noise) at time t = tn-1. η (tn) is the vehicle position and speed by INS positioning, vehicle posture error δr (INS), δv (INS), δε (INS), gyro sensor bias error δb, acceleration sensor drift error δd, tire radius Error δs and receiver clock error δCt in the GPS receiver.

  η (tn) = F · η (tn−1) + G · u (tn−1) + Γ · w (tn−1) (4)

  In a certain state, the observation amount z (the above-described difference value) has a relationship represented by the following equation (5) with η (tn). In the equation, H is an observation model that plays a role of linearly mapping the state space to the observation space, and v (tn) is noise that follows a Gaussian distribution.

  z (tn) = H (tn) .η (tn) + v (tn) (5)

  The INS positioning calculation unit 52 performs corrections such as posture conversion, gravity correction, and Coriolis force correction based on the output of the Kalman filter 54. As a result, the accuracy of INS positioning can be improved.

  According to the mobile positioning device 3 of the present embodiment, it is possible to perform INS positioning with higher accuracy.

  The best mode for carrying out the present invention has been described above with reference to the embodiments. However, the present invention is not limited to these embodiments, and various modifications can be made without departing from the scope of the present invention. And substitutions can be added.

  The present invention can be used in the automobile manufacturing industry, the automobile parts manufacturing industry, and the like.

1, 2, 3 Positioning device for moving body 10 GPS antenna 20 Positioning signal receiving unit 22 Position calculating unit 24 Speed calculating unit 26 Projection availability determining unit 28 Projecting unit 30 Map matching unit 32 Map database 40 Speed vector integrating unit 42 Reliability determination 50 Inertial sensor 52 INS positioning calculation unit 54 Kalman filter 56 Wheel speed sensor

Claims (3)

Position calculating means for calculating the position of the moving body using satellite radio waves;
Speed calculating means for calculating a speed vector of the moving body by satellite radio waves or inertial navigation;
Storage means for storing map data in which a road is represented by a plurality of links;
Projection means for outputting a projection speed vector obtained by projecting the speed vector calculated by the speed vector calculation means onto a link estimated to have a moving body among the plurality of links;
Projection propriety determining means for determining whether or not to adopt a projection velocity vector output by the projecting means,
When it is determined by the projection availability determination means that the projection speed vector should be adopted, the projection speed vector is output as a speed vector of the moving body,
A positioning device for moving objects. The mobile positioning device according to claim 1,
A velocity vector integrating means for calculating the position change of the moving body by integrating the velocity vectors of the moving body output based on the determination result of the projection propriety determining means;
Which is more reliable, the position of the moving body calculated by the position calculating means and the position of the moving body obtained by taking into account the position change of the moving body calculated by the speed vector integrating means? Reliability determination means for determining
The reliability determining means outputs the position of the moving body determined to be highly reliable,
A positioning device for moving objects. The mobile positioning device according to claim 1,
Inertial navigation positioning calculation means for calculating the position and velocity of the moving body by inertial navigation;
A Kalman filter for inputting a difference between the position and speed of the moving body calculated by the inertial navigation positioning calculation means and the position and speed of the moving body calculated by satellite radio waves,
The inertial navigation positioning calculation means performs correction according to the output of the Kalman filter,
A positioning device for moving objects.