JP2009025045A - Positioning device for moving body - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To prevent properly a time delay in the main moving direction of a moving body, while suppressing dispersion of positioning results caused by randomness of positioning. <P>SOLUTION: This positioning device for the moving body is equipped with a satellite signal reception means, a velocity vector calculation means for calculating a velocity vector of the moving body by using a Doppler shift of a carrier wave of a satellite signal, a moving vector calculation means for calculating a moving vector of the moving body based on a position change of the moving body derived based on a reception result of the satellite signal, a correction means for correcting the moving vector calculated by the moving vector calculation means by using the direction of the velocity vector calculated by the velocity vector calculation means as a reference direction, and a positioning means for positioning the position and/or the velocity of the moving body by using the moving vector corrected by the correction means and the velocity vector calculated by the velocity vector calculation means. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a mobile body positioning apparatus that measures the position of a mobile body and the like.

Conventionally, receiving means for receiving a GPS signal for measuring the absolute position of the moving body, and current position of the moving body by calculating positioning information of the moving body based on the GPS signal received by the receiving means In a navigation device comprising a recognizing means for recognizing the position, a vertical speed calculating means for calculating a vertical speed of the moving body based on the positioning information received by the receiving means, and a calculation by the vertical speed calculating means. A vertical movement amount calculating means for calculating a vertical movement amount of the moving body by integrating the vertical speeds, and a horizontal movement amount calculating means for calculating a horizontal movement amount of the moving body; Map data acquisition means for acquiring map data having shape information of a multi-layered road in which a plurality of traveling roads are three-dimensionally overlapped, There is known a navigation device comprising: a determination unit that determines a travel path of the moving body in the map data based on a horizontal movement amount and a vertical movement amount (for example, Patent Document 1).
JP 2002-206934 A

  By the way, in positioning such as the position of a mobile body (typically a vehicle) using a satellite system such as GPS, the positioning results during the movement of the mobile body vary due to the randomness of the positioning. The movement becomes an unnatural trajectory. For this reason, in this type of positioning, generally, variation in positioning results is suppressed by performing a smoothing process in the time axis direction using a filter or the like. However, when smoothing processing using a filter or the like is performed, the positioning result becomes smooth, but on the other hand, the time delay increases, and the main moving direction of the moving body (for example, when the moving body is a vehicle, Positioning errors in the front-rear direction (that is, the traveling direction of the vehicle) may increase.

  Therefore, an object of the present invention is to provide a positioning device for a moving body that can appropriately prevent a time delay in the main moving direction of the moving body while suppressing variations in positioning results due to randomness of positioning.

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 and / or speed of the moving body.
Receiving means for receiving satellite signals broadcast from the satellite;
Velocity vector calculating means for calculating a velocity vector of the moving body using a Doppler shift of a carrier wave of the satellite signal;
Movement vector calculation means for calculating a movement vector of the moving body based on a change in the position of the moving body derived based on the reception result of the satellite signal;
Correction means for correcting the movement vector calculated by the movement vector calculation means using the direction of the velocity vector calculated by the speed vector calculation means as a reference direction;
It comprises positioning means for positioning the position and / or speed of the moving body using the movement vector corrected by the correcting means and the velocity vector calculated by the velocity vector calculating means.

2nd invention is the positioning apparatus for moving bodies which concerns on 1st invention,
The velocity vector calculating means includes
A pre-filtering speed vector calculating means for calculating a pre-filtering speed vector of the mobile body using a Doppler shift of a carrier wave of the satellite signal;
Filtering means for filtering the speed vector before filtering calculated by the speed vector calculating means in a time axis direction to calculate the speed vector;
The positioning means includes
An inner product velocity vector is calculated by taking an inner product of the velocity vector before filtering calculated by the velocity vector calculating unit and a unit vector of the velocity vector calculated by the filtering unit, and multiplying the inner vector by the unit vector of the velocity vector. And calculating the position and / or speed of the moving body based on the inner product speed vector calculated by the inner product means and the movement vector corrected by the correction means. To do.

3rd invention is the positioning apparatus for moving bodies which concerns on 1st or 2nd invention,
The correction means takes an inner product of a unit vector of the speed vector calculated by the speed vector calculation means and a movement vector calculated by the movement vector calculation means, and multiplies the inner product by the unit vector of the speed vector. The obtained velocity vector is derived as the corrected movement vector.

4th invention is the positioning apparatus for mobile bodies which concerns on any one of 1st-3rd invention,
The movement vector calculation means includes moving body position means for deriving the position of the moving body based on a pseudo distance between the satellite and the moving body derived based on the pseudo noise code of the satellite signal, Based on a change between the position of the mobile body derived by the mobile body position means in a period and the position of the mobile body measured by the positioning means in a period before the predetermined period, the movement vector is It is characterized by calculating.

  ADVANTAGE OF THE INVENTION According to this invention, the positioning apparatus for moving bodies which can prevent appropriately the time delay in the main moving direction of a moving body is obtained, suppressing the dispersion | variation in the positioning result resulting from the randomness of positioning.

  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 body 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. The vehicle 90 is merely an example of a moving body, and other moving bodies include motorcycles, railways, ships, airplanes, hawk lifts, robots, information terminals such as mobile phones that move with the movement of people, and the like. There can be.

  The GPS satellite 10 constantly broadcasts navigation messages (satellite signals) 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 code strings in which -1 and 1 are irregularly arranged periodically.

  Currently, 31 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-orbiting orbits 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 equipped with a GPS receiver 20 as a mobile body positioning device.

FIG. 2 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 DLL (Delay-Locked Loop) 110, a PLL (Phase-Locked Loop) 120, and a satellite position calculation unit. 124 and the positioning unit 50. The DLL 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 DLL 110, the PLL 120, and the like.

The replica C / A code generation unit 117 of the DLL 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 θ ₁ .

  In addition, 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 ρ ₁ is calculated based on, for example, the following formula based on the phase information of the replica C / A code generated by the replica C / A code generation unit 117. As a meaning of the code, the subscript “ ₁ ” indicates that the pseudo distance ρ is calculated based on the C / A code related to the GPS satellite 10 ₁ .
ρ ₁ = N ₁ × 300
Here, _{N 1} 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 ρ ₁ calculated in this way is input from the DLL 110 to the positioning unit 50.

In the PLL 120, the Doppler frequency Δf _{1 of} the Doppler-shifted received carrier wave (received carrier) is measured using a carrier replica signal generated inside. In other words, 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 DLL 110 by a mixer (not shown). A signal representing the Doppler frequency Δf ₁ from the PLL 120 is input to the positioning unit 50.

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 (see FIG. _{9) S 1 = (X 1} , Y 1, Z 1) and the mobile Calculate the velocity V ₁ = (V _x1 , V _y1 , V _z1 ). The satellite moving velocity vector V ₁ = (V _x1 , V _y1 , V _z1 ) may be calculated by dividing the difference between the calculated current value and the previous value of the satellite position S ₁ by the time width of the calculation cycle. The satellite position S ₁ and the satellite moving speed vector V ₁ derived by the satellite position calculation unit 124 in this way are input to the positioning unit 50.

  Next, the details of the positioning unit 50 according to the present embodiment will be described with reference to FIG. 3 and subsequent drawings.

  FIG. 3 is a flowchart illustrating an example of main processing executed by the positioning unit 50 according to the present embodiment.

In step 200, based on the observation data of the C / A code acquired in the cycle corresponding to the current cycle (i), the position of the vehicle 90 in the current cycle (i) (X ′ _u (i), Y ′ _u ( i), the positioning calculation of Z ′ _u (i)) is executed. The positioning calculation may be executed based on the following relational expression, for example.

ここで、下付き文字「ｋ」は、ＧＰＳ衛星１０_ｋに係る値を示す(以下も同様)。ここでは、ρ_ｋは、ＧＰＳ衛星１０_ｋに係る擬似距離を表し、ＤＬＬ１１０から出力される値が用いられる。（Ｘ_ｋ（ｉ），Ｙ_ｋ（ｉ），Ｚ_ｋ（ｉ））は、同ＧＰＳ衛星１０_ｋに係る衛星位置を表し、衛星位置算出部１２４から出力される値が用いられる。また、（Ｘ’_ｕ（ｉ），Ｙ’_ｕ（ｉ），Ｚ’_ｕ（ｉ））は、今回周期（ｉ）の車両９０の位置を表し、未知数である。また、ｃ・ΔＴは、ＧＰＳ受信機２０における時計誤差を表し、未知数である。この場合、例えば現在観測可能なＧＰＳ衛星１０_ｋの数が４つである場合には、数１の式が４つ立つので、最小二乗法等を用いて時計誤差ｃ・ΔＴを除去した測位が実現される。

Here, the subscript “k” indicates a value related to the GPS satellite 10 _k (and so on). Here, ρ _k represents a pseudorange related to the GPS satellite 10 _k , and a value output from the DLL 110 is used. (X _k (i), Y _k (i), Z _k (i)) represents a satellite position related to the GPS satellite 10 _k , and a value output from the satellite position calculation unit 124 is used. Further, (X ′ _u (i), Y ′ _u (i), Z ′ _u (i)) represents the position of the vehicle 90 in the current cycle (i) and is an unknown number. C · ΔT represents a clock error in the GPS receiver 20 and is an unknown number. In this case, for example, when the number of GPS satellites 10 _{k that} can be observed is four, since the formula 1 is four, the positioning with the clock error c · ΔT removed using the least squares method or the like is possible. Realized.

In step 202, the position of the vehicle 90 that is positioning in the previous cycle in step 212 described later _{(X u (i-1)} , Y u (i-1), Z u (i-1)) and, in step 200 Thus, the difference vector from the position (X ′ _u (i), Y ′ _u (i), Z ′ _u (i)) of the vehicle 90 obtained in the current cycle is calculated. That is, the difference vector = (X ′ _u (i) −X _u (i−1), Y ′ _u (i) −Y _u (i−1), Z ′ _u (i) −Z _u (i−1) ) Is calculated. This difference vector represents the movement vector of the vehicle 90 between the current cycle and the previous cycle.

In step 204, using the Doppler frequency Δf _k , the speed vector v = (v _x (i−1), v _y (i−1), v _z (i−1) of the vehicle 90 in the previous period (i−1). )) Is positioned. The positioning of the speed of the vehicle 90 may be executed using a least square method or the like based on the following relational expression, for example. The symbol black circle on the letter represents a dot (time differentiation), for example, the Doppler range dρ _k is ρ _k dot (time differentiation of the pseudorange ρ _k ).

