SLAM优化的常用模型

ref：无人物流配送车的 SLAM 与路径规划研究

1. 算法概述

传统路径规划算法分为基于图搜索和基于采样两种，智能算法能在一定程度上弥补传统算法不足，比如图搜索的算法计算时间过长，采样的算法找到可能不是最优路径。

1. 基于图搜索算法主要有 Dijkstra 算法和 A*算法等：Dijkstra 算法采用贪心思想，由起始点向外层层拓展，直至拓展到目标点； Dijkstra 算法与广度优先搜索算法相结合所形成的 A*算法，相较于 Dijkstra 算法而言，A*通过设置启发式函数，减少了搜索节点数量，使该算法能够更快地找到最优路径。
2. 基于采样的算法主要有 RRT 算法及其变形和 PRM 算法等，可以规划出一条从起始点到目标点的路径，但不一定是最优路径：RRT 算法通过在起点开始随机采样空间中的点来构建树状结构，直到拓展到目标点；PRM 随机路线图通过将连续空间离散化，再使用 Dijkstra 和 A*等图搜索算法寻找最优路径。
3. 蚁群算法，是一种基于蚂蚁觅食行为的启发式优化算法，模拟了蚂蚁在觅食过程中的信息交流行为，通过信息素和启发式信息来引导搜索过程。
4. 遗传算法（GA）是一种模拟生物进化过程的优化算法，模拟了自然界中的生物遗传、交叉和变异等操作，并通过适应度评价和选择策略来进行搜索和优化。
5. 强化学习的基本思想是通过机器人与环境之间不断进行交互，依据环境给予的奖励反馈，让机器人朝着获得最大奖励的方向学习，进而获得最优策略，结合了强化学习的路径规划算法目的在于学习一个策略模型，在未知环境中搜索到最优路径。

7900cc241247c9267c388a346bb75b21.png

2. 模型

系统框架

41a4fe2697c12374e517d38e9c780976.png

激光雷达模型

0559a0fdf1b9afc25e23fef8ea132511.png

2dfa914e11e4e06ea3babe341df1e874.png

地图模型

8659bc5a7f87d5b9e2d01647622b572d.png

SLAM导航框架

44bfcc1ef0132f4dbacae43456938185.png

SLAM 位姿估计模型

76c5d5afa9e8775fbaf75d62ccdb4ee0.png

Gmapping算法流程（2D）

97f390f6ee11ce64fb830fe449c66765.png

Lego-SLAM算法框架（3D）

传统 Lego-loam 算法采用 ICP 方法对点云的匹配度进行估计计算，然后基于匹配度最高的历史帧对当前帧的位姿估计进行约束。另外，对激光雷达数据进行地面平面提取和地面点云去除，可以有效减少地面点云对定位和建图的干扰；然后对非地面点进行聚类，将点数较少的聚类（少于 30 点）视为噪声进行去除，以进一步减少干扰，最大程度保留静态物体。

5061503397f3eb747fcf7ef3f6037028.png

扫描上下文算法(Scan Context)改进的Lego-SLAM

激光雷达的点云数据可通过几何匹配方式进行回环检测，Scan Context 算法通过降维将 3D 点云转换为带有深度信息的 2D图像，对该图像进行描述符编码，通过比较描述符编码的相似度来实现回环检测，该方法采用非直方图的全局描述符来实现对当前数据和历史数据进行更快、更有效的搜索与匹配，Scan Context 对Lego_loam 算法的回环检测模块优化如下：

ad31dd60018ae910504635cbbe6e550b.png

马尔可夫决策模型

状态集合 S ，动作集合A ，状态转移概率集合P 以及奖励函数 R

7cd53b64bf8b3b879290cc8312d93852.png

Q 学习算法模型 Q 学习算法的核心是使用一个 Q 表格来记录状态动作对之间的对应关系。

路径连续性 贝塞尔曲线：通过控制点的位置和相对关系来调整曲线的形状，生成复杂的平滑曲线所需的控制点数量很少。

e707bb8374a25badb29e62e853e20cc7.png

B 样条曲线：逼近一组控制点来产生曲线，多项式的次数可独立于控制点的数目，但受到一定限制，且允许局部控制曲线和曲面生成曲线，本质上是在控制点前加了一个权重，再进行累加。按矢量节点的情况可分为四种：均匀 B 样条曲线、准均匀 B样条曲线、分段贝塞尔曲线和非均匀贝塞尔曲线

122a9089010b4fffdcfa1cecd096488c.png

0fe74433f390be9c0d4fef2359b1cf57.png