专利名称：一种提高航姿参考系统解算精度和系统抗干扰能力的方法
技术领域：
本发明涉及的是ー种基于�：刂谱允视alman滤波算法来提高航姿參考系统解算精度和系统抗干扰能力的方法。
背景技术：
航姿參考系统(Attitudeand Heading Reference System, AHRS)主要由微机械陀螺仪、微机械加速度计和地磁传感器等器件组成，用来实时测量运动载体的姿态信息。航姿參考系统依靠惯性元件进行测量，无需向外发射或接受电磁波，可完全自主确定载体的姿态信息，因此其隐蔽性很好，在军事上有很广泛的应用，如无人飞机、导航与制导、航空航天等；同吋，由于航姿參考系统采用微机械器件，系统的集成度高、体积小巧、成本低，因此其在民用领域也有着广泛的应用，如车辆导航与控制、平台控制、船舶姿态控制、机器人
坐坐寸寸o在航姿參考系统中，载体的航向信息主要是依靠地磁传感器来确定，然而，在当今的社会生活中，电器、无线通信设备等电子产品的广泛应用，使得载体经常处于有电磁干扰的环境之中。航姿參考系统中的kalman滤波�？椋蠖韵低吃肷土坎庠肷妊橐阎低吃肷梢跃韵低车亩啻尾饬慷玫剑欢丛拥牡绱呕肪呈沟昧坎庠肷蝗范ǎ饨现赜跋斐９鎘alman滤波结果的精度，严重时甚至会导致系统滤波发散。因此，如何实时估计量测噪声，成为了提高系统抗干扰能力及解算精度的首要问题。J. Z. Sasiadek 在论又〈〈Sensor Fusion Based on Fuzzy Kalman FilteringforAutonomous Robot Vehicle》中提出了基于�：刂频淖允视alman滤波算法，并将其成功的应用到了 GPS/INS组合导航系统中。目前，基于�：刂频淖允视alman滤波算法已经广泛的应用于GPS/INS组合导航系统中，然而其在AHRS中的应用很少，QinWei 社论又《Fuzzy Adaptive Extended Kalman Filter for Miniature Attitude andHeadingReference System》中虽将该算法应用于航姿參考系统中，然而其采用扩展kalman滤波并同时对系统噪声和量测噪声同时进行实时调整，这无疑大大的増加系统的计算复杂度，严重影响了系统的实时性。
发明内容
本发明的目的在于克服现有技术存在的不足，而提供ー种以常规Kalman滤波为基础，结合�：刂评砺郏凳惫兰屏坎庠肷迪肿允视β瞬ǎ蕴岣呦低车奈榷ㄐ院涂煽啃缘奶岣吆阶藚⒖枷低辰馑憔群拖低晨垢扇拍芰Φ姆椒�。本发明提出的方法以常规kalman滤波为基�。岷夏：刂评砺郏凳惫兰屏坎庠肷迪肿允视β瞬ǎ岣呦低车慕馑憔群涂垢扇拍芰Γ龅暮阶藚⒖枷低持饕晌⒒低勇菀恰⑽⒒导铀俣燃坪偷卮糯衅髯槌桑美词凳辈饬吭硕靥宓淖颂畔ⅲ凰龅姆椒ň咛灏ㄒ韵虏街璧谝桓菁铀俣燃坪痛糯衅鞯氖涑鼋邢低车某跏蓟瓿纱侄宰脊蹋�
第二根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程；第三以加速度计和磁传感器的输出作为參考，运用kalman滤波技术，实现系统的修正过程；第四结合�：刂评砺郏谙低持屑尤肽：刂颇？椋凳惫兰葡低车牧坎庠肷迪肿允视alman滤波。本发明在所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传 感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数；所述的系统更新过程中，四元数的更新采用龙格-库塔算法，根据更新得到的四元数，更新捷联矩阵和姿态角；所述的系统修正过程中，四元数误差矢量和陀螺仪的零偏误差矢量作为状态向量、以加速度误差矢量和地磁误差矢量作为观测向量，确定kalman滤波的状态方程和观测方程，实时估计系统误差，并将误差反馈到系统中，修正四元数、捷联矩阵和姿态角；所述的�：刂颇？橹杏辛礁瞿：刂破鳎趉alman滤波过程中，分别实时计算残差向量中加速度部分的理论、实际方差和磁部分的理论、实际方差，分别以二者的理论方差与实际方差的比值作为两个�：刂破鞯氖淙�。本发明所述的�：刂破鞲菔淙耄直鹗涑鲂拚凳拚铀俣鹊牧坎庠肷痛诺牧坎庠肷迪质凳惫兰屏坎庠肷淖饔�。本发明可以在提高航姿參考系统精度的同时，大大降低了系统的计算复杂度；尤其在系统受较大噪声干扰时，该算法能够正确估计量测噪声特性，改善姿态精度，提高系统的稳定性和可靠性。
图I是本发明所述的为基于�：刂谱允视alman滤波的航姿參考系统图。图2是本发明所述的为�：刂颇？榈氖淙�、输出隶属度函数图。
具体实施例方式下面将结合附图对本发明作详细的介绍本发明是一种提高航姿參考系统解算精度和系统抗干扰能力的方法。本发明提出的方法以常规kalman滤波为基�。岷夏：刂评砺郏凳惫兰屏坎庠肷迪肿允视β瞬ǎ岣呦低车慕馑憔群涂垢扇拍芰Γ龅暮阶藚⒖枷低持饕晌⒒低勇菀�、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；所述的方法具体包括以下步骤第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程；第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程；第三、以加速度计和磁传感器的输出作为參考，运用kalman滤波技木，实现系统的修正过程；
第四、结合模糊控制理论，在系统中加入模糊控制�？椋凳惫兰葡低车牧坎庠肷迪肿允视alman滤波。本发明在所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数；由于航向角的改变，并不改变加速度计的输出，因此令航向角y = 0，则载体坐标系和导航坐标系下的加速度转换关系可表示为
权利要求
1.ー种提高航姿參考系统解算精度和系统抗干扰能力的方法，所述的航姿參考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；其特征在于所述的方法具体包括以下步骤 第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程； 第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程； 第三、以加速度计和磁传感器的输出作为參考，运用kalman滤波技木，实现系统的修正过程； 第四、结合模糊控制理论，在系统中加入�：刂颇？椋凳惫兰葡低车牧坎庠肷迪肿允视alman滤波。
2.根据权利要求I所述的提高航姿參考系统解算精度和系统抗干扰能力的方法，其特征在于所述的粗对准过程中，根据加速度计的输出，确定系统的初始俯仰角和横滚角；结合磁传感器的输出，确定初始航向角。根据初始姿态角，确定初始捷联矩阵和初始四元数； 所述的系统更新过程中，四元数的更新采用龙格-库塔算法，根据更新得到的四元数，更新捷联矩阵和姿态角； 所述的系统修正过程中，四元数误差矢量和陀螺仪的零偏误差矢量作为状态向量、以加速度误差矢量和地磁误差矢量作为观测向量，确定kalman滤波的状态方程和观测方程，实时估计系统误差，并将误差反馈到系统中，修正四元数、捷联矩阵和姿态角； 所述的模糊控制�？橹杏辛礁瞿：刂破鳎趉alman滤波过程中，分别实时计算残差向量中加速度部分的理论、实际方差和磁部分的理论、实际方差，分别以二者的理论方差与实际方差的比值作为两个�：刂破鞯氖淙�。
3.根据权利要求2所述的提高航姿參考系统解算精度和系统抗干扰能力的方法，其特征在于所述的�：刂破鞲菔淙耄直鹗涑鲂拚凳拚铀俣鹊牧坎庠肷痛诺牧坎庠肷迪质凳惫兰屏坎庠肷淖饔�。
全文摘要
一种提高航姿参考系统解算精度和系统抗干扰能力的方法，所述的航姿参考系统主要由微机械陀螺仪、微机械加速度计和地磁传感器组成，用来实时测量运动载体的姿态信息；所述的方法具体包括以下步骤第一、根据加速度计和磁传感器的输出进行系统的初始化，完成粗对准过程；第二、根据陀螺仪的输出，确定四元数的更新方程，实时计算更新系统的四元数，实现系统的更新过程；第三、以加速度计和磁传感器的输出作为参考，运用kalman滤波技术，实现系统的修正过程；第四、结合�：刂评砺郏谙低持屑尤肽：刂颇？椋凳惫兰葡低车牧坎庠肷迪肿允视alman滤波；本发明可以在提高航姿参考系统精度的同时，大大降低了系统的计算复杂度；尤其在系统受较大噪声干扰时，该算法能够正确估计量测噪声特性，改善姿态精度，提高系统的稳定性和可靠性。
文档编号G01C21/18GK102654404SQ20111005508
公开日2012年9月5日 申请日期2011年3月2日 优先权日2011年3月2日
发明者田易 申请人:浙江中科无线授时与定位研发中心
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
