机器人姿态估计-IMU、互补滤波算法应用

附赠自动驾驶学习资料和量产经验：链接

机器人的姿态测量对于许多应用至关重要，如导航、运动控制等。在这篇文章中，我们将介绍如何利用MPU6050传感器以及互补滤波和卡尔曼滤波算法来实现自平衡车的姿态测量。我们将从原理出发，逐步介绍互补滤波和卡尔曼滤波的基本原理，并提供相应的C代码实现。

原理

我们需要测量机器人的姿态，这里是自平衡车的倾斜角度。对于机器人姿态等运动状态物理量，我们一般采用惯性传感器来直接测量。

主要包括加速度计（Accelerometer）、陀螺仪（Gyroscope）和磁力计（Magnetometer）。这些传感器通过测量物体的加速度、角速度和磁场强度等信息，来获取物体在空间中的运动状态，如位置、速度、加速度、角度等。

惯性传感器

当然这种传感器有很多种，这里我们使用MPU6050。MPU6050是一款集成了三轴陀螺仪和三轴加速度计二合一的传感器。如下图：

MPU6050

其中加速度计用于测量物体由于地球重力或者物体运动所产生的加速度，即物体在单位时间内速度的变化率。陀螺仪用于测量物体的角速度，即物体在单位时间内的旋转速度。通过积分陀螺仪输出的角速度数据，可以获取物体相对于初始位置的旋转角度。

利用加速度计测量倾斜角

我们通过I2C接口可以读取到MPU6050 X,Y,Z三个轴方向上的加速度实际测量数据。

那么我们如何可以利用三个轴的加速度数据算出倾斜角度呢？

理想情况下，我们知道如果将小车静止不动，这个时候机器人或者传感器就只受到地球重力作用，如上图，此时MPU6050水平，这时候只有Z轴方向上有加速度，而且是地球重力加速度g，其它两个轴X或者Y上加速度都是零。当小车有一定倾斜角的时候，地球重力会在X或者Y轴方向上有分力，也就是地球重力加速度g会在X或者Y轴上有分量，而且分量大小和倾斜角度β相关。如下图所示：

g分解

其中β=arctan(x/z)或者β=arctan(y/z),通过这个反正切计算我们就可以得到倾斜角度β。

至于说具体是用X轴还是Y轴数据来算,这个要看MUPU6050在平衡车上实际安装方位来看。如果MPU6050安装方式是X轴与小车轮轴平行，Z轴朝上，那么g就分量到Y轴上；

如果MPU6050安装方式是Y轴与小车轮轴平行，Z轴朝上，那么g就分量到X轴上；

是不是觉得到此应该结束了？这不是已经测得倾斜角度了吗？

我们来看一下加速度计测量的在小车实际运动过程中倾斜角度的数据。如下图：

加速度计测量的倾斜角度β

我们可以看到为什么有很大毛刺，因为上面反正切计算方法有个理想前提条件大家不要忘了，就是我们机器人只是受到重力作用，但是实际上，当小车运动的时候其实是有运动加速度a的，也就是说不单纯是重力加速度g，还有其他运动加速度的累加。如下图：

运动加速度累加

也就是说，在没有运动加速度理想情况下，X/Y轴加速度测量值是sinβg，而实际运动情况下，测量值就是sinβg+cosβa，这样自然就不对了，所以上面数据出现毛刺。线性加速度计是无法避免运动加速度对角度测量的影响。

所以行业上一般说，加速度计的动态性能不太好，静态性能好就是这个意思。

好，那既然加速度动起来时候测量效果有问题，那么怎么解决呢？下面我们来看看陀螺仪能否测量吧！

利用陀螺仪测量倾斜角度

陀螺仪是利用高速回转体的动量矩敏感壳体对自转轴的角运动检测装置，它输出的是围绕XYZ三轴旋转的角速度值。MPU6050内部除了加速度计，还集成了三轴陀螺仪。

由于陀螺仪输出是围绕三轴自转的角速度，所以不会受到小车或者机器人运动的影响。我们通过对其输出的角速度进行积分就可以得到角度，这样我们通过陀螺仪也可以测出小车倾角了。

例如，如果我们对陀螺仪的采样周期是5ms，也就是0.005s，如果小车发生倾角，其实就是MPU6050围绕平行于轮轴的那根轴自旋转角度。那么通过下面积分公式可以算出某一轴旋转角度值：

Angle_X = Angle_X + Gyro_X*0.005

其中，Gyro_X就是陀螺仪传感器平行于轮轴的轴角速度值。Angle_X表示旋转的角度。

但是陀螺仪测量的角速度值如果存在误差或者噪音，经过长时间积分之后，误差是会累积的，时间长了就会出漂移了，比如说传感器静止情况下输出也不为零，随着时间增长，误差也会逐渐增大，输出的角度值就不对了。如下图白色曲线所示。

陀螺仪漂移问题

融合算法

通过上面分析过程我们知道，加速度计和陀螺仪其实都可以单独测量倾斜角度值，但是各自有自己的缺点，那到底怎么解决这个问题呢？如果才能得到一个比较理想完美测角度测量值呢？

其实解决思路也很简单，那既然这两个传感器有自己的缺点，那我们就把他们的测量值做一个结合，互补一下，输出一个融合后的值不就完了。

那到底怎么样融合呢？目前工程上用的比较多的就是互补滤波和卡尔曼滤波算法。

互补滤波

先来看看互补滤波算法思想其实非常简单，就是既然两个传感器的测量值都有各自问题，那就分别给这两个值前面乘以一个权重系数，也就是两个传感器分别贡献一部分，其实就是加权平均。公式如下：

angle = k*angle_m + (1-k)*(angle+gyro_m*dt)

其中，angle是融合之后角度值，angle_m是加速度计测量的角度，angle+gyro_m*dt就是陀螺仪积分得到的角度，dt为采样周期，k就是互补滤波权重系数，如果想要加速度计的测量结果对最终结果影响大些，那就把k设置大些，反之，设置小点就是让陀螺仪的测量值对最终结果影响大些。

下面就是具体arduino C++实现代码：

C++代码实现

代码比较简单，RAD2GRAD是弧度转成度。陀螺仪数据除以65.5，是为了从寄存器数值转成度单位，数值得来由陀螺仪传感器量程决定，这里我们不讲MPU6050怎么使用，不清楚建议自己好好学习一下相关资料。

融合效果

上图是我们将小车或者MPU6050静置桌面，轻拍桌子的情况下测得的实际效果数据。图中白色线是陀螺仪测量角度，黄色线是加速度计数据，而红色线是互补滤波融合之后的数据，可以看到互补滤波还是很理想的。

卡尔曼滤波算法的实现我们后续文章来讲。