CN102654404B - Method for improving resolving precision and anti-jamming capability of attitude heading reference system - Google Patents

Abstract

The invention discloses a method for improving resolving precision and anti-jamming capability of an attitude heading reference system. The attitude heading reference system mainly comprises a micro mechanical gyro, a micro mechanical accelerometer and a geomagnetic sensor, and is used for measuring attitude information of a movement carrier in real time. The method specifically comprises the following steps of: firstly, initializing the system according to output of the accelerometer and the geomagnetic sensor for completing a coarse alignment process; secondly, according to output of the gyro, determining an updating equation of a quaternion, calculating a quaternion of an updating system in real time, and realizing an updating process of the system; thirdly, with the output of the accelerometer and the geomagnetic sensor as the reference, realizing a correction process of the system by applying a kalman filtering technology; and fourthly, by combining with a fuzzy control theory, adding a fuzzy control module in the system, estimating the measurement noise of the system in real time, and realizing self-adaptive kalman filtration. According to the invention, the calculating complexity of the system is greatly lowered while the precision of the attitude heading reference system is improved; and especially, when the system is interfered by larger noise, the method can be used for rightly estimating and measuring the characteristic of the noise, thereby improving the attitude precision and enhancing the stability and reliability of the system.

Description

Method for improving resolving precision and system anti-interference capability of attitude and heading reference system

Technical Field

The invention relates to a method for improving resolving precision of a navigation attitude reference system and anti-interference capability of the system based on a fuzzy control self-adaptive Kalman filtering algorithm.

Technical Field

An Attitude and Heading Reference System (AHRS) mainly comprises a micro-mechanical gyroscope, a micro-mechanical accelerometer, a geomagnetic sensor and other devices, and is used for measuring Attitude information of a moving carrier in real time. The navigation attitude reference system is based on the inertial element for measurement, and can completely and autonomously determine the attitude information of the carrier without emitting or receiving electromagnetic waves, so that the navigation attitude reference system has good concealment and is widely applied to military affairs, such as: unmanned planes, navigation and guidance, aerospace, and the like; meanwhile, the navigation attitude reference system adopts a micro mechanical device, and the system has high integration level, small volume and low cost, so the system also has wide application in the civil field, such as: vehicle navigation and control, platform control, vessel attitude control, robots, and the like.

In the heading reference system, heading information of a carrier is mainly determined by a geomagnetic sensor, however, in the current social life, the carrier is often in an environment with electromagnetic interference due to the wide application of electronic products such as electric appliances and wireless communication equipment. The kalman filtering module in the attitude reference system requires that system noise and measurement noise are known a priori, and the system noise can be obtained through multiple measurements of the system, however, the measurement noise is uncertain due to a complex electromagnetic environment, which seriously affects the precision of a conventional kalman filtering result, and even leads to system filtering divergence in serious cases. Therefore, how to estimate the measurement noise in real time becomes the primary problem of improving the anti-interference capability and the resolving accuracy of the system.

Sasiaadek proposes an adaptive Kalman Filtering algorithm Based on Fuzzy control in a paper "Sensor Fusion Based on Fuzzy Kalman Filtering for auto nomous Robot Vehicle", and successfully applies the adaptive Kalman Filtering algorithm to a GPS/INS combined navigation system. At present, an Adaptive Kalman filtering algorithm based on Fuzzy control is widely applied to a GPS/INS combined navigation System, however, the application of the Adaptive Kalman filtering algorithm in AHRS is very little, and although the algorithm is applied to an Attitude reference System in a paper "Fuzzy Adaptive Extended Kalman Filter for Miniature attribute and heading reference System", Qin Wei adopts Extended Kalman filtering and simultaneously adjusts System noise and measurement noise in real time, which undoubtedly greatly increases the computational complexity of the System and seriously affects the real-time performance of the System.

Disclosure of Invention

The invention aims to overcome the defects in the prior art, and provides a method for improving the resolving precision of a navigation attitude reference system and the anti-interference capability of the system by estimating the measurement noise in real time and realizing adaptive filtering based on the conventional Kalman filtering in combination with a fuzzy control theory so as to improve the stability and the reliability of the system.

