CN104596515A - Semi-physical simulation method of inertial navigation/laser velocimeter combined navigation - Google Patents

Abstract

The invention discloses a semi-physical simulation method of inertial navigation/laser velocimeter combined navigation. The semi-physical simulation method comprises the following steps of 1, installing an inertial navigation/laser velocimeter on a carrier and starting the inertial navigation/laser velocimeter by electric power, 2, setting initial parameters in a navigation computer, 3, keeping a carrier static state and acquiring and storing output data of a gyro and an accelerometer in 1h, and calculating an average value and differences of the average value and actual values so that actual noise of the gyro and the accelerometer are obtained, 4, keeping a carrier static state and carrying out static initial alignment on the inertial navigation unit for 5min, 5, moving the carrier, carrying out inertial navigation calculation, and acquiring and storing data such as inertial navigation output rate values and attitude matrix in 50s after motion, 6, projecting the rate to a laser velocimeter rate direction to obtain a reference rate, and calculating the difference of the rate and the reference rate to obtain a noise value of the output rate, and 7, setting a simulation locus to obtain an attitude matrix, a velocity component and a rate standard value, carrying fusion on the related data and the noise data obtained by the steps of 3 and 6, and carrying out inertial navigation/laser velocimeter combined navigation simulation by the simulation data of the device.

Description

The Hardware In The Loop Simulation Method of a kind of inertial navigation/laser velocimeter integrated navigation

Technical field:

The present invention relates to the Hardware In The Loop Simulation Method of a kind of inertial navigation/laser velocimeter integrated navigation, belong to technical field of inertial.

Background technology:

Laser velocimeter can measure the speed of the relative ground moving of motion carrier in real time according to Doppler shift, and its measuring error does not accumulate in time, combines can have complementary advantages with inertial navigation system, can realize complete autonomous, high precision navigator fix.

When carrying out inertial navigation/laser velocimeter integrated navigation research, generally that setting simulation track (comprises the movement velocity of carrier, position, attitude), path generator is utilized to generate accelerometer, the standard value of gyro, the velocity standard value of laser velocimeter is calculated by the information such as movement velocity of carrier in setting simulation track, and the Calibration errors of given laser velocimeter, established angle and error of fixed angles and noise, the Calibration errors of given accelerometer and gyro, zero is inclined, misalignment error and noise, the established angle of given inertial navigation and error of fixed angles, above data fusion is obtained the device simulation data for integrated navigation emulation, then these simulated datas are adopted to carry out simulation calculation and the analysis of integrated navigation.Wherein, given simulator and noise can not reflect the noisiness of laser velocimeter, accelerometer, gyro comprehensively, causes simulation result can not reflect actual conditions more all sidedly.In order to carry out inertial navigation/laser velocimeter integrated navigation research better, propose the Hardware In The Loop Simulation Method of a kind of inertial navigation/laser velocimeter integrated navigation herein.

Summary of the invention

1, goal of the invention: the object of this invention is to provide the Hardware In The Loop Simulation Method of a kind of inertial navigation/laser velocimeter integrated navigation, it can simulate inertial navigation/laser velocimeter integrated navigation process better.

2, technical scheme:

The Hardware In The Loop Simulation Method of the present invention's a kind of inertial navigation/laser velocimeter integrated navigation, the method concrete steps are as follows:

Step 1, inertial navigation/laser velocimeter combined system to be installed on carrier, and electrifying startup.

Step 2, bookbinding initial parameter (comprise initial longitude, latitude, highly, laser velocimeter scale value, laser velocimeter established angle, inertial navigation established angle) to navigational computer.

Step 3, maintenance carrier stationary, gather the output data of preservation 1 hour gyro and accelerometer.Ask for the mean value of the output data of gyro and accelerometer, by gather gyro and accelerometer data poor with its mean value respectively, obtain the actual noise of gyro and accelerometer.

Step 4, carrier keep static, and 5 minutes static initial alignments are carried out in inertial navigation.

Step 5, complete aim at after carrier setting in motion, in motion process, inertial navigation calculating is carried out in inertial navigation, gathers and the speed of the velocity amplitude that in 50 seconds, inertial navigation exports after preserving setting in motion, attitude matrix that inertial navigation exports, laser velocimeter output.

The speed that step 6, inertial navigation export is the speed under sky, northeast coordinate system, attitude matrix is exported by the rate conversion under sky, northeast coordinate system under used group coordinate system by inertial navigation, again according to inertial navigation and carrier, position relationship between laser velocimeter and carrier, the velocity projections under used group of coordinate system is obtained reference velocity to the velocity reversal of laser velocimeter.By the speed of laser velocimeter and reference velocity poor, obtain the noise figure of laser velocimeter output speed.

Step 7, setting simulation track (comprising the movement velocity of carrier, position, attitude).The attitude matrix emulating carrier is obtained by the attitude of carrier in setting simulation track, obtain the speed component of carrier under carrier system according to the movement velocity of carrier in the attitude matrix obtained and simulation track, then calculate the velocity standard value of laser velocimeter by the established angle of the speed component of carrier and given laser velocimeter, inertial navigation established angle; Path generator is utilized to generate the standard value of accelerometer, the standard value of gyro; And the Calibration errors of given accelerometer and gyro, zero inclined, misalignment error; The Calibration errors of given laser velocimeter, error of fixed angles; The error of fixed angles of given inertial navigation.The noise data obtained in these data and step 3, step 6 is merged, obtains the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation, then adopt these simulated datas to carry out inertial navigation/laser velocimeter integrated navigation emulation.

Wherein, " the obtaining the actual noise of gyro, accelerometer " described in step 3, it is as follows that it realizes procedure declaration:

Definition w _gfor gyro noise, w _afor accelerometer noise, the data gathering three the axles outputs of gyro x, y, z are respectively ω _x(1), ω _x(2) ... ω _x(n), ω _y(1), ω _y(2) ... ω _y(n), ω _z(1), ω _z(2) ... ω _zn (), mean value is respectively the data gathering three the axles outputs of accelerometer x, y, z are respectively a _x(1), a _x(2) ... a _x(n), a _y(1), a _y(2) ... a _y(n), a _z(1), a _z(2) ... a _zn (), mean value is respectively gyro noise computing formula is as follows:

$w_g\left(i\right)=\left[\begin{array}{c}{w}_{\mathrm{gx}}\left(i\right)\\ w_{\mathrm{gy}}\left(i\right)\\ w_{\mathrm{gz}}\left(i\right)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x\left(i\right)\\ {\mathrm{\ω}}_y\left(i\right)\\ {\mathrm{\ω}}_z\left(i\right)\end{array}\right]-\left[\begin{array}{c}{\stackrel{\‾}{\mathrm{\ω}}}_x\\ {\stackrel{\‾}{\mathrm{\ω}}}_y\\ {\stackrel{\‾}{\mathrm{\ω}}}_z\end{array}\right],(i=1...n)$

Accelerometer noise calculation formula is as follows:

$w_a\left(i\right)=\left[\begin{array}{c}{w}_{\mathrm{ax}}\left(i\right)\\ w_{\mathrm{ay}}\left(i\right)\\ w_{\mathrm{az}}\left(i\right)\end{array}\right]=\left[\begin{array}{c}{a}_x\left(i\right)\\ a_y\left(i\right)\\ a_z\left(i\right)\end{array}\right]-\left[\begin{array}{c}{\stackrel{\‾}{a}}_x\\ {\stackrel{\‾}{a}}_y\\ {\stackrel{\‾}{a}}_z\end{array}\right],(i=1...n)$

Wherein, " the obtaining the noise figure of laser velocimeter output speed " described in step 6, it is as follows that it realizes procedure declaration:

Carrier setting in motion, the speed having collected 50 seconds inner laser knotmeters exports speed under inertial navigation output sky, northeast coordinate system $V_E^n\left(1\right),V_E^n\left(2\right)...V_E^n\left(n\right),V_N^n\left(1\right),V_N^n\left(2\right)...V_N^n\left(n\right),V_U^n\left(1\right),V_U^n\left(2\right)...V_U^n\left(n\right),$ The attitude matrix that inertial navigation exports the established angle of the laser velocimeter of navigational computer bookbinding is θ, and the established angle of inertial navigation is α, β, γ; The speed of inertial navigation under used group of coordinate system exports and is respectively V _x(1), V _x(2) ... V _x(n), V _y(1), V _y(2) ... V _y(n), V _z(1), V _z(2) ... V _z(n).The velocity projections of inertial navigation under used group of coordinate system to the reference velocity on the velocity reversal of laser velocimeter is under the speed that inertial navigation exports under sky coordinate system northeastward forwards used group coordinate system to, computing formula is as follows:

$\left[\begin{array}{c}{V}_x\left(i\right)\\ V_y\left(i\right)\\ V_z\left(i\right)\end{array}\right]=C_n^b\left(i\right)\left[\begin{array}{c}{V}_E^n\left(i\right)\\ V_N^n\left(i\right)\\ V_U^n\left(i\right)\end{array}\right](i=1...n)$

The velocity projections of inertial navigation under used group of coordinate system is V to the reference velocity on the velocity reversal of laser velocimeter ^m(i), computing formula is as follows:

V ^m(i)＝[V _x(i)cosα+V _y(i)cosβ+V _z(i)cosγ]cosθ (i＝1...n)

The speed noise figure that laser velocimeter exports is w _l(i), computing formula is as follows:

$w_L\left(i\right)=V_L^m\left(i\right)-V^m\left(i\right),(i=1...n)$

Wherein, " obtain the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation, then adopt these simulated datas to carry out inertial navigation/odometer integrated navigation emulation " described in step 7, it is as follows that it realizes procedure declaration:

Setting simulation track (comprising the movement velocity of carrier, position, attitude).The attitude matrix emulating carrier is obtained by the attitude of carrier in setting simulation track, obtain the speed component of carrier under carrier system according to the movement velocity of carrier in the attitude matrix obtained and simulation track, then calculate the velocity standard value of laser velocimeter by the established angle of the speed component of carrier and given laser velocimeter, inertial navigation established angle; Path generator is utilized to generate the standard value of accelerometer, the standard value of gyro; And the Calibration errors of given accelerometer and gyro, zero inclined, misalignment error; The Calibration errors of given laser velocimeter, error of fixed angles; The error of fixed angles of given inertial navigation.The noise data obtained in these data and step 3, step 6 is merged, obtains the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation, then adopt these simulated datas to carry out inertial navigation/odometer integrated navigation emulation.

