2D激光SLAM导航算法

REF：基于 ２Ｄ 激光雷达的 ＳＬＡＭ 算法研究综述

1. 2D激光SLAM原理

激光雷达主流的测距方法可以分为三角法与飞行时间法（time of flight, TOF）

• 三角法： 激光以一定的角度射向被测物体，反射光以一定的角度反射回光源旁的接收器，不同距离的被测物体在接收器上的成像会在不同的位置，通过三角公式就可以计算出被测物体的距离

39e5823bd14a5b1a5d3273dd1146b328.png

• 飞行时间法： 激光器发射一个激光脉冲，反射光由接收器接收，通过激光雷达中的计时器记录下发射、接收时间，可以计算出被测物体的距离

90028f1674ba7b4b7dd7a66b13bc9f23.png

• SLAM流程： ２D激光 SLAM的系统框架主要可以分为：前端 扫描匹配、后端优化、回环检测和地图构建
  1. 前端扫描匹配：可以分为基于点（前后两帧激光点集：ICP）、特征（激光点集提取点、线、面等元素）、数学特性（前后帧之间的各种数学特性：NDT）、相关性（似然场模型方差）和优化方法（非线性最小二乘问题的梯度下降法）的扫描匹配
  2. 后端优化：用来减少扫描匹配产生的累积误差，分为基于贝叶斯滤波（马尔可夫假设下，基于后验概率不同有卡尔曼滤波和粒子滤波）和图优化（调整位姿图中机器人位姿，以最大程度满足空间约束关系）的后端优化方法

  

  f5d8fe212e80ca9366404be1d505000b.png

  1. 回环检测：指机器人通过传感器测量的数据检测到自己又回到之前来过的位置，包括帧与帧（scan-to-scan，两帧激光雷达数据相对平移和旋转进行匹配）、帧与子图（scan-to-map，当前帧的激光雷达数据与子图进行匹配，该子图由一段时间内连续的激光数据帧构成）、子图与子图（map-to-map，当前连续时间内的激光数据帧构建成子图，与之前生成的子图进行匹配，改善单帧激光数据所含信息量少的问题）和多分辨率扫描匹配（当前激光数据帧和多分辨率地图进行匹配）的回环检测
  2. 地图构建：包括栅格地图（将环境空间划分成大小相等的栅格单元，每个栅格单元中含有被障碍物占据的概率值）、特征地图（由环境信息中提取到的点、线或圆弧等几何特征构成）等

2. 2D激光SLAM算法

主流的 ２Ｄ 激光SLAM可以分为：基于贝叶斯滤波、基于图优化和基于高斯牛顿优化的激光 SLAM。

1da16a89bfecbc7633d57fc8b59bc028.png

• 基于贝叶斯滤波： 通过联合状态向量来同时表示机器人的位姿和路标所处的位置，如Gmapping算法

b55f0265d72f53d62456915f1d3c519e.png

• 基于图优化： 通过最小二乘法来解决前端匹配所产生的累积误差，如Karto、Cartographer算法

ef884e26fbf20f4e52e910730c56dd12.png

63a5eb8af0535b82627a787cfc0cc78f.png

• 基于高斯牛顿优化： 当前帧的激光雷达数据与子图进行匹配，通过高斯牛顿法进行位姿优化，如Hector算法

5cf3819eae9ebfae6bdc8a249f8eb938.png