基于MEMS的运动体姿态角度测量系统设计与实现

徐万芝 高国伟 潘宏生 杨寰 毛瑞燕

1.北京信息科技大学传感器重点实验室，北京100192；

2.北京国科舰航传感技术有限公司，北京 100101

3.北京机械设备研究所物资供应室，北京 100039

一、引言

随着我国工业技术的不断发展，姿态测量广泛应用于各个领域中，无论是定位、定向，还是导航都离不开姿态测量。运动体姿态测量系统是提供运动体三轴的角速率、俯仰角和横滚角信息的装置，是运动体实现姿态控制的前提，它不仅可以为运动体姿态控制系统提供三维姿态信息，也可以为数据记录器提供运动体的三维姿态信息以备事后分析。

随着微电子技术的发展而出现的微电子机械系统（MEMS）具有尺寸小、重量轻、成本低、功耗小和动态范围宽等优点，被广泛应用于测量领域[1,2]。本文应用MEMS技术，设计出一种成本低、性能好、功耗低的运动体姿态测量系统，能较好的实现运动体姿态角度的测量。

二、系统总体设计

系统基本的结构框图如图1所示。 本文选用六轴运动处理组件，包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计测量运动体在坐标系统中独立三轴的加速度信号，而陀螺仪检测运动体相对于导航坐标系的角速度信号，从而测量运动体在三维空间中的角速度和加速度。本文的设计思路是通过惯性测量模块测量运动体的加速度和角速度信号，再经微处理器模块进行姿态解算，进而得出运动体的姿态信息，最后通过串口通信模块将数据传输到上位机，得到实时且可视化的姿态信息。

三、系统硬件设计

1、微处理器模块

本系统微处理器模块选用STM32F103C8T6，这是意法半导体公司 生 产 的ARM 32位 的Cortex™-M3 CPU，工作频率最高可达72MHz，内置高速存储器，拥有64K字节的闪存和20K字节的SRAM，丰富的增强I/O端口和联接到两条APB总线的外设，工作电压范围在2.0～3.6V，具有高速性、低成本、高性价比等优点。STM32F103C8T6的最小系统如图2所示。

2、惯性测量模块

惯性测量模块是姿态测量系统的重要组成部分，本系统所选用的是硬件为MPU-6050模块。MPU-6050是一个可直接使用的模块，整合了MPU-6050芯片、供电电路及其端口电路，测量精准且性价比较高。MPU-6050芯片设有3个16位模拟-数字转换器，用于数字化陀螺仪和加速度计输出。用户可对陀螺仪和加速度计进行编程控制，陀螺仪满量程测量范围为 ±250、±500、±1000与 ±2000°/s，加速度计满量程测量范围为±2g、±4g、±8g与±16g，对于快速和慢速运动可进行精确跟踪。模块实物及端口电路如图3所示。

MPU-6050芯片是全球首例整合性六轴运动处理传感器，它消除了组合陀螺仪与加速度计时存在轴间差的问题，使封装空间得到了大量减少，消除了加速度计与陀螺仪的轴间敏感度，降低了人为设定产生的影响与传感器的漂移。内含数字运动处理(DMP: Digital Motion Processing)引擎，可减少复杂的融合演算数据、传感器同步化、姿势感应等的负荷。MPU-6050芯片连接电路如图4所示。

MPU-6050的供电电压为3.3V，通过一个低压差线性稳压器供电，电路图如图5所示。

3、串口通信模块

为了将计算得到的运动体姿态信息显示出来，我们采用RS232 接口与PC机进行交互通信。一方面，PC机实时接收运动体的加速度和角速度等参数；另一方面，可通过PC机对测量系统的工作参数进行补偿设置。STM32103C8T6的最高工作电压3.6V，但与RS232的电压标准本不匹配，故而本系统采用MAX3232 芯片来实现电平转换。串行通信接口电路如图6所示。

四、系统软件设计

1、姿态解算原理

