文献［２］给出姿态四元素满足跚：
ｇｏ
ｏ
～彬 附
ｇｏ
９１
‰
０ｏ
＝１／２
ｑ２
～ ‰‰
ｑ１ （２）
ｑ２
９３
一 ％
甜竹
９３
状态角可以由方向余旋矩阵ｃ：的成分计算
航向角Ｈ＝ａｒｃｔａｎｃｌ２／ｃ１１
（３）
纵摇角妒＝一ａｒｃｓｉｎｃ。，
（４）
横摇角臼＝ａＩ℃协ｎ％／ｃ。
（５）
姿态矩阵拜为正交矩阵，所以有曩＝（碍）～＝（冗）７。因 此，若知道砖的元素值，就可以求出ｃ：的元素值。
万方数据
捷联式航姿系统中四元素算法Kalman滤波器的实现研究
作者： 作者单位：
刊名： 英文刊名： 年，卷(期)： 被引用次数：
施闻明， 徐彬， 陈利敏， SHI Wen-ming， XU Bin， CHEN Li-min 施闻明,徐彬,SHI Wen-ming,XU Bin(青岛海军潜艇学院,山东,青岛,266071)， 陈利敏,CHEN Li-min(北舰航保处,山东,青岛,266071)
《自动化技术与应用》２００５年第２４卷第１１期
控韦Ｕ理论与ｌ立用
捷联式航姿系统中四元素算法 Ｋａｌｍａｎ滤波器的实现研究
施闻明１，徐彬１Biblioteka 陈利敏２（１．青岛海军潜艇学院，山东青岛２㈣１；２．北舰航保处，山东青岛２６６０７１）
摘要：本文基于四元素算法推导了姿态算法和捷联惯导系统误差模型，并设计了Ｋａｈａｎ滤波器。在此基础上分析了误差模型的 随机噪声补偿和提出了航向修正：仿真结果表明，本文讨论的这种ＫａＩⅡ砌滤波器能保证航向精度，具有实际应用意义：
其中，仃乙，ｄ乙，盯：为陀螺的白噪声漂移均方根值，ｄ乞，盯乙，
ｄ乞为陀螺的一阶马尔可夫漂移均方根值，仃乙，盯：，，盯乞为加速度
计白噪声均方根值，打为相关时间。
Ｓｔｅｐ Ｒｅｓｐｏｎｓｅ
㈨肌 ＾＾．＾。 一
ｙ ｙ Ｖ ｙ｝『’７ｌｙ
Ｓｔｅｐ Ｒｅｓｐｏｎｓｏ
／一 ——‘—、——Ｌ
，
｛
Ｔｉｍｅ（ｓｅｃ） 原函数
自动化技术与应用 TECHNIQUES OF AUTOMATION AND APPLICATIONS 2005，24(11) 2次
参考文献(4条) 1.张树侠 捷联式惯性导航系统 1992
2.韦锡华.杨晓东 基于最优控制的四元数据误差传播捷联矩阵算法分析[期刊论文]-中国惯性技术学报 2003(02)
２．２状态模型 真实姿态可以由四元素乘法的形式表示，如下
ｇ＝ｇ《多ｇｅ
小：卧㈩
㈤
带进方程ｃｓ，得［三］＝ｃ厶。，一２ｃ吼，，毪［兰］＋”ｃｔ，，即
ｃ《｝艿：６＝［圣］一爸：·［兰］，：６＝一爸：·［兰］，
最后，构造测量矢量ｚ＝［主！］，测量噪声矢量ｙｃ“，＝ 【釜ｌ‘女’］，和钡ｎ量矩阵，了ｃｔ。，：＝ｃ，，。ｃ ｚ；，，ｃ，ｔ。，，，其中Ｈ，ｃｔ。，＝＝ 嘲
厂ｇ：＋ｑ：一ｇ；一ｑ；
ｃ：＝ｌ ２（ｇ。ｇ：一ｇ。９３）
Ｌ ２（ｑｌ ９３＋ｇｏ ９２）
２（ｇｌ ９２＋ｇｏ ｑ３） ｇ：一９；＋ｇ；一ｇ； ２（９２ ９３一ｇｏ ｑ１）
们 啦 一 ％啦
吼 啦 ＋ 铷吼
１Ｉ Ｊ 纵纵：铀 一 ：吼 一 ：啦 ｝ ，，靠
ｑ
这里上标６和下标ｎ分别表示“载体坐标系”和“地理坐标系”。
２．１ 姿态角和方向余旋矩阵
文献［１］给出姿态矩阵的表达式…如下：
ｒ ｃｏｓ＾ｃｏｓ口＋ｓｉｎ日ｓｉｎ驴ｓｉｎ目 ｓｉｎＨｃｏｓ驴
死＝Ｉ ｃｏｓＨｓｉｎ驴ｓｉｎ目一ｓｉｎ日ｃｏｓ日ｃｏｓ妒ｃｏｓＨ
ｌ
一。０ｓ驴。ｉ。口
。ｉ。妒
ｃｏｓ日ｓｉｎ目一ｓｉｎＨｓｉｎ妒ｃｏｓ口］
—ｓｉｎ日ｓｉｎ日一ｃｏｓ日ｓｉｎ ５ｆｌｃｏｓ８
运算。
３误差模型的随机噪声补偿
将加速度计的过程噪声和陀螺的常值漂移等相关噪声都简 化为白噪声，这使得估计的实际误差增大，甚至会滤波发散。文
献［３］指出，可以采用加大噪声方差的方法来补偿模型误差。 取系统的白噪声矢量为：
形＝［训弘训盯
％ 训攒
训叮
加。
训麒
叫＂
叫ⅢＪ
对应地取白噪声方差Ｏ（￡）为：
ｄ嵫［叮：盯≥ｄ；２盯：／死２仃≥／巧２盯乙／砭ｄ乞 盯乙 ｄ乞］
［３］赖际舟，刘建业，林雪原，等．捷联航姿系统中的降阶滤波器的 实现研究［Ａ］．南京惯性技术学会第九次学术年会论文［ｃ］．南京，２００３．
１０．２４—２７
［４］普航仪器［Ｍ］．北京：海军司令部航海保证部，１９８５．７ｌ一８０
作者简介：施闻明（１９８２一），男。硕士研究生，专业方向为载运工具运用工程。
Ｔｉｍｅ（ｓｅｃ）
加Ｋａｌｍａｎ滤波器的函数
图２加Ｋａｌｌｌ，ａｎ滤波器前后系统幅频比较
６结束语
本文从计算速度和计算精度的角度出发，采用四元素法来 解算姿态矩阵和推导了Ｉ（ａｌＩＩｌａｎ滤波器的方程，并在此基础上对 误差模型的随机噪声补偿和航向修正进行了分析。仿真结果表
明，基于四元素算法的Ｉ（ａｌｒ咖滤波器具有较高的滤波性能。
２ ｋａｌｍａｎ滤波器的设计二
在捷联惯导系统中，三个陀螺互相垂直配置来测量运载体
收稿日期：２００５—０５一０８ ６ Ｔｅｃｈｎｉｑｕｅｓ ｏｆ ＡＩｌｌｏｍａｔ咖＆Ａｐｐ¨例ｉｏｎｓ
万方数据
的角速度矢量（ｐ，ｇ，ｒ）。同样，三个加速度计测量加速度矢量 （吼，ｏ，，吼）。然后三个姿态角和他们的角速度可以通过相关的 转换和硒ｌＩＩｌａｎ滤波器获得。
（１卜１）
Ｂ（后，七）＝Ｂ（知，七一１）＋△Ｂ（知，南）
（１６—２）
埘（１｜｝，｜｜｝）＝ｌｌＪ（｜ｊ｝，尼一１）＋曰（七／七）
（１卜３）
从加速度计的信号口６，可以通过相关变换和最小迭带法，
盛羞二！：卢 ［堕［］＿Ｊ塑叫
５仿真和测试结果
得到初始姿态四元素ｑ（ｏ／０）和它的估计误差ｇ。（ｏ／Ｏ） 方程（１１）一（１６）加上初始值便构成完整的ＫａｌＩＩｌａｎ滤波器
3.赖际舟.刘建业.林雪原 捷联航姿系统中的降阶滤波器的实现研究 2003 4.普航仪器 1985
相似文献(1条)
1.学位论文 张力 基于卡尔曼滤波器的捷联惯性航姿系统研究 2005
惯性导航作为一种自主的导航方法，在导航技术中占有突出的地位，捷联式惯性导航系统是惯性导航技术的发展方向。为适应现代作战飞机要求惯 性导航系统向小型化、数字化和综合化的方向发展，有必要对基于低成本的微型惯性测量单元组成的捷联式组合惯性导航系统做进一步的理论研究和工 程化样机的研发。 本文首先阐述了一般捷联式惯性导航系统的原理，在此基础上，介绍了这种先进的速度组合捷联式航姿系统的原理和构成。本文着重讨论了卡尔曼 (Kalman)滤波器在本项目中的工程应用：间接法闭环校正的线性卡尔曼滤波器结构的确定；选择适当的参数进行误差建模；给出速度组合的离散卡尔曼 滤波器的状态方程、量测方程及卡尔曼算法流程。通过理论分析和对多传感器组合的导航系统仿真结果说明，采用Kalman滤波器对各信号组合后，克服 了由低精度低成本IMU构成的纯捷联惯导系统导航参数发散很快的现象，导航参数的精度得到了很大的提高；引入速度再进行组合后，姿态精度也得到较 大改善，各项导航参数可以满足一般导航定位的要求。为此，确定了捷联式惯性航姿系统的最小化组成。 在相关单位的大力支持下，先期完成了捷联式惯性航姿系统工程样机的硬件制作、软件开发和软硬件的组装和调试；地面跑车试验结果也证明，这 种基于低成本、低精度微型惯性测量器件的采用卡尔曼滤波器实现最优组合的捷联式惯性航姿系统不仅具有性能稳定、可靠性高、成本低的优点，而且 输出了较高精度的航姿参数，是一种具有独特优点的先进实用的捷联式惯性导航系统。 本项目仅以IMU、磁传感器和导航计算机组成，是目前国内最小的捷联式惯性航姿系统；也是自主开发的捷联式惯导在国产歼击机上的首次应用。
这里ｉ是真实状态四元素，ｉ是姿态四元素的估计，ｉ是；的误
差。 利用方程（２）对方程（６）微分，并在线性估计后得到ｊ。＝一
叫吼一１／２（加一埘）这里估计误差（训一埘）为系统白噪声阢（ｆ）所 致。陀螺的随机误差综合考虑为白噪声漂移和随机一阶马尔可
夫过程。令陀螺偏差△Ｂ，它由随机偏差率曲＝％（￡）模型化。
扩展状态矢量ｘ＝㈦和状态噪蒯扯哦ｎ
扩展状态方程表示为 克＝Ｆ（ｔ）Ｘ（ｔ）＋形（ｔ）
（７）’
．这里６×６维矩阵Ｆｃｔ，＝［：二１一芝：３”］，｛形ｃ ｔ，｝是
独立的高斯白噪声，即均值为Ｅ｛形（￡）｝＿０，方差为哳｛形（￡）
旷（ｆ）｝＿Ｑ（￡）艿（￡一ｒ）。
？
２．３测量模型
讨论加速度计的测量方程。在捷联状态系统中，三个加速
Ａｎｄ ｔｈｅ Ｋａｌｌｌｌａｎ矗ｌｔｅｒ ｉｓ ｄｅｓｉｇｎｅｄ．Ｏｎ ｔｈｉｓ ｂａｓｅ，Ｔｈｅ ｅⅡｂｃｔ ０ｆ ｔｈｅ ｍｎｄｏｍ ｎｏｉｓｅ ｉｓ ａｎａｌｙｚｅｄ ａｒ】ｄ出ｅ ａｚｉｍｕｔｈ ｃｏｍｐｅｎｓａｔｉｏｎ ｉ５ ｐｒｏｐｏｓｅｄ．Ｔｈｅ ｓｉｍｍａｔｉｏｎ ｒｅｓｕｌｔ ｓｈｏｗｓ ｔｈａｔ ｔｈｉｓ Ｋａｌｍａｎ ６ｌｔｅｒ ｃａｎ ｅｎｓｕｒｅ ｔｈｅ ｒｅｑｕｉｒｅｄ ａＺｉｍｕｔｈ ｐｒｅｃｉｓｉｏｎ．
ｊ
。０ｓ妒。ｏｓ口
：弹斗…：瞄篡＝／驯；
Ｌ乃。 乃： 乃，Ｊ
【横摇角：臼＝ａｒｃｔａｎ（一乃，／乃，）
假设姿态四元素ｑ＝％＋ｇ。ｉ＋ｇ：Ｊ．＋９３＆，满足限制条件时
７＊ｉ＝１，称ｉ为单位四元素。
将地理坐标系利用四元素转换为载体坐标系时，其方向余
控带Ｕ理论与应用
婴！型塑塑型璺型娅巳！！型幽
《自动化技术与应用》２００５年第２４卷第”舅
△ｃ，。＝Ａ＋Ｂｓｉｎ￡，＋Ｃｃｏｓ￡，＋Ｄｓｉｎ（２毋≯＋Ｅｃｏｓ（２己，） 其中：Ａ——圆周罗差，由软铁磁场产生；