CN101694389B - Rapid measurement method of initial attitude of gyro free strap down inertial navigation system - Google Patents

Abstract

The invention provides a rapid measurement method of initial attitude of a gyro free strap down inertial navigation system. The gyro free strap down inertial navigation system on a low-cost unmanned aerial vehicle is used as a sub inertial navigation system, and a strap down inertial navigation system on a low-cost unmanned aerial vehicle loading mechanism is used as a main initial navigation system. The measurement method comprises the following steps: filtering by using the gyro free strap down inertial navigation system, evaluating horizontal installation variation of the main inertia navigation system and the sub inertial navigation system, and performing the matching measurement to the horizontal installation variation of the main inertia navigation system and the sub inertial navigation system by using output information of accelerometer in the gyro free strap down inertial navigation system. The invention can rapidly measure the initial attitude of the low-cost unmanned aerial vehicle, thereby improving the rapid response capacity of the low-cost unmanned aerial vehicle, and having practical value. The invention is suitable to a medium precision strap down inertia navigation system and a high precision gyro free strap down inertia navigation system which are installed in the low-cost unmanned aerial vehicle.

Description

The rapid measurement method of initial attitude of gyro free strap down inertial navigation system

(1) technical field

The present invention is to provide a kind of rapid measurement method of initial attitude of gyro free strap down inertial navigation system.

(2) background technology

Strapdown inertial navitation system (SINS) is generally used the accelerometer sensitive linear acceleration, the angular velocity of gyroscope sensitive carrier.But there is the weakness of impact resistance difference in mechanical gyro, can't be applied to this carrier that big overload, big angular acceleration are arranged of low-cost unmanned vehicle.Optical gyroscope (optical fibre gyro and laser gyro) has bigger dynamic range, but the equipment of optical gyroscope strapdown inertial navitation system (SINS) can improve the manufacturing cost of low-cost unmanned vehicle, and has reduced the reliability of strapdown inertial navitation system (SINS).Gyro free strap down inertial navigation system uses accelerometer to replace gyroscope, calculates the angular velocity of carrier from the specific force of accelerometer measures, and then only finishes the measurement of carrier navigational parameter with accelerometer.

The characteristics of gyro free strap down inertial navigation system comprise:

1, cost is low: because not utilization structure complexity, difficulty of processing gyro big, difficult in maintenance, so the gyro free strap down inertial navigation system cost is lower.

2, energy consumption is little: accelerometer is compared with gyro, does not have high-speed rotary body, so power consumption is little, and does not need complicated power supply.

3, dynamic range is big: for the gyro strap-down inertial navigation system was arranged, gyro was directly installed on the carrier, during high maneuvering condition, required gyro that very big range of dynamic measurement is arranged, and this implements the comparison difficulty on engineering.And the measurement range of accelerometer is big, can satisfy the requirement of great dynamic range.

4, reaction is fast: no matter be mechanical gyro, or optical gyroscope all needs the longer starting time from starting to operate as normal, so reaction is slower.

5, reliability height: the more and complex structure of the component part of gyro, the accelerometer component part is few and simple in structure.

By the characteristics of gyro free strap down inertial navigation system as can be seen: gyro free strap down inertial navigation system has been avoided the mechanical gyro strapdown inertial navitation system (SINS) and has been caused a difficult problem because of the dynamic range restriction institute of mechanical gyro.Simultaneously, it is cheap many that accelerometer comparison optical gyroscope price is wanted, general about 20,000 yuan of high-precision accelerometer, therefore relatively the optical gyroscope strapdown inertial navitation system (SINS) is much lower to use the gyro free strap down inertial navigation system cost of accelerometer, makes that the low-cost unmanned vehicle that is equipped with gyro free strap down inertial navigation system is suitable for producing in enormous quantities.

Gyro free strap down inertial navigation system needed to determine the initial attitude parameter in the past entering the navigation duty.The measuring accuracy of initial attitude parameter is directly connected to the precision of gyro free strap down inertial navigation system navigation, and the needed time length of the measuring process of initial attitude parameter has determined the reaction time of gyro free strap down inertial navigation system navigation.

