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

Abstract

A data processing method for endless-track vehicle strapdown inertial navigation system, the steps include: that Inertial Measurement Unit is fixedly connected with the crawler belt of endless-track vehicle by (1); (2) strap-down inertial calculates; (3) static detection over the ground; (4) error calculation; (5) error correction.The orientation problem of endless-track vehicle is converted to the orientation problem of crawler belt by the present invention, utilize the singularity of caterpillar drive, whether the Inertial Measurement Unit that constantly detection is fixedly connected with crawler belt is relative to ground static, and calculate and revise the error of inertia measurement signal and strap-down inertial parameter when its relative ground static, thus effectively inhibit the positioning error of strap-down inertial system, substantially increase inertia system positioning precision.

Description

A kind of data processing method of endless-track vehicle 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 being mainly 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, for the high-precision independent navigation and localization of endless-track vehicle under circumstances not known, have very important significance in some typical apply.

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

Strapdown inertial navigation system is a kind of typical dead reckoning system, and due to the impact of sensor error and initial error, simple inertial navigation system, its positioning error is ever-increasing in time.Therefore, in order in satisfied application to the requirement of navigation positioning error bounded, usually adopt inertia together with other navigation means, form integrated navigation system, such as conventionally comprise inertia/satellite combined guidance system or inertia/odometer integrated navigation system.

Wherein, inertia/satellite combined guidance system can provide the navigator fix result of high precision, error bounded for carrier, but owing to relying on satellite, therefore loses its independence.Although inertia/odometer integrated navigation system had both maintained the independence of system, substantially increase the navigation and positioning accuracy of system simultaneously, but its positioning error still increases with the growth of range ability, such as usually its error is at about 5 ‰ of stroke, and namely its error remains unbounded.

Up to the present, also there is no relevant measuring equipment and method in prior art, other navigation means can not be relied on, the error of endless-track vehicle strapdown inertial navigation system is effectively suppressed, 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 is just: the technical matters existed for prior art, the invention provides one and can not rely on other navigation means, calculate the error of sensor and navigational parameter in Inertial Measurement Unit and carry out revising and then can effectively suppressing the positioning error of strap-down inertial system, greatly improve the data processing method of the endless-track vehicle strapdown inertial navigation system of strap-down inertial system positioning precision.

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

A data processing method for endless-track vehicle strapdown inertial navigation system, the steps include:

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

(2) strap-down inertial calculates: calculate the position of carrier, speed and attitude according to the inertia measurement signal that Inertial Measurement Unit exports;

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

(4) error calculation: utilize Inertial Measurement Unit relative to ground static process, carries out error calculation constantly to inertia measurement signal and navigational parameter;

(5) error correction: according to the inertia measurement signal errors calculated and navigational parameter error, carries out error correction to inertia measurement signal and navigational parameter.

As a further improvement on the present invention:

Described step (3) utilizes the gyro in Inertial Measurement Unit or/and the signal that obtains of accelerometer measures, judges that whether Inertial Measurement Unit is static relative to the earth.The angular velocity vector of the ratio force vector or gyro output that are located at the output of i moment accelerometer in 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 a period of time is calculated 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, then judge that the i-th moment is static over the ground; Combine judgement according to specific force vector angular velocity vector, then need to select two threshold values respectively, i.e. gyro static threshold and accelerometer static threshold, if the variance of two amplitude of the vector sequences is all less than corresponding static threshold, then judge that this moment is static over the ground.

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

The concrete steps that described step (4) medial error calculates are: set up following Kalman filter model, when the relative ground static of Inertial Measurement Unit being detected, carry out Kalman filter calculating, obtain the error of inertia measurement signal and navigational parameter.Described Kalman filter model, comprise a system equation and an observation equation, 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δψ _db _axb _ayb _azb _gxb _gyb _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 _axw _ayw _azw _gxw _gyw _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$ For the ratio force vector of accelerometer measures; 0 _3*3represent the null matrix of 3*3; represent carrier system and the direction cosine matrix between being that navigates; V=[v _nv _ev _d] ^tfor the east northeast ground velocity that inertial navigation system exports;