运动体的姿态解算算法是实现运动体姿态精确测量的核心技术之一。目前描述动态坐标系方位关系的方法有多种，主要分为三个类别：欧拉角法、方向余弦法、四元数法。其中，欧拉角法存在“奇点”现象，不能用于全姿态运动载体上，且实时计算较为困难。方向余弦法虽然避免了欧拉角法的“奇点”现象，但方程的计算量较大，使得工作效率降低[3]。相比于前两类方法，四元数法能更加合理有效地描述载体的刚体空间运动，因此本系统选用四元数法进行姿态解算。

四元数微分方程的表达式为：

其中，Ωb—运动体坐标系相对地理坐标系的旋转角速度的斜对称矩阵。

式（1）可扩展为：

其中，λ、P1、P2、P3—四元数的四个元；

ω—运动载体坐标系相对参考坐标系的旋转角速度，且有：

采用和矩阵微分方程式求解相似的方法，求得：

其中，I—单位四元数。

（3）式为四元数微分方程式的解， 且有：

通过矩阵实时计算，由四元数可确定出运动体机体轴确定的坐标系M到导航坐标系E的坐标变换矩阵：

设运动体的航向角为Ψ，俯仰角为θ，横滚角为γ，选取导航坐标系为地理坐标系，地理坐标系可由三次基本旋转得到运动体的机体坐标系，其坐标变换矩阵为：

通过式（7）和式（8）可得：

通过上述分析可知：若表征坐标系变换的旋转四元数以确定，则按式（6）可计算出姿态矩阵，再按式（9）即可确定出运动体的航向角、俯仰角和横滚角[4～7]。

2、系统主程序设计

系统软件设计的主程序流程图如图7所示。首先进行的是系统初始化，主要任务有确定运动体的初始信息，如初始位置、初始速度等；数学平台的初始对准，以确定姿态矩阵的初始值；惯性器件的初始校准，对MPU-6050的各标度因数进行标定。然后进行姿态矩阵的更新计算，以提供运动体姿态以及计算姿态参数时的必要数据，这是算法中最重要的一部分。最后进行姿态计算，以获取个姿态参数，并对获得的参数进行提取验证。

五、测试结果及分析

本系统的测试过程是通过SGT320E型三轴转台来完成的，如图8所示，它是U—O—O结构，即台体由U形外框、O形中框以及O形内框组成，三轴均可连续无限旋转，既可单轴旋转也可多轴旋转，它可以精确地提供系统在工作期间可能经受的各种形式的角运动模式。当转台采用本机控制方式时，三轴均具有速率模式、位置模式和正弦摆动模式，通过串口仿真控制转台以设定的方式转动，可为待测系统提供更为复杂的仿真测试环境。

本系统的测试方法为设定三轴转台的旋转速率为10°/s和20°/s 对系统分别进行两次测量，得到两组测量数据，测量过程中各方向角单独测量，其他方向角置零位。俯仰角和横滚角的测量范围都是-90°～+90°，每10°一测，航向角的测量范围是0°～360°，每20°一测。经过测量得到俯仰角，横滚角，航向角的测量数据，各角度误差如图9、图10、图11所示，其中测量数据中第一组转台旋转速率为10°/s，第二组转台旋转速率为 20°/s 。

通过测量数据可知，系统俯仰角测量的角度误差基本保持在±2°以内，横滚角测量的角度误差除个别点以外基本保持在±1.5°以内，航向角测量的角度误差则较好地保持在±1.5°以内，且在转台不同的转动速率下，转动速率对系统的测量有一定影响，但测量误差基本都保持在一定的误差范围内。可见，系统对运动体姿态角度的检测具有较好的测量效果，系统设计可行。

六、结束语

本文应用惯性传感器模块MPU-6050，设计了一款高性价比、高稳定性、低功耗的运动体姿态角度测量系统，可实时测量显示运动体各姿态角的角度。实验结果显示，该系统测量角度较为准确，在各姿态角的测量过程中，最大测量误差不超过2°，且系统测量稳定性较高，达到了系统设计的测量目的。