CN117029810A - 一种基于迭代EKF的Mahony姿态测量方法 - Google Patents

Abstract

本发明公开了一种基于迭代EKF的Mahony姿态测量方法，该方法首先获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息；根据采集数据对加速度信息归一化处理；再通过四元数得到理论重力加速度向量；步骤4：求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差；然后对陀螺偏差修正，进行四元数姿态更新；再选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果。然后对滤波方程进行更新；最后将四元数转换为欧拉角，输出姿态结果。本方法能够提高姿态测量系统的精度。

Description

一种基于迭代EKF的Mahony姿态测量方法

技术领域

本发明属于六轴MEMS(微机电系统)姿态测量技术，具体涉及一种基于迭代EKF(扩展卡尔曼滤波)的Mahony姿态测量方法。

背景技术

对于六轴数据，计算角度有两种方法，一种是通过对角速度积分得到角度，另一种则是通过对加速度进行正交分解得到角度。但这两种方式均存在不足，通过角速度积分得到角度时，角速度的误差会在积分过程中被不断放大从而影响数据准确性。而加速度计是一种特别敏感的传感器，电机旋转产生的震动会给加速度计的数据中带来高频噪声。不难看出，第一种方法测得的数据中存在结果偏差，而第二种方法测得的数据中存在高频噪声。因此可通过融合两种数据以获得准确姿态，这里通过加速度计补偿角速度。

加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。

Mahony互补滤波算法的基本原理是：通过加速度计测量值和采用陀螺积分后的姿态推算出来的重力加速度值之间的误差来对陀螺测量值进行校正，进而保证陀螺数据的准确性。然后，采用一阶龙格—库塔法解四元数微分方程来获取载体姿态。

发明内容

本发明的技术解决问题是：针对Mahony互补滤波算法对传感器数据优化不足以及非线性问题引起的缺陷，提出一种基于迭代EKF的Mahony姿态测量方法，利用迭代EKF算法进行加速度计的姿态信息和惯性的姿态进行最优融合，提高姿态测量系统的精度。

为达到上述目的，本发明的技术方案如下：

一种基于迭代EKF的Mahony姿态测量方法，该方法包括如下步骤：

步骤1：获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息；

步骤2：根据步骤1的采集数据对加速度信息归一化处理；

步骤3：通过四元数得到理论重力加速度向量；

步骤4：求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差；

步骤5：陀螺偏差修正，进行四元数姿态更新；

步骤7：选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果；

步骤8：对滤波方程进行更新；

步骤9：将四元数转换为欧拉角，输出姿态结果。

进一步地，步骤1所述获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息的具体方法是：

在初始时刻用四元数表示姿态角为：

Q＝[q₀q₁q₂q₃]

其中，q₀、q₁、q₂、q₃表示四元数；

通过姿态转移矩阵与四元数之间的转换关系可得：

其中，表示姿态转移矩阵；

记录MEMS陀螺和加速度输出为：

其中，表示陀螺输出，w_x、w_y、w_z分别表示陀螺仪x,y,z三个轴的输出信息，表示加速度输出，a_x、a_y、a_z分别表示加速度计x,y,z三个轴的输出信息。

进一步地，步骤2所述加速度信息归一化处理公式如下：

其中，a′_x、a′_y、a′_z分别表示归一化处理后的x,y,z三个轴的加速度信息。

进一步地，步骤3所述通过四元数得到理论重力加速度向量的具体方法是：

假设导航坐标系下的重力加速度为gⁿ＝[001]^T，通过姿态转移矩阵把导航系下的加速度gⁿ转换到载体坐标系下的加速度g^b：

其中，v_x、v_y、v_z分别表示载体x,y,z三个方向的速度信息。

进一步地，步骤4所述求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差的具体方法如下：

假设MEMS加速度计测量得到的载体系下的三轴加速度信息表示为：

此时，载体系下理论重力加速度向量g^b和MEMS测量载体系下的加速度矢量之间的叉乘Δθ可表示为：

在载体坐标系上，MEMS加速度计测出来的重力向量是[a_x a_y a_z]^T，陀螺积分后的姿态来推算出的重力向量是[v_x v_y v_z]^T，它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差[e_x e_y e_z]^T。

进一步地，步骤5所述陀螺偏差修正的具体方法是：

得到向量偏差后，即可通过构建PI补偿器来计算角速度补偿值Gyro_error：

Gyro_error＝K_p·Δθ+K_i·∫Δθ

