JP2008232761A - Positioning device for mobile - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To converge variations in pseudorange in an early stage. <P>SOLUTION: This positioning device 1 for a mobile includes an inertial navigation positioning section 60, a GPS receiver 20 for measuring a pseudorange between a satellite and the mobile, a pseudorange presumption section 80, and a variance calculation section 30 for calculating the variance ρ<SB>presumption</SB>of pseudoranges ρ<SB>presumption</SB>presumed by the presumption section 80, on the basis of the variance of positions of the satellite by information on satellite orbits, the variance of the positions of the mobile derived by the positioning section 60, and the variance of errors of the clock of the GPS receiver 20. Favorably, the device 1 further includes a pseudorange combining section 40 for combining the pseudorange ρ measured by the GPS receiver 20, and the pseudorange ρ<SB>presumption</SB>presumed by the presumption section 80 to calculate a combined pseudorange ρ<SB>com</SB>, and a positioning calculation section 50 for performing positioning of the mobile position by using this combined pseudorange ρ<SB>com</SB>. The combining section 40 calculates the combined pseudorange ρ<SB>com</SB>by using the variance ρ<SB>presumption</SB>calculated by the calculation section 30. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a positioning device for a moving body that measures the position of a moving body.

  2. Description of the Related Art Conventionally, a locator device having a compound navigation that calculates the current position, traveling direction, speed, etc. of a vehicle using observation information of a GPS receiver and measurement information of a compound sensor for autonomous navigation (inertial navigation) is known. (For example, refer to Patent Document 1). In this locator device, the scaling factor of the wheel speed sensor is corrected using a Kalman filter that fuses inertial navigation and satellite navigation in a loose coupling manner.

Further, in recent years, when combining inertial navigation and satellite navigation, tight coupling type that directly uses GPS receiver observation raw data (for example, pseudo-range measurement data) instead of GPS receiver positioning results. A Kalman filter has been proposed.
JP 2000-346661 A

  By the way, in the radio wave (satellite signal) from an actual GPS satellite, thermal noise (noise existing in the natural world) is added between the GPS satellite and the GPS receiver, so that the pseudo noise code such as a C / A code is added. In the configuration in which the pseudo distance is calculated based on this, it takes a long time to converge the pseudo distance variation.

  In this respect, in the literature described in the background art described above, from the viewpoint of improving the positioning accuracy by correcting the bias error and drift error of the sensor, inertial navigation and satellite navigation are performed using a loose coupling type or a tight coupling type. Although they are integrated, no proposal has been made from the viewpoint of converging the pseudorange variation at an early stage.

  Therefore, an object of the present invention is to provide a positioning device for a moving body that can converge a variation in pseudo distance at an early stage.

In order to achieve the above object, a first invention is a positioning device for a moving body that is mounted on a moving body and measures the position of the moving body.
Inertial navigation positioning means for calculating the position of the moving body using inertial navigation;
A pseudo distance measuring means for measuring a pseudo distance between the satellite and the mobile body based on the code information carried on the radio wave from the satellite and the time information of the receiver clock;
The pseudo distance measured by the pseudo distance measuring means is calculated by calculating a satellite position based on satellite orbit information, a calculated position of the moving body by the inertial navigation positioning means, and a clock error of the receiver clock. A pseudo-range estimation means for estimating based on the estimated value;
Estimated by the pseudo-range estimating means based on the dispersion of the position of the satellite related to the satellite orbit information, the dispersion of the position of the moving object calculated by the inertial navigation positioning means, and the variance of the clock error. A variance calculating means for calculating the variance of the pseudo distance.

2nd invention is the positioning apparatus for moving bodies which concerns on 1st invention,
A combined pseudo distance calculating means for calculating a combined pseudo distance by combining the pseudo distance measured by the pseudo distance measuring means and the pseudo distance estimated by the pseudo distance estimating means;
A positioning calculation means for positioning a moving body position using the combined pseudo distance calculated by the combined pseudo distance calculation means;
The combined pseudo distance calculating unit performs the pseudo distance combining process using the variance calculated by the variance value calculating unit.

3rd invention is the positioning apparatus for moving bodies based on 1st invention,
The pseudo distance stage estimated by the pseudo distance estimating means and the pseudo distance measured by the pseudo distance measuring means are input to a tight coupling type Kalman filter.