Require low-cost unmanned vehicle to have quick-reaction capability (QRC) and precise guidance in the actual engineering to guarantee that it is in good performance.The reaction time of low-cost unmanned vehicle depends primarily on the used time of gyro free strap down inertial navigation system initial attitude parameter measurement.Simultaneously, the measuring accuracy of the precision guidance capability of low-cost unmanned vehicle and gyro free strap down inertial navigation system initial attitude parameter is closely related.Therefore, utilize the navigation information of inertial navigation output on the loader mechanism of low-cost unmanned vehicle and the initial attitude that accelerometer output information is measured gyro free strap down inertial navigation system quickly and accurately, for the performance that improves gyro free strap down inertial navigation system, thereby the quick-reaction capability (QRC) that improves low-cost unmanned vehicle has practical value.

(3) summary of the invention

The object of the present invention is to provide a kind of initial attitude that can measure low-cost unmanned vehicle fast, improve the quick-reaction capability (QRC) of low-cost unmanned vehicle, have the rapid measurement method of initial attitude of the gyro free strap down inertial navigation system of practical value.

The object of the present invention is achieved like this:

As sub-inertial navigation system, the strapdown inertial navigation system on the low-cost unmanned vehicle loader mechanism is measured initial attitude as main inertial navigation system according to the following step with the gyro free strap down inertial navigation system on the low-cost unmanned vehicle:

Step 1, the strapdown inertial navigation system on gyro free strap down inertial navigation system on the low-cost unmanned vehicle and the high-precision low-cost unmanned vehicle loader mechanism is linked by data cable;

After step 2, gyro free strap down inertial navigation system are carried out preheating, gather the data of accelerometer output;

Step 3, utilize main inertial navigation system with the initial velocity parameter with comprise that the initial position parameters of initial longitude, latitude binds to gyro free strap down inertial navigation system navigational computer;

Step 4, the attitude measurement information that comprises pitch angle, roll angle and course angle that main inertial navigation system is exported transfer to the gyro free strap down inertial navigation system by data cable, rough measure goes out the initial attitude of low-cost unmanned vehicle, finishes once transmitting fast of initial attitude;

The attitude initial value that step 5, gyro free strap down inertial navigation system are obtained by step 3 and step 4, utilize the ratio force information of the low-cost unmanned vehicle of accelerometer output measurement on the gyro free strap down inertial navigation system, calculate the angular velocity information of low-cost unmanned vehicle again by specific force;

Step 6, in sub-inertial navigation system, utilize the ratio force information of low-cost unmanned vehicle and the angular velocity information recursion of navigating to resolve, measure the velocity amplitude of low-cost unmanned vehicle;

The speed that step 7, speed and sub-inertial navigation system that main inertial navigation system is measured are measured is poor, and with difference as observation vector, the employing Kalman Filter Technology is estimated the horizontal direction installation deviation φ that between main inertial navigation system and the sub-inertial navigation system _x, φ _yThe estimation time is 120 seconds, is filtering estimation stabilization time, and uses the average filter technology by 80 seconds to 120 seconds and carry out smoothing processing for data in wherein preceding 80 seconds;

${\mathrm{\φ}}_x=\frac{{\widetilde{\mathrm{\φ}}}_x\left(1\right)......+{\widetilde{\mathrm{\φ}}}_x\left(k\right)......+{\widetilde{\mathrm{\φ}}}_x\left(N\right)}{N}$

${\mathrm{\φ}}_y=\frac{{\widetilde{\mathrm{\φ}}}_y\left(1\right)......+{\widetilde{\mathrm{\φ}}}_y\left(k\right)......+{\widetilde{\mathrm{\φ}}}_y\left(N\right)}{N}$

Wherein

Expression is by in 80 seconds to 120 seconds data smoothing processes, the filtering estimated value of k filtering estimation time point; In 80 seconds to 120 seconds data smoothing processes, time point, i.e. k=1～N are estimated in total N filtering;

Step 8, utilize step 7 to estimate the horizontal direction installation deviation φ between main inertial navigation system and the sub-inertial navigation system _x, φ _y, and sub-inertial navigation system is measured the specific force value $f_{\mathrm{ib}}^s={\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^s& f_{\mathrm{iby}}^s& f_{\mathrm{ibz}}^s\end{array}\right]}^T,$ Systematic survey specific force value in the main inertial navigation $f_{\mathrm{ib}}^m={\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^m& f_{\mathrm{iby}}^m& f_{\mathrm{ibz}}^m\end{array}\right]}^T$ Measure the orientation installation deviation φ between main inertial navigation system and the sub-inertial navigation system _z