δ v=[δ v _nδ v _eδ v _d] ^tfor system east northeast ground velocity error; δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^tfor posture error;

B _a=[b _axb _ayb _az] ^tfor accelerometer bias error; b _g=[b _gxb _gyb _gz] ^tfor gyro zero error partially;

W _a=[w _axw _ayw _az] ^tfor accelerometer white noise; w _g=[w _gxw _gyw _gz] ^tfor gyro white noise.T represents transposition,

× represent vectorial antisymmetric matrix.

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

z＝HX+υ (12)

Here,

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

(13)

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

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

In step (5), inertia measurement signal is carried out to the concrete steps of error correction: the ratio force vector being 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 performs according to the 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 the i moment is relative ground static, then, after completing Kalman filter calculating, from filter state, extract accelerometer bias error b _awith gyro zero error b partially _g, calculate δ f first according to the following formula ^bwith δ ω ^b:

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

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

Then the b in system state is made _aand b _greset, that is:

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

If the i moment is not relative ground static, then δ f ^bwith δ ω ^bconstant.

In step (5), navigational parameter is carried out to the concrete steps of error correction: set i moment inertial navigation system outgoing position as 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 that inertial navigation system exports 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 that inertial navigation system exports is attitude error is δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^t, revised attitude direction cosine matrix is

When the i moment is relative ground static, after completing Kalman filter calculating, from filter state, extract velocity error and attitude error, the correction of navigational parameter performs according to the 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 calculates 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 the δ v in system state and δ ψ is made to reset, that is:

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

Here, t represents one, and Still time is to the time of current time over the ground, and I represents 3*3 unit matrix.

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

Compared with prior art, the invention has the advantages that: the orientation problem that the present invention will be converted into the orientation problem of crawler belt car body certain point on crawler belt.The motion of crawler belt has singularity, a point of fixity from crawler belt, its motion was made up of two different stages, one is the stage static relative to earth surface, namely with stage of earth surface, another is the stage relative to earth surface motion, and constantly replacing of these two stages, constitutes the entire motion process of this point of fixity on crawler belt.Utilize this singularity that on crawler belt, certain a bit moves, Inertial Measurement Unit in 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 kiss the earth process, Inertial Measurement Unit carries out static detection over the ground relative to the stationary state on ground, and then according to the difference between the output valve of system during stationary state and theoretical value, calculate the error of Inertial Measurement Unit and navigational parameter, and it is revised, restrained effectively the positioning error of strap-down inertial system, substantially increase the positioning precision of inertia system.

Accompanying drawing explanation

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

Fig. 2 be the present invention in a particular embodiment Inertial Measurement Unit be arranged on the structural representation on creeper truck crawler belt.

Fig. 3 is the treatment scheme schematic 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 data processing method of endless-track vehicle strapdown inertial navigation system of the present invention, its concrete steps are:

(1) be fixedly connected with (shown in Figure 2) with the crawler belt 1 of endless-track vehicle by Inertial Measurement Unit 1, be fixedly mounted on the crawler belt 2 of creeper truck by Inertial Measurement Unit 1, Inertial Measurement Unit 1 moves with the motion of crawler belt 2.Inertial Measurement Unit 1 is generally made up of multiple gyro and accelerometer, for measure carrier angular velocity and than force vector, the output of Inertial Measurement Unit 1 is called inertia measurement signal.

(2) strap-down inertial calculates: calculate the position of carrier, speed and attitude according to the inertia measurement signal that Inertial Measurement Unit 1 exports;

(3) static detection over the ground: the process of crawler belt 2 kiss the earth at Inertial Measurement Unit 1 mounting points place is detected; Namely Inertial Measurement Unit 1 is relative to the quiescing process on ground; Namely, can be utilize information that in Inertial Measurement Unit 1, sensor measurement obtains or to be arranged on crawler belt 2 and other sensor information near Inertial Measurement Unit 1, carry out processing and judging, to determine whether Inertial Measurement Unit 1 is in the state of relative ground static.Because certain point of Inertial Measurement Unit 1 and crawler belt 2 is fixedly connected with, therefore also namely judge that whether this point of crawler belt 2 is static relative to the earth.

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

