基于MEMS器件的高精度动态倾角传感器研制

张 波，孙 哲，欧阳宁，闻小龙，田 跃

(北京科技大学数理学院北京市弱磁检测及应用工程技术研究中心，北京 100083)

0 引言

倾角传感器，是一种用于测量物体在静态或动态环境下与初始状态所成倾斜角度的测量工具，常用于航空航天、桥梁检测、自动控制等领域。目前常用的加速度式倾角传感器、导电液倾角传感器等在静态测量中表现出了较高的性能，然而由于在动态环境中引入了更多的干扰信号，使其测得的动态倾角并不正确[1]。目前测量物体动态运动条件下的倾斜角一般采用MEMS陀螺仪与加速度计作为惯性测量器件，利用互补滤波[2]、扩展卡尔曼滤波[3]等方法对其输出进行数据融合后解算姿态角。扩展卡尔曼滤波作为数据融合算法有着精度高的优点，但其计算较复杂。而互补滤波充分利用陀螺仪的动态响应能力与加速度计的静态精度进行数据融合，在低成本的动态系统中广泛应用。

本文选用低成本中较高精度的MEMS器件，设计了一种基于自适应互补滤波的动态倾角传感器，利用加速度计输出自适应调整互补参数，对比固定参数互补滤波精度有所提升，并且通过建立误差模型对MEMS惯性器件进行输出校正，进一步提高精度。

1 测量原理

倾角的变化，即物体姿态的变化。首先建立参考坐标系(常为东北天坐标系)以描述物体在空间中所处的位置，同时以物体固定三轴为载体坐标系。在导航系统中，常通过航向角Ψ、俯仰角θ及滚转角γ来描述一个物体的姿态。物体在空间中的姿态变化，即任意的空间转动，可表示为坐标转换，从而通过一个旋转矩阵来表示。根据欧拉旋转定理，1次空间转动可以通过3次独立的旋转来实现。如图1所示，载体坐标系与导航坐标系在初始时刻重合，当物体在空间中做一次转动时，可通过Z-X-Y的顺序进行3次的绕轴旋转来表示，每次转动可通过左乘旋转矩阵实现，3次旋转得到最终旋转矩阵为：

(1)

由式(1)可得，当已知姿态旋转矩阵时，可通过其反解3个姿态角，如下式：

(2)

表示姿态转换的方法主要有欧拉角法、九元素法及四元数法。四元数法较前两者在工程上有着广泛的应用，因其没有万向节死锁，可全角度表示旋转并且只有4个元素，解法较九元素简单，所以本文采用四元数法表示姿态转换。用四元数所表示的姿态转换矩阵为：

(3)

通过捷联惯导相关原理[4]，可得四元数微分方程使用一阶龙格库塔法解，可得:

(4)

式中Δt为时间间隔。

将陀螺仪测得的角速度[ωx,ωy,ωz]对四元数进行实时更新，即可得到3个姿态角。

2 系统设计

硬件系统设计主要由微控制器STM32F103C8最小系统电路、ICM20602惯性测量电路、RS232/RS485通信电路以及电源电路组成。由STM32F103C8微控制器通过I2C读取ICM20602输出，进行数据处理与姿态解算，再由RS232/485电路将数据包传输至上端设备，系统硬件框图如图2所示。

图2 系统硬件框图

2.1 STM32最小系统及其外围电路

本文选择STM32F103C8T6作为动态倾角传感器的主控芯片。它具有64KB FLASH、20KB SDRAM、4个TIMER以及37个通用GPIO口、两路硬件I2C等外设。LQFP64的封装使得传感器的尺寸能够进一步缩小，且本文所设计的动态倾角传感器对外设的要求仅为一路I2C接口及UART口，符合系统设计要求。图3为硬件系统STM32最小系统电路，包含晶振电路，复位电路及调试电路。

图3 STM32F103C8T6主控电路

2.2 ICM20602惯性测量电路

图4 ICM20602惯性测量电路

2.3 RS232/RS485通信电路

串口通信电路采用RS485芯片作为核心器件，通过9013 NPN三极管控制其使能发送接收端以控制输出，图5为传感器通信电路。

图5 通信电路

2.4 电源电路

对于大多数工业应用中采用的24 V供电，本文利用78L15先将24 V降为15 V，再利用MIC5202-3.3 V将供电电压稳压至3.3 V，对整体系统进行供电，图6为传感器电源电路。

图6 电源电路

3 软件设计

3.1 自适应互补滤波及角度解算

由式(1)～式(4)可知，可根据陀螺仪测量角速度更新四元数，而MEMS陀螺仪含有漂移误差随着时间积累，对积分得到的四元数中的累计误差将越来越大。互补滤波通过将式中的四元数矩阵中重力分量与加速度计输出的重力向量做向量叉积，得到加速度误差通过互补系数调整陀螺仪角速度误差融合至陀螺仪输出，从而达到传感器信息融合。其主要过程如下：

(5)

(6)

(7)

(8)

以上为固定参数互补滤波进行姿态估计的过程，当系统运动状态发生变化时，由于互补系数的固定，将出现估计值与实际值之间较大的误差。若对于互补系数kP的选择通过对当前加速度输出加以调整，将实现自适应变化运动状态下的姿态解算，其中自适应系数的算法为：

(9)

