基于互补滤波器的两轮平衡车姿态角度测量

微机电系统（Micro—Electro—Mechanical Systems，MEMS）陀螺仪和MEMS加速度计在两轮平衡车姿态测量中存在扰动和噪声，引起姿态角度测量误差。通过对陀螺仪和加速度计输入信号进行滑动扣除均值方法来抑制直流分量，利用滑动滤波算法抑制加速度计高频噪声，引入互补滤波算法将预处理后的陀螺仪和加速度计信号进行融合，得到更加准确稳定的角度测量值，分析了融合算法中加权因子与滤波频率特征之间的关系。该方法应用到两轮平衡车的运行姿态角度控制中，提高了对姿态角度测量的精度。

两轮平衡车具有广阔的应用前景，使其成为了当前研究的热点。其中，两轮平衡车的姿态角度测量是研究的关键问题之一。姿态角度测量是两轮平衡车运行和控制实现的前提。姿态角度测量的精度和速度，将直接影响两轮平衡车控制算法的稳定性和可靠性。随着惯性测量元件的微型化与微处理器运算能力的提高，两轮平衡车姿态测量普遍采用低成本的惯性测量组合元件（Inertal Measurement Uint，IMU），结合微处理器数据处理算法实现高精度的姿态测量。IMU主要由低成本的MEMS陀螺仪和三轴加速度计组成。MEMS陀螺仪有自主性好、功耗低、机电性能好易集成等优点。但是，MEMS陀螺仪具有温度漂移特性，其测量误差会随着时间的累加而不断的累积，从而影响测量精度。加速度计会受到平衡车振动的影响，混叠额外的振动量干扰。所以单一的传感器测量难以得到精确的姿态角度。需采用多传感器信号融合的方法，来获得准确的姿态角度量。

多传感器数据的融合方法有神经网络、小波分析、卡尔曼滤波等姿态解算算法，但这些方法建立稳定可靠的更新方程通常具有较高的阶数，且计算量大，不适合于低运算能力系统的实时计算。相比以上方法，互补滤波算法对处理器运算速度要求不高，且简单可靠。本文基于互补滤波算法，设计了两轮平衡车姿态角度测量电路与数据处理算法，设计了信号滤波预处理，利用互补滤波算法融合两种传感器数据，分析了互补滤波算法中关键参数的计算方法。并将此方法应用于两轮平衡车角度测量，进行了验证性试验，给出了实验测试数据。

1姿态角度测量原理

沿平衡车3个机体轴即直立时正前、正右、正上方向定义为x、y、z三轴参考坐标系。所受的3轴重力加速度分量定义为gx、gy、gz.假设两轮平衡车处于静止或匀速运行的状态。得到重力加速度与平衡车姿态角度的关系如式1所示：

其中，为惯性坐标系到载体坐标系的变换矩阵：θ为俯仰角；φ为横滚角；g为重力加速度；可以通过测量重力加速度分量gx、gy、gz，计算出平衡车俯仰角θ1和横滚角φ1.估计值

若使用陀螺仪来测量平衡车姿态角度，设陀螺仪测量载体相对惯性坐标系的x、y、z三轴旋转角速度分别为ωx、ωy、ωz.并定义O时刻平衡车直立静止。可得到俯仰角θ2和横滚角φ2估计值与ωx、ωy之间的关系如式3所示：

在实际应用中，由于平衡车机体运行时存在运动加速度、测量噪声，以及陀螺仪本身存在漂移等因素的影响，式（2）、（3）姿态角度测量方法失效，为了准确的获得姿态角度。可将以上的2种姿态角度测量得到的姿态角度信息相融合。

2惯性组合测量电路

该系统中惯性组合测量电路如图1所示，由加速度计MMA7361、陀螺仪ENC-03及放大电路组成。实现对加速度计和陀螺仪测得信号进行放大。加速度计和陀螺仪信号经放大，分别由angle引脚和gyro引脚输出后，信号通过AD采样转换为数字信号，传递到微控制器中，再利用互补滤波算法，得到姿态角度。

图1 惯性组合角度测量电路图

3陀螺仪漂移和加速度传感器数据的预处理

MEMS陀螺仪的漂移误差由常值漂移、随机漂移组成，漂移信号不符合平稳、正态、零均值的时间序列特征。图2为陀螺仪采集到的一段数据，对其积分后得到的俯仰角θ2，表现为误差不断累加，逐渐增大。通过式（4）所示算法进行均值扣除。

其中，ωgy（n）为第n次的陀螺仪角速度值；ω（n）为第n次去除均值后的角速度值；N为缓冲数据个数。[page]

图2 角速度与角度的曲线

去掉陀螺仪信号的均值（即常值分量）后，这样陀螺仪的漂移只含有随机漂移，此时的陀螺仪信号将作为互补滤波融合的输入。

加速度计受机体振动的影响，对其采用滑动均值滤波的方法对加速度传感器原始数据进行处理，滤波原理如式（5）所示：

其中，θocc（n）为加速度计第n次测得的角度值；θ1（n）为第n次滑动滤波后的角度值：N为滑动窗阶数。图3给出了滑动滤波前后的加速度计测得的角度值，从图3中可以看出，滑动滤波算法对高频扰动具有滤除效果。

图3 滤波前后加速度计数据图

4基于互补滤波器的姿态角度测量设计

陀螺仪动态响应特性优良，解算姿态角时，由于陀螺仪低频漂移的影响，积分后低频扰动会产生较大误差；加速度计解算的姿态角会受到平衡车运行中机体高频振动的影响，输出角度中携带较大分量的高频干扰。二者在频域上具有互补特性，采用互补滤波器对这两种传感器数据融合，可提高姿态角度测量的精度和动态响应的性能。

互补滤波器的基本原理图如图4所示。

图4 互补滤波器的原理图

其中θ为实际的角度值，ω为陀螺仪测量的角速度，θ为互补滤波算法后估计的角度值，加速度计测量中引入的高频噪声n1，陀螺仪测量中引入的低频噪声n2，用低通滤波器G1（s）消除加速度计中的高频噪声n1，用高通滤波器G2（s）消除陀螺仪中的低频噪声n2.两个滤波器的传递函数，被设计为（6）、（7）所示，


选用的滤波传递函数需满足G1（s）+G2（s）=1，由加速度计得到θ1经低通滤波器和陀螺仪得到θ2经高通滤波器后的数据融合为角度估计值，适当的选取权重因子K值，可以使系统中高、低通滤波器具有合适的截止率。得到稳定的姿态角度。

由（8）式可看出，在小于截止频率的低频段，加速度计对姿态解算结果起主要作用；在大于截止频率的高频段，陀螺仪对姿态解算结果起主要作用。通过调整时间常数，改变滤波器的截止频率τ，实现对陀螺仪和加速度计权重的调整。[page]

5实验验证

为了验证上述设计方案的可行性，利用直立两轮小车为实验验证平台。本实验使用惯性测量组合元件（IMU）中的惯性传感器分别选用了ENC—03（测量范围：±300（deg/s））陀螺仪，采样频率为1.25 kHz和MMA7361加速度计（测量范围：±1.5 g）。角度更新频率为1.25 kHz.互补滤波器截止频率为138 Hz.以俯仰角（θ）为例，进行了测试。

5.1角速度和角度用互补滤波算法融合的分析

把陀螺仪测得的角速度数据和加速度计测得的角度数据通过（10）式进行融合后的波形如图6所示，从图6中可以看出，经互补滤波算法融合后得到角度消除了陀螺仪的漂移和加速度计的高频扰动，可得以下结论：

图6 角速度与角度的融合曲线图

1）单从陀螺仪获取的角速度积分后得到的角度是不正确的，要把加速度计测得的角度值和陀螺仪测得角速度积分后的角度进行互补滤波算法融合，提高角度精度。

2）经互补滤波后陀螺仪的随机漂移得到较为明显的抑制，表现出了此互补滤波算法的有效性和优越性。

5. 2经互补滤波处理后的角度与没有经处理后的角度比较分析

经互补滤波处理后的角度与直接由加速度计测得的角度时域比较，对其互补滤波处理前后的信号进行FFT变换其频谱图如图7所示，通过互补滤波算法可以降低随机噪声的干扰，可以使测得角度的波形更加的平滑。

图7 融合前后角度比较图

6结论

文中分析了两轮平衡车姿态角度解算时陀螺仪漂移和加速度计高频扰动的影响，针对陀螺仪漂移和加速度计高频扰动采用互补滤波融合加速度计和陀螺仪信号。互补滤波能有效消除陀螺仪的漂移，抑制加速度计的高频扰动，减少输出姿态角的动态误差，提高了角度测量精度，能够满足两轮平衡车的姿态控制需要。实验结果表明了该方法的有效性，可推广应用于车载导航、两轮平衡车、微小型机器人的姿态角度测量系统。