Definition: in simulation track, navigational coordinate system downloads speed of moving body vector is V ⁿ, under navigational coordinate system, the component in three directions is respectively in simulation track, the component of the movement velocity of carrier x, y, z three axles under carrier system is respectively the attitude of carrier matrix obtained by the attitude of carrier in simulation track is the established angle θ of given laser velocimeter _f, laser velocimeter error of fixed angles is δ θ _f, the established angle of inertial navigation is respectively α _b, β _b, γ _band the error of fixed angles of inertial navigation is respectively δ α _b, δ β _b, δ γ _b, the Calibration errors value δ K of laser velocimeter _l; The standard value of accelerometer x, y, z three axles is respectively a _tx(1), a _tx(2) ... a _tx(n), a _ty(1), a _ty(2) ... a _ty(n), a _tz(1), a _tz(2) ... a _tz(n); The Calibration errors of accelerometer x, y, z three axles is respectively δ K _ax, δ K _ay, δ K _az; The misalignment error of accelerometer is δ M _axy, δ M _axz, δ M _ayx, δ M _ayz, δ M _azx, δ M _azy; Zero of accelerometer x, y, z three axles are respectively δ B partially _ax, δ B _ay, δ B _az; The standard value of gyro x, y, z three axles is respectively ω _tx(1), ω _tx(2) ... ω _tx(n), ω _ty(1), ω _ty(2) ... ω _ty(n), ω _tz(1), ω _tz(2) ... ω _tz(n); The Calibration errors of gyro x, y, z three axles is respectively δ K _gx, δ K _gy, δ K _gz; The misalignment error of gyro is δ M _gxy, δ M _gxz, δ M _gyx, δ M _gyz, δ M _gzx, δ M _gzy; Zero of gyro x, y, z three axles are respectively δ B partially _gx, δ B _gy, δ B _gz.The noise of gyro x, y, z three axles is respectively w _gx(1), w _gx(2) ... w _gx(n), w _gy(1), w _gy(2) ... w _gy(n), w _gz(1), w _gz(2) ... w _gz(n); The noise of accelerometer x, y, z three axles is respectively w _ax(1), w _ax(2) ... w _ax(n), w _ay(1), w _ay(2) ... w _ay(n), w _az(1), w _az(2) ... w _az(n); The speed noise of laser velocimeter is w _l(1), w _l(2) ... w _l(n).

In simulation track, the rate conversion of carrier under navigation system is calculated as follows to the speed of x, y, z three axles under carrier system:

$\left[\begin{array}{c}{v}_x^b\left(i\right)\\ v_y^b\left(i\right)\\ v_z^b\left(i\right)\end{array}\right]=T_n^b\left(i\right)\left[\begin{array}{c}{v}_x^n\left(i\right)\\ v_y^n\left(i\right)\\ v_z^n\left(i\right)\end{array}\right](i=1...n)$

The velocity standard value calculating laser velocimeter is computing formula is as follows:

$V_{\mathrm{Lt}}^m\left(i\right)=[v_x^b\left(i\right)\cos {\mathrm{\α}}_f+v_y^b\left(i\right)\cos {\mathrm{\β}}_f+v_z^b\left(i\right)\cos {\mathrm{\γ}}_f]\cos {\mathrm{\θ}}_f(i=1...n)$

By the speed noise of laser velocimeter calculating the velocity standard value of laser velocimeter, the Calibration errors value of given laser velocimeter, error of fixed angles, inertial navigation error of fixed angles and extraction, comprehensively obtain the speed of the laser velocimeter for integrated navigation hardware-in-the-loop simulation computing formula is as follows:

$\begin{array}{c}{V}_{\mathrm{Lf}}^m\left(i\right)=(1+\mathrm{\δ}{K}_L)[v_x^b\left(i\right)\cos ({\mathrm{\α}}_f+\mathrm{\δ}{\mathrm{\α}}_f)+v_y^b\left(i\right)\cos ({\mathrm{\β}}_f+\mathrm{\δ}{\mathrm{\β}}_f)+v_z^b\left(i\right)\cos ({\mathrm{\γ}}_f+\mathrm{\δ}{\mathrm{\γ}}_f)]\cos ({\mathrm{\θ}}_f+\mathrm{\δ}{\mathrm{\θ}}_f)+w_L\left(i\right)\\ (i=1...n)\end{array}$

By the gyro calculated, accelerometer standard value, given gyro, accelerometer bias, Calibration errors, misalignment error and the gyro of extraction, the noise of accelerometer, comprehensively obtain for the gyro of integrated navigation hardware-in-the-loop simulation, accelerometer data computing formula as follows:

${\mathrm{\ω}}_f\left(i\right)=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{fx}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{fy}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{fz}}\left(i\right)\end{array}\right]=\left[\begin{array}{ccc}1+\mathrm{\δ}{K}_{\mathrm{gx}}& \mathrm{\δ}{M}_{\mathrm{gxy}}& \mathrm{\δ}{M}_{\mathrm{gxz}}\\ {\mathrm{\δM}}_{\mathrm{gyx}}& 1+\mathrm{\δ}{K}_{\mathrm{gy}}& \mathrm{\δ}{M}_{\mathrm{gyz}}\\ \mathrm{\δ}{M}_{\mathrm{gzx}}& \mathrm{\δ}{M}_{\mathrm{gzy}}& 1+\mathrm{\δ}{K}_{\mathrm{gz}}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{tx}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{ty}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{tz}\left(i\right)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{B}_{\mathrm{gx}}\\ {\mathrm{\δB}}_{\mathrm{gy}}\\ {\mathrm{\δB}}_{\mathrm{gz}}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{gx}}\left(i\right)\\ w_{\mathrm{gy}}\left(i\right)\\ w_{\mathrm{gz}}\left(i\right)\end{array}\right](i=1...n)$

