关于LOAM中IMU去除重力加速度的个人理解

引言：最近把LOAM系列的文章和代码都看了一遍，最开始看的时候只知道LOAM中在IMU的回调函数中对IMU进行了重力加速度对线加速度linear_acceleration影响的消除，后面回过头结合一些博客再来看的时候发现一直不明白为什么最后的RzRyRx旋转矩阵要进行转置以后再与重力加速度相乘，我最开始的时候认为不需要转置，但是发现手推了一遍，如若不转置的话就和源代码对不上了，搞了两天终于有点自己的理解了，不知道对不对，写出来和大家探讨一下。

前提：大部分IMU输出的线加速度linear_acceleration.xyz都是受到重力加速度的影响的，这不能代表车体或者机器人相对世界坐标系W的实际线加速度，需要将重力加速度对IMU影响给消除才能得到世界坐标系下车体的实际加速度。

自己的理解：首先我们知道，重力加速度是在世界坐标系下的，表示为[0,0,g]T（T代表转置），我们要将世界坐标系下的重力加速度分别投影到IMU坐标系下的三个轴上得到重力加速度在IMU坐标轴上的分量，然后才能用测量值减去这个分量得到实际值。现在问题就变成了如何得到IMU坐标系和世界坐标系的旋转矩阵，我们只有知道了这个旋转矩阵才能进行投影。

这个时候我们可以通过IMU测量数据中的四元数orientation，通过四元数计算得到相对于世界坐标系的旋转角RPY，此时我们要注意啊，这里的RPY是IMU相对于世界坐标系的RPY，由于在LOAM中，IMU坐标系为前（X）->左（Y）->上（Z），那么Roll角就对应X轴，Pitch角就对应Y轴，Yaw角就对应Z轴。此时我们遵循常规欧拉角物理定义，按照实际意义的Yaw、Pitch、Roll角进行旋转，可以得到IMU相对于世界坐标系的旋转矩阵R_iw（即IMU坐标系到世界坐标系的转换）:

R_iw = Rz * Ry * Rx

但是我们回过头去看，我们要做的是将世界坐标系下的重力加速度投影到IMU坐标系，就是要得到世界坐标系相对于IMU的旋转矩阵R_wi，而R_wi正好就是R_iw的转置，即为[Rz * Ry * Rx]T

Rz,Ry,Rx如下所示：

就可以得到R_wi = [Rz * Ry * Rx]T如下：

得到R_wi以后就可以让测量值减去R_wi * [0,0,g]的结果，就可以得到实际的线加速度了。

        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;

大家有不懂的上面哪一步的，或者觉得我哪一步讲错了的可以直接发在评论区，我们一起来讨论，我肯定会回复的！感谢大家！！