尚、Ｉドット及びＴドットは、電離層誤差の変動量及び対流圏誤差の変動量を表すが、非常に小さいので、ここでは、白色ノイズεとして扱う。また、ｂドットは、時計誤差の微分値である。また、（Ｖ_ｋ−ｖ）・ｌ_ｋのＶ_ｋ・ｌ_ｋの部分は、前回周期（ｉ−１）における単位ベクトルｌ_ｋ（ｉ−１）と衛星移動速度ベクトルＶ_ｋ（ｉ−１）との内積であり、衛星移動速度ベクトルＶ_ｋ（ｉ−１）は、上述の如く衛星位置算出部１２４にて航法メッセージの衛星軌道情報に基づいて算出され、単位ベクトルｌ_ｋ（ｉ−１）は、前回周期（ｉ−１）において測位部５０により測位された車両９０の位置（Ｘ_ｕ（ｉ−１），Ｙ_ｕ（ｉ−１），Ｚ_ｕ（ｉ−１））、及び、前回周期（ｉ−１）において衛星位置算出部１２４により算出される衛星位置（Ｘ_ｋ（ｉ−１），Ｙ_ｋ（ｉ−１），Ｚ_ｋ（ｉ−１））を用いて、以下のように、算出されてよい。

The I dot and T dot represent the amount of variation in ionospheric error and the amount of variation in tropospheric error, but are very small and are treated as white noise ε here. The b dot is a differential value of the clock error. In addition, the portion of (V _k −v) · l _k in V _k · l _k includes the unit vector l _k (i−1) and the satellite moving velocity vector V _k (i−1) in the previous period (i−1). The satellite moving velocity vector V _k (i−1) is calculated based on the satellite orbit information of the navigation message by the satellite position calculating unit 124 as described above, and the unit vector l _k (i−1). the position of the vehicle 90 is positioning by the positioning unit 50 in the previous period _{(i-1) (X u} (i-1), Y u (i-1), Z u (i-1)), and the previous Using the satellite positions (X _k (i−1), Y _k (i−1), Z _k (i−1)) calculated by the satellite position calculation unit 124 in the period (i−1), the following is performed. Or may be calculated.

