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