专利名称：一种提高航姿参考系统解算精度和系统抗干扰能力的方法
技术领域：
本发明涉及的是ー种基于模糊控制自适应Kalman滤波算法来提高航姿參考系统解算精度和系统抗干扰能力的方法。
背景技术：
航姿參考系统(Attitudeand Heading Reference System, AHRS)主要由微机械陀螺仪、微机械加速度计和地磁传感器等器件组成，用来实时测量运动载体的姿态信息。航姿參考系统依靠惯性元件进行测量，无需向外发射或接受电磁波，可完全自主确定载体的姿态信息，因此其隐蔽性很好，在军事上有很广泛的应用，如无人飞机、导航与制导、航空航天等；同吋，由于航姿參考系统采用微机械器件，系统的集成度高、体积小巧、成本低，因此其在民用领域也有着广泛的应用，如车辆导航与控制、平台控制、船舶姿态控制、机器人
坐坐寸寸o在航姿參考系统中，载体的航向信息主要是依靠地磁传感器来确定，然而，在当今的社会生活中，电器、无线通信设备等电子产品的广泛应用，使得载体经常处于有电磁干扰的环境之中。航姿參考系统中的kalman滤波模块，要求对系统噪声和量测噪声先验已知，系统噪声可以经过对系统的多次测量而得到，然而，复杂的电磁环境使得量测噪声不确定，这将严重影响常规kalman滤波结果的精度，严重时甚至会导致系统滤波发散。因此，如何实时估计量测噪声，成为了提高系统抗干扰能力及解算精度的首要问题。J. Z. Sasiadek 在论又〈〈Sensor Fusion Based on Fuzzy Kalman FilteringforAutonomous Robot Vehicle》中提出了基于模糊控制的自适应kalman滤波算法，并将其成功的应用到了 GPS/INS组合导航系统中。目前，基于模糊控制的自适应kalman滤波算法已经广泛的应用于GPS/INS组合导航系统中，然而其在AHRS中的应用很少，QinWei 社论又《Fuzzy Adaptive Extended Kalman Filter for Miniature Attitude andHeadingReference System》中虽将该算法应用于航姿參考系统中，然而其采用扩展kalman滤波并同时对系统噪声和量测噪声同时进行实时调整，这无疑大大的増加系统的计算复杂度，严重影响了系统的实时性。

发明内容
本发明的目的在于克服现有技术存在的不足，而提供ー种以常规Kalman滤波为基础，结合模糊控制理论，实时估计量测噪声，实现自适应滤波，以提高系统的稳定性和可靠性的提高航姿參考系统解算精度和系统抗干扰能力的方法。本发明提出的方法以常规kalman滤波为基础，结合模糊控制理论，实时估计量测噪声，实现自适应滤波，提高系统的解算精度和抗干扰能力，所述的航姿參考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；所述的方法具体包括以下步骤第一根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程；
第二根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程；第三以加速度计和磁传感器的输出作为參考，运用kalman滤波技术，实现系统的修正过程；第四结合模糊控制理论，在系统中加入模糊控制模块，实时估计系统的量测噪声，实现自适应kalman滤波。本发明在所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传 感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数；所述的系统更新过程中，四元数的更新采用龙格-库塔算法，根据更新得到的四元数，更新捷联矩阵和姿态角；所述的系统修正过程中，四元数误差矢量和陀螺仪的零偏误差矢量作为状态向量、以加速度误差矢量和地磁误差矢量作为观测向量，确定kalman滤波的状态方程和观测方程，实时估计系统误差，并将误差反馈到系统中，修正四元数、捷联矩阵和姿态角；所述的模糊控制模块中有两个模糊控制器，在kalman滤波过程中，分别实时计算残差向量中加速度部分的理论、实际方差和磁部分的理论、实际方差，分别以二者的理论方差与实际方差的比值作为两个模糊控制器的输入。本发明所述的模糊控制器根据输入，分别输出修正系数修正加速度的量测噪声和磁的量测噪声，实现实时估计量测噪声的作用。本发明可以在提高航姿參考系统精度的同时，大大降低了系统的计算复杂度；尤其在系统受较大噪声干扰时，该算法能够正确估计量测噪声特性，改善姿态精度，提高系统的稳定性和可靠性。


图I是本发明所述的为基于模糊控制自适应kalman滤波的航姿參考系统图。图2是本发明所述的为模糊控制模块的输入、输出隶属度函数图。
具体实施例方式下面将结合附图对本发明作详细的介绍本发明是一种提高航姿參考系统解算精度和系统抗干扰能力的方法。本发明提出的方法以常规kalman滤波为基础，结合模糊控制理论，实时估计量测噪声，实现自适应滤波，提高系统的解算精度和抗干扰能力，所述的航姿參考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；所述的方法具体包括以下步骤第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程；第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程；第三、以加速度计和磁传感器的输出作为參考，运用kalman滤波技木，实现系统的修正过程；
第四、结合模糊控制理论，在系统中加入模糊控制模块，实时估计系统的量测噪声，实现自适应kalman滤波。本发明在所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数；由于航向角的改变，并不改变加速度计的输出，因此令航向角y = 0，则载体坐标系和导航坐标系下的加速度转换关系可表示为
权利要求
1.ー种提高航姿參考系统解算精度和系统抗干扰能力的方法，所述的航姿參考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；其特征在于所述的方法具体包括以下步骤 第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程； 第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程； 第三、以加速度计和磁传感器的输出作为參考，运用kalman滤波技木，实现系统的修正过程； 第四、结合模糊控制理论，在系统中加入模糊控制模块，实时估计系统的量测噪声，实现自适应kalman滤波。
2.根据权利要求I所述的提高航姿參考系统解算精度和系统抗干扰能力的方法，其特征在于所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数； 所述的系统更新过程中，四元数的更新采用龙格-库塔算法，根据更新得到的四元数，更新捷联矩阵和姿态角； 所述的系统修正过程中，四元数误差矢量和陀螺仪的零偏误差矢量作为状态向量、以加速度误差矢量和地磁误差矢量作为观测向量，确定kalman滤波的状态方程和观测方程，实时估计系统误差，并将误差反馈到系统中，修正四元数、捷联矩阵和姿态角； 所述的模糊控制模块中有两个模糊控制器，在kalman滤波过程中，分别实时计算残差向量中加速度部分的理论、实际方差和磁部分的理论、实际方差，分别以二者的理论方差与实际方差的比值作为两个模糊控制器的输入。
3.根据权利要求2所述的提高航姿參考系统解算精度和系统抗干扰能力的方法，其特征在于所述的模糊控制器根据输入，分别输出修正系数修正加速度的量测噪声和磁的量测噪声，实现实时估计量测噪声的作用。
全文摘要
一种提高航姿参考系统解算精度和系统抗干扰能力的方法，所述的航姿参考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；所述的方法具体包括以下步骤第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程；第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程；第三、以加速度计和磁传感器的输出作为参考，运用kalman滤波技术，实现系统的修正过程；第四、结合模糊控制理论，在系统中加入模糊控制模块，实时估计系统的量测噪声，实现自适应kalman滤波；本发明可以在提高航姿参考系统精度的同时，大大降低了系统的计算复杂度；尤其在系统受较大噪声干扰时，该算法能够正确估计量测噪声特性，改善姿态精度，提高系统的稳定性和可靠性。
文档编号G01C21/18GK102654404SQ20111005508
公开日2012年9月5日 申请日期2011年3月2日 优先权日2011年3月2日
发明者田易 申请人:浙江中科无线授时与定位研发中心