また、ドップラレンジｄρ_ｋ（ｉ−１）は、搬送波の波長λ（既知）と、前回周期（ｉ−１）で得られるＧＰＳ衛星１０_ｋに関するドップラ周波数Δｆ_ｋ（ｉ−１）を用いて、例えばｄρ_ｋ（ｉ−１）＝λ・Δｆ_ｋ（ｉ−１）により、算出される。

Further, the Doppler range dρ _k (i−1) is obtained by using the wavelength λ (known) of the carrier wave and the Doppler frequency Δf _k (i−1) related to the GPS satellite 10 _k obtained in the previous period (i−1), For example, it is calculated by dρ _k (i−1) = λ · Δf _k (i−1).

  In step 206, the smoothing process of the speed vector v of the vehicle 90 calculated in the above step 204 is executed. This smoothing process is realized by performing filtering on the time axis. For example, the smoothed velocity vector v ′ (hereinafter referred to as “smoothed velocity vector v ′”) may be calculated as follows.

ここで、Ｔ_１、Ｔ_２及びＴ_３は、適切なフィルタ定数である。尚、平滑化処理は、他のフィルタが用いられてもよい。

Here, T ₁ , T ₂ and T ₃ are suitable filter constants. For the smoothing process, other filters may be used.

  In step 208, the difference vector obtained in step 202 is corrected by taking the inner product of the difference vector obtained in step 202 and the smoothed velocity vector v 'obtained in step 206. That is, the difference vector obtained in step 202 is corrected using the direction of the smoothed velocity vector v ′ obtained in step 206 as a reference direction. Specifically, a corrected difference vector P (hereinafter referred to as “corrected difference vector P”) is calculated as follows.