The method provided by the invention is based on conventional kalman filtering, combines a fuzzy control theory, estimates the measurement noise in real time, realizes self-adaptive filtering, and improves the resolving precision and the anti-interference capability of the system, wherein the navigation attitude reference system mainly comprises a micro-mechanical gyroscope, a micro-mechanical accelerometer and a geomagnetic sensor and is used for measuring the attitude information of a moving carrier in real time; the method specifically comprises the following steps:

firstly, initializing a system according to the output of the accelerometer and the magnetic sensor to finish the course of coarse alignment;

determining an updating equation of the quaternion according to the output of the gyroscope, calculating the quaternion of the updating system in real time, and realizing the updating process of the system;

thirdly, the outputs of the accelerometer and the magnetic sensor are used as references, and a kalman filtering technology is applied to realize the correction process of the system;

and fourthly, combining a fuzzy control theory, adding a fuzzy control module into the system, estimating the measurement noise of the system in real time, and realizing self-adaptive kalman filtering.

In the course of coarse alignment, the invention determines the initial pitch angle and roll angle of the system according to the output of the accelerometer; an initial heading angle is determined in conjunction with the output of the magnetic sensor. Determining an initial strapdown matrix and an initial quaternion according to the initial attitude angle;

in the system updating process, updating the quaternion by adopting a Longge-Kutta algorithm, and updating the strapdown matrix and the attitude angle according to the quaternion obtained by updating;

in the system correction process, a quaternion error vector and a zero offset error vector of a gyroscope are used as state vectors, an acceleration error vector and a geomagnetic error vector are used as observation vectors, a state equation and an observation equation of Kalman filtering are determined, a system error is estimated in real time, the error is fed back to the system, and the quaternion, the strapdown matrix and the attitude angle are corrected;

the fuzzy control module is provided with two fuzzy controllers, the theory and the actual variance of a acceleration part and the theory and the actual variance of a magnetic part in a residual vector are respectively calculated in real time in the Kalman filtering process, and the ratio of the theoretical variance and the actual variance of the two is respectively used as the input of the two fuzzy controllers.

The fuzzy controller respectively outputs the correction coefficient to correct the measurement noise of the acceleration and the measurement noise of the magnetism according to the input, thereby realizing the effect of estimating the measurement noise in real time.

The invention can improve the accuracy of the attitude and heading reference system and greatly reduce the calculation complexity of the system; particularly, when the system is interfered by large noise, the algorithm can correctly estimate the measurement noise characteristic, improve the attitude precision and improve the stability and reliability of the system.

Drawings

FIG. 1 is a diagram of an attitude and heading reference system based on fuzzy control adaptive kalman filtering according to the present invention.

FIG. 2 is a diagram of input and output membership functions of a fuzzy control module according to the present invention.

Detailed Description

The invention will be described in detail below with reference to the following figures: the invention relates to a method for improving resolving accuracy and anti-interference capability of a navigation attitude reference system.

The method provided by the invention is based on conventional kalman filtering, combines a fuzzy control theory, estimates the measurement noise in real time, realizes self-adaptive filtering, and improves the resolving precision and the anti-interference capability of the system, wherein the navigation attitude reference system mainly comprises a micro-mechanical gyroscope, a micro-mechanical accelerometer and a geomagnetic sensor and is used for measuring the attitude information of a moving carrier in real time; the method specifically comprises the following steps:

firstly, initializing a system according to the output of an accelerometer and a magnetic sensor to complete a coarse alignment process;

secondly, determining an updating equation of the quaternion according to the output of the gyroscope, calculating the quaternion of the updating system in real time, and realizing the updating process of the system;

thirdly, the outputs of the accelerometer and the magnetic sensor are used as references, and a kalman filtering technology is applied to realize the correction process of the system;

and fourthly, combining a fuzzy control theory, adding a fuzzy control module into the system, estimating the measurement noise of the system in real time, and realizing self-adaptive kalman filtering.

In the course of coarse alignment, the invention determines the initial pitch angle and roll angle of the system according to the output of the accelerometer; an initial heading angle is determined in conjunction with the output of the magnetic sensor. Determining an initial strapdown matrix and an initial quaternion according to the initial attitude angle;

since the output of the accelerometer is not changed due to the change of the heading angle, the heading angle y is 0, and the acceleration conversion relationship between the carrier coordinate system and the navigation coordinate system can be represented as follows:

$\left(\begin{array}{c}{a}_x^b\\ a_y^b\\ a_z^b\end{array}\right)=C_n^b\left(\begin{array}{c}{a}_x^n\\ a_y^n\\ a_z^n\end{array}\right)\left(\begin{array}{ccc}\cos r& 0& \sin r\\ \sin r\sin p& \cos p& -\cos r\sin p\\ -\sin r\cos p& \sin p& \cos r\cos p\end{array}\right)\left(\begin{array}{c}0\\ 0\\ -g\end{array}\right)$

from this, the pitch angle p and roll angle r can be calculated as:

$\left\{\begin{array}{c}p=\arcsin \left(\frac{a_y^b}{-g}\right)\\ r=\arctan \left(\frac{a_x^b}{{-a}_z^b}\right)\end{array}\right.$

according to the calculated pitch angle p and roll angle r, the conversion relation of the geomagnetic vector can be expressed as:

$\left(\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right)=\left(\begin{array}{ccc}\cos r& \sin r\sin p& -\sin r\cos p\\ 0& \cos p& \sin p\\ \sin r& -\cos r\sin p& \cos r\cos p\end{array}\right)\left(\begin{array}{c}{m}_x^b\\ m_y^b\\ m_z^b\end{array}\right)$

then, the heading angle is expressed as:

$y=\arctan \left(\frac{m_x}{m_y}\right)$

after the initial attitude angle of the carrier is determined, calculating an initial strapdown matrix:

$T=\left(\begin{array}{ccc}\cos r\cos y-\sin r\sin p\sin y& -\cos p\sin y& \sin r\cos y+\cos r\sin p\sin y\\ \cos y\sin y+\sin r\sin p\cos y& \cos p\cos y& \sin r\sin y-\cos r\sin p\cos y\\ -\sin r\cos p& \sin p& \cos r\cos p\end{array}\right)$

then, the initial value of the quaternion is calculated:

$\left\{\begin{array}{c}\left|q_0\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}},\sin {\mathrm{gnq}}_0=+\\ \left|q_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}},{\mathrm{signq}}_1=\mathrm{sign}(T_{32}-T_{23})\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}},\mathrm{si}{\mathrm{gnq}}_2=\mathrm{sign}(T_{13}-T_{31})\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}},{\mathrm{signq}}_3=\mathrm{sign}(T_{21}-T_{12})\end{array}\right.$

in the system updating process, updating the quaternion by adopting a Longge-Kutta algorithm, and updating the strapdown matrix and the attitude angle according to the quaternion obtained by updating;

the quaternion update equation is:

wherein,the angular velocity of the navigation coordinate system relative to the carrier coordinate system is obtained through the output of the gyroscope under the carrier coordinate system.

In the system correction process, a quaternion error vector and a zero offset error vector of a gyroscope are used as state vectors, an acceleration error vector and a geomagnetic error vector are used as observation vectors, a state equation and an observation equation of Kalman filtering are determined, a system error is estimated in real time, the error is fed back to the system, and the quaternion, the strapdown matrix and the attitude angle are corrected;

in the Kalman filtering module, a quaternion error vector and a zero offset error vector of a gyroscope are selected as state vectors:

X＝[q_e1 q_e2 q_{e3 x y z}]^T

then the state equation for Kalman filtering is:

wherein:is composed ofIs used to generate the inverse symmetric matrix. [ W ]₁(t) W₂(t)]^TIs the system state noise, which is gaussian self-noise with mean 0 and variance q (t).

Selecting output a of accelerometer and geomagnetic sensor^b、m^bProjection of gravity acceleration vector and geomagnetic vector in carrier coordinate systemAs an observation vector:

<math><mrow> <mi>Z</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='(' close=')'> <mtable> <mtr> <mtd> <msup> <mi>a</mi> <mi>b</mi> </msup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <msup> <mi>a</mi> <mi>n</mi> </msup> </mtd> </mtr> <mtr> <mtd> <msup> <mi>m</mi> <mi>b</mi> </msup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <msup> <mi>m</mi> <mi>n</mi> </msup> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open='(' close=')'> <mtable> <mtr> <mtd> <msup> <mrow> <mi>&delta;</mi> <mover> <mi>a</mi> <mo>^</mo> </mover> </mrow> <mi>b</mi> </msup> </mtd> </mtr> <mtr> <mtd> <msup> <mrow> <mi>&delta;</mi> <mover> <mi>m</mi> <mo>^</mo> </mover> </mrow> <mi>b</mi> </msup> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

