基于共轭梯度的EKF姿态估计算法

曾 聪，章 政,2，王 龙

(1.武汉科技大学 信息科学与工程学院， 湖北 武汉 430081；2.武汉科技大学 冶金自动化与检测技术教育部工程研究中心， 湖北 武汉 430081； 3.湖北汽车工业学院 电气与信息工程学院， 湖北 十堰 442000)

0 引 言

姿态解算是四旋翼飞行器[1-3]设计的核心环节，由于微机电系统(micro-electro mechanical systems，MEMS)具有成本低、体积小、功耗低等特点，它已成为了四旋翼姿态测量的首选器件[4]。但是实际的导航问题中，低成本的陀螺仪存在漂移，长时间运行漂移严重会产生累积误差，易导致解算姿态不稳定[5]，因此实现多传感器融合及设计稳定快速的滤波算法是获取飞行器精准的姿态信息的有效途径之一[6]。

文献[7]和文献[8]提出了一种基于互补滤波的姿态解算算法，该算法利用加速度计和磁力计来补偿陀螺仪漂移所引起的姿态偏差。文献[9]和文献[10]提出了一种基于梯度下降法的姿态融合算法，利用梯度下降法求解出目标函数误差方程姿态四元数与微分方程求解出的姿态四元数进行融合，得到最优姿态。由于互补滤波和梯度算法均没有对系统过程噪声和传感器的观测噪声进行分析，从理论上已证明Kalman滤波算法是不同导航器件的测量信息和系统状态信息融合的有效方法。文献[11]采用了基于四元数的卡尔曼(Kalman filter，KF)滤波器姿态估计算法，将过程噪声和观测噪声引入姿态估计系统，通过KF滤波器进行姿态融合，但是KF滤波器针对的是线性系统，而飞行器姿态估计系统一般是非线性系统。文献[12]提出了一种扩展卡尔曼滤波(extended Kalman filter，EKF)算法,该算法把非线性系统进行线性化，提高了姿态估计的精度，但是在观测方程线性化的同时引入了线性误差。……