$a_f\left(i\right)=\left[\begin{array}{c}{a}_{\mathrm{fx}}\left(i\right)\\ a_{\mathrm{fy}}\left(i\right)\\ a_{\mathrm{fz}}\left(i\right)\end{array}\right]=\left[\begin{array}{ccc}1+\mathrm{\δ}{K}_{\mathrm{ax}}& \mathrm{\δ}{M}_{\mathrm{axy}}& \mathrm{\δ}{M}_{\mathrm{axz}}\\ {\mathrm{\δM}}_{\mathrm{ayx}}& 1+\mathrm{\δ}{K}_{\mathrm{ay}}& \mathrm{\δ}{M}_{\mathrm{ayz}}\\ \mathrm{\δ}{M}_{\mathrm{azx}}& \mathrm{\δ}{M}_{\mathrm{azy}}& 1+\mathrm{\δ}{K}_{\mathrm{az}}\end{array}\right]\left[\begin{array}{c}{a}_{\mathrm{tx}}\left(i\right)\\ a_{\mathrm{ty}}\left(i\right)\\ a_{\mathrm{tz}\left(i\right)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{B}_{\mathrm{ax}}\\ {\mathrm{\δB}}_{\mathrm{ay}}\\ {\mathrm{\δB}}_{\mathrm{az}}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{ax}}\left(i\right)\\ w_{\mathrm{ay}}\left(i\right)\\ w_{\mathrm{az}}\left(i\right)\end{array}\right](i=1...n)$

Adopt laser velocimeter simulated data obtained above accelerometer simulated data a _f, gyro simulated data ω _f, in conjunction with by carrier initial position, initial attitude, initial velocity given in simulation track, carry out integrated navigation hardware-in-the-loop simulation.

3, advantage and effect: the advantage of the method substitutes simulator and noise by the actual noise of laser velocimeter, gyro, accelerometer, then the hardware-in-the-loop simulation of inertial navigation/laser velocimeter integrated navigation is carried out, for research inertial navigation/laser velocimeter integrated navigation provides better support.

Accompanying drawing explanation

Fig. 1 is that noise of the present invention extracts block diagram

Fig. 2 is the Hardware In The Loop Simulation Method block diagram of inertial navigation of the present invention/laser velocimeter integrated navigation;

Fig. 3 is the Hardware In The Loop Simulation Method process flow diagram of inertial navigation of the present invention/laser velocimeter integrated navigation;

In Fig. 1, symbol description is as follows:

the speed in the east that inertial navigation exports, north, direction, sky

V _x, V _y, V _z: the speed component in inertial navigation x, y, z three directions under used group of coordinate system

: the speed that laser velocimeter exports

V ^m: the reference velocity on laser speedometer direction

A: accelerometer output valve

: the mean value of accelerometer output valve

ω: gyro output valve

: the mean value of gyro output valve

W _l: the noise of laser velocimeter

W _a: the noise of accelerometer

W _g: the noise of gyro

In Fig. 2, symbol description is as follows:

: the velocity standard value of the laser velocimeter calculated

δ K _l: the Calibration errors of given laser velocimeter

ω _t: the gyro data standard value that path generator generates

A _t: the accelerometer data standard value that path generator generates

δ B _g: given gyro zero is inclined

δ B _a: given accelerometer bias

ω _c: the gyro data comprising Calibration errors, error of fixed angles

A _c: the accelerometer data comprising Calibration errors, error of fixed angles

f: for the velocity amplitude of the laser velocimeter of integrated navigation hardware-in-the-loop simulation

ω _f: for the gyro data of integrated navigation hardware-in-the-loop simulation

A _f: for the accelerometer data of integrated navigation hardware-in-the-loop simulation

P: the carrier positions set in simulation track

A: the attitude of carrier set in simulation track

V: the bearer rate set in simulation track

P ₀: the carrier initial position set in simulation track

A ₀: the carrier initial attitude set in simulation track

V ₀: the carrier initial velocity set in simulation track

W _l: the speed noise of laser velocimeter

W _a: accelerometer noise

W _g: gyro noise

Embodiment

See Fig. 1-Fig. 3, the Hardware In The Loop Simulation Method of the present invention's a kind of inertial navigation/laser velocimeter integrated navigation, the method concrete steps are as follows:

Step 1, inertial navigation/laser velocimeter combined system to be installed on carrier, and electrifying startup.

Step 2, bookbinding initial parameter (comprise initial longitude, latitude, highly, laser velocimeter scale value, laser velocimeter established angle, inertial navigation established angle) to navigational computer.

Step 3, maintenance carrier stationary, gather the output data of preservation 1 hour gyro and accelerometer.Ask for the mean value of the output data of gyro and accelerometer, by gather gyro and accelerometer data poor with its mean value respectively, obtain the actual noise of gyro and accelerometer.

Step 4, carrier keep static, and 5 minutes static initial alignments are carried out in inertial navigation.

Step 5, complete aim at after carrier setting in motion, in motion process, inertial navigation calculating is carried out in inertial navigation, gathers and the speed of the velocity amplitude that in 50 seconds, inertial navigation exports after preserving setting in motion, attitude matrix that inertial navigation exports, laser velocimeter output.

The speed that step 6, inertial navigation export is the speed under sky, northeast coordinate system, attitude matrix is exported by the rate conversion under sky, northeast coordinate system under used group coordinate system by inertial navigation, again according to inertial navigation and carrier, position relationship between laser velocimeter and carrier, the velocity projections under used group of coordinate system is obtained reference velocity to the velocity reversal of laser velocimeter.By the speed of laser velocimeter and reference velocity poor, obtain the noise figure of laser velocimeter output speed.

Step 7, setting simulation track (comprising the movement velocity of carrier, position, attitude).The attitude matrix emulating carrier is obtained by the attitude of carrier in setting simulation track, obtain the speed component of carrier under carrier system according to the movement velocity of carrier in the attitude matrix obtained and simulation track, then calculate the velocity standard value of laser velocimeter by the established angle of the speed component of carrier and given laser velocimeter, inertial navigation established angle; Path generator is utilized to generate the standard value of accelerometer, the standard value of gyro; And the Calibration errors of given accelerometer and gyro, zero inclined, misalignment error; The Calibration errors of given laser velocimeter, error of fixed angles; The error of fixed angles of given inertial navigation.The noise data obtained in these data and step 3, step 6 is merged, obtains the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation, then adopt these simulated datas to carry out inertial navigation/laser velocimeter integrated navigation emulation.

Wherein, " the obtaining the actual noise of gyro, accelerometer " described in step 3, it is as follows that it realizes procedure declaration:

Definition w _gfor gyro noise, w _afor accelerometer noise, the data gathering three the axles outputs of gyro x, y, z are respectively ω _x(1), ω _x(2) ... ω _x(n), ω _y(1), ω _y(2) ... ω _y(n), ω _z(1), ω _z(2) ... ω _zn (), mean value is respectively the data gathering three the axles outputs of accelerometer x, y, z are respectively a _x(1), a _x(2) ... a _x(n), a _y(1), a _y(2) ... a _y(n), a _z(1), a _z(2) ... a _zn (), mean value is respectively

Gyro noise computing formula is as follows:

$w_g\left(i\right)=\left[\begin{array}{c}{w}_{\mathrm{gx}}\left(i\right)\\ w_{\mathrm{gy}}\left(i\right)\\ w_{\mathrm{gz}}\left(i\right)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x\left(i\right)\\ {\mathrm{\ω}}_y\left(i\right)\\ {\mathrm{\ω}}_z\left(i\right)\end{array}\right]-\left[\begin{array}{c}{\stackrel{\‾}{\mathrm{\ω}}}_x\\ {\stackrel{\‾}{\mathrm{\ω}}}_y\\ {\stackrel{\‾}{\mathrm{\ω}}}_z\end{array}\right],(i=1...n)$

Accelerometer noise calculation formula is as follows:

$w_a\left(i\right)=\left[\begin{array}{c}{w}_{\mathrm{ax}}\left(i\right)\\ w_{\mathrm{ay}}\left(i\right)\\ w_{\mathrm{az}}\left(i\right)\end{array}\right]=\left[\begin{array}{c}{a}_x\left(i\right)\\ a_y\left(i\right)\\ a_z\left(i\right)\end{array}\right]-\left[\begin{array}{c}{\stackrel{\‾}{a}}_x\\ {\stackrel{\‾}{a}}_y\\ {\stackrel{\‾}{a}}_z\end{array}\right],(i=1...n)$

Wherein, " the obtaining the noise figure of laser velocimeter output speed " described in step 6, it is as follows that it realizes procedure declaration:

Carrier setting in motion, the speed having collected 50 seconds inner laser knotmeters exports speed under inertial navigation output sky, northeast coordinate system $V_E^n\left(1\right),V_E^n\left(2\right)...V_E^n\left(n\right),V_N^n\left(1\right),V_N^n\left(2\right)...V_N^n\left(n\right),V_U^n\left(n\right),V_U^n\left(1\right),V_U^n\left(2\right)...V_U^n\left(n\right),$ The attitude matrix that inertial navigation exports the established angle of the laser velocimeter of navigational computer bookbinding is θ, and the established angle of inertial navigation is α, β, γ; The speed of inertial navigation under used group of coordinate system exports and is respectively V _x(1), V _x(2) ... V _x(n), V _y(1), V _y(2) ... V _y(n), V _z(1), V _z(2) ... V _z(n).The velocity projections of inertial navigation under used group of coordinate system to the reference velocity on the velocity reversal of laser velocimeter is under the speed that inertial navigation exports under sky coordinate system northeastward forwards used group coordinate system to, computing formula is as follows:

$\left[\begin{array}{c}{V}_i\left(i\right)\\ V_y\left(i\right)\\ V_z\left(i\right)\end{array}\right]=C_n^b\left(i\right)\left[\begin{array}{c}{V}_E^n\left(i\right)\\ V_N^n\left(i\right)\\ V_U^n\left(i\right)\end{array}\right](i=1...n)$

The velocity projections of inertial navigation under used group of coordinate system is V to the reference velocity on the velocity reversal of laser velocimeter ^m(i), computing formula is as follows:

V ^m(i)＝[V _x(i)cosα+V _y(i)cosβ+V _z(i)cosγ]cosθ (i＝1...n)

The speed noise figure that laser velocimeter exports is w _l(i), computing formula is as follows:

$w_L\left(i\right)=V_L^m\left(i\right)-V^m\left(i\right),(i=1...n)$

Wherein, " obtain the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation, then adopt these simulated datas to carry out inertial navigation/odometer integrated navigation emulation " described in step 7, it is as follows that it realizes procedure declaration:

Setting simulation track (comprising the movement velocity of carrier, position, attitude).The attitude matrix emulating carrier is obtained by the attitude of carrier in setting simulation track, obtain the speed component of carrier under carrier system according to the movement velocity of carrier in the attitude matrix obtained and simulation track, then calculate the velocity standard value of laser velocimeter by the established angle of the speed component of carrier and given laser velocimeter, inertial navigation established angle; Path generator is utilized to generate the standard value of accelerometer, the standard value of gyro; And the Calibration errors of given accelerometer and gyro, zero inclined, misalignment error; The Calibration errors of given laser velocimeter, error of fixed angles; The error of fixed angles of given inertial navigation.The noise data obtained in these data and step 3, step 6 is merged, obtains the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation, then adopt these simulated datas to carry out inertial navigation/odometer integrated navigation emulation.

