LOAM中消除IMU重力影响解析

最新推荐文章于 2024-10-23 22:03:05 发布

原创最新推荐文章于 2024-10-23 22:03:05 发布

· 7k 阅读

101 ·

版权

文章标签：

#slam

代码解析专栏收录该内容

2 篇文章

订阅专栏

在LOAM的处理中，IMU数据甫一进入系统第一步是对重力分量的去除。LOAM中对重力影响的去除有以下公式：

float accX = vec_imuData[pointer].accy - sin(imuRoll[pointer]) * cos(imuPitch[pointer]) * 9.81;
float accY = vec_imuData[pointer].accz - cos(imuRoll[pointer]) * cos(imuPitch[pointer]) * 9.81;
float accZ = vec_imuData[pointer].accx + sin(imuPitch[pointer]) * 9.81;

以上公式处理对初始IMU的数据进行了以下两个操作：

去除了重力的影响；
变换了坐标表示。

以下对两个方面逐个进行说明。

IMU数据去除重力

IMU中的加速度计有以下原理图：
在这里插入图片描述
加速度传感器在感受轴线上进行加速运动时，质量块将压缩弹簧，弹簧按牛顿第二定律为质量块提供相应的力以推动质量块进行加速运动：
$F = m a = - k x$
在该过程中，可建立弹簧形变量与加速度的直接关系，弹簧的形变量将直接反映加速度的大小：
$a = - k x / m$
在地球上，受重力的影响，加速度计中的质量块在竖直方向将压缩弹簧，等效将记录一个方向竖直向上的加速度。将该加速度按IMU在空间中的姿态将受重力影响的加速度分解到三个敏感轴方向上，之后在读数基础上减去该部分影响，即可得到去除重力加速度的三轴加速度。

将重力影响在IMU系中的三轴分量记录为 $V_I$ ,在世界坐标系中重力影响可记为 $V_W=[0,0,9.8]$ .则：
$V_I=R^I_WV_W\tag{1}$
式中， $R^I_W$ 表示World系中坐标向IMU系映射的坐标变换矩阵，也可以表示为从IMU系到World系的旋转矩阵。

另外，值得一提的是，在LOAM中定义了与物理常规相异的roll、pitch、yaw定义。在日常实际中，常有以车体（或机体）前、右、下为roll、pitch、yaw方向的定义。而在LOAM框架的输入¹中，采用了与xyz三轴前、左、上方向一一绑定的roll、pitch、yaw方向定义。这点在后面的处理与理解上需要格外注意。无特殊说明，本文之后提到的roll、pitch、yaw方向将采用LOAM定义。

欧拉角转旋转矩阵的规则有众多类别，同样的欧拉角在不同的规则下将组合成不同的旋转矩阵，从而构成不同的旋转结果。换句话说，对某一次确定的三维旋转，根据不同的规则能够分解成多组不同的欧拉角表示。

而遵循常规欧拉角物理定义，常采用先进行实际意义偏航yaw，再进行实际意义俯仰pitch，最后进行实际意义滚转roll的规则顺序来进行组合旋转构成。仅从转轴顺序上进行对应，则对应了z、y、x的坐标轴顺序，即可以表示为：
$R^W_I=R_zR_yR_x\tag{2}$
式中， $R^W_I$ 表示从World系到IMU系的旋转矩阵。
由于是欧拉角连续旋转，旋转矩阵的累积采用右乘模式。（机器人学）
将（2）式子带入（1）式，有：
$V_I=R^I_WV_W=(R^W_I)^TV_W=(R_zR_yR_x)^TV_W\tag{3}$
其中：
$R_x= \begin{bmatrix} 1&0&0\\ 0&cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{bmatrix} ;R_y= \begin{bmatrix} cos\beta&0&sin\beta\\ 0&1 & 0 \\ -sin\beta& 0 &cos\beta \end{bmatrix} ;R_z= \begin{bmatrix} cos\gamma&-sin\gamma&0\\ sin\gamma&cos\gamma &0 \\ 0& 0 & 1 \end{bmatrix}$

$(R_zR_yR_x)^T= \begin{bmatrix} cos\gamma cos\beta&cos\beta sin\gamma&-sin\beta\\ cos\gamma sin\beta sin\alpha -cos\alpha sin\gamma & cos\gamma cos\alpha +sin\gamma sin\beta sin\alpha & cos\beta sin\alpha \\ sin\gamma sin\alpha+cos\gamma cos\alpha sin\beta & cos\alpha sin\gamma sin\beta -cos\gamma sin\alpha & cos\beta cos\alpha \end{bmatrix}$
所以，有
$V_I=\begin{bmatrix} -9.8sin\beta\\9.8cos\beta sin\alpha\\9.8cos\beta cos\alpha \end{bmatrix}$
按LOAM定义将与x、y、z轴关联的相应轴向旋转，按右手握轴正向分别记为imuroll、imupitch、imuyaw。在三轴方向的加速度上分别减去重力的分量，得：

Y向实际加速度 = vec_imuData[pointer].accy - sin(imuRoll[pointer]) * cos(imuPitch[pointer]) * 9.81;
Z向实际加速度 = vec_imuData[pointer].accz - cos(imuRoll[pointer]) * cos(imuPitch[pointer]) * 9.81;
X向实际加速度 = vec_imuData[pointer].accx + sin(imuPitch[pointer]) * 9.81;

LOAM框架坐标变换

LOAM框架作者为了与其他关于视觉SLAM的工作进行匹配统一，将点云坐标系进行了坐标变换。

变换进行了y->x, z->y, x->z的坐标交换，实际对整体坐标进行了一次旋转。而imuroll、imupitch、imuyaw的旋转轴方向没有进行变换，仍然与原有的x、y、z轴方向保持一致，即与变换后的z、x、y一一对应。这导致了后面的处理中看似旋转顺序的表示产生了变化，而本质上旋转顺序按物理意义的偏航-俯仰-滚转保持了一致。

经过坐标变换，最终得到了以下的三行代码：

float accX = vec_imuData[pointer].accy - sin(imuRoll[pointer]) * cos(imuPitch[pointer]) * 9.81;
float accY = vec_imuData[pointer].accz - cos(imuRoll[pointer]) * cos(imuPitch[pointer]) * 9.81;
float accZ = vec_imuData[pointer].accx + sin(imuPitch[pointer]) * 9.81;