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

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

背景技术：

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

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

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

技术实现思路

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

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

3、一种基于迭代ekf的mahony姿态测量方法，该方法包括如下步骤：

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

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

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

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

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

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

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

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

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

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

14、q＝[q0q1q2q3]

15、其中，q0、q1、q2、q3表示四元数；

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

17、

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

19、记录mems陀螺和加速度输出为：

20、

21、其中，表示陀螺输出，wx、wy、wz分别表示陀螺仪x,y,z三个轴的输出信息，表示加速度输出，ax、ay、az分别表示加速度计x,y,z三个轴的输出信息。

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

23、

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

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

26、假设导航坐标系下的重力加速度为gn＝[001]t，通过姿态转移矩阵把导航系下的加速度gn转换到载体坐标系下的加速度gb：

27、

28、其中，vx、vy、vz分别表示载体x,y,z三个方向的速度信息。

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

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

31、

32、此时，载体系下理论重力加速度向量gb和mems测量载体系下的加速度矢量之间的叉乘δθ可表示为：

33、

34、在载体坐标系上，mems加速度计测出来的重力向量是[ax ay az]t，陀螺积分后的姿态来推算出的重力向量是[vx vy vz]t，它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差[ex ey ez]t。

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

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

37、gyroerror＝kp·δθ+ki·∫δθ

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

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

40、

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

42、

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

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

45、

46、其中，q′0、q′1、q′2、q′3分别表示经过归一化处理后的四元数。

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

48、选取姿态四元数作为滤波模型的状态变量x＝[q0 q1 q2 q3]t，

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

50、

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

52、

53、其中，表示陀螺的真实输出，δx、δy、δz表示随机噪声，系统状态方程可表示为：

54、

55、建立滤波状态方程：

56、

57、其中，w(t)为状态噪声，f(t)为状态转移矩阵，g(t)为状态噪声系数矩阵，x(t)为状态向量，为状态向量估计值；

58、将状态方程离散化：

59、xk+1＝φk+1/kxk+gkwk

60、其中，φk+1/k为状态转移矩阵，gk为状态噪声系数矩阵，xk+1为k+1时刻的状态向量，xk为k时刻的状态向量，wk为k时刻的系统噪声；

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

62、zk+1＝hk+1xk+1+vk+1

63、其中，hk+1＝i4×4为量测矩阵；vk+1为量测噪声，其协方差矩阵为rk，zk+1为k+1时刻的量测信息。

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

65、时间更新如下：

66、xk+1|k＝φk+1/kxk

67、

68、量测更新如下：

69、

70、xk+1＝xk+1|k+kk+1(zk+1-hk+1xk+1|k)

71、pk+1＝pk+1|k-kk+1hk+1pk+1|k

72、定义迭代截止条件：

73、||kk+1(zk+1-hk+1xk+1|k)||<ε

74、其中，xk+1|k为k时刻相对于k+1时刻的状态向量，pk+1|k为k时刻相对于k+1时刻的协方差矩阵，pk为k时刻的协方差矩阵，为k+i-1时刻的状态转移向量，qk为k时刻的系统噪声矩阵，kk+1为k+1时刻的增益矩阵，为k时刻相对于k+1时刻状态转移向量的转置，hk+1为k+1时刻的量测转移矩阵，pk+1为k+1时刻的协方差矩阵，ε为迭代门限值，此处取0.01；

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

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

77、

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