马陟昶 王勤湧 郭张乐 范鹏宇 项畅清

摘要：六轴陀螺仪是三轴陀螺仪与三轴加速计的合称，它是在三轴陀螺仪这个传感器的基础上增加上三轴加速器这个元器件。由于六轴陀螺仪收集的加速度、角速度等数据会受到噪声信号的影响，所以要对这些收集到的数据进行滤波处理。在众多的滤波方式中，选用较为普遍的一阶互补滤波、卡尔曼滤波和DMP，在滤波后获得更加精确的数据同时对这三种滤波方式进行对比。

关键词：六轴陀螺仪；一阶互补滤波；卡尔曼滤波；DMP（Digital Motion Processor）

中图分类号：TP302 文献标识码：A 文章编号：1009-3044（2018）03-0243-02

陀螺仪是自动控制系统中的一个信号传感器，因为它成本低、体积小、重量轻等优点被广泛运用于飞行器行业，为飞行器提供准确的方位、水平、位置、速度和加速度等信号。本文通过对六轴陀螺仪姿态解算的方法分析，来阐述六轴陀螺仪如何向飞行器的单片机反馈信号，同时在匿名上位机该仿真软件的帮助下，我们会分别使用三种算法对六轴陀螺仪的波形进行仿真测试并记录，从而来分析各个滤波方式的优劣[1]。

1 一阶互补滤波

因为MPU6050收集获得的加速度和角速度数据会被传感器噪声信号的影响，从而使得我們不能直观的通过加速度和角速度这两个数据来获得该轴上的角度。所以我们需要对加速度和角速度这两个数据进行互补滤波来近似得到一个比较准确的角度。

互补滤波算法可以同时滤除低频和高频的干扰，能更好地实现传感器的数据融合，以下为一阶互补滤波的实函数

voidfilter（float angle_m， float gyro_m）

{angle = K1 * angle_m+ （1-K1） * （angle + gyro_m * dt）；

//本次滤波的输出值 = 本次采样值 + 上次滤波的输出值

}

公式中angle_m和gyro_m分别是经过陀螺仪采集数据计算后得到的角度与角加速度；K1是对加速度计取值的权重；dt是滤波器的采样时间。在获得上一次滤波的输出值以及本次陀螺仪采集得到数据的情况下，我们就可以通过该公式获得本次滤波后的输出值[2]。

2 卡尔曼滤波

在测量方差已知的情况时，卡尔曼滤波能够从测量噪声的数据中估计动态系统的状态，所以卡尔曼滤波对于六轴陀螺仪收集的动态数据的处理有很大的帮助[3]。

首先，卡尔曼滤波需要一个离散控制过程的系统，这一过程可以使用一个线性随机微分方程来描述：

上述方程中k表示一个实际的值，也就是第k时刻的真实量，比如X（k）为第k时刻系统状态、Z（k）为第k时刻测量值，而U（k）则为第k时刻对系统的控制量。A和B为系统参数，是相对于多模型系统的矩阵；H为测量系统的参数，是相对于多测量系统的矩阵。W（k）和V（k）分别为过程中的噪音及测量中的噪声。它们被假设为高斯白噪声（White Gaussian Noise），他们的协方差（covariance）分别是Q和R。若以上条件满足，那么卡尔曼滤波将比较理想。

在该系统中，需利用该过程模型去预测下一状态的系统，也即下一状态结果=上一状态结果+现在上状态控制量（控制量可为0），公式为：

接着在系统结果已经更新之后，我们需要对协方差（covariance）进行更新，至于如何更新协方差呢，这就要使用到X（k|k-1）、X（k-1|k-1）的协方差，也就是下一状态和上一状态的协方差，其公式为：

其中C表示协方差，A表示A的转置矩阵，Q是系统过程的协方差。

随着得到对系统的预测结果之后，我们便需要开始收集现在状态的测量值。结合预测的结果及收集好的测量值，便可得到现今的最优估算值，相应公式为：

上式中Kg为卡尔曼增益（Kalman Gain），由协方差和H矩阵计算得到，相应的公式为：

有了第k时刻状态下最优的估算值X（k|k），我们还需更新这第k时刻状态下的协方差以便卡尔曼滤波能运行下去直至系统过程结束，该过程公式为：

其中I为1的矩阵，对于单模型单测量，I=1。C（k|k）是系统进入k+1状态时上述公式中的C（k-1|k-1）[4]。

3 DMP（Digital Motion Processor）

DMP（Digital Motion Processor）为MPU6050 自带的数字运动处理器硬件加速引擎，通过I2C接口可以输出6轴姿态数据。同时，InvenSense 公司提供了相应的嵌入式运动驱动库，结合 DMP可以将原始数据直接转换成四元数输出。而通过四元数可以计算出欧拉角，即航向角（yaw）、横滚角（roll）和俯仰角（pitch）。使用内置的 DMP，不但可以让6轴的代码设计更加简洁，而且省略了 MCU的姿态解算过程。可以技巧有效的降低 MCU负担，进而提高系统实时性。

在对四元数进行相应的格式转换后，可以采用如下公式计算欧拉角

pitch=asin（-2 * q1 * q3 + 2 * q0* q2）* 57.3； //俯仰角

roll=atan2（2 * q2 * q3 + 2 * q0 * q1， -2 * q1 * q1 - 2 * q2* q2 + 1）* 57.3； //横滚角

yaw=atan2（2*（q1*q2 + q0*q3），q0*q0+q1*q1-q2*q2-q3*q3） * 57.3； //航向角

其中quat[0]～quat[3]是 MPU6050 的 DMP 解算后的四元数，q30格式，所以要除以一个2的30次方，其 q30是一个常量：1073741824，即2的30次方，然后带入公式，计算得到欧拉角[5]。

4 对比解析

以上对比可知，一阶互补滤波计算量小跟随性好，卡尔曼计算量大，动态性能更优，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，数据较为平滑。 参考文献：

[1] 郭秀中.陀螺仪理论及应用[M].航空工业出版社，1987.

[2] 郭晓鸿，杨忠，陈喆，等.EKF和互补滤波器在飞行姿态确定中的应用[J].传感器与微系统，2011，30（11）.

[3] 宋文尧.卡尔曼滤波[M].科学出版社，1991.

[4] 孔令磊，汤洁.由TMS320C32芯片实现陀螺仪漂移卡尔曼滤波算法研究[J].计算机工程与设计， 2008，29（7）.

[5] 张允华.互补滤波器在四元数法姿态解算中的应用[J].通讯世界，2015，35.