其中，比例项用于控制传感器的“可信度”，积分项用于消除静态误差，比例项系数K_p越大，意味着通过加速度计得到误差后补偿越显著，即是越信任加速度计；反之比例项系数K_p越小时，加速度计对陀螺仪的补偿作用越弱，也就越信任陀螺仪；而积分项则用于消除角速度测量值中的有偏噪声，故对于经过零篇矫正的角速度测量值，一般选取很小的积分项系数K_i；最后将补偿值补偿给角速度测量值，带入四元数差分方程中即可更新当前四元数；

经过误差修正后的陀螺信息可表示为：

根据一阶龙格库塔法(毕卡法)解四元数微分方程，更新四元数：

其中，t表示时间，Δt表示时间间隔，即数据更新周期，分别表示载体坐标系下估计得到的x,y,z三个轴的陀螺信息。

进一步地，步骤6所述四元数归一化处理的具体方法是：

其中，q′₀、q′₁、q′₂、q′₃分别表示经过归一化处理后的四元数。

进一步地，步骤7所述选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果的具体方法是：

选取姿态四元数作为滤波模型的状态变量X＝[q₀ q₁ q₂ q₃]^T，

根据陀螺输出计算四元数为：

由于陀螺测量包含噪声，表示为：

其中，表示陀螺的真实输出，δ_x、δ_y、δ_z表示随机噪声，系统状态方程可表示为：

建立滤波状态方程：

其中，W(t)为状态噪声，F(t)为状态转移矩阵，G(t)为状态噪声系数矩阵，X(t)为状态向量，为状态向量估计值；

将状态方程离散化：

X_k+1＝Φ_k+1/kX_k+G_kW_k

其中，Φ_k+1/k为状态转移矩阵，G_k为状态噪声系数矩阵，X_k+1为k+1时刻的状态向量，X_k为k时刻的状态向量，W_k为k时刻的系统噪声；

选取步骤6Mahony算法输出的姿态四元数作为量测量，建立量测方程：

Z_k+1＝H_k+1X_k+1+V_k+1

其中，H_k+1＝I_4×4为量测矩阵；V_k+1为量测噪声，其协方差矩阵为R_k，Z_k+1为k+1时刻的量测信息。

进一步地，步骤8所述对滤波方程进行更新，具体方法如下：

时间更新如下：

X_k+1|k＝Φ_k+1/kX_k

量测更新如下：

X_k+1＝X_k+1|k+K_k+1(Z_k+1-H_k+1X_k+1|k)

P_k+1＝P_k+1|k-K_k+1H_k+1P_k+1|k

定义迭代截止条件：

||K_k+1(Z_k+1-H_k+1X_k+1|k)||<ε

其中，X_k+1|k为k时刻相对于k+1时刻的状态向量，P_k+1|k为k时刻相对于k+1时刻的协方差矩阵，P_k为k时刻的协方差矩阵，为k+i-1时刻的状态转移向量，Q_k为k时刻的系统噪声矩阵，K_k+1为k+1时刻的增益矩阵，/>为k时刻相对于k+1时刻状态转移向量的转置，H_k+1为k+1时刻的量测转移矩阵，P_k+1为k+1时刻的协方差矩阵，ε为迭代门限值，此处取0.01；

若迭代条件满足，退出迭代；若迭代条件不满足，则继续量测更新。

进一步地，步骤9所述将步骤8的迭代后的最优四元数转换为欧拉角，输出姿态结果：

其中，θ、γ、分别表示俯仰角、横滚就和航向角。

附图说明

图1为本发明的具体框图。

具体实施方式

下面结合附图对本发明进一步详细说明。

一种基于迭代EKF的Mahony姿态测量方法，其特征为：

步骤1：获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息；

在初始时刻用四元数表示姿态角为：

Q＝[q₀ q₁ q₂ q₃]

其中，q₀、q₁、q₂、q₃表示四元数。

通过姿态转移矩阵与四元数之间的转换关系可得：

其中，表示姿态转移矩阵。

记录MEMS陀螺和加速度输出为：

其中，表示陀螺输出，/>表示加速度输出。

步骤2：加速度信息归一化处理：

因为加速度计数据容易受到干扰且噪声较大，而陀螺数据基本不受外界干扰，且对于线性加速(震动)不敏感，因此只需要对加速度信息进行归一化处理。

步骤3：通过四元数得到理论重力加速度向量：

假设导航坐标系下的重力加速度为gⁿ＝[001]^T，我们可以通过姿态转移矩阵把导航系下的加速度gⁿ转换到载体坐标系：

步骤4：求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差：