In other embodiments, this step (3) utilizes other sensor signals, such as: auxiliary pressure sensor signal, judges that whether Inertial Measurement Unit 1 is static relative to the earth.

(4) error calculation: utilize Inertial Measurement Unit 1 relative to the information of ground static, constantly carry out the error calculation of inertia measurement signal 1 and inertial navigation parameter; When Inertial Measurement Unit 1 being detected relative to ground static, in theory in Inertial Measurement Unit 1 gyro to measure be the angular velocity of the earth relative to inertial coordinates system, accelerometer measures be earth local gravity, the speed on relative ground is zero, and the acceleration on relative ground is zero.And the real output value that Inertial Measurement Unit 1 and inertial navigation calculate, due to error, different from corresponding theoretical value, these difference to be compared, treatment and analysis, the error that Inertial Measurement Unit 1 and inertial navigation export can be obtained.It can adopt " Zero velocity Updating algorithm ", and namely when Inertial Measurement Unit 1 mounting points is relative to ground static, then its speed relative to the earth is zero in theory, and the speed that inertial navigation system calculates, due to the impact of various error component, and non-vanishing.So, the speed that inertial navigation system calculates is exactly its velocity error, sets up Kalman filter model, utilizes Kalman filter algorithm, using this velocity error as observed quantity, can estimate the various errors of inertial navigation system.Concrete Kalman filter model comprises system equation and observation equation.

(5) error correction: according to the inertia measurement signal errors calculated and navigational parameter error, carries out error correction to inertia measurement signal and navigational parameter.

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

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

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

In the present embodiment, if the angular velocity vector that the ratio force vector of i moment accelerometer output or gyro export 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 a period of time is calculated 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, then judge that the i-th moment is static over the ground; Combine judgement according to specific force vector angular velocity vector, then need to select two threshold values respectively, i.e. gyro static threshold and accelerometer static threshold, if the variance of two amplitude of the vector sequences is all less than corresponding static threshold, then judge that this moment is static over the ground.

In the present embodiment, the concrete steps of error calculation are: set up following Kalman filter model, when detect Inertial Measurement Unit 1 relatively ground static time, carry out Kalman filter calculating, obtain the error of inertia measurement signal and navigational parameter.Kalman filter model, comprise a system equation and an observation equation, 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δψ _db _axb _ayb _azb _gxb _gyb _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 _axw _ayw _azw _gxw _gyw _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$ For the ratio force vector of accelerometer measures; 0 _3*3represent the null matrix of 3*3; represent carrier system and the direction cosine matrix between being that navigates; V=[v _nv _ev _d] ^tfor the east northeast ground velocity that inertial navigation system exports;

δ v=[δ v _nδ v _eδ v _d] ^tfor system east northeast ground velocity error; δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^tfor posture error;

B _a=[b _axb _ayb _az] ^tfor accelerometer bias error; b _g=[b _gxb _gyb _gz] ^tfor gyro zero error partially;

W _a=[w _axw _ayw _az] ^tfor accelerometer white noise; w _g=[w _gxw _gyw _gz] ^tfor gyro white noise.T represents the antisymmetric matrix of transposition × expression vector.

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

z＝HX+υ (12)

Here,

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

(13)

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

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

In the present embodiment, in described step (5) to the concrete steps that inertia measurement signal errors is revised be:

The ratio force vector being 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 performs according to the 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 the i moment is relative ground static, then, after completing Kalman filter calculating, from filter state, extract accelerometer bias error b _awith gyro zero error b partially _g, calculate δ f first according to the following formula ^bwith δ ω ^b:

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

(16)

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

Then the b in system state is made _aand b _greset, that is:

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

If the i moment is not relative ground static, then δ f ^bwith δ ω ^bconstant.

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

If 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 that inertial navigation system exports 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 that inertial navigation system exports is attitude error is δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^t, revised attitude direction cosine matrix is

When the i moment is relative ground static, after completing Kalman filter calculating, from filter state, extract velocity error and attitude error, the correction of navigational parameter performs according to the 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 calculates 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 the δ v in system state and δ ψ is made to reset, that is:

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