4th invention is the positioning apparatus for moving bodies which concerns on 3rd invention,
The variance calculating means includes a variance of the position of the moving body calculated by the inertial navigation positioning means based on a diagonal component of an error covariance matrix derived by the tight coupling type Kalman filter, and the clock error. Is calculated.

  ADVANTAGE OF THE INVENTION According to this invention, the positioning apparatus for moving bodies which can converge the dispersion | variation in a pseudo distance at an early stage is obtained.

  The best mode for carrying out the present invention will be described below with reference to the drawings.

  FIG. 1 is a system configuration diagram showing an overall configuration of a GPS (Global Positioning System) to which a mobile positioning device according to the present invention is applied. As shown in FIG. 1, the GPS is composed of a GPS satellite 10 that orbits the earth and a vehicle 90 that is located on the earth and can move on the earth. However, the vehicle 90 is an example of a mobile station, and other mobile objects are assumed to be information terminals such as motorcycles, railways, ships, airplanes, hawk lifts, robots, and mobile phones that move as people move. Is done.

  The GPS satellite 10 always broadcasts navigation messages toward the earth. The navigation message includes satellite orbit information (ephemeris and almanac) regarding the corresponding GPS satellite 10, a clock correction value, and an ionosphere 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 a code string in which -1 and 1 are irregularly arranged periodically.

  Currently, 24 GPS satellites 10 orbit the earth at an altitude of about 20,000 km, and each of the four GPS satellites 10 is evenly arranged on six Earth orbit planes inclined by 55 degrees. ing. Therefore, as long as the sky is open, at least five GPS satellites 10 can be observed at any time on the earth.

  The vehicle 90 is mounted with the moving body positioning device 1. FIG. 2 is a main functional block diagram of an embodiment of the mobile positioning device 1 according to the present invention.

  As shown in FIG. 2, the mobile positioning apparatus 1 includes a tight coupling type positioning system 5 in which inertial navigation and satellite navigation are fused, a dispersion calculating unit 30, a pseudo-range coupling unit 40, And the positioning calculation part 50 is added.

  The tight coupling type positioning system 5 includes a GPS receiver 20, an inertial navigation positioning unit 60, a state estimator 70, and a pseudorange estimation unit 80.

  The GPS receiver 20 may have any configuration as long as it has a function of measuring the pseudorange and the Doppler velocity.

FIG. 3 shows an example of the internal configuration of the GPS receiver 20. Hereinafter, to avoid complicating the description, it will be described as a representative signal processing relating satellite signals from one single GPS satellite 10 ₁ (1-channel signal processing). The signal processing described below is executed in parallel (simultaneously) with respect to the satellite signals from the observable GPS satellites 10 ₁ , 10 ₂ , 10 _3, etc. for each observation period (for example, 1 ms).

  The GPS receiver 20 includes a GPS antenna 21, a high-frequency circuit 22, an A / D (analog-to-digital) conversion circuit 24, a DDL (Delay-Locked Loop) 110, a PLL (Phase-Locked Loop) 120, a Doppler velocity calculation unit. 122, a satellite position calculation unit 124, and a filter 130. The DDL 110 includes a cross-correlation calculation units 111 and 112, a phase advancement unit 113, a phase delay unit 114, a phase shift calculation unit 115, a phase correction amount calculation unit 116, a replica C / A code generation unit 117, and a pseudo distance calculation unit 118. including.

GPS antenna 21 receives a hygienic signal transmitted from the GPS satellite 10 _1, the voltage signal satellite signal received (in this example, frequency 1.5 GHz) is converted to. A voltage signal of 1.5 GHz is referred to as an RF (radio frequency) signal.

  The high-frequency circuit 22 amplifies a weak RF signal supplied via the GPS antenna 21 to a level at which A / D conversion can be performed later, and at the same time, an intermediate frequency (typically 1 MHz to 20 MHz). A signal obtained by down-converting the RF signal in this way is referred to as an IF (Intermediate frequency) signal.

  The A / D conversion circuit 24 converts the IF signal (analog signal) supplied from the high frequency circuit 22 into a digital IF signal so that digital signal processing can be performed. The digital IF signal is supplied to the DDL 110, the PLL 120, and the like.

The replica C / A code generation unit 117 of the DDL 110 generates a replica C / A code. The replica C / A code with respect to the C / A code, which is put on the satellite signals from the GPS satellites 10 _1, + 1, the arrangement of -1 is the same code.

The replica C / A code generated by the replica C / A code generation unit 117 is input to the cross correlation calculation unit 111 via the phase advancement unit 113. That is, an early replica code is input to the cross correlation calculation unit 111. In the phase advancer 113, the replica C / A code is advanced by a predetermined phase. Let the phase advance amount advanced by the phase advancer 113 be θ ₁ .

  Further, the digital correlation signal is input to the cross-correlation calculation unit 111 after being multiplied by a replica carrier generated by the PLL 120 by a mixer (not shown).

The cross-correlation calculation unit 111 calculates a correlation value (Early correlation value E _CA ) using the input digital IF signal and the Early replica code of the phase advance amount θ ₁ . The Early correlation value E _CA is calculated by the following equation, for example.
Early correlation value E _CA = Σ {(digital IF) × (Early replica code)}
The replica C / A code generated by the replica C / A code generation unit 117 is input to the cross correlation calculation unit 112 via the phase delay unit 114. That is, the late replica code is input to the cross-correlation calculation unit 112. In the phase delay unit 114, the replica C / A code is delayed by a predetermined phase. The phase delay amount delayed by the phase delay unit 114 is the same as the phase advance amount θ ₁ but has a different sign.

  Further, the digital correlation signal is input to the cross-correlation calculation unit 112 after being multiplied by a replica carrier generated by the PLL 120 by a mixer (not shown).

The cross-correlation calculation unit 112 calculates a correlation value (Late correlation value L _CA ) using the input digital IF signal and the Late replica code of the phase delay amount −θ ₁ . The Late correlation value L _CA is calculated by the following equation, for example.
Late correlation value L _CA1 = Σ {(digital IF) × (Late replica code)}
In this way, the cross-correlation calculation units 111 and 112 execute the correlation value calculation with the correlator interval d ₁ (also referred to as “spacing”) being 2θ ₁ . The Early correlation value E _CA and the Late correlation value L _CA calculated by the cross correlation calculation units 111 and 112 are input to the phase shift calculation unit 115.