${\mathrm{\&phi;}}_z=\arcsin \left(\frac{k_1{k}_4+k_3{k}_2}{{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="H2009100730747E00022.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}^2+{{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">k</figure-callout>}}_2}^2}\right)$

Parameter k wherein ₁, k ₂, k ₃, k ₄For

${\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="H2009100730747E00022.png"\; state="\{\{state\}\}">k</figure-callout>}}_1=f_{\mathrm{ibx}}^m$

${\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">k</figure-callout>}}_2=f_{\mathrm{iby}}^m$

$k_3=\frac{f_{\mathrm{ibx}}^s+\sin {\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\&phi;}}_y}-\frac{\sin {\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{iby}}^s}{\cos {\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_y}+\frac{\sin {\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_y};$

$k_4=\frac{\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m-f_{\mathrm{iby}}^s}{\cos {\mathrm{\&phi;}}_x}$

Step 9, utilize step 7 to estimate the horizontal direction installation deviation φ between main inertial navigation system and the sub-inertial navigation system _x, φ _y, and step 8 is measured the orientation installation deviation φ between main inertial navigation system and the sub-inertial navigation system _zConstruct installation deviation direction cosine matrix C _s ^m

$C_s^m=\left(\begin{array}{ccc}1& {\mathrm{\&phi;}}_z& -{\mathrm{\&phi;}}_y\\ -{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="H2009100730747E00022.png"\; state="\{\{state\}\}">z</figure-callout>}}& 1& {\mathrm{\&phi;}}_x\\ {\mathrm{\&phi;}}_y& -{\mathrm{\&phi;}}_x& 1\end{array}\right);$

Step 10, the main inertial navigation system that utilizes step 8 structure and the installation deviation Matrix C between the sub-inertial navigation system _s ^m, and the direction cosine matrix C of main inertial navigation output _m ⁿConstruct the carrier coordinate system of low-cost unmanned vehicle and the direction cosine matrix C between the navigation coordinate system _s ⁿ

$C_s^n=C_m^n{C}_s^m;$

Step 11, by the direction cosine matrix C between the carrier coordinate system of low-cost unmanned vehicle and the navigation coordinate system _s ⁿMeasure unmanned vehicle initial attitude angle accurately.

The present invention can also comprise following feature:

1, used system state equation and measurement equation are as follows in the step 7:

$\left\{\begin{array}{c}\stackrel{.}{X}=\mathrm{AX}\\ Z=\mathrm{HX}\end{array}\right.$

The state variable of selecting system is

X＝[δv _x?δv _y?φ _mx?φ _my?φ _mz?φ _x?φ _y?φ _z] ^T

Wherein:

δ v is the speed difference between main inertial navigation system and the sub-inertial navigation system;

φ is real installation deviation angle between main inertial navigation system and the sub-inertial navigation system;

φ _mFor utilizing main, sub-inertial navigation system output, the installation deviation angle between the master that actual measurement obtains, the sub-inertial navigation system;

Subscript x, y, z represent three axles of projection place coordinate system, are respectively navigation coordinate herein and are the east orientation axle, north orientation axle of n system and day to axle;

The state matrix of system is

$A=\left[\begin{array}{ccc}{A}_1& A_2& -{\mathrm{<figure-callout\; id="2"\; label="A"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">A</figure-callout>}}_2\\ 0_{3\&times;2}& A_3& -A_3\\ 0_{3\&times;2}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

Wherein:

$A_1=\left[\begin{array}{cc}0& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e}\\ -(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e})& 0\end{array}\right]$

R wherein _eBe earth radius, L is local latitude, ω _IeBe rotational-angular velocity of the earth; v _xBe the speed that main inertial navigation system is measured;

$A_2=\left[\begin{array}{ccc}{f}_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\\ f_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\end{array}\right]$

Wherein $f_{\mathrm{ib}}^s={\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^s& f_{\mathrm{iby}}^s& f_{\mathrm{ibz}}^s\end{array}\right]}^T$ For sub-inertial navigation system is measured the specific force value;

$A_3=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ibz}}& -{\mathrm{\&omega;}}_{\mathrm{iby}}\\ -{\mathrm{\&omega;}}_{\mathrm{<figure-callout\; id="0"\; label="ibz"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">ibz</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}\\ {\mathrm{\&omega;}}_{\mathrm{iby}}& -{\mathrm{\&omega;}}_{\mathrm{<figure-callout\; id="0"\; label="ibx"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">ibx</figure-callout>}}& 0\end{array}\right]$

