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

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

【技术实现步骤摘要】
一种基于迭代EKF的Mahony姿态测量方法


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

技术介绍

[0002]对于六轴数据，计算角度有两种方法，一种是通过对角速度积分得到角度，另一种则是通过对加速度进行正交分解得到角度。但这两种方式均存在不足，通过角速度积分得到角度时，角速度的误差会在积分过程中被不断放大从而影响数据准确性。而加速度计是一种特别敏感的传感器，电机旋转产生的震动会给加速度计的数据中带来高频噪声。不难看出，第一种方法测得的数据中存在结果偏差，而第二种方法测得的数据中存在高频噪声。因此可通过融合两种数据以获得准确姿态，这里通过加速度计补偿角速度。
[0003]加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。
[0004]Mahony互补滤波算法的基本原理是：通过加速度计测量值和采用陀螺积分后的姿态推算出来的重力加速度值之间的误差来对陀螺测量值进行校正，进而保证陀螺数据的准确性。然后，采用一阶龙格—库塔法解四元数微分方程来获取载体姿态。

技术实现思路

[0005]本专利技术的技术解决问题是：针对Mahony互补滤波算法对传感器数据优化不足以及非线性问题引起的缺陷，提出一种基于迭代EKF的Mahony姿态测量方法，利用迭代EKF算法进行加速度计的姿态信息和惯性的姿态进行最优融合，提高姿态测量系统的精度。
[0006]为达到上述目的，本专利技术的技术方案如下：
[0007]一种基于迭代EKF的Mahony姿态测量方法，该方法包括如下步骤：
[0008]步骤1：获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息；
[0009]步骤2：根据步骤1的采集数据对加速度信息归一化处理；
[0010]步骤3：通过四元数得到理论重力加速度向量；
[0011]步骤4：求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差；
[0012]步骤5：陀螺偏差修正，进行四元数姿态更新；
[0013]步骤7：选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果；
[0014]步骤8：对滤波方程进行更新；
[0015]步骤9：将四元数转换为欧拉角，输出姿态结果。
[0016]进一步地，步骤1所述获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息的具体方法是：
[0017]在初始时刻用四元数表示姿态角为：
[0018]Q＝[q0q1q2q3][0019]其中，q0、q1、q2、q3表示四元数；
[0020]通过姿态转移矩阵与四元数之间的转换关系可得：
[0021][0022]其中，表示姿态转移矩阵；
[0023]记录MEMS陀螺和加速度输出为：
[0024][0025]其中，表示陀螺输出，w
x
、w
y
、w
z
分别表示陀螺仪x,y,z三个轴的输出信息，表示加速度输出，a
x
、a
y
、a
z
分别表示加速度计x,y,z三个轴的输出信息。
[0026]进一步地，步骤2所述加速度信息归一化处理公式如下：
[0027][0028]其中，a
′
x
、a
′
y
、a
′
z
分别表示归一化处理后的x,y,z三个轴的加速度信息。
[0029]进一步地，步骤3所述通过四元数得到理论重力加速度向量的具体方法是：
[0030]假设导航坐标系下的重力加速度为g
n
＝[001]T
，通过姿态转移矩阵把导航系下的加速度g
n
转换到载体坐标系下的加速度g
b
：
[0031][0032]其中，v
x
、v
y
、v
z
分别表示载体x,y,z三个方向的速度信息。
[0033]进一步地，步骤4所述求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差的具体方法如下：
[0034]假设MEMS加速度计测量得到的载体系下的三轴加速度信息表示为：
[0035][0036]此时，载体系下理论重力加速度向量g
b
和MEMS测量载体系下的加速度矢量之间的叉乘Δθ可表示为：
[0037][0038]在载体坐标系上，MEMS加速度计测出来的重力向量是[a
x a
y a
z
]T
，陀螺积分后的
姿态来推算出的重力向量是[v
x v
y v
z
]T
，它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差[e
x e
y e
z
]T
。
[0039]进一步地，步骤5所述陀螺偏差修正的具体方法是：
[0040]得到向量偏差后，即可通过构建PI补偿器来计算角速度补偿值Gyro
error
：
[0041]Gyro
error
＝K
p
·
Δθ+K
i
·
∫Δθ
[0042]其中，比例项用于控制传感器的“可信度”，积分项用于消除静态误差，比例项系数K
p
越大，意味着通过加速度计得到误差后补偿越显著，即是越信任加速度计；反之比例项系数K
p
越小时，加速度计对陀螺仪的补偿作用越弱，也就越信任陀螺仪；而积分项则用于消除角速度测量值中的有偏噪声，故对于经过零篇矫正的角速度测量值，一般选取很小的积分项系数K
i
；最后将补偿值补偿给角速度测量值，带入四元数差分方程中即可更新当前四元数；
[0043]经过误差修正后的陀螺信息可表示为：
[0044][0045]根据一阶龙格库塔法(毕卡法)解四元数微分方程，更新四元数：
[0046][0047]其中，t表示时间，Δt表示时间间隔，即数据更新周期，分别表示载体坐标系下估计得到的x,y,z三个轴的陀螺信息。
[0048]进一步地，步骤6所述四元数归一化处理的具体方法是：
[0049][0050]其中，q
′0、q
′1、q
′2、q
′3分别表示经过归一化处理后的四元数。
[0051]进一步地，步骤7所述选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果的具体方法是：
[0052]选取姿态四元数作为滤波模型的状态变量X＝本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于迭代EKF的Mahony姿态测量方法，其特征在于，该方法包括如下步骤：步骤1：获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息；步骤2：根据步骤1的采集数据对加速度信息归一化处理；步骤3：通过四元数得到理论重力加速度向量；步骤4：求取采用陀螺推算出的加速度和加速度计测出来的加速度之间的误差；步骤5：陀螺偏差修正，进行四元数姿态更新；步骤7：选取姿态四元数作为滤波模型的状态变量，利用迭代EKF进行滤波估计，提高姿态解算结果。步骤8：对滤波方程进行更新；步骤9：将四元数转换为欧拉角，输出姿态结果。2.根据权利要求1所述的一种基于迭代EKF的Mahony姿态测量方法，其特征在于，步骤1所述获取当前时刻IMU传感器的陀螺和加速度计的角速度和比力信息的具体方法是：在初始时刻用四元数表示姿态角为：Q＝[q0q1q2q3]其中，q0、q1、q2、q3表示四元数；通过姿态转移矩阵与四元数之间的转换关系可得：其中，表示姿态转移矩阵；记录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
n
＝[0 0 1]
T
，通过姿态转移矩阵把导航系下的加速度g
n
转换到载体坐标系下的加速度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
；最后将补偿值补偿给角速度测量值，带入四元数差分方程中即可更新当前四元数；经过误差修正后的陀螺信息可表...

【专利技术属性】
技术研发人员：刘明明，李二虎，山发荣，吕南宁，
申请(专利权)人：西宁市第一人民医院，
类型：发明
国别省市：