CN103630146A - Laser gyroscope IMU (inertial measurement unit) calibration method combining discrete analysis and Kalman filtration - Google Patents

Abstract

The invention provides a laser gyroscope IMU (inertial measurement unit) calibration method combining discrete analysis and Kalman filtration. The method combines the advantages of two calibration methods, namely discrete analysis and system-level filtration. All 24 calibration coefficients of an angular speed and acceleration channel are preliminarily calibrated by a four-direction rotating rate discrete analysis method according to an angular speed and acceleration channel error model of a laser gyroscope IMU; then error values of the calibration coefficients are estimated by a Kalman filtration method with a lever-arm effect compensation function by taking the error values of the calibration coefficients as state variables, and the precise calibration coefficients are corrected. According to the method, errors caused by random errors such as deformation of a shock absorption device, and drifting of a gyroscope and an accelerator in a calibration process are eliminated, the precision of the calibration coefficient is improved, and the error of navigation calculation is alleviated.

Description

The laser gyro IMU scaling method that a kind of discrete parsing is combined with Kalman filtering

Technical field

The present invention relates to the laser gyro IMU scaling method that a kind of discrete parsing is combined with Kalman filtering, can be applied to Accurate Calibration position and attitude measuring system (Position and Or ientation System, POS) and inertial navigation system (Inertial Navigation System, INS), and INS/GPS (Global Position System, GPS) calibration coefficient of integrated navigation system, the precision of raising navigation calculation.

Background technology

Inertial Measurement Unit (Inertial Measurement Unit, IMU) be the core component of the inertial measurement systems such as POS, INS, INS/GPS, inertial measurement system is to take Newton mechanics law as basis, utilize IMU to measure line motion and the angular motion parameter of carrier, under given starting condition, by computing machine, calculated the position and speed attitude parameter that obtains carrier.It is a kind of position and speed Attitude Measuring Unit of autonomous type, rely on airborne equipment independently to complete measurement task completely, there is not any optical, electrical contact with the external world, there is good concealment, work is not subject to the advantage of meteorological condition restriction, therefore at Aeronautics and Astronautics and navigational field, is widely used.IMU mainly consists of inertial measurement clusters such as gyroscope and accelerometers, and gyrostatic precision has determined the measuring accuracy of IMU to a great extent.Because laser gyro has, height is dynamic, low-cost, highly reliable, the ratio of performance to price is high, aspect military, civilian, is widely used.But in actual applications, the latch up effect of laser gyro has had a strong impact on its measuring accuracy, conventionally adopt the method for Dithered to eliminate latch up effect.Owing to there are mechanical shaking parts, in the IMU that utilizes laser gyro to form, conventionally adopt vibration absorber, by the high dither of IMU inside and extraneous isolation.

The calibration coefficient of IMU is the key factor that affects the navigational system precision such as POS, INS, INS/GPS.But in laser gyro IMU, because vibration absorber adopts elastomeric material, at IMU, demarcating in the process of upset vibration absorber can cause distortion because of gravity and outside input angular velocity, acceleration etc., thereby causes the decline of stated accuracy.In addition, in calibration process, the stochastic errors such as random drift of gyro and accelerometer can be introduced calibrated error too.

Summary of the invention

Technology of the present invention is dealt with problems and is: improve the laser gyro IMU stated accuracy with vibration absorber, the laser gyro IMU scaling method that provides a kind of discrete parsing to be combined with Kalman filtering, the method correction the calibrated error causing because of stochastic errors such as vibration absorber distortion, gyro and accelerometer drifts, improve the precision of calibration coefficient, reduced the error of navigation calculation.

Technical solution of the present invention is: the laser gyro IMU scaling method that a kind of discrete parsing is combined with Kalman filtering, its feature is to comprise the following steps:

Step (1), according to the characteristic of laser gyro and accelerometer, set up the determinacy error model of laser gyro IMU angular velocity and acceleration passage, model parameter comprises: x, y, z three axle gyro input angular velocity ω _x, ω _y, ω _z, three axle Output speed G _x, G _y, G _z, three axle gyros zero are G partially _bx, G _by, G _bz, three axle gyro constant multiplier S _x, S _y, S _z, three axle Gyro Random error delta G _x, δ G _y, δ G _z, six alignment error coefficient E of angular velocity passage _xy, E _xz, E _yx, E _yz, E _zx, E _zy; The input acceleration a of x, y, z three axis accelerometer _x, a _y, a _z, the output acceleration A of three axis accelerometer _x, A _y, A _z, three axis accelerometer zero is A partially _bx, A _by, A _bz, three axis accelerometer constant multiplier K _x, K _y, K _z, three axis accelerometer stochastic error δ A _x, δ A _y, δ A _z, and six alignment error coefficient M of acceleration passage _xy, M _xz, M _yx, M _yz, M _zx, M _zy.

Step (2), according to laser gyro IMU angular velocity and the acceleration passage ascertainment error model set up in step (1), adopt position, four directions speed of rotation discrete solution analysis method, tentatively calibrate three axle gyros zero G partially _bx, G _by, G _bz, three axle gyro constant multiplier S _x, S _y, S _z, six alignment error coefficient E of angular velocity passage _xy, E _xz, E _yx, E _yz, E _zx, E _zy, three axis accelerometer zero is A partially _bx, A _by, A _bz, three axis accelerometer constant multiplier K _x, K _y, K _z, and six alignment error coefficient M of acceleration passage _xy, M _xz, M _yx, M _yz, M _zx, M _zytotally 24 calibration coefficients.

Step (3), set up the random error model of laser gyro IMU angular velocity and acceleration passage, model parameter comprises: three axle Gyro Random error delta G _x, δ G _y, δ G _z, three axle Gyro Random migration ε _x, ε _y, ε _z, three axle Gyro Random migration drive noises

three axle gyro scale factor error δ S _x, δ S _y, δ S _z, six alignment error coefficients deviation δ E of angular velocity passage _xy, δ E _xz, δ E _yx, δ E _yz, δ E _zx, δ E _zy; Three axis accelerometer stochastic error δ A _x, δ A _y, δ A _z, three axis accelerometer random walk

three axis accelerometer random walk drives noise

, three axis accelerometer scale factor error δ K _x, δ K _y, δ K _z, acceleration passage alignment error coefficients deviation δ M _xy, δ M _xz, δ M _yx, δ M _yz, δ M _zx, δ M _zy.The ultimate principle of resolving according to random error model and inertial navigation, sets up the state equation of Kalman filtering.

Step (4), choose east, north, day to velocity error δ V _e, δ V _n, δ V _uas observed quantity z, set up the Kalman filtering measurement equation with lever arm effect compensation.

24 random error model parameter ε in step (5), use Kalman filtering estimating step (3) _x, ε _y, ε _z, δ S _x, δ S _y, δ S _z, δ E _xy, δ E _xz, δ E _yx, δ E _yz, δ E _zx, δ E _zy,

δ K _x, δ K _y, δ K _z, δ M _xy, δ M _xz, δ M _yx, δ M _yz, δ M _zx, δ M _zy, and revise with the corresponding addition of 24 calibration coefficients in step (2).Repeat this step, until all calibration coefficients are all restrained, the condition of convergence is that in continuous ten minutes, 24 calibration coefficients relatively change and are less than 0.05% before and after revising.Now obtain accurate calibration coefficient, for compensating laser gyro IMU data.

In described position, the four directions speed of rotation discrete solution analysis method of step (2), position, four directions be by laser gyro IMU adjust to respectively x, y, z ,-x axle refers to respectively sky, all the other diaxon levels; When x, y, z axle refers to respectively day, the housing axle of turntable counterclockwise at the uniform velocity rotates two weeks with the arbitrary constant angle speed between 5 °/s-100 °/s; When-x refers to day, the housing axle clockwise direction of turntable is at the uniform velocity rotated two weeks with the arbitrary constant angle speed between 5 °/s-100 °/s.

In the described Kalman filter state equation of step (4), state variable has 30 dimensions, comprise east, north, day to velocity error δ V _e, δ V _n, δ V _u, pitching, roll, course angle error φ _e, φ _n, φ _u, and three axle Gyro Random migration ε _x, ε _y, ε _z, gyro scale factor error δ S _x, δ S _y, δ S _z, six alignment error coefficients deviation δ E of angular velocity passage _xy, δ E _xz, δ E _yx, δ E _yz, δ E _zx, δ E _zy, three axis accelerometer random walk

, three axis accelerometer scale factor error δ K _x, δ K _y, δ K _z, acceleration passage alignment error coefficients deviation δ M _xy, δ M _xz, δ M _yx, δ M _yz, δ M _zx, δ M _zy.

In the described Kalman filtering measurement equation with lever arm effect compensation of step (5), measurement amount z be laser gyro IMU east after lever arm effect compensation, north, sky to velocity error δ V _e, δ V _n, δ V _u.

$Z=\left[\begin{array}{c}\mathrm{\δ}{V}_E\\ \mathrm{\δ}{V}_N\\ \mathrm{\δ}{V}_U\end{array}\right]=\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]-\left[\begin{array}{c}d{V}_{\mathrm{lE}}^n\\ d{V}_{\mathrm{lN}}^n\\ d{V}_{\mathrm{lU}}^n\end{array}\right]=\left[\begin{array}{c}{V}_E-d{V}_{\mathrm{lE}}^n\\ V_N-d{V}_{\mathrm{lN}}^n\\ V_U-d{V}_{\mathrm{lU}}^n\end{array}\right]$

Wherein, V _e, V _n, V _ube respectively laser gyro IMU east, north, day to speed,

be respectively lever arm effect in east, north, day to additional velocity error.

Principle of the present invention is: position, four directions speed of rotation discrete solution analysis method is according to the angular velocity of laser gyro IMU and acceleration passage ascertainment error model, the multivariate linear equations that foundation comprises 24 calibration coefficients, utilization solves all 24 calibration coefficients of the principle calculating laser gyro IMU of multivariate linear equations solution; Kalman filtering method is the principle of utilizing recursion minimum variance estimate, from measurement information, eliminate white Gaussian noise, extract by the information of estimated parameter, and then revise the calibration coefficient that position, four directions speed of rotation discrete solution analysis method primary Calculation goes out, finally improve calibration coefficient precision.

The present invention's advantage is compared with prior art: the present invention is on the basis of discrete parsing scaling method, the stochastic errors such as the vibration absorber distortion in calibration process, gyro and accelerometer drift have been considered, and adopt Kalman filtering method to estimate and eliminated these stochastic errors, realized the laser gyro IMU high-precision calibrating with vibration absorber, improve the precision of calibration coefficient, reduced the error of navigation calculation.

Accompanying drawing explanation

Fig. 1 is theory diagram of the present invention.

Fig. 2 is the discrete parsing scaling scheme of four directions position speed of rotation of the present invention.

Fig. 3 is the Kalman filtering of step of the present invention (5) and revises calibration coefficient process flow diagram.

Fig. 4 is Kalman filtering algorithm schematic diagram of the present invention.

Embodiment

As shown in Figure 1, concrete grammar of the present invention is as follows:

(1), according to the characteristic of laser gyro and accelerometer, set up the error model of laser gyro IMU angular velocity and acceleration passage.

Angular velocity channel error model is:

G _x=G _bx+S _xω _x+E _xyω _y+E _xzω _z+δG _x

G _y=G _by+E _yxω _x+S _yω _y+E _yzω _z+δG _y (1)

