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

摘要

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

主权项

1.一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，其特征在于，包括如下步骤：(1)搭建姿态估计系统，获取载体固定参考系坐标系下多轴传感器数据，即b系，坐标原点位于惯性测量组件中心的“右‑前‑上”直角坐标系，陀螺仪采集的三轴角速度数据，加速度计采集的三轴加速度数据，磁力计采集的三轴磁感应强度数据；(2)对采集的加速度数据和磁感应强度数据做滤波处理，并对这两个传感器采集的数据做归一化处理；三轴陀螺仪采集的数据为w＝[wx wy wz]T，其中wx、wy、wz分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴角速度数据；T表示转置；归一化后的三轴加速度计采集的数据为a＝[ax ay az]T，其中ax、ay、az分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴加速度数据；归一化后的三轴磁力计采集的数据为m＝[mx my mz]T，其中mx、my、mz分别表示在载体固定坐标系中x轴、y轴和z轴采集到的三轴磁感应强度数据；(3)根据四元数微分方程和姿态矩阵构建载体系统的状态方程，并得到系统的过程噪声方差矩阵；(4)利用快速高斯‑牛顿法构建系统的观测模型，并得到系统的测量噪声方差矩阵；(5)根据建立的系统状态方程和观测模型建立扩展卡尔曼滤波递推方程；(6)利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ。