IMU 计算 yaw，pitch 和roll

IMU 计算 yaw，pitch 和roll

IMU是机器人领域中一种很重要的传感器，可以用于许多不同的用途。在，视觉slam，机器人导航，VR头盔中都有它的身影。
不管要搞VSLAM还是机器人导航，我TM都绕不过这个传感器。所以，这篇文章主要是想记录一下看到的一下IMU的资料，以防自己以后忘记了。
由于在市面上IMU的价格幅度变化很大，每个IMU也有很大的不同。这篇blog的IMU默认为是拥有9DOF的IMU， 比如MPU6050。
下面开始进入正文了哈。

一般情况下，IMU有三个部分组成：

• 加速度计（Accelerometer)
• 陀螺仪 (Gyroscope)
• 磁力计(Magnetometer)

加速度计(Accelerometer)

加速度计这个东西告诉你，当前时刻在x轴，y轴和z轴上面的加速度是多少？ 这个时候如果你把一个IMU放在一个平面上不动，那个它只会收到重力的影响，当然这里不考虑噪声的影响。
根据高中的力学基础我们可以知道，如果重力是指向地面的，并且和地面垂直。所以我们可以得出，IMU的三个轴上面的加速度的和也一定指向地面且大小为恒定的地球的重力。
然后根据x，y,z轴之间的关系，可以同步算出IMU的roll 和pitch的值是多少？
$$\begin{gathered} pitch=180\ast atan2(accelX,sqrt(accelY\ast accelY+accelZ\ast accelZ))/PI; \\ roll=180\ast atan2(accelY,sqrt(accelX\ast accelX+accelZ\ast accelZ))/PI; \end{gathered}$$

陀螺仪 （gyroscope)

陀螺仪这个东西可以告诉你，当前时刻x轴，y轴和z轴的角速度是多少？知道了角速度之后，我们就可以对角速度进行积分，就可以算出IMU在一定时间内的roll， pitch和yaw 值，是不是很爽？ 太天真，真实的情况是，陀螺仪的数据一般有很大的误差，所以通常是不用陀螺仪得到IMU的roll，pitch和yaw值的。

那么问题来了，IMU的yaw 值一般是怎么计算的呢？
这个就要引出磁力计这个东西了。

磁力计 （Magnetometer)

磁力计这个东西它可以感受到地球的磁场，用它可以帮助我们找到自己的朝向，有点像指南针，罗盘之类的东西。然后呢，它可以计算IMU的yaw值（其实公式我TM也不懂）
$$ma{g}_x=magRea{d}_X\ast cos(pitch)+magRea{d}_Y\ast sin(roll)\ast sin(pitch)+magRea{d}_Z\ast cos(roll)\ast sin(pitch)$$
$$ma{g}_y=magRea{d}_Y\ast cos(roll)-magRea{d}_Z\ast sin(roll)$$
$$yaw=180\ast atan2(-ma{g}_y,ma{g}_x)/\pi ;$$

总结一下：

IMU 部件	作用	备注
加速度计	告诉我们当前时刻x轴，y轴和z轴的加速度	可以利用加速度计的读数计算imu的roll和pitch
陀螺仪	告诉我们当前时刻x轴，y轴和z轴的角速度	通常数值不准
磁力计	感受地球的磁场，告诉我们当前的朝向	可以利用它的数值计算imu的yaw角

这样的话，我们就可以利用IMU计算出它的roll，pitch和yaw了。