激光雷达slam之LOAM中的坐标转换与IMU融合

需要用到的一些知识和假设：

（1）  来源于 github中的讨论：

由于IMU累积推算位置的误差大，程序中粗略地计算了IMU的位置漂移。

_imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;

上式成立的前提是认为一个扫描周期内，Lidar的运动是匀速的，上式计算出了非线性误差部分。

 

（2） X、Y、Z轴对应俯仰（pitch）、航向（yaw）、横滚（roll）机动，可知Lidar坐标系为“右下前”坐标系。

 

（3）  从Lidar系到global IMU系，类似于惯导系统中的C（b->n）,即载体系到地理系的转换。

旋转顺序为：横滚->俯仰->航向

rotateZXY(point, roll, pitch, yaw);

从global IMU系到Lidar系，旋转顺序正好相反。

rotateYXZ(point, -yaw, -pitch, -roll);

 

（4）  transform代表将k时刻的点云转换到k+1时刻下，与视觉slam中的相对位姿定义相同。

 

坐标转换与IMU融合

1、 transformToStartIMU

注册点云时（MultiScanRegistration.cpp中），当判断有IMU数据时，会进行一步坐标转换的预处理，体现在函数transformToStartIMU中。

这个函数进行了三步处理：

（1）       rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);

将原始点云从当前Lidar系转换到global IMU系下；

（2）       补偿了_imuPositionShift，也即估算的IMU位置漂移；

（3）       rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);

将global IMU系下的点云转换到Start时刻的Lidar系下。

         经过这个函数的处理，点云的position部分处于当前Lidar系下一个相对准确的位置上（基于扫描周期内匀速运动的假设），但点云的Rotation部分是Start时刻Lidar系下观察所得的，而非处于当前Lidar系下。更清晰地来说，即此时观察到的点云坐标，是以当前Lidar的坐标（一个估计值）为原点，而坐标轴是与Start时刻的Lidar系的坐标轴对齐的。

 

2、  OD初始化：

根据第一次开始扫描时的IMU pitch与roll，作为累积位姿的初始值。  

 _transformSum.rot_x += _imuPitchStart;

_transformSum.rot_z += _imuRollStart;

Yaw角度和pos部分都未赋初值，即假设开始时刻的偏航角为0，位于global系下的原点位置。

 

3、  运动估计初值：

（1）       _transform.pos -= _imuVeloFromStart * _scanPeriod; 

其中，imuVeloFromStart的计算，可知imuVeloFromStart为Start时刻Lidar系下的速度变化矢量：

imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity;

  rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);

对于匀速运动假设的一个补偿，并且基于运动曲线的连续性，做了递推形式的计算，可能乘以1/2会更合适？

（2）       _transform的rotation部分未赋初值，认为为0。

 

4、  transformToStart

在进行KDTree最近点搜索前，首先将进行畸变处理后的点云转换到每一次扫描的开始时刻。

先根据匀速运动假设计算出当前点时刻Lidar的位移和旋转。

但这里其实是和前面的转换transformToStartIMU有些冲突的。因为transformToStartIMU之后点云所处的坐标轴是与Start时刻的Lidar系的坐标轴对齐的。现在又通过_transform将点云转换到start时刻。有一种转了两次的感觉。

有一种解释：认为k次扫描中的旋转部分大部分由IMU积分获得，_transform中的旋转估算的只是一个小量。

 

5、  transformToEnd

（1）       先进行transformToStart，此时点云处于start时刻的Lidar系下；

（2）       通过_transform转换到end时刻的Lidar系下；

（3）       rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart);

转换到global系下

（4）       rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd);

转换到end时刻的Lidar系下。

 

总结点云的旋转过程从1->5, 可用公式表示为：

 

6、accumulateRotation

该函数的作用是将计算的两帧之间的位姿“累加”起来，获得相对于第一帧的旋转矩阵，具体公式如下：

                                                                            

7、 pluginIMURotation

该函数与accumulateRotation，联合起来完成了更新_transformSum的rotation部分的工作。该函数可视为transformToEnd的下部分的逆过程。具体公式如下：