The phase shift calculation unit 115 calculates the degree of phase shift between the digital IF signal and the replica C / A code generated by the replica C / A code generation unit 117. That is, the phase shift calculation unit 115 calculates (estimates) the phase shift amount Δφ of the replica C / A code with respect to the received C / A code. The phase shift amount Δφ of the replica C / A code is calculated by the following equation, for example.
(Phase shift amount Δφ) = (E _CA −L _CA ) / 2 (E _CA + L _CA )
The phase shift amount Δφ calculated in this way is input to the phase correction amount calculation unit 116.

In the phase correction amount calculation unit 116, an appropriate phase correction amount is calculated so as to eliminate the phase shift amount Δφ. An appropriate phase correction amount is calculated, for example, according to the following arithmetic expression.
(Phase correction amount) = (P gain) × (phase shift amount Δφ) + (I gain) × Σ (phase shift amount Δφ)
This equation is an equation representing feedback control using PI control, and the P gain and the I gain are experimentally determined from the balance between variation and response, respectively. The phase correction amount calculated in this way is input to the replica C / A code generation unit 117.

  In the replica C / A code generation unit 117, the phase of the generated replica C / A code is corrected by the phase correction amount calculated by the phase correction amount calculation unit 116. That is, the tracking point of the replica C / A code is corrected. The replica C / A code generated in this way is input to the cross-correlation calculation units 111 and 112 via the phase advance unit 113 and the phase delay unit 114 as described above, and also to the pseudo distance calculation unit 118. In the cross-correlation calculators 111 and 112, the replica C / A code generated in this way is used for correlation value calculation for the IF digital signal input in the next observation cycle.

In the pseudo distance calculation unit 118, the pseudo distance ρ ′ _CA is calculated based on the phase information of the replica C / A code generated by the replica C / A code generation unit 117 using, for example, the following expression. In addition, as a meaning of a code, “′” added to the pseudo distance ρ _CA indicates that a filtering process described later is not executed, and “ _CA ” indicates a pseudo distance calculated based on the C / A code. Indicates ρ.
ρ ′ _CA = N _CA × 300
_{Here, N CA} is, C / A code corresponds to the number of bits, the phase and the reception of the replica C / A code generated by the C / A code replica generation unit 117 between the GPS satellite 10 ₁ and the vehicle 90 It is calculated based on the receiver clock inside the machine 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). A signal representing the pseudo distance ρ ′ _CA calculated in this way is input from the DDL 110 to the filter 130.

The PLL 120 measures the Doppler frequency Δf of the Doppler-shifted received carrier wave (received carrier) using a carrier replica signal generated inside. That is, the PLL 120 measures the Doppler frequency Δf (= fr−f _L1 ) based on the replica carrier frequency fr and the known carrier frequency f _L1 (1575.42 MHz). The digital IF signal input to the PLL 120 is obtained by multiplying the replica C / A code supplied from the DDL 110 by a mixer (not shown). A signal representing the Doppler frequency Δf from the PLL 120 is input to the Doppler velocity calculation unit 122 and the filter 130.

In the filter 130, the filter processing of the pseudo distance ρ ′ _CA is executed using the Doppler frequency Δf. In the filter 130, for example, the pseudo distance ρ _CA after the filter processing is calculated according to the following arithmetic expression.

ここで、（ｉ）は今回値を表し、（ｉ−１）は前回値を表し、Ｍは、重み係数である。Ｍの値は、精度と応答性を考慮しつつ適切に決定される。また、ΔＶは、ＧＰＳ衛星１０_１と車両９０との間の相対速度（ドップラ速度）である。フィルタ処理は、本分野で知られているキャリアスムージングと呼ばれる処理であってよく、上記のハッチフィルタの他、カルマンフィルタを用いても実現可能である。

Here, (i) represents the current value, (i-1) represents the previous value, and M is a weighting factor. The value of M is appropriately determined in consideration of accuracy and responsiveness. Further, [Delta] V is the relative velocity between the GPS satellite 10 ₁ and the vehicle 90 (Doppler velocity). The filter process may be a process called carrier smoothing known in the field, and can be realized by using a Kalman filter in addition to the hatch filter.

In Doppler velocity calculation unit 122, based on the Doppler frequency Δf that is input, the relative velocity (Doppler velocity) [Delta] V between the GPS satellite 10 ₁ and the vehicle 90 is, for example, using the following equation is calculated.
Δf = ΔV · f _L1 / (c−ΔV)
Note that c represents the speed of light.

Satellite position calculation unit 124, based on the satellite orbit information of the navigation message, the GPS satellite 10 _1, the current position in the world coordinate system _{r (SV) = (X SV} , Y SV, Z SV) and the moving speed V ( SV). Since the GPS satellite 101 is _one of artificial satellites, its movement is limited to a certain plane (orbital plane) including the center of gravity of the earth. Further, the trajectory of the GPS satellites 10 ₁ are elliptic motion to one focus global centroid, by sequentially numerical equations of Kepler, can be calculated GPS satellite 10 ₁ in position on the track surface. Furthermore, GPS satellites 10 ₁ position r (SV), in consideration of the fact that the GPS satellite 10 ₁ of the raceway surface and the equatorial plane of the world coordinate system is in rotational relationship, the position of the GPS satellites 10 ₁ in orbit plane Can be obtained by three-dimensional rotational coordinate transformation. As shown in FIG. 4, the world coordinate system is defined by an X axis and a Y axis that are orthogonal to each other within the equator plane, and a Z axis that is orthogonal to both axes, with the center of gravity as the origin.

  The satellite position r (SV) and the satellite moving speed V (SV) derived by the GPS receiver 20 are input to a pseudo-range estimation unit 80 described later.

Returning to FIG. For example, the inertial navigation positioning unit 60 applies an IMU (IMU) while applying an appropriate constraint using a vehicle model that receives an output value of an in-vehicle sensor such as a wheel speed sensor that detects a state quantity that can represent the behavior of the vehicle. Based on the output signal of an inertial measurement device comprising an acceleration sensor and a gyro sensor, the vehicle position, speed, and vehicle attitude (azimuth angle) are calculated. The vehicle model may be based on, for example, a Gaussian Markov model considering that the sensor bias or drift has a property of changing over time. The vehicle position positioning method by this kind of inertial navigation can be various and any method can 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 conversion formula for posture conversion is created using the value obtained by integrating the output value of the gyro sensor with correction taking the earth rotation into account and integrating the integrated value (yaw angle) and the previous value of the posture. May be. The vehicle position and speed obtained in this way are here called vehicle estimated position r (INS) = (X _INS , Y _INS , Z _INS ) and vehicle estimated speed V (INS), respectively. The estimated vehicle position r (INS) and estimated vehicle speed V (INS) are input to a pseudo-range estimation unit 80 described later.

The pseudo-range estimation unit 80 includes a satellite position r (SV) and a satellite moving speed V (SV) obtained from the GPS receiver 20, and a vehicle estimated position r (INS) and a vehicle estimated speed V obtained from the inertial navigation positioning unit 60. Based on (INS), the estimated pseudorange ρ _estimate and the estimated Doppler velocity ΔV _estimate are calculated by the following equations.
ρ _guess = √ {(X _INS −X _SV ) ² + (Y _INS −Y _SV ) ² + (Z _INS −Z _SV ) ² } + C _t
ΔV _guess = V (INS) −V (SV)
Here, C _t is an estimated value of a clock error included in a pseudo distance ρ _CA (or pseudo distance ρ ′ _CA before filtering, hereinafter referred to as a measured pseudo distance ρ) measured by the GPS receiver 20. , i.e., an estimate of the error between the receiver clock in the GPS receiver 20, the atomic clock mounted on a GPS satellite 10 _1. C _t is supplied from a state estimator 70 described later.

The estimated pseudorange ρ _estimation and estimated Doppler velocity ΔV _estimation derived by the pseudorange estimation unit 80 in this way are temporally related to the measured pseudorange ρ and Doppler velocity ΔV obtained from the GPS receiver 20, respectively. Differences are taken in a synchronized manner (for example, differences are taken between values related to the same GPS time). Then, these difference values are input to the state estimator 70 as the observation amount z.

The state estimator 70 estimates various state quantities η using a tight coupling type Kalman filter. The state equation is set as follows.
η (t _n ) = F · η (t _n−1 ) + G · u (t _n−1 ) + Γ · w (t _n−1 )
Here, η (t _n ) represents a state variable at time t = t _n , and u (t _n−1 ) and w (t _n−1 ) respectively represent time t = t _n−1. Are known inputs and disturbances (system noise: normal white noise). η (t _n ) is an error δr (INS) and δv (INS) of the estimated vehicle position r (INS) and estimated vehicle speed V (INS), and an error in the posture of the vehicle estimated by the inertial navigation positioning unit 60. δε (INS), gyro sensor bias error δb, acceleration sensor drift error δd, vehicle 90 tire radius error δs, and receiver clock error δC _t in the GPS receiver 20.

The observation equation is set as follows.
z (t _n ) = H (t _n ) · η (t _n ) + v (t _n )
As described above, the observation amount z is calculated by the estimated pseudorange ρ _estimation and the estimated Doppler velocity ΔV _estimated by the pseudorange estimation unit 80 and the measured pseudorange ρ and Doppler velocity ΔV derived by the GPS receiver 20. Are the difference values.

Here, the observation amount z is (ρ−ρ _estimation , ΔV−ΔV _estimation ), but various errors δr (INS) and δv (INS) are added to the vehicle estimated position r (INS) and the vehicle estimated speed V (INS). ), Δε (INS), δb, δd, and δs. In this case, for example, the observed amount related to the pseudorange is ρ−ρ _guess = √ {(X _INS + Σδ _X * −X _SV ) ² + (Y _INS + Σδ _Y * −Y _SV ) ² + (Z _INS + Σδ _Z * −Z the ^{_{SV) 2} + C t +}} δC t. _{(Δ X *, δ Y *} , δ Z *) is various error δr (INS), δv (INS ), δε (INS), δb, each error of X corresponding to δd and .delta.s, Y, and Z components To express. Thus, while the above state equation is linear, the observation quantity z is non-linear with respect to the state variable η (various errors), and thus the observation matrix H of the above observation equation is obtained by linearization.

The variance calculation unit 30 first has a function of calculating the variance σ ₁ ² of the measured pseudorange ρ derived by the GPS receiver 20. The variance σ ₁ ² may be calculated according to the following formula, for example.
σ ₁ ² = Σ (ρ ″ ₁ −ave (ρ ″ ₁ )) ² / (n−1) Equation (1)
In Expression (1), ρ ″ ₁ is obtained by subtracting the pseudo distance change amount for each observation period from the measured pseudo distance ρ. The pseudo distance change amount may be derived from the Doppler frequency Δf described above. ρ ″ ₁ ) represents an average of ρ ″ ₁ of the number of data n.

Alternatively, the variance value sigma ₁ ² measuring pseudorange ρ may for example be calculated according to the following equation.
σ ₁ ² = λ × √ (d ₁ / τ · C / N) Equation (2)
In Equation (2), C / N is the ratio of the carrier strength (power) to the noise strength (power). The values other than C / N are fixed values, and λ is the length of one bit of the C / A code, which is about 300 [m]. d ₁ is a phase difference (correlator interval) between the Early replica code and the Late replica code related to the replica C / A code used when the measurement pseudorange ρ is calculated. τ is a filter time constant in the filter and corresponds to the number of data.
In this case, there are various C / N calculation methods. For example, the C / N may be calculated according to the following equation (3).
^{C / N = {-α × L} 2 ( outbound radio field intensity of the GPS satellite 10 _1)} / (thermal noise level) formula (3)
This is a method of estimating C / N by a theoretical formula, and utilizes the feature that the thermal noise is basically always constant and the signal attenuates in proportion to the square of the distance. Here, the originating radio wave strength and the thermal noise level of the GPS satellite 10 ₁ is used a known value. L is the distance between the GPS satellite 10 ₁ and the vehicle 90, the position of the GPS satellite 10 _1, on the basis of the position of the vehicle 90 is calculated. Position of the GPS satellite 10 ₁ may information is used from the satellite position calculation unit 124, the position of the vehicle 90 may be a positioning result of the previous cycle in positioning operation unit 50, or the positioning result of the current cycle You may use it with feedback. Α is an attenuation coefficient. In Expression (2), it may be added a term accounts for the elevation of directional or GPS satellites 10 ₁ of the GPS antenna 21.
Alternatively, C / N may be calculated according to the following formula (4), for example.
C / N = log (E _CA + L _CA ) + β Formula (4)
This is represented by C / N = 10 log (Wc / Wn) using noise power Wn and signal power (carrier power) Wc, where Wc / Wn is an early correlation value E _CA and a late correlation value L. _This is in consideration of a proportional relationship with the sum of _CA (= E _CA + L _CA ). In Expression (3), β is an appropriate fixed value, and may be represented by −10 log Wn using, for example, a known noise power Wn. Since the Early correlation value E _CA and the Late correlation value L _CA vary due to thermal noise, variation occurs in the C / N obtained from Equation (4). Therefore, as shown in the following equation (5), it is possible to reduce the influence of variations in the correlation values E _CA and L _CA by performing filter (average) processing by accumulating data to some extent.
C / N = Σ (C / N ′) / n Formula (5)
C / N ′ is represented by C / N calculated by the above equation (4). n is the number of data (for example, when data is collected every 1 ms and data for 100 ms is used, n = 100). Note that the number of data n is incremented by 1 every observation period (for example, 1 ms) after setting the observation start to zero.
Alternatively, C / N may be calculated according to the following formula (6), for example.
C / N = (C / N _guess + n × C / N _{actual measurement} ) / (n + 1) Equation (6)
The C / N _estimation represents the C / N calculated by the above formula (3), and the C / N _{actual measurement} represents the C / N calculated by the above formula (4) or (5). In this equation (6), as the number of data n increases, a larger weight is given to the C / N _{actual measurement} . That is, immediately after the start of observation, a relatively large weight is given to the C / N _estimation in consideration of variations in C / N _{actual measurement} due to the small amount of actual measurement data. A relatively large weight is given to the C / N _{actual measurement} whose reliability has increased with the increase.

The variance calculation unit 30 further has a function of calculating the variance σ _estimation ² of the estimated pseudo distance ρ _estimation derived by the pseudo distance estimation unit 80.

Since the variance σ _guess ² is ρ _guess = √ {(X _INS −X _SV ) ² + (Y _INS −Y _SV ) ² + (Z _INS −Z _SV ) ² } + C _t, it is related to the satellite orbit information. It is calculated from the variance of the satellite position, the variance of the estimated vehicle position r (INS) derived by the inertial navigation positioning unit 60, and the variance of the clock error of the GPS receiver 20.

The variance σ _estimation ² is a variance σ _f calculated from the variance of the satellite position according to the satellite orbit information and the variance of the vehicle estimated position r (INS) derived by the inertial navigation positioning unit 60 according to the propagation law of variance. ² may be calculated by adding the variance σ _Ct ² of the clock error of the GPS receiver 20. In other words, the variance σ _guess ² = σ _f ² + σ _Ct ² may be calculated.

The variance σ _f ² may be calculated as follows according to the dispersion propagation law.

数２において、ｆ＝√｛（Ｘ_ＩＮＳ−Ｘ_ＳＶ）^２＋（Ｙ_ＩＮＳ−Ｙ_ＳＶ）^２＋（Ｚ_ＩＮＳ−Ｚ_ＳＶ）^２｝である。また、（Ｘ_ＩＮＳ０、Ｙ_ＩＮＳ０、Ｚ_ＩＮＳ０）は、前回周期（１サンプル前）の値を表し、（Ｘ_ＳＶ０、Ｙ_ＳＶ０、Ｚ_ＳＶ０）は、前回周期（１サンプル前）の値を表す。但し、今回周期の各値が用いられてもよい。また、（σ_ＸＩＮＳ ^２、σ_ＹＩＮＳ ^２、σ_ＺＩＮＳ ^２）は、それぞれ、車両推定位置ｒ（ＩＮＳ）のＸ，Ｙ，Ｚ成分の分散を表し、状態推定器７０で導出される誤差共分散行列Ｐにおける対応する対角成分の値が用いられる。（σ_ＸＳＶ ^２、σ_ＹＳＶ ^２、σ_ＺＳＶ ^２）は、それぞれ、衛星位置ｒ（ＳＶ）のＸ，Ｙ，Ｚ成分の分散を表す。（σ_ＸＳＶ ^２、σ_ＹＳＶ ^２、σ_ＺＳＶ ^２）は、過去に得られた衛星位置の分散値を考慮して設定されてもよいし、予め適切な固定値として適宜設定するようにしてもよい。固定値の場合、（σ_ＸＳＶ ^２、σ_ＹＳＶ ^２、σ_ＺＳＶ ^２）は、例えばそれぞれ±10程度であってよい。

