JP2007248271A - Positioning device and positioning method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To precisely estimate measurement errors in autonomous navigation even when an absolute position and an absolute velocity are not acquired by radio navigation or when positioning accuracy by the radio navigation is low. <P>SOLUTION: It is determined whether or not the absolute positioning information acquired by the radio navigation is at high reliability on the basis of the number of satellites, from which the radio waves are received, arrangements, and so forth (step S2). In the case of high reliability, the absolute positioning information by the radio navigation is used to estimate the measurement errors of the positioning result by the autonomous navigation (step S4). When the absolute positioning information by the radio navigation is not at high reliability, a high-reliability satellite having reception power or an elevation angle larger than a threshold is selected from among the captured satellites (step S5). A pseudo distance to the high-reliability satellite and a Doppler frequency are calculated and utilized to estimate the measurement errors of the positioning result by the autonomous navigation (step S6). The positioning result by the autonomous navigation is corrected using the estimated measurement errors, and the measured position of a positioning apparatus 10 body is acquired. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to a positioning device and a positioning method that perform positioning based on a measurement value obtained by radio navigation that performs positioning using radio waves from a GPS satellite and the like, and a measurement value obtained by autonomous navigation. .

In recent years, provision of current position information of vehicles and pedestrians has been required in various fields such as in-vehicle terminal devices and portable terminal devices. As a positioning method of the current position, a radio navigation using GPS (Global Positioning System), an orientation sensor such as an angular velocity sensor, an autonomous navigation using a speed sensor such as an acceleration sensor, a vehicle speed sensor, and the like have been proposed. .
Radio navigation using GPS satellites has long-term stability, but has dependency on the positioning environment. In other words, in urban areas and mountainous areas where high-rise buildings are lined up, if radio waves from GPS satellites are blocked by buildings, etc., it may not be possible to acquire a sufficient number of satellites for measurement, and positioning may not be possible. is there. In addition, if it is affected by a multipath reflected from a building or the like, it may lead to a decrease in positioning accuracy.

Quasi-zenith satellite (QZSS) is attracting attention as a next-generation satellite system that compensates for the shortcomings of such GPS satellites. That is, the QZSS satellite is always placed near the zenith in Japan, and the GPS service is interpolated and augmented by using the QZSS satellite.
On the other hand, in autonomous navigation, an absolute position and an absolute direction are estimated by integrating angular velocities and accelerations measured by various sensors. For this reason, although the estimation accuracy in a short time is good, the error of the estimated position obtained by integrating this measurement value increases with time due to the influence of the error of the measurement value.

In order to compensate for the respective drawbacks of radio navigation and autonomous navigation, hybrid navigation has been proposed in which a plurality of types of navigation means are combined to increase the accuracy of speed and position detection.
In this hybrid navigation, for example, a Kalman filter is used to estimate a measurement error of a measurement value by autonomous navigation using a measurement value by radio wave navigation, and the measurement value by autonomous navigation is corrected using this. As an estimation method of this measurement error, for example, there are Loosely Coupled Mode and Tightly Coupled Mode.

  In the Loosely Coupled Mode, in radio navigation, if four or more GPS satellites can be captured, the absolute position of the moving body can be specified by performing positioning calculation. Therefore, the absolute positioning information ( The measurement error of various autonomous sensors used in autonomous navigation is estimated by the Kalman filter using the absolute position and the absolute velocity), and the measurement value by the autonomous sensor is corrected using the estimated error of the autonomous sensor.

On the other hand, in Tightly Coupled Mode, instead of the absolute positioning information (absolute position, absolute velocity) obtained by radio navigation, it uses the pseudorange (range) to the GPS satellite and the Doppler frequency as the observation value, and the measurement of the autonomous sensor An error is estimated.
Further, for example, in Patent Document 1, even if four GPS satellites cannot always be captured, if at least one GPS satellite can be captured, the pseudo distance (range) to the captured GPS satellite is not limited. ) And the rate of change (range rate), and a navigation device that estimates the measurement error of an autonomous sensor mounted on the navigation device has been proposed.
JP-A-6-341847

  By the way, in the Loosely Coupled Mode, since the observation model is simple, the size of the filter is small and easy to implement, and there is no dependency on the model of the receiver and portability is good. On the other hand, if four or more GPS satellites cannot be captured, absolute positioning information (absolute position, absolute velocity) cannot be obtained. The Kalman filter uses a prediction mode that predicts a measurement error from the previous estimated value of the measurement error, a measurement error predicted in this prediction mode, and absolute positioning information (absolute position and velocity) calculated by radio navigation. Since the measurement error is estimated by repeatedly performing the estimation mode for estimating the measurement error, as described above, if absolute positioning information (absolute position, absolute velocity) cannot be obtained, only the prediction mode is used. Thus, the measurement error is estimated.

  Therefore, the estimation accuracy of the measurement error depends mainly on the accuracy of the system model, the system noise, and the estimation accuracy of the measurement error immediately before positioning by radio navigation becomes impossible. In urban areas and mountainous areas where high-rise buildings are lined up, radio waves from satellites are often blocked or reflected by buildings or the like, so that improvement in positioning accuracy is a problem.

  In Tightly Coupled Mode, the pseudorange (range) and Doppler frequency can be obtained if at least one GPS satellite can be captured. Therefore, even when four or more GPS satellites cannot always be captured, the measurement error of the autonomous sensor used in autonomous navigation can be estimated. On the other hand, since the observation model becomes complicated, the size of the filter increases and the calculation load increases. In addition, since the pseudo-range (range) to the GPS satellite and the Doppler frequency are used instead of the absolute positioning information (absolute position and absolute velocity), there is a problem that it depends on the model of the GPS receiver.

