一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法与流程

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

背景技术：

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

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

技术实现要素：

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

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

(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)利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ。

优选的，步骤(3)中，根据四元数微分方程和姿态矩阵构建系统的状态方程，并得到系统过程噪声方差矩阵的具体过程如下：

四元数微分方程如下：

式中，qk表示k次采样时刻四元数，其中qk＝[qk1 qk2 qk3 qk0]^T，[qk1 qk2 qk3]^T是四元数qk的矢量部分，用表示，qk0是标量部分；表示qk的微分；wk表示k次采样时刻的陀螺仪采集的数据；其中Ω(wk)的定义如下：

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

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

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

根据四元数qk＝[qk1 qk2 qk3 qk0]^T与姿态矩阵(n系：“东-北-天”地理坐标系)的关系得到姿态矩阵如下：

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

Xk＝qk

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

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

Xk+1＝ΦkXk+VK

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

Φk＝exp(ΩkTs)

式中，Ωk为k次采样时刻的Ω(wk)；Ts为采样周期；

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

式中，为k次采样时刻实测的加速度ak＝[akx aky akz]^T与预测的加速度vk＝[vkxvky vkz]^T之间的旋转误差，akx、aky、akz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据，vkx、vky、vkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据；为k次采样时刻实测的磁感应强度mk＝[mkx mky mkz]^T与预测的磁感应强度hk＝[hkx hky hkz]^T之间的旋转误差，mkx、mky、mkz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据，hkx、hky、hkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据；加速度预测向量vk＝[vkx vky vkz]^T和磁感应强度预测向量hk＝[hkx hky hkz]^T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影，同时投影可以通过姿态转换得到；

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

Qk＝(Ts/2)²ΞkΣkΞk^T。

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

利用实测的加速度ak＝[akx aky akz]^T减去预测的加速度vk＝[vkx vky vkz]^T和实测的磁感应强度mk＝[mkx mky mkz]^T减去预测的磁感应强度hk＝[hkx hky hkz]^T得到误差函数∈(qk)，误差函数计算公式如下：

利用快速高斯-牛顿法求出满足的四元数的最优解，快速高斯-牛顿法公式如下：

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

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

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

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

优选的，步骤(5)中，根据建立的系统状态方程和系统观测模型建立如下的扩展卡尔曼滤波递推方程：

状态一步预测方程计算公式如下：

Xk,k-1＝ΦkXk-1

式中，Xk-1表示k-1次采样时刻状态矩阵；Xk,k-1表示k次采样时刻预测状态矩阵；

一步预测方差矩阵计算公式如下：

式中，为矩阵Φk的转置；Qk-1为k-1次采样时刻的系统过程噪声方差矩阵；Pk-1表示k-1次采样时刻的估计误差方差矩阵；Pk,k-1表示k次采样时刻的一步预测误差方差矩阵；

滤波增益矩阵计算公式如下：

式中，Rk表示k次采样时刻系统量测噪声方差矩阵；T表示转置；Hk为4阶单位矩阵，且为k次采样时刻观测矩阵；Kk表示k次采样时刻的增益矩阵；

状态估计计算公式如下：

式中，Zk表示k次采样时刻的由快速高斯牛顿法计算出的观测值；

估计误差方差矩阵计算公式如下：

Pk＝[I4×4-KkHk]Pk,k-1

式中，I4×4表示四阶单位矩阵；Pk表示k次采样时刻的估计误差方差矩阵。

优选的，步骤(6)中，利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ的具体过程如下：

根据方向余弦矩阵和四元数之间的关系可得：

γk＝arctan[-(qk1qk3-qk2qk0)/0.5-qk1²-qk2²)]

Ψk＝[(qk1qk2-qk3qk0)/0.5-qk1²-qk3²)]

式中，θk、γk、Ψk分别表示k次采样时刻的航向角、俯仰角和横滚角。

本发明的有益效果为：(1)本发明的一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，是一种四元数估计法，系统过程方程是线性模型，具有估计精度高的优点，同时计算不复杂；(2)本发明提出了使用快速高斯-牛顿法来建立系统观测模型，相对于其他的扩展卡尔曼滤波方法，它极大地简化了计算量，同时也有不错的效果；(3)本发明给出了过程噪声方差矩阵和测量噪声方差矩阵在实际过程中的计算方法，解决了现有参数计算不详等问题。

附图说明

图1为本发明的方法流程示意图。

图2为本发明采用的从AHRS设备记录的校准陀螺仪、加速度计和磁力计数据示意图。

图3为本发明通过姿态解算得到的欧拉角数据示意图。

具体实施方式

如图1所示，本发明的一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法，包括以下步骤：

步骤1：搭建姿态估计系统，获取载体固定参考系坐标系下多轴传感器数据；

步骤2：对采集的加速度数据和磁感应强度数据做滤波处理，并对这两个传感器采集的数据做归一化处理；

三轴陀螺仪采集的数据为w＝[wx wy wz]^T，归一化后的三轴加速度计采集的数据为a＝[ax ay az]^T，归一化后的三轴磁力计采集的数据为m＝[mx my mz]^T；

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