the Kalman filter's observation equation is then:

<math><mrow> <mi>Z</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='(' close=')'> <mtable> <mtr> <mtd> <mn>2</mn> <mo>*</mo> <mo>[</mo> <msup> <mover> <mi>a</mi> <mo>^</mo> </mover> <mi>b</mi> </msup> <mo>&times;</mo> <mo>]</mo> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>*</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mn>2</mn> <mo>*</mo> <mo>[</mo> <msup> <mover> <mi>m</mi> <mo>^</mo> </mover> <mi>b</mi> </msup> <mo>]</mo> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>*</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mfenced open='(' close=')'> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mi>e</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>&Delta;&epsiv;</mi> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open='(' close=')'> <mtable> <mtr> <mtd> <msub> <mi>V</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

wherein [ V ]₁(t) V₂(t)]^TTo observe noise, the mean is 0 and the variance is R (t), the noise is measured byAnd discretizing a continuous state equation and an observation equation, wherein the discrete Kalman filtering process comprises the following steps:

<math><mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mtable> <mtr> <mtd> <mrow> <mfenced open='(' close=')'> <mtable> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <msub> <mi>e</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mrow> <mi>&Delta;</mi> <mover> <mi>&epsiv;</mi> <mo>^</mo> </mover> </mrow> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open='(' close=')' separators=''> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mrow> <mi>&Delta;</mi> <mover> <mi>&epsiv;</mi> <mo>^</mo> </mover> </mrow> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mtd> </mtr> <mtr> <mtd> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&alpha;</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>&alpha;</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msub> <mi>R</mi> <mi>k</mi> </msub> <msubsup> <mi>K</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced></math>

wherein,for a state one-step prediction value, P_k|k-1One-step prediction value for mean square error, K_kFor optimum filter gain, alpha_kIs an innovation vector,Is state estimation, P_kIs the mean square error estimate.

The fuzzy control module is provided with two fuzzy controllers, the theory and the actual variance of a acceleration part and the theory and the actual variance of a magnetic part in a residual vector are respectively calculated in real time in the Kalman filtering process, and the ratio of the theoretical variance and the actual variance of the two is respectively used as the input of the two fuzzy controllers.

Innovation vector alpha_kAlso called residual vector, whose actual variance matrix is:

<math><mrow> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&alpha;</mi> <mi>k</mi> </msub> <msubsup> <mi>&alpha;</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow></math>

the theoretical variance matrix is:

<math><mrow> <msub> <mi>S</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> </mrow></math>

if the actual variance matrix is equal to the theoretical variance matrix, the actual measurement noise is equal to the measurement noise estimated by the user; if the actual variance matrix is larger than the theoretical variance matrix, the system is interfered by noise, the actual measurement noise is larger than the estimated measurement noise, and the estimated noise needs to be increased; similarly, if the actual variance matrix is smaller than the theoretical variance matrix, the estimation noise needs to be reduced, and the adaptive Kalman filtering is realized.

The actual variance matrix and the theoretical variance matrix can be respectively expressed as:

$C_k=\left[\begin{array}{cc}{C}_A& C_{\mathrm{AM}}\\ C_{\mathrm{MA}}& C_M\end{array}\right]$

$S_k=\left[\begin{array}{cc}{S}_A& S_{\mathrm{AM}}\\ S_{\mathrm{MA}}& S_M\end{array}\right]$

wherein, C_A、S_AMeasuring variance and theoretical variance of acceleration information; c_AM、C_MA、S_AM、S_MAThe measurement covariance and the theoretical covariance of the acceleration information and the geomagnetic information are as follows: c_M、S_MThe measured variance and the theoretical variance of the geomagnetic information are provided. To prevent interference between the accelerometer and the magnetic sensor, two fuzzy control adaptive modules (FLAS) are usedEach of which defines q_A＝tr(C_A)/tr(S_A)、q_M＝tr(C_M)/tr(S_M) The accelerometer and magnetic sensor are monitored for observation noise as inputs to two fuzzy control modules, where tr (-) is the trace of the matrix.

The fuzzy controller respectively outputs the correction coefficient to correct the measurement noise of the acceleration and the measurement noise of the magnetism according to the input, thereby realizing the effect of estimating the measurement noise in real time.

The two fuzzy control modules have the same input and output membership functions as shown in fig. 2. The reasoning rule of the fuzzy control is as follows:

and judging the noise variation trend as follows according to the input membership function, and obtaining the membership ambiguity beta:

if q belongs to [0.5, 0.7], then' noise is reduced;

if q ∈ [0.7, 1.3], then 'noise invariant':

if q ∈ [1.3, 1.5], then 'noise increase';

according to the output membership function, the weight of the output is obtained as follows:

if 'noise reduction', then W ═ 0.3 × β + 1;

if 'noise is unchanged', then

If 'noise increase', then W ═ 1-0.3 x β;

obtaining the corrected weight W from two fuzzy control modules respectively_A、W_MAnd correcting the observation noise in real time:

$R_k=\left[\begin{array}{cc}{W}_A& 0\\ 0& W_M\end{array}\right]\left[\begin{array}{cc}{R}_{A_{k-1}}& 0\\ 0& R_{M_{k-1}}\end{array}\right]=W*R_{k-1}$

the real-time estimation of the measurement noise by the fuzzy control module enables Kalman filtering to estimate system errors more stably and with higher precision.

$Q_{\mathrm{real}}=\left(\begin{array}{cccc}1& -q_{e1}& {-q}_{e2}& {-q}_{e3}\\ q_{e1}& 1& q_{e3}& -q_{e2}\\ q_{e2}& -q_{e3}& 1& q_{e1}\\ q_{e3}& q_{e2}& {-q}_{e1}& 1\end{array}\right)*Q$

The corrected quaternion Q_realConverted into a strapdown matrix T_real：

$T_{\mathrm{real}}=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)$

And obtaining a carrier attitude angle through the strapdown matrix, and outputting the carrier attitude angle as a result:

$\left\{\begin{array}{c}p=\arcsin \left(T_{32}\right)\\ r=\arctan \left(\frac{-T_{31}}{T_{33}}\right)\\ y=\arctan \left(\frac{-T_{12}}{{-T}_{22}}\right)\end{array}\right.$

FIG. 1 is a diagram of an attitude and heading reference system based on fuzzy control adaptive Kalman filtering, shown in the figure: updating quaternion according to the output of the gyroscope to further realize attitude updating, and correcting an attitude angle and a course angle by Kalman filtering by taking the outputs of the accelerometer and the magnetic sensor as references; the fuzzy control module estimates the observation noise in real time according to the observed quantity, thereby realizing the self-adaptive Kalman filtering based on the fuzzy control

FIG. 2 is a diagram of input and output membership functions of the fuzzy control module, which is output after fuzzification according to the input and output membership function.

Claims (1)

1. A method for improving resolving accuracy and system anti-interference capability of an attitude and heading reference system mainly comprises a micromechanical gyroscope, a micromechanical accelerometer and a geomagnetic sensor, and is used for measuring attitude information of a moving carrier in real time; the method is characterized by comprising the following steps:

firstly, initializing a system according to the output of an accelerometer and a magnetic sensor to complete a coarse alignment process;

secondly, determining an updating equation of the quaternion according to the output of the gyroscope, calculating the quaternion of the updating system in real time, and realizing the updating process of the system;

thirdly, the outputs of the accelerometer and the magnetic sensor are used as references, and a kalman filtering technology is applied to realize the correction process of the system;

fourthly, combining a fuzzy control theory, adding a fuzzy control module into the system, estimating the measurement noise of the system in real time, and realizing self-adaptive kalman filtering;

in the course of the coarse alignment, determining an initial pitch angle and a roll angle of the system according to the output of the accelerometer; determining an initial course angle by combining the output of the magnetic sensor; determining an initial strapdown matrix and an initial quaternion according to the initial attitude angle;

since the output of the accelerometer is not changed due to the change of the heading angle, the heading angle y is 0, and the acceleration conversion relationship between the carrier coordinate system and the navigation coordinate system can be represented as follows:

from this, the pitch angle p and roll angle r can be calculated as:

according to the calculated pitch angle p and roll angle r, the conversion relation of the geomagnetic vector can be expressed as:

then, the heading angle is expressed as:

after the initial attitude angle of the carrier is determined, calculating an initial strapdown matrix:

then, the initial value of the quaternion is calculated:

in the system updating process, updating the quaternion by adopting a Longge-Kutta algorithm, and updating the strapdown matrix and the attitude angle according to the quaternion obtained by updating;

the quaternion update equation is:

wherein,under a carrier coordinate system, the angular speed of the navigation coordinate system relative to the carrier coordinate system is obtained through the output of a gyroscope;

in the system correction process, a quaternion error vector and a zero offset error vector of a gyroscope are used as state vectors, an acceleration error vector and a geomagnetic error vector are used as observation vectors, a state equation and an observation equation of Kalman filtering are determined, a system error is estimated in real time, the error is fed back to the system, and the quaternion, the strapdown matrix and the attitude angle are corrected;

in the Kalman filtering module, a quaternion error vector and a zero offset error vector of a gyroscope are selected as state vectors:

X＝[q_e1 q_e2 q_{e3 x y z}]^T

then the state equation for Kalman filtering is:

wherein:is composed ofAn antisymmetric matrix of (a); [ W ]₁(t) W₂(t)]^TIs the system state noise, which is the Gaussian self-noise with mean 0 and variance Q (t);