In Equation 2, f = √ {(X _INS −X _SV ) ² + (Y _INS −Y _SV ) ² + (Z _INS −Z _SV ) ² }. Further, (X _INS0 , Y _INS0 , Z _INS0 ) represents the value of the previous cycle (one sample before), and (X _SV0 , Y _SV0 , Z _SV0 ) represents the value of the previous cycle (one sample before). However, each value of the current cycle may be used. Also, (σ _XINS ² , σ _YINS ² , σ _ZINS ² ) represents the variance of the X, Y, and Z components of the vehicle estimated position r (INS), and the error covariance matrix derived by the state estimator 70. The value of the corresponding diagonal component in P is used. (Σ _XSV ² , σ _YSV ² , σ _ZSV ² ) represent the variance of the X, Y, and Z components of the satellite position r (SV), respectively. (Σ _XSV ² , σ _YSV ² , σ _ZSV ² ) may be set in consideration of the dispersion value of the satellite position obtained in the past, or may be set appropriately as an appropriate fixed value in advance. . In the case of fixed values, (σ _XSV ² , σ _YSV ² , σ _ZSV ² ) may be about ± 10, for example.

For the clock error variance σ _Ct ² of the GPS receiver 20, the value of the corresponding diagonal component in the error covariance matrix P derived by the state estimator 70 is used.

