CN102654404A - 一种提高航姿参考系统解算精度和系统抗干扰能力的方法 - Google Patents

Abstract

一种提高航姿参考系统解算精度和系统抗干扰能力的方法，所述的航姿参考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；所述的方法具体包括以下步骤：第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程；第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程；第三、以加速度计和磁传感器的输出作为参考，运用kalman滤波技术，实现系统的修正过程；第四、结合模糊控制理论，在系统中加入模糊控制模块，实时估计系统的量测噪声，实现自适应kalman滤波；本发明可以在提高航姿参考系统精度的同时，大大降低了系统的计算复杂度；尤其在系统受较大噪声干扰时，该算法能够正确估计量测噪声特性，改善姿态精度，提高系统的稳定性和可靠性。

Description

一种提高航姿参考系统解算精度和系统抗干扰能力的方法

技术领域

本发明涉及的是一种基于模糊控制自适应Kalman滤波算法来提高航姿参考系统解算精度和系统抗干扰能力的方法。 

技术背景

航姿参考系统(Attitude and Heading Reference System，AHRS)主要由微机械陀螺仪、微机械加速度计和地磁传感器等器件组成，用来实时测量运动载体的姿态信息。航姿参考系统依靠惯性元件进行测量，无需向外发射或接受电磁波，可完全自主确定载体的姿态信息，因此其隐蔽性很好，在军事上有很广泛的应用，如：无人飞机、导航与制导、航空航天等；同时，由于航姿参考系统采用微机械器件，系统的集成度高、体积小巧、成本低，因此其在民用领域也有着广泛的应用，如：车辆导航与控制、平台控制、船舶姿态控制、机器人等等。 

在航姿参考系统中，载体的航向信息主要是依靠地磁传感器来确定，然而，在当今的社会生活中，电器、无线通信设备等电子产品的广泛应用，使得载体经常处于有电磁干扰的环境之中。航姿参考系统中的kalman滤波模块，要求对系统噪声和量测噪声先验已知，系统噪声可以经过对系统的多次测量而得到，然而，复杂的电磁环境使得量测噪声不确定，这将严重影响常规kalman滤波结果的精度，严重时甚至会导致系统滤波发散。因此，如何实时估计量测噪声，成为了提高系统抗干扰能力及解算精度的首要问题。 

J.Z.Sasiadek在论文《Sensor Fusion Based on Fuzzy Kalman Filtering forAutonomous Robot Vehicle》中提出了基于模糊控制的自适应kalman滤波算法，并将其成功的应用到了GPS/INS组合导航系统中。目前，基于模糊控制的自适应kalman滤波算法已经广泛的应用于GPS/INS组合导航系统中，然而其在AHRS中的应用很少，Qin Wei在论文《Fuzzy Adaptive Extended Kalman Filter for Miniature Attitude and HeadingReference System》中虽将该算法应用于航姿参考系统中，然而其采用扩展kalman滤波并同时对系统噪声和量测噪声同时进行实时调整，这无疑大大的增加系统的计算复杂度，严重影响了系统的实时性。 

发明内容

本发明的目的在于克服现有技术存在的不足，而提供一种以常规Kalman滤波为基础，结合模糊控制理论，实时估计量测噪声，实现自适应滤波，以提高系统的稳定性和可靠性 的提高航姿参考系统解算精度和系统抗干扰能力的方法。 

本发明提出的方法以常规kalman滤波为基础，结合模糊控制理论，实时估计量测噪声，实现自适应滤波，提高系统的解算精度和抗干扰能力，所述的航姿参考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；所述的方法具体包括以下步骤： 

第一根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程； 

第二根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程； 

第三以加速度计和磁传感器的输出作为参考，运用kalman滤波技术，实现系统的修正过程； 

第四结合模糊控制理论，在系统中加入模糊控制模块，实时估计系统的量测噪声，实现自适应kalman滤波。 

本发明在所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数； 

所述的系统更新过程中，四元数的更新采用龙格-库塔算法，根据更新得到的四元数，更新捷联矩阵和姿态角； 

所述的系统修正过程中，四元数误差矢量和陀螺仪的零偏误差矢量作为状态向量、以加速度误差矢量和地磁误差矢量作为观测向量，确定kalman滤波的状态方程和观测方程，实时估计系统误差，并将误差反馈到系统中，修正四元数、捷联矩阵和姿态角； 

所述的模糊控制模块中有两个模糊控制器，在kalman滤波过程中，分别实时计算残差向量中加速度部分的理论、实际方差和磁部分的理论、实际方差，分别以二者的理论方差与实际方差的比值作为两个模糊控制器的输入。 

本发明所述的模糊控制器根据输入，分别输出修正系数修正加速度的量测噪声和磁的量测噪声，实现实时估计量测噪声的作用。 

