多传感信息融合的改进扩展卡尔曼滤波定姿.PDFVIP

第３１卷第４期 计 算 机 应 用 研 究 Ｖｏｌ．３１Ｎｏ．４ ２０１４年４月　 ＡｐｐｌｉｃａｔｉｏｎＲｅｓｅａｒｃｈｏｆＣｏｍｐｕｔｅｒｓ Ａｐｒ．２０１４ 多传感信息融合的改进扩展卡尔曼滤波定姿 邹　波，张　华，姜　军 （西南科技大学特殊环境机器人技术四川省重点实验室，四川 绵阳６２１０１０） 摘　要：针对现有多传感器融合方法精度低、计算复杂等问题，提出了利用微惯性航姿系统的高精度传感器陀 螺仪、加速度计和磁力计组合确定姿态的算法。对采集到的原始数据进行误差补偿和滤波，降低传感器本身噪 声，采用四元素的四阶龙格库塔法求解陀螺仪的角度消除奇点问题。将陀螺仪数据作为预测数据，估计出卡尔 曼的过程协方差；加速度计和磁力计数据作为观测数据，结合陀螺仪的误差估计出测量噪声协方差，通过卡尔曼 滤波实现多传感信息融合，实现了准确的姿态定位。实验分析不同算法的误差，证明了算法的精确性和可靠性。 关键词：扩展卡尔曼滤波；陀螺仪；加速度计；磁力计；协方差；姿态角估计 中图分类号：ＴＰ２４２．６　　　文献标志码：Ａ　　　文章编号：１００１３６９５（２０１４）０４１０３５０４ ｄｏｉ：１０．３９６９／ｊ．ｉｓｓｎ．１００１３６９５．２０１４．０４．０１９ Ｍｕｌｔｉｓｅｎｓｏｒｉｎｆｏｒｍａｔｉｏｎｆｕｓｉｏｎ’ｓｉｍｐｒｏｖｅｄｅｘｔｅｎｄｅｄ Ｋａｌｍａｎｆｉｌｔｅｒａｔｔｉｔｕｄｅｄｅｔｅｒｍｉｎａｔｉｏｎ ＺＯＵＢｏ，ＺＨＡＮＧＨｕａ，ＪＩＡＮＧＪｕｎ （ＳｐｅｃｉａｌＥｎｖｉｒｏｎｍｅｎｔＲｏｂｏｔｉｃｓＫｅｙＬａｂｏｒａｔｏｒｉｙｏｆＳｉｃｈｕａｎＰｒｏｖｉｎｃｅ，ＳｏｕｔｈｗｅｓｔＵｎｉｖｅｒｓｉｔｙｏｆＳｃｉｅｎｃｅ＆Ｔｅｃｈｎｏｌｏｇｙ，ＭｉａｎｙａｎｇＳｉｃｈｕａｎ６２１０１０， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｉｎｖｉｅｗｏｆｔｈｅｅｘｉｓｔｉｎｇｍｕｌｔｉｓｅｎｓｏｒｆｕｓｉｏｎｍｅｔｈｏｄ’ｓｌｏｗｐｒｅｃｉｓｉｏｎａｎｄｃｏｍｐｕｔａｔｉｏｎａｌｃｏｍｐｌｅｘｉｔｙ，ｔｈｉｓｐａｐｅｒｓｔｕｄｉｅｄ ｕｓｉｎｇｍｉｃｒｏｉｎｅｒｔｉａｌＡＨＲＳｐｒｅｃｉｓｉｏｎｓｅｎｓｏｒｇｙｒｏｓｃｏｐｅ，ａｃｃｅｌｅｒｏｍｅｔｅｒａｎｄｍａｇｎｅｔｏｍｅｔｅｒｃｏｍｂｉｎｅｄａｔｔｉｔｕｄｅｄｅｔｅｒｍｉｎａｔｉｏｎａｌｇｏ ｒｉｔｈｍ．Ｅｒｒｏｒｃｏｍｐｅｎｓａｔｉｏｎａｎｄｆｉｌｔｅｒｉｎｇｔｈｅｒａｗｄａｔａ，ｉｔｒｅｄｕｃｅｄｔｈｅｎｏｉｓｅｏｆｔｈｅｓｅｎｓｏｒｉｔｓｅｌｆ，ｕｓｅｄｆｏｕｒｅｌｅｍｅｎｔｓｆｏｕｒｏｒｄｅｒ ＲｕｎｇｅＫｕｔｔａｍｅｔｈｏｄｔｏｓｏｌｖｅｔｈｅｇｙｒｏａｎｇｌｅ，ｅｌｉｍｉｎａｔｅｄｔｈｅｐｒｏｂｌｅｍｏｆｓｉｎｇｕｌａｒｉｔｉｅｓ．Ｔｈｅｇｙｒｏｓｃｏｐｅｄａｔａａｓｆｏｒｅｃａｓｔｄａｔａｔｏｅｓ ｔｉｍａｔｅｔｈｅＫａｌｍａｎｐｒｏｃｅｓｓｃｏｖａｒｉａｎｃｅＱ．Ａｃｃｅｌｅｒｏｍｅｔｅｒａｎｄｍａｇｎｅｔｏｍｅｔｅｒｄａｔａａｓｏｂｓｅｒｖｅｄｄａｔａ，ｃｏｍｂｉｎｅｄｇｙｒｏｅｒｒｏｒｅｓｔｉｍａｔｅ ｏｆｔｈｅｍｅａｓｕｒｅｍｅｎｔｎｏｉｓｅｃｏｖａｒｉａｎｃｅＲ，ｔｈｒｏｕｇｈＫａｌｍａｎｆｉｌｔｅｒｔｏｃｏｍｐｌｅｔｅｔｈｅｍｕｌｔｉｓｅｎｓｏｒｉｎｆｏｒｍａｔｉｏｎｆｕｓｉｏｎ，ａｃｈｉｅｖｅｄａｎａｃ ｃｕｒａｔｅａｔｔｉｔｕｄｅｄｅｔｅｒｍｉｎａｔｉｏｎ．Ｅｘｐｅｒｉｍｅｎｔａｌａｎａｌｙｓｉｓｏｆｅｒｒｏｒｓｏｆｄｉｆｆｅｒｅｎｔａｌｇｏｒｉｔｈｍｓ，ｐｒｏｖｅｔｈｅａｃｃｕｒａｃｙａｎｄｒｅｌｉａｂｉｌｉｔｙｏｆｔｈｅ ａｌｇｏｒｉｔｈｍ． Ｋｅｙｗｏｒｄｓ：ｅｘｔｅｎｄＫａｌｍａｎｆｉｌｔｅｒ；ｇｙｒｏｓｃｏｐｅ；ａｃｃｅｌｅｒｏｍｅｔｅｒ；ｍａｇｎｅｔｏｍｅｔｅｒ；ｃｏｖａｒｉａｎｃｅ；ａｔｔｉｔｕ