ω wherein _Ib=[ω _Ibxω _Ibyω _Ibz] ^TFor utilizing the angular velocity information that accelerometer output specific force metrical information calculates on the gyro free strap down inertial navigation system;

The measurement vector of system is

Z＝[δv _x?δv _y] ^T

The observing matrix of system is

$H=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right].$

2, three coordinate axis of three coordinate axis of the gyro free strap down inertial navigation system of gyro free strap down inertial navigation system and low-cost unmanned vehicle carrier coordinate system coincide.

The invention provides and a kind ofly can measure the initial attitude of low-cost unmanned vehicle fast, thereby improve the quick-reaction capability (QRC) of low-cost unmanned vehicle, have the gyro free strap down inertial navigation system initial attitude measuring method of practical value.

As sub-inertial navigation system, the strapdown inertial navigation system on the low-cost unmanned vehicle loader mechanism is as main inertial navigation system with the gyro free strap down inertial navigation system on the low-cost unmanned vehicle in the present invention.Utilize the speed reference information of main inertial navigation output to carry out filtering, estimate the horizontal direction installation deviation that between main inertial navigation system and the sub-inertial navigation system, utilize the accelerometer output information in the gyro free strap down inertial navigation system that the orientation installation deviation between main inertial navigation system and the sub-inertial navigation system is mated measurement again.

Technology of the present invention has the following advantages: navigation information and the accelerometer output information of utilizing inertial navigation output on the loader mechanism of low-cost unmanned vehicle, under the situation that guarantees alignment precision, measure the initial attitude of gyro free strap down inertial navigation system at short notice.Guaranteed the rapid reaction rate of low-cost unmanned vehicle.

Beneficial effect of the present invention is verified by the following method: under the high dynamic condition, gyro free strap down inertial navigation system initial attitude experiment with measuring on the low-cost unmanned vehicle.The loader mechanism of low-cost unmanned vehicle is set to aircraft in test.

Test condition comprises: the device precision of (1) low-cost unmanned vehicle, promptly the normal at random value of accelerometer is biased to 0.0001g.(2) flight speed of aircraft is 150 meter per seconds, and to do 40 seconds around azimuth axis be the sinusoidal oscillating motion in cycle.(3) the horizontal direction installation deviation φ between aircraft and the low-cost unmanned vehicle carrier coordinate system _x=φ _y=0.2 degree, the orientation installation deviation φ between aircraft and the low-cost unmanned vehicle carrier coordinate system _z=1 degree.

Test findings: the estimation time is from beginning by 80 seconds, according to the horizontal direction installation deviation φ between main inertial navigation system of step 7 and the sub-inertial navigation system _x, φ _yFiltering estimation tend towards stability.The estimation time was used the average filter technology and carries out smoothing processing for data from 80 seconds to 100 seconds, and measured orientation installation deviation φ between main inertial navigation system and the sub-inertial navigation system according to step 8 _zHorizontal direction installation deviation φ between main inertial navigation system and the sub-inertial navigation system _x, φ _yFiltering estimation effect as shown in Figure 1 and Figure 2.Orientation installation deviation φ between main inertial navigation system and the sub-inertial navigation system _zFiltering estimation effect as shown in Figure 3.As seen from the figure: measured value of the present invention and theory setting value basically identical, it is fast to estimate speed simultaneously, can guarantee the rapid-action requirement of low-cost unmanned vehicle.

(4) description of drawings

Fig. 1 is the horizontal x direction installation deviation φ between main inertial navigation system of the present invention and the sub-inertial navigation system _xFiltering estimation curve map.

Fig. 2 is the horizontal y direction installation deviation φ between main inertial navigation system of the present invention and the sub-inertial navigation system _yFiltering estimation curve map.

Fig. 3 is the z direction orientation installation deviation φ between main inertial navigation system of the present invention and the sub-inertial navigation system _zThe estimation curve map.

(5) embodiment

