CN100587641C - A kind of attitude determination system that is applicable to the arbitrary motion mini system - Google Patents

Abstract

A kind of attitude determination system that is applicable to the arbitrary motion mini system, it is characterized in that: comprise micro-magnetic sensor MMS module, micro inertial measurement unit MIMU module, GPS module and Kalman filtration module, under mini system arbitrary motion state, the MIMU module decomposites mini system acceleration of gravity according to output data by the strap-down navigation algorithm; The MMS module is utilized roll angle, the angle of pitch and the course angle 3 d pose information of described acceleration of gravity by deciding appearance algorithm real-time resolving mini system; Simultaneously, utilize CFAR CFAR filter smoothing to fall noise in GPS output speed and the acceleration information; 3 d pose information is delivered to the Kalman filtration module on the one hand, intermittently proofreaies and correct the MIMU error with position and velocity information that GPS provides by information fusion algorithm, utilizes 3 d pose information real-time update C on the other hand _n ^bCoordinate conversion matrix; Coordinate conversion matrix C _n ^bTo be fed to MIMU module and GPS module, with the information translation of horizontal coordinates under carrier coordinate system.The present invention has utilized the iterative calculation relation between GPS, MIMU, the MMS, has solved the difficult problem of definite heading of moving vehicle under the arbitrary motion state, has broad application prospects.

Description

A kind of attitude determination system that is applicable to the arbitrary motion mini system

Technical field

The present invention relates to a kind of attitude determination system that is applicable to the arbitrary motion mini system, be applicable to determining of course under the mini system arbitrary motion state.

Background technology

Any movable body all needs high precision navigational system and control system, and the high precision navigational system provides current movable informations such as position, speed and attitude for movable body; High-precision control system moves according to the movable information controlled motion body mode as requested that navigational system provides, and wherein the precision of attitude information has decisive influence to the control system performance.

Large-scale movable body utilizes high precision, large volume, expensive inertial navigation system or star sensor to realize that the attitude under the arbitrary motion situation is definite.But large-scale movable body is used decide appearance equipment because bulky, cost is high and the attitude that can't be applied under the mini system arbitrary motion situation is determined.Maneuverability is big and randomness is strong for mini system (small aircraft, small submarine navigation device, small ground robot), and the difficulty that attitude is determined is very big.

Accelerometer and obliquity sensor are to measure mini system roll angle and angle of pitch most common form at present, but because two kinds of sensors all can't well be distinguished mini system acceleration of motion and acceleration of gravity, therefore can not provide the roll angle and the angle of pitch under the mini system arbitrary motion situation, only be suitable for static and the linear uniform motion situation.And above-mentioned two kinds of sensors all can not provide course angle.

Can measure 3 d pose information such as the roll angle of mini system under any motion state, the angle of pitch, course angle on the inertial navigation system principle, but it needs the initial value of initial attitude benchmark as integration, and the sharply increase in time of the error of microminiature inertial navigational system, generally can not work alone.

Magnetic Sensor can be measured the magnetic field intensity of three orthogonal directionss, in order to be scaled 3 d pose information, also must at first rely on other means, coordinate transformation relation determining under the mini system arbitrary motion state between three-dimensional Magnetic Sensor direction of measurement and the local level promptly must pre-determine the roll angle and the angle of pitch.Present most Magnetic Sensor attitude determination system all relies on accelerometer or obliquity sensor and provides the roll angle and the angle of pitch under static and the linear uniform motion state, and Magnetic Sensor is unavailable substantially under other motion of automobile state.

As seen, any existing attitude measurement mode all can't accurately be determined the 3 d pose information of mini system under any motion state.The present invention proposes a kind of MIMU (Micro Inertial Measure-ment Unit that is applicable to the arbitrary motion mini system, micro inertial measurement unit) and auxiliary MMS (the Micro Magnetic Sensors of GPS, micro-magnetic sensor) attitude determination system, utilize iterative calculation relation between GPS, MIMU, the MMS, constantly revise the MIMU error and provide 3 d pose information under the mini system arbitrary motion state, have broad application prospects.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art, a kind of attitude determination system that is applicable to the arbitrary motion mini system is provided, this system utilizes MIMU and the auxiliary MMS of GPS to decide appearance, for the mini system under the arbitrary motion state provides the high-precision three-dimensional attitude information.

