【激光SLAM】激光雷达数学模型和运动畸变去除

概念介绍

激光雷达传感器介绍

测距原理

三角测距

特点：

• 中近距离精度较高，距离越近，精度越高
• 价格便宜
• 远距离精度较差
• 易受干扰
• 一般在室内使用

原理：
由于雷达特性，α与L已标定，β为通过雷达内参数投影得出，设AB距离为L，A至垂足间的距离为L1，B至垂足间的距离为L2

飞行时间（TOF)

特点：

• 测距范围广
• 测距精度高，距离越远，精度越高
• 抗干扰能力强
• 价格昂贵
• 室内外皆可

原理：

基于时间：
D=0.5c(t2-t1)，c为光速，t1与t2分别为发射与接受到光的时间点。
但是，由于光速过大，在测量距离较小的物体时，要达到这个精度极其艰难。

基于相位：
通过测量发射与接收的相位差测量Δt，再得出D。这样精度稍微高一些。

激光雷达数学模型介绍

光束模型（beam model）

激光的观测值有四种可能性：

把激光看成一个激光束。激光束射到物体上返回得到的距离，由于噪声的存在，通常呈高斯分布，如图a所示，d在期望值附近形成高斯分布。通过raytracking得到期望距离。

图b为激光束碰到障碍物的情况，此时得出的d小于期望距离。一般用指数分布来描述这种情况。

图c为受噪声影响，得到一个测量最大值的情况；图d为均匀分布的误差。不过在实际运用中，这两种影响相对较少，所以一般忽略。

激光雷达的观测模式：
给定一个机器人的位置x_t，地图m，以及这一帧观测值z_t，于是我们得到这一帧机器人观测值的概率及其分布：

一般认为每一个激光束是相互独立的，彼此之间信息无关。

对i从1到n的每一个激光都求一个raytracking的过程，得到对应的z_t^k*，随后代入至公式中求解。一般把最终结果称为得分（score），score(z_t^k)用于表示当前这束激光跟地图的重合程度。重合程度越好，得分越高，最大值为1。

如果我们拿到一帧的激光数据，那我们评估的就是一帧激光数据跟地图的重合程度。因此，我们的观测模型就是评估激光数据跟地图的重合度。

似然场模型（likelihood model）

左图中灰色部分为障碍物。将sensor noise 与 pose noise统一到一起，得到一个呈高斯分布的噪声。利用该高斯分布，对图像进行高斯平滑。白色得分为1，黑色得分为0，距离障碍物距离越远，得分越低。

运动畸变介绍

在机器人运动过程中，每个激光点都在不同的基准位姿上产生（就是由于机器人自身的运动，在不同的时刻发出激光时，机器人的位置是不同的，这也是运动畸变产生的根源所在）。

激光扫描时伴随着机器人的运动，每个角度的激光数据都不是瞬时获得的，当激光雷达扫描的频率比较低的时候，机器人运动带来的激光帧的运动误差是不能被忽略的。

当扫描频率是5Hz的激光雷达，一帧数据的收尾时间差是200ms，如果机器人以0.5m/s的速度沿着x方向行走并扫描前面的墙体，那么200ms后尾部的测量距离和首部的测量距离在x方向上就差10cm。

综上，激光雷达产生畸变的原因有三：

1. 激光点数据不是瞬时获得
2. 激光测量时伴随着机器人的运动
3. 激光帧率较低时，机器人的运动不能忽略（目前 国产的激光雷达 旋转的频率大约在是10Hz ~~100ms）
   

蓝色是产生运动畸变之后轨迹。

畸变去除

去除激光雷达运动畸变的原理是把一帧激光雷达数据的每个激光点对应的激光雷达坐标转换到不同时刻的机器人里程计上（近似对应的里程计的位置，达到尽可能去除畸变的目的）

纯估计方法（ICP Variants)

ICP方法

基本目的： 两个点云进行配准

数学描述：

设x_i和p_i是匹配的，即两个点云中的点打向的是物理空间中的同一个点。因此，我们需要找到R和t，使得两个匹配点的距离最小：

已知对应点的求解方法（解析解）：

u_x表示点云集合X的几何中心（基准位姿/平均位姿），u_p表示点云集合P的几何中心（基准位姿/平均位姿），随后把每个点云都移动到平均位姿来。

对W进行SVD分解：

则ICP的解为：

效果如下：

未知对应点的求解方法：

ICP方法在激光匹配中的缺点：
没有考虑激光的运动畸变，因此得到的激光数据是错误的（计算的时候假设是正确的），从而造成点云匹配发生错误，通过不断的迭代，虽然能够收敛，能够求解，但是存在误差。

VICP（Velocity estimation ICP）

VICP算法考虑了激光的运动畸变，也就是考虑了机器人的运动和速度的存在。

• ICP算法的变种
• 考虑了机器人的运动
• 假设是匀速运动
• 进行匹配的同时估计机器人的速度

VICP矫正效果：

VICP算法流程：

VICP 方法的缺点：

• 低频率情况下（5Hz）,匀速运动假设不成立
• 数据预处理和状态估计过程耦合

里程计辅助方法

为了解决VICP的缺点，我们尽可能的反应运动情况，实现预处理和状态估计的解耦，我们提出了传感器辅助的方法。

传感器辅助方法（Odom/IMU）的好处：

• 极高的位姿更新频率（200Hz），可以比较准确的反应运动情况
• 较高精度的局部位姿估计
• 与状态估计完全解耦

惯性测量单元（IMU）：

• 直接测量角速度和线加速度
• 具有较高的角速度测量精度（比较准确）
• 测量频率极高(1kHz~8kHz)
• 线加速度精度太差，二次积分在局部的精度依然很差（这一点我们就可以否定他在移动机器人上面的使用，主要原因是精度不高）故不使用IMU

轮式里程计：

• 直接测量机器人的位移和角度（真正需要的量）
• 具有较高的局部角度测量精度
• 具有较高的局部位置测量精度
• 更新速度较高(100Hz~200Hz)
  
  
  
  
  

“位姿通过线性插值获取”中的线性插值表示在里程计的数据中，即Odomlists中进行线性插值。“在这K个位姿之间进行线性插值”中的线性插值表示在先前线性插值得到的位姿中，再进行线性插值，即为每一个激光点分配一个位姿。