For example the present invention is done description in more detail below in conjunction with accompanying drawing:

In the present embodiment, as sub-inertial navigation system, the strapdown inertial navigation system on the low-cost unmanned vehicle loader mechanism is as main inertial navigation system with the gyro free strap down inertial navigation system on the low-cost unmanned vehicle.The concrete implementation step that initial attitude is measured fast is as follows:

Step 1, the strapdown inertial navigation system on gyro free strap down inertial navigation system on the low-cost unmanned vehicle and the high-precision low-cost unmanned vehicle loader mechanism is linked by data cable, make the navigation information data transmission between sub-inertial navigation and the main inertial navigation unobstructed.

Step 2, gyro free strap down inertial navigation system are carried out preheating, gather the data of accelerometer output then.Preheating time is according to concrete default.

Step 3, utilize main inertial navigation system that initial velocity parameter and initial position parameters (comprising initial longitude, latitude) are bound to gyro free strap down inertial navigation system navigational computer.

Step 4, the attitude measurement information (comprising pitch angle, roll angle and course angle) that main inertial navigation system is exported transfer to the gyro free strap down inertial navigation system by data cable, rough measure goes out the initial attitude of low-cost unmanned vehicle, finishes once transmitting fast of initial attitude.

The attitude initial value that step 5, gyro free strap down inertial navigation system are obtained by step 3 (bookbinding initial velocity parameter and initial position parameters) and step 4 (once transmitting fast of initial attitude), utilize the ratio force information of the low-cost unmanned vehicle of accelerometer output measurement on the gyro free strap down inertial navigation system, calculate the angular velocity information of low-cost unmanned vehicle again by specific force.

Step 6, in sub-inertial navigation system, utilize the ratio force information of low-cost unmanned vehicle and the angular velocity information recursion of navigating to resolve, measure the velocity amplitude of low-cost unmanned vehicle;

The speed that step 7, speed and sub-inertial navigation system that main inertial navigation system is measured are measured is poor, and with difference as observation vector, the employing Kalman Filter Technology is estimated the horizontal direction installation deviation φ that between main inertial navigation system and the sub-inertial navigation system _x, φ _yThe estimation time is 120 seconds, is filtering estimation stabilization time, and uses the average filter technology by 80 seconds to 120 seconds and carry out smoothing processing for data in wherein preceding 80 seconds;

${\mathrm{\&phi;}}_x=\frac{{\widetilde{\mathrm{\&phi;}}}_x\left(1\right)......+{\widetilde{\mathrm{\&phi;}}}_x\left(k\right)......+{\widetilde{\mathrm{\&phi;}}}_x\left(N\right)}{N}$

${\mathrm{\&phi;}}_y=\frac{{\widetilde{\mathrm{\&phi;}}}_y\left(1\right)......+{\widetilde{\mathrm{\&phi;}}}_y\left(k\right)......+{\widetilde{\mathrm{\&phi;}}}_y\left(N\right)}{N}$

Wherein

Expression is by in 80 seconds to 120 seconds data smoothing processes, the filtering estimated value of k filtering estimation time point; In 80 seconds to 120 seconds data smoothing processes, time point, i.e. k=1～N are estimated in total N filtering;

Step 8, utilize step 7 to estimate the horizontal direction installation deviation φ between main inertial navigation system and the sub-inertial navigation system _x, φ _y, and sub-inertial navigation system is measured the specific force value $f_{\mathrm{ib}}^s={\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^s& f_{\mathrm{iby}}^s& f_{\mathrm{ibz}}^s\end{array}\right]}^T,$ Systematic survey specific force value in the main inertial navigation $f_{\mathrm{ib}}^m={\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^m& f_{\mathrm{iby}}^m& f_{\mathrm{ibz}}^m\end{array}\right]}^T$ Measure the orientation installation deviation φ between main inertial navigation system and the sub-inertial navigation system _z

${\mathrm{\&phi;}}_z=\arcsin \left(\frac{k_1{k}_4+k_3{k}_2}{{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="H2009100730747E00022.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}^2+{{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">k</figure-callout>}}_2}^2}\right)$

Parameter k wherein ₁, k ₂, k ₃, k ₄For

${\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="H2009100730747E00022.png"\; state="\{\{state\}\}">k</figure-callout>}}_1=f_{\mathrm{ibx}}^m$

${\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">k</figure-callout>}}_2=f_{\mathrm{iby}}^m$

$k_3=\frac{f_{\mathrm{ibx}}^s+\sin {\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\&phi;}}_y}-\frac{\sin {\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{iby}}^s}{\cos {\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_y}+\frac{\sin {\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_y};$

$k_4=\frac{\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m-f_{\mathrm{iby}}^s}{\cos {\mathrm{\&phi;}}_x}$

Step 9, utilize step 7 to estimate the horizontal direction installation deviation φ between main inertial navigation system and the sub-inertial navigation system _x, φ _y, and step 8 is measured the orientation installation deviation φ between main inertial navigation system and the sub-inertial navigation system _zConstruct installation deviation direction cosine matrix C _s ^m

$C_s^m=\left(\begin{array}{ccc}1& {\mathrm{\&phi;}}_z& -{\mathrm{\&phi;}}_y\\ -{\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="1"\; label="z"\; filenames="H2009100730747E00022.png"\; state="\{\{state\}\}">z</figure-callout>}}& 1& {\mathrm{\&phi;}}_x\\ {\mathrm{\&phi;}}_y& -{\mathrm{\&phi;}}_x& 1\end{array}\right);$

Step 10, the main inertial navigation system that utilizes step 8 structure and the installation deviation Matrix C between the sub-inertial navigation system _s ^m, and the direction cosine matrix C of main inertial navigation output _m ⁿConstruct the carrier coordinate system of low-cost unmanned vehicle and the direction cosine matrix C between the navigation coordinate system _s ⁿ

$C_s^n=C_m^n{C}_s^m;$

Step 11, by the direction cosine matrix C between the carrier coordinate system of low-cost unmanned vehicle and the navigation coordinate system _s ⁿMeasure unmanned vehicle initial attitude angle accurately.

Used system state equation and measurement equation are as follows in the step 7:

$\left\{\begin{array}{c}\stackrel{.}{X}=\mathrm{AX}\\ Z=\mathrm{HX}\end{array}\right.$

The state variable of selecting system is

X＝[δv _x?δv _y?φ _mx?φ _my?φ _mz?φ _x?φ _y?φ _z] ^T

Wherein:

δ v is the speed difference between main inertial navigation system and the sub-inertial navigation system;

φ is real installation deviation angle between main inertial navigation system and the sub-inertial navigation system;

φ _mFor utilizing main, sub-inertial navigation system output, the installation deviation angle between the master that actual measurement obtains, the sub-inertial navigation system;

Subscript x, y, z represent three axles of projection place coordinate system, are respectively navigation coordinate herein and are the east orientation axle, north orientation axle of n system and day to axle;

The state matrix of system is

$A=\left[\begin{array}{ccc}{A}_1& A_2& -{\mathrm{<figure-callout\; id="2"\; label="A"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">A</figure-callout>}}_2\\ 0_{3\&times;2}& A_3& -A_3\\ 0_{3\&times;2}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

Wherein:

$A_1=\left[\begin{array}{cc}0& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e}\\ -(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e})& 0\end{array}\right]$

R wherein _eBe earth radius, L is local latitude, ω _IeBe rotational-angular velocity of the earth; v _xBe the speed that main inertial navigation system is measured;

$A_2=\left[\begin{array}{ccc}{f}_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\\ f_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\end{array}\right]$

Wherein $f_{\mathrm{ib}}^s={\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^s& f_{\mathrm{iby}}^s& f_{\mathrm{ibz}}^s\end{array}\right]}^T$ For sub-inertial navigation system is measured the specific force value;

$A_3=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ibz}}& -{\mathrm{\&omega;}}_{\mathrm{iby}}\\ -{\mathrm{\&omega;}}_{\mathrm{<figure-callout\; id="0"\; label="ibz"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">ibz</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}\\ {\mathrm{\&omega;}}_{\mathrm{iby}}& -{\mathrm{\&omega;}}_{\mathrm{<figure-callout\; id="0"\; label="ibx"\; filenames="H2009100730747E00011.png,H2009100730747E00021.png"\; state="\{\{state\}\}">ibx</figure-callout>}}& 0\end{array}\right]$

ω wherein _Ib=[ω _Ibxω _Ibyω _Ibz] ^TFor utilizing the angular velocity information that accelerometer output specific force metrical information calculates on the gyro free strap down inertial navigation system;

The measurement vector of system is

Z＝[δv _x?δv _y] ^T

The observing matrix of system is

$H=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right].$

The installation of gyro free strap down inertial navigation system should guarantee three coordinate axis of three coordinate axis of gyro free strap down inertial navigation system and low-cost unmanned vehicle carrier coordinate system coincide (the optics calibration when installing guarantees).Therefore the initial attitude of measuring the gyro free strap down inertial navigation system is exactly an initial attitude of measuring low-cost unmanned vehicle.

Claims (3)

1. the rapid measurement method of initial attitude of a gyro free strap down inertial navigation system, it is characterized in that the gyro free strap down inertial navigation system on the low-cost unmanned vehicle as sub-inertial navigation system, strapdown inertial navigation system on the low-cost unmanned vehicle loader mechanism is measured initial attitude as main inertial navigation system according to the following step:

Step 1, the strapdown inertial navigation system on gyro free strap down inertial navigation system on the low-cost unmanned vehicle and the high-precision low-cost unmanned vehicle loader mechanism is linked by data cable;

After step 2, gyro free strap down inertial navigation system are carried out preheating, gather the data of accelerometer output;

Step 3, utilize main inertial navigation system with the initial velocity parameter with comprise that the initial position parameters of initial longitude, latitude binds to gyro free strap down inertial navigation system navigational computer;

Step 4, the attitude measurement information that comprises pitch angle, roll angle and course angle that main inertial navigation system is exported transfer to the gyro free strap down inertial navigation system by data cable, rough measure goes out the initial attitude of low-cost unmanned vehicle, finishes once transmitting fast of initial attitude;

The attitude initial value that step 5, gyro free strap down inertial navigation system are obtained by step 3 and step 4, utilize accelerometer output on the gyro free strap down inertial navigation system, measure the ratio force information of low-cost unmanned vehicle, calculate the angular velocity information of low-cost unmanned vehicle again by specific force;

Step 6, in sub-inertial navigation system, utilize the ratio force information of low-cost unmanned vehicle and the angular velocity information recursion of navigating to resolve, measure the velocity amplitude of low-cost unmanned vehicle;

The speed that step 7, speed and sub-inertial navigation system that main inertial navigation system is measured are measured is poor, and with difference as observation vector, the employing Kalman Filter Technology is estimated the horizontal direction installation deviation φ that between main inertial navigation system and the sub-inertial navigation system _x, φ _yThe estimation time is 120 seconds, is filtering estimation stabilization time, and uses the average filter technology by 80 seconds to 120 seconds and carry out smoothing processing for data in wherein preceding 80 seconds;

${\mathrm{\&phi;}}_x=\frac{{\widetilde{\mathrm{\&phi;}}}_x\left(1\right)......+{\widetilde{\mathrm{\&phi;}}}_x\left(k\right)......+{\widetilde{\mathrm{\&phi;}}}_x\left(N\right)}{N}$

${\mathrm{\&phi;}}_y=\frac{{\widetilde{\mathrm{\&phi;}}}_y\left(1\right)......+{\widetilde{\mathrm{\&phi;}}}_y\left(k\right)......+{\widetilde{\mathrm{\&phi;}}}_y\left(N\right)}{N}$

Wherein

Expression is by in 80 seconds to 120 seconds data smoothing processes, during k filtering estimation

Between the point the filtering estimated value; In 80 seconds to 120 seconds data smoothing processes, time point, i.e. k=1～N are estimated in total N filtering;

Step 8, utilize step 7 to estimate the horizontal direction installation deviation φ between main inertial navigation system and the sub-inertial navigation system _x, φ _y, and sub-inertial navigation system is measured the specific force value

Systematic survey specific force value in the main inertial navigation

Measure the orientation installation deviation φ between main inertial navigation system and the sub-inertial navigation system _z

${\mathrm{\&phi;}}_z=\arcsin \left(\frac{k_1{k}_4+k_3{k}_2}{{k_1}^2+{k_2}^2}\right)$

Parameter k wherein ₁, k ₂, k ₃, k ₄For

$k_1=f_{\mathrm{ibx}}^m$

$k_2=f_{\mathrm{iby}}^m$

$k_3=\frac{f_{\mathrm{ibx}}^s+\sin {\mathrm{\&phi;}}_y\cos {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\&phi;}}_y}-\frac{\sin {\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{iby}}^s}{\cos {\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_y}+\frac{\sin {\mathrm{\&phi;}}_y\sin {\mathrm{\&phi;}}_x\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\&phi;}}_x\cos {\mathrm{\&phi;}}_y};$

