激光雷达运动畸变去除

激光雷达传感器测距原理：三角测距与飞行时间（TOF)
三角测距：

在这里插入图片描述

TOF

在这里插入图片描述

TOF精度更高更常用。

激光雷达的数学模型
现在广泛使用的是似然场模型，即采用高斯模糊理论。

在这里插入图片描述

运动畸变
产生原因：激光点数据不是瞬时获得的，激光测量时伴随着机器人的运动，激光帧率较低。
个人思考：激光有自己的坐标系，当激光运动到不同的位置，两个位置的激光坐标系也是需要转化的。但机器人以为自己是在一个位置测出来的，所以会出现畸变。如图所示

在这里插入图片描述

畸变去除
1、纯估计方法
2、里程计辅助方法

纯估计方法：ICP(迭代最近邻匹配 iterative closest point）–点对点匹配
即未知对应点的求解方法，采用极大似然估计方法。
流程：

• 寻找对应点
• 根据对应点，计算R与T
• 对点云进行转换，计算误差
• 不断迭代，直到误差小于某一值

VICP(速度估计ICP—velocity estimation ICP）
即ICP算法的变种，考虑了机器人的运动为匀速运动，进行匹配的时候同时估计机器人的速度。

里程计辅助：
直接测量机器人的位移和角度，具有较高的局部角度测量精度，具有较高的局部位置测量精度。用CPU读取激光雷达数据，同时单片机上传里程计数据，两者进行时间同步，在CPU上统一进行运动畸变去除。
流程：

1. 已知当前激光帧的起始时间ts,te.
2. 两个激光束间的时间间隔*t
3. 里程计数据按照时间顺序存储在一个队列里。
4. 求解当前帧激光数据中的每一个激光点对应的里程计数据（即机器人位姿）
5. 根据求解的位姿把所有的激光点转换到同一坐标系下
6. 重新封装成一帧激光数据发布出去