Here, t represents one, and Still time is to the time of current time over the ground, and I represents 3*3 unit matrix.

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

Below be only the preferred embodiment of the present invention, protection scope of the present invention be not only confined to above-described embodiment, all technical schemes belonged under thinking of the present invention all belong to protection scope of the present invention.It should be pointed out that for those skilled in the art, some improvements and modifications without departing from the principles of the present invention, should be considered as protection scope of the present invention.

Claims (8)

1. a data processing method for endless-track vehicle strapdown inertial navigation system, is characterized in that, step is:

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

(2) strap-down inertial calculates: calculate the position of carrier, speed and attitude according to the inertia measurement signal that Inertial Measurement Unit exports;

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

(4) error calculation: utilize Inertial Measurement Unit relative to the quiescing process on ground, constantly carries out error calculation to inertia measurement signal and navigational parameter;

(5) error correction: according to the inertia measurement signal errors calculated and navigational parameter error, carries out error correction to inertia measurement signal and navigational parameter.

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

3. the data processing method of endless-track vehicle strapdown inertial navigation system according to claim 2, is characterized in that, the angular velocity vector of the ratio force vector or gyro output that are located at the output of i moment accelerometer in 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 a period of time is calculated and variance namely

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

${\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 _v; If then judge that the i-th moment is static over the ground; Combine judgement according to specific force vector angular velocity vector, then need to select two threshold values respectively, i.e. gyro static threshold and accelerometer static threshold, if the variance of two amplitude of the vector sequences is all less than corresponding static threshold, then judge that this moment is static over the ground.

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

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

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

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δψ _db _axb _ayb _azb _gxb _gyb _gz] ^T(4)

$F=\left[\begin{array}{cccc}{F}_{\mathrm{vv}}& 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 _axw _ayw _azw _gxw _gyw _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$ For the ratio force vector of accelerometer measures; 0 _3*3represent the null matrix of 3*3; represent carrier system and the direction cosine matrix between being that navigates; V=[v _nv _ev _d] ^tfor the east northeast ground velocity that inertial navigation system exports; δ v=[δ v _nδ v _eδ v _d] ^tfor system east northeast ground velocity error; δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^tfor posture error; b _a=[b _axb _ayb _az] ^tfor accelerometer bias error; b _g=[b _gxb _gyb _gz] ^tfor gyro zero error partially; w _a=[w _axw _ayw _az] ^tfor accelerometer white noise; w _g=[w _gxw _gyw _gz] ^tfor gyro white noise; T represents transposition, × represent vectorial antisymmetric matrix;

The observation equation of Kalman filter model is:

z＝HX+υ (12)

Here,

z＝[δv _nδv _eδv _d] ^T(13)

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

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

7. according to the data processing method of the endless-track vehicle strapdown inertial navigation system in Claims 1 to 4 described in any one, it is characterized in that, in described step (5), inertia measurement signal is carried out to the concrete steps of error correction: the ratio force vector being 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 performs according to the 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 the i moment is relative ground static, then, after completing Kalman filter calculating, from filter state, extract accelerometer bias error b _awith gyro zero error b partially _g, calculate δ f first according to the following formula ^bwith δ ω ^b:

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

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

Then the b in system state is made _aand b _greset, namely

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

If the i moment is not relative ground static, then δ f ^bwith δ ω ^bconstant.

8. according to the data processing method of the endless-track vehicle strapdown inertial navigation system in Claims 1 to 4 described in any one, it is characterized in that, in described step (5), navigational parameter is carried out to the concrete steps of error correction: set i moment inertial navigation system outgoing position as 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 that inertial navigation system exports 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 that inertial navigation system exports is attitude error is δ ψ=[δ ψ _nδ ψ _eδ ψ _d] ^t, revised attitude direction cosine matrix is

When the i moment is relative ground static, after completing Kalman filter calculating, from filter state, extract velocity error and attitude error, the correction of navigational parameter performs according to the 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 calculates 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 the δ v in system state and δ ψ is made to reset, namely

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

Here, t represents one, and Still time is to the time of current time over the ground, and I represents 3*3 unit matrix.