传感器数据处理2：激光雷达运动畸变的去除

激光雷达运动畸变的去除比里程计标定更重要，但也取决于用的雷达型号。我用的思岚A2雷达频率小于10Hz，畸变也是比较明显的。

概念介绍

激光雷达传感器介绍（分类、原理）
- 分类介绍
  - 三角测距（A2，EAI，一般10m左右的都是），双目摄像头也是用的三角测距法，左右各一个摄像头
    
    优点：中近距离精度较高、价格便宜
    
    缺点：远距离精度较差、易受干扰、一般在室内使用
  - 飞行时间：ToF测距
    
    优点：测距范围广、测距精度高、抗干扰能力强
    
    缺点：价格昂贵、室内室外皆可。
    
    基本思想就是一个激光发射器，在t0时刻发一束光出去，在t1时刻接收到他，那么距离就是C(t1-t0)/2他是由光的飞行时间来决定的。
- 测距原理
  - 三角法测距原理
    
    A，B点是激光发射头，α和β是激光发射的两个角，L就是两个发射头之间的距离，根据角边角我们就可以确定一个三角形，然后L边的中点到三角形顶点的距离也是可以算出来的，通过和差化积公式计算L，再换成d。d公式中，α和L都是标定好的，β是通过射出去的α计算出来的。双目摄像头也是这样，但是都是有噪声的。
  - TOF测距原理
    
    测距公式就是：光速*时间差/2：C(t1-t0)/2
    
    光速是30万Km/s，如果我们要测一个1m左右的物体，就是ns级别的。
激光雷达数学模型介绍（数学模型）
- 光束模型
  
  把一条激光看做一个激光束，射到墙上时会有一个期望距离，在这个期望值的附近会形成一个高斯分布，这是正常的情况。
  
  如果有障碍物挡住了的话，也就是d>d1时，这样会形成一个指数分布
  
  基本上是上面的两种情况。那激光雷达的光束模型就是我们给定一个机器人的位置xt，和地图m，来获得这一帧观测值zt的概率。我们默认认为每一个激光束都是线性无关的，是独立的。得到一个累计乘。
  
  用矩阵形式表示就是
  
  等式左边是这束激光的概率（之后都叫得分），反应的是当前这束激光与地图的重合程度，重合程度越好，得分也就越高，如果正好落在期望值的上面，那么就说这个得分就是1
  
  激光发出的一帧激光数据与地图的重合程度这个东西就是我们的光束模型，光束模型评估的就是这个。
  - 光束模型的缺点（基本没人用光束模型，因为有缺点）
    
    机器人在非结构化环境中，位姿发生微小的变化，就会造成期望值发生巨大的变化，从而导致得分突变。
    
    就是说当前机器人正前方1m有障碍物，障碍物后面3m是一堵墙，激光发出激光束可以正常射到这个障碍物上，但是如果此时机器人的位姿发生微小变化，很有可能这个光束就射到障碍物后面的墙上去了，从而导致这个期望值发生突变。他是病态的，并且是无法进行优化的。所以说在复杂环境中不会使用这种
- 似然场模型
  - 他是将所有的噪声，例如传感器噪声，机器人位姿噪声，等等…，得到一个高斯分布，用这个高斯分布对整个图像进行模糊，对障碍物进行高斯平滑，越白表示得分越高，越黑代表得分越低，他是不需要计算期望值的，我们对图像进行模糊之后，已经存在障碍物和模糊过的图像了，也就是说把之前计算期望值，然后用高斯分布算出一个得分换成查表的形式，查表的话，这个似然场从程序一开始就是在进行离线计算，所以对一帧激光来说，只需要查360次表，查表的计算量可以忽略不计。他是同时适合结构化环境和非结构化环境的。因为就算位姿发生一点改变，他的得分也不会太低。AMCL代码中两种模型都有体现。
运动畸变的介绍（原因与效果）
- 示意图
- 机器人不动，肯定不会产生运动畸变，如果机器人的运动频率很高，假设50Hz也就是2ms，如果认为在这2ms内机器人没动，那么运动畸变可以忽略，但是，在一般现实机器人中，运动频率是在5~10Hz左右，那么这个运动畸变就会很大了。反正就是说在雷达在测量的过程中，机器人如果正在运动，那么就会产生运动畸变。他的根源就是每个激光点都有不同的基准位置，但我们处理的时候都认为所有的激光点还是在同一位置。所以我们今天要求解的就是这每一个激光点的位置。

畸变去除