selecting output a of accelerometer and geomagnetic sensor^b、m^bProjection of gravity acceleration vector and geomagnetic vector in carrier coordinate systemAs an observation vector:

the Kalman filter's observation equation is then:

wherein [ V ]₁(t) V₂(t)]^TIn order to observe noise, the mean value is 0, the variance is R (t), the continuous state equation and the observation equation are discretized, and the discrete Kalman filtering process is as follows:

wherein,for a state one-step prediction value, P_k|k-1One-step prediction value for mean square error, K_kFor optimum filter gain, alpha_kIs an innovation vector,Is state estimationMeter, P_kIs a mean square error estimate;

the fuzzy control module is provided with two fuzzy controllers, the theory and the actual variance of a acceleration part and the theory and the actual variance of a magnetic part in a residual vector are respectively calculated in real time in the Kalman filtering process, and the ratio of the theoretical variance and the actual variance of the two is respectively used as the input of the two fuzzy controllers;

innovation vector alpha_kAlso called residual vector, whose actual variance matrix is:

the theoretical variance matrix is:

if the actual variance matrix is equal to the theoretical variance matrix, the actual measurement noise is equal to the measurement noise estimated by the user; if the actual variance matrix is larger than the theoretical variance matrix, the system is interfered by noise, the actual measurement noise is larger than the estimated measurement noise, and the estimated noise needs to be increased; similarly, if the actual variance matrix is smaller than the theoretical variance matrix, the estimation noise needs to be reduced, and the adaptive Kalman filtering is realized;

the actual variance matrix and the theoretical variance matrix can be respectively expressed as:

wherein, C_A、S_AMeasuring variance and theoretical variance of acceleration information; c_AM、C_MA、S_AM、S_MAThe measurement covariance and the theoretical covariance of the acceleration information and the geomagnetic information are as follows: c_M、S_MMeasuring variance and theoretical variance of geomagnetic information; to prevent interference between the accelerometer and the magnetic sensor, two fuzzy control adaptive modules (FLAS) are used, each defining q_A＝tr(C_A)/tr(S_A)、q_M＝tr(C_M)/tr(S_M) Monitoring the observation noise of the accelerometer and the magnetic sensor as the input of two fuzzy control modules, wherein tr (-) is the trace of the matrix;

the fuzzy controller respectively outputs correction coefficients to correct the measurement noise of acceleration and the measurement noise of magnetism according to the input, so as to realize the effect of real-time estimation of the measurement noise;

the two fuzzy control modules have the same input and output membership functions; the reasoning rule of the fuzzy control is as follows:

if q belongs to [0.5, 0.7], then' noise is reduced;

if q ∈ [0.7, 1.3], then 'noise invariant':

if q ∈ [1.3, 1.5], then 'noise increase';

according to the output membership function, the weight of the output is obtained as follows:

if 'noise reduction', then W ═ 0.3 × β + 1;

if 'noise is unchanged', then

If 'noise increase', then W ═ 1-0.3 x β;

obtaining the corrected weight W from two fuzzy control modules respectively_A、W_MAnd correcting the observation noise in real time:

the real-time estimation of the measurement noise by the fuzzy control module enables Kalman filtering to estimate system errors more stably and with higher precision.

The corrected quaternion Q_realConverted into a strapdown matrix T_real：

And obtaining a carrier attitude angle through the strapdown matrix, and outputting the carrier attitude angle as a result: