激光雷达运动畸变的去除比里程计标定更重要，但也取决于用的雷达型号。我用的思岚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级别的。
激光雷达数学模型介绍（数学模型）
光束模型
把一条激光看做一个激光束，射到墙上时会有一个期望距离，在这个期望值的附近会形成一个高斯分布，这是正常的情况。ztk * 是期望值
如果有障碍物挡住了的话，也就是d>d1时，这样会形成一个指数分布
基本上是上面的两种情况。那激光雷达的光束模型就是我们给定一个机器人的位置xt，和地图m，来获得这一帧观测值zt的概率。我们默认认为每一个激光束都是线性无关的，是独立的。得到一个累计乘。
用矩阵形式表示就是
等式左边是这束激光的概率（之后都叫得分），反应的是当前这束激光与地图的重合程度，重合程度越好，得分也就越高，如果正好落在期望值的上面，那么就说这个得分就是1
激光发出的一帧激光数据与地图的重合程度这个东西就是我们的光束模型，光束模型评估的就是这个。
光束模型的缺点（基本没人用光束模型，因为有缺点）
机器人在非结构化环境中，位姿发生微小的变化，就会造成期望值发生巨大的变化，从而导致得分突变。
就是说当前机器人正前方1m有障碍物，障碍物后面3m是一堵墙，激光发出激光束可以正常射到这个障碍物上，但是如果此时机器人的位姿发生微小变化，很有可能这个光束就射到障碍物后面的墙上去了，从而导致这个期望值发生突变。他是病态的，并且是无法进行优化的。所以说在复杂环境中不会使用这种
似然场模型
他是将所有的噪声，例如传感器噪声，机器人位姿噪声，等等…，得到一个高斯分布，用这个高斯分布对整个图像进行模糊，对障碍物进行高斯平滑，越白表示得分越高，越黑代表得分越低，他是不需要计算期望值的，我们对图像进行模糊之后，已经存在障碍物和模糊过的图像了，也就是说把之前计算期望值，然后用高斯分布算出一个得分换成查表的形式，查表的话，这个似然场从程序一开始就是在进行离线计算，所以对一帧激光来说，只需要查360次表，查表的计算量可以忽略不计。他是同时适合结构化环境和非结构化环境的。因为就算位姿发生一点改变，他的得分也不会太低。AMCL代码中两种模型都有体现。
运动畸变的介绍（原因与效果）
示意图
机器人不动，肯定不会产生运动畸变，如果机器人的运动频率很高，假设50Hz也就是2ms，如果认为在这2ms内机器人没动，那么运动畸变可以忽略，但是，在一般现实机器人中，运动频率是在5~10Hz左右，那么这个运动畸变就会很大了。反正就是说在雷达在测量的过程中，机器人如果正在运动，那么就会产生运动畸变。他的根源就是每个激光点都有不同的基准位置，但我们处理的时候都认为所有的激光点还是在同一位置。所以我们今天要求解的就是这每一个激光点的位置。
纯估计方法（不借助任何设备，纯靠匹配过程中去除）
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
然后对点云进行转换，计算误差
然后就不断迭代，直至误差小于某一个值（这个迭代的过程）
参考：https://blog.csdn.net/weixin_35695879/article/details/103125575
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已知了，然后可以反推出来每一个点的位姿是多少，然后再根据每一个点的速度对激光数据进行校正。
算法流程
里程计辅助方法（辅助方法的其中一种）
使用里程计辅助去除
VICP的缺点
解决方法
传感器辅助方法（里程计/IMU）
惯性测量单元（IMU）与轮式里程计传感器辅助
已知数据：1.已知当前 帧激光的起始时间 𝑡𝑠,𝑡𝑒
2.两个激光束间 的时Δ𝑡
3.里程 计数据按照时间顺序存储在一个队列 中
4.最早的里程计数据时间戳 <𝑡𝑠
5.最早的里程计数据时间戳 >𝑡𝑒。
他要做的就是1.求解当前帧激光数据中每一个点对应的机器人位姿，即求解｛ 𝑡𝑠,𝑡𝑠+Δ𝑡,⋯,𝑡𝑒｝时刻的机器人位姿。2.根据求解的位姿把所有的激光点转换到同一坐标系下。3.重新封装成一帧激光数据，发布出去。
过程：
1.求解ts，te时刻的位姿ps，pe
里程计队列中正好和激光数据同步，假设第 i和第 j跟数据是时刻分别为 𝑡𝑠,𝑡𝑒:
𝑝𝑠=𝑂𝑑𝑜𝑚𝐿𝑖𝑠𝑡[𝑖]
𝑝𝑒=𝑂𝑑𝑜𝑚𝐿𝑖𝑠𝑡[𝑗]
在𝑡𝑠时刻没有对应的里程计位姿，则进行线性插值设在 l,k时刻有位姿，且 l<s<k,则：
𝑝𝑙=𝑂𝑑𝑜𝑚𝐿𝑖𝑠𝑡[𝑙]
𝑝𝑘=𝑂𝑑𝑜𝑚𝐿𝑖𝑠𝑡[𝑘]
𝑝𝑠=𝐿inar𝐼𝑛𝑡𝑒𝑟𝑝(𝑝𝑙,𝑝𝑘,𝑠−𝑙𝑘−𝑙)
2.进行二次插值
3.二次曲线的近似
4.坐标系统和激光数据的发布
一帧激光数据 n个激光点，每对应的位姿 𝑝1,𝑝2,⋯,𝑝𝑛通过上述介绍的方法插 值得到
𝑥𝑖为转化之前的坐标， 𝑥𝑖′为转换之后的坐标，则：
𝑥𝑖′=𝑝𝑖𝑇𝑥𝑖
把转换之后的坐标为激光数据发布出去：
𝑥𝑖′=（𝑝𝑥,𝑝𝑦）
𝑟𝑎𝑛𝑔𝑒=sqrt（𝑝𝑥∗𝑝𝑥+𝑝𝑦∗𝑝𝑦）
𝑎𝑛𝑔𝑙𝑒=𝑎𝑡𝑎𝑛2(𝑝𝑦,𝑝𝑥)
在单片机上处理，直接用单片机接雷达，直接在单片机上消除运动畸变（因为单片机测量的里程计数据和雷达数据是一起上发的，所以他两时间同步，就不会产生运动畸变），无需考虑时间同步的问题
在处理器上处理，用USB接单片机和激光雷达，但他两的数据需要进行时间同步才能消除运动畸变。
轮式里程计去除运动畸变也就是求解ts，te时刻的位姿 ps，pe然后根据所求位姿把所以激光点转换到同意坐标系下，重新封装一帧激光数据并发布
二次曲线的近似
激光数据发布
作业
