CN1948910A - Combined positioning method and apparatus using GPS, gyroscope, speedometer - Google Patents

Abstract

The invention relates to GPS and MEMS gyroscope and speedometer combination location method and device. While the GPS signal is good, it uses the GPS location signal to locate, rectifies by the MEMS gyroscope and speedometer, synthesize the signal by Kalman filtering algorithm. While the GPS is lost, or the number of the receiving satellite is less than three, it switches to MEMS gyroscope and speedometer to locate. The combination navigation can improve GPS location precision, makes up the unable location defect caused by the GPS is covered.

Description

Utilize the combined positioning method and the device of GPS and gyroscope, odometer

Technical field

The invention belongs to the GPS field of locating technology, be specifically related to a kind of MEMS of utilization gyroscope, odometer is proofreaied and correct the GPS positioning signal, and when GPS loses starlike attitude, switch locator meams, realize the combined positioning method and the device of uninterrupted location.

Background technology

Current, the GPS positioning system is widely adopted, and the GPS bearing accuracy is relevant with the satellite-signal intensity that receives.When receiving above 3 satellite-signals, accurate positioning; When receiving 3 satellite-signals, locate more inadequate; Receiving satellite-signal when being less than 3, can't export locating information.Because the restriction of geographical environment, on the mountain ridge, tunnel, architecture ensemble etc. block the signal intensity that gps receiver receives in the environment and can significantly reduce, even can not find satellite-signal.Therefore the GPS Positioning System can be subjected to very big influence in this case separately, needs other locator meamss to compensate and proofread and correct.

Summary of the invention

The object of the present invention is to provide the bearing accuracy height, and can solve GPS can't locate defective under the situation of being blocked localization method and device.

The localization method that the present invention proposes is the method for the integrated positioning of a kind of GPS of utilization and MEMS gyroscope, odometer, and adopts Kalman filtering algorithm that locator data is carried out optimal estimation.Wherein adopt MEMS gyroscope and odometer that the GPS location is compensated and proofreaies and correct, the MEMS gyroscope has keeping with respect to traditional gyroscope and reduces size under the prerequisite of precision, reduces power consumption, can solve GPS effectively can't location defect under the situation of being blocked, switching time is little, makes system can continue uninterruptedly to locate work; Simultaneously, under the normal situation of gps signal intensity (GPS receiving satellite signal for time), utilize Kalman filtering algorithm that GPS, MEMS gyroscope, odometer signal are carried out comprehensively, and carry out location Calculation, to improve bearing accuracy, level and smooth auditory localization cues; When gps signal intensity is undesired when (when the GPS receiving satellite signal is less than 3), switch to inertial navigation and calculate and position, promptly utilize MEMS gyroscope and odometer to position.

Among the present invention, Kalman filtering algorithm is specific as follows:

If state equation is: $\stackrel{\·}{X}\left(t\right)=\mathrm{AX}\left(t\right)+W\left(t\right)+U,---\left(1\right)$

State variable wherein $X={\left[\begin{array}{cccccccccc}e& n& \stackrel{\·}{e}& \stackrel{\·}{n}& \stackrel{\·\·}{e}& \stackrel{\·\·}{n}& {\mathrm{\ϵ}}_e& {\mathrm{\ϵ}}_n& {\mathrm{\δ}}_{\mathrm{\θ}}& {\mathrm{\δ}}_s\end{array}\right]}^T,$ E and n be respectively east to the north to the position,

With Be respectively east to the north to speed,

With

Be respectively east to the north to acceleration, ε _eAnd ε _nBe respectively various error sources in the Orient to the make progress summation of error of the north, δ _θBe the error of MEMS gyro, δ _sError for odometer.

$A=\left[\begin{array}{ccccc}{0}_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}\\ 0_{2\&times;2}& 0_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}\\ 0_{2\&times;2}& 0_{2\&times;2}& {\mathrm{<figure-callout\; id="1"\; label="A"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">A</figure-callout>}}_1& 0_{2\&times;2}& 0_{2\&times;2}\\ 0_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="A"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">A</figure-callout>}}_2& 0_{2\&times;2}\\ 0_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}& {\mathrm{<figure-callout\; id="3"\; label="A"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">A</figure-callout>}}_3\end{array}\right],---\left(2\right)$

Wherein ${\mathrm{<figure-callout\; id="1"\; label="A"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">A</figure-callout>}}_1=\left[\begin{array}{cc}-{\mathrm{\&alpha;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">e</figure-callout>}}& 0\\ 0& -{\mathrm{\&alpha;}}_n\end{array}\right],$ ${\mathrm{<figure-callout\; id="2"\; label="A"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">A</figure-callout>}}_2=\left[\begin{array}{cc}-{\mathrm{\&tau;}}_{\mathrm{<figure-callout\; id="0"\; label="e"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">e</figure-callout>}}& 0\\ 0& -{\mathrm{\&tau;}}_n\end{array}\right],$ ${\mathrm{<figure-callout\; id="3"\; label="A"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">A</figure-callout>}}_3=\left[\begin{array}{cc}-{\mathrm{\&tau;}}_{\mathrm{\&theta;}}& 0\\ 0& -{\mathrm{\&tau;}}_s\end{array}\right],$ I _{2 * 2}Be 2 rank unit matrix, 0 _{2 * 2}Be the second order null matrix.

U＝[0 _1×4 α _ea _e α _na _n 0 _1×4] ^T。

In the model hypothesis of wave filter:

$\stackrel{\&CenterDot;\&CenterDot;}{e}={\stackrel{\&OverBar;}{a}}_e+a_e$ $\stackrel{\&CenterDot;\&CenterDot;}{n}={\stackrel{\&OverBar;}{a}}_n+a_n---\left(3\right)$

Adopt the assembly average of Maneuver Acceleration to explain acceleration herein.

(4)

Wherein: α _e, α _n, τ _e, τ _n, τ _θ, τ _sBe corresponding time constant inverse,

Be respectively corresponding parameter α _e, α _n, e, n, δ _θ, δ _sThe zero-mean white Gaussian noise, their variance is respectively 2 α _eσ _Ae ², 2 α _nσ _An ²(σ _Ae ², σ _An ²Variance for acceleration), σ _e ², σ _n ², σ _{δ θ} ², σ _{δ s} ²

After the model discretize, the observed quantity of foundation is through calculating the positional information e of back output by the GPS module _mn _m, velocity information And the angle θ of MEMS gyro output, odometer output apart from s.

X(k+1)＝Φ(k+1)X(k)+U(k)+W(k) (5)

Wherein observed quantity:

Observation equation is non-linear, adopts expanded Kalman filtration algorithm, and it is launched into Taylor series at one-step prediction value place, ignores high-order term.The Kalman filter recursive algorithm that obtains dispersing:

X(k)＝X·(k，k-1)+K(k){Z(k)-h[X(k，k-1)]}

K(k)＝P((k，k-1)H ^T[X(k，k-1)]·{H[X(k，k-1)]P(k，k-1)·H ^T[X(k，k-1)]+R(K)} ^-1

P(k，k-1)＝Φ(k，k-1)P(k-1)·Φ ^T(k，k-1)+Q(k-1)

P(k)＝{I-K(k)·H[X(k，k-1)]}P(k，k-1)

X(k，k-1)＝Φ′(k，k-1)+X(k-1) (7)

Wherein,

${\mathrm{\&Phi;}}^{\&prime;}(k,k-1)=\left[\begin{array}{ccccc}{\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="TI"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">TI</figure-callout>}}_{2\&times;2}& 0.5T^2{\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}\\ 0_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="TI"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">TI</figure-callout>}}_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}\\ 0_{2\&times;2}& 0_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="I"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">I</figure-callout>}}_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}\\ 0_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="E"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">E</figure-callout>}}_2& 0_{2\&times;2}\\ 0_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}& 0_{2\&times;2}& {\mathrm{<figure-callout\; id="3"\; label="E"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">E</figure-callout>}}_3\end{array}\right]$

${\mathrm{<figure-callout\; id="2"\; label="E"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">E</figure-callout>}}_2=\left[\begin{array}{cc}{e}^{-{\mathrm{\&tau;}}_e\mathrm{<figure-callout\; id="0"\; label="T"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">T</figure-callout>}}& 0\\ 0& e^{-{\mathrm{\&tau;}}_{n}T}\end{array}\right],$ ${\mathrm{<figure-callout\; id="3"\; label="E"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">E</figure-callout>}}_3=\left[\begin{array}{cc}{e}^{-{\mathrm{\&tau;}}_{\mathrm{\&theta;}}\mathrm{<figure-callout\; id="0"\; label="T"\; filenames="A20061011810200102.png"\; state="\{\{state\}\}">T</figure-callout>}}& 0\\ 0& e^{-{\mathrm{\&tau;}}_{s}T}\end{array}\right].$ (8)

Carry out the location Calculation that GPS adds the integrated navigation of inertial navigation with this recursion formula.

If when generation GPS receiving satellite is less than 3, stop Kalman filtering algorithm, adopt following recursion formula to carry out inertial navigation and calculate;

e(k)＝e(k-1)+s(k)×sin[β(k-1)+θ(k)]

n(k)＝n(k-1)+s(k)×cos[β(k-1)+θ(k)]

Wherein, β and θ all are to be 0 with direct north, and clockwise direction is positive angle.

The locating device that the present invention proposes is made of following several modules: inertial navigation module, GPS module, processing module, output module.

The function of inertial navigation module is carried out filtering for connecting the output of MEMS gyroscope and odometer with signal, and analog to digital conversion adopts serial communication mode regularly to export processing module to.

The function of GPS module is handled back output locating information for receiving gps satellite signal, adopts serial communication mode regularly to export processing module to.

Processing module receives GPS locating information and inertial navigation information, and information is carried out time adjustment, judges whether GPS information is available, selects to adopt Kalman filtering algorithm or inertia to calculate and positions calculating, and export the location Calculation result to output module.

Output module shows current positioning states according to the locating and displaying information of communications protocol receiving processing module output at visualization interface.

This device adopts modular construction, and the each several part design is independent, can compatiblely replace flexible configuration based on the module of unified interface protocol.

Description of drawings

Fig. 1, system form structure.

Fig. 2, inertial navigation module.

Fig. 3, method flow of the present invention.

Wherein, 1 is processing module, and 2 is output module, and 3 is the GPS module, and 4 is inertial navigation module, and 5 is the MEMS gyroscope, and 6 is odometer, and 7 is the A/D modular converter, and 8 is counter, and 9 is single-chip microcomputer, and 10 are single-chip microcomputer output.

Embodiment

In conjunction with the accompanying drawings, the specific embodiment of the present invention is described.

Fig. 1 is the composition structural drawing of total system, the embedded system that the main body processing module 1 of system adopts based on ARM9, and CPU adopts Samsung S3C2410 (based on ARM920t).Processing module 1 connects GPS module 3, inertial navigation module 4, output module 2.The output signal that inertial navigation module 4 is gathered MEMS gyroscope, odometer is handled the packing back and is exported to processing module by serial line interface.The GPS module meets the serial locator data of NMEA 0183 ASCII standard by serial line interface output.The output information of output module receiving processing module is presented at LCDs with locating information in conjunction with electronic chart.

Fig. 2 is the hardware structure diagram of inertial navigation module 4.The magnitude of voltage output signal that is input as MEMS gyroscope 5 (ADXRS150) of inertial navigation module 4 and the pulse signal of odometer 6.The output voltage of MEMS gyroscope 5 obtains digital quantity by analog to digital converter 7, and the pulse signal of odometer 6 converts digital quantity to by counter, and digital quantity inputs to MCU by the MCU data bus.MCU carries out synchronized sampling according to the input of odometer to the input of MEMS gyroscope, obtains the displacement vector in a certain moment, outputs to processing module 2 by serial line interface.

Fig. 3 is the major software process flow diagram of system.Processing module 1 receive the gps signal module 3 with output signal inertial navigation module 4, carry out time synchronized according to certain hour.Gps signal is longer interval time, thereby the inertial positioning data are got average in the discrete sampling time interval, as the interior at interval inertial positioning input of a discrete time.Judge at each discrete time point whether gps signal is good,, then gps data and inertial positioning data are carried out Kalman filtering, comprehensive proper prelocalization data if signal is good; If gps signal is bad, then adopt inertia to infer algorithm and carry out independently voyage by MEMS gyroscope and odometer and infer, obtain locator data.Adopt Kalman filtering need set the correlation parameter and the initial value of filter model.In the present embodiment, first the beginning of wave filter is set at:

τ _e＝τ _n＝0.01 α _e＝α _n＝1 ${\mathrm{\&sigma;}}_{{\mathrm{\&delta;}}_s}=2$

σ _e＝σ _n＝10 ${\mathrm{\&sigma;}}_{a_e}={\mathrm{\&sigma;}}_{a_n}=0.5$ ${\mathrm{\&sigma;}}_{{\mathrm{\&delta;}}_{\mathrm{\&theta;}}}=0.2$

X(0)＝[0，0，0，0，0，0，0，0，0] ^T

P(0)＝diag{10 ²，10 ²，1，1，0.2 ²，0.2 ²，5 ²，5 ²，0.1 ²，2 ²}

R＝diag{10 ²，10 ²，1，1，0.05 ²，2 ²}

The substitution recursion formula calculates location output.

Attention: this implementation just realizes a kind of approach of this device, and the professional can modify specific implementation as required in this area, as replacing MEMS gyroscope model, MCU model etc.Thereby, realizing that some details in the example should not constitute limitation of the invention, the scope that the present invention will define with appended claims is as protection scope of the present invention.

Claims (5)

1, a kind of combined positioning method that utilizes GPS, gyroscope, odometer is characterized in that adopting MEMS gyroscope and odometer that the GPS location is compensated and proofreaies and correct; Wherein:

When the GPS receiving satellite signal was 3, gps signal intensity was normal, then utilized Kalman filtering algorithm that the signal synthesis of GPS, MEMS gyroscope and odometer is positioned;

When the GPS receiving satellite signal was less than 3, gps signal was undesired, then switched to the inertial navigation reckoning and positioned.

2, localization method according to claim 1 is characterized in that the step of described Kalman filtering algorithm is as follows:

If state equation is: $\stackrel{\&CenterDot;}{X}\left(t\right)=\mathrm{AX}\left(t\right)+W\left(t\right)+U,$

State variable wherein $X={\left[\begin{array}{cccccccccc}e& n& \stackrel{\&CenterDot;}{e}& \stackrel{\&CenterDot;}{n}& \stackrel{\&CenterDot;\&CenterDot;}{e}& \stackrel{\&CenterDot;\&CenterDot;}{n}& {\mathrm{\&epsiv;}}_e& {\mathrm{\&epsiv;}}_n& {\mathrm{\&delta;}}_{\mathrm{\&theta;}}& \end{array},{\mathrm{\&delta;}}_s\right]}^T,$ E and n be respectively east to the north to the position,

With

Be respectively east to the north to speed,

With Be respectively east to the north to acceleration, ε _eAnd ε _nBe respectively various error sources in the Orient to the make progress summation of error of the north, δ _θBe the error of MEMS gyro, δ _sError for odometer;

$A=\left[\begin{array}{ccccc}{O}_{2\&times;2}& I_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}\\ O_{2\&times;2}& O_{2\&times;2}& I_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}\\ O_{2\&times;2}& O_{2\&times;2}& A_1& O_{2\&times;2}& O_{2\&times;2}\\ O_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}& A_2& O_{2\&times;2}\\ O_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}& A_3\end{array}\right],$

Wherein $A_1=\left[\begin{array}{cc}-{\mathrm{\&alpha;}}_e& 0\\ 0& -{\mathrm{\&alpha;}}_n\end{array}\right],$ $A_2=\left[\begin{array}{cc}-{\mathrm{\&tau;}}_e& 0\\ 0& -{\mathrm{\&tau;}}_n\end{array}\right],A_3=\left[\begin{array}{cc}-{\mathrm{\&tau;}}_{\mathrm{\&theta;}}& 0\\ 0& -{\mathrm{\&tau;}}_s\end{array}\right],$ I _{2 * 2}Be 2 rank unit matrix, O _{2 * 2}Be the second order null matrix;

$U={\left[\begin{array}{cccc}{O}_{1\&times;4}& {\mathrm{\&alpha;}}_e{\stackrel{\&OverBar;}{a}}_e& {\mathrm{\&alpha;}}_n{\stackrel{\&OverBar;}{a}}_n& O_{1\&times;4}\end{array}\right]}^T;$

In the model hypothesis of wave filter:

$\stackrel{\&CenterDot;\&CenterDot;}{e}={a^{-}}_e+a_e$ $\stackrel{\&CenterDot;\&CenterDot;}{n}={a^{-}}_n+a_n,$

Adopt the assembly average of Maneuver Acceleration to explain acceleration herein.

