激光slam理论与实践

深蓝学院的激光slam课程

第一章：激光SLAM的发展和应用

激光SLAM的两大框架：滤波slam与图优化slam。
滤波的框架图为：

图优化的框架图为（谷歌的cartographer为代表）

二维激光slam

其中，PI-ICP是点对点匹配，已经不太用了。主要流行CSM及梯度优化。回环检测scan-to-map现在是主流，map-to-map更好但现在还没有普及，理论还不够丰富。分支界定是cartographer在用。

滤波现在也将逐渐被图优化代替。我自己跑过hector slam与cartographer，对比很明显，图优化确实棒！
三维激光slam

第二章：传感器数据处理：里程计运动模型及标定

里程计模型：两轮差分地盘的运动模型、三轮全向底盘的运动学模型、航迹推算
里程计标定：线性最小二乘基本原理、最小二乘直线拟合、最小二乘在里程计标定中的应用

两轮差分底盘的运动学模型
优点：结构简单、便宜（两个电机)、模型简单越障好。
差分模型：

三轮全向底盘运动模型

航迹推算

线性最小二乘
线性方程组：Ax=b;其中A为m行n列矩阵，当m>n时，超定方程组，方程组通常无解，此时需要最小二乘来寻找靠近真实的解。通解为:

例如：
最小二乘用于直线拟合问题

此时，所有的x的值即为方程Ax=b中的A，直线拟合中的a与b为所求，对应方程的x，测出的y对应方程的b。所以重要的是找到对应关系，根据通解即可求出x（即直线的a、b).

最小二乘在里程计标定中的作用

第三章：激光雷达运动畸变去除

激光雷达传感器测距原理：三角测距与飞行时间（TOF)
三角测距：

TOF

TOF精度更高更常用。

激光雷达的数学模型
现在广泛使用的是似然场模型，即采用高斯模糊理论。

运动畸变
产生原因：激光点数据不是瞬时获得的，激光测量时伴随着机器人的运动，激光帧率较低。
个人思考：激光有自己的坐标系，当激光运动到不同的位置，两个位置的激光坐标系也是需要转化的。但机器人以为自己是在一个位置测出来的，所以会出现畸变。如图所示

畸变去除
1、纯估计方法
2、里程计辅助方法

纯估计方法：ICP(迭代最近邻匹配 iterative closest point）–点对点匹配
即未知对应点的求解方法，采用极大似然估计方法。
流程：

• 寻找对应点
• 根据对应点，计算R与T
• 对点云进行转换，计算误差
• 不断迭代，直到误差小于某一值

VICP(速度估计ICP—velocity estimation ICP）
即ICP算法的变种，考虑了机器人的运动为匀速运动，进行匹配的时候同时估计机器人的速度。

里程计辅助：
直接测量机器人的位移和角度，具有较高的局部角度测量精度，具有较高的局部位置测量精度。用CPU读取激光雷达数据，同时单片机上传里程计数据，两者进行时间同步，在CPU上统一进行运动畸变去除。
流程：

• 已知当前激光帧的起始时间ts,te.
• 两个激光束间的时间间隔*t
• 里程计数据按照时间顺序存储在一个队列里。
• 求解当前帧激光数据中的每一个激光点对应的里程计数据（即机器人位姿）
• 根据求解的位姿把所有的激光点转换到同一坐标系下
• 重新封装成一帧激光数据发布出去

第四章：激光的前端配准算法

帧间匹配算法：

• ICP匹配方法
• PL-ICP匹配方法
• 基于优化的匹配方法
• 相关匹配方法及分枝定界加速

1.ICP匹配方法

数学描述为

分为已知对应点匹配与未知对应点匹配。其中，已知对应点匹配这种情况很少发生，

未知对应点匹配

2.PL-ICP方法

如图所示，点线匹配与点点匹配相比，误差函数不同，ICP为点点距离为误差，PL-ICP为点线距离为误差。因为激光数据为离散的而墙体为连续的，所以在前后匹配的两帧之间可能扫描到的点不是一个地方而强行匹配造成误差，所以点线匹配精度更高。

算法流程为

• 把当前帧的数据根据初始位姿投影到参考帧坐标系下（因不同时刻激光自身坐标系不同）
• 对于当前帧的点i,在参考帧下找到最近的两个点j1,j2
• 计算误差，并去除误差过大的点。
• 最小化误差函数（目标函数）

3 基于优化的方法

参考hector slam 算法。基于梯度下降法，易陷入局部极值。

在求解时会用到地图的拉格朗日双线性插值方法，要自己掌握。

4.相关匹配方法及分枝定界加速

主要指的是cartographer用到的方法。
帧间匹配似然场：高度非凸，存在很多的局部极值；初值非常敏感（初值选不好就会陷入局部极值，如上图所示）；进行暴力匹配，排除初值影响；过加速策略，降低计算量（分枝定界）；计算位姿匹配方差。

算法流程：

• 构造似然场
• 在指定的搜索空间内，进行搜索，计算每一个位姿的得分，寻找最优得分
• 根据上一步位姿的得分，计算本次位姿匹配的方差
  

分枝定界加速

其中，分枝定界能保证一定可以找到最优解。

---

作者：东晓子
来源：CSDN
原文：https://blog.csdn.net/qq_38338228/article/details/85271944
版权声明：本文为博主原创文章，转载请附上博文链接！

作者：东晓子
来源：CSDN
原文：https://blog.csdn.net/qq_38338228/article/details/85262348
版权声明：本文为博主原创文章，转载请附上博文链接！