基于四元数的低成本姿态测量系统设计

陈建翔，万子敬，王向军

（天津大学微光机电教育部重点实验室，天津300072）

基于四元数的低成本姿态测量系统设计

陈建翔，万子敬，王向军*

（天津大学微光机电教育部重点实验室，天津300072）

针对运动载体姿态测量的需求，以MEMS惯性测量器件MPU6050为核心，设计了一种低成本姿态测量系统。系统采用四元数作为姿态解算的基础，融合陀螺仪和加速度计数据以修正陀螺仪累积误差引起的姿态漂移。为了降低MCU负担提高系统实时性，数据融合采用运算量较小的梯度下降和互补滤波相结合的方法。实验结果表面：此测量系统实时性好，输出姿态角稳定，且成本低，具有一定的实用价值。

MPU6050；梯度下降；互补滤波；姿态解算

EEACC：2575doi：10.3969/j.issn.1004-1699.2016.05.014

近年来，四旋翼飞机、自平衡电动车、机器人等项目十分流行。在这些项目中，都需要实时进行载体或部件的姿态测量，以实现精确控制。常见的姿态测量单元，一般的包含三轴陀螺仪、三轴加速度计和三轴磁强计［1］。常见的姿态解算方法有：四元数法，欧拉角法，方向余弦法，等效旋转矢量法等［2］。其中，四元数法虽对有限转动引起的不可交换误差的补偿程度不够，但是能满足大多数低速情况下的需求，且四元数法只涉及四个线性微分方程，计算量小、算法简单，故在工程中常被采用［3］。

陀螺仪动态性能好，但存在漂移问题，且积分累积误差的存在，长期使用时需要对其进行修正。加速度计和磁强计不存在积分误差的问题，静态性能好，但是分别受载体振动和外界磁场干扰严重，动态性能很差。融合三者数据，即可得到同时具有良好的动态性能和静态性能的姿态角［4］。

相比传统传感器，MEMS传感器体积小、质量轻、功耗低、成本低，MEMS技术的发展使得低成本姿态测量系统有了更广泛的应用［5］。本文以InvenSense公司生产的MPU6050作为核心，设计了一个低成本的双轴姿态测量系统，以单片机为数据处理器，采用一种基于梯度下降和互补滤波相结合的计算量低且效果较好的数据融合算法，使用四元数进行姿态解算。

1　基于四元数的姿态计算方法

假设导航坐标系为N，其中坐标轴Xn指正东，Yn指正北，Zn指天，坐标系B为与载体相固定的载体坐标系，三轴加速度计和三轴陀螺仪分别固定到载体坐标系的X、Y、Z三轴上。

角θ、γ和ψ表示载体相对于导航坐标系N的姿态，即俯仰角、横滚角和航向角。由3个姿态角可定义导航坐标系N到载体坐标系B的旋转矩阵为：

定义一个用来表示姿态信息的姿态四元数Q，其中Q=q0+q1i0+q2j0+q3z0且‖‖Q=1，则式（1）所示的旋转矩阵可用四元数表示为

根据式（1）和式（2）可得姿态角与四元数之间的关系，分别表示为：

姿态四元数Q的微分方程用矩阵形式可表示为：

其中，在忽略地球自转、位置速率等因素影响下wx、wy、wz为陀螺仪输出的三轴角速率。给定初始四元数Q0（即初始姿态角已知），由上述微分方程求得当前时刻的四元数Q，代入式（3）进而可得当前时刻的姿态角［6-7］。

2　基于梯度下降和互补滤波的数据融合方法

2.1梯度下降法

加速度计具有良好的静态特性，长时间使用不会引入积分误差，但是加速度计受载体振动和运动影响大，非静态环境下使用，瞬时误差大，本文先使用加速度计数据对姿态四元数进行优化，得到一个静态特性较好的姿态四元数，优化过程使用梯度下降法。

梯度下降法，又叫最速下降法，是一种迭代求极值的算法［8-9］，具体实现是按负梯度方向搜索寻优的过程。

假设重力加速度垂直向下，则在导航坐标系下重力加速度矢量为，其中g是地表重力加速度的大小。利用旋转四元数矩阵将g转换到载体坐标系，记为a=［ax′，ay′，az′］，其中

载体坐标系下实际测得的载体加速度矢量为a=［ax，ay，az］，其中ax、ay、az分别为三轴加速度计测得的加速度大小。理想情况下，a=a′。实际上两者之间存在一个测量误差，根据此测量误差，定义误差函数为：