本发明可以在提高航姿参考系统精度的同时，大大降低了系统的计算复杂度；尤其在系统受较大噪声干扰时，该算法能够正确估计量测噪声特性，改善姿态精度，提高系统的稳定性和可靠性。 

附图说明

图1是本发明所述的为基于模糊控制自适应kalman滤波的航姿参考系统图。 

图2是本发明所述的为模糊控制模块的输入、输出隶属度函数图。 

具体实施方式

下面将结合附图对本发明作详细的介绍：本发明是一种提高航姿参考系统解算精度和系统抗干扰能力的方法。 

本发明提出的方法以常规kalman滤波为基础，结合模糊控制理论，实时估计量测噪声，实现自适应滤波，提高系统的解算精度和抗干扰能力，所述的航姿参考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；所述的方法具体包括以下步骤： 

第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程； 

第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程； 

第三、以加速度计和磁传感器的输出作为参考，运用kalman滤波技术，实现系统的修正过程； 

第四、结合模糊控制理论，在系统中加入模糊控制模块，实时估计系统的量测噪声，实现自适应kalman滤波。 

本发明在所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数； 

由于航向角的改变，并不改变加速度计的输出，因此令航向角y＝0，则载体坐标系和导航坐标系下的加速度转换关系可表示为： 

$\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)$

由此，可以计算俯仰角p和横滚角r为： 

$\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.$

根据计算得到的俯仰角p和横滚角r，可以将地磁矢量的转换关系表示为： 

$\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)$

则，航向角表示为： 

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

确定载体初始姿态角后、计算初始捷联矩阵： 

$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)$

然后，计算四元数的初始值： 

$\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.$

所述的系统更新过程中，四元数的更新采用龙格-库塔算法，根据更新得到的四元数，更新捷联矩阵和姿态角； 

四元数更新方程为： 

其中， 

为在载体坐标系下，导航坐标系相对载体坐标系的角速度，通过陀螺仪的输出得到。 

所述的系统修正过程中，四元数误差矢量和陀螺仪的零偏误差矢量作为状态向量、以加速度误差矢量和地磁误差矢量作为观测向量，确定kalman滤波的状态方程和观测方程，实时估计系统误差，并将误差反馈到系统中，修正四元数、捷联矩阵和姿态角； 

Kalman滤波模块中，选取四元数误差矢量和陀螺仪的零偏误差矢量作为状态矢量： 

X＝[q_e1 q_e2 q_e3 ε_x ε_y ε_z]^T

则Kalman滤波的状态方程为： 

其中： 为 

的反对称矩阵。[W₁(t) W₂(t)]^T为系统状态噪声，其为均值为0、方差为Q(t)的高斯自噪声。 

选取加速度计和地磁传感器的输出a^b、m^b与重力加速度向量和地磁向量在载体坐标系中的投影 

的差值作为观测向量： 

$Z\left(t\right)=\left(\begin{array}{c}{a}^b-C_n^b{a}^n\\ m^b-C_n^b{m}^n\end{array}\right)=\left(\begin{array}{c}{\mathrm{\δ}\hat{a}}^b\\ {\mathrm{\δ}\hat{m}}^b\end{array}\right)$

则Kalman滤波的观测方程为： 

$Z\left(t\right)=\left(\begin{array}{cc}2*[{\hat{a}}^b\×]& 0_{3*3}\\ 2*\left[{\hat{m}}^b\right]& 0_{3*3}\end{array}\right)\left(\begin{array}{c}{q}_e\\ \mathrm{\Δ\ϵ}\end{array}\right)+\left(\begin{array}{c}{V}_1\left(t\right)\\ V_2\left(t\right)\end{array}\right)$

其中，[V₁(t) V₂(t)]^T为观测噪声，均值为0、方差为R(t)，将连续的状态方程与观测方程离散化，离散Kalman滤波过程如下： 

$\left\{\begin{array}{c}{\hat{X}}_{k|k-1}=\begin{array}{c}\left(\begin{array}{c}{\hat{q}}_{e_{k|k-1}}\\ {\mathrm{\Δ}\widehat{\mathrm{\ϵ}}}_{k|k-1}\end{array}\right)=\left(\begin{array}{c}0\\ {\mathrm{\Δ}\widehat{\mathrm{\ϵ}}}_{k-1}\end{array}\right)\end{array}\\ P_{k|k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T\\ K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}\\ {\mathrm{\α}}_k=Z_k-H_k{\hat{X}}_{k|k-1}\\ {\hat{X}}_k={\hat{X}}_{k|k-1}+K_k{\mathrm{\α}}_k\\ P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T\end{array}\right.$

其中， 

为状态一步预测值、P_k|k-1为均方误差一步预测值、K_k为最优滤波增益、α_k为新息向量、 

为状态估计、P_k为均方误差估计。 

所述的模糊控制模块中有两个模糊控制器，在Kalman滤波过程中，分别实时计算残差向量中加速度部分的理论、实际方差和磁部分的理论、实际方差，分别以二者的理论方差与实际方差的比值作为两个模糊控制器的输入。 

新息向量α_k又被称为残差向量，其实际方差矩阵为： 

$C_k={\mathrm{\α}}_k{\mathrm{\α}}_k^T$

其理论方差矩阵为： 

$S_k=H_k({\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1})H_k^T+R_k$

若实际方差矩阵与理论方差矩阵相等，则实际量测噪声与我们估计的量测噪声相等；若实际方差矩阵大于理论方差矩阵，则系统受到噪声干扰，实际量测噪声大于估计的量测噪声，需要增大估计噪声；同理，若实际方差矩阵小于理论方差矩阵，则需要减小估计噪声，实现自适应Kalman滤波。 

又实际方差矩阵和理论方差矩阵可分别表述为： 

$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]$

其中，其中C_A、S_A为加速度信息的量测方差和理论方差；C_AM、C_MA、S_AM、S_MA为加速度信息与地磁信息的量测协方差和理论协方差：C_M、S_M为地磁信息的量测方差和理论方差。为防止加速度计和磁传感器之间相互干扰，因此采用了两个模糊控制自适应模块(FLAS)，分别定义q_A＝tr(C_A)/tr(S_A)、q_M＝tr(C_M)/tr(S_M)作为两个模糊控制模块的输入，来监测加速度计和磁传感器的观测噪声，其中tr(·)为取矩阵的迹。 

本发明所述的模糊控制器根据输入，分别输出修正系数修正加速度的量测噪声和磁的量测噪声，实现实时估计量测噪声的作用。 

两个模糊控制模块具有相同的输入、输出隶属度函数.如图2所示。模糊控制的推理规则为：

根据输入隶属度函数，判断噪声变化趋势为，并得到隶属模糊度β： 

If q∈[0.5，0.7]，then‘噪声减小； 

If q∈[0.7，1.3]，then‘噪声不变’： 

If q∈[1.3，1.5]，then’噪声增大’； 

根据输出隶属度函数，可得到输出的权值为： 

If‘噪声减小’，then W＝0.3*β+1； 

If‘噪声不变’，then 

If’噪声增大’，then W＝1-0.3*β； 

从两个模糊控制模块分别得到修正权值W_A、W_M，对观测噪声进行实时修正： 

$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}$

经过模糊控制模块对量测噪声的实时估计，使得Kalman滤波可以更稳定、精度更高的估计系统误差.根据滤波得到的误差.对系统进行修正： 

$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$

将修正后的四元数Q_real转化为捷联矩阵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)$

通过捷联矩阵得到载体姿态角，并将其作为结果输出： 

$\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.$

图1为基于模糊控制自适应Kalman滤波的航姿参考系统图，图中所示：根据陀螺仪的输出进行四元数更新进而实现姿态更新，以加速度计和磁传感器的输出为参考，通过Kalman滤波对姿态角和航向角进行校正；模糊控制模块根据观测量实时估计观测噪声，从而实现了基于模糊控制的自适应Kalman滤波 

图2为模糊控制模块的输入、输出隶属度函数图，根据输入、输出隶属度函数图将输出模糊化后输出。 

Claims (3)

1.一种提高航姿参考系统解算精度和系统抗干扰能力的方法，所述的航姿参考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；其特征在于所述的方法具体包括以下步骤：

第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程；

第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程；

第三、以加速度计和磁传感器的输出作为参考，运用kalman滤波技术，实现系统的修正过程；

第四、结合模糊控制理论，在系统中加入模糊控制模块，实时估计系统的量测噪声，实现自适应kalman滤波。

2.根据权利要求1所述的提高航姿参考系统解算精度和系统抗干扰能力的方法，其特征在于所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数；

所述的系统更新过程中，四元数的更新采用龙格-库塔算法，根据更新得到的四元数，更新捷联矩阵和姿态角；

所述的系统修正过程中，四元数误差矢量和陀螺仪的零偏误差矢量作为状态向量、以加速度误差矢量和地磁误差矢量作为观测向量，确定kalman滤波的状态方程和观测方程，实时估计系统误差，并将误差反馈到系统中，修正四元数、捷联矩阵和姿态角；

所述的模糊控制模块中有两个模糊控制器，在kalman滤波过程中，分别实时计算残差向量中加速度部分的理论、实际方差和磁部分的理论、实际方差，分别以二者的理论方差与实际方差的比值作为两个模糊控制器的输入。

3.根据权利要求2所述的提高航姿参考系统解算精度和系统抗干扰能力的方法，其特征在于所述的模糊控制器根据输入，分别输出修正系数修正加速度的量测噪声和磁的量测噪声，实现实时估计量测噪声的作用。

Images