LOAM中关于坐标转换与IMU融合

激光雷达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中。

如上图所示，我们可以作如下描述：坐标系O-XY逆时针旋转theta（或顺时针旋转-theta）后与坐标系O'-X'Y'重合，或者坐标系O‘-X’Y‘顺时针旋转theta（或逆时针旋转-theta）后与坐标系O-XY重合。利用简单的之间三角形的余弦定理和正玄定理就可以得到以下公式

这里transformToStartIMU，先旋转回到imu的原点，然后再旋转到i=0时刻。

其中旋转回原点：类似于把o'-X'Y'坐标系下的点转换到o-XY坐标系下。

而从原点旋转到i=0时刻：类似于把o-XY转换到o'-X'Y'下。

经过此步处理后，点云是在imu中某一起始时刻下的点，即所有点都依据imu角度进行了对齐，对齐到某一起始时刻。

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

（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);

平移量的初值赋值为加减速的位移量，为其梯度下降的方向（沿用上次转换的T（一个sweep匀速模型）；

同时在其基础上减去匀速运动位移，即只考虑加减速的位移量）；

S = v_2*t+0.5*(v_2-v_1)*t;   这段时间的位移，相比上个时刻，多了0.5*（v_2-v_1）*t

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

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

4、  transformToStart

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

这里考虑，由于运动，当前的点云数据，存在由于运动导致的偏移；需要转换到开始时刻即start时刻，去除匀速运动产生的漂移

类似于把o-XY转换到o'-X'Y'坐标系下 

注意，code中transform初始值如何赋值；负值，反方向旋转角；把坐标从o-xy转换到o'-X'Y'，反向处理；

即(P-T)*R_inv = P';效果等同于

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

5、  transformToEnd

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

（2）       通过_transform转换到end时刻的Lidar系下；即o-XY转换到o'-X'Y'坐标系下；

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

转换到global系下； 即o’-X'Y'转换到o-XY坐标系下；

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

转换到end时刻的Lidar系下；即o-XY转换到o'-X'Y'坐标系下；

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

6、accumulateRotation

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

                                                                            

7、 pluginIMURotation

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

                                                         

8. 关于transformToStart，transformToStartIMU的理解

查看loam_velodyne代码中，可以发现，transformToStartIMU中使用的是ShiftToStartIMU和VeloToStartIMU先计算imuShiftFromStartXCur和imuVeloFromStartXCur，在TransformToStartIMU中消除加减速产生的位移畸变；

（2021-11-18 added）loam类的去畸变：①imu角速度积分，根据一帧数据的首尾点时间，去除旋转带来的畸变；②imu加速度积分，去除一帧数据中，由于加减速带来的畸变；实际中①②是一起处理的；③，经过①②后，一帧数据中只剩下由于匀速运动带来的畸变，之后可以使用计算出来的运动量，去除平移方面的畸变；【由于imu积分，平移部分随着时间推移，累计误差比较大】

而transformToStart  中，是利用帧间匹配，计算出的motion，消除由于匀速运动产生的畸变；

以下部分参考：l1323的博客_易子岚_CSDN博客 

 关于imu消除重力加速度的理解：

 

关于AccumulateRotation函数的理解

AccumulateRotation计算当前帧终点相对于世界坐标系的欧拉角，即上一帧的位姿结合当前帧的运动量，累加计算出来当前帧的世界坐标系位姿

transformSum： 前一帧数据的终点在当前世界坐标系下的位姿

transform：当前帧起点到终点的位姿

参考V-LOAM源码解析（二） - zhch_pan - 博客园中的注释

公式推导如下：

 可以看出程序中对应的公式与推算的R23,R13,R33,R21,R22一致。

关于PluginIMURotation函数

PluginIMURotation，可以理解为将之前计算的rx,ry,rz,累加上imu的旋转变化量；主要操作为Mbc*Mbl.inv()*Mal，展开后与代码中公式一致。

主要还是绕Z-X-Y旋转的变换，公式推导如下：

LOAM中激光里程计部分

如上公式所示，把之前通过点到线的距离以及点到面的距离构建的误差函数转化成关于Tk+1的函数，即源码中的transform[i]数组，这部分和建图部分求导不同在于transform数组表示的是当前帧的起始点相对于终点的位姿，欧拉角表示的是Y，X，Z的旋转，旋转矩阵对应上式的R矩阵，通过链式法则求导，计算过程如下：

la,lb,lc代表距离对起始点的偏导，在前面已经计算过了即源码对应的coeff.x，coeff.y,coeff.z

计算出来的arx和源码一模一样，依次可算出ary,arz,源码如下

对平移分量tx,ty,tz的偏导计算如下：

和源码中的atx一模一样，以此类推即可计算出aty,atz