Wherein: α _e, α _n, τ _e, τ _n, τ _θ, τ _sBe corresponding time constant inverse, Be respectively corresponding parameter α _e, α _n, e, n, δ _θ, δ _sThe zero-mean white Gaussian noise, their variance is respectively 2 α _eσ α _e ², 2 α _hσ α _n ², σ _e ², σ _n ², σ _{δ θ} ², σ _{δ s} ²

After the model discretize, the observed quantity of foundation is through calculating the positional information e of back output by the GPS module _mn _m, velocity information

And the angle θ of MEMS gyro output, odometer output apart from s;

X(k+1)＝Φ(k+1)X(k)+U(k)+W(k)

Wherein observed quantity:

Observation equation is non-linear, and it is launched into Taylor series at one-step prediction value place, ignores high-order term; The Kalman filter recursive algorithm that obtains dispersing:

X(k)＝X(k，k-1)+K(k){Z(k)-h[X(k，k-1)]}

K(k)＝P(k，k-1)H ^T[X(k，k-1)]·{H[X(k，k-1)]P(k，k-1)·H ^T[X(k，k-1)]+P(k)} ^-1

P(k，k-1)＝Φ(k，k-1)P(k-1)·Φ ^T(k，k-1)+Q(k-1)

P(k)＝{I-K(k)·H[X(k，k-1)]}P(k，k-1)