$k_4=\frac{\sin {\mathrm{\&phi;}}_x{f}_{\mathrm{ibz}}^m-f_{\mathrm{iby}}^s}{\cos {\mathrm{\&phi;}}_x}$

Step 9, utilize step 7 to estimate the horizontal direction installation deviation φ between main inertial navigation system and the sub-inertial navigation system _x, φ _y, and step 8 is measured the orientation installation deviation φ between main inertial navigation system and the sub-inertial navigation system _zConstruct the installation deviation direction cosine matrix

$C_s^m=\left(\begin{array}{ccc}1& {\mathrm{\&phi;}}_z& -{\mathrm{\&phi;}}_y\\ -{\mathrm{\&phi;}}_z& 1& {\mathrm{\&phi;}}_x\\ {\mathrm{\&phi;}}_y& -{\mathrm{\&phi;}}_x& 1\end{array}\right);$

Step 10, the main inertial navigation system that utilizes step 8 structure and the installation deviation matrix between the sub-inertial navigation system

And the direction cosine matrix of main inertial navigation output Construct the carrier coordinate system of low-cost unmanned vehicle and the direction cosine matrix between the navigation coordinate system

$C_s^n=C_m^n{C}_s^m;$

Step 11, by the direction cosine matrix between the carrier coordinate system of low-cost unmanned vehicle and the navigation coordinate system