G _z=G _bz+E _zxω _x+E _zyω _y+S _zω _z+δG _z

Wherein, ω _x, ω _y, ω _zfor x, y, z three axle gyro input angular velocities, G _x, G _y, G _zbe three axle Output speed, G _bx, G _by, G _bzbe that three axle gyros zero are inclined to one side, S _x, S _y, S _zfor gyro constant multiplier, δ G _x, δ G _y, δ G _zbe three axle Gyro Random errors, E _xy, E _xz, E _yx, E _yz, E _zx, E _zysix alignment error coefficients for angular velocity passage.

Acceleration channel error model is:

A _x=A _bx+K _xa _x+M _xya _y+M _xza _z+δA _x

A _y=A _bx+M _yxa _x+K _ya _y+M _yza _z+δA _y (2)

A _z=A _bz+M _zxa _x+M _zya _y+K _za _z+δA _z

Wherein, a _x, a _y, a _zfor x, y, z three axle input accelerations, A _x, A _y, A _zbe three axle output acceleration, A _bx, A _by, A _bzfor three axis accelerometer zero partially, K _x, K _y, K _zfor constant multiplier, δ A _x, δ A _y, δ A _zfor three axis accelerometer stochastic error, M _xy, M _xz, M _yx, M _yz, M _zx, M _zysix alignment error coefficients for acceleration passage.

(2) according to laser gyro IMU angular velocity and the acceleration channel error model set up in step (1), adopt position, four directions speed of rotation discrete solution analysis method, tentatively calibrate three axle gyros zero G partially _bx, G _by, G _bz, gyro constant multiplier S _x, S _y, S _z, six alignment error coefficient E of angular velocity passage _xy, E _xz, E _yx, E _yz, E _zx, E _zy, three accelerometer bias A _bx, A _by, A _bz, three accelerometer constant multiplier K _x, K _y, K _z, and six alignment error coefficient M of acceleration passage _xy, M _xz, M _yx, M _yz, M _zx, M _zytotally 24 calibration coefficients.Concrete steps are as follows:

(a) laser gyro IMU is passed through to a special-purpose rebound, be connected with the inner axis of three-axle table.As shown in 4 orientation in Fig. 2, by laser gyro IMU adjust to respectively x ,-x, y, z axle refers to respectively sky, with turntable turning axle z _poverlap, all the other diaxon levels.When x axle as shown in orientation 1 refers to day, as shown in orientation 3, y axle refers to day and when as shown in orientation 4, z axle refers to day, the housing axle of turntable counterclockwise at the uniform velocity rotates two weeks with Constant Angular Velocity Ω (100 °/s of 5 °/s < Ω <, gets 10 °/s conventionally); When as shown in orientation in Fig. 2 2-x is when refer to day, the housing axle clockwise direction of turntable is at the uniform velocity rotated two weeks with Constant Angular Velocity Ω (100 °/s of 5 °/s < Ω <, gets 10 °/s conventionally).

(b) according to such scheme, can set up following equation:

Angular velocity passage equation:

$\left[\begin{array}{ccc}{G}_{x1}& G_{y1}& G_{z1}\\ G_{x2}& G_{y2}& G_{z2}\\ G_{x3}& G_{y3}& G_{z3}\\ G_{x4}& G_{y4}& G_{z4}\end{array}\right]=\left[\begin{array}{cccc}1& \stackrel{\&OverBar;}{\mathrm{\&omega;}}& 0& 0\\ 1& -\stackrel{\&OverBar;}{\mathrm{\&omega;}}& 0& 0\\ 1& 0& \stackrel{\&OverBar;}{\mathrm{\&omega;}}& 0\\ 1& 0& 0& \stackrel{\&OverBar;}{\mathrm{\&omega;}}\end{array}\right]\left[\begin{array}{ccc}{G}_{\mathrm{bx}}& G_{\mathrm{by}}& G_{\mathrm{bz}}\\ S_x& E_{\mathrm{xy}}& E_{\mathrm{xz}}\\ E_{\mathrm{yx}}& S_y& E_{\mathrm{yz}}\\ E_{\mathrm{zx}}& E_{\mathrm{zy}}& S_z\end{array}\right]---\left(3\right)$

Wherein,

g _x1, G _y1, G _z1be the output of inertia system x, y, z three axle gyros under the 1st orientation, G _x2, G _y2, G _z2be the output of three axle gyros under the 2nd orientation, G _x3, G _y3, G _z3be the output of three axle gyros under the 3rd orientation, G _x4, G _y4, G _z4be the output of three axle gyros under the 4th orientation, Ω is turntable input angle speed, ω _iefor earth rotation angular speed (°/h), φ is local latitude.

Acceleration passage equation:

$\left[\begin{array}{ccc}{A}_{x1}& A_{y1}& A_{z1}\\ A_{x2}& A_{y2}& A_{z2}\\ A_{x3}& A_{y3}& A_{z3}\\ A_{x4}& A_{y4}& A_{z4}\end{array}\right]=\left[\begin{array}{cccc}1& g& 0& 0\\ 1& -g& 0& 0\\ 1& 0& g& 0\\ 1& 0& 0& g\end{array}\right]\left[\begin{array}{ccc}{A}_{\mathrm{bx}}& A_{\mathrm{by}}& A_{\mathrm{bz}}\\ K_x& M_{\mathrm{yx}}& M_{\mathrm{zx}}\\ M_{\mathrm{xy}}& K_y& M_{\mathrm{zy}}\\ M_{\mathrm{xz}}& M_{\mathrm{yz}}& K_z\end{array}\right]---\left(4\right)$

