６ ２
传感器与微系统 （ Ｔ ｒ ａ ｎ ｓ ｄ ｕ ｃ ｅ ｒ ａ ｎ ｄ Ｍｉ ｃ ｒ ｏ ｓ ｙ ｓ ｔ ｅ ｍ Ｔ ｅ ｃ ｈ ｎ ｏ ｌ ｏ ｇ ｉ ｅ ｓ ）
２ ０ １ ７年 第 ３ ６卷 第 ３期
ＤＯＩ ： １ ０ ． １ ３ ８ ７ ３ ／ Ｊ ． １ ０ ０ ０ － ９ ７ ８ ７ （ ２ ０ １ ７ ） ０ ３ － － ０ ０ ６ ２ － － ０ ４
Ｂ ａ ｏ ｄ ｉ ｎ ｇ ０ ７ １ ０ ０ ３ ， Ｃ ｈ ｉ ｎ ａ ）
Ａｂ ｓ ｔ ｒ ａ ｃ ｔ ：Ａ ｉ ｍｉ ｎ ｇ ａ ｔ ｐ ｒ ｏ ｂ ｌ ｅ ｍ ｏ ｆ ｈ ｉ ｇ ｈ ｎ ｏ ｉ ｓ ｅ ， ｌ ｏ ｗ ｐ ｒ ｅ ｃ ｉ ｓ ｉ ｏ ｎ ｏ ｆ ｉ ｎ ｅ ｒ ｔ ｉ ａ ｌ ｍ ｅ ａ ｓ ｕ ｒ ｅ ｍｅ ｎ ｔ ｕ ｎ ｉ ｔ （ Ｉ ＭＵ） ａ ｎ ｄ ｌ ｏ ｗ ｐ ｒ ｅ ｃ ｉ ｓ ｉ ｏ ｎ ｏ ｆ
出了一种互补滤波和卡尔曼滤波相结合的融合算法 。该算法基于姿态角微分方程建 立系统的状态 方程 模 型， 利用互 补滤 波后 的姿态角作为 系统 的观测量 ， 再应用扩展 卡尔曼滤波 （ Ｅ Ｋ Ｆ ） 算法融合 了陀螺 仪 、 加 速
度计 和电子罗盘 的测量数据 。为验证该算法有效性 ， 用带有传感器 的开发板依次进行静态和动态测试 ， 实 验结果表 明： 结合 了互补 滤波 和卡尔曼滤波 的融合算 法 ， 在静 态时能够抑 制姿态角漂移 和滤 出噪声 ， 在 动 态时能够快速跟踪姿态 的变化 ， 提高了姿态角的解算精度 。 关键词 ：卡尔曼滤波 ； 互补 滤波 ； 姿态估计 ； 数据融合 ；惯性测量单元
ｐ ｒ ｏ ｐ ｏ ｓ ｅ ｄ ． Ｔ ｈ ｅ ａ ｌ ｇ ｏ ｉ ｒ ｔ ｈ ｍ ｅ ｓ ｔ ａ ｂ ｌ ｉ ｓ ｈ ｔ ｈ ｅ ｓ ｔ ａ ｔ ｅ ｅ ｑ ｕ ａ ｔ ｉ ｏ ｎ ｍｏ ｄ ｅ ｌ ｂ ａ ｓ ｅ ｄ ｏ ｎ ｄ ｉ ｆ ｆ ｅ ｒ ｅ ｎ ｔ ｉ ａ ｌ ｅ ｑ ｕ ａ ｔ ｉ ｏ ｎ ｏ ｆ ａ ｔ ｔ ｉ ｔ ｕ ｄ ｅ ａ ｎ ｇ ｌ ｅ ａ ｎ ｄ
ｃ ｌ ａ ｓ ｓ ｉ ｃ ａ ｌ ａ ｔ ｔ ｉ ｔ ｕ ｄ ｅ ｓ ｏ ｌ ｕ ｔ ｉ ｏ ｎ ａ ｌｇ ｏ ｒ ｉ ｔ ｈ ｍ， ａ ｆ ｕ ｓ ｅ ｄ ａ ｌ ｇ ｏ ｒ ｉ ｔ ｈ ｍ ｗｉ ｔ ｈ ｃ ｏ ｍｐ ｌ ｅ ｍｅ ｎ ｔ ａ ￣ ｆ ｉ ｌ ｔ ｅ ｒ ｉ ｎ ｇ ａ ｎ ｄ Ｋａ ｌ ｍａ ｎ ｆ ｉ ｌ ｔ ｅ ｉｎ ｒ ｇ ｉ ｓ
互 补 滤 波和 卡 尔 曼 滤 波 的 融合 姿 态 解 算 方 法
张 栋， 焦 嵩鸣 ，刘延 泉
（ 华 北 电 力 大 学 自动化 系 ， 河北 保定 测量单元 （ Ｉ ＭＵ） 噪声大 、 精度低 的缺点和常规 的姿态解算算 法精度不 高等问题 ， 提
ｗ ｉ ｔ ｈ ｉ ｎ ｅ ｒ ｔ ｉ ａ ｌ ｓ ｅ ｌ ｆ ｓ ｏ Ｙ ｓ ｔ ｏ ｔ ｅ ｓ ｔ ｉ ｎ ｓ ｔ ａ ｔ ｉ ｃ ａ ｎ ｄ ｄ ｙ ｎ ａ ｍｉ ｃ ｃ ｏ ｎ ｄ ｉ ｔ ｉ ｏ ｎ ． Ｔ ｈ ｅ ｒ ｅ ｓ ｕ ｌ ｔ ｓ ｏ ｆ ｅ ｘ ｐ ｅ ｉｍｅ ｒ ｎ ｔ ｓ ｓ ｈ ｏ ｗ ｔ ｈ ａ ｔ ｔ ｈ ｅ ａ ｌ ｇ ｏ ｉ ｒ ｔ ｈ ｍ ｆ ｕ ｓ ｅ ｄ ｗ ｉ ｔ ｈ ｃ ｏ ｍｐ ｌ ｅ ｍｅ ｎ ｔ ａ ｒ ｙ ｉ ｆ ｌ ｔ ｅ ｉｎ ｒ ｇ ａ ｎ ｄ Ｋａ ｌ ｍａ ｎ ｉ ｆ ｌ ｔ ｅ ｉ ｒ ｎ ｇ ｃ ａ ｎ ｃ ｏ ｎ ｓ ｔ ｒ ａ ｉ ｎ ｔ ｄ ｒ ｉ ｔ ｆ ａ ｎ ｄ ｎ ｏ ｉ ｓ ｅ ｏ ｆ ａ ｔ ｔ ｉ ｔ ｕ ｄ ｅ ａ ｎ ｇ ｌ ｅ ｉ ｎ ｓ ｔ ａ ｔ ｉ ｃ
ｄ ａ ｔ ａ ｏ ｆ ｇ ｙ ｏ， ｒ ａ ｃ ｃ ｅ ｌ ｅ ｏｍｅ ｒ ｔ ｅ ｒ ａ ｎ ｄ ｅ ｌ ｅ ｃ ｔ ｒ ｏ ｎ ｉ ｃ ｃ ｏ ｍｐ ａ ｓ ｓ ． Ｔ ｏ ｖ ｅ ｉ ｒ ｆ ｙ ｔ ｈ ａ ｔ ｔ ｈ ｅ ａ ｌ ｇ ｏ ｒ ｉ ｔ ｈ ｍ ｉ ｓ ｅ ｆ ｆ ｅ ｃ ｔ ｉ ｖ ｅ ，ｕ ｓ ｅ ｄ ｅ ｖ ｅ ｌ ｏ ｐ ｍｅ ｎ ｔ ｂ ｏ ａ ｒ ｄ
中图分类号 ：Ｖ２ ４ ９
文献标识码 ：Ａ
文章编 号 ：１ ０ ０ ０ － － ９ ７ ８ ７ （ ２ ０ １ ７ ） Ｏ ３ － － ０ ０ ６ ２ － － ０ ４
Ｆｕ ｓ ｅ ｄ ａ ｔ ｔ ｉ ｔ ｕ ｄ ｅ ｅ ｓ ｔ ｉ ｍａ ｔ ｉ ｏ ｎ ａ ｌ ｇ ｏ ｒ ｉ ｔ ｈ ｍ ｂａ ｓ ｅ ｄ ｏ ｎ ｃ ｏ ｍｐ ｌ ｅ ｍｅ ｎ ｔ ａ ｒ ｙ
ｉｌ ｆ ｔ ｅ ｒ ｉ ｎ ｇ ａ ｎ ｄ Ｋａ ｌ ｍａ ｎ ｉ ｆ ｌ ｔ ｅ ｒ ｉ ｎ ｇ
Ｚ ＨＡＮ Ｇ Ｄｏ ｎ ｇ，Ｊ Ｉ ＡＯ Ｓ ｏ ｎ ｇ — ｍｉ ｎ ｇ ，Ｌ Ｉ Ｕ Ｙａ ｎ — ｑ ｕ ａ ｎ
（ Ｄ ｅ ｐ ａ ｒ ｔ ｍｅ ｎ ｔ ｏ ｆ Ａ ｕ ｔ ｏ ｍａ ｔ ｉ ｏ ｎ ， Ｎｏ ｒ ｔ ｈ Ｃ ｈ ｉ ｎ ａ Ｅ ｌ ｅ ｃ ｔ ｒ ｉ ｃ Ｐ ｏ ｗ ｅ ｒ Ｕ ｎ ｉ ｖ ｅ ｒ ｓ ｉ ｔ ｙ ，
ｃ ｈ ｏ ￣ｅ ａ ｔ ｔ ｉ ｔ ｕ ｄ ｅ ａ ｎ ｇ ｌ ｅ ａ ｆ ｔ ｅ ｒ ｃ ｏ ｍｐ ｅ ｎ ｓ ａ ｔ ｉ ｏ ｎ ｉ ｆ ｌ ｔ ｅ ｉ ｒ ｎ ｇ ａ ｓ ｔ ｈ ｅ ｏ ｂ ｓ ｅ ｒ ｖ ａ ｔ ｉ ｏ ｎ ｏ ｆ ｓ ｙ ｓ ｔ ｅ ｍ． Ｕｓ ｅ Ｅ ＫＦ ｌｇ ａ ｏ ｒ ｉ ｔ ｈ ｍ ｆ ｕ ｓ ｅ ｄ ｍｅ ａ ｓ ｕ ｒ ｅ ｄ