基于六轴MEMS器件姿态测量系统

2021-06-30 12:44高国伟

计算机测量与控制 2021年6期

景希,高国伟,2

(1.北京信息科技大学传感器北京市重点实验室，北京 100101;2.北京信息科技大学现代测控技术教育部重点实验室，北京 100101)

0 引言

随着MEMS技术的日益发展，MEMS(micro electromechanical system)的加工工艺应用在一些惯性器件，陀螺仪是一种能够敏感载体角度或角速度的惯性器件[1]。传统的陀螺仪是利用高速转动的物体具有保持其角动量的特性来测量角速度的，传统机械陀螺仪由于在制作上成本较高，而且体积较大，寿命短，不适合用于批量生产，限制了其应用，一般仅用于导航方面，而MEMS陀螺仪利用科里奥利力[2]，可以感知载体的转动角速率，有更好的性能，微细的加工工艺大幅度地减小了体积，降低了生产成本，功耗低，质量轻，拓展了应用领域，除了复杂的导航、军工领域，在摄像机、运动机械、汽车等领域也发挥了较大的作用[3]。

照相机拍摄时需要稳定的拍摄环境才能有清晰的拍摄效果，目前除了照相机内部有防抖动模式和动态模式，还有很多工具能够帮助稳定相机，比如豆袋、吸盘支架、三脚架等。三脚架是稳定相机最好的装置，性能较高的三脚架价格也比较昂贵，一般用于专业影视剧拍摄，比如美国欧克诺品牌一款云台三脚套装，金额高达十六万。本文讨论捷联式测量系统，将惯性器件陀螺仪和加速度计模块固定在三脚架上，可以测量三脚架静态姿态，保持其稳定拍摄。

1 系统结构及原理

1.1 MEMS陀螺仪工作原理

MEMS陀螺仪的主要原理是通过科里奥利加速度，在驱动模式和检测模式之间转移能量。陀螺仪通常有径向和横向两个方向的电容板，径向电容板使物体做径向运动，横向的电容板测量由于横向科里奥利运动带来的电容变化，从而得到科里奥利力。因为科里奥利力正比于角速度，所以由电容的变化可以计算出角速度[4]。

1.2 姿态检测优点及发展趋势

姿态检测系统不依赖外部信息，并且具有良好的隐蔽性，不向外辐射能量，也不受电磁干扰影响，它能够提供各种不同的数据，比如位置、航向、速度和姿态角，提供良好的连续性和低噪声的导航信息，数据更新率高，稳定性好[5]。

陀螺仪和加速度计是惯性制导系统中必要的两个核心装置[6]。随着需求精度的提高，对陀螺仪和加速度计提出了更高的要求。陀螺仪的漂移误差和加速度计的零偏差影响着整个系统精度[7]。陀螺仪和加速度计测量精度的提高和性能的提高一直备受关注。我国惯导技术已经逐渐发展，有了飞跃性的进步，利用捷联惯导技术对采煤机进行定位，液体浮动陀螺平台系统和动态调优陀螺四轴平台系统，已应用于世界著名的长征系列运载火箭。

1.3 系统框架

根据系统的功能需求，姿态测量又陀螺仪模块和加速度计输出模块，系统由两个部分组成：上位机和下位机。下位机是一个硬件结构。它主要由MEMS传感器、STM32微控制器和通信模块组成，负责数据的收集、处理和传输。MEMS陀螺仪和加速度计形成精度高、抗振动能力强的姿态模块，并能实时准确地获得物体运动中的角速度和加速度数据。

通信模块将接收到的数据传输到上位机，完成系统之间的通信，通信模块选用RS232串口作为系统与上位机的连接。

系统原理如图1所示。

图1 系统原理框图

系统采集到三轴加速度计和三轴陀螺仪的信号，通过SPI串口接受加速度计和陀螺仪的信号，通过时钟频率设置计数频率以及采样频率，控制器内部对读取到的数据信息进行数据滤波[8]、解算处理，然后将控制器与上位机以RS232串口相连接，在上位机中显示输出数据，输出为处理后的加速度计数据和三个陀螺仪数据。

2 系统硬件设计

2.1 ADXL355加速度器

选用ADXL355型号的加速度器，其封装能够保证长期的稳定性，低功耗，噪声低，使获得的数据信息比较稳定，工作温度范围：-40～125 ℃，分辨率高(256 000/g)，内部集成20位ADC,具有数字串行外设接口SPI接口，便于加速度器与其他传感器和主控制器相连接，更好地进行通信，提高可靠性。加速度计的SPI通信接口引脚与控制器STM32引脚PB0连接，SPI接口中的MOSI、MISO、SCK、分别接STM32的SPI总线。ADXL355原理如图2所示。

图2 ADXL355原理图

2.2 STM32F103RBT6介绍

姿态测量以STM32F103RB为核心，是一种功耗低性能高的微控制器，控制外围电路以及完成对数据的处理。供电电压为2.0 ～3 V,以高达72 MHz的CPU频率工作，能够高速处理数据，快速解算姿态角，运用了串行外设接口(SPI)，支持三线全双工同步传输，内置的I2C、USART 模块可以简化程序设计、减轻控制器的工作强度。STM32具有12位模数转换器，将敏感器件模拟信号转化为数字信号，输入时钟经过PCLK2分频产生8 MHz。

单边机正常供电需要3.3 V电压，所以我们选用ASM1117-3.3 V的稳压芯片稳压，为确保AMS1117的稳定性，输出需要连接一个10 μF的钽电容。5 V电压输入AMS1117进行转换为3.3 V，如图3所示。

图3 MCU供电模块

2.3 陀螺仪介绍

ADXR450芯片系统是一款角速率陀螺仪，由陀螺芯片、BOOST升压电路和滤波电容构成，分辨率高，噪声低，0.015°/(sec/g)，采用差分式传感器设计，可以抑制线性加速度的影响，数字串行外设接口SPI接口，便于陀螺仪和主控制器相连接，更好地实现通信，通信接口与控制器具有SPI串口功能的引脚连接，提高可靠性，每个传感结构都包含了一个抖动框架，该框架通过经典驱动实现共振，这就产生了必要的速度元素，在经历角速度时产生科里奥利力，感知科里奥利力的电容式传感器结构将所得信号馈送到一系列的增益和解调阶段，这些阶段产生电速率信号输出。

本次使用的是一款单轴陀螺仪，因此需要用到三个陀螺仪分别测量X、Y、Z三个轴方向的角速率。在电路板中放置时，应确保使三个陀螺仪敏感轴相互垂直，有利于提高敏感轴输出值的准确度。将测量X轴陀螺仪芯片10号引脚与控制器PC4相连接，测量Y轴陀螺仪芯片10号引脚与控制器PB1相连接，测量Z轴陀螺仪芯片10号引脚与控制器PB11相连接，每个陀螺仪的3、5、12引脚分别与控制器PB13、PB14、PB15引脚连接，完成信号的通信。该陀螺仪电路如图4所示。

图4 陀螺仪电路

3 系统软件设计

3.1 坐标变换及姿态角

惯性测量单元固定安装在运动物体上，将运动物体作为研究对象，确定运动物体的姿态的坐标系称为载体坐标系b，导航坐标系n是不变的，将导航坐标系n分别绕Z轴X轴Y轴旋转Ψ、θ、γ就得到载体坐标系，姿态角指两个坐标系各轴之间夹角。探究载体坐标系和导航坐标系的方位关系就可以确定研究对象的姿态，将导航坐标系分别绕Z轴X轴Y轴旋转Ψ、θ、γ，每旋转一次对应的矩阵变换可以表示为：

(1)

(2)

(3)

三次旋转后载体坐标系和导航坐标系的位置可以由方向余弦矩阵表示:

(4)

即实现导航坐标系转换为载体坐标系：

(5)

Cbn=CnbT=

(6)

即实现载体坐标系转换为导航坐标系：

(7)

(8)

(9)

θ=acrsin(R32)

(10)

(11)

由此可以计算出姿态角数据。

3.2 姿态测量

姿态检测系统是指量测各传感器的信息,通过数据处理解析出载体的姿态、速度和位置等信息,提供给检测控制系统,达到对载体的感知[9-10]。使用捷联式安装，测量技术的原理是将陀螺仪直接固定到需要测量的物体上，陀螺仪测量在惯性空间中3个方向轴旋转的角速度[11]，对速度进行积分以获得姿态信息，然后根据坐标变换计算得出数据，将数据转换为系统坐标，获得新的坐标系统中的姿态信息，从而可以判断平台的稳定性[12]。

将陀螺仪固定在照相机三脚架上，先测量静态时由于重力所得角速度值，然后再测量动态时由于照相抖动或外界因素造成的抖动大小。

姿态测量系统用模块化方式，包含了系统初始化、采集传感器信号模块和姿态角解算模块。数据采集模块包含了加速度计陀螺仪和控制器的模数转换依据数据读取模块。姿态解算系统工作流程如图5所示。

图5 姿态解算系统工作流程图

3.3 滤波算法

卡尔曼滤波算法是一种利用递推的方法估算系统运动过程的状态，状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，能够实现实时运行状态的估计和预测功能。

系统状态空间模型：

X(k)=AX(k-1)+BU(k-1)+W(k-1)

(12)

Z(k)=HX(k)+V(k)

(13)

X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数矩阵。Z(k)是k时刻的测量值，H是测量系统的参数矩阵。W(k)和V(k)分别表示过程和测量的噪声，并假设它们是相互独立并满足正态分布。

卡尔曼滤波算法：

时间更新方程：

X(k|k-1)=AX(k-1|k-1)+BU(k)

(14)

P(k│k-1)=AP(k-1|k-1)A′+Q

(15)

校正：

(16)

X(k|k)=X(k|k-1)+K(Z(k)-HX(k|k-1))

(17)

P(k|k)=(I-KH)P(k|k-1)

(18)

陀螺仪噪声的存在对结果会有较大的影响，如果信号不加处理直接输出，会有较大的误差，不能准确地获得精准的角度，因此需要对数据进行卡尔曼滤波，获取在静止时刻器件信号输出的数据。

RC滤波算法：

Xout0=Xout1+(Xin-Xout1)/num

(19)

Xin是输入值，Xout1是上次滤波后的输出值，Xout0是本次滤波结果，num是和RC值有关的一个数值，称之为滤波系数，num的值决定新的采样值在本次滤波结果中所占的比例的大小。当前采样的输入值对滤波起到修正作用，滤波结果主要依据上次滤波后的输出值。滤波系数num越小，响应快，灵敏度较高，但滤波结果越不稳定，滤波系数num越大，响应慢，滤波结果平稳，但是灵敏度低。

4 实验结果与分析

4.1 实验方案

为了测试传感器准确性，将传感器稳定置于三轴转台上，上电预热十分钟，控制三周专挑，首先令内轴、中轴、外轴均为0°，控制内轴中轴位置方式，速率方式和加速度速率，令速率方式为10°/s，加速度速率为10°/s2，将内轴角度依次设置为-20～20°，每10°旋转一个角度，中轴设置为0°，然后中轴角度依次设置为-20～20°，每10°旋转一个角度，内轴设置为0°。记录每个角度下传感器输出角度，利用误差模型计算传感器数据，然后代入公式分别解算X轴和Y轴的姿态角。

4.2 实验结果

测试静态下传感器输出角度，将三轴转台设定内轴为43.5°，中轴设定为0°。分别用3种方式进行数据处理。图6为加速度计X轴原始数据输出结果，该曲线表示在静态环境下传感器滤波降噪前的数据输出，分析曲线可知，传感器输出最大值为69.9°，最小值为23.8°，和设定值43.5°相差甚远，角度偏差过大的原因可以考虑为在某一时刻转台因人为因素受震动导致的数据偏差，除了人为因素导致的偏差数据段，其他时刻的数据输出约在38～50°，最大偏差角达到了6.3°，最大偏差达到了14.94%，数据输出不稳定，波动幅度大。原始数据如图6所示。

图6 原始数据

图7是对加速度计X轴原始数据进行卡尔曼滤波处理后数据输出，令过程噪声Q=0.018，分析曲线可知，传感器输出最大值为54.9°，最小值为32.7°，经过卡尔曼滤波后，因人为因素导致的数据波动较原始输出数据波动减少了23.9°，可见卡尔曼滤波效果显著，体现该滤波算法实时运行状态的估计和预测功能。其他时刻的数据输出约在41°至46°，最大偏差角达到了1.4°，最大偏差达到了3.2%，数据输出仍有波动，较原始波动性小，受波动影响较大，但数据响应较快。

图7 卡尔曼滤波

图8是对加速度计X轴原始数据进行一阶RC滤波的结果，其中系数num为100，图9是经过一阶RC滤波的结果，系数num为1 000。RC滤波对数据处理的速率较缓，num为100时数据稳定需要11 s,而num为1 000时数据稳定需要72 s，其稳定速率受系数num影响，num越大，数据达稳定较慢，图8中，震动情况下，传感器输出最大值43.774°，最小值42.562°，经过一阶RC滤波后，因人为因素导致的数据波动较原始输出数据波动减少了44.888°，震动时刻对整体滤波结果影响微乎其微。其他时刻的数据输出约在43.604～43.510°，最大偏差角达到了0.104°，最大偏差达到了0.24%，数据波动幅度小，稳定度较高。经过一阶RC滤波，系统响应快，灵敏度较高，滤波结果越稳定，选取系数num应适合于系统实际应用情况。

图8 RC滤波num=100

图9 RC滤波num=1 000

综合分析得卡尔曼滤波性能动态响应快，实时性能较好，但滤波效果不理想，这是由于选取滤波参数量测噪声和过程噪声系数而造成的，在时间更新方程中，由于对过程噪声的不确定，对状态预测时，将量测噪声设置成0。

实验中，将状态转移矩阵设定为单位向量，过程噪声参数Q设置为0.018，参数的选取取决于硬件设施和外部环境等因素，需要结合经验合适的选取，难于合适的选取，卡尔曼滤波具有比较好的实时性，而实验中对实时性要求不高，所以考虑了一阶RC滤波。经过对比发现，当系数为100时，一阶RC动态响应较快，而且滤波效果也能达到满意的效果，当系数为1 000时，滤波效果更加平稳，经过滤波后的数据和原始输出数据相比较，由于系统输出响应太慢，缺失了很大一部分数据，不能够快速的对数据进行处理，反映出经过RC滤波降噪后的输出噪声数据受到了极大的抑制，该系统选取num=100。

4.3 误差分析

在本文中讨论的MEMS陀螺仪采用的是ADXR450型号，该陀螺仪用作检测加速度和加速度的敏感性元器件，MEMS陀螺仪的精度随环境的变化而产生误差，误差来源主要是两个方面：第一个方面是由于室外温度的变化是陀螺仪产生干扰力矩；第二个方面是由于照相机拍摄时，地球的自传和路面的平缓度以及拍照瞬时造成的抖动，对陀螺仪产生干扰。随机漂移造成的误差，由零偏和速度随机游走造成，零偏是传感器在零输入时系统由自身原因而产生的输出值。系统漂移，由预热偏差和温度敏感造成，还有交叉耦合误差和轴失准误差。在环境变化较大时测量势必会造成检测值的降低，所以首先需要对误差进行分析，了解误差来源，然后对数据进行测量分析，建立误差模型，进行校准，最后尽可能得到实际测量的角度。

5 结束语

基于STM32单片机的姿态测量系统，以STM32微控制器核心芯片，搭配加速度计模块和陀螺仪模块和通信模块，文中介绍了MEMS器件的工作原理以及系统的硬件内容分析和软件部分，对测量数据的误差进行了分析。实验采用的单轴陀螺仪测量角度，加速度计测量加速度值，分别比较两种不同的数据处理方式，得到了较理想的结果。但是误差标定实验是在实验室常温环境下完成，忽略了温度对敏感元器件的影响，在实际运用中由于温度对敏感元器造成的漂移情况不能忽视，因此可以加入温度补偿，标定不同温度下的温度影响因子，这是一个可以改进传感器精度的方面。随着微电子技术和集成电路的发展，除了软件温度补偿方式以外，可以在传感器敏感元器件外围增加恒温控制电路模块，从硬件电路角度改进温度漂移产生的误差。