理论重力加速度向量和实际重力加速度向量均是向量，反应向量间夹角关系的运算有两种：内积(点乘)和外积(叉乘)，考虑到向量外积模的大小与向量夹角呈正相关，故通过计算外积(叉乘)来得到向量方向差值。假设MEMS加速度计测量得到的载体系下的三轴加速度信息可表示为：

此时，载体系下理论重力加速度向量g^b和MEMS测量载体系下的加速度矢量之间的叉乘可表示为：

在载体坐标系上，MEMS加速度计测出来的重力向量是。陀螺积分后的姿态来推算出的重力向量是[v_xv_yv_z]^T。它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差[e_xe_ye_z]^T。

步骤5：陀螺偏差修正：

得到向量偏差后，即可通过构建PI补偿器来计算角速度补偿值：

Gyro_error＝K_p·Δθ+K_i·∫Δθ

其中，比例项用于控制传感器的“可信度”，积分项用于消除静态误差。Kp越大，意味着通过加速度计得到误差后补偿越显著，即是越信任加速度计。反之Kp越小时，加速度计对陀螺仪的补偿作用越弱，也就越信任陀螺仪。而积分项则用于消除角速度测量值中的有偏噪声，故对于经过零篇矫正的角速度测量值，一般选取很小的Ki。最后将补偿值补偿给角速度测量值，带入四元数差分方程中即可更新当前四元数。

经过误差修正后的陀螺信息可表示为：

根据一阶龙格库塔法(毕卡法)解四元数微分方程，更新四元数：

步骤6：四元数归一化处理：

步骤7：选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果。

选取姿态四元数作为滤波模型的状态变量X＝[q₀ q₁ q₂ q₃]^T。

根据陀螺输出计算四元数为：

由于陀螺测量包含噪声，表示为：

其中，表示陀螺的真实输出，δ_x、δ_y、δ_z表示随机噪声。系统状态方程可表示为：

建立滤波状态方程：

其中，W(t)为状态噪声，F(t)为状态转移矩阵，G(t)为状态噪声系数矩阵。

将状态方程离散化：

X_k+1＝Φ_k+1/kX_k+G_kW_k

其中，Φ_k+1/k为状态转移矩阵，G_k为状态噪声系数矩阵。

选取步骤6Mahony算法输出的姿态四元数作为量测量，建立量测方程：

Z_k+1＝H_k+1X_k+1+V_k+1

其中，H_k+1＝I_4×4为量测矩阵；V_k+1为量测噪声，其协方差矩阵为R_k。

步骤8：对滤波方程进行更新：

时间更新如下：

X_k+1|k＝Φ_k+1/kX_k

量测更新如下：

X_k+1＝X_k+1|k+K_k+1(Z_k+1-H_k+1X_k+1|k)

P_k+1＝P_k+1|k-K_k+1H_k+1P_k+1|k

定义迭代截止条件：

||K_k+1(Z_k+1-H_k+1X_k+1|k)||<ε

其中，ε为迭代门限值，此处取0.01。

若迭代条件满足，退出迭代；若迭代条件不满足，则继续量测更新。

步骤9：将步骤8的迭代后的最优四元数转换为欧拉角，输出姿态结果：

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (10)

1.一种基于迭代EKF的Mahony姿态测量方法，其特征在于，该方法包括如下步骤：

步骤1：获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息；

步骤2：根据步骤1的采集数据对加速度信息归一化处理；

步骤3：通过四元数得到理论重力加速度向量；

步骤4：求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差；

步骤5：陀螺偏差修正，进行四元数姿态更新；

步骤7：选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果。

步骤8：对滤波方程进行更新；

步骤9：将四元数转换为欧拉角，输出姿态结果。

2.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤1所述获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息的具体方法是：

在初始时刻用四元数表示姿态角为：

Q＝[q₀q₁q₂q₃]

其中，q₀、q₁、q₂、q₃表示四元数；

通过姿态转移矩阵与四元数之间的转换关系可得：

其中，表示姿态转移矩阵；

记录MEMS陀螺和加速度输出为：

其中，表示陀螺输出，w_x、w_y、w_z分别表示陀螺仪x,y,z三个轴的输出信息，/>表示加速度输出，a_x、a_y、a_z分别表示加速度计x,y,z三个轴的输出信息。

3.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤2所述加速度信息归一化处理公式如下：

其中，a′_x、a′_y、a′_z分别表示归一化处理后的x,y,z三个轴的加速度信息。

4.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤3所述通过四元数得到理论重力加速度向量的具体方法是：