式中：eg=(||||acccal||-9.8||)2，0.01为多组实验数据得到的加速度计动静态输出方差阈值；acccal为校正后的加速度计输出；kP0为初始互补系数，初始取值为10，在静态情况下稳定修正陀螺仪输出，f(e)为自适应函数，根据加速度计与标准重力之间的误差增加而增加，最终使得互补系数kp缩减至0。

根据PI算法经典理论，当加速度计归一化得到的重力分量偏差与标准重力值偏差较大时，减小互补系数，输出取决于陀螺仪的动态响应，而偏差值较小时，增大互补系数，利用加速度计静态测量修正陀螺仪静态下的输出，自适应互补滤波算法流程框图如图7所示。

图7 自适应互补滤波流程图

在实时更新得到四元数后，对四元数进行解算得到各姿态角，在大角度即超过85°时解算将出现奇异值，且当俯仰角超过85°时旋转横滚角等效于旋转航向角，在姿态解算中可锁死航向角为零，从而避免奇异值的出现[5]，系统角度解算算法流程如图8所示。

图8 姿态角解算流程图

3.2 惯性测量输出校正

在姿态角算法流程输入加速度与角速度即可解算姿态角。大多数MEMS惯性测量芯片都会在出厂时进行输出校正，但对于低成本芯片其校正往往不够准确，大多数低精度传感器往往忽略该步骤，采用芯片内部校正，而高精度传感器则利用外部设备进行校正，但需要较长时间(多则长达几h)的参数离线计算。通过建立误差模型，对传感器进行静止，利用静止数据对模型进行参数拟合可实现无需外部设备的快速校正。对加速度计与陀螺仪输出建立如下误差模型[6]：

(10)

(11)

式中：acal、ωcal分别为校正过后的加速度与角速度；araw、ωraw为ICM20602的原始输出；Ta、Tω分别为加速度计与陀螺仪的轴正交误差旋转矩阵；Sa、Sω为尺度因子；aoffset、ωoffset分别为加速度计与陀螺仪的偏置。

gω_q(k)=f(Tω,Sω,ωoffset,ωk)

(12)

则定义目标函数为

(13)

同样通过最小二乘参数拟合得到陀螺仪误差模型参数解[βij,sgyro,i,ostgyro,i](i≠j;i,j=x,y,z)。惯性测量输出校正算法流程如图9所示。

图9 惯性测量输出校正流程图

3.3 系统软件设计

系统软件设计包括自适应滤波的程序实现、分段解算算法程序、ICM20602器件的初始化配置程序、定时器初始化、定时器中断程序、回调程序、RS232/485通信程序、I2C总线读写程序。首先系统上电后执行复位操作及系统初始化，然后通过I2C读写操作对ICM20602进行初始化配置，在接收到ICM20602初始化成功标志后，启动定时器，在定时器达到2 ms或10 ms标志时，在中断服务函数中分别调用回调函数，2 ms中断时回调函数为进行读取数据，输出校正，数据融合及角度解算，在10 ms中断时回调函数进行角度数据根据通信协议封装为数据包，通过RS232/485发送数据至上位机，整个软件流程框图如图10所示。

图10 传感器软件框图

4 实验验证

首先对加速度计与陀螺仪原始输出进行校正，将传感器6个面反转13次，每次翻转之间静置间隔从2～10 s不等，整个校正过程共50 s左右，得到陀螺仪与加速度计原始输出，如图11所示，拟合误差模型参数如表1所示。

表1 拟合误差模型参数结果

(a)加速度计

其中默认初始时传感器X轴与校正平面X轴重合，故αxz、αxy、αyx为0。将未经校正、数据融合采用定参数的传感器和本文所设计的校正后采用自适应互补滤波的传感器固定于三维转台，对动态倾角传感器进行对比测试，其测试结构如图12所示。

图12 测试结构示意图

随意旋转三维转台，对传感器输出进行记录，得到旋转情况下的角度输出如图13所示。

(a)定参数与自适应传感器横滚角对比

由图13可知，自适应互补滤波传感器可以在旋转动态环境下更好地适应旋转引起的加速度计突变，减少角度异常突变情况，更加准确的计算角度，如图13中小图放大所示。将传感器静置10 min，得到其静态条件下输出误差曲线如图14所示。

(a)横滚角输出误差

由图14可得，自适应&校正传感器在静态条件下输出噪声幅度小，自适应互补滤波能够在静态条件下，能够更好地通过加速度修正陀螺仪的随机漂移，由数据计算得其噪声方差如表2所示，由表2可知，校正后自适应传感器稳定性更好。

表2 静置条件下传感器噪声方差

通过对各轴旋转不同角度，读出传感器输出后，得到误差值绘制图如图15所示。

图15 传感器各角度误差绝对值曲线

由图15可得，本文所设计的传感器倾角的精度在0.36°以内。

5 结束语

本文设计了一种基于ICM20602惯性器件的高精度动态倾角传感器，建立误差模型，利用参数拟合方法进行自校正其原始数据，并根据加速度计在互补滤波中低频稳定的特性，设计一种根据加速度计输出自适应的互补滤波进行数据融合。通过与未校正及定参数互补滤波的动态倾角传感器对比实验，得出本文设计的校正输出后利用自适应互补滤波融合数据的动态倾角传感器静态稳定性较好，动态对加速度计畸变抑制能力强，精度为0.36°以内，且硬件成本较低，可广泛应用于工业领域。