X(k，k-1)＝Φ′(k，k-1)+X(k-1)

Wherein,

${\mathrm{\&Phi;}}^{\&prime;}(k,k-1)=\left[\begin{array}{ccccc}{I}_{2\&times;2}& {\mathrm{TI}}_{2\&times;2}& 0.5T^2{I}_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}\\ O_{2\&times;2}& I_{2\&times;2}& {\mathrm{TI}}_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}\\ O_{2\&times;2}& O_{2\&times;2}& I_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}\\ O_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}& E_2& O_{2\&times;2}\\ O_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}& O_{2\&times;2}& E_3\end{array}\right]$

$E_2=\left[\begin{array}{cc}{e}^{-{\mathrm{\&tau;}}_{e}T}& 0\\ 0& e^{-{\mathrm{\&tau;}}_{n}T}\end{array}\right],$ $E_3=\left[\begin{array}{cc}{e}^{-{\mathrm{\&tau;}}_{\mathrm{\&theta;}}T}& 0\\ 0& e^{-{\mathrm{\&tau;}}_{s}T}\end{array}\right]$

Carry out the location Calculation that GPS adds the integrated navigation of inertial navigation with this recursion formula.

3, localization method according to claim 2 is characterized in that the formula of described inertial navigation reckoning is as follows:

e(k)＝e(k-1)+s(k)×sin[β(k-1)+θ(k)]

n(k)＝n(k-1)+s(k)×cos[β(k-1)+θ(k)]

Wherein, β and θ all are to be 0 with direct north, and clockwise direction is positive angle.

4, a kind of integrated positioning device that utilizes GPS, gyroscope, odometer is characterized in that being made of following several modules: inertial navigation module, GPS module, processing module, output module; Wherein:

The function of inertial navigation module is carried out filtering for connecting the output of MEMS gyroscope and odometer with signal, and analog to digital conversion adopts serial communication mode regularly to export processing module to;

The function of GPS module is handled back output locating information for receiving gps satellite signal, adopts serial communication mode regularly to export processing module to;

Processing module receives GPS locating information and inertial navigation information, and information is carried out time adjustment, judges whether GPS information is available, selects to adopt Kalman filtering algorithm or inertia to calculate and positions calculating, and export the location Calculation result to output module;

Output module shows current positioning states according to the locating and displaying information of communications protocol receiving processing module output at visualization interface.

5, locating device according to claim 4 is characterized in that described inertial navigation module (4) is made up of A/D modular converter (7), counter (8) and single-chip microcomputer (9); Wherein, the pulse signal of the magnitude of voltage output signal that is input as MEMS gyroscope (5) of inertial navigation module (4) and odometer (6); The output voltage of MEMS gyroscope (5) obtains digital quantity by analog to digital converter (7), and the pulse signal of odometer (6) converts digital quantity to by counter (8), and digital quantity inputs to MCU by the MCU data bus; MCU carries out synchronized sampling according to the input of odometer to the input of MEMS gyroscope, obtains the displacement vector in a certain moment, outputs to processing module (2) by serial line interface.

Images