激光slam学习日记——激光雷达数学模型和运动畸变去除

激光slam学习日记——激光雷达数学模型和运动畸变去除

一、测距原理
1、三角测距：与双目摄像头原理相似；距离远近精度越高；10到15米精度10到20厘米；不适合远距离，适合室内；容易受干扰；便宜；小于15m。

2、飞行时间（TOF）：距离短效果不好；测距范围广；精度高；抗干扰强；价格贵；室内外都行。
D = 1/2 * C *（t1 - t0） 但是光线速度不好计算，所以通过波形图方式计算距离。
C:光线速度

二、激光雷达数学模型
（1）光束模型：（很少有人用）
a:受高斯分布影响 （正常情况）
b:有物体遮挡，呈指数分布
c:最大值
d:均匀噪声影响，不能确定具体距离
右下方公式表示：测量准确度的概率；后面用作该光束的得分（用于选择最佳光束作为求解值）；与地图的重合度；重合程度越好，得分越好，数据越准。
缺点：每一个位姿都需要对N束激光进行搜素，计算量大；在不均匀、非结构化的场景中，位姿微小变化会导致期望值（这里意思就是实际的测量距离，噪声就是不确定度：可看做方差）巨大的变化，得分突变，不稳定，后面无法进行优化。’

（2）似然场模型：
对周围进行高斯模糊，通俗来讲就是有物体的地方给予较低的评分（图中白色），没有给予较高的评分（图中黑色）。
优点：无论本体怎么动，得分变化不大，所得期望值比较平滑，不容易突变；得分是通过查表计算，计算量小；适合于结构化和非结构化的环境。

三、运动畸变：（对于帧率低的雷达）
原因：激光数据点不能瞬时得到；机器人运动；帧率低，运动不能忽略的时候。
每个激光点有多种位姿，我们处理时候当成一种处理，出现问题。
旋转比较严重：假设机器人90度/秒,用帧率5HZ的雷达，1帧200毫秒，机器人可以旋转18°，误差很大 ，建图发生偏斜。
平移影响小

（1）畸变去除：
纯估计：主要就是用ICP的方法求解一个最小二乘法问题，直接求解析解，X P为两帧对应的点云，已知点云在第二帧上的投影与第二帧的观测做差，通过求解使这个差最小，得到旋转矩阵和平移。

先求解一块点云的均值，在计算与均值的距离，在通过SVD奇异值分解进行求解R t。

EM算法，大致意思就是，假如可以用B求A，但我们不知B，所以我们先估计一个A，反过来求B，在根据B求A,不断迭代求解。ICP就是Em算法的一个特例。R T相当于A，两匹配点云相当于B.

（3）VICP方法：
特点：考虑了机器人 的速度；认为是匀速运动；匹配的同时进行机器人的速度估计。
根据位姿变化计算一个平均速度，然后用这个平均速度，从当前时刻开始，对之前每一个位姿进行校正。用到李群李代数部分知识。

估计位姿的同时在不断的估计速度，以消除累计误差。

2、里程计辅助方法（imu/odom）
我们希望数据预处理与状态估计是分开的，想要真实反映机器人的运动状态。

由于里程计与雷达的数据可能不同步，所以要进行线性差值使得两传感器时间同步。（认为局部变化为匀速，假设50HZ，200毫秒一帧，可近似匀速直线运动）