第三章 传感器数据处理II：激光雷达
3.2 激光雷达畸变的矫正
3.2.1 产生的原因
激光雷达由于小车在旋转，激光雷达会有一个角度的运动畸变，需要矫正，否则和实际偏差很大；
3.2.2 激光雷达的分类
① 三角测距原理（A1雷达）
② TOF测距（飞行时间）
精度是固定的（由于光速不变，时间计数器精度不变 ）
3.2.3 激光雷达的数学模型
激光雷达光束有四种数学模型：
a、光束混合模型，计算期望值、得分。缺陷明显，一般不用
b、似然场计算模型，计算量低，适合结构化环境和非结构化环境，最常用；
b、似然场模型
3.2.4 运动畸变及其去除
- 产生原因：运动导致激光雷达未采集完一帧时，机器人已经运动出一段距离（雷达坐标系原点相对世界坐标系不断移动）。（硬件帧率较低）可以使用算法（修正雷达坐标系原点）修正运动畸变问题。
- 去除激光雷达运动畸变：
- 1、纯激光估计方法，VICP（迭代最近点）方法，EM算法（固定A点，计算B点；固定B点，计算A点~~不断迭代）的特例。【缺点，没有考虑激光的运动畸变，当前的激光数据源是错误的、认为小车匀速运动、慢速运动才好（激光帧率5Hz）、数据预处理和状态估计过程耦合】，获取每一个点的真实坐标，通过运动速度、位姿变换等逆算出激光点原点去统一原点的数据。
– 3.2.4.1 纯估计方法去除畸变（仅激光）：
- ICP方法求解流程：
- 1、求解两帧点云的几何中心ux、up；
- 2、在点云矩阵中减去几何中心ux、up（为了提高数据的稳定性）；
- 3、得到一个W矩阵
- 4、代入对应点，得到ICP的解：
- 注意：实际中，并不知道对应点，不能一步求解出R、t，所以需要迭代求解对应点；
- 求解对应点算法流程（一种EM算法）：
- 1、寻找对应点（需要初始输入的R、t），由R、t求‘对应点’；
- 2、根据对应点，再求解进一步的R、t，由‘对应点’求解R、t；
- 3、对点云进行转换，计算误差；
- 4、不断重复123点，迭代求解，直到误差小于某阈值；
缺点：没有考虑小车的运动速度，求解的点云偏差较大，没法修正；
- VICP方法求解流程：
解决了ICP的缺点。
简介：
1、ICP算法的变种；
2、考虑了机器人的运动（假设是匀速运动）；
3、进行点云匹配的同时，估计机器人的速度；
数据变量的设置：
1、Xi、Xi-1表示第i帧和第i-1帧的数据；
2、Ti，Ti-1表示其对应的位姿矩阵；
3、机器人的速度Vi；
4、设第Ti帧的时刻表（全部以Ti时刻加或减去一个固定的时间间隔Ts，因为假设匀速运动）；
5、则第i帧第i个节点的位姿矩阵可求得：李群矩阵
VICP方法的缺点：
1、低帧率激光，（肯定有加减速的过程）匀速运动假设不成立；
2、数据预处理和状态估计过程耦合；
算法流程:
– 3.2.4.2 传感器辅助去除畸变（IMU、odom）
解决了VICP的问题，可以尽可能的准确反映运动情况；实现预处理和状态估计的解耦合；
- 里程计/传感器辅助
- 使用Odom/IMU，200Hz位姿更新频率
- a、IMU精度效果很差，一般不用（只用角速度数据）；
- b、轮式里程计精度很高；
具体方法：
线性插值，假设在200hz，5ms的时间内是匀速的。求速度
1、激光数据和里程计数据的时间已经同步，用下图的第1点；
2、激光数据和里程计数据的时间没有同步，则线性插值处理，用下图的第2点；
- 求ts、te时刻里程计位姿：
- 1、二次插值：
得到激光数据的起点的位姿后，认为机器人做匀加速运动，求终点位姿（这段路程的终点）。
* **5ms内的路程：S = Vot + 1/2 at²**
* **目标是求一条****二次曲线的近似****。**
- 2、二次曲线的近似
Psi：获取这段曲线中的每一个点的位姿（间隔5ms，近似）
- 3、坐标系统以及矫正的激光数据发布
– 3.2.4.3 融合方法去除畸变
配合5.4.1和5.4.2的方法，按照下面的方法融合两者：
- 一般5.4.2的轮式里程计畸变去除已足够，这是拓展内容
