1. 运动畸变
运动畸变是在机器人运动中出现的激光雷达测量数据的偏移。
1.1 产生原因
- 激光点数据不是瞬时获得
- 激光测量时伴随着机器人的运动
- 激光频率较低时，机器人的运动不能忽略。
2. 运动畸变的去除
2.2 纯估计方法-ICP
2.2.1 目的
实现两个点云的匹配
2.2.2 数学描述
给定两个点云的集合：
求解R和t，使得下式最小：
2.2.3 已知对应点的求解方法
首先求x点云的基准(平均)位姿，再求p点云的基准(平均)位姿，
之后，把每个点云都移到平均位姿上，
构造一个W函数，并对其进行奇异值(SVD)分解,
之后可解得ICP的解为：
产生的效果如下图，我们可以看到两条点云线完全重合（理想情况）。
2.2.4 未知点云的求解方法
在实际中，并不知道对应点匹配，不能一步到位计算出R和t，要进行迭代计算（EM算法）。ICP实际上是EM算法的一个特例。
EM算法流程：首先固定A求B，再固定B求A，再固定A求B，再固定B求A...这样不断迭代，最终便可收敛得到一个位姿。
2.2.5 VICP
由于ICP方法在激光匹配中没有考虑到激光的运动畸变和当前激光数据是否准确，因此再ICP方法的基础上加入速度估计得到VICP方法。
VICP方法:
- ICP算法的变种
- 考虑了机器人的运动
- 匀速运动
- 进行匹配的同时估计机器人的速度
,
表示第i帧和第i-1帧数据
,
表示对应的位姿矩阵
机器人速度：
设第i帧的时刻表为：
则第i帧第j个节点的位姿矩阵为：
可得，矫正结果为：
2.2.6 VICP矫正结果
2.2.7 VICP算法流程
认为两帧之间机器人是匀速运动的
2.3 里程计辅助方法
由于VICP方法在低帧率激光（5HZ）时，匀速运动假设不成立，不能真是地反应机器人地运动情况；同时在数据预处理和状态估计过程会出现耦合。
因此我们需要尽可能地准确反映机器人地运动情况，同时实现预处理状态估计的解耦。由此提出传感器辅助的方法（Odom/IMU）。
2.3.1 优势
- 极高的位姿更新频率（2000Hz），可以准确地反映机器人地运动情况；
- 较高精度地局部位姿估计；
- 跟状态估计完全解耦。
2.3.2 IMU和odom对比
| 惯性测量单元(IMU) | 轮式里程计(odom) |
|---|---|
| 直接测量角速度和线加速度 | 直接测量机器人地位移和角度 |
| 测量频率极高 | 具有较高地局部角度测量精度 |
| 线加速度精度太差，二次积分在局部地精度依然很差 | 具有较高地局部位置测量精度 |
| 具有较高地角速度测量精度 | 更新速度较高 |
由上表我们可以看出里程计(odom)是一个更优地选择。
2.3.3 已知数据
- 当前两帧激光地起始时间
； - 两个激光束地时间；
- 里程计数据依照时间顺序储存在一个队列中，队首时间最早；
- 最早里程计数据地时间戳；
- 最早里程计数据地时间戳。
2.3.4 目标
- 求解当前帧激光数据中每一个激光点对应地机器人位姿，即求解{}；
- 根据求解地位姿吧所有激光点转换到同一坐标系下；
- 重新封装成一帧激光数据，发布出去。
2.3.5 求解
1 求解时刻的位姿
里程计队列正好和激光雷达数据同步，假设第i和第j帧数据的时刻分别为：
在
时刻没有对应的里程计位姿，则进行线性插值，设在l,k时刻有位姿，且l<s<k，则：
2-1 二次插值
- 在一帧激光数据之间，认为机器人是做匀加速运动的；
- 机器人的位姿是关于时间t的二次函数；
- 设，且l<m<k则：
- 已知，可以插值一条二次曲线：
2-2 二次曲线的近似
- 用分段线性函数对二次曲线进行近似；
- 分段数大于3时，近似误差可以忽略不计；
- 在和
时间段内，一共取k个位姿{}; - 位姿通过线性插值获取，在这K个位姿之间，进行线性插值：设
和
之间有N个位姿{}，则：
3 坐标系统-激光数据发布
- 一帧激光数据n个激光点，每个激光点对应位姿{}通过上述介绍方法插值得到；
为转化之前的坐标，为转换之后的坐标，则：
- 把转换之后的坐标转换为激光数据发布出去。
3. 总结
由此我们便完成了运动畸变的去除和激光数据的发布。