Definition: in simulation track, navigational coordinate system downloads speed of moving body vector is V ⁿ, under navigational coordinate system, the component in three directions is respectively in simulation track, the component of the movement velocity of carrier x, y, z three axles under carrier system is respectively the attitude of carrier matrix obtained by the attitude of carrier in simulation track is the established angle θ of given laser velocimeter _f, laser velocimeter error of fixed angles is δ θ _f, the established angle of inertial navigation is respectively α _b, β _b, γ _band the error of fixed angles of inertial navigation is respectively δ α _b, δ β _b, δ _{γ b}, the Calibration errors value δ K of laser velocimeter _l; The standard value of accelerometer x, y, z three axles is respectively a _tx(1), a _tx(2) ... a _tx(n), a _ty(1), a _ty(2) ... a _ty(n), a _tz(1), a _tz(2) ... a _tz(n); The Calibration errors of accelerometer x, y, z three axles is respectively δ K _ax, δ K _ay, δ K _az; The misalignment error of accelerometer is δ M _axy, δ M _axz, δ M _ayx, δ M _ayz, δ M _azx, δ M _azy; Zero of accelerometer x, y, z three axles are respectively δ B partially _ax, δ B _ay, δ B _az; The standard value of gyro x, y, z three axles is respectively ω _tx(1), ω _tx(2) ... ω _tx(n), ω _ty(1), ω _ty(2) ... ω _ty(n), ω _tz(1), ω _tz(2) ... ω _tz(n); The Calibration errors of gyro x, y, z three axles is respectively δ K _gx, δ K _gy, δ K _gz; The misalignment error of gyro is δ M _gxy, δ M _gxz, δ M _gyx, δ M _gyz, δ M _gzx, δ M _gzy; Zero of gyro x, y, z three axles are respectively δ B partially _gx, δ B _gy, δ B _gz.The noise of gyro x, y, z three axles is respectively w _gx(1), w _gx(2) ... w _gx(n), w _gy(1), w _gy(2) ... w _gy(n), w _gz(1), w _gz(2) ... w _gz(n); The noise of accelerometer x, y, z three axles is respectively w _ax(1), w _ax(2) ... w _ax(n), w _ay(1), w _ay(2) ... w _ay(n), w _az(1), w _az(2) ... w _az(n); The speed noise of laser velocimeter is w _l(1), w _l(2) ... w _l(n).

In simulation track, the rate conversion of carrier under navigation system is calculated as follows to the speed of x, y, z three axles under carrier system:

$\left[\begin{array}{c}{v}_x^b\left(i\right)\\ v_y^b\left(i\right)\\ v_z^b\left(i\right)\end{array}\right]=T_n^b\left(i\right)\left[\begin{array}{c}{v}_x^n\left(i\right)\\ v_y^n\left(i\right)\\ v_z^n\left(i\right)\end{array}\right](i=1...n)$

The velocity standard value calculating laser velocimeter is computing formula is as follows:

$V_{\mathrm{Lt}}^m\left(i\right)=[v_x^b\left(i\right)\cos {\mathrm{\α}}_f+v_y^b\left(i\right)\cos {\mathrm{\β}}_f+v_z^b\left(i\right)\cos {\mathrm{\γ}}_f]\cos {\mathrm{\θ}}_f(i=1...n)$

By the speed noise of laser velocimeter calculating the velocity standard value of laser velocimeter, the Calibration errors value of given laser velocimeter, error of fixed angles, inertial navigation error of fixed angles and extraction, comprehensively obtain the speed of the laser velocimeter for integrated navigation hardware-in-the-loop simulation computing formula is as follows:

$\begin{array}{c}{V}_{\mathrm{Lf}}^m\left(i\right)=(1+\mathrm{\δ}{K}_L)[v_x^b\left(i\right)\cos ({\mathrm{\α}}_f+\mathrm{\δ}{\mathrm{\α}}_f)+v_y^b\left(i\right)\cos ({\mathrm{\β}}_f+\mathrm{\δ}{\mathrm{\β}}_f)+v_z^b\left(i\right)\cos ({\mathrm{\γ}}_f+\mathrm{\δ}{\mathrm{\γ}}_f)]\cos ({\mathrm{\θ}}_f+\mathrm{\δ}{\mathrm{\θ}}_f)+w_L\left(i\right)\\ (i=1...n)\end{array}$

By the gyro calculated, accelerometer standard value, given gyro, accelerometer bias, Calibration errors, misalignment error and the gyro of extraction, the noise of accelerometer, comprehensively obtain for the gyro of integrated navigation hardware-in-the-loop simulation, accelerometer data computing formula as follows:

${\mathrm{\ω}}_f\left(i\right)=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{fx}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{fy}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{fz}}\left(i\right)\end{array}\right]=\left[\begin{array}{ccc}1+\mathrm{\δ}{K}_{\mathrm{gx}}& \mathrm{\δ}{M}_{\mathrm{gxy}}& \mathrm{\δ}{M}_{\mathrm{gxz}}\\ {\mathrm{\δM}}_{\mathrm{gyx}}& 1+\mathrm{\δ}{K}_{\mathrm{gy}}& \mathrm{\δ}{M}_{\mathrm{gyz}}\\ \mathrm{\δ}{M}_{\mathrm{gzx}}& \mathrm{\δ}{M}_{\mathrm{gzy}}& 1+\mathrm{\δ}{K}_{\mathrm{gz}}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{tx}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{ty}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{tz}\left(i\right)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{B}_{\mathrm{gx}}\\ {\mathrm{\δB}}_{\mathrm{gy}}\\ {\mathrm{\δB}}_{\mathrm{gz}}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{gx}}\left(i\right)\\ w_{\mathrm{gy}}\left(i\right)\\ w_{\mathrm{gz}}\left(i\right)\end{array}\right](i=1...n)$