The pseudo distance combining unit 40 combines the measured pseudo distance ρ and the estimated pseudo distance ρ _estimation related to the same GPS time to calculate one pseudo distance ρ _COM (hereinafter referred to as “combined pseudo distance”). The pseudo-range coupling unit 40 may calculate the coupling pseudo-range ρ _COM by the following formula, for example.
ρ _COM = (ρ + ρ _guess ) / 2
That, coupled pseudorange [rho _COM may be calculated by averaging the guess pseudorange [rho _presumed measured pseudorange [rho. However, preferably, the pseudo-range coupling unit 40 uses the variance sigma ₁ ² and inferred pseudorange ρ _guess variance sigma _guess ² measuring pseudorange ρ obtained from the variance calculation unit 30, for example according to the following formula The combined pseudo distance ρ _COM is calculated by combining the first pseudo distance ρ and the second pseudo distance ρ.
ρ _COM = (σ _guess ^2m × ρ + σ ₁₂ ^m × ρ _guess ) / (σ _guess ^2m + σ ₁₂ ^m )
Here, the superscript “ ^m ” represents a factorial, and an appropriate value of positive integers (m = 1, 2,...) Is used. For example, m = 1 may be set. In this case, the combined pseudorange ρ _COM is calculated according to the following equation.
ρ _COM = (σ _guess ² × ρ + σ ₁ ² × ρ _guess ) / (σ _guess ² + σ ₁ ² )
The combined pseudo distance ρ _COM calculated by the pseudo distance combining unit 40 in this way is input to the positioning calculation unit 50.

The positioning calculation unit 50 determines the position (X _u , Y) of the vehicle 90 based on the satellite position r (SV) obtained from the satellite position calculation unit 124 and the combined pseudorange ρ _COM supplied from the pseudorange combining unit 40. Position _u , _Zu ). The positioning result may be output to a navigation system, for example. The position of the vehicle 90 may be derived on the principle of triangulation using the respective combined pseudoranges ρ _COM and satellite positions r (SV) obtained for the three GPS satellites 10. In this case, the binding pseudorange [rho _COM because it contains clock error as described above, by using the fourth coupling pseudoranges obtained for GPS satellites 10 [rho _COM and satellite position r (SV), the clock error component May be removed. Alternatively, using the combined pseudorange ρ _COM and satellite position r (SV) obtained for five or more GPS satellites 10, the final vehicle 90 position (X _u , Y _u ) using a statistical algorithm. , Z _u ) may be determined. In addition, the positioning method of the position of the vehicle 90 is not limited to the single positioning as described above, 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 90 is measured using the single phase difference or double phase difference of the pseudo distances obtained by the fixed station and the vehicle 90 as described above.

Next, main operations of the mobile positioning apparatus 1 of the present embodiment described above will be described with reference to FIG. The process shown in FIG. 5 is repeatedly executed at every predetermined period, it will be described here period according to the time t _n as a current cycle.

  In step 300, the output value of the IMU (eg, acceleration in three axes and yaw about three axes) is sampled. The value of the IMU may be corrected based on an error calculated by the state estimator 70 in the previous cycle of step 350 described later. That is, the respective errors may be corrected based on the estimated values of the bias error δb of the gyro sensor and the drift error δd of the acceleration sensor. Also, the scaling factor (vehicle model) of the wheel speed sensor may be corrected based on the estimated value of the tire radius error δs derived by the state estimator 70 in the previous cycle of step 350 described later.

In step 310, the inertial navigation positioning unit 60 derives the estimated vehicle position r (INS), estimated vehicle speed V (INS), and the like based on the output value of the IMU obtained in step 300. The estimated vehicle position r (INS) and the like are used by the pseudo distance estimation unit 80 to calculate the estimated pseudo distance ρ _estimation (t _n ).

In step 320, time update of the Kalman filter is performed in the state estimator 70. For example, the time update of the Kalman filter is expressed as follows.
η (t _n ) ⁽⁻⁾ = η (t _n−1 ) ⁽⁺⁾ + u (t _n−1 )
P (t _n ) ⁽⁻⁾ = F · P (t _n−1 ) ⁽⁺⁾ · F ^T + Γ · Q (t _n−1 ) · Γ ^T
P is a covariance matrix of prediction / estimation errors, and Q is a covariance matrix (positive definite symmetric matrix) of disturbance w.

  In steps 330 to 360, the Kalman filter observation update and error estimation processing are executed in the state estimator 70. Specifically, it is as follows.

In step 330, the estimated pseudorange ρ _estimation (t _n ) is calculated by the pseudorange estimation unit 80 for each of the currently visible (observable) GPS satellites 10, and at the GPS receiver 20. A measurement pseudorange ρ (t _n ) is measured. At this time, the clock error C _t used for calculation of the estimated pseudo distance ρ _estimation (t _n ) derived by the pseudo distance estimation unit 80 is an error derived by the state estimator 70 in the previous cycle of step 350 described later. based on the estimated value of .delta.C _t, it may be corrected. Then, based on these measurement pseudorange ρ and guess pseudorange ρ _guess, as described above observed quantity z (t _n) is derived. In step 330, an observation matrix H (t _n ) corresponding to the currently visible GPS satellite 10 is calculated. Note that the observation equation (that is, the observation matrix H (t _n )) changes according to the number of GPS satellites 10 currently visible.

In step 340, the Kalman gain K (t _n ) is calculated as follows.
^{_{K (t n) = P (}} t n) (-) · H T (t n) · (H (t n) · P (t n) (-) · H T (t n) + R (t n)) ^-1
R is a covariance matrix (positive definite symmetric matrix) of observation noise (normal white noise) v.

In step 350, based on the Kalman gain K (t _n ) obtained in step 340, the estimation error δη (t _n ) is calculated as follows.
δη (t _n ) = K (t _n ) · z (t _n )
The estimated error δη (t _n ) of the state quantity η derived in this way is used for error correction in the above steps 310 and 330 in the next cycle (t _{n + 1} ).

In step 360, the covariance matrix P is updated as follows based on the Kalman gain K (t _n ) obtained in step 340 above.
P (t _n ) ⁽⁺⁾ = P (t _n ) ⁽⁻⁾ −K (t _n ) · H (t _n ) · P (t _n ) ⁽⁻⁾
In step 370, the variance σ ₁ ² (t _n ) and σ _guess ² (t _n ) are calculated in the variance calculation unit 30. For the calculation of the variance σ _estimation ² (t _n ), the value of the corresponding diagonal component in P (t _n ) ⁽⁺⁾ derived in step 360 described above and a predetermined fixed value are used.

In step 380, measured in the pseudo-range coupling portion 40 pseudorange ρ _{(t n)} and dead pseudorange [rho _{guess (t n)} is coupled with coupled pseudorange ρ _{COM (t n)} is calculated. The variance σ ₁ ² (t _n ) and σ _guess ² (t _n ) derived in step 370 are used for the combination of the measured pseudo distance ρ (t _n ) and the estimated pseudo distance ρ _guess (t _n ). .

In step 390, the position of the vehicle 90 is measured by the positioning calculation unit 50. For this positioning, the combined pseudorange ρ _COM (t _n ) derived in step 380 is used.

  According to the mobile positioning apparatus 1 of the present embodiment described above, the following advantageous effects can be obtained.

As described above, in this embodiment, two pseudoranges (measured pseudorange ρ, estimated pseudorange ρ _guess ) for one GPS satellite 10 are calculated from the observation data of the C / A code in one observation period. since, compared to one of the GPS satellites 10 to the configuration for calculating only one pseudorange at one monitoring period (e.g. measured pseudorange [rho only), to converge the variation in the pseudo range (binding pseudorange [rho _COM) early be able to.

Further, by combining two pseudo distances (measured pseudo distance ρ, estimated pseudo distance ρ _estimated ) using respective variances σ ₁ ² and σ _estimated ² (an index indicating the degree of variation), the combined pseudo distance ρ _A highly accurate coupled pseudorange ρ _COM can be obtained while converging _COM variations at an early stage. Furthermore, speculation pseudorange ρ variance σ _guess ² _guess, since it is derived using the diagonal component of the covariance matrix P is derived in a tight coupling type Kalman filter, guess pseudorange ρ _inferred variance σ _guess ² can be calculated with high accuracy.

Further, by obtaining the estimated pseudo distance ρ _estimation and the variance σ _estimation ² from the variance calculating unit 30 and the pseudo distance estimating unit 80, the estimated pseudo distance ρ can be used as a parameter for narrowing down the pseudo distance in the receiver. It is also possible to carry out.

  Further, in the state estimator 70, a tight coupling type Kalman filter that takes into account the tire radial error δs of the vehicle 90 is configured to use a highly accurate vehicle model that can compensate for the error factor of the wheel speed sensor. In addition, the positioning accuracy of the inertial navigation positioning unit 60 and thus the positioning accuracy of the positioning calculation unit 50 can be increased.

  The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the above-described embodiments, and various modifications and substitutions can be made to the above-described embodiments without departing from the scope of the present invention. Can be added.

  For example, in the above-described embodiment, the positioning for the moving body is performed by adding the variance calculation unit 30, the pseudo-range coupling unit 40, and the positioning calculation unit 50 to the tight coupling type positioning system 5 including the existing GPS receiver 20. Although the device 1 is realized, the present invention is not limited to this, and the GPS receiver 20 includes all or part of the variance calculation unit 30, the pseudorange combination unit 40, and the positioning calculation unit 50 in software and / or Alternatively, it may be incorporated in hardware.

In the above-described embodiment, the measurement pseudo distance ρ is derived using the C / A code. However, the measurement pseudo distance ρ may be derived based on the P code of the L2 wave. In the case of a P code, since it is encrypted with a W code, when performing P code synchronization, the P code may be extracted by a DLL using a cross correlation method. Pseudorange [rho based on P-code _'P, by M _P bit of P code to measure whether it is received by the vehicle 90 as P code GPS satellite 10 ₁ is bit 0, [rho' _P = _MP * 30.

  From the same viewpoint, in the above-described embodiment, instead of the measurement pseudo distance ρ based on the C / A code, a road sign using an in-vehicle camera or the like (for example, a rhombus paint in front of the intersection) An estimated pseudorange that can be derived based on an image recognition result of a known sign or the like, an estimated pseudorange that can be derived based on a vehicle position obtained through road-to-vehicle communication or vehicle-to-vehicle communication, or the like may be used.

Further, in the above embodiment, two pseudo-range (measured pseudorange [rho, guess pseudorange [rho _guess) is bonded to a calculates the binding pseudorange [rho _COM, 3 pieces containing guess pseudorange [rho _guess The combined pseudo distance ρ _COM may be calculated by combining the above pseudo distances. In this case, as other pseudo distances, a measurement pseudo distance ρ ′ _P based on the P code, a measurement pseudo distance based on the C / A code obtained by a different tracking method, an image recognition result such as a road sign using an in-vehicle camera, etc. The estimated pseudo distance that can be derived based on the vehicle position, the estimated pseudo distance that can be derived based on the vehicle position obtained through road-to-vehicle communication or vehicle-to-vehicle communication, and the like can be considered. In this case, as a combination mode, one pseudo distance may be combined with another pseudo distance one by one, or all pseudo distances may be combined at once.

  In the above-described embodiment, an example in which the present invention is applied to the GPS has been shown. However, the present invention can also be applied to a satellite system below the GPS, for example, another GNSS (Global Navigation Satellite System) such as Galileo. is there.

1 is a system configuration diagram showing an overall configuration of a GPS to which a mobile positioning device according to the present invention is applied. It is a main functional block diagram in one Example of the positioning device 1 for moving bodies by this invention. 2 is a diagram illustrating an example of an internal configuration of a GPS receiver 20. FIG. It is a figure which shows the relationship between a world coordinate system and a local coordinate system, and the relationship between a local coordinate system and a body coordinate. It is a flowchart which shows the main operation | movement of the positioning apparatus 1 for a mobile body of a present Example.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Positioning apparatus for moving bodies 10 GPS satellite 20 GPS receiver 21 GPS antenna 22 High frequency circuit 24 A / D conversion circuit 30 Dispersion calculation part 40 Pseudo distance coupling part 50 Positioning calculation part 60 Inertial navigation positioning part 70 State estimator 80 Pseudo distance Guess part 110 DDL
120 PLL
130 Filter

Claims (4)

In a mobile positioning device that is mounted on a mobile body and measures the position of the mobile body,
Inertial navigation positioning means for calculating the position of the moving body using inertial navigation;
A pseudo distance measuring means for measuring a pseudo distance between the satellite and the mobile body based on the code information carried on the radio wave from the satellite and the time information of the receiver clock;
The pseudo distance measured by the pseudo distance measuring means is calculated by calculating a satellite position based on satellite orbit information, a calculated position of the moving body by the inertial navigation positioning means, and a clock error of the receiver clock. A pseudo-range estimation means for estimating based on the estimated value;
Estimated by the pseudo-range estimating means based on the dispersion of the position of the satellite related to the satellite orbit information, the dispersion of the position of the moving object calculated by the inertial navigation positioning means, and the variance of the clock error. Dispersion calculating means for calculating the variance of the pseudo distance, and a mobile positioning device. A combined pseudo distance calculating means for calculating a combined pseudo distance by combining the pseudo distance measured by the pseudo distance measuring means and the pseudo distance estimated by the pseudo distance estimating means;
A positioning calculation means for positioning a moving body position using the combined pseudo distance calculated by the combined pseudo distance calculation means;
2. The mobile positioning apparatus according to claim 1, wherein the combined pseudo distance calculating unit performs the pseudo distance combining process using the variance calculated by the variance value calculating unit.   The positioning device for a moving body according to claim 1, wherein the pseudo distance stage estimated by the pseudo distance estimating means and the pseudo distance measured by the pseudo distance measuring means are input to a tight coupling type Kalman filter.   The variance calculating means includes a variance of the position of the moving body calculated by the inertial navigation positioning means based on a diagonal component of an error covariance matrix derived by the tight coupling type Kalman filter, and the clock error. The positioning device for a moving body according to claim 3, wherein the variance of the moving body is calculated.

Images