LegoLoam IMU加速度去除重力影响

IMU数据去除重力

这就涉及IMU的测量模型，包含两种：加速度计和陀螺仪。加速度计用来测量加速度的大小，陀螺仪用来测量角速度的大小。

IMU的测量值是基于自身坐标系(body坐标系)的三轴加速度和三轴角速度数据，也就是 $a^{measure}_{B}$ ，由于受重力加速度的影响，IMU 的测量值并不直接反映其本身的加速度，并且其测量的加速度的坐标是在 Body 系下表示的，与世界坐标系之间存在一个旋转关系，所以需要转换为世界坐标系下的数据，而角速度不需要转换。

IMU的姿态解算选用的旋转顺序为ZYX，即IMU坐标系初始时刻与世界坐标系重合，然后依次绕自己的Z、Y、X轴进行旋转，这样坐标系就会改变，也可以称新的坐标系为body坐标系

IMU加速度计测量的是其感受到的加速度，在静止的时候，其本身是没有加速运动的，但因为重力加速度的作用，其感受的加速度与重力加速度正好相反，所以读到的线加速度z是9.8，而不是-9.8

当加速度计水平放置，即Z轴竖直向上，而且在静止时，读到的加速度可以记作 (0, 0, 9.8) (世界坐标系)

当加速度计旋转一定的姿态时，重力加速度会在加速度的3个轴上产生相应的分量，其本质是世界坐标系下的(0, 0, 9.8)在新的加速度计坐标系(body坐标系)下的坐标，此时读到的是三轴加速度的新坐标。数学关系如下

这个结果是在body坐标系

在世界坐标系下， $a^{measure}_{W} = a^{true}_{W} + g$
转换到body坐标系， $a^{measure}_{B} = a^{true}_{B} + R^B_{W}g$ ，即 $a^{true}_{B} = a^{measure}_{B} - R^B_{W}g$ ，上面图中的计算结果就是 $R^B_{W}g$ ， $a^{measure}_{B}$ 是IMU直接输出的。

那么按说真实加速度应该是这样的

1
2
3

float accX = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
float accY = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;

坐标系变换

IMU不能随意安装时，其坐标系和载体坐标系是对齐的，都是右手坐标系，所以rosrun tf echo imu base_link的结果中，角度应该都是0，这样IMU得到的角度就是IMU当前姿态向载体坐标系对齐的过程。LOAM框架作者为了与其他视觉SLAM的工作进行匹配统一，将点云坐标系进行了坐标变换。

所有常用坐标系
在这张图里，只看IMU和camera坐标系的关系，IMU的x变成Camera的z，y—->x, z—->y。比如车上装相机识别二维码时，镜头总是朝前的，也就是图中的朝向。

这样一来，实际三轴加速度就成了源码里的

1
2
3

float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

在`imuHandler()`函数中，只对加速度做了这个转换，方位角、角速度还在原来的右手坐标系下。

这导致了后面的处理中看似旋转顺序的表示产生了变化，而本质上旋转顺序按物理意义的偏航-俯仰-滚转保持了一致。

参考：IMU去除重力影响