Wherein, A _x1, A _y1, A _z1be the output of inertia system x, y, z three axis accelerometer under the 1st orientation, A _x2, A _y2, A _z2be the output of three axis accelerometer under the 2nd orientation, A _x3, A _y3, A _z3be the output of three axis accelerometer under the 3rd orientation, A _x4, A _y4, A _z4be the output of three axis accelerometer under the 4th orientation, g is local gravitational acceleration.

(c) solve angular velocity and acceleration passage equation, can obtain calibration coefficient.

Angular velocity passage calibration coefficient:

$\left[\begin{array}{cc}{G}_{\mathrm{bx}}& S_x\\ G_{\mathrm{by}}& E_{\mathrm{yx}}\\ G_{\mathrm{bz}}& E_{\mathrm{zx}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cc}{G}_{x2}+G_{x1}& (G_{x1}-G_{x2})/\stackrel{\&OverBar;}{\mathrm{\&omega;}}\\ G_{y2}+G_{y1}& (G_{y1}-G_{y2})/\stackrel{\&OverBar;}{\mathrm{\&omega;}}\\ G_{z2}+G_{z1}& (G_{z1}-G_{z2})/\stackrel{\&OverBar;}{\mathrm{\&omega;}}\end{array}\right]---\left(5\right)$

$\left[\begin{array}{cc}{E}_{\mathrm{xy}}& E_{\mathrm{xz}}\\ S_y& E_{\mathrm{yz}}\\ E_{\mathrm{zy}}& S_z\end{array}\right]=\frac{1}{\stackrel{\&OverBar;}{\mathrm{\&omega;}}}\left[\begin{array}{cc}{G}_{x3}-G_{\mathrm{bx}}& G_{x4}-G_{\mathrm{bx}}\\ G_{y3}-G_{\mathrm{by}}& G_{y4}-G_{\mathrm{by}}\\ G_{z3}-G_{\mathrm{bz}}& G_{z4}-G_{\mathrm{bz}}\end{array}\right]---\left(6\right)$

Acceleration passage calibration coefficient:

$\left[\begin{array}{ccc}{A}_{\mathrm{bx}}& A_{\mathrm{by}}& A_{\mathrm{bz}}\\ K_x& M_{\mathrm{yx}}& M_{\mathrm{zx}}\\ M_{\mathrm{xy}}& K_y& M_{\mathrm{zy}}\\ M_{\mathrm{xz}}& M_{\mathrm{yz}}& K_z\end{array}\right]=\frac{1}{2g}\left[\begin{array}{ccc}g(A_{x1}+A_{x2})& g(A_{\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA0000382317630000032.png"\; state="\{\{state\}\}">y</figure-callout>}1}+A_{y2})& g(A_{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="HDA0000382317630000032.png"\; state="\{\{state\}\}">z</figure-callout>}1}+A_{z2})\\ A_{x1}-A_{x2}& A_{y1}-A_{y2}& A_{z1}-A_{z2}\\ 2(A_{x3}-A_{\mathrm{bx}})& 2(A_{y3}-A_{\mathrm{by}})& 2(A_{z3}-A_{\mathrm{bz}})\\ 2(A_{x4}-A_{\mathrm{bx}})& 2(A_{y4}-A_{\mathrm{by}})& 2(A_{z4}-A_{\mathrm{bz}})\end{array}\right]---\left(7\right)$

(3) set up the random error model of laser gyro IMU angular velocity and acceleration passage, and according to the ultimate principle that random error model and inertial navigation resolve, set up the state equation of Kalman filtering.

(a) random error model of laser gyro IMU angular velocity and acceleration passage.

The random error model of angular velocity passage:

$\begin{array}{c}{\mathrm{\&delta;G}}_x={\mathrm{\&delta;S}}_x{\mathrm{\&omega;}}_x+\mathrm{\&delta;}{E}_{\mathrm{xy}}{\mathrm{\&omega;}}_y+{\mathrm{\&delta;E}}_{\mathrm{xz}}{\mathrm{\&omega;}}_z+{\mathrm{\&epsiv;}}_x+{w_{\mathrm{\&epsiv;}}}_x\\ {\mathrm{\&delta;G}}_y={\mathrm{\&delta;E}}_{\mathrm{yx}}{\mathrm{\&omega;}}_x+{\mathrm{\&delta;S}}_y{\mathrm{\&omega;}}_y+{\mathrm{\&delta;E}}_{\mathrm{yz}}{\mathrm{\&omega;}}_z+{\mathrm{\&epsiv;}}_y+{w_{\mathrm{\&epsiv;}}}_y\\ {\mathrm{\&delta;G}}_z={\mathrm{\&delta;E}}_{\mathrm{zx}}{\mathrm{\&omega;}}_x+{\mathrm{\&delta;E}}_{\mathrm{zy}}{\mathrm{\&omega;}}_y+{\mathrm{\&delta;S}}_z{\mathrm{\&omega;}}_z+{\mathrm{\&epsiv;}}_z+{w_{\mathrm{\&epsiv;}}}_z\end{array}---\left(8\right)$

Wherein, δ G _x, δ G _y, δ G _zrepresent three axle Gyro Random errors, ε _x, ε _y, ε _zbe three axle Gyro Random migration, for random walk drives noise, δ S _x, δ S _y, δ S _zfor gyro scale factor error, δ E _xy, δ E _xz, δ E _yx, δ E _yz, δ E _zx, δ E _zysix alignment error coefficients deviation of angular velocity passage.

The random error model of acceleration passage:

$\begin{array}{c}{\mathrm{\&delta;A}}_x={\mathrm{\&delta;K}}_x{a}_x+{\mathrm{\&delta;M}}_{\mathrm{xy}}{a}_y+{\mathrm{\&delta;M}}_{\mathrm{xz}}{a}_z+{\&dtri;}_x+{w_{\&dtri;}}_x\\ {\mathrm{\&delta;A}}_y={\mathrm{\&delta;M}}_{\mathrm{yx}}{a}_x+{\mathrm{\&delta;K}}_y{a}_y+{\mathrm{\&delta;M}}_{\mathrm{yz}}{a}_z+{\&dtri;}_y+{w_{\&dtri;}}_y\\ {\mathrm{\&delta;A}}_z={\mathrm{\&delta;M}}_{\mathrm{zx}}{a}_x+{\mathrm{\&delta;M}}_{\mathrm{zy}}{a}_y+{\mathrm{\&delta;K}}_z{a}_z+{\&dtri;}_z+{w_{\&dtri;}}_z\end{array}---\left(9\right)$

Wherein, δ A _x, δ A _y, δ A _zrepresent 3-axis acceleration stochastic error,

for accelerometer random walk,

for random walk drives noise, δ K _x, δ K _y, δ K _zfor accelerometer scale factor error, δ M _xy, δ M _xz, δ M _yx, δ M _yz, δ M _zx, δ M _zyfor acceleration passage alignment error coefficients deviation.

(b) ultimate principle of resolving according to laser gyro random error model and inertial navigation, sets up the state equation of Kalman filtering.

Choose east, north, day to velocity error δ V _e, δ V _n, δ V _u, pitching, roll, course angle error φ _e, φ _n, φ _u, and random error model parameter ε in step (3) _x, ε _y, ε _z, δ S _x, δ S _y, δ S _z, δ E _xy, δ E _xz, δ E _yx, δ E _yz, δ E _zx, δ E _zy,

δ K _x, δ K _y, δ K _z, δ M _xy, δ M _xz, δ M _yx, δ M _yz, δ M _zx, δ M _zyfor state variable X, the dimension of X is 30 dimensions.

State equation is:

$\stackrel{\&CenterDot;}{X}=F\&CenterDot;X+G\&CenterDot;w---\left(10\right)$

Wherein, X is system state variables:

$\begin{array}{c}X=\left[\begin{array}{cccccccccccccccccc}{\mathrm{\&delta;V}}_E& {\mathrm{\&delta;V}}_N& {\mathrm{\&delta;V}}_U& {\mathrm{\&phi;}}_E& {\mathrm{\&phi;}}_N& {\mathrm{\&phi;}}_U& {\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z& {\mathrm{\&delta;K}}_x& {\mathrm{\&delta;K}}_y& {\mathrm{\&delta;K}}_z& {\mathrm{\&delta;M}}_{\mathrm{xy}}& {\mathrm{\&delta;M}}_{\mathrm{xz}}& {\mathrm{\&delta;M}}_{\mathrm{yx}}& {\mathrm{\&delta;M}}_{\mathrm{yz}}& {\mathrm{\&delta;M}}_{\mathrm{zx}}& {\mathrm{\&delta;M}}_{\mathrm{zy}}\end{array}\right.\\ {\left.\begin{array}{cccccccccccc}{\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& {\mathrm{\&delta;S}}_x& {\mathrm{\&delta;S}}_y& {\mathrm{\&delta;S}}_z& {\mathrm{\&delta;E}}_{\mathrm{xy}}& {\mathrm{\&delta;E}}_{\mathrm{xz}}& {\mathrm{\&delta;E}}_{\mathrm{yx}}& {\mathrm{\&delta;E}}_{\mathrm{yz}}& {\mathrm{\&delta;E}}_{\mathrm{zx}}& {\mathrm{\&delta;E}}_{\mathrm{zy}}\end{array}\right]}^T\end{array}---\left(11\right)$

W is system noise vector:

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

G is noise battle array:

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

be from IMU carrier, to be tied to the attitude transition matrix of navigation system, with θ, φ, γ, represent respectively course, pitching, the roll angle of IMU:

$C_b^n=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&phi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}& \cos \mathrm{\&theta;}\sin \mathrm{\&phi;}& \sin \mathrm{\&gamma;}\cos \mathrm{\&phi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}\\ -\cos \mathrm{\&gamma;}\sin \mathrm{\&phi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&phi;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&phi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}\\ -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(14\right)$

T _ij(i, j=1,2,3) represent attitude transition matrix

9 elements.

F is system transition matrix:

$F=\left[\begin{array}{ccc}{A}_{3\&times;6}& C_{3\&times;12}& 0_{3\&times;12}\\ B_{3\&times;6}& 0_{3\&times;12}& D_{3\&times;12}\\ 0_{24\&times;6}& 0_{24\&times;12}& 0_{24\&times;12}\end{array}\right]---\left(15\right)$

$A_{3\&times;6}=\left[\begin{array}{cccccc}\frac{V_N}{R}\tan L& {2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R}& {-2\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-\frac{V_E}{R}& 0& g& 0\\ {-2\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{{2V}_E\tan L}{R}& -\frac{V_U}{R}& -\frac{V_N}{R}& -g& 0& 0\\ {2\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{{2V}_E}{R}& \frac{{2V}_N}{R}& 0& 0& 0& 0\end{array}\right]---\left(16\right)$

Wherein, V _e, V _n, V _ube respectively laser gyro IMU east, north, day to speed, L is local latitude, R is earth radius, ω _iefor earth rotation angular speed, g is terrestrial gravitation acceleration.

$B_{3\&times;6}=\left[\begin{array}{cccccc}0& -\frac{1}{R}& 0& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R}& -{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-\frac{V_E}{R}\\ \frac{1}{R}& 0& 0& {-\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-\frac{V_E\tan L}{R}& 0& -\frac{V_N}{R}\\ \frac{1}{R}\tan L& 0& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}& \frac{V_N}{R}& 0\end{array}\right]---\left(17\right)$

$C_{3\&times;12}=\left[\begin{array}{cccccccccccc}{T}_{11}& T_{12}& T_{13}& T_{11}{a}_x& T_{12}{a}_y& T_{13}{a}_z& T_{11}{a}_y& T_{11}{a}_z& T_{12}{a}_x& T_{12}{a}_z& T_{13}{a}_x& T_{13}{a}_y\\ T_{21}& T_{22}& T_{23}& T_{21}{a}_x& T_{22}{a}_y& T_{23}{a}_z& T_{21}{a}_y& T_{21}{a}_z& T_{22}{a}_x& T_{22}{a}_z& T_{23}{a}_x& T_{23}{a}_y\\ T_{31}& T_{32}& T_{33}& T_{31}{a}_x& T_{32}{a}_y& T_{33}{a}_z& T_{31}{a}_y& T_{31}{a}_z& T_{32}{a}_x& T_{32}{a}_z& T_{33}{a}_x& T_{33}{a}_y\end{array}\right]---\left(18\right)$

$D_{3\&times;12}=\left[\begin{array}{cccccccccccc}{T}_{11}& T_{12}& T_{13}& T_{11}{\mathrm{\&omega;}}_x& T_{12}{\mathrm{\&omega;}}_y& T_{13}{\mathrm{\&omega;}}_z& T_{11}{\mathrm{\&omega;}}_y& T_{11}{\mathrm{\&omega;}}_z& T_{12}{\mathrm{\&omega;}}_x& T_{12}{\mathrm{\&omega;}}_z& T_{13}{\mathrm{\&omega;}}_x& T_{13}{\mathrm{\&omega;}}_y\\ T_{21}& T_{22}& T_{23}& T_{21}{\mathrm{\&omega;}}_x& T_{22}{\mathrm{\&omega;}}_y& T_{23}{\mathrm{\&omega;}}_z& T_{21}{\mathrm{\&omega;}}_y& T_{21}{\mathrm{\&omega;}}_z& T_{22}{\mathrm{\&omega;}}_x& T_{22}{\mathrm{\&omega;}}_z& T_{23}{\mathrm{\&omega;}}_x& T_{23}{\mathrm{\&omega;}}_y\\ T_{31}& T_{32}& T_{33}& T_{31}{\mathrm{\&omega;}}_x& T_{32}{\mathrm{\&omega;}}_y& T_{33}{\mathrm{\&omega;}}_z& T_{31}{\mathrm{\&omega;}}_y& T_{31}{\mathrm{\&omega;}}_z& T_{32}{\mathrm{\&omega;}}_x& T_{32}{\mathrm{\&omega;}}_z& T_{33}{\mathrm{\&omega;}}_x& T_{33}{\mathrm{\&omega;}}_y\end{array}\right]---\left(19\right)$

(4) choose δ V _e, δ V _n, δ V _uas observed quantity Z, set up the Kalman filtering measurement equation with lever arm effect compensation.

In actual calibration process, because laser gyro IMU not exclusively overlaps with the center of turntable, there is lever arm.In turntable rotation, laser gyro IMU can produce an additional linear velocity.Therefore, need to by laser gyro IMU under navigation coordinate system because the additional linear velocity of lever arm effect compensates.ω _bfor the angular velocity of rotation under carrier coordinate system, r _bshiIMU sensitivity center arrives the position vector of turntable rotation center,

it is the transition matrix of system from carrier coordinate system to navigation coordinate.

r _b＝[r _x r _y r _z] ^T ω _b＝[ω _x ω _y ω _z] ^T (20)

The velocity error being caused by lever arm effect can be expressed as under navigation coordinate system:

${\mathrm{dV}}_l^n=\left[\begin{array}{c}{\mathrm{dV}}_{\mathrm{lE}}^n\\ {\mathrm{dV}}_{\mathrm{lN}}^n\\ {\mathrm{dV}}_{\mathrm{lU}}^n\end{array}\right]=C_b^n({\mathrm{\&omega;}}_b\&times;r_b)---\left(21\right)$

The measurement equation of system:

Z＝HX+η (22)

Wherein, Z is measurement vector, and H is observing matrix, and η is measurement noise.

$Z=\left[\begin{array}{c}{\mathrm{\&delta;V}}_E\\ {\mathrm{\&delta;V}}_N\\ {\mathrm{\&delta;V}}_U\end{array}\right]=\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]-\left[\begin{array}{c}{\mathrm{dV}}_{\mathrm{lE}}^n\\ {\mathrm{dV}}_{\mathrm{lN}}^n\\ {\mathrm{dV}}_{\mathrm{lU}}^n\end{array}\right]=\left[\begin{array}{c}{V}_E{-\mathrm{dV}}_{\mathrm{lE}}^n\\ V_N-{\mathrm{dV}}_{\mathrm{lN}}^n\\ V_U-{\mathrm{dV}}_{\mathrm{lU}}^n\end{array}\right]---\left(23\right)$

H＝[I _3×3 0 _3×27] (24)

η＝[η _E η _N η _U] ^T (25)

(5) with Kalman filtering, estimate random error model parameter, and revise calibration coefficient.

As shown in Figure 3, first by current 24 calibration coefficients compensation IMU data, and bring into inertial navigation resolve solve IMU in east, north, sky to speed; Next utilize lever arm effect compensation method in step (4) compensation IMU east, north, sky to speed; Finally adopt Kalman filtering to carry out 24 random error model parameters in estimating step (3), and revise with the corresponding addition of 24 calibration coefficients in step (2).

The layout of Kalman filtering rudimentary algorithm is as shown in Figure 4:

State one-step prediction equation:

${\stackrel{\mathrm{\&Lambda;}}{X}}_{k/k-1}={\mathrm{\&phi;}}_{k,k-1}{\stackrel{\mathrm{\&Lambda;}}{X}}_{k-1}---\left(26\right)$

State Estimation accounting equation:

${\stackrel{\mathrm{\&Lambda;}}{X}}_k={\stackrel{\mathrm{\&Lambda;}}{X}}_{k/k-1}+K_k(Z_k-H_k{\stackrel{\mathrm{\&Lambda;}}{X}}_{k/k-1})---\left(27\right)$

Filtering Incremental Equation:

${\stackrel{\mathrm{\&Lambda;}}{K}}_k={\stackrel{\mathrm{\&Lambda;}}{P}}_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(28\right)$

One-step prediction square error equation:

${\stackrel{\mathrm{\&Lambda;}}{P}}_{k/k-1}={\mathrm{\&phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T---\left(29\right)$

Estimate square error equation:

${\stackrel{\mathrm{\&Lambda;}}{P}}_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(30\right)$

24 random error model parameters that Kalman filtering estimates are revised 24 calibration coefficients in step (2) by the following method:

The correction of angular velocity passage calibration coefficient:

$\left[\begin{array}{ccc}{\stackrel{\&OverBar;}{G}}_{\mathrm{bx}}& {\stackrel{\&OverBar;}{G}}_{\mathrm{by}}& {\stackrel{\&OverBar;}{G}}_{\mathrm{bz}}\\ {\stackrel{\&OverBar;}{S}}_x& {\stackrel{\&OverBar;}{E}}_{\mathrm{yx}}& {\stackrel{\&OverBar;}{E}}_{\mathrm{zx}}\\ {\stackrel{\&OverBar;}{E}}_{\mathrm{xy}}& {\stackrel{\&OverBar;}{S}}_y& {\stackrel{\&OverBar;}{E}}_{\mathrm{zy}}\\ {\stackrel{\&OverBar;}{E}}_{\mathrm{xz}}& {\stackrel{\&OverBar;}{E}}_{\mathrm{yz}}& {\stackrel{\&OverBar;}{S}}_z\end{array}\right]=\left[\begin{array}{ccc}{G}_{\mathrm{bx}}& G_{\mathrm{by}}& G_{\mathrm{bz}}\\ S_x& E_{\mathrm{yx}}& E_{\mathrm{zx}}\\ E_{\mathrm{xy}}& S_y& E_{\mathrm{zy}}\\ E_{\mathrm{xz}}& E_{\mathrm{yz}}& S_z\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_{\mathrm{gx}}& {\mathrm{\&epsiv;}}_{\mathrm{gy}}& {\mathrm{\&epsiv;}}_{\mathrm{gz}}\\ {\mathrm{\&delta;S}}_x& {\mathrm{\&delta;E}}_{\mathrm{yx}}& {\mathrm{\&delta;E}}_{\mathrm{zx}}\\ {\mathrm{\&delta;E}}_{\mathrm{xy}}& {\mathrm{\&delta;S}}_y& {\mathrm{\&delta;E}}_{\mathrm{zy}}\\ {\mathrm{\&delta;E}}_{\mathrm{xz}}& {\mathrm{\&delta;E}}_{\mathrm{yz}}& {\mathrm{\&delta;S}}_z\end{array}\right]---\left(31\right)$

Wherein,

for the three axle gyros zero that obtain after revising partially,

for revised gyro constant multiplier,

for revised six angular velocity passage alignment error coefficients.

The correction of acceleration passage calibration coefficient:

$\left[\begin{array}{ccc}{\stackrel{\&OverBar;}{A}}_{\mathrm{bx}}& {\stackrel{\&OverBar;}{A}}_{\mathrm{by}}& {\stackrel{\&OverBar;}{A}}_{\mathrm{bz}}\\ {\stackrel{\&OverBar;}{K}}_x& {\stackrel{\&OverBar;}{M}}_{\mathrm{yx}}& {\stackrel{\&OverBar;}{M}}_{\mathrm{zx}}\\ {\stackrel{\&OverBar;}{M}}_{\mathrm{xy}}& {\stackrel{\&OverBar;}{K}}_y& {\stackrel{\&OverBar;}{M}}_{\mathrm{zy}}\\ {\stackrel{\&OverBar;}{M}}_{\mathrm{xz}}& {\stackrel{\&OverBar;}{M}}_{\mathrm{yz}}& {\stackrel{\&OverBar;}{K}}_z\end{array}\right]=\left[\begin{array}{ccc}{A}_{\mathrm{bx}}& A_{\mathrm{by}}& A_{\mathrm{bz}}\\ K_x& M_{\mathrm{yx}}& M_{\mathrm{zx}}\\ M_{\mathrm{xy}}& K_y& M_{\mathrm{zy}}\\ M_{\mathrm{xz}}& M_{\mathrm{yz}}& K_z\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{\&epsiv;}}_{\mathrm{ax}}& {\mathrm{\&epsiv;}}_{\mathrm{ay}}& {\mathrm{\&epsiv;}}_{\mathrm{az}}\\ {\mathrm{\&delta;K}}_x& {\mathrm{\&delta;M}}_{\mathrm{yx}}& {\mathrm{\&delta;M}}_{\mathrm{zx}}\\ {\mathrm{\&delta;M}}_{\mathrm{xy}}& {\mathrm{\&delta;K}}_y& {\mathrm{\&delta;M}}_{\mathrm{zy}}\\ {\mathrm{\&delta;M}}_{\mathrm{xz}}& {\mathrm{\&delta;M}}_{\mathrm{yz}}& {\mathrm{\&delta;K}}_z\end{array}\right]---\left(32\right)$

Wherein, for the three axis accelerometer zero that obtains after revising partially,

for revised accelerometer constant multiplier,

for revised six acceleration passage alignment error coefficients.

Repeating step (5), until all 24 calibration coefficients are all restrained, the condition of convergence is that in continuous ten minutes, 24 calibration coefficients relatively change and are less than 0.05% before and after revising.Now obtain accurate calibration coefficient, for compensating laser gyro IMU data.

Claims (4)

1. the laser gyro IMU scaling method that discrete parsing is combined with Kalman filtering, is characterized in that comprising the following steps:

Step (1), according to the characteristic of laser gyro and accelerometer, set up the determinacy error model of laser gyro IMU angular velocity and acceleration passage, model parameter comprises: x, y, z three axle gyro input angular velocity ω _x, ω _y, ω _z, three axle gyro output angle speed G _x, G _y, G _z, three axle gyros zero are G partially _bx, G _by, G _bz, three axle gyro constant multiplier S _x, S _y, S _z, three axle Gyro Random error delta G _x, δ G _y, δ G _z, six alignment error coefficient E of angular velocity passage _xy, E _xz, E _yx, E _yz, E _zx, E _zy; The input acceleration a of x, y, z three axis accelerometer _x, a _y, a _z, three axis accelerometer output acceleration A _x, A _y, A _z, three axis accelerometer zero is A partially _bx, A _by, A _bz, three axis accelerometer constant multiplier K _x, K _y, K _z, three axis accelerometer stochastic error δ A _x, δ A _y, δ A _zsix alignment error coefficient M with acceleration passage _xy, M _xz, M _yx, M _yz, M _zx, M _zy;

Step (2), according to laser gyro IMU angular velocity and the acceleration passage ascertainment error model set up in step (1), adopt position, four directions speed of rotation discrete solution analysis method, tentatively calibrate three axle gyros zero G partially _bx, G _by, G _bz, three axle gyro constant multiplier S _x, S _y, S _z, six alignment error coefficient E of angular velocity passage _xy, E _xz, E _yx, E _yz, E _zx, E _zy, three axis accelerometer zero is A partially _bx, A _by, A _bz, three axis accelerometer constant multiplier K _x, K _y, K _zwith six alignment error coefficient M of acceleration passage _xy, M _xz, M _yx, M _yz, M _zx, M _zytotally 24 calibration coefficients;

Step (3), set up the random error model of laser gyro IMU angular velocity and acceleration passage, model parameter comprises: three axle Gyro Random error delta G _x, δ G _y, δ G _z, three axle Gyro Random migration ε _x, ε _y, ε _z, three axle Gyro Random migration drive noises

three axle gyro scale factor error δ S _x, δ S _y, δ S _z, six alignment error coefficients deviation δ E of angular velocity passage _xy, δ E _xz, δ E _yx, δ E _yz, δ E _zx, δ E _zy; Three axis accelerometer stochastic error δ A _x, δ A _y, δ A _z, three axis accelerometer random walk

three axis accelerometer random walk drives noise

three axis accelerometer scale factor error δ K _x, δ K _y, δ K _z, acceleration passage alignment error coefficients deviation δ M _xy, δ M _xz, δ M _yx, δ M _yz, δ M _zx, δ M _zy, the ultimate principle of resolving according to random error model and inertial navigation, sets up the state equation of Kalman filtering;

Step (4), choose east, north, day to velocity error δ V _e, δ V _n, δ V _uas observed quantity Z, set up the Kalman filtering measurement equation with lever arm effect compensation;

24 random error model parameter ε in step (5), use Kalman filtering estimating step (3) _x, ε _y, ε _z, δ S _x, δ S _y, δ S _z, δ E _xy, δ E _xz, δ E _yx, δ E _yz, δ E _zx, δ E _zy, δ K _x, δ K _y, δ K _z, δ M _xy, δ M _xz, δ M _yx, δ M _yz, δ M _zx, δ M _zy, and revise with the corresponding addition of 24 calibration coefficients in step (2), repeat this step, until all calibration coefficients are all restrained, the condition of convergence is that in continuous ten minutes, 24 calibration coefficients relatively change and are less than 0.05% before and after revising.

2. the laser gyro IMU scaling method that discrete parsing according to claim 1 is combined with Kalman filtering, it is characterized in that: in position, the four directions speed of rotation discrete solution analysis method described in step (2), four directions position be by laser gyro IMU adjust to respectively x, y, z ,-x axle refers to respectively sky, all the other diaxon levels; When x, y, z axle refers to respectively day, the housing axle of turntable counterclockwise at the uniform velocity rotates two weeks with the arbitrary constant angle speed between 5 °/s-100 °/s; When-x refers to day, the housing axle clockwise direction of turntable is at the uniform velocity rotated two weeks with the arbitrary constant angle speed between 5 °/s-100 °/s.

3. the laser gyro IMU scaling method that discrete parsing according to claim 1 is combined with Kalman filtering, it is characterized in that: the described Kalman of step (4) filters in state equation, state variable has 30 dimensions, comprise east, north, day to velocity error δ V _e, δ V _n, δ V _u, pitching, roll, course angle error φ _e, φ _n, φ _u, and three axle Gyro Random migration ε _x, ε _y, ε _z, three axle gyro scale factor error δ S _x, δ S _y, δ S _z, six alignment error coefficients deviation δ E of angular velocity passage _xy, δ E _xz, δ E _yx, δ E _yz, δ E _zx, δ E _zy, three axis accelerometer random walk

three axis accelerometer scale factor error δ K _x, δ K _y, δ K _z, acceleration passage alignment error coefficients deviation δ M _xy, δ M _xz, δ M _yx, δ M _yz, δ M _zx, δ M _zy.

4. the laser gyro IMU scaling method that discrete parsing according to claim 1 is combined with Kalman filtering, it is characterized in that: in the described Kalman filtering measurement equation with lever arm effect compensation of step (5), measurement amount z be laser gyro IMU east after lever arm effect compensation, north, sky to velocity error δ V _e, δ V _n, δ V _u;

Wherein, V _e, V _n, V _ube respectively laser gyro IMU east, north, day to speed, be respectively lever arm effect in east, north, day to additional velocity error.

Images