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

权利要求书

1.一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，其特征在于，包括如下步骤：

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

(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)根据四元数微分方程和姿态矩阵构建载体系统的状态方程，并得到系统的过程噪声方差矩阵；具体过程如下：

四元数微分方程如下：

式中，q_k表示k次采样时刻四元数，其中q_k＝[q_k1 q_k2 q_k3 q_k0]^T，[q_k1 q_k2 q_k3]^T是四元数q_k的矢量部分，用表示，q_k0是标量部分；表示q_k的微分；w_k表示k次采样时刻的陀螺仪采集的数据；其中Ω(w_k)的定义如下：

式中，w_k＝[w_kx w_ky w_kz]^T，其中w_kx、w_ky、w_kz分别为k次采样时刻陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据；T表示转置；

根据四元数微分方程得到的离散时间模型为：

式中，q_k+1表示k+1次采样时刻四元数；Ω_k是k次采样时刻的Ω(w_k)；q_k为k次采样时刻的四元数，q(0)为初始条件得到的四元数；T_s为采样周期；

根据四元数q_k＝[q_k1 q_k2 q_k3 q_k0]^T与姿态矩阵的关系得到姿态矩阵如下：

以姿态四元数作为系统的状态，即：

X_k＝q_k

式中，X_k表示k次采样时刻状态矩阵；q_k表示k次采样时刻四元数；

建立的系统的状态方程如下式：

X_k+1＝Φ_kX_k+V_K

式中，X_k+1表示k+1次采样时刻状态矩阵；Φ_k表示k次采样时刻状态转移矩阵；X_k表示k次采样时刻四元数；V_K为k次采样时刻过程噪声阵；

Φ_k＝exp(Ω_kT_s)

式中，Ω_k为k次采样时刻的Ω(w_k)；T_s为采样周期；

式中，为高斯白噪声，其协方差矩阵为∑_k＝||e_k||I，I为3阶单位矩阵；Ξ_k表示k次采样时刻的误差系数矩阵；

式中，为k次采样时刻实测的加速度a_k＝[a_kx a_ky a_kz]^T与预测的加速度v_k＝[v_kx v_kyv_kz]^T之间的旋转误差，a_kx、a_ky、a_kz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据，v_kx、v_ky、v_kz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据；为k次采样时刻实测的磁感应强度m_k＝[m_kx m_ky m_kz]^T与预测的磁感应强度h_k＝[h_kx h_ky h_kz]^T之间的旋转误差，m_kx、m_ky、m_kz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据，h_kx、h_ky、h_kz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据；加速度预测向量v_k＝[v_kxv_ky v_kz]^T和磁感应强度预测向量h_k＝[h_kx h_ky h_kz]^T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影，同时投影通过姿态转换得到；

系统过程噪声方差矩阵计算公式如下：

Q_k＝(T_s/2)²Ξ_k∑_kΞ_k^T；

(4)利用快速高斯-牛顿法构建系统的观测模型，并得到系统的测量噪声方差矩阵；具体过程如下：

利用实测的加速度a_k＝[a_kx a_ky a_kz]^T减去预测的加速度v_k＝[v_kx v_ky v_kz]^T和实测的磁感应强度m_k＝[m_kx m_ky m_kz]^T减去预测的磁感应强度h_k＝[h_kx h_ky h_kz]^T得到误差函数∈(q_k)，误差函数计算公式如下：

利用快速高斯-牛顿法求出满足的四元数的最优解，[b_kx 0 b_kz]^T是假设的实际地球磁场的大小和方向，快速高斯-牛顿法公式如下：

式中，表示k+1次采样时刻的最优四元数解；为k次采样时刻的一步预测四元数值；λ_k在每次迭代过程中都会改变为最优值；J(0)为对应的雅可比矩阵，雅可比矩阵J(k)计算公式如下：

选取快速高斯-牛顿法求出的最优解作为观测值，即：

令得到系统测量噪声方差矩阵如下：

式中，I_3×3为3阶单位矩阵。