CN102967313A - Data processing method for strapdown inertial navigation system of tracked vehicle - Google Patents

Abstract

The invention discloses a data processing method for a strapdown inertial navigation system of a tracked vehicle. The data processing method comprises the following steps of: fixedly connecting an inertia measurement unit and a track of the tracked vehicle; performing strapdown inertial navigation calculation; performing geostationary detection; calculating errors; and correcting the errors. The data processing method can be used for converting a positioning problem of the tracked vehicle into a positioning problem of the track, continuously detecting whether the inertia measurement unit which is fixedly connected with the track is static relative to the ground according to the specificity of track motion, and calculating and correcting the errors of an inertia measurement signal and strapdown inertial navigation parameters when the inertia measurement unit is static relative to the ground, so that the positioning errors of the strapdown inertial navigation system are effectively suppressed, and the positioning accuracy of the inertial system is greatly improved.

Description

A kind of endless-track vehicle data processing method of strapdown inertial navigation system

Technical field

The present invention is mainly concerned with the Camera calibration method field of vehicle, refers in particular to a kind of data processing method based on strapdown inertial navigation system that mainly is applicable to endless-track vehicle.

Background technology

At present, the endless-track vehicle of various forms, various uses is widely used in every field.In the application process of endless-track vehicle, high-precision independent navigation and location for endless-track vehicle under the circumstances not known have very important significance in some typical cases use.

Although have a variety of navigator fix means, be widely used such as satellite navigation etc., for complete autonomous navigation and location, particularly some typical cases use, and then mainly still rely on inertial navigation system.At present, then mainly be strapdown inertial navigation system, its sensor then mainly is gyro and two kinds of inertia devices of accelerometer.In the conventional art, the endless-track vehicle strapdown inertial navigation system is to be fixedly connected with the car body of endless-track vehicle, and the car body that usually is installed in endless-track vehicle is inner.

Strapdown inertial navigation system is a kind of typical dead reckoning system, because the impact of sensor error and initial error, simple inertial navigation system, its positioning error are ever-increasing in time.Therefore, in satisfy using to the requirement of navigation positioning error bounded, usually adopt inertia with other navigation means, consist of integrated navigation system, for example inertia/satellite combined guidance system or the inertia/odometer integrated navigation system of comprising commonly used.

Wherein, inertia/satellite combined guidance system can provide for carrier the navigator fix result of high precision, error bounded, but owing to rely on satellite, has therefore lost its independence.Although inertia/odometer integrated navigation system had both kept the independence of system, greatly improved simultaneously the navigation and positioning accuracy of system, but its positioning error still increases with the growth of range ability, and for example usually its error is at about 5 ‰ of stroke, and namely its error remains unbounded.

Up to the present, also do not have relevant measuring equipment and method in the prior art, can not rely on other navigation means, endless-track vehicle is carried out establishment with the error of strapdown inertial navigation system, for endless-track vehicle provides high-precision independent navigation and location.

Summary of the invention

The technical problem to be solved in the present invention just is: for the technical matters of prior art existence, the invention provides and a kind ofly can not rely on other navigation means, but calculate the error of sensor and navigational parameter in the Inertial Measurement Unit and revise and then the positioning error of establishment strap down inertial navigation system, greatly improve the endless-track vehicle of strap down inertial navigation system accuracy with the data processing method of strapdown inertial navigation system.

For solving the problems of the technologies described above, the present invention by the following technical solutions:

A kind of endless-track vehicle the steps include: with the data processing method of strapdown inertial navigation system

(1) Inertial Measurement Unit is fixedly connected with the crawler belt of endless-track vehicle;

(2) strap-down inertial calculates: according to position, speed and the attitude of the inertia measurement calculated signals carrier of Inertial Measurement Unit output;

(3) static detection over the ground: the process to the crawler belt kiss the earth at Inertial Measurement Unit mounting points place detects; Namely the static process of Inertial Measurement Unit with respect to ground detected;

(4) error is calculated: utilize Inertial Measurement Unit with respect to the static process in ground, constantly the error of carrying out of inertia measurement signal and navigational parameter is calculated;

(5) error correction: according to the inertia measurement signal errors that calculates and navigational parameter error, inertia measurement signal and navigational parameter are carried out error correction.

As a further improvement on the present invention:

Described step (3) is to utilize the gyro in the Inertial Measurement Unit or/and the signal that accelerometer measures obtains judges whether Inertial Measurement Unit is static relative to the earth.Being located at the ratio force vector of constantly accelerometer output of i or the angular velocity vector of gyro output in the described step (3) is

Then static detection comprises following step:

1) compute vectors amplitude

Namely calculate:

$v_i^b=\left|v_i^b\right|---\left(1\right)$

2) average of the vector magnitude sequence in calculating a period of time

And variance

Namely

${\stackrel{\‾}{v}}_i^b=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{v}_i^b$

(2)

${\mathrm{\σ}}_{v_i}^2=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{(v_i^b-{\stackrel{\‾}{v}}_i^b)}^2$

Here, N is the length of time window;

3) judge: according to the error characteristics of gyro and accelerometer, select suitable static threshold T _vIf,

Judge that then i is constantly static over the ground; If adopt and to unite judgement than force vector and angular velocity vector, then need to select respectively two threshold values, i.e. gyro static threshold and accelerometer static threshold are if the variance of two amplitude of the vector sequences all less than the static threshold of correspondence, judges that then this moment is static over the ground.

Described step (3) is to utilize auxiliary pressure sensor signal, judges whether Inertial Measurement Unit is static relative to the earth.

The concrete steps that error is calculated in the described step (4) are: set up following Kalman filter model, when detecting the relative ground of Inertial Measurement Unit when static, carry out Kalman filtering and calculate, obtain the error of inertia measurement signal and navigational parameter.Described Kalman filter model comprises a system equation and an observation equation, and the system equation of Kalman filter model is:

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(3\right)$

Here,

X＝[δv _n δv _e δv _d δψ _n δψ _e δψ _d b _ax b _ay b _az b _gx b _gy b _gz] ^T (4)

$F=\left[\begin{array}{cccc}{F}_w& F_{\mathrm{v\ψ}}& C_b^n& 0_{3*3}\\ F_{\mathrm{\ψv}}& F_{\mathrm{\ψ\ψ}}& 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\end{array}\right]---\left(5\right)$

$G=\left[\begin{array}{cc}{C}_b^n& 0_{3*3}\\ 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}\end{array}\right]---\left(6\right)$

W＝[w _ax w _ay w _az w _gx w _gy w _gz] ^T (7)

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{v_d}{R+h}& -2(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ 2\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& \frac{v_d+v_n\tan L}{R+h}& 2\mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ -\frac{{2v}_n}{R+h}& -2(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(8\right)$

$F_{\mathrm{v\ψ}}=\left(C_b^n{f}^b\right)\×---\left(9\right)$

$F_{\mathrm{\ψv}}=\left[\begin{array}{ccc}0& \frac{1}{R+h}& 0\\ \frac{-1}{R+h}& 0& 0\\ 0& \frac{-\tan L}{R+h}& 0\end{array}\right]---\left(10\right)$

$F_{\mathrm{\ψ\ψ}}=\left[\begin{array}{ccc}0& -(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ \mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& 0& \mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ \frac{{-v}_n}{R+h}& -(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(11\right)$

Wherein: Ω represents rotational-angular velocity of the earth; L represents local latitude; R represents earth radius; H represents local sea level elevation. $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ Ratio force vector for accelerometer measures; 0 _3*3The null matrix of expression 3*3;

Direction cosine matrix between expression carrier system and the navigation system; V=[v _nv _ev _d] ^TEast northeast ground velocity for inertial navigation system output;

δ v=[δ v _nδ v _eδ v _d] ^TBe system's east northeast ground velocity error; δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^TBe system's attitude error;

b _a=[b _Axb _Ayb _Az] ^TBe the accelerometer bias error; b _g=[b _Gxb _Gyb _Gz] ^TBe gyro zero inclined to one side error;

w _a=[w _Axw _Ayw _Az] ^TBe the accelerometer white noise; w _g=[w _Gxw _Gyw _Gz] ^TBe the gyro white noise.T represents transposition,

The antisymmetric matrix of * expression vector.

Adopt the inertial navigation velocity error when static over the ground, the output speed that the inertial navigation when also namely static over the ground calculates is measured as systematic perspective, makes up the observation equation of Kalman filter model:

z＝HX+υ (12)

Here,

z＝[δv _n δv _e δv _d] ^T

(13)

H＝[I _3*3 0 _3*3 0 _3*3 0 _3*3]

Wherein, I _3*3Unit matrix for 3*3; υ is the velocity survey noise;

In the step (5) the inertia measurement signal is carried out the concrete steps of error correction: the ratio force vector that is located at i moment accelerometer measures is f ^b, the specific force vector error of accelerometer measures is δ f ^b, revisedly than force vector be

The angular velocity vector of gyro to measure is ω ^b, the angular velocity error of gyro to measure is δ ω ^b, revised angular velocity vector is

Then the correction of inertia measurement signal errors is carried out according to following formula:

${\hat{f}}^b=f^b-{\mathrm{\δf}}^b---\left(14\right)$

${\widehat{\mathrm{\ω}}}^b={\mathrm{\ω}}^b-\mathrm{\δ}{\mathrm{\ω}}^b$

Wherein, δ f ^bWith δ ω ^bInitial value be:

δf ^b＝δω ^b＝[0 0 0] ^T (15)

If i is that relative ground is static constantly, then after finishing Kalman filtering calculating, from filter state, extract accelerometer bias error b _aWith gyro zero inclined to one side error b _g, at first be calculated according to the following formula δ f ^bWith δ ω ^b:

$\mathrm{\δ}{\hat{f}}^b={\mathrm{\δf}}^b+b_a---\left(16\right)$

δω ^b＝δω ^b+b _g

Then make the b in the system state _aAnd b _gZero clearing, that is:

b _a＝b _g＝[0 0 0] ^T (17)

If i is not that relative ground is static constantly, then δ f ^bWith δ ω ^bConstant.

In the step (5) navigational parameter is carried out the concrete steps of error correction: establishing i moment inertial navigation system outgoing position is P=[L λ h] ^T, site error is δ P=[δ L δ λ δ h] ^T, revised position is $\hat{P}={\left[\begin{array}{ccc}\hat{L}& \widehat{\mathrm{\λ}}& \hat{h}\end{array}\right]}^T;$ The speed of inertial navigation system output is v=[v _nv _ev _d] ^T, velocity error is δ v=[δ v _nδ v _eδ v _d] ^T, revised speed is $\hat{v}={\left[\begin{array}{ccc}{\hat{v}}_n& {\hat{v}}_e& {\hat{v}}_d\end{array}\right]}^T;$ The attitude direction cosine matrix of inertial navigation system output is

Attitude error is δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^T, revised attitude direction cosine matrix is

When i is relative ground when static constantly, after finishing Kalman filtering and calculating, from filter state, extract velocity error and attitude error, the correction of navigational parameter is carried out according to following formula:

$\hat{P}=P-\mathrm{\δP}$

$\hat{v}=v-\mathrm{\δv}---\left(18\right)$

${\hat{C}}_b^n=C_b^n[I+\mathrm{\δ\ψ}\×]$

δ ψ * be the antisymmetric matrix of δ ψ.δ P is calculated according to the following formula:

$\mathrm{\δP}=\frac{t}{2}{\left[\begin{array}{ccc}\frac{{\mathrm{\δv}}_n}{R+h}& \frac{{\mathrm{\δv}}_e}{(R+h)\cos L}& {\mathrm{\δv}}_d\end{array}\right]}^T---\left(19\right)$

Then make δ v and δ ψ zero clearing in the system state, that is:

δv＝δψ＝[0 0 0] ^T (20)

Here, t represents a time that is carved into current time when static over the ground, and I represents the 3*3 unit matrix.

If i is not that relative ground is static constantly, then do not carry out the navigational parameter error correction.

Compared with prior art, the invention has the advantages that: the present invention will be converted into to the orientation problem of crawler belt car body the orientation problem to certain point on the crawler belt.The motion of crawler belt has singularity, point of fixity from the crawler belt, its motion was made of two different stages, one is the stage static with respect to earth surface, the stage that namely contacts with ground, another is the stage with respect to the earth surface motion, and constantly replacing of these two stages consisted of the entire motion process of this point of fixity on the crawler belt.Utilize certain this singularity of a bit moving on the crawler belt, Inertial Measurement Unit in the strapdown inertial navigation system is fixedly connected with crawler belt, on this basis to the crawler belt at Inertial Measurement Unit mounting points place in the kiss the earth process, Inertial Measurement Unit carries out over the ground static detection with respect to the stationary state on ground, and then the output valve of system and the difference between the theoretical value during according to stationary state, calculate the error of Inertial Measurement Unit and navigational parameter, and it is revised, the positioning error that has effectively suppressed the strap down inertial navigation system has improved the bearing accuracy of inertia system greatly.

Description of drawings

Fig. 1 is the principle schematic of disposal route of the present invention.

Fig. 2 is that the present invention's Inertial Measurement Unit in specific embodiment is installed in the structural representation on the creeper truck crawler belt.

Fig. 3 is the treatment scheme synoptic diagram in the specific embodiment of the invention.

Marginal data:

1, Inertial Measurement Unit; 2, crawler belt.

Embodiment

Below with reference to Figure of description and specific embodiment the present invention is described in further details.

As shown in figures 1 and 3, the endless-track vehicle of the present invention data processing method of strapdown inertial navigation system, its concrete steps are:

(1) Inertial Measurement Unit 1 is fixedly connected with (referring to shown in Figure 2) with the crawler belt 1 of endless-track vehicle, is about to Inertial Measurement Unit 1 and is fixedly mounted on the crawler belt 2 of creeper truck, Inertial Measurement Unit 1 is with the motion campaign of crawler belt 2.Inertial Measurement Unit 1 generally is made of a plurality of gyros and accelerometer, and for the angular velocity of measuring carrier with than force vector, the output of Inertial Measurement Unit 1 is called the inertia measurement signal.

(2) strap-down inertial calculates: according to position, speed and the attitude of the inertia measurement calculated signals carrier of Inertial Measurement Unit 1 output;

(3) static detection over the ground: the process to crawler belt 2 kiss the earths at Inertial Measurement Unit 1 mounting points place detects; Namely Inertial Measurement Unit 1 is with respect to the static process on ground; Namely, can be to utilize the information that sensor measurement obtains in the Inertial Measurement Unit 1 or be installed near on the crawler belt 2 and Inertial Measurement Unit 1 other sensor information, process and judge, to determine whether Inertial Measurement Unit 1 is in the static state in relative ground.Because Inertial Measurement Unit 1 is fixedly connected with crawler belt 2 certain point, therefore judge namely also whether crawler belt 2 these points are static relative to the earth.

This step (3) can utilize the gyro in the Inertial Measurement Unit 1 or/and the signal that accelerometer measures obtains judges whether Inertial Measurement Unit 1 is static relative to the earth.

In other embodiments, this step (3) is to utilize other sensor signals, for example: auxiliary pressure sensor signal, judge whether Inertial Measurement Unit 1 is static relative to the earth.

(4) error is calculated: utilize Inertial Measurement Unit 1 information static with respect to ground, constantly carry out the error of inertia measurement signal 1 and inertial navigation parameter and calculate; When detecting Inertial Measurement Unit 1 when static with respect to ground, in theory in the Inertial Measurement Unit 1 gyro to measure be that the earth is with respect to the angular velocity of inertial coordinates system, accelerometer measures be earth local gravity, relatively the speed on ground is zero, the acceleration on ground is zero relatively.And the real output value that Inertial Measurement Unit 1 and inertial navigation calculate because error is different from corresponding theoretical value, compares, analyzes and process these difference, can obtain the error of Inertial Measurement Unit 1 and inertial navigation output.It can adopt " Zero velocity Updating algorithm ", and namely when Inertial Measurement Unit 1 mounting points was static with respect to ground, then its speed with respect to the earth was zero in theory, and the speed that inertial navigation system is calculated, because the impact of various error components, and non-vanishing.So, the speed that inertial navigation system calculates is exactly its velocity error, sets up the Kalman Filtering Model, utilizes the Kalman filtering algorithm, as observed quantity, can estimate the various errors of inertial navigation system with this velocity error.Concrete Kalman filter model comprises system equation and observation equation.

(5) error correction: according to the inertia measurement signal errors that calculates and navigational parameter error, inertia measurement signal and navigational parameter are carried out error correction.

(5.1) sensor error correction: be according to the inertia measurement signal errors that calculates, the output of inertia measurement signal is revised.

(5.2) navigational parameter error correction: be according to the navigational parameter error of calculating, the inertial navigation parameter is revised.

In view of the position of crawler belt car body and crawler belt 2 approaches, the present invention is fixedly installed in Inertial Measurement Unit 1 on the crawler belt 2 of creeper truck, and the orientation problem to the crawler belt car body is converted into orientation problem to certain point on the crawler belt 2.Inertial Measurement Unit 1 in the strapdown inertial navigation system is fixedly connected with crawler belt 2, on this basis to the process of crawler belt 2 kiss the earths at Inertial Measurement Unit 1 mounting points place, also be that Inertial Measurement Unit 1 is with respect to the static process on ground, carry out over the ground static detection, when detecting when static, utilize Inertial Measurement Unit 1 information static with respect to ground, constantly calculate the error of inertia measurement signal and inertial navigation parameter, and carry out error correction, improve the autonomous bearing accuracy of strap-down inertial.

In the present embodiment, establishing the ratio force vector of constantly accelerometer output of i or the angular velocity vector of gyro output is

Then static detection comprises following step:

1) compute vectors amplitude

Namely calculate

$v_i^b=\left|v_i^b\right|---\left(1\right)$

2) average of the vector magnitude sequence in calculating a period of time

And variance

That is:

${\stackrel{\‾}{v}}_i^b=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{v}_i^b$

(2)

${\mathrm{\σ}}_{v_i}^2=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{(v_i^b-{\stackrel{\‾}{v}}_i^b)}^2$

Here, N is the length of time window;

3) judge: according to the error characteristics of gyro and accelerometer, select suitable static threshold T _vIf,

Judge that then i is constantly static over the ground; If adopt and to unite judgement than force vector and angular velocity vector, then need to select respectively two threshold values, i.e. gyro static threshold and accelerometer static threshold are if the variance of two amplitude of the vector sequences all less than the static threshold of correspondence, judges that then this moment is static over the ground.

In the present embodiment, the concrete steps that error is calculated are: set up following Kalman filter model, when detecting Inertial Measurement Unit 1 relative ground when static, carry out Kalman filtering and calculate, the error of acquisition inertia measurement signal and navigational parameter.The Kalman filter model comprises a system equation and an observation equation, and the system equation of Kalman filter model is:

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(3\right)$

Here,

X＝[δv _n δv _e δv _d δψ _n δψ _e δψ _d b _ax b _ay b _az b _gx b _gy b _gz] ^T (4)

$F=\left[\begin{array}{cccc}{F}_w& F_{\mathrm{v\ψ}}& C_b^n& 0_{3*3}\\ F_{\mathrm{\ψv}}& F_{\mathrm{\ψ\ψ}}& 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\end{array}\right]---\left(5\right)$

$G=\left[\begin{array}{cc}{C}_b^n& 0_{3*3}\\ 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}\end{array}\right]---\left(6\right)$

W＝[w _ax w _ay w _az w _gx w _gy w _gz] ^T (7)

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{v_d}{R+h}& -2(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ 2\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& \frac{v_d+v_n\tan L}{R+h}& 2\mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ -\frac{{2v}_n}{R+h}& -2(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(8\right)$

$F_{\mathrm{v\ψ}}=\left(C_b^n{f}^b\right)\×---\left(9\right)$

$F_{\mathrm{\ψv}}=\left[\begin{array}{ccc}0& \frac{1}{R+h}& 0\\ \frac{-1}{R+h}& 0& 0\\ 0& \frac{-\tan L}{R+h}& 0\end{array}\right]---\left(10\right)$

$F_{\mathrm{\ψ\ψ}}=\left[\begin{array}{ccc}0& -(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ \mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& 0& \mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ \frac{{-v}_n}{R+h}& -(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(11\right)$

Wherein: Ω represents rotational-angular velocity of the earth; L represents local latitude; R represents earth radius; H represents local sea level elevation. $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ Ratio force vector for accelerometer measures; 0 _3*3The null matrix of expression 3*3;

Direction cosine matrix between expression carrier system and the navigation system; V=[v _nv _ev _d] ^TEast northeast ground velocity for inertial navigation system output;

δ v=[δ v _nδ v _eδ v _d] ^TBe system's east northeast ground velocity error; δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^TBe system's attitude error;

b _a=[b _Axb _Ayb _Az] ^TBe the accelerometer bias error; b _g=[b _Gxb _Gyb _Gz] ^TBe gyro zero inclined to one side error;

w _a=[w _Axw _Ayw _Az] ^TBe the accelerometer white noise; w _g=[w _Gxw _Gyw _Gz] ^TBe the gyro white noise.T represents the antisymmetric matrix of transposition * expression vector.

Adopt the inertial navigation velocity error when static over the ground, the output speed that the inertial navigation when also namely static over the ground calculates is measured as systematic perspective, makes up the observation equation of Kalman filter model:

z＝HX+υ (12)

Here,

z＝[δv _n δv _e δv _d] ^T

(13)

H＝[I _3*3 0 _3*3 0 _3*3 0 _3*3]

Wherein, I _3*3Unit matrix for 3*3; υ is the velocity survey noise;

In the present embodiment, the concrete steps of in the described step (5) the inertia measurement signal errors being revised are:

The ratio force vector that is located at i moment accelerometer measures is f ^b, the specific force vector error of accelerometer measures is δ f ^b, revisedly than force vector be The angular velocity vector of gyro to measure is ω ^b, the angular velocity error of gyro to measure is δ ω ^b, revised angular velocity vector is

Then the correction of inertia measurement signal errors is carried out according to following formula:

${\hat{f}}^b=f^b-{\mathrm{\δf}}^b---\left(14\right)$

${\widehat{\mathrm{\ω}}}^b={\mathrm{\ω}}^b-\mathrm{\δ}{\mathrm{\ω}}^b$

Wherein, δ f ^bWith δ ω ^bInitial value be:

δf ^b＝δω ^b＝[0 0 0] ^T (15)

If i is that relative ground is static constantly, then after finishing Kalman filtering calculating, from filter state, extract accelerometer bias error b _aWith gyro zero inclined to one side error b _g, at first be calculated according to the following formula δ f ^bWith δ ω ^b:

$\mathrm{\δ}{\hat{f}}^b={\mathrm{\δf}}^b+b_a$

(16)

δω ^b＝δω ^b+b _g

Then make the b in the system state _aAnd b _gZero clearing, that is:

b _a＝b _g＝[0 0 0] ^T (17)

If i is not that relative ground is static constantly, then δ f ^bWith δ ω ^bConstant.

In the present embodiment, in the described step (5) navigational parameter is carried out the concrete steps of error correction:

If i constantly inertial navigation system outgoing position is P=[L λ h] ^T, site error is δ P=[δ L δ λ δ h] ^T, revised position is $\hat{P}={\left[\begin{array}{ccc}\hat{L}& \widehat{\mathrm{\λ}}& \hat{h}\end{array}\right]}^T;$ The speed of inertial navigation system output is v=[v _nv _ev _d] ^T, velocity error is δ v=[δ v _nδ v _eδ v _d] ^T, revised speed is $\hat{v}={\left[\begin{array}{ccc}{\hat{v}}_n& {\hat{v}}_e& {\hat{v}}_d\end{array}\right]}^T;$ The attitude direction cosine matrix of inertial navigation system output is

Attitude error is δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^T, revised attitude direction cosine matrix is

When i is relative ground when static constantly, after finishing Kalman filtering and calculating, from filter state, extract velocity error and attitude error, the correction of navigational parameter is carried out according to following formula:

$\hat{P}=P-\mathrm{\δP}$

$\hat{v}=v-\mathrm{\δv}---\left(18\right)$

${\hat{C}}_b^n=C_b^n[I+\mathrm{\δ\ψ}\×]$

δ ψ * be the antisymmetric matrix of δ ψ.δ P is calculated according to the following formula:

$\mathrm{\δP}=\frac{t}{2}{\left[\begin{array}{ccc}\frac{{\mathrm{\δv}}_n}{R+h}& \frac{{\mathrm{\δv}}_e}{(R+h)\cos L}& {\mathrm{\δv}}_d\end{array}\right]}^T---\left(19\right)$

Then make δ v and δ ψ zero clearing in the system state, that is:

δv＝δψ＝[0 0 0] ^T (20)

Here, t represents a time that is carved into current time when static over the ground, and I represents the 3*3 unit matrix.

If i is not that relative ground is static constantly, then do not carry out the navigational parameter error correction.

Below only be preferred implementation of the present invention, protection scope of the present invention also not only is confined to above-described embodiment, and all technical schemes that belongs under the thinking of the present invention all belong to protection scope of the present invention.Should be pointed out that for those skilled in the art the some improvements and modifications not breaking away under the principle of the invention prerequisite should be considered as protection scope of the present invention.

Claims (8)

1. an endless-track vehicle is characterized in that step is with the data processing method of strapdown inertial navigation system:

(1) Inertial Measurement Unit is fixedly connected with the crawler belt of endless-track vehicle;

(2) strap-down inertial calculates: according to position, speed and the attitude of the inertia measurement calculated signals carrier of Inertial Measurement Unit output;

(3) static detection over the ground: the process to the crawler belt kiss the earth at Inertial Measurement Unit mounting points place detects; Namely the static process of Inertial Measurement Unit with respect to ground detected;

(4) error is calculated: utilize Inertial Measurement Unit with respect to the static process on ground, constantly inertia measurement signal and navigational parameter are carried out error calculating;

(5) error correction: according to the inertia measurement signal errors that calculates and navigational parameter error, inertia measurement signal and navigational parameter are carried out error correction.

2. endless-track vehicle according to claim 1 is with the data processing method of strapdown inertial navigation system, it is characterized in that, described step (3) is to utilize the gyro in the Inertial Measurement Unit or/and the signal that accelerometer measures obtains judges whether Inertial Measurement Unit is static relative to the earth.

3. endless-track vehicle according to claim 2 is characterized in that with the data processing method of strapdown inertial navigation system, is located at the ratio force vector of constantly accelerometer output of i or the angular velocity vector of gyro output in the described step (3) to be Then static detection comprises following step:

1) compute vectors amplitude

Namely calculate

$v_i^b=\left|v_i^b\right|---\left(1\right)$

2) average of the vector magnitude sequence in calculating a period of time And variance

Namely

${\stackrel{\‾}{v}}_i^b=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{v}_i^b$

(2)

${\mathrm{\σ}}_{v_i}^2=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{(v_i^b-{\stackrel{\‾}{v}}_i^b)}^2$

Here, N is the length of time window;

3) judge: according to the error characteristics of gyro and accelerometer, select static threshold T _vIf

Judge that then i is constantly static over the ground; If adopt and to unite judgement than force vector and angular velocity vector, then need to select respectively two threshold values, i.e. gyro static threshold and accelerometer static threshold are if the variance of two amplitude of the vector sequences all less than the static threshold of correspondence, judges that then this moment is static over the ground.

4. endless-track vehicle according to claim 1 is characterized in that with the data processing method of strapdown inertial navigation system, and described step (3) is to utilize auxiliary pressure sensor signal to judge whether Inertial Measurement Unit is static relative to the earth.

5. the described endless-track vehicle of any one data processing method of strapdown inertial navigation system according to claim 1～4, it is characterized in that, the concrete steps that error is calculated in the described step (4) are: set up the Kalman filter model, when detecting the relative ground of Inertial Measurement Unit when static, carry out Kalman filtering and calculate, obtain the error of inertia measurement signal and navigational parameter.

6. endless-track vehicle according to claim 5 is characterized in that described Kalman filter model comprises: a system equation and an observation equation with the data processing method of strapdown inertial navigation system;

The system equation of Kalman filter model is:

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(3\right)$

Here,

X＝[δv _n δv _e δv _d δψ _n δψ _e δψ _d b _ax b _ay b _az b _gx b _gy b _gz] ^T (4)

$F=\left[\begin{array}{cccc}{F}_w& F_{\mathrm{v\ψ}}& C_b^n& 0_{3*3}\\ F_{\mathrm{\ψv}}& F_{\mathrm{\ψ\ψ}}& 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\end{array}\right]---\left(5\right)$

$G=\left[\begin{array}{cc}{C}_b^n& 0_{3*3}\\ 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}\end{array}\right]---\left(6\right)$

W＝[w _ax w _ay w _az w _gx w _gy w _gz] ^T (7)

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{v_d}{R+h}& -2(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ 2\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& \frac{v_d+v_n\tan L}{R+h}& 2\mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ -\frac{{2v}_n}{R+h}& -2(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(8\right)$

$F_{\mathrm{v\ψ}}=\left(C_b^n{f}^b\right)\×---\left(9\right)$

$F_{\mathrm{\ψv}}=\left[\begin{array}{ccc}0& \frac{1}{R+h}& 0\\ \frac{-1}{R+h}& 0& 0\\ 0& \frac{-\tan L}{R+h}& 0\end{array}\right]---\left(10\right)$

$F_{\mathrm{\ψ\ψ}}=\left[\begin{array}{ccc}0& -(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ \mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& 0& \mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ \frac{{-v}_n}{R+h}& -(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(11\right)$

Wherein: Ω represents rotational-angular velocity of the earth; L represents local latitude; R represents earth radius; H represents local sea level elevation; $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ Ratio force vector for accelerometer measures; 0 _3*3The null matrix of expression 3*3; Direction cosine matrix between expression carrier system and the navigation system; V=[v _nv _ev _d] ^TEast northeast ground velocity for inertial navigation system output;

δ v=[δ v _nδ v _eδ v _d] ^TBe system's east northeast ground velocity error; δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^TBe system's attitude error;

b _a=[b _Axb _Ayb _Az] ^TBe the accelerometer bias error; b _g=[b _Gxb _Gyb _Gz] ^TBe gyro zero inclined to one side error;

w _a=[w _Axw _Ayw _Az] ^TBe the accelerometer white noise; w _g=[w _Gxw _Gyw _Gz] ^TBe the gyro white noise; T represents transposition, the antisymmetric matrix of * expression vector.

The observation equation of Kalman filter model is:

z＝HX+υ (12)

Here,

z＝[δv _n δv _e δv _d] ^T

(13)

H＝[I _3*3 0 _3*3 0 _3*3 0 _3*3]

Wherein, I _3*3Unit matrix for 3*3; υ is the velocity survey noise.

7. the described endless-track vehicle of any one data processing method of strapdown inertial navigation system according to claim 1～5, it is characterized in that, in the described step (5) the inertia measurement signal is carried out the concrete steps of error correction: the ratio force vector that is located at i moment accelerometer measures is f ^b, the specific force vector error of accelerometer measures is δ f ^b, revisedly than force vector be

The angular velocity vector of gyro to measure is ω ^b, the angular velocity error of gyro to measure is δ ω ^b, revised angular velocity vector is

Then the correction of inertia measurement signal errors is carried out according to following formula:

${\hat{f}}^b=f^b-{\mathrm{\δf}}^b$

(14)

${\widehat{\mathrm{\ω}}}^b={\mathrm{\ω}}^b-\mathrm{\δ}{\mathrm{\ω}}^b$

Wherein, δ f ^bWith δ ω ^bInitial value be:

δf ^b＝δω ^b＝[0 0 0] ^T (15)

If i is that relative ground is static constantly, then after finishing Kalman filtering calculating, from filter state, extract accelerometer bias error b _aWith gyro zero inclined to one side error b _g, at first be calculated according to the following formula δ f ^bWith δ ω ^b:

$\mathrm{\δ}{\hat{f}}^b={\mathrm{\δf}}^b+b_a---\left(16\right)$

δω ^b＝δω ^b+b _g

Then make the b in the system state _aAnd b _gZero clearing, namely

b _a＝b _g＝[0 0 0] ^T (17)

If i is not that relative ground is static constantly, then δ f ^bWith δ ω ^bConstant.

8. the described endless-track vehicle of any one data processing method of strapdown inertial navigation system according to claim 1～5, it is characterized in that, in the described step (5) navigational parameter is carried out the concrete steps of error correction: establishing i moment inertial navigation system outgoing position is P=[L λ h] ^T, site error is δ P=[δ L δ λ δ h] ^T, revised position is $\hat{P}={\left[\begin{array}{ccc}\hat{L}& \widehat{\mathrm{\λ}}& \hat{h}\end{array}\right]}^T;$ The speed of inertial navigation system output is v=[v _nv _ev _d] ^T, velocity error is δ v=[δ v _nδ v _eδ v _d] ^T, revised speed is $\hat{v}={\left[\begin{array}{ccc}{\hat{v}}_n& {\hat{v}}_e& {\hat{v}}_d\end{array}\right]}^T;$ The attitude direction cosine matrix of inertial navigation system output is

Attitude error is δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^T, revised attitude direction cosine matrix is

When i is relative ground when static constantly, after finishing Kalman filtering and calculating, from filter state, extract velocity error and attitude error, the correction of navigational parameter is carried out according to following formula:

$\hat{P}=P-\mathrm{\δP}$

$\hat{v}=v-\mathrm{\δv}---\left(18\right)$

${\hat{C}}_b^n=C_b^n[I+\mathrm{\δ\ψ}\×]$

δ ψ * be the antisymmetric matrix of δ ψ; δ P is calculated according to the following formula:

$\mathrm{\δP}=\frac{t}{2}{\left[\begin{array}{ccc}\frac{{\mathrm{\δv}}_n}{R+h}& \frac{{\mathrm{\δv}}_e}{(R+h)\cos L}& {\mathrm{\δv}}_d\end{array}\right]}^T---\left(19\right)$

Then make δ v and δ ψ zero clearing in the system state, namely

δv＝δψ＝[0 0 0] ^T (20)

Here, t represents a time that is carved into current time when static over the ground, and I represents the 3*3 unit matrix.

Images