ここで、ベクトルＰ_０は、上記のステップ２０２で得られる差分ベクトルを表し、ベクトルｖ’は、平滑化速度ベクトルを表す。図４は、この補正差分ベクトルＰの幾何学的関係を示す。

Here, the vector P ₀ represents the difference vector obtained in the above step 202, and the vector v ′ represents the smoothing speed vector. FIG. 4 shows the geometric relationship of the correction difference vector P.

  In step 210, the speed vector v of the vehicle 90 is corrected by taking the inner product of the speed vector v of the vehicle 90 calculated in step 204 and the smoothed speed vector v ′ obtained in step 206. . That is, the speed vector v of the vehicle 90 calculated in step 204 is corrected using the direction of the smoothed speed vector v ′ obtained in step 206 as a reference direction. Specifically, a corrected velocity vector v ″ (hereinafter referred to as “corrected velocity vector v ″”) is calculated as follows.

図５は、この補正速度ベクトルｖ”の幾何学的関係を示す。

FIG. 5 shows the geometric relationship of this corrected velocity vector v ″.

In step 212, the correction difference vector P calculated in step 208 is corrected by the correction speed vector v '' calculated in step 210. In other words, the correction speed vector v calculated in step 210 is corrected. and ", by using the corrected differential vector P is calculated in step 208, the position of the vehicle 90 in the current cycle positioning results of _{(X u (i), Y} u (i), Z u (i)) is Derived. For example, the position of the vehicle 90 in the current cycle position vector _u representing the _{(X u (i), Y} u (i), Z u (i)) (i) = (X u (i), Y u (i), Z _u (i)) may be calculated as follows.

ここで、Δｔは、測位演算周期であり、速度ベクトルｖの演算周期に対応する。尚、測位演算周期Δｔは、擬似距離ρ_ｋの演算周期（観測周期）に一致してもよく、或いは、擬似距離ρ_ｋの演算周期の所定の整数倍に対応してもよい。また、ｍ、ｎは、適切な重み付け係数であり、例えば双方共に１であってもよい。ｍ及びｎの双方が共に１の場合、補正速度ベクトルｖ”と補正差分ベクトルＰに基づく速度ベクトルとが平均化されることになる。尚、図６は、このステップ２１２の処理態様を図示する。尚、図６の例では、補正差分ベクトルＰの終点位置よりも手前の位置（内分位置）に、今回周期の車両９０の位置の測位結果が算出されている。