对e（Q）取2范数的平方，取为梯度下降法寻优目标函数：

进而对姿态四元数的优化就可以转换为对f（Q）求最小值。

根据梯度下降法，姿态四元数可按下式进行修正［10］：

其中η为搜索步长。当利用四元数法得到当前时刻的姿态四元数后，用上式进行数次迭代得到四元数最优值。为了降低运算量，本文实际程序中只进行一次迭代。

2.2互补滤波法

利用三轴加速度计测得的数据根据梯度下降法对姿态四元数进行修正后，得到优化的四元数记为Qa。由于加速度计的特性，Qa不会引入累积误差，但是非静止情况下，Qa会引入较大的瞬时误差。相反，根据陀螺仪测得的姿态瞬时精度高，但是会随时间累积误差。记陀螺仪测量数据直接解算得出的姿态四元数为Qg。利用Qa和Qg在频域上的互补特性，通过互补滤波方法［11］融合两者，可得到新姿态四元数Q，同时具有较高的瞬时精度且不会引入累积误差而产生漂移。互补滤波公式如下：

其中k为滤波系数，在程序调试过程中调整至最佳值。

将Q代入式（3）即可解算得到实时姿态角。

3　系统硬件和程序设计

3.1硬件设计

传感器方面，选择InvenSense公司推出的一款6轴传感器模块MPU6050，整合了三轴加速度计和三轴陀螺仪，其中加速度计可编程测量范围为±2 gn、±4 gn、±8 gn和±16 gn，陀螺仪可编程测量范围为±250°/s、±500°/s、±1 000°/s和±2 000°/s。MCU芯片可通过I2C接口与MPU6050进行传感器初始化控制和数据交换。

作为一种整合性MEMS器件，它的优点是体积小、成本低，而且MPU6050作为全球首例整合性6轴运动处理组件，相对于多个独立的加速度计陀螺仪组合的多组件方案，有效的免除了安装陀螺仪和加速度计时轴间误差的问题，且大大减少了占用尺寸［12］。并且MPU6050内部集成了低通滤波器，可在不增加MCU运算量的前提下对陀螺仪和加速度数据进行滤波处理，提高原始数据质量。

MCU方面，考虑到性能和功耗，选用飞思卡尔生产的MK10DN32VFM5单片机作为系统的控制芯片进行传感器数据处理、姿态四元数的解算和数据的融合。

硬件框图如图1所示，MCU将陀螺仪加速度计数据和解算出的姿态角通过串口传输到上位机以供使用。

图1　系统硬件框图

3.2程序设计

陀螺仪与加速度计的原始数据存在误差，在用于姿态解算前用滑动均值滤波［13］进行平滑处理，并对陀螺仪数据进行零偏补偿。

系统上电后，对MPU6050和MCU进行初始化工作，随后依次完成数据采集、数据滤波、原始姿态四元数解算、梯度下降和互补滤波数据融合、最终姿态角输出。其中，其中加速度计测量范围取±2 gn，陀螺仪测量范围设为±2 000°/s。陀螺仪和加速度数据以及解算出来的俯仰横滚姿态角以串口形式输出，供上层处理。

其中姿态四元数计算即对式（4）的姿态四元数微分方程求解，本文采用毕卡求解法［14］：

其中

4　实验结果分析

4.1静止实验分析

将传感器置于水平桌面上，采集俯仰角（Pitch）并绘制俯仰角随时间变化曲线。其中，图3为由三轴陀螺仪数据使用四元数法直接解算出的俯仰角随时间变化曲线，因为陀螺仪数据积分导致的累积误差，4 s内，俯仰角漂移了约3°。图4为将加速度计数据与陀螺仪数据使用前述融合算法进行融合后解算出的俯仰角随时间变化曲线，如图4所示，加速度计数据具有静态稳定性，不会随时间漂移，修正了陀螺仪数据的积分误差，俯仰角长期保持稳定。

图3　静止，未融合

图4　静止，融合

静止条件下，以高精度转台的输出作为俯仰角真值，与传感器输出俯仰角进行对比，角度越大，误差越大，正负90°静态精度可达2°，正负30内静态精度可达0.3°。部分数据如表1所示。

表1　静态精度测试

4.2动态实验分析