Technical solution of the present invention is: a kind of attitude determination system that is applicable to the arbitrary motion mini system, it is characterized in that: comprise micro-magnetic sensor MMS module, micro inertial measurement unit MIMU module, GPS module and Kalman filtration module, under mini system arbitrary motion state, the MIMU module decomposites mini system acceleration of gravity according to output data by the strap-down navigation algorithm; The MMS module is utilized roll angle, the angle of pitch and the course angle 3 d pose information of described acceleration of gravity by deciding appearance algorithm real-time resolving mini system; Simultaneously, utilize CFAR CFAR filter smoothing to fall noise in GPS output speed and the acceleration information; 3 d pose information is delivered to the Kalman filtration module on the one hand, intermittently proofreaies and correct the MIMU error with position and velocity information that GPS provides by information fusion algorithm, utilizes 3 d pose information real-time update C on the other hand _n ^bCoordinate conversion matrix; Coordinate conversion matrix C _n ^bTo be fed to MIMU module and GPS module, with the information translation of horizontal coordinates under carrier coordinate system.

Principle of the present invention is: general carrier coordinate system is seen shown in Figure 2, and being defined as mini system longitudinal axis working direction is Y-axis, upwards is the Z axle, and X-axis is determined according to the right-hand rule.MIMU, GPS, MMS three-dimensional orthogonal direction of measurement separately all overlaps with three of carrier coordinate system, sees shown in Figure 3ly, and GPS is positioned at the initial point of carrier coordinate system, and MMS, MIMU, three orthogonal measuring axles overlap with three of carrier coordinate system respectively.

Silicon MEMS gyro and accelerometer that the MIMU module is installed by three-dimensional orthogonal constitute, totally six inertia devices.Silicon MEMS gyro to measure mini system angular velocity, the silicon mems accelerometer is measured the mini system linear acceleration.Under static state the responsive acceleration of gravity of silicon mems accelerometer is in the projection of carrier coordinate system, thereby provides the coordinate conversion matrix of carrier coordinate system and local horizontal coordinates and roll angle, the angle of pitch; Under other arbitrary motion state, the measured value of MIMU module is through the strap-down navigation algorithm, utilize mini system line of motion speed, mini system motion angular velocity and rotational-angular velocity of the earth, decompose heterogeneities such as mini system line of motion acceleration in the acceleration measuring value, brother's formula acceleration, centripetal acceleration, acceleration of gravity in real time.Utilize acceleration of gravity at carrier coordinate system projection [g _x ^bg _y ^bg _z ^b] ^TAnd acceleration of gravity is at local horizontal coordinates projection [0 0 g] ^TBetween relation, calculate the roll angle γ and the pitching angle theta of mini system,