纯估计方法（不借助任何设备，纯靠匹配过程中去除）
- ICP方法介绍（是纯估计方法的变种）：简单说就是给定一个点云，求出R和T，然后进行欧式变换就得到了两个点云点的最短距离
  - ICP方法常用在点云中，是点云匹配的一种较为广泛的方法。他的基本目的就是两个点云进行配种。给定两个点云集合，假设他们对应是匹配的，也就是他们的元素对应在物理空间中是同一个点。ICP的目标函数就是如下图，找到一个R和t，进行欧式变换偶得两个匹配点的距离最小。
    
    基本目的就是：找到一个欧式变换E(R,t)，使得两个点云的距离最小化，这样就将畸变去除了
  - 已知对应点的求解方法
    
    通过闭环的方式求出一个解析解，先求出一个点云的基准位姿，再求另一个
    
    然后把每个点云都移到平均位姿来
    
    然后我们构造一个W，然后进行SVD分解
    
    然后就得到ICP的解（求出R和t），t的话比较简单，他就是同一坐标系下两个点的差，R=UVT，然后进行欧式变换就可以得到最短距离
    
    产生的效果如下，蓝色和红色分别是两个点云点在未处理前，处理后如右图，基本完全重合
  - 未知对应点的求解方法
    
    在实际中，不知道对应的匹配点，他不能一步到位计算出R和t。说白了就是我们要求一个变量A，要求这个变量之前，我们必须知道这个隐变量B，那么就先固定A求B，再固定B求A，再固定A求B…如此迭代。
    
    算法流程就是首先寻找对应点：找与第一个点云距离最近的点
    
    然后根据找到的对应点，计算R和t
    
    然后对点云进行转换，计算误差
    
    然后就不断迭代，直至误差小于某一个值（这个迭代的过程）
VICP方法介绍
- ICP方法在激光匹配中的缺点
  - 没有考虑激光的运动畸变
  - 当前激光数据是错误的
    
    ICP默认给定的点云数据是正确的，给错数据他也认为是对的
  - 所以提出了VICP
- VICP是ICP算法的变种，听名字就知道他在ICP的基础上提供了速度估计因为他是考虑了机器人的运动过程和激光的测量过程所产生的运动畸变。
- 他的速度估计是认为机器人运动过程中是匀速运动的，虽然这也不一定是匀速运动，但总比在ICP算法中认为他没动要好得多。进行匹配的同时估计机器人的速度。
- 详细介绍
  - 用Xi，Xi-1表示第i帧和第i-1帧数据，使用Ti，Ti-1表示对应X的位姿矩阵，Ti相当于是Xi的位姿，Ti-1相当于是Xi-1的位姿，那么当前机器人的速度（位姿的变化量/时间变化量）就是如图
    
    假设第i帧我们激光的时刻表是ti，那么我两帧激光间的数据的时间差是△t，那么第n个就是ti（最近的），那么第n-1个就是ti-△t，第0个开始就是ti-n△t
    
    如果我们已知Ti的话，那么通过下面这个公式，就可以得到从0到n任意一个点的位姿矩阵
    
    所以校正过程可为：（运动校正），根据每两帧之间的时间差得到激光数据和位姿数据，随后通过计算得到机器人速度，来用每个点的速度来校准激光数据
    
    VICP的基本思路就是：我知道了每一个时刻，认为ti已知了，然后可以反推出来每一个点的位姿是多少，然后再根据每一个点的速度对激光数据进行校正。
  - 算法流程
    - 首先我们认为他是匀速运动的，也就是Vi = Vi-1，T△ts代表的是两次位姿的变化量，那么就可以算出每一时刻的位姿，然后进行校正，然后进行ICP匹配
里程计辅助方法（辅助方法的其中一种）
- 使用里程计辅助去除
  - VICP的缺点
    - 低帧率激光（5Hz），匀速运动假设不成立，不能真实地反应机器人的运动情况
    - 数据预处理和状态估计过程耦合
  - 解决方法
    - 尽可能准确的反应运动情况
    - 实现预处理和状态该估计的解耦
  - 传感器辅助方法（里程计/IMU）
    - 极高的位姿更新频率（200Hz），可以比较准确的反应运动情况。
    - 较高精度的局部位姿估计
    - 跟状态估计完全解耦
  - 惯性测量单元（IMU）与轮式里程计传感器辅助
    - 轮式里程计的处理方式（看图）
      - 在单片机上处理，直接用单片机接雷达，直接在单片机上消除运动畸变（因为单片机测量的里程计数据和雷达数据是一起上发的，所以他两时间同步，就不会产生运动畸变），无需考虑时间同步的问题
      - 在处理器上处理，用USB接单片机和激光雷达，但他两的数据需要进行时间同步才能消除运动畸变。
    - 轮式里程计去除运动畸变也就是求解ts，te时刻的位姿 ps，pe然后根据所求位姿把所以激光点转换到同意坐标系下，重新封装一帧激光数据并发布
      - 二次插值
    - 二次曲线的近似
  - 激光数据发布
  - 作业