激光SLAM--(3)传感器数据处理之Lidar数学模型和运动畸变去除

激光雷达传感器

1.激光雷达：

• 激光雷达由四部分构成
  • 激光器：激光器是激光雷达中的激光发射机构。在工作过程中，它会以脉冲的方式点亮
  • 接收器：激光器发射的激光照射到障碍物以后，通过障碍物的反射，反射光线会经由镜头组汇聚到接收器上
  • 信号处理单元：信号处理单元负责控制激光器的发射，以及接收器收到的信号的处理。根据这些信息计算出目标物体的距离信息。
  • 旋转机构：以上3个组件构成了测量的核心部件。旋转机构负责将上述核心部件以稳定的转速旋转起来，从而实现对所在平面的扫描，并产生实时的平面图信息。
• 测距原理

  • 三角测距：主要是A2，EAI这种小于10m的

    • 
    • 计算过程：

  • TOF 测距：速度乘以飞行时间

  • 各自特点：

    • 三角测距：中近距离精度较高（误差1%左右）、价格便宜、远距离精度较差、易受干扰、一般在室内使用。
    • TOF：测距范围广，测距精度高，抗干扰能力强，价格贵，室内室外皆可

2.激光雷达数学模型

• 光束模型
  • 模型图：
  • 缺点：
• 似然场模型（likelihood model）
• 

3.运动畸变

• 示意图：
• 产生原因：
  • 激光点数据不是瞬时获得
  • 激光测量时伴着机器人的运动
  • 激光帧率较低时，机器人的运动不能忽略

畸变去除（目的就是要得到每帧的更为准确的位姿）

1.纯估计方法

方法一：ICP方法

• 目的：进行两个点云的配准，如给定两个点云集合，其中$x{}_i与p{}_i$匹配，则有数学表示（求解一个欧式变换（R，t））

• 

• 已知对应点的求解：步骤是1首先求平均位置，2去中心化，3构造矩阵并进行SVD分解

• 未知对应点的求解（对应点就是EM的隐变量）

• ICP方法在激光匹配中间那个的缺点：没有考虑激光的运动畸变；如果当前的激光匹配数据是错误的。

方法二：VICP方法

• 特点：ICP算法的变种，考虑了机器人的运动并且假设为匀速运动，进行匹配的同时估计机器人的速度。
• 介绍：
• VICP矫正效果：
• 算法流程：

2.里程计辅助方法

• 前瞻
  • VICP缺点：
    • 对于低帧率激光（5HZ），匀速运动假设不成立
    • 数据预处理和状态估计过程耦合
  • 解决办法：
    • 尽可能准确的反应运动情况，
    • 实现预处理和状态估计的解耦
  • 传感器辅助方法（Odom/IMU）
    • 极高的位姿更新频率（200HZ），可以比较准确的反应运动情况
    • 较高精度的局部位位姿估计
    • 跟状态估计完全解耦
• IMU辅助：好处是可以直接测量角速度和线加速度、具有较高的角速度测量精度、测量频率较高（1kHz～8kHz），但是线加速度精度太差，二次积分在局部的精度依然很差（就是时间太短达不到那么高的时间精度）
• 轮式里程计：能直接测量机器人的位移和角度（核心），具有较高的局部角度和位置测量精度，更新速度较高（100Hz～200Hz）
  • 
  • 问题模型：
  • 求解过程：
    • a.线性插值方法（假设是匀速运动）：
      • 若里程计队列数据正好和激光数据同步，假设第i帧和第j帧数据时刻分别是$t{}_s和t{}_e$，则$\begin{array}{l}{p}_s=\text{ OdomList }[i]\\ p_e=\text{ OdomList }[j]\end{array}$
      • 若不同步（即没有$t{}_s$时刻对应的里程计位姿），则需要进行线性插值，设在l，k时刻有里程计位姿，且l<s<k，则：$\begin{array}{l}{p}_l=\text{ OdomList }[l]\\ p_k=\text{ OdomList }[k]\\ p_s=\mathrm{LinarInterp}\left(p_l,p_k,\frac{s-l}{k-l}\right)\end{array}$
    • b.二次插值（假设是匀加速运动）：
    • c.用分段线性函数对方法二中的二次曲线进行近似来求解
    • 统一坐标系&激光数据发布

实现一个激光雷达运动畸变去除模块