$\left[\begin{array}{c}{g}_x^b\\ g_y^b\\ g_z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

The MMS module is made up of the microminiature Magnetic Sensor that is installed on three orthogonal directionss, measures the magnetic field intensity of three orthogonal directionss of current location, thereby provides the direction and the size of current location magnetic vector.Calculate the projection of geomagnetic fieldvector under local horizontal coordinates according to roll angle γ and pitching angle theta:

$H_x^p=H_x^b\cos \mathrm{\γ}+H_z^b\sin \mathrm{\γ}$

$H_y^p=H_x^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_y^b\cos \mathrm{\θ}-H_z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

Calculate course angle under the situation of consideration magnetic declination λ:

$\mathrm{\φ}=\arctan (H_x^p/H_y^p)-\mathrm{\λ}$

The 3 d pose information that information fusion algorithm utilizes three-dimensional position that current time GPS provides and velocity information, MMS to provide, by navigation error and the inertia device error of Kalman filter correction MIMU, improve the computational accuracy of acceleration of gravity in carrier coordinate system.

The present invention's advantage compared with prior art is: (1) MIMU module constantly decomposites the composition of acceleration of gravity in the acceleration measuring value in real time in conjunction with the strap-down navigation algorithm, and provide the projection of acceleration of gravity in carrier coordinate system, the acceleration measuring value solved owing to can't be differentiated the problem of acceleration of motion and acceleration of gravity, thereby obtain the transformational relation between carrier coordinate system and the local horizontal coordinates, i.e. the roll angle and the angle of pitch; (2) adopt the CFAR wave filter to suppress the noise of GPS output data, improved the measuring accuracy of mini system position, speed.(3) the present invention's 3 d pose information of adopting GPS three-dimensional position, velocity information and MMS to provide error of constantly proofreading and correct MIMU has guaranteed under the situation that works long hours the MIMU measuring accuracy and has decomposed the precision of acceleration of gravity in the carrier coordinate system projection; (4) the MMS module can be measured carrier coordinate system three-dimensional magnetic field intensity in conjunction with deciding the appearance algorithm, utilizes the acquired roll angle and the angle of pitch, and loop iteration calculates the 3 d pose information under the mini system arbitrary motion state.

Description of drawings

Fig. 1 is MIMU and the auxiliary MMS attitude determination system composition frame chart of GPS that is applicable to the arbitrary motion mini system of the present invention;

Fig. 2 is the carrier coordinate system synoptic diagram;

Fig. 3 assists the installation method of MMS attitude determination system in carrier coordinate system for the MIMU of arbitrary motion mini system and the GPS of being applicable to of the present invention;

Fig. 4 is MIMU and the auxiliary MMS attitude determination system workflow diagram of GPS that is applicable to the arbitrary motion mini system of the present invention.

Embodiment

Fig. 1 is MIMU (micro inertial measurement unit) and auxiliary MMS (micro-magnetic sensor) the attitude determination system composition frame chart of GPS that is applicable to the arbitrary motion mini system, comprises hardware and software two parts, and hardware components is made up of MMS module, MIMU module, GPS module; Software algorithm partly comprises strap-down navigation, information fusion and decides the appearance algorithm.Under mini system arbitrary motion state, utilize the data and the strap-down navigation algorithm of MIMU module to decomposite mini system acceleration of gravity, roll angle, the angle of pitch and course angle that the MMS module is utilized acceleration of gravity and decided appearance algorithm real-time resolving mini system, adopt CFAR (CFAR) filter smoothing GPS output noise, by GPS module, 3 d pose information and the intermittent correction of information fusion algorithm MIMU error, improve acceleration of gravity and attitude algorithm precision.Fig. 4 has described strap-down navigation algorithm among Fig. 1, has decided appearance algorithm, information fusion algorithm are determined attitude based on the data of hardware output workflow.

At first, the auxiliary MMS attitude determination system of MIMU and GPS is under static state started working, and provides the projection [g of acceleration of gravity in carrier coordinate system by accelerometer in the MIMU module _x ^bg _y ^bg _z ^b] ^T, known acceleration of gravity is at be projected as [0 0 g] of local horizontal coordinates ^T,

$\left[\begin{array}{c}{g}_x^b\\ g_y^b\\ g_z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

Calculate the roll angle γ and the pitching angle theta of mini system,

$\mathrm{\θ}=\arcsin (g_y^b/g)$

$\mathrm{\γ}=-\arcsin (g_x^b/g\cos \mathrm{\θ})$

The projection of terrestrial magnetic field under horizontal coordinates:

$H_x^p=H_x^b\cos \mathrm{\γ}+H_z^b\sin \mathrm{\γ}$

$H_y^p=H_x^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_y^b\cos \mathrm{\θ}-H_z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

Because the magnetic line of force direction of terrestrial magnetic field is always pointed to magnetic north, i.e. H _x ^p, H _y ^pResultant vector point to magnetic north,

Magnetic heading is

${\mathrm{\φ}}^{\′}=\arctan (H_x^p/H_y^p)$

Use φ ' ' to deduct geomagnetic declination λ again, the angle that is exactly Digital Magnetic Compass and geographical north is course angle φ.

φ＝φ′-λ

$\mathrm{\φ}=\arctan (H_x^p/H_y^p)-\mathrm{\λ}$

θ, γ, φ are pitching, roll and the course angle 3 d pose information of being asked, and and then refresh coordinate conversion matrix between carrier coordinate system b and the local geographic coordinate system n

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]$

According to 3 d pose information decomposition GPS position and the velocity measurement obtained under the quiescent conditions, and and then the three-dimensional position, three-dimensional velocity and the course information that utilize to obtain MIMU is carried out initial alignment, mini system enters the arbitrary motion state.

GPS exports longitude, latitude and height in real time, obtains speed and percentage speed variation through single order differential and second-order differential operation, and the CFAR wave filter adopts the method for running mean to suppress the noise of GPS output data.According to the maneuverability of mini system motion the length T=M Δ t of sliding window is set, adopts the mode of first in first out to determine to participate in average data in the sliding window, segment data is through the average noise that suppresses gps data when utilizing before the current time T.V ^E _{T-M Δ t}～V ^E _t, V ^N _{T-M Δ t}～V ^N _t, V ^U _{T-M Δ t}～V ^U _t, V _t ^E, V _t ^N, V _t ^U Represent before CFAR smoothly and the movement velocity and the acceleration of motion of sky, northeast coordinate system afterwards. $V_{\mathrm{en}}^b={\left[\begin{array}{ccc}{\stackrel{\‾}{V}}_t^x& {\stackrel{\‾}{V}}_t^x& {\stackrel{\‾}{V}}_t^x\end{array}\right]}^T$ With ${\stackrel{\·}{V}}_{\mathrm{en}}^b={\left[\begin{array}{ccc}{\stackrel{\‾}{\stackrel{\·}{V}}}_t^x& {\stackrel{\‾}{\stackrel{\·}{V}}}_t^x& {\stackrel{\‾}{\stackrel{\·}{V}}}_t^x\end{array}\right]}^T$ The movement velocity and the acceleration of expression carrier coordinate system.

${\stackrel{\‾}{V}}_t^E=\frac{{V^E}_{t-\mathrm{M\Δt}}+{V^E}_{t-(M-1)\mathrm{\Δt}}+{V^E}_{t-(M-2)\mathrm{\Δt}}+...+{V^E}_t}{\mathrm{M\Δt}}$

${\stackrel{\‾}{V}}_t^N=\frac{{V^N}_{t-\mathrm{M\Δt}}+{V^N}_{t-(M-1)\mathrm{\Δt}}+{V^N}_{t-(M-2)\mathrm{\Δt}}+...+{V^N}_t}{\mathrm{M\Δt}}$

${\stackrel{\‾}{V}}_t^U=\frac{{V^U}_{t-\mathrm{M\Δt}}+{V^U}_{t-(M-1)\mathrm{\Δt}}+{V^U}_{t-(M-2)\mathrm{\Δt}}+...+{V^U}_t}{\mathrm{M\Δt}}$

${\stackrel{\‾}{\stackrel{\·}{V}}}_t^E=\frac{{{\stackrel{\·}{V}}^E}_{t-\mathrm{M\Δt}}+{{\stackrel{\·}{V}}^E}_{t-(M-1)\mathrm{\Δt}}+{{\stackrel{\·}{V}}^E}_{t-(M-2)\mathrm{\Δt}}+...+{{\stackrel{\·}{V}}^E}_t}{\mathrm{M\Δt}}$

${\stackrel{\‾}{\stackrel{\·}{V}}}_t^N=\frac{{{\stackrel{\·}{V}}^N}_{t-\mathrm{M\Δt}}+{{\stackrel{\·}{V}}^N}_{t-(M-1)\mathrm{\Δt}}+{{\stackrel{\·}{V}}^N}_{t-(M-2)\mathrm{\Δt}}+...+{{\stackrel{\·}{V}}^N}_t}{\mathrm{M\Δt}}$

${\stackrel{\‾}{\stackrel{\·}{V}}}_t^U=\frac{{{\stackrel{\·}{V}}^U}_{t-\mathrm{M\Δt}}+{{\stackrel{\·}{V}}^U}_{t-(M-1)\mathrm{\Δt}}+{{\stackrel{\·}{V}}^U}_{t-(M-2)\mathrm{\Δt}}+...+{{\stackrel{\·}{V}}^U}_t}{\mathrm{M\Δt}}$

$V_{\mathrm{en}}^b=\left[\begin{array}{c}{\stackrel{\‾}{V}}_t^x\\ {\stackrel{\‾}{V}}_t^y\\ {\stackrel{\‾}{V}}_t^z\end{array}\right]=C_n^b\·\left[\begin{array}{c}{\stackrel{\‾}{V}}_t^E\\ {\stackrel{\‾}{V}}_t^N\\ {\stackrel{\‾}{V}}_t^U\end{array}\right],{\stackrel{\·}{V}}_{\mathrm{en}}^b=\left[\begin{array}{c}{\stackrel{\‾}{\stackrel{\·}{V}}}_t^x\\ {\stackrel{\‾}{\stackrel{\·}{V}}}_t^y\\ {\stackrel{\‾}{\stackrel{\·}{V}}}_t^z\end{array}\right]=C_n^b\·\left[\begin{array}{c}{\stackrel{\‾}{\stackrel{\·}{V}}}_t^E\\ {\stackrel{\‾}{\stackrel{\·}{V}}}_t^N\\ {\stackrel{\‾}{\stackrel{\·}{V}}}_t^U\end{array}\right]$

MIMU module real-time continuous output gyro, acceleration measuring value, and it is imported strap-down navigation resolve, ask for the acceleration of gravity under the carrier coordinate system

$g^b=\left[\begin{array}{c}{g}_x^b\\ g_y^b\\ g_z^b\end{array}\right]={\stackrel{\·}{V}}_{\mathrm{en}}^b({2\mathrm{\ω}}_{\mathrm{ie}}^b+{\mathrm{\ω}}_{\mathrm{en}}^b)\×V_{\mathrm{en}}^b-f^b$

The every vector that is of following formula, f ^bBe the direct measured value of accelerometer (directly measured value is in carrier coordinate system); Suppose that L is the latitude that GPS provides, rotational-angular velocity of the earth ω in the local geographic coordinate system _Ie ⁿCan be decomposed into

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{iex}}^n& {\mathrm{\ω}}_{\mathrm{iey}}^n& {\mathrm{\ω}}_{\mathrm{iez}}^n\end{array}\right]}^T={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]}^T$

