[发明专利]一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法

说明书

本发明公开了一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，包括如下步骤：获取载体固定坐标系下多传感器数据；对加速度计和磁力计采集的数据进行滤波，并对这两个传感器采集的数据做归一化处理；根据四元数微分方程和姿态矩阵构建载体系统的状态方程，并确定系统的过程噪声方差矩阵；利用快速高斯‑牛顿法构建系统观测模型，并确定系统量测噪声方差矩阵；根据建立的系统状态方程和观测模型建立卡尔曼滤波递推方程；利用递推得到的最佳四元数解算出载体的三个姿态角。本发明能够极大简化计算量，解决了现有参数计算不详的问题。

技术领域

本发明涉及运动载体导航的多传感器数据融合技术领域，尤其是一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法。

背景技术

在人体运动跟踪、飞行器导航等运动载体导航的多传感器数据融合领域，对载体的姿态进行精准的实时估计有着广泛的应用。低成本的微机电系统的迅速发展使得更小更便宜的惯性传感器得到了广泛的应用。但是低成本的惯性测量单元测得的数据容易受到高频噪声和时变的偏置的影响，所以传感器数据融合算法中需要进行平滑处理以及无偏置估计。为了获得更精准的姿态，常常将加速度计和磁力计的数据与陀螺仪输出的角速度数据融合。而最常用的非线性姿态估计方法所使用的技术就是互补滤波和扩展卡尔曼滤波。

互补滤波方法中的互补滤波系数在动态系统中需要实时修正，但是如何修正并没有具体的模型方程，常使用的方法是采用自适应调参。黄鹤等人提出的“一种基于自适应互补融合的无人机姿态控制系统及方法”提出了使用梯度下降法来获得第二姿态角的方法，但是不同的传感器的使用会导致自适应调参的方法不同，参数的估计会在实际过程中会造成很大的麻烦，甚至导致姿态估计不精确。李国朋等人提出的“一种基于互补卡尔曼滤波算法计算融合姿态角度的方法”提出了互补滤波与卡尔曼滤波结合的方法，但是建立的系统模型只适用于单轴系统，不适合实际的三轴陀螺仪和三轴加速度计系统。陈洋等人提出的“共轭梯度与扩展卡尔曼滤波结合的四旋翼解算方法”提出了使用共轭梯度方法建立扩展卡尔曼滤波观测模型，但是系统状态方程建立不够精确，没有给出过程噪声方差矩阵和测量噪声方差矩阵在实际过程中的计算方法。

发明内容

本发明所要解决的技术问题在于，提供一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，能够极大简化计算量，解决了现有参数计算不详的问题。

为解决上述技术问题，本发明提供一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，包括如下步骤：

(1)搭建姿态估计系统，获取载体固定参考系坐标系(即b系，坐标原点位于惯性测量组件中心的“右-前-上”直角坐标系)下多轴传感器数据，陀螺仪采集的三轴角速度数据，加速度计采集的三轴加速度数据，磁力计采集的三轴磁感应强度数据；

(2)对采集的加速度数据和磁感应强度数据做滤波处理，并对这两个传感器采集的数据做归一化处理；三轴陀螺仪采集的数据为w＝[w_x w_y w_z]^T，其中w_x、w_y、w_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴角速度数据；T表示转置；归一化后的三轴加速度计采集的数据为a＝[a_x a_y a_z]^T，其中a_x、a_y、a_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴加速度数据；归一化后的三轴磁力计采集的数据为m＝[m_x m_y m_z]^T，其中m_x、m_y、m_z分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴磁感应强度数据；

(3)根据四元数微分方程和姿态矩阵构建载体系统的状态方程，并得到系统的过程噪声方差矩阵；