步骤4：利用快速高斯-牛顿法构建系统的观测模型，并得到系统的测量噪声方差矩阵；

步骤5：根据建立的系统状态方程和观测模型建立扩展卡尔曼滤波递推方程；

步骤6：利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ；

上述中，构建系统的状态方程，并求出系统的过程噪声方差矩阵的具体过程如下：

四元数微分方程如下：

式中，qk表示k次采样时刻四元数，其中qk＝[qk1 qk2 qk3 qk0]^T，[qk1 qk2 qk3]^T是四元数qk的矢量部分，用表示，qk0是标量部分；表示qk的微分；wk为k次采样时刻的陀螺仪采集的数据；其中Ω(wk)的定义如下：

式中，wk＝[wkx wky wkz]^T，其中wkx、wky、wkz分别为k次采样时刻陀螺仪采集的x轴、y轴和z轴方向上的角速度数据；T表示转置；

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

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

通过四元数qk＝[qk1 qk2 qk3 qk0]^T可以得到姿态矩阵n系(导航坐标系)：“东-北-天”地理坐标系；

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

Xk＝qk

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

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

Xk+1＝ΦkXk+VK

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

Φk＝exp(ΩkTs)

式中，Ωk为k次采样时刻的Ω(wk)；Ts为采样周期；

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

式中，为k次采样时刻实测的加速度向量ak＝[akx aky akz]^T与加速度预测向量vk＝[vkx vky vkz]^T之间的旋转误差，akx、aky、akz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴加速度数据，vkx、vky、vkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴加速度数据；为k次采样时刻实测的磁感应强度向量mk＝[mkxmky mkz]^T与磁感应强度预测向量hk＝[hkx hky hkz]^T之间的旋转误差，mkx、mky、mkz为k次采样时刻载体固定坐标系下实测的x轴、y轴和z轴采集到的三轴磁感应强度数据，hkx、hky、hkz为k次采样时刻载体固定坐标系下预测的x轴、y轴和z轴采集到的三轴磁感应强度数据；

加速度预测向量vk＝[vkx vky vkz]^T和磁感应强度预测向量hk＝[hkx hky hkz]^T分别为理想状态下导航坐标系中的地球重力矢量和地磁场强度矢量在载体坐标系中的投影，投影过程通过姿态转换得到，计算公式如下：

bkz＝dkz

式中，式中[dkx dky dkz]^T是磁力计在n系的输出；[bkx 0 bkz]^T是假设的实际地球磁场的大小和方向；是的转置；

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

Qk＝(Ts/2)²ΞkΣkΞk^T。

上述技术方案中，步骤4得到观测值并求出系统的测量噪声方差矩阵的具体过程如下：利用实测的加速度ak＝[akx aky akz]^T减去预测的加速度vk＝[vkx vky vkz]^T和实测的磁感应强度mk＝[mkx mky mkz]^T减去预测的磁感应强度hk＝[hkx hky hkz]^T得到误差函数∈(qk)，误差函数计算公式如下：

利用快速高斯-牛顿法求出满足的四元数的最优解，快速高斯-牛顿法公式如下：

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

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

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

式中，I3×3为3阶单位矩阵；

上述技术方案中，步骤5得到的扩展卡尔曼滤波递推方程的具体过程如下：状态一步预测方程计算公式如下：

Xk,k-1＝ΦkXk-1

式中，Xk-1表示k-1次采样时刻状态矩阵；Xk,k-1表示k次采样时刻预测状态矩阵；

一步预测方差矩阵计算公式如下：

式中，为矩阵Φk的转置，Qk-1为k-1次采样时刻的系统过程噪声方差矩阵；Pk-1表示k-1次采样时刻的估计误差方差矩阵；Pk,k-1表示k次采样时刻的一步预测误差方差矩阵；

滤波增益矩阵计算公式如下：

式中，Rk表示k次采样时刻系统量测噪声方差矩阵；T表示转置；Hk为4阶单位矩阵，且为k次采样时刻观测矩阵；Kk表示k次采样时刻的增益矩阵；

状态估计计算公式如下：

式中，Zk表示k次采样时刻的由快速高斯牛顿法计算出的观测值；

估计误差方差矩阵计算公式如下：

Pk＝[I4×4-KkHk]Pk,k-1

式中，I4×4表示四阶单位矩阵；Pk表示k次采样时刻的估计误差方差矩阵；

上述技术方案中，步骤6利用每次递推得到的最佳四元数解算载体的三个姿态角：航向角为Ψ，俯仰角为θ，横滚角为γ的具体过程如下：

根据方向余弦矩阵和四元数之间的关系可得：

γk＝arctan[-(qk1qk3-qk2qk0)/(0.5-qk1²-qk2²)]

Ψk＝[(qk1qk2-qk3qk0)/（0.5-qk1²-qk3²)]

式中，θk、γk、Ψk分别表示k次采样时刻的航向角、俯仰角和横滚角。

尽管本发明就优选实施方式进行了示意和描述，但本领域的技术人员应当理解，只要不超出本发明的权利要求所限定的范围，可以对本发明进行各种变化和修改。