Here, Δt is a positioning calculation cycle and corresponds to the calculation cycle of the velocity vector v. Incidentally, the positioning operation period Δt may be consistent with the operation cycle of the pseudo range [rho _k (observation period), or may correspond to a predetermined integer multiple of the operation cycle of the pseudo-range [rho _k. M and n are appropriate weighting factors, and both may be 1, for example. When both m and n are 1, the corrected velocity vector v ″ and the velocity vector based on the corrected difference vector P are averaged. FIG. 6 illustrates the processing mode of this step 212. 6, the positioning result of the position of the vehicle 90 in the current cycle is calculated at a position (internal division position) before the end point position of the correction difference vector P.

The position (X _u (i), Y _u (i), Z _u (i)) (that is, the position vector u (i)) of the vehicle 90 in the current cycle obtained in this way is the vehicle in the current cycle. The final positioning result at the 90 position may be supplied to a navigation device (not shown), for example. Moreover, in this way the position of the vehicle 90 in the current cycle obtained _{(X u (i), Y} u (i), Z u (i)) is used in step 202 such as the value of the previous cycle in the next cycle It will be.

In this step 212, the speed (v _x (i), v _y (i), v _z (i)) of the vehicle 90 in the current cycle is obtained as described above by the position (X _u (i), Y _u (i), Z _u (i)), the satellite position (X _k (i), Y _k (i), Z _k (i)) and the Doppler range dρ _k (i) of the current cycle. ) Using the least square method or the like from the above equation (2).

  According to the positioning apparatus for a moving body according to the present embodiment described above, the following excellent effects are obtained.

As described above, according to the present embodiment, the difference vector based on the observation data of the C / A code is corrected (smoothed) with the direction of the smoothed velocity vector v ′ obtained from the Doppler frequency Δf _k as the reference direction. Therefore, the difference vector can be corrected with high accuracy.

More specifically, a comparison configuration in which the end point position of the difference vector based on the observation data of the C / A code is the final positioning result of the position of the vehicle 90 in the current cycle (that is, the vehicle obtained in the current cycle in step 200 above). At 90 positions (comparison configuration in which X ′ _u (i), Y ′ _u (i), and Z ′ _u (i)) are final positioning results), the positioning results are strongly influenced by random errors. In the time axis direction. This randomness is independent of the movement (traveling direction and speed) of the vehicle 90. On the other hand, for example, the position (X ′ _u (i), Y ′ _u (i), Z ′ _u (i)) of the vehicle 90 obtained in the current cycle and the position (X ′ _{u of the} vehicle 90 obtained in the previous cycle). (I-1), Y ′ _u (i−1), Z ′ _u (i-1)) smoothing (for example, including smoothing for the pseudorange ρ, but carrier information such as carrier smoothing or Doppler information) In the comparative configuration for deriving the position of the vehicle 90, variation is reduced, but all components are smoothed without considering the traveling direction of the vehicle 90. A time delay occurs in the positioning result in the traveling direction, and an accurate positioning result cannot be obtained. On the other hand, according to the present embodiment, the smoothing of the positioning solution (X ′ _u (i), Y ′ _u (i), Z ′ _u (i)) based on the observation data of the C / A code is performed. Only the component orthogonal to the direction of the velocity vector v ′ will be smoothed. Here, the movement of the vehicle 90 has little change in the lateral direction, that is, the direction orthogonal to the traveling direction (that is, the movement of the vehicle 90 has a large change in the traveling direction accompanying acceleration or deceleration in the traveling direction). Therefore, even if smoothing is performed in the lateral direction of the vehicle 90, delay is not a problem (that is, priority should be given to reducing variation in the lateral direction of the vehicle 90). On the other hand, the smoothing velocity vector v ′ is smoothed and because the final positioning result of the position of the vehicle 90 in the previous cycle is used, as shown in FIG. The traveling direction of the vehicle 90 is expressed with higher accuracy than the difference vector based on the observation data of the A code. Therefore, according to the present embodiment, only the component orthogonal to the traveling direction of the vehicle 90 (lateral component) is accurately extracted and smoothed in the time axis direction, so that positioning in the traveling direction of the vehicle 90 is performed. It is possible to obtain a positioning result in which no time delay occurs in the result, and particularly, there is little variation in components orthogonal to the traveling direction of the vehicle 90.

Further, according to the present embodiment, the traveling direction of the vehicle 90 among the positioning solutions (X ′ _u (i), Y ′ _u (i), Z ′ _u (i)) based on the observation data of the C / A code. Is corrected by the corrected velocity vector v ″ based on the Doppler frequency Δf, so that the accuracy of the component of the positioning result in the traveling direction of the vehicle 90 is improved. That is, the correction difference vector P based on the C / A code, By calculating (positioning) the position of the vehicle 90 based on the corrected velocity vector v ″ based on the Doppler frequency Δf, the accuracy of the component of the positioning result in the traveling direction of the vehicle 90 is improved. Therefore, according to the present embodiment, the accuracy of the component of the positioning result in the traveling direction of the vehicle 90 can be increased without causing a time delay in the positioning result in the traveling direction of the vehicle 90.

  In the present embodiment, in step 212 of FIG. 3, the traveling direction component of the positioning result may be additionally corrected using a vehicle speed sensor such as a wheel speed sensor (not shown). .

  FIG. 8 is a flowchart illustrating another example of the main processing executed by the positioning unit 50 according to the present embodiment. 8, processes that may be the same as those in FIG. 3 described above are denoted by the same step numbers as those in FIG. 3, and description thereof is omitted.

  In step 205, the difference between the speed vector v of the vehicle 90 calculated in step 204 and the difference vector obtained in step 202 is evaluated, and abnormality determination is executed. For example, when the difference between the magnitude of the velocity vector v and the magnitude of the difference vector exceeds a predetermined reference value, and / or when the angle formed by the direction of the velocity vector v and the direction of the difference vector exceeds a predetermined reference angle. Alternatively, it may be determined that some abnormality has occurred when calculating the velocity vector v and / or the difference vector. In this case, if the difference between the magnitude of the velocity vector v and the magnitude of the difference vector exceeds a predetermined reference value continuously for a predetermined period, and / or the angle formed between the direction of the velocity vector v and the direction of the difference vector is predetermined. If the reference angle is exceeded, it may be determined that an abnormality has occurred. If it is determined that an abnormality has occurred, the process proceeds to step 216; otherwise, the process proceeds to step 206.

In step 214, the position of the vehicle 90 (X ′ _u (i), Y ′ _u (i), Z ′ _u (i)) obtained in step 200, that is, the positioning result based on the C / A code, position of the vehicle 90 obtained in step 212 evaluates the difference between _{(X u (i), Y} u (i), Z u (i)), i.e., the final positioning result, the abnormality determination is executed. For example, the magnitude of a vector (X ′ _u (i) −X _u (i), Y ′ _u (i) −Y _u (i), Z ′ _u (i) −Z _u (i)) representing these differences If the value exceeds a predetermined reference value, it may be determined that some abnormality has occurred. In this case, the vectors (X ′ _u (i) −X _u (i), Y ′ _u (i) −Y _u (i), Z ′ _u (i) −Z _u ( When the magnitude of i)) exceeds a predetermined reference value, it may be determined that some abnormality has occurred. If it is determined that an abnormality has occurred, the process proceeds to step 216. In other cases, the processing of the current cycle ends. In this case, the final positioning result may be supplied to a navigation device (not shown).

  In step 216, an abnormality process is executed. The abnormality process may be, for example, a process of clearing (resetting) all data before the current cycle. In this case, for example, the process in step 202 and the process in step 206 are substantially reset. Alternatively, the abnormal time process may be a process of temporarily interrupting the supply of the final positioning result to a navigation device (not shown).

  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 difference vector calculated in the above step 202 corresponds to the movement vector of the vehicle 90 between cycles, but is converted into a velocity vector by dividing the difference vector in the positioning calculation cycle Δt. May be handled. That is, the difference vector (movement vector) and the velocity vector are equivalent from the physical meaning.