Measure the initial attitude angle of unmanned vehicle.

2. the rapid measurement method of initial attitude of gyro free strap down inertial navigation system according to claim 1 is characterized in that used system state equation and measurement equation are as follows in the step 7:

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{X}=\mathrm{AX}\\ Z=\mathrm{HX}\end{array}\right.$

The state variable of selecting system is

X＝[δv _x?δv _y?φ _mx?φ _my?φ _mz?φ _x?φ _y?φ _z] ^T

Wherein:

δ v is the speed difference between main inertial navigation system and the sub-inertial navigation system;

φ is real installation deviation angle between main inertial navigation system and the sub-inertial navigation system;

φ _mFor utilizing main, sub-inertial navigation system output, the installation deviation angle between the master that actual measurement obtains, the sub-inertial navigation system;

Subscript x, y, z represent three axles of projection place coordinate system, are respectively navigation coordinate herein and are the east orientation axle, north orientation axle of n system and day to axle;

The state matrix of system is

$A=\left[\begin{array}{ccc}{A}_1& A_2& -A_2\\ 0_{3\&times;2}& A_3& -A_3\\ 0_{3\&times;2}& 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

Wherein:

$A_1=\left[\begin{array}{cc}0& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e}\\ -(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e})& 0\end{array}\right]$

R wherein _eBe earth radius, L is local latitude, ω _IeBe rotational-angular velocity of the earth; v _xBe the speed that main inertial navigation system is measured;

$A_2=\left[\begin{array}{ccc}{f}_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\\ f_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\end{array}\right]$

Wherein

For sub-inertial navigation system is measured the specific force value;

$A_3=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ibz}}& -{\mathrm{\&omega;}}_{\mathrm{iby}}\\ {-\mathrm{\&omega;}}_{\mathrm{ibz}}& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}\\ {\mathrm{\&omega;}}_{\mathrm{iby}}& {-\mathrm{\&omega;}}_{\mathrm{ibx}}& 0\end{array}\right]$

ω wherein _Ib=[ω _Ibxω _Ibyω _Ibz] ^TFor utilizing the angular velocity information that accelerometer output specific force metrical information calculates on the gyro free strap down inertial navigation system;

The measurement vector of system is

Z＝[δv _x?δv _y] ^T

The observing matrix of system is

$H=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right].$

3. the rapid measurement method of initial attitude of gyro free strap down inertial navigation system according to claim 1 and 2 is characterized in that: three coordinate axis of gyro free strap down inertial navigation system and three coordinate axis of unmanned vehicle carrier coordinate system coincide.

Images