如图5所示，将传感器固定于三轴转台，传感器俯仰轴与转台的俯仰轴平行，转台绕俯仰轴以20°/s的速率在正负25°之间来回摆动。图6为传感器输出俯仰角随时间变化曲线，如图，曲线光滑，25°处最大误差为0.52°，可见此传感器随动性较好，低速情况下，具有较好的动态性能。

图5　转台实验

图6　动态测试

4.3结论

综合静止实验和动态实验分析，本文设计的基于MEMS器件MPU6050的低成本姿态测量系统具有较好的动态性能和静态性能，在低速场合下具有一定的实用价值。采用梯度下降和互补滤波相结合的数据融合算法，不仅计算量小实时性高，而且具有良好的性能。由于航向角（RAW）的改变并不会影响三轴加速度计的数据，即三轴加速度计数据不包含横滚角信息，所以与三轴陀螺仪数据融合后，不能修正RAW角的漂移。如需使用RAW角，需融合磁强计（电子罗盘）数据，对其进行修正。

［1］彭孝东，张铁民，李继宇，陈瑜.基于传感器校正与融合的农用小型无人机姿态估计算法［J］.自动化学报，2015，41（4）：854-860.

［2］刘凯，梁晓庚.基于陀螺仪和磁强计的姿态解算方法研究［J］.计算机仿真，2014，31（5）：39-42.

［3］丁君.基于微惯性传感器的姿态算法研究［D］：［硕士学位论文］，上海：上海交通大学，2013.

［4］GebreEgziabher D，Hayward R C，Powell J D.Design of Multi Sen⁃sor Attitude Determination Systems［J］.IEEE Transactions onAerospace and Electronic System，2004，40（2）：627-649.

［5］赵鹤，王喆垚.基于UKF的MEMS传感器姿态测量系统［J］.传感技术学报，2011，24（5）：642-646.

［6］秦永元.惯性导航［M］.北京：科学出版社，2014：288-297.

［7］叶锃锋，冯恩信.基于四元数和卡尔曼滤波的两轮车姿态稳定方法［J］.传感技术学报2012，25（4）：524-528.

［8］王俊，鲁晓天.对四轴飞行器基于姿态算法的分析与实现［J］.河南科技，2015，556（2）：51-53.

［9］Madgwick S.An Efficient Orientation Filter for Inertial and Iner⁃tial/Magnetic Sensor Arrays［R］.Report x-io and University of Bristol（UK），2010.

［10］吴秋峰.Stiefel流形上的梯度下降法［J］.应用数学学报，2012，35（4）：719-726.

［11］Won M，Kim S S.Design and Control of a Marine Satellite Antenna［J］.Journal of Mechanical Science and Technology，2005，19（1）：473-480.

［12］邱云平，伍宝玉.MPU-6050模块角度算法处理及在嵌入式中的应用［J］.江西科技学院学报，2014，9（2）：26-29.

［13］樊毅，秦会斌.基于STM32的高精度电子天平［J］机电工程，2013，30（1）：51-54.

［14］邢质皙，王爱民.捷联惯导系统姿态算法比较［J］.舰船电子工程，2009，181（7）：81-84.

陈建翔（1990-），男，浙江金华人，天津大学精密仪器与光电子工程学院硕士，研究方向为姿态测量，email：jianxiang_chen@ 126.com；

王向军（1955-），男，黑龙江哈尔滨人，博士，天津大学精密仪器与光电子工程学院教授，博士生导师，主要研究方向为精密测试技术与仪器、光电探测与传感技术、影像与视觉测量，xdocuxjw@ vip.163.com。

Design of Low-Cost Attitude Determination System Based on Quaternion

CHEN Jianxiang，WAN Zijing，WANG Xiangjun*
（MOEMS Education Ministry Key Lab，Tianjin University，Tianjin 300072，China）

To satisfy the requirements of attitude determination of moving carrier，a low-cost attitude determination system is designed.The attitude algorithm is based on quaternion representation.To correct drift error caused by cu⁃mulative error of the gyroscope，it combines gyroscope data and accelerometer data by the algorithm based on gradi⁃ent descent and complementary filter，which can reduce the load of MCU and improve real timing by costing less computation amounts.The experimental results indicate that the low-cost system can work in real time and output stable attitude.It has practical value of engineering.

MPU6050；gradient descent；complementary filter；attitude algorithm

TP212.9

A

1004-1699（2016）05-0706-05

2015-10-21修改日期：2016-01-18