Further, as in the invention described in Patent Document 1 described above, in the method of estimating the measurement error using the pseudorange (range) to the GPS satellite and the rate of change (range rate), absolute positioning information ( (Absolute position, absolute speed)
If one GPS satellite can be captured, the measurement error of the autonomous sensor can be estimated and the measurement value of the autonomous sensor can be corrected by using the pseudorange (range) and the rate of change (range rate). Therefore, position estimation can always be performed with high accuracy.

  On the other hand, when a large number of GPS satellites are received, the number of pseudoranges (ranges) used to estimate the measurement error and the rate of change (range rate) also increase, which is used for the calculation of Kalman gain. The size of the observation matrix, that is, the H matrix will increase with the number of captured GPS satellites. For this reason, the calculation load increases as the size of the H matrix increases. In addition, since the measurement error is estimated using the pseudorange (range) to the captured GPS satellite and the rate of change (range rate), it can be affected by multipath or from a satellite with low reception power. The measurement error is also estimated using the observation information, and in some cases, the measurement error may not be estimated accurately.

  Therefore, the present invention has been made paying attention to the above-mentioned conventional unsolved problems, and when absolute positioning information (absolute position, absolute velocity) cannot be obtained by radio navigation, or absolute positioning by radio navigation. It is possible to obtain more accurate positioning results by accurately estimating the measurement error in autonomous navigation and correcting the positioning information by autonomous navigation using this measurement difference even in an environment with low positioning accuracy. It aims at providing a positioning device and a positioning method.

  In order to solve the above-described problems, the positioning device of the present invention performs radio navigation using satellite radio waves from a plurality of satellites and obtains absolute positioning information including the absolute position and velocity of the receiver. Positioning means, high-reliability satellite selection means for selecting a high-reliability satellite based on the reception power of the satellite radio wave and the elevation angle of the satellite, and a satellite from the high-reliability satellite selected by the high-reliability satellite selection means Relative relationship observing means for measuring the pseudo-range between the receiver and the high-reliability satellite and the Doppler frequency of the high-reliability satellite using radio waves, and autonomous navigation positioning for positioning the receiver by autonomous navigation And a measurement error for estimating a measurement error of the positioning information of the autonomous navigation positioning means using the absolute positioning information obtained by the radio navigation positioning means and a relative relation observation value measured by the relative relation observation means A positioning device comprising: fixing means; and correcting means for correcting the positioning information of the autonomous navigation positioning means using the measurement error estimated by the measurement error estimating means to obtain the measurement position of the receiver, The measurement error estimation means includes a high reliability determination means for determining whether or not the absolute positioning information is highly reliable based on a positioning situation of the receiver, and the absolute positioning information is determined by the high reliability determination means. When it is determined to be highly reliable, the absolute positioning information is not highly reliable by the first measurement error estimating means for estimating the measurement error using the absolute positioning information and the highly reliable determining means. And a second measurement error estimating means for estimating the measurement error using the relative relationship observation value.

  According to the above configuration, based on the positioning status at the receiver, it is determined whether or not the absolute positioning information of the receiver obtained by radio navigation is highly reliable. The measurement error is estimated by the first measurement error estimating means using the positioning information. When it is determined that the reliability is not high, the measurement error is estimated by the second measurement error estimating means using the pseudorange and the Doppler frequency. I do.

  Then, the positioning information by the autonomous navigation is corrected by the measurement error estimated by the first measurement error estimation means or the second measurement error estimation means to obtain the measurement position of the receiver. Can be obtained by estimating the measurement error using the absolute positioning information, so that the measurement error with less influence of multipath or the like can be estimated. Conversely, when absolute positioning information with high reliability cannot be obtained, measurement is performed using satellite radio waves from high-reliability satellites that are less affected by multipath, etc., without using absolute positioning information with low reliability. By estimating the measurement error based on the pseudorange and the Doppler frequency, it is possible to estimate the measurement error with less influence of multipath or the like. Therefore, it is possible to avoid the estimation of the measurement error based on information with low accuracy, and to perform correction using a more accurate measurement error, thereby obtaining a more accurate positioning result.

Further, in the above positioning device, the high reliability determination means is based on at least one of the number and arrangement of satellites receiving the satellite radio waves by the receiver, and position sigma. , Determining whether or not the absolute positioning information is highly reliable.
According to the above configuration, since absolute positioning information is determined to be highly reliable based on the number of satellites receiving satellite radio waves, their arrangement, or at least one of position sigma, absolute It is possible to accurately determine whether or not the reliability of the positioning information is high.

Further, in the positioning device described above, the measurement error estimation means estimates the measurement error using a Kalman filter, and uses the absolute positioning information and the relative relationship observation value as an observation value of an observation equation in the Kalman filter. It is a feature.
According to the above configuration, the measurement error is estimated using the Kalman filter, and the absolute positioning information and the relative relation observation value are used as the observation value of the observation equation. Can be small. Therefore, it is possible to reduce the processing load accompanying the calculation process of the Kalman filter due to the large size of the observation value matrix.

In the above positioning device, the autonomous navigation positioning means measures the position and speed of the receiver, and the first measurement error estimating means includes the absolute position and the absolute speed measured by the radio navigation positioning means. The difference between the measurement position and the measurement speed measured by the autonomous navigation positioning means is the observed value.
According to the above configuration, the first measurement error estimating means calculates the difference between the absolute position and absolute speed measured by the radio navigation positioning means and the measurement position and measurement speed measured by the autonomous navigation positioning means, as an observation equation of the Kalman filter. Therefore, the observation matrix is composed of a position error and a velocity error, and the size of the observation matrix can be reduced.

  Further, in the above positioning device, the second position error estimating means estimates the measurement error individually using the pseudo-range observation equation and the Doppler frequency observation equation, and the autonomous navigation positioning Based on the positioning information of the means and the position information of the high-reliability satellite included in the satellite radio wave from the high-reliability satellite, and the pseudo-range between the receiver and the high-reliability satellite and the high-reliability satellite A relative relationship prediction unit that predicts a Doppler frequency, and the difference between the pseudorange measured by the relative relationship observation unit and the pseudorange predicted by the relative relationship prediction unit of the high-reliability satellite; And the difference between the Doppler frequency measured by the relative relationship observation means and the Doppler frequency predicted by the relative relationship prediction means is the observation value of the observation equation of the Doppler frequency. It is characterized in Rukoto.

  According to the above configuration, the second position error estimation means estimates the measurement error individually using the pseudo-range observation equation and the Doppler frequency observation equation, and the satellite radio wave of the high-reliability satellite. The difference between the pseudorange measured by the relative relationship observation means and the pseudorange predicted by the relative relationship prediction means was set as the observation value of the pseudorange observation equation, and was similarly measured by the relative relationship observation means. The difference between the Doppler frequency and the Doppler frequency predicted by the relative relationship predicting means is set as an observation value of the Doppler frequency observation equation. At this time, only the relative relationship using the satellite radio waves from the high-reliability satellites is set as the observation value, so the size of the observation matrix becomes the size according to the number of high-reliability satellites, When the absolute positioning information of reliability cannot be obtained, the number of high-reliability satellites is limited to some extent, so the size of the observation matrix can be limited to some value, and the size of the observation matrix increases Can be suppressed.

In the positioning device described above, the receiver is configured to receive satellite radio waves from GPS satellites and quasi-zenith satellites.
According to the above configuration, the receiver can receive satellite radio waves from GPS satellites and quasi-zenith satellites. Here, since the quasi-zenith satellite is a satellite that is always located near the zenith in Japan, the receiver always receives satellite radio waves from the quasi-zenith satellite, that is, a high-reliability satellite. Therefore, even if it is not possible to obtain highly reliable absolute position information by radio navigation and estimation of measurement error using this absolute position information, the second quasi-zenith satellite is used. The measurement error can be reliably estimated by the measurement error estimation means. And since this quasi-zenith satellite is a high-reliability satellite, it is possible to accurately estimate measurement errors, and to accurately estimate measurement errors regardless of the reception status of satellite radio waves from GPS satellites. Can do.

Further, the positioning method of the present invention calculates the absolute positioning information including the absolute position and the absolute velocity of the receiver, which is measured using satellite radio waves from a plurality of satellites, and the satellite radio waves from a high reliability satellite. Using the pseudo-range between the receiver and the high-reliability satellite and the Doppler frequency of the high-reliability satellite, the measurement error of the positioning information by autonomous navigation is estimated, and the autonomous navigation using the estimated measurement error A positioning method for correcting the positioning information by the receiver to obtain the measurement position of the receiver, based on the positioning status of the receiver, determining whether the absolute positioning information is highly reliable, and the absolute positioning information Is determined to be highly reliable, the measurement error is estimated using the absolute positioning information, and when the absolute positioning information is determined not to be highly reliable, the pseudorange and the Doppler frequency are used. It is characterized by estimating the measurement error Te.

  According to the above configuration, when it is determined that the absolute positioning information by radio navigation is highly reliable, the absolute positioning information is used to estimate the measurement error of the positioning information by autonomous navigation, and the absolute positioning information is highly reliable. When it is determined that the angle is not high, the pseudo-range to the high-reliability satellite and the Doppler frequency are used for estimation. Therefore, when high-reliability absolute positioning information can be obtained, this absolute positioning information is used to reduce the measurement error. By estimating it, it is possible to estimate measurement errors that are less affected by multipath, etc. On the contrary, when absolute positioning information with high reliability cannot be obtained, absolute positioning information with low reliability is not used. In addition, by estimating the measurement error based on the pseudorange and Doppler frequency measured using satellite radio waves from highly reliable satellites that are less affected by multipath, etc., the effects of multipath are reduced. There measurement error can be estimated. Therefore, it is possible to avoid estimating the measurement error based on information with low accuracy, and it is possible to more accurately estimate the measurement error.

Embodiments of the present invention will be described below.
FIG. 1 is a block diagram showing an example of a positioning device 10 to which the present invention is applied. This positioning device 10 receives a radio wave transmitted from a GPS satellite orbiting from the atmosphere of the earth and a quasi-zenith satellite QZSS. / QZSS receiver 1, an angular velocity sensor 2 as an azimuth sensor that detects an angular velocity in the yaw direction of the positioning device 10 body, an acceleration sensor 3 that detects acceleration, and the GPS / QZSS receiver. 1 based on the received signal received in 1 and performing radio navigation to calculate observation information such as a pseudo-range and Doppler frequency to the satellite representing the relative relationship between the GPS satellite or the QZSS satellite and the GPS / QZSS receiver 1, Further, positioning calculation by autonomous navigation is performed based on detection signals of the angular velocity sensor 2 and the acceleration sensor 3, and this positioning information is used as the radio wave. And an arithmetic processing unit 4 that corrects the measurement error of the positioning calculation by the autonomous navigation estimated based on the absolute positioning information and the observation information by the method, and uses this as a positioning result. The result is output to the display unit 5, for example.

  In this arithmetic processing unit 4, it is determined whether the reliability of the absolute positioning information obtained based on the positioning status of the GPS / QZSS receiver 1 is high, and the reliability of the absolute positioning information obtained by the positioning calculation is high. When the determination is made, the measurement error of the positioning calculation by the autonomous navigation is estimated in the filter mode of the positioning result level. Specifically, based on absolute positioning information (absolute position, absolute speed) by radio navigation, measurement errors of various sensors by autonomous navigation and errors of positioning calculation results are estimated.

On the other hand, when it is predicted that the positioning calculation can be performed by radio navigation but the reliability is low, or the positioning calculation cannot be performed by radio navigation from the positioning situation, the observation information level filter In the mode, the measurement error of the positioning calculation by the autonomous navigation is estimated. Specifically, based on observation information of radio waves from GPS / QZSS satellites, that is, pseudorange and Doppler frequency, measurement errors of various sensors by autonomous navigation and errors of positioning calculation results are estimated.
Then, based on the measurement error and positioning calculation error of each sensor by autonomous navigation estimated in this way, the measurement value of each sensor and the positioning calculation result are corrected, and the corrected positioning calculation result is It is set as the positioning result of the apparatus 10 main body.

FIG. 2 is a flowchart showing an example of the processing procedure of the positioning calculation process executed by the calculation processing device 4.
In the arithmetic processing unit 4, the GPS / QZSS receiver 1 receives a radio wave from a GPS satellite or a QZSS satellite, performs positioning calculation, shifts from step S1 to step S2, determines the positioning situation, and performs absolute positioning by radio wave navigation. If information can be obtained, and if absolute positioning information can be obtained, it is determined whether the absolute positioning information has high reliability or low reliability.

This determination is made, for example, by the number of captured satellites that have been able to receive radio waves by the GPS / QZSS receiver 1, the position accuracy decrease rate PDOP that indicates the rate of decrease in positioning accuracy due to the arrangement of the satellites, and the position sigma that indicates the positioning accuracy. Based on.
If the positioning status is determined in this way, the process proceeds to step S3. If it is determined that the absolute positioning information with high reliability can be obtained as a result of determining the positioning status, the process proceeds to step S4. Positioning calculation is performed in the result level filter mode, the measurement error in autonomous navigation is estimated using absolute positioning information (absolute position, absolute speed) by radio wave navigation, and a predetermined correction is made based on this, positioning device Get the positioning result of 10 main bodies.

On the other hand, as a result of determining the positioning status, it is possible to obtain absolute positioning information. However, if the reliability is low, or if the positioning calculation cannot be performed, the process proceeds to step S5 and the acquired satellite Select a high-reliability satellite.
Specifically, each of the captured GPS satellite and QZSS satellite is determined from the power of the received signal and its elevation angle. For example, the correlation value between the PN code contained in the radio wave from the satellite and the PN code held by itself is taken, and the difference between the peak value of the correlation value and the noise floor value is greater than or equal to a preset threshold value. When the elevation angle of the satellite is equal to or greater than a preset threshold value, the satellite is determined to be a high reliability satellite.

  Here, the GPS / QZSS receiver 1 is configured to receive radio waves from the quasi-zenith satellite QZSS, and any one of the quasi-zenith satellites QZSS is always a satellite that exists near the zenith in Japan. It is a satellite that exists at an elevation angle of 70 degrees or more and is not easily affected by multipaths without being interrupted by valleys of buildings. Therefore, at least one highly reliable satellite exists.

  Next, the process proceeds to step S6, and the positioning operation in the filter mode at the observation information level is performed based on the observation information of the radio wave, that is, the pseudorange and the Doppler frequency, for the acquired satellite determined as the high reliability satellite, A measurement error in the autonomous navigation is estimated using the observation information, and a predetermined correction is performed based on the estimation, thereby obtaining a positioning result of the positioning device 10 main body. The pseudo distance can be calculated by multiplying the difference between the satellite radio wave reception time at the positioning device 10 and the satellite transmission time included in the received signal by the speed of light. The Doppler frequency can be calculated as f ± Δf from the carrier frequency f [Hz] and the frequency shift Δf (= f · ρ / c) [Hz] due to the Doppler effect. Here, ρ is the relative speed between the positioning device 10 main body and the satellite, and c is the speed of light.

  Then, the process proceeds to step S 7, and the positioning result calculated in the positioning result level filter mode calculated in step S 4 or the pseudo distance level filter mode calculated in step S 6 is output to the display unit 5. And it returns to step S1 and performs the above-mentioned process repeatedly.

FIG. 3 is a block diagram showing a processing procedure of positioning calculation in the filter mode of the positioning result level.
In the positioning result level filter mode, the positioning calculation unit 11 performs positioning calculation by a known radio navigation procedure based on the received signal received from the GPS satellite or the QZSS satellite. That is, in this positioning result level filter mode, since it has been determined that highly reliable positioning calculation is possible, a determination result indicating that high reliability positioning calculation is possible among the acquired satellites has been obtained. The absolute position and absolute velocity of the positioning device 10 main body are calculated by a known procedure using received signals from one or more GPS satellites or QZSS satellites.

  On the other hand, the autonomous sensor navigation 12 performs a positioning calculation by a well-known autonomous navigation based on the angular velocity and acceleration detected by the angular velocity sensor 2 and the acceleration sensor 3 and a measurement error estimated by a Kalman filter 13 to be described later. Estimate the position, speed and orientation of the body. Then, based on the absolute positioning information (absolute position and absolute speed) calculated by the radio wave navigation calculated by the positioning calculation unit 11 and the positioning calculation result by the autonomous navigation calculated by the autonomous sensor navigation 12, the measurement by the autonomous navigation is performed by the Kalman filter 13. Guess the error. Then, the measurement error estimated by the Kalman filter 13 and the current positioning calculation result are added by the correction unit 14, and this is used as the current positioning result of the positioning device 10 main body.

Here, the Kalman filter will be briefly described.
The Kalman filter is derived as an algorithm that uses the observed signal and statistical a priori information to sequentially determine the optimum estimate (minimum variance estimate) of the introduced variable introduced when modeling the signal generation process. The process is repeated in two stages, a prediction mode and an estimation mode.
Here, when the state vector is x, the state space model can be expressed by the following equations (1) and (2). Note that the subscript k of the variable represents time.

x _{k + 1} = φ _k x _k + u _k (equation of state) (1)

z _k = H _k x _k + ε _k (observation equation) (2)

Incidentally, x _k is the state vector in the formula, phi _k is the state transition matrix, u _k is Gaussian white error (mean vector 0 as process noise, a normal distribution N with covariance matrix Q _k (0, Q _k) Z _k is an observation value, H _k is an observation matrix for associating the observation value z _k with the state vector x _k, and ε _k is a Gaussian white error (mean vector 0, Normal distribution N (0, R _k ) having a dispersion matrix R _k ).

In the prediction mode, the state vector prediction shown in equation (3) and the estimation error covariance prediction shown in equation (4) are performed, and the Kalman gain K _k is calculated according to equation (5).
In the estimation mode, state vector estimation shown in equation (6) and estimation error covariance estimation shown in equation (7) are performed.

x ⁻ _k = φ _k x ⁺ _k−1 (3)

P ⁻ _k = φ _k P ⁺ _k−1 φ _k ^T + Q _k−1 (4)

^{_{K k = P - k H k}} T [H k P - k H k T + R k] -1 ...... (5)

^{_{x + k = x - k +}} K k [z k -H k x - k] ...... (6)

_{^{P + k = [I-K}} k H k] P - k ...... (7)

Note that the subscript T in the expression represents a transposed matrix. X ^- _k is a predicted value of the state vector x, x ⁺ _k and x ⁺ _k-1 are estimated values of the state vector, P ^- _k is a predicted value of the estimation error covariance, P ⁺ _k and P ⁺ _k-1 Represents the estimated error covariance. Φ _k is a state transition matrix, Q _k−1 is a covariance matrix of a Gaussian white error u as the process noise, H _k is the observation matrix, and R _k is a Gaussian white error ε _k as an error of the observation value. Is the covariance matrix.

Then, using the state space model and the observed value z _k of the above formulas (1) and (2),
By repeating equation (7), the Kalman filter gain K _k converges according to the observed value z _k , and a more accurate state vector x ⁺ _k is estimated.
Next, the case where the Kalman filter 13 performs processing in the positioning device 10 including the GPS / QZSS receiver 1, the angular velocity sensor 2, and the acceleration sensor 3 as shown in FIG. 1 will be described. Here, in order to simplify the description, a case where two-dimensional navigation is performed will be described.

The state vector X in the positioning device 10 is expressed by the following equation (8).
_{X = [δP E δP N δV} E δV N δθ g B g Ba x Ba y] T ...... (8)

In equation (8), δP _E is an east position error, δP _N is a north position error, δV _E is an east speed error, δV _N is a north speed error, δθ _g is a yaw angle error, B _g is the angular velocity sensor bias, Ba _x is the acceleration sensor of X axis direction bias, the Ba _y is the bias of the acceleration sensor in the Y-axis direction.

  The bias and acceleration bias of the angular velocity sensor do not have deterministic changes, and can be expressed by the following equations (9) to (11) assuming that noise is loaded on the previous error.

B _g (k + 1) = B _g (k) + w _g (9)

Ba _x (k + 1) = Ba _x (k) + _wax (10)

_{Ba y (k + 1) =} Ba y (k) + w ay ...... (11)

The yaw direction angle error δθ _g is calculated by multiplying the previous angular velocity sensor bias B _g by the elapsed time from the previous time, that is, the sampling interval T, as shown in the following equation (12). Is added to the previous error.

δθ _g (k + 1) = δθ _g (k) + T · B _g (k) + wθ _g (12)

Here, the state equation is represented by the following equation (13).
X _{k + 1} = φX _k + w (13)

In the equation, φ is a state transition matrix that gives a temporal change of the state vector, w is system noise, and is expressed by the following equations (14) and (15). Incidentally, T in the formula is the sampling interval, is a _x acceleration in the X-axis direction, is a _y Y-axis direction of the acceleration, theta is the yaw direction angle.

Next, let the observation equation be the following equation (16).
Y = H · X + r (16)

  Y is an observation value and is expressed by the following equation (17), H is an observation matrix for associating the observation value Y with the state vector X, and r is an observation noise.

(17) In the equation _{, PE, INS} , _{PN, INS} , _{VE, INS} , _{VN, INS} are the east position, north position, east speed, north direction obtained by positioning calculation by autonomous navigation. P _{E, GPS} , P _{N, GPS} , V _{E, GPS} , V _{N, GPS} are east direction position, north direction position, east direction speed, north direction speed obtained by positioning calculation by radio navigation. is there.
First, an initial positioning position (x, y, z) of the positioning device 10 is set, and further, as a state vector estimated value X ⁺ _k−1 and an error covariance matrix P estimated value P ⁺ _k−1 , a predetermined value is set. Set the initial value of. As the initial value, for example, the current position of the positioning device 10 and the state vector estimated value X ⁺ _k−1 can be obtained by performing a positioning calculation by the least square method. A predetermined value set in advance is set as the estimated value P ⁺ _k−1 of the error covariance matrix P.

In addition, although the case where the calculation result by the least square method was used as the initial value of the positioning device 10 and the initial value of the state vector has been described here, the present invention is not limited to this, and the initial value is set by another method. Also good.
Then, the positioning calculation unit 11 performs positioning calculation by radio wave navigation, and the east direction position P _{E, GPS} ,
The north direction position P _{N, GPS} , the east direction speed V _{E, GPS} , and the north direction speed V _{N, GPS} are acquired. Similarly, the autonomous sensor navigation 12 obtains the east direction position P _{E, INS} , the north direction position P _{N, INS} , the east direction speed V _{E, INS} , and the north direction speed V _{N, INS} by autonomous navigation.

Then, an observation equation is obtained from the positioning calculation result by the radio navigation and the autonomous navigation and the equations (16) and (17).
Then, the state vector X ^- _k and the estimated error matrix variance P ^- _k are predicted from the equations (3) and (4), and the Kalman filter gain _Kk is calculated from the equation (5).
Next, the state vector X ⁺ _k and the estimated error matrix variance P ⁺ _k are estimated from the Kalman filter gain K _k obtained from the equation (5) and the equations (6) and (7).

The above procedure is repeated, and each component of the state vector X expressed by the above equation (8), that is, an error in the positioning calculation result when performing positioning calculation in autonomous navigation, measurement by the angular velocity sensor 2 or the acceleration sensor 3 Guess the error.
The correction unit 14 uses the estimated measurement error to calculate the eastward position P _{E, INS} , the northward position P _{N, INS} , the eastward speed V _{E, INS} , and the northward speed V _{N, INS} is corrected and this is output as a positioning result.
Further, the estimated value of the measurement error obtained by the Kalman filter 13 is fed back to the autonomous sensor navigation 12.

In the autonomous sensor navigation 12, the current positioning result based on the estimation value of the measurement error, that is, the east position P _{E, INS} , the north position P _{N, INS} , the east speed V _{E, INS} , north The direction velocity V _{N, INS} is corrected and stored as the current positioning result. Further, the yaw direction angular error .delta..theta _g, bias B _g of the angular velocity sensor, bias Ba _x of the acceleration sensor in the X-axis direction, a bias Ba _y of the acceleration sensor in the Y-axis direction, is stored in a predetermined storage area, the following When the positioning calculation process is performed, the detection signals of the angular velocity sensor 2 and the acceleration sensor 3 are corrected using the estimated values of the measurement errors, and the corrected detection signals of the angular velocity sensor 2 and the acceleration sensor 3 and the measurement error are corrected. The current positioning calculation is performed based on the previous positioning result corrected by the estimated value.

Next, FIG. 4 is a block diagram showing a processing procedure in the filter mode of the observation information level.
In the filter mode at the observation information level, the positioning calculation unit 21 uses the GPS satellite or QZSS satellite that has been captured to the satellite selected as a high-reliability satellite based on the correlation value or elevation angle of the PN code. Are calculated as an observation pseudorange and an observation Doppler frequency.

  That is, in this observation information level filter mode, radio waves from four or more GPS satellites or QZSS satellites are not received and positioning calculation cannot be performed, or highly reliable positioning results are obtained by radio navigation. Since it is a situation that cannot be obtained, measurement error estimation by autonomous navigation is performed using a pseudorange and Doppler frequency based on a highly reliable satellite, that is, a highly reliable received signal that is not affected by multipath etc. Do.

On the other hand, the autonomous navigation 22 performs positioning calculation by a known autonomous navigation based on the angular velocity and acceleration detected by the angular velocity sensor 2 and the acceleration sensor 3 and the estimated value of the measurement error estimated by the Kalman filter 13 described later. The position, speed and direction of the apparatus 10 main body are calculated. Furthermore, the pseudo distance to the high reliability satellite is predicted based on the position of the positioning apparatus 10 main body obtained as a result of the positioning calculation and the current position of the high reliability satellite included in the received signal. Further, the relative speed between the positioning apparatus 10 and the high reliability satellite is calculated based on the speed of the positioning apparatus 10 main body and the speed of the high reliability satellite, and the Doppler frequency is predicted based on this. These are set as a predicted pseudorange and a predicted Doppler frequency.

Then, based on the observed pseudorange and observed Doppler frequency of the high-reliability satellite selected by the positioning calculation unit 21 and the predicted pseudorange and predicted Doppler frequency predicted by the autonomous sensor navigation 22, measurement by the autonomous navigation is performed by the Kalman filter 23. Estimate the estimation error of the estimation result. Then, using the estimated measurement error, the correction unit 24 calculates the positioning result calculated by the autonomous navigation, that is, the east position P _{E, INS} , the north position P _{N, INS} , the east speed V _{E, INS} , north The direction velocity V _{N, INS} is corrected and this is output as a positioning result.
Further, the estimated value of the measurement error obtained by the Kalman filter 23 is fed back to the autonomous sensor navigation 22.

In the autonomous sensor navigation 22, the current positioning result based on the estimated measurement error, that is, the east position P _{E, INS} , the north position P _{N, INS} , the east speed V _{E, INS} , north The direction velocity V _{N, INS} is corrected and stored as the current positioning result. Further, the yaw direction angular error .delta..theta _g, bias B _g of the angular velocity sensor, bias Ba _x of the acceleration sensor in the X-axis direction, a bias Ba _y of the acceleration sensor in the Y-axis direction, is stored in a predetermined storage area, the following When the positioning calculation process is performed, the detection signals of the angular velocity sensor 2 and the acceleration sensor 3 are corrected using the estimated values of the measurement errors, and the corrected detection signals of the angular velocity sensor 2 and the acceleration sensor 3 and the measurement error are corrected. The current positioning calculation is performed based on the previous positioning result corrected by the estimated value.

  In the Kalman filter 23 in the filter mode of the observation information level, the extended Kalman filter is applied for linearization, and the observation equations are expressed by the following equations (18) and (19).

Y1 = H · X + r1 (18)
Y2 = H · X + r2 (19)

  Y1 and Y2 are observed values and are represented by the following equations (20) and (21).

Then, similarly to the above-described positioning result level filter mode, first, the initial positioning position (x, y, z) of the positioning apparatus 10 main body is set, and further, the state vector estimated value X ⁺ _k−1 and the error covariance are set. A predetermined initial value is set as the estimated value P ⁺ _k−1 of the matrix P.

Then, an observation equation is obtained from the observation pseudorange and observation Doppler frequency of the high-reliability satellite from the positioning calculation unit 21 and the autonomous sensor navigation 22, the prediction pseudorange and the prediction Doppler frequency, and (18) to (21). .
Then, the state vector X ^- _k and the estimated error matrix variance P ^- _k are predicted from the equations (1) and (2), and the Kalman filter gain _Kk is calculated from the equation (3).

Next, the state vector X ⁺ _k and the estimated error matrix variance P ⁺ _k are estimated from the Kalman filter gain K _k obtained from the equation (3) and the equations (4) and (5).
Repeating the above procedure, each component of the state vector X represented by the above equation (8), that is, an error in the positioning calculation result when performing the positioning calculation in autonomous navigation, a measurement error due to the angular velocity sensor 2 or the acceleration sensor 3 Guess.

Then, using the estimated measurement error, the correction unit 24 uses the autonomous navigation to calculate the eastward position P _{E, INS} , the northward position P _{N, INS} , the eastward speed V _{E, INS} , and the northward speed V _{N. , INS} is corrected and this is output as a positioning result.
The measurement error obtained by the Kalman filter 23 is fed back to the autonomous sensor navigation 22.

The autonomous sensor navigation 22 corrects the current positioning result based on the measurement error, and stores this as the current positioning result. Further, the yaw direction angular error .delta..theta _g, bias B _g of the angular velocity sensor, bias Ba _x of the acceleration sensor in the X-axis direction, a bias Ba _y of the acceleration sensor in the Y-axis direction, is stored in a predetermined storage area, the following When performing positioning calculation processing
Using these measurement error estimation values, the detection signals of the angular velocity sensor 2 and the acceleration sensor 3 are corrected, the detection signals of the angular velocity sensor 2 and the acceleration sensor 3 after correction, and the previous positioning result corrected by the estimated measurement error, Based on this, the current positioning calculation is performed.

Next, the operation of the present invention will be described.
The GPS / QZSS receiver 1 receives radio waves from GPS satellites or QZSS satellites, performs positioning calculation, and determines the position based on the number of captured satellites, the rate of accuracy decrease according to the position of the captured satellites, PDOP, position sigma, etc. The situation is determined, and it is determined whether or not highly reliable absolute positioning information can be acquired by radio navigation (steps S1 and S2).

When it is determined that highly reliable absolute positioning information can be obtained, the process proceeds from step S3 to step S4, and the measurement error is estimated in the positioning result level filter mode. That is, the absolute position and absolute velocity of the positioning device 10 main body calculated by radio navigation, the detection signals of the angular velocity sensor 2 and the acceleration sensor 3, the yaw angle direction angle error δθ _g calculated in the previous sampling period, and the gyro bias B _g, the acceleration sensor bias Ba _x, Ba _y and the east direction and the north direction of the position error [delta] P _E, [delta] P _N, east direction and north direction of the velocity error .DELTA.V _E, the previous positioning apparatus 10 body corrected by .DELTA.V _N Based on the positioning result, the current position, speed and direction are calculated.

Based on these, an observation value of the measurement error is set according to the equation (17), Kalman filter processing is performed, and the measurement error is estimated. Then, the positioning result obtained by the autonomous navigation is corrected based on the estimated value of the measurement error, and the corrected value is output to the display unit 5 as the positioning result of the positioning device 10 main body. Further, based on the estimated value of the measurement error, the positioning result by the autonomous navigation is corrected and stored as the current positioning result, and the estimated value of the measurement error of the angular velocity sensor 2 or the acceleration sensor 3 is stored in a predetermined storage area. To do.
The above process is repeated while a highly reliable positioning calculation can be performed.

Here, in the Kalman filter processing, as shown in the equation (17), processing is performed using the east and north positions and the east and north velocities obtained by radio navigation as measured values. It is composed of a 4 × 1 matrix.
In the Kalman filter process, as shown in the equation (5), since the calculation is performed using the inverse matrix of the observation matrix of the observation equation, when the size of the observation matrix is large, the calculation processing load of the calculation processing device 4 is large. Will increase. However, as shown in the equation (17), since a 4 × 1 matrix is used as the observed value Y and a matrix having a relatively small size is used, an increase in processing load can be reduced.

At this time, since the observed value Y of the measurement error is set based on the highly reliable positioning result calculated by the radio navigation and the positioning result by the autonomous navigation, it is influenced by multipaths and the like. Thus, the measurement error can be estimated using the observed value Y with less error, and the measurement error can be estimated more accurately.
Therefore, a more accurate positioning result can be obtained by correcting the positioning result by the autonomous navigation using the measurement error estimated in this way and obtaining the position of the positioning device 10 main body using this.

On the other hand, when absolute positioning information by radio navigation can be obtained but the reliability is determined to be relatively low, or when four or more satellites cannot be captured and positioning calculation cannot be performed. Shifts from step S3 to step S5, selects a high-reliability satellite from the captured satellites based on the correlation value of the PN code and the elevation angle of the satellite, and sets the observation pseudorange and observation Doppler frequency of this high-reliability satellite. To win.
Further, based on the position and speed of the high-reliability satellite and the position and speed of the receiver, the present pseudorange and Doppler frequency are predicted, and based on these, the observed value of the measurement error from the equations (20) and (21). And Kalman filter processing is performed to calculate an estimated value of measurement error.

Then, the positioning result obtained by the autonomous navigation is corrected by the estimated value of the measurement error, and the corrected positioning result is output as the positioning result of the positioning device 10 main body. Further, based on the estimated value of the measurement error, the positioning result by the autonomous navigation is corrected and stored as the current positioning result, and the estimated value of the measurement error of the angular velocity sensor 2 or the acceleration sensor 3 is stored in a predetermined storage area. To do.
This process is repeated while the positioning result by radio navigation is relatively low in reliability, or while four or more satellites cannot be captured and the positioning result by autonomous navigation cannot be obtained.

  Here, in the Kalman filter processing, as shown in the equations (20) and (21), the measurement values are calculated based on the observation pseudorange and observation Doppler frequency from the high-reliability satellite and the positioning result by autonomous navigation. The difference between the predicted pseudorange and the predicted Doppler frequency is used as an observed value, and if the number of high-reliability satellites is n, the matrix is composed of 2n × 1. As described above, when highly reliable positioning results can be obtained by radio navigation, the positioning calculation is performed in the filter mode of the positioning result level, so the number of satellites determined to be highly reliable satellites. Is limited to some extent. Therefore, the observed value Y is represented by a relatively small matrix.

For this reason, also in this observation information level filter mode, since a matrix with a relatively small size is used as the observation value Y, the processing load on the arithmetic processing unit 4 when executing the Kalman filter processing is reduced. Can do.
At this time, since the observation error value Y is set using the observation pseudorange and the observation Doppler frequency with high accuracy from the satellites that are determined to be high reliability among the acquisition satellites, the error is further improved. The measurement error can be estimated using the observation value with a small measurement error. Therefore, a more accurate positioning result of the positioning device 10 main body can be obtained by correcting the positioning result by the autonomous navigation using the measurement error with less error estimated in this way. In particular, the accuracy drops significantly when affected by multipath, but more accurate positioning can be achieved by using the observation pseudorange and observation Doppler frequency of the satellite determined to be a highly reliable satellite as described above. The result can be obtained.

  In addition, when there is no high-reliability satellite, the observation value Y cannot be set. Therefore, in the Kalman filter processing, error estimation must be performed only in the prediction mode. However, as described above, the GPS / QZSS receiver 1 receives not only GPS satellites but also radio waves from QZSS satellites. The QZSS satellite is a satellite with an elevation angle of 70 degrees or more that is always present near the zenith in Japan, and is a highly reliable satellite that is less affected by multipath. Therefore, at least one highly reliable satellite is present. Will exist.

Therefore, even if there is no GPS satellite that is a high-reliability satellite, one high-reliability satellite (QZSS satellite) is always present and can receive radio waves. A more appropriate observation pseudorange and observation Doppler frequency from the satellite can be obtained.
Therefore, measurement errors can be estimated more appropriately by using these more appropriate observation pseudoranges and observation Doppler frequencies. Therefore, by using this measurement error to correct the positioning result by autonomous navigation, a positioning result with less error can be obtained.

  In the above embodiment, the case of receiving radio waves from GPS satellites and QZSS satellites has been described. However, for example, the present invention can be applied even when receiving radio waves from GPS satellites. Also, the same effect as above can be obtained. When the QZSS satellite is not used, there may be no high-reliability satellite depending on the reception status of the radio wave from the GPS satellite. When positioning calculation based on a received signal from a satellite that is not a high-reliability satellite is performed, the positioning accuracy decreases. For this reason, when a highly reliable satellite cannot be secured in this way, the measurement error may be estimated by using only the prediction mode in the Kalman filter processing without using the observation value, or the positioning accuracy may be somewhat However, the observation value may be set using a received signal from a satellite with a relatively high reliability among the captured satellites, and may be selected according to the operation status.

  Here, in the above embodiment, the positioning calculation unit 11 in FIG. 3 corresponds to the radio navigation positioning unit, the processing in step S5 in FIG. 2 corresponds to the high-reliability satellite selection unit, and the positioning calculation unit 21 in FIG. Corresponds to the relative relationship observation means, the autonomous sensor navigations 12 and 22 in FIGS. 3 and 4 correspond to the autonomous navigation positioning means, the Kalman filters 13 and 23 in FIGS. 3 and 4 correspond to the measurement error estimation means, 3 and 4 correspond to correction means. 2 corresponds to the high reliability determination means, the process of step S4 corresponds to the first measurement error estimation means, and the process of step S6 corresponds to the second measurement error estimation means. It corresponds to.

  In the autonomous sensor navigation 22 of FIG. 4, the process of calculating the predicted pseudorange and the predicted Doppler frequency corresponds to the relative relationship predicting means.

It is a block diagram which shows an example of the positioning apparatus of this invention. It is a flowchart which shows an example of the process sequence of the positioning calculation process performed with a calculation processing apparatus. It is a block diagram which shows the procedure of the positioning calculation in the filter mode of a positioning result level. It is a block diagram which shows the procedure of the positioning calculation in the filter mode of an observation information level.

Explanation of symbols

  1 GPS / QZSS receiver, 2 angular velocity sensor, 3 acceleration sensor, 4 arithmetic processing unit, 11, 21 positioning operation unit, 12, 22 autonomous sensor navigation, 13, 23 Kalman filter, 14, 24 correction unit

Claims (7)

Radio navigation positioning means that performs positioning by radio navigation using satellite radio waves from a plurality of satellites, and obtains absolute positioning information including the absolute position and velocity of the receiver;
High-reliability satellite selection means for selecting a high-reliability satellite based on the reception power of the satellite radio wave and the elevation angle of the satellite;
Relative to measure the pseudo-range between the receiver and the high-reliability satellite and the Doppler frequency of the high-reliability satellite using satellite radio waves from the high-reliability satellite selected by the high-reliability satellite selection means Relationship observation means,
Autonomous navigation positioning means for positioning the receiver by autonomous navigation;
A measurement error estimating means for estimating a measurement error of the positioning information of the autonomous navigation positioning means using the absolute positioning information obtained by the radio navigation positioning means and a relative relation observation value measured by the relative relation observation means;
A correction device comprising: correction means for correcting the positioning information of the autonomous navigation positioning means using the measurement error estimated by the measurement error estimation means to obtain the measurement position of the receiver,
The measurement error estimation means is a high reliability determination means for determining whether or not the absolute positioning information is high reliability based on the positioning status of the receiver;
A first measurement error estimating unit that estimates the measurement error using the absolute positioning information when the absolute positioning information is determined to be high reliability by the high reliability determination unit;
A second measurement error estimating unit configured to estimate the measurement error using the relative observation value when the absolute positioning information is determined not to be high reliability by the high reliability determination unit. A unique positioning device.   The high reliability determination means is configured to make the absolute positioning information highly reliable based on at least one of the number and arrangement of satellites receiving the satellite radio waves by the receiver and position sigma. The positioning device according to claim 1, wherein it is determined whether or not the degree is a degree. The measurement error estimation means estimates the measurement error using a Kalman filter,
3. The positioning device according to claim 1, wherein the absolute positioning information and the relative relation observation value are used as observation values of an observation equation in the Kalman filter. The autonomous navigation positioning means measures the position and speed of the receiver,
The first measurement error estimating means uses a difference between an absolute position and an absolute velocity measured by the radio navigation positioning means and a measurement position and a measurement speed measured by the autonomous navigation positioning means as the observed value. The positioning device according to claim 3. The second position error estimation means estimates the measurement error individually using the pseudorange observation equation and the Doppler frequency observation equation,
Based on the positioning information of the autonomous navigation positioning means and the position information of the high reliability satellite included in the satellite radio wave from the high reliability satellite, the pseudo distance between the receiver and the high reliability satellite and the high A relative relationship prediction means for predicting the Doppler frequency of the reliability satellite is provided.
The difference between the pseudorange measured by the relative relationship observation unit of the high reliability satellite and the pseudorange predicted by the relative relationship prediction unit is set as an observation value of the observation equation of the pseudorange, and the relative relationship observation unit 5. The positioning device according to claim 3, wherein a difference between the measured Doppler frequency and the Doppler frequency predicted by the relative relationship predicting means is used as an observation value of the observation equation of the Doppler frequency.   The positioning device according to any one of claims 1 to 5, wherein the receiver is configured to receive satellite radio waves from a GPS satellite and a quasi-zenith satellite. Absolute positioning information including absolute position and absolute velocity of a receiver measured using satellite radio waves from a plurality of satellites, and the receiver and high reliability satellites calculated using satellite radio waves from a high reliability satellite The measurement error of the positioning information by autonomous navigation is estimated using the pseudo-range between and the Doppler frequency of the high-reliability satellite, and the positioning information by the autonomous navigation is corrected using the estimated measurement error and the reception is performed. A positioning method for obtaining the measurement position of the machine,
Based on the positioning status of the receiver, determine whether the absolute positioning information is highly reliable,
When the absolute positioning information is determined to be highly reliable, the measurement error is estimated using the absolute positioning information,
A positioning method characterized by estimating the measurement error using the pseudorange and the Doppler frequency when it is determined that the absolute positioning information is not highly reliable.

Images