$a_f\left(i\right)=\left[\begin{array}{c}{a}_{\mathrm{fx}}\left(i\right)\\ a_{\mathrm{fy}}\left(i\right)\\ a_{\mathrm{fz}}\left(i\right)\end{array}\right]=\left[\begin{array}{ccc}1+\mathrm{\δ}{K}_{\mathrm{ax}}& \mathrm{\δ}{M}_{\mathrm{axy}}& \mathrm{\δ}{M}_{\mathrm{axz}}\\ {\mathrm{\δM}}_{\mathrm{ayx}}& 1+\mathrm{\δ}{K}_{\mathrm{ay}}& \mathrm{\δ}{M}_{\mathrm{ayz}}\\ \mathrm{\δ}{M}_{\mathrm{azx}}& \mathrm{\δ}{M}_{\mathrm{azy}}& 1+\mathrm{\δ}{K}_{\mathrm{az}}\end{array}\right]\left[\begin{array}{c}{a}_{\mathrm{tx}}\left(i\right)\\ a_{\mathrm{ty}}\left(i\right)\\ a_{\mathrm{tz}\left(i\right)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{B}_{\mathrm{ax}}\\ {\mathrm{\δB}}_{\mathrm{ay}}\\ {\mathrm{\δB}}_{\mathrm{az}}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{ax}}\left(i\right)\\ w_{\mathrm{ay}}\left(i\right)\\ w_{\mathrm{az}}\left(i\right)\end{array}\right](i=1...n)$

Adopt laser velocimeter simulated data obtained above accelerometer simulated data a _f, gyro simulated data ω _f, in conjunction with by carrier initial position, initial attitude, initial velocity given in simulation track, carry out integrated navigation hardware-in-the-loop simulation.

Claims (4)

1. a Hardware In The Loop Simulation Method for inertial navigation/laser velocimeter integrated navigation, is characterized in that: the method concrete steps are as follows:

Step 1, inertial navigation/laser velocimeter combined system to be installed on carrier, and electrifying startup;

Step 2, bookbinding initial parameter, comprise initial longitude, latitude, highly, laser velocimeter scale value, laser velocimeter established angle, inertial navigation established angle be to navigational computer;

Step 3, maintenance carrier stationary, gather the output data of preservation 1 hour gyro and accelerometer; Ask for the mean value of the output data of gyro and accelerometer, by gather gyro and accelerometer data poor with its mean value respectively, obtain the actual noise of gyro and accelerometer;

Step 4, carrier keep static, and 5 minutes static initial alignments are carried out in inertial navigation;

Step 5, complete aim at after carrier setting in motion, in motion process, inertial navigation calculating is carried out in inertial navigation, gathers and the speed of the velocity amplitude that in 50 seconds, inertial navigation exports after preserving setting in motion, attitude matrix that inertial navigation exports, laser velocimeter output;

The speed that step 6, inertial navigation export is the speed under sky, northeast coordinate system, attitude matrix is exported by the rate conversion under sky, northeast coordinate system under used group coordinate system by inertial navigation, again according to inertial navigation and carrier, position relationship between laser velocimeter and carrier, the velocity projections under used group of coordinate system is obtained reference velocity to the velocity reversal of laser velocimeter; By the speed of laser velocimeter and reference velocity poor, obtain the noise figure of laser velocimeter output speed;

Step 7, setting simulation track, comprise the movement velocity of carrier, position, attitude, the attitude matrix emulating carrier is obtained by the attitude of carrier in setting simulation track, obtain the speed component of carrier under carrier system according to the movement velocity of carrier in the attitude matrix obtained and simulation track, then calculate the velocity standard value of laser velocimeter by the established angle of the speed component of carrier and given laser velocimeter, inertial navigation established angle; Path generator is utilized to generate the standard value of accelerometer, the standard value of gyro; And the Calibration errors of given accelerometer and gyro, zero inclined, misalignment error; The Calibration errors of given laser velocimeter, error of fixed angles; The error of fixed angles of given inertial navigation; The noise data obtained in these data and step 3, step 6 is merged, obtains the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation, then adopt these simulated datas to carry out inertial navigation/laser velocimeter integrated navigation emulation.

2. the Hardware In The Loop Simulation Method of a kind of inertial navigation/laser velocimeter integrated navigation according to claim 1, is characterized in that:

" obtaining the actual noise of gyro, accelerometer " described in step 3, it is as follows that it realizes procedure declaration:

Definition w _gfor gyro noise, w _afor accelerometer noise, the data gathering three the axles outputs of gyro x, y, z are respectively ω _x(1), ω _x(2) ... ω _x(n), ω _y(1), ω _y(2) ... ω _y(n), ω _z(1), ω _z(2) ... ω _zn (), mean value is respectively the data gathering three the axles outputs of accelerometer x, y, z are respectively a _x(1), a _x(2) ... a _x(n), a _y(1), a _y(2) ... a _y(n), a _z(1), a _z(2) ... a _zn (), mean value is respectively

Gyro noise computing formula is as follows:

Accelerometer noise calculation formula is as follows:

3. the Hardware In The Loop Simulation Method of a kind of inertial navigation/laser velocimeter integrated navigation according to claim 1, is characterized in that: " the obtaining the noise figure of laser velocimeter output speed " described in step 6, and it is as follows that it realizes procedure declaration:

Carrier setting in motion, the speed having collected 50 seconds inner laser knotmeters exports speed under inertial navigation output sky, northeast coordinate system the attitude matrix that inertial navigation exports the established angle of the laser velocimeter of navigational computer bookbinding is θ, and the established angle of inertial navigation is α, β, γ; The speed of inertial navigation under used group of coordinate system exports and is respectively V _x(1), V _x(2) ... V _x(n), V _y(1), V _y(2) ... V _y(n), V _z(1), V _z(2) ... V _z(n); The velocity projections of inertial navigation under used group of coordinate system to the reference velocity on the velocity reversal of laser velocimeter is under the speed that inertial navigation exports under sky coordinate system northeastward forwards used group coordinate system to, computing formula is as follows:

The velocity projections of inertial navigation under used group of coordinate system is V to the reference velocity on the velocity reversal of laser velocimeter ^m(i), computing formula is as follows:

V ^m(i)＝[V _x(i)cosα+V _y(i)cosβ+V _z(i)cosγ]cosθ (i＝1...n)；

The speed noise figure that laser velocimeter exports is w _l(i), computing formula is as follows:

4. the Hardware In The Loop Simulation Method of a kind of inertial navigation/laser velocimeter integrated navigation according to claim 1, it is characterized in that: " the obtaining the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation; then adopt these simulated datas to carry out inertial navigation/odometer integrated navigation emulation " described in step 7, it is as follows that it realizes procedure declaration:

Setting simulation track, comprise the movement velocity of carrier, position, attitude, the attitude matrix emulating carrier is obtained by the attitude of carrier in setting simulation track, obtain the speed component of carrier under carrier system according to the movement velocity of carrier in the attitude matrix obtained and simulation track, then calculate the velocity standard value of laser velocimeter by the established angle of the speed component of carrier and given laser velocimeter, inertial navigation established angle; Path generator is utilized to generate the standard value of accelerometer, the standard value of gyro; And the Calibration errors of given accelerometer and gyro, zero inclined, misalignment error; The Calibration errors of given laser velocimeter, error of fixed angles; The error of fixed angles of given inertial navigation; The noise data obtained in these data and step 3, step 6 is merged, obtains the device simulation data for inertial navigation/laser velocimeter integrated navigation hardware-in-the-loop simulation, then adopt these simulated datas to carry out inertial navigation/odometer integrated navigation emulation;

Definition: in simulation track, navigational coordinate system downloads speed of moving body vector is V ⁿ, under navigational coordinate system, the component in three directions is respectively in simulation track, the component of the movement velocity of carrier x, y, z three axles under carrier system is respectively the attitude of carrier matrix obtained by the attitude of carrier in simulation track is the established angle θ of given laser velocimeter _f, laser velocimeter error of fixed angles is δ θ _f, the established angle of inertial navigation is respectively α _b, β _b, γ _band the error of fixed angles of inertial navigation is respectively δ α _b, δ β _b, δ γ _b, the Calibration errors value δ K of laser velocimeter _l; The standard value of accelerometer x, y, z three axles is respectively a _tx(1), a _tx(2) ... a _tx(n), a _ty(1), a _ty(2) ... a _ty(n), a _tz(1), a _tz(2) ... a _tz(n); The Calibration errors of accelerometer x, y, z three axles is respectively δ K _ax, δ K _ay, δ K _az; The misalignment error of accelerometer is δ M _axy, δ M _axz, δ M _ayx, δ M _ayz, δ M _azx, δ M _azy; Zero of accelerometer x, y, z three axles are respectively δ B partially _ax, δ B _ay, δ B _az; The standard value of gyro x, y, z three axles is respectively ω _tx(1), ω _tx(2) ... ω _tx(n), ω _ty(1), ω _ty(2) ... ω _ty(n), ω _tz(1), ω _tz(2) ... ω _tz(n); The Calibration errors of gyro x, y, z three axles is respectively δ K _gx, δ K _gy, δ K _gz; The misalignment error of gyro is δ M _gxy, δ M _gxz, δ M _gyx, δ M _gyz, δ M _gzx, δ M _gzy; Zero of gyro x, y, z three axles are respectively δ B partially _gx, δ B _gy, δ B _gz; The noise of gyro x, y, z three axles is respectively w _gx(1), w _gx(2) ... w _gx(n), w _gy(1), w _gy(2) ... w _gy(n), w _gz(1), w _gz(2) ... w _gz(n); The noise of accelerometer x, y, z three axles is respectively w _ax(1), w _ax(2) ... w _ax(n), w _ay(1), w _ay(2) ... w _ay(n), w _az(1), w _az(2) ... w _az(n); The speed noise of laser velocimeter is w _l(1), w _l(2) ... w _l(n);

In simulation track, the rate conversion of carrier under navigation system is calculated as follows to the speed of x, y, z three axles under carrier system:

The velocity standard value calculating laser velocimeter is computing formula is as follows:

By the speed noise of laser velocimeter calculating the velocity standard value of laser velocimeter, the Calibration errors value of given laser velocimeter, error of fixed angles, inertial navigation error of fixed angles and extraction, comprehensively obtain the speed of the laser velocimeter for integrated navigation hardware-in-the-loop simulation computing formula is as follows:

By the gyro calculated, accelerometer standard value, given gyro, accelerometer bias, Calibration errors, misalignment error and the gyro of extraction, the noise of accelerometer, comprehensively obtain for the gyro of integrated navigation hardware-in-the-loop simulation, accelerometer data computing formula as follows:

Adopt laser velocimeter simulated data obtained above accelerometer simulated data a _f, gyro simulated data ω _f, in conjunction with by carrier initial position, initial attitude, initial velocity given in simulation track, carry out integrated navigation hardware-in-the-loop simulation.