In the above description, each vector including the position (X _u (i), Y _u (i), Z _u (i)) of the vehicle 90 is managed in the world coordinate system (for example, WGS84). A local coordinate system or a polar coordinate system as shown in FIG. 9 may be used. As shown in FIG. 9, the world coordinate system is defined by an X axis and a Y axis that are orthogonal to each other in the equator plane with the earth's center of gravity as the origin, and a Z axis that is orthogonal to both axes.

  In the above-described embodiment, each vector is handled in three dimensions. However, for example, a two-dimensional vector in which the Z direction is ignored may be handled.

In the above-described embodiment, the pseudo distance ρ is derived using the C / A code. However, the pseudo distance ρ may be measured based on another pseudo noise code such as an L2 wave P code. Good. 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. The pseudorange [rho based on P-code, that 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 =} M _P It can be calculated as x30.

  In the above-described embodiment, an example in which the present invention is applied to the GPS has been described. 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. 2 is a diagram illustrating an example of an internal configuration of a GPS receiver 20. FIG. It is a flowchart which shows an example of the main processes performed by the positioning part 50 of a present Example. It is a figure which shows the derivation aspect of the correction | amendment difference vector P. FIG. It is a figure which shows the derivation aspect of correction | amendment speed vector v ". It is a figure which shows the positioning aspect of the position of the vehicle 90 of this period. It is a figure which shows typically the dispersion | variation aspect with respect to each true value of smoothing velocity vector v 'and difference vector P. It is a flowchart which shows another example of the main process performed by the positioning part 50 of a present Example. It is a figure which shows a world coordinate system and a local coordinate system.

Explanation of symbols

10 GPS Satellite 20 GPS Receiver 50 Positioning Unit 90 Vehicle

Claims (4)

In a mobile positioning device that is mounted on a mobile and measures the position and / or speed of the mobile,
Receiving means for receiving satellite signals broadcast from the satellite;
Velocity vector calculating means for calculating a velocity vector of the moving body using a Doppler shift of a carrier wave of the satellite signal;
Movement vector calculation means for calculating a movement vector of the moving body based on a change in the position of the moving body derived based on the reception result of the satellite signal;
Correction means for correcting the movement vector calculated by the movement vector calculation means using the direction of the velocity vector calculated by the speed vector calculation means as a reference direction;
Using a movement vector corrected by the correction means and a speed vector calculated by the speed vector calculation means, and positioning means for measuring the position and / or speed of the moving body, A positioning device for moving objects. The velocity vector calculating means includes
A pre-filtering speed vector calculating means for calculating a velocity vector of the moving body (hereinafter referred to as a pre-filtering speed vector for distinction from the speed vector) using a Doppler shift of a carrier wave of the satellite signal;
Filtering means for filtering the speed vector before filtering calculated by the speed vector calculating means in a time axis direction to calculate the speed vector;
The positioning means includes
An inner product of the speed vector before filtering calculated by the speed vector calculating means and a unit vector of the speed vector calculated by the filtering means is taken, and the inner product is multiplied by the unit vector of the speed vector to obtain a speed vector ( Hereinafter, for the purpose of distinguishing from the velocity vector, it is referred to as inner product velocity vector), and based on the inner product velocity vector calculated by the inner product device and the movement vector corrected by the correction device, The positioning device for a moving body according to claim 1, which measures the position and / or speed of the moving body.   The correction means takes an inner product of a unit vector of the speed vector calculated by the speed vector calculation means and a movement vector calculated by the movement vector calculation means, and multiplies the inner product by the unit vector of the speed vector. The positioning device for a moving body according to claim 1, wherein the obtained velocity vector is derived as the corrected movement vector.   The movement vector calculation means includes moving body position means for deriving the position of the moving body based on a pseudo distance between the satellite and the moving body derived based on the pseudo noise code of the satellite signal, Based on a change between the position of the mobile body derived by the mobile body position means in a period and the position of the mobile body measured by the positioning means in a period before the predetermined period, the movement vector is The mobile positioning device according to any one of claims 1 to 3, which is calculated.

Images