假设导航坐标系下的重力加速度为gⁿ＝[0 0 1]^T，通过姿态转移矩阵把导航系下的加速度gⁿ转换到载体坐标系下的加速度g^b：

其中，v_x、v_y、v_z分别表示载体x,y,z三个方向的速度信息。

5.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤4所述求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差的具体方法如下：

假设MEMS加速度计测量得到的载体系下的三轴加速度信息表示为：

此时，载体系下理论重力加速度向量g^b和MEMS测量载体系下的加速度矢量之间的叉乘Δθ可表示为：

在载体坐标系上，MEMS加速度计测出来的重力向量是[a_x a_y a_z]^T，陀螺积分后的姿态来推算出的重力向量是[v_x v_y v_z]^T，它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差[e_x e_y e_z]^T。

6.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤5所述陀螺偏差修正的具体方法是：

得到向量偏差后，即可通过构建PI补偿器来计算角速度补偿值Gyro_error：

Gyro_error＝K_p·Δθ+K_i·∫Δθ

其中，比例项用于控制传感器的“可信度”，积分项用于消除静态误差，比例项系数K_p越大，意味着通过加速度计得到误差后补偿越显著，即是越信任加速度计；反之比例项系数K_p越小时，加速度计对陀螺仪的补偿作用越弱，也就越信任陀螺仪；而积分项则用于消除角速度测量值中的有偏噪声，故对于经过零篇矫正的角速度测量值，一般选取很小的积分项系数K_i；最后将补偿值补偿给角速度测量值，带入四元数差分方程中即可更新当前四元数；

经过误差修正后的陀螺信息可表示为：

根据一阶龙格库塔法(毕卡法)解四元数微分方程，更新四元数：

其中，t表示时间，Δt表示时间间隔，即数据更新周期，分别表示载体坐标系下估计得到的x,y,z三个轴的陀螺信息。

7.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤6所述四元数归一化处理的具体方法是：

其中，q′₀、q′₁、q′₂、q′₃分别表示经过归一化处理后的四元数。

8.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤7所述选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果的具体方法是：

选取姿态四元数作为滤波模型的状态变量X＝[q₀ q₁ q₂ q₃]^T，

根据陀螺输出计算四元数为：

由于陀螺测量包含噪声，表示为：

其中，表示陀螺的真实输出，δ_x、δ_y、δ_z表示随机噪声，系统状态方程可表示为：

建立滤波状态方程：

其中，W(t)为状态噪声，F(t)为状态转移矩阵，G(t)为状态噪声系数矩阵，X(t)为状态向量，为状态向量估计值；

将状态方程离散化：

X_k+1＝Φ_k+1/kX_k+G_kW_k

其中，Φ_k+1/k为状态转移矩阵，G_k为状态噪声系数矩阵，X_k+1为k+1时刻的状态向量，K_k为k时刻的状态向量，W_k为k时刻的系统噪声；

选取步骤6Mahony算法输出的姿态四元数作为量测量，建立量测方程：

Z_k+1＝H_k+1X_k+1+V_k+1

其中，H_k+1＝I_4×4为量测矩阵；V_k+1为量测噪声，其协方差矩阵为R_k，Z_k+1为k+1时刻的量测信息。

9.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤8所述对滤波方程进行更新，具体方法如下：

时间更新如下：

X_k+1|k＝Φ_k+1/kX_k

量测更新如下：

X_k+1＝K_k+1|k+K_k+1(Z_k+1-H_k+1X_k+1|k)

P_k+1＝P_k+1|k-K_k+1H_k+1P_k+1|k

定义迭代截止条件：

||K_k+1(Z_k+1-H_k+1X_k+1|k)||＜ε

其中，X_k+1|k为k时刻相对于k+1时刻的状态向量，P_k+1|k为k时刻相对于k+1时刻的协方差矩阵，P_k为k时刻的协方差矩阵，为k+i-1时刻的状态转移向量，Qk为k时刻的系统噪声矩阵，K_k+1为k+1时刻的增益矩阵，/>为k时刻相对于k+1时刻状态转移向量的转置，H_k+1为k+1时刻的量测转移矩阵，P_k+1为k+1时刻的协方差矩阵，ε为迭代门限值，此处取0.01；

若迭代条件满足，退出迭代；若迭代条件不满足，则继续量测更新。

10.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤9所述将步骤8的迭代后的最优四元数转换为欧拉角，输出姿态结果：

其中，θ、γ、分别表示俯仰角、横滚就和航向角。