The angular velocity omega of the spherical coordinate system relatively that the mini system motion produces in the local geographic coordinate system _En ⁿFor

${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{enx}}^n& {\mathrm{\ω}}_{\mathrm{eny}}^n& {\mathrm{\ω}}_{\mathrm{enz}}^n\end{array}\right]}^T={\left[\begin{array}{ccc}-\frac{{\stackrel{\‾}{V}}_t^N}{R_{\mathrm{yt}}}& \frac{{\stackrel{\‾}{V}}_t^E}{R_{\mathrm{xt}}}& \frac{{\stackrel{\‾}{V}}_t^E}{R_{\mathrm{xt}}}\tan L\end{array}\right]}^T$

V _y ⁿ, V _x ⁿBe north orientation speed and the east orientation velocity information of mini system in local geographic coordinate system of utilizing course angle information decomposition GPS speed to obtain, R _Yt, R _XtBe respectively the meridian ellipse radius and the prime plane radius of the earth.

Obtain $g^b={\left[\begin{array}{ccc}{g}_x^b& g_y^b& g_z^b\end{array}\right]}^T$ After, can adopt algorithm computation mini system roll angle, the angle of pitch and the course angle identical with stationary state.

Information fusion is utilized three-dimensional velocity, three-dimensional position and the 3 d pose information of current acquisition, adopts the error of standard K alman filter correction MIMU.Kalman filter status variable

$X\left(t\right)={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\mathrm{\δv}}_x& {\mathrm{\δv}}_y& {\mathrm{\δv}}_z& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T$

Kalman wave filter observational variable is the difference of three-dimensional velocity, three-dimensional position and the 3 d pose information of the three-dimensional velocity, three-dimensional position and the 3 d pose information that above obtain and MIMU output.

Z(t)＝[φ _xφ _yφ _zδv _xδv _yδv _zδLδλδh] ^T

Set up the state equation and the observation equation of Kalman wave filter according to inertial navigation standard error equation, all the other calculating are identical with standard K alman wave filter.

Claims (6)

1, a kind of attitude determination system that is applicable to the arbitrary motion mini system, it is characterized in that: comprise micro-magnetic sensor MMS module, micro inertial measurement unit MIMU module, GPS module and Kalman filtration module, under mini system arbitrary motion state, the MIMU module decomposites mini system acceleration of gravity according to output data by the strap-down navigation algorithm, by deciding roll angle, the angle of pitch of appearance algorithm real-time resolving mini system; The MMS module is utilized acquired roll angle, angle of pitch real-time resolving course angle; Simultaneously, utilize CFAR CFAR filter smoothing to fall noise in GPS output speed and the acceleration information; 3 d pose information is delivered to the Kalman filtration module on the one hand, intermittently proofreaies and correct the MIMU error with position and velocity information that GPS provides by information fusion algorithm, utilizes 3 d pose information real-time update C on the other hand _n ^bCoordinate conversion matrix; Coordinate conversion matrix C _n ^bTo be fed to MIMU module and GPS module, with the information translation of horizontal coordinates under carrier coordinate system.

2, the attitude determination system that is applicable to the arbitrary motion mini system according to claim 1, it is characterized in that: described MMS module is made up of the microminiature Magnetic Sensor that is installed on three orthogonal directionss, measure the earth magnetic field intensity of three orthogonal directionss of current location, thereby provide the direction and the size of current location magnetic-field vector of the earth.

3, the attitude determination system that is applicable to the arbitrary motion mini system according to claim 1 is characterized in that: the silicon MEMS gyro to measure mini system angular velocity in the described MIMU module, and the silicon mems accelerometer is measured the mini system linear acceleration; Under static state, silicon mems accelerometer in the MIMU module is measured the component of acceleration of gravity in the three-dimensional orthogonal direction, provides the coordinate conversion matrix of carrier coordinate system and local horizontal coordinates and roll angle, the angle of pitch.

4, the attitude determination system that is applicable to the arbitrary motion mini system according to claim 1, it is characterized in that: described CFAR wave filter adopts the method for running mean to suppress the noise of GPS output data, the time span T=M Δ t of sliding window is set according to maneuverability, M is the slip number, Δ t is the time slot, the mode of employing first in first out is determined the average data of the interior participation of sliding window, utilizes the noise of current time data process CFAR wave filter inhibition gps data in the T period before.

5, the attitude determination system that is applicable to the arbitrary motion mini system according to claim 1, it is characterized in that: described strap-down navigation algorithm is set up basic mechanical layout equation under carrier coordinate system, thereby decomposites the projection of acceleration of gravity three of carrier coordinate system from the acceleration measuring value.

6, the attitude determination system that is applicable to the arbitrary motion mini system according to claim 1 is characterized in that: described MMS module utilizes the process of roll angle, the angle of pitch and the course angle 3 d pose information of described acceleration of gravity by deciding appearance algorithm real-time resolving mini system as follows: utilize the projection [g of acceleration of gravity in carrier coordinate system _x ^bg _y ^bg _z ^b] ^TAnd acceleration of gravity is in the projection [0 0 g] of local horizontal coordinates ^TBetween relation, calculate the roll angle γ and the pitching angle theta of mini system,

$\left[\begin{array}{c}{g}_x^b\\ g_y^b\\ g_z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}\sin \mathrm{\θ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$

Calculate the projection H of current geomagnetic fieldvector under horizontal coordinates according to γ and θ _x ^p, H _y ^p:

$H_x^p=H_x^b\cos \mathrm{\γ}+H_z^b\sin \mathrm{\γ}$

$H_y^p=H_x^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_y^b\cos \mathrm{\θ}-H_z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

Calculate course angle φ under the situation of consideration magnetic declination λ:

$\mathrm{\φ}=\arctan (H_x^p/H_y^p)-\mathrm{\λ}.$

Images