基于MPU-6050和UKF的姿态测量系统设计

陶子涵 高国伟 徐万芝

北京信息科技大学 信息获取与检测实验室 北京 100101

一、引言

姿态信息是导航系统中最为关键的参数之一[1]，姿态测量系统在航空航天[2]、车辆船舶、机器人、人体姿态捕捉等诸多领域中被广泛使用。而在姿态测量系统中，人们最关心的就是测量精度。

对姿态测量系统精度造成影响的因素主要来源于两个方面：

一是系统的硬件配置，主要包括获取原始姿态信息的传感器原件和所选择的处理器。现今陀螺仪测姿定姿技术已经相当成熟，在航空航天、空间或军事等重要领域，采用高精度光学陀螺能进行准确的导航定姿，但因其成本高而使应用范围受到限制。而基于MEMS惯性测量元件设计的姿态测量系统价格低廉、稳定可靠，目前已被广泛的应用于民用微小型飞行器、老人防跌倒报警器、智能家居等许多方面。

其次，姿态估计所使用的算法也对姿态测量的精度有很大影响，只有选取的算法适用于系统的实际情况才能取得良好的效果，例如Hajiyev C及赵琳等人将EKF算法应用于姿态估计[3-4]取得了良好的估计效果。

综合考虑微小型无人机的应用背景[5]，设计了一款性价比较高的姿态测量系统。系统选用MPU-6050传感器[6-7]作为其数据采集模块的核心元件，考虑到UPF算法的实时性和有效性均能满足系统需求，所以采用UPF算法[8-10]完成系统姿态估计。针对MEMS传感器精度低、漂移大等特点[11]，设计了合理可行的软件解决方案。

二、系统总体设计

1、系统硬件设计

系统的整体结构框图如图1所示，主要由MPU-6050姿态检测电路及其外围电路、微控制器电路、串口通信接口电路、电源电路、等组成。通过姿态检测电路获取载体的姿态信息，经过A/D转换电路将数据传输至微处理器对姿态进行解算和更新，最后采用串口输出姿态解算结果在PC机中显示。

MPU-6050惯性测量模块是美国InvenSense公司于2009年推出的一款低成本的MEMS传感器，是全球第一款整合型六轴运动传感器，其数据采集能力具有高速、精确的特点，在本系统中用于获取姿态信息原始数据。

控制核心选用STM32F103C8T6微处理器。由于姿态估计过程要进行大量的矩阵运算和高阶运算，以完成状态预测、状态估计和更新、量测更新、均值和均方差等诸多运算，因此对系统的运算精度和速度要求较高，STM32F103C8T6是ST公司的ARM32位MCU，属于STM32系列增强型芯片之一，处理频率为72Hz，能够满足姿态解算的要求。

系统采用RS232接口与PC机进行通信，传输方式选择为非平衡传输，工作为电压±12V，由于STM32F103C8T6最高工作电压为3.6V，不符合RS232接口的电压要求，因此采用MAX3232芯片来进行电平转换。

2、系统软件设计

本系统的软件设计整体流程图如图2所示。首先对系统进行初始化。初始对准即将姿态测量系统对准到运动体的初始姿态，初始对准之后，采集运动体的姿态信息传输至STM32微处理器，作为姿态估计算法的输入量，然后进行姿态解算和更新，并将四元数表示的姿态信息转化为俯仰角、横滚角和偏航角经由串口输出，在上位机中显示。

三、姿态估计算法

1、状态方程

不同的姿态参数表示的动力学模型表达形式不同，基于四元数推导的姿态动力学模型[7]为：

其中，Ωb—载体坐标系相对于地理坐标系上的角速度分量，可由MPU-6050的陀螺仪部分测得，其矩阵表达式为：

其中，ωx—滚转角速率；

ωy—俯仰角速率；

ωz—偏航角速率。

即四元数动力学微分方程为：

假设陀螺仪的偏差仅受高斯白噪声影响，陀螺仪部分的输出模型为：

ω—载体真实角速度；

ωd—陀螺仪真实漂移；

W1—陀螺仪输出噪声；

—载体速度估计值；

—陀螺漂移估计值。

设Δωd为陀螺漂移估计误差，则：

其中，W1，W2设为零均值白噪声。

设过程噪声Wk为可加性噪声，其统计特性为：Wk～N(0,Qk)，由（2）（5）组成的状态方程为：

将陀螺漂移与姿态四元数一起选为系统状态向量，可以对陀螺漂移进行实时补偿，解决了MEMS陀螺漂移较大引起的姿态估计精度下降的问题。

2、量测方程

MPU-6050传感器的加速度输出模型可按照下式建立：

an—地理系下的重力向量，an=[0 0g]T；

V—传感器输出噪声。

设量测噪声也为可加性噪声，其统计特性为Vk～N(0,Rk)，即量测方程为：

综上，状态方程和量测方程满足：

3、UKF算法[6]

微小型无人机姿态测量系统是一种典型的非线性系统，若采用EKF算法进行滤波，必须对原系统和量测作泰勒级数展开并且只保留线性项，在系统非线性度较高的情况下，EKF的滤波精度会大大降低，因此只适用于弱非线性对象的估计问题。

UKF是基于一种UT变换的滤波算法，它首先根据初始状态均值和方差矩阵产生k时刻的sigma点，计算sigma点时所给的方差矩阵必须是正定矩阵。将sigma点带入到状态方程中计算出变换的样本点并确定相应的权值，完成状态预测。然后计算k时刻的增广样本点，带入到观测方程计算出预测观测模型的样本点，完成观测模型预测。最后根据上面计算出的状态预测方程和观测预测方程，计算出变换后的均值和方差矩阵及增益矩阵，计算出滤波值，完成状态估计和更新从而达到滤波的目的。

UKF算法步骤如下：

（1）选定滤波初值：

（2）计算sigma样本点

（3）时间更新

（4）量测更新

4、实验测试

选用SGT320E型多功能三轴转台对本文所设计的姿态测量系统进行测试，将测量系统固定在转台上，控制转台位置使得测量系统初始角均为零度，采样频率设置为500Hz，转台转速设置为2°/s，采样点为600个，测量出系统输出的角度误差以验证系统对运动体各姿态角度的检测效果。

通过实验对比了采用EKF和采用UKF两种姿态估计算法下系统的俯仰角、滚转角和偏航角的输出角度误差，如图3、4、5所示，EKF和UKF滤波误差对比如表1所示。

由表1统计结果表明，UKF用于姿态估计的误差均值、方差及最大误差都明显低于EKF，说明UKF具有更高的滤波精度。

四、结论

表1 EKF和UKF滤波误差对比

本文设计了一种基于MPU-6050六轴姿态传感器的微型姿态测量系统，针对现有低成本MEMS传感器所存在的测量精度较低、漂移大等问题，提出了基于UKF算法的姿态估计方法应用于所设计的姿态测量系统。文章首先简要的介绍了姿态测量系统的整体硬件结构设计和软件设计流程，然后对系统的状态方程和量测方程进行了分析并给出了UKF算法，最后通过实验及实验数据对所设计的系统进行了分析和验证。结果表明，UKF算法应用于基于MPU-6050姿态测量系统的姿态估计精度较EKF算法有很大提高，验证了UKF算法的优越性以及系统的可用性。