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

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

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

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

图4 互补滤波器的原理图

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


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

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

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 融合前后角度比较图