机器人SLAM导航学习-All in one-Toy模板网

这篇具有很好参考价值的文章主要介绍了机器人SLAM导航学习-All in one。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

参考引用

张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.

本博客未详尽之处可自行查阅上述书籍

移动机器人激光SLAM导航（文章链接汇总）

一、编程基础篇

1. ROS 入门必备知识

ROS学习笔记（文章链接汇总）

2. C++ 编程范式

《21天学通C++》读书笔记（文章链接汇总）

3. OpenCV 图像处理

《OpenCV 计算机视觉编程攻略》学习笔记（持续更新中）

二、硬件基础篇

1. 机器人传感器

1.1 IMU

1.1.1 定义

惯性测量单元 (Inertial Measurement Unit，IMU) 是用来测量惯性物理量的设备
- 比如测量加速度的加速度计、测量角速度的陀螺仪等
- 由于 IMU 具有非常高的测量频率，而相机、雷达、轮式里程计等测量频率较低，因此往往将 IMU 与相机或雷达或轮式里程计做融合会得到更好的测量效果
航空航天、无人机、自动驾驶汽车、机器人、智能穿戴等领域广泛使用 IMU 进行运动测量和状态估计，一般从 IMU 设备采集到的原始数据都存在较大的误差，所以需要对测量到的原始数据进行标定和滤波

1.1.2 常见术语解释

IMU 惯性测量单元，是测量物体三轴角速度和加速度的设备。一个IMU内可能会装有三轴陀螺仪和三轴加速度计，来测量物体在三维空间中的角速度和加速度。严格来说 IMU 只为用户提供三轴角速度以及三轴加速度数据
VRU 垂直参考单元，是在 IMU 的基础上，以重力向量作为参考，用卡尔曼或者互补滤波等算法为用户提供有重力向量参考的俯仰角、横滚角以及无参考标准的航向角。通常所说的 6 轴姿态模块就属于这类系统
MARG 是指能测量磁力、角速度、重力这三个物理量的传感器。可以说，IMU 是 6 轴传感器，而 MARG 是 9 轴传感器，MARG 比 IMU 多了一个 3 轴的磁力计（一般组合导航不用磁力计，干扰问题不好解决）
AHRS 是指航姿参考系统，即以地球重力场和磁场为参考，计算导航物体的空间姿态（即 “航姿”）。AHRS 比 MARG 多了一个航姿计算的过程，通常所说的 9 轴姿态传感器就属于这类系统
GNSS/INS 组合导航系统，是将两种或以上的导航设备组合在一起使用的导航系统，利用全球卫星导航系统 (GNSS，它是 GPS，北斗，GLONASS、GALILEO 等系统的统称) 与惯性导航 (Inertial Navigation System) 各自的优势进行算法融合，能够提供更精确、全面的导航信息，比如航姿信息位置信息、速度信息等

1.2 激光雷达（LiDAR）

1.2.1 定义

雷达是指利用探测介质探测物体距离的设备，比如无线电测距雷达、激光测距雷达、超声波测距雷达等
- 由于激光具有很好的抗干扰性和直线传播特性，因此激光测距具有很高的精度，基于激光测距原理的激光雷达，测距精度往往可以达到厘米级或毫米级
- 激光雷达测距方式主要是三角测距和 TOF (Time of Fly，飞行时间) 测距两种
- 在软件层面上，通常是在上位机电脑上运行 ROS 驱动程序获取雷达扫描数据，对扫描数据进行滤波等必要的数据处理，最后扫描数据就交给上层的建图、避障、导航等算法进行运算
- 激光雷达数据与轮式里程计、IMU 和相机等进行多传感器融合，往往效果更好

1.2.2 激光雷达的两种测距原理

三角测距
- 随着物体距离不断变远，反射激光在图像传感器上的位置变化会越来越小，也就是越来越难以分辨。这正是三角测距的大缺点，物体距离越远，测距误差越大
TOF 测距
- 由于光速传播太快，要获取精确的传播时间很难。所以这种激光雷达自然成本也会高很多，但是其测距精度很高

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

上面介绍测距原理时，只是单个激光器，也就是说只能扫描单个点，这个装置也叫单线激光测距模组。激光雷达中，将单线激光测距模组安装到旋转机构中，就能对环境进行多点扫描
- 激光雷达中的旋转机构需要考虑的关键问题是通信与供电问题，这也直接决定了激光雷达的性能和寿命。因为激光测距模组处于旋转状态，所以模组不能直接用导线与底座连接，行业内一般用滑环或光磁耦合实现连接
  - 1、滑环是机械式的触点，旋转过程中触点始终保持接触，实现电路的连接导通，缺点是使用久了会磨损，机械触点也会导致旋转发出很多噪音
  - 2、光磁耦合不需要接触式的连接，模组与底座通过电磁感应进行电能输送，另外模组和底座之间有一一对应的发光二极管和光接收二极管用于光通信，成本更高

1.2.3 常见激光雷达类型

单线激光雷达
- 单线激光模组和旋转机构构成了单线激光雷达，单线激光雷达扫描点通常处在同一平面上的 360 度范围内，因此也叫 2D 激光雷达
多线激光雷达
- 单线激光雷达只能扫描同一平面上的障碍信息，也就是环境的某一个横截面的轮廓，这样扫描的数据信息很有限。在垂直方向同时发射多束激光，再结合旋转机构，就能扫描多个横截面的轮廓，这就是多线激光雷达，也叫 3D 激光雷达
固态激光雷达
- 固态激光雷达的扫描不需要机械旋转部件，而是用微机电系统、光学相控阵、脉冲成像等技术替代。固态激光雷达的优点是结构简单、体积小、扫描精度高、扫描速度快等缺点是扫描角度有限、核心部件加工难度大、生产昂贵等

1.2.4 激光雷达性能参数

激光线数
- 是指测距模组发射激光束的个数，一般有单线、16 线、32线、64 线、128 线激光线数越多，扫描到的信息越多，相应的数据处理所消耗的计算资源也将更大
测距频率
- 也叫测距模组的采样率，是指每秒模组能完成的测距操作的次数。采样率越高对雷达硬件的性能要求也越高
扫描频率
- 是指带动测距模组旋转的电机每秒钟转过的圈数，扫描频率越高，获取一圈扫描点数据帧的时间越短，对障碍物的探测和避让实时性更好
测距量程
- 是对障碍物能有效探测的范围，只有落在量程范围内的障碍物才能被探测到。小于量程下限值的范围就是盲区，大于量程上限值的范围就是超量程区域。通常机器人会在必要部位安装其他避障传感器来弥补激光雷达的盲区问题
扫描角度
- 也可以说是雷达的探测视野，通常都是 360° 全方位的探测视野。不过有一些激光雷达由于构造的原因，有一些角度范围被外壳等结构遮挡，所以探测视野会是 270° 或 180°
- 比如，当激光雷达被安装在机器人的正前方时，由于雷达后面的视野完全被遮挡了，所以有效的数据只有前面视野部分，将雷达的扫描角度设置成 180° 就很有必要，这样既能去除被遮挡区域的无效数据，又能因数据量的减少而加快算法运算效率
距离分辨率
- 测距精度，测距精度越高越有利于机器人的导航避障。基于三角测距原理的雷达，测距精度在厘米级，精度随测量距离增大而增大，这也是三角测距的典型缺点。基于 TOF 测距原理的雷达，测距精度可达毫米级，当然价格也更高
角度分辨率
- 两个相邻扫描点之间的夹角，由于雷达是通过旋转进行扫描的，随着距离增加，点云会越来越稀疏。如果角度分辨率比较低，在扫描远距离物体时只能得到非常稀疏的几个点云，这样的点云基本上没有什么用
- 角度分辨率 = 扫描频率 × 360° / 测距频率
使用寿命
- 采用机械滑环连接的激光雷达连续工作寿命只有几个月，而采用光磁耦合连接的激光雷达连续工作寿命可长达几年时间。滑环结构的雷达寿命短，价格也便宜，光磁耦合的雷达寿命长，价格也高一些

1.2.5 激光雷达数据处理

上位机 ROS 驱动程序

将装配到机器人上的激光雷达用串口或网口与上位机进行连接，上位机一般会运行 Ubuntu 和 ROS 系统
ROS 解析雷达传回的数据并发布到 /scan 话题中，这样其他 ROS 程序就可通过订阅该话题来获取雷达数据

扫描点处理

通过驱动程序获取到激光雷达的扫描数据后，一般要对数据进行一些必要的预处理
- 单线激光雷达在 ROS 中发布的数据格式通常是 LaserScan 类型
  - 其实就是用极坐标的方式表示扫描点的坐标值。单线激光雷达的数据处理相对容易，比如过滤掉特定范围内的无效点、扫描角度范围截取、插值等，如功能包 laser_filters
- 多线激光雷达在 ROS 中发布的数据格式通常是 PointCloud2 类型
  - PCL (Point Cloud Library，点云库) 是处理点云数据非常有名的工具。PCL 中包含点云滤波、特征点提取、点云配准、点云分割、点云曲面重建等操作

扫描点时间同步

由于激光雷达安装在机器人上，当机器人移动时，激光雷达自身的旋转扫描就会受到影响
- 当机器人静止时，激光雷达旋转一圈扫描到的点序列都是以机器人当前静止位置作为参考的，激光雷达的测距误差仅来自测距方法本身
- 当机器人移动时，因为激光雷达并不知道自身处于移动状态，所以激光雷达旋转一圈扫描到的点序列依然是以该帧时间戳时刻的机器人位置为参考，此时激光雷达的测距误差除来自测距方法本身外，还来自机器人运动产生的畸变
  - 例如，当机器人以雷达相同大小的角速度反方向旋转时，雷达一帧扫描点 $\{P_0,P_1,P_2,\cdots\}$ 其实是对 P 点的重复多次测量，但是雷达并不知道自身的运动，依旧把各个扫描点当成机器人在起始时间戳位置静止状态参考下的测量，也就是雷达畸变数据点 $\{P_0^{\prime},P_1^{\prime},P_2^{\prime},\cdots\}$ ，显然畸变数据远远偏离了实际情况，如下左图所示
  - 再如，当机器人匀速前进时雷达一帧扫描点 $\{P_0,P_1,P_2,\cdots\}$ 其实是机器人在不同位置上的测量，但是雷达并不知道自身的运动，依旧把各个扫描点当成机器人在起始时间戳位置静止状态参考下的测量，也就是雷达畸变数据点 $\{P_0^{\prime},P_1^{\prime},P_2^{\prime}\cdots\}$ ，显然畸变数据远远偏离了实际情况，如下右图

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

校正方法
- 纯估计法
  - 不借助额外的传感器，只利用激光雷达的数据对机器人的运动进行估计，并利用估计的运动信息对雷达扫描数据进行补偿，这就是纯估计法
  - 已知机器人中两帧雷达扫描数据 $X^{i-1}$ 和 $X^{i}$ ，求机器人的位置转移关系 T，也就是激光里程计的问题，最常用的求解方法是 ICP(Iterative Closest Point，最近邻点迭代) 算法，用位置转移关系 T 近似表示其当前的运动 V，然后利用这个运动信息 V 对当前的雷达数据 $X^{i}$ 做补偿
- 里程计辅助法
  - 采用外部的 IMU 或者轮式里程计来提供机器人的运动信息，就可直接对雷达运动畸变做校正，这就是里程计辅助法。IMU 虽然可以提供极高频率的姿态更新，但是存在很高的累积误差，选用轮式里程计会更稳定（也存在累计误差，但相对更小）
  - 下图所示，轮式里程计采集到的机器人位姿序列点为 $\{p_1,p_2,p_3,\cdots\}$ ，激光雷达一顿扫描数据点为 $\{x_1,x_2,x_3,\cdots\}$ ，由于里程计序列和激光雷达序列之间点的个数不匹配和时间上不对齐，为了让里程计与激光雷达点之间能一一对应起来，因此对 $\{p_1,p_2,p_3,\cdots\}$ 做二次曲线插值得到 $\{p_1^{\prime},p_2^{\prime},p_3^{\prime},\cdots\}$ ，接下来就可以利用对齐后的里程计位姿 $p_{k}^{\prime}$ 对激光雷达点 $x_{k}$ 逐一进行校正。最后，将校正后的雷达点 $x_k^{\prime}$ 转换回极坐标的形式 $(\rho,\theta)$ ，重新发布出去就可以了
可以先利用 IMU 或轮式里程计进行初步校正，再利用 ICP 迭代修正

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

1.3 相机

1.3.1 单目相机

特点
- 单目相机其实就是通常说的摄像头，由镜头和图像传感器(CMOS 或 CCD)构成
- 简单点说，摄像头的原理就是小孔成像，成像信息由图像传感器转换成数字图像输出
- 利用对应的摄像头驱动程序，就可以读取并显示该数字图像
- 受摄像头制造偏差等因素的影响，原始的数字图像存在畸变问题，需要对摄像头进行标定
- 最后图像数据可以被压缩成不同的形式，便于传输和访问
- 单目相机无法测量物体的深度信息
上位机 ROS 驱动程序
- 按照视频传输协议的不同，相机设备的接口主要分为 DVP、LVDS、MIPI、USB 等接口类型
- 将 USB 相机直接用 USB 线连接到上位机，上位机运行 Ubuntu 和 ROS 系统，在上位机上运行对应 ROS 驱动程序，解析相机传回来的数据并将结果发布到 ROS 话题 <cam_name>/image_raw 之中，这样上位机上的其他 ROS 程序就可以通过订阅该话题来获取图像数据

1.3.2 双目相机

单目相机无法测量物体点的深度信息，用两个单目相机组成的双目相机就可测量深度信息，也把双目相机叫深度相机

上位机 ROS 驱动程序
- 双目相机的 ROS 驱动图像发布主要有两种方式
  - 一种是双目相机直接被上位机系统识别成两个独立的单目相机设备，按单目相机的方法分别读取左、右相机的图像并分别发布到对应的话题
  - 另一种是双目相机上传的左图、右图拼接在起形成一张大图，上位机可以直接发布这张拼接的大图，也可以将大图拆分后分别发布

1.3.3 RGB-D 相机

双目相机虽然能测量深度信息，但是需要事先找到同一物体点在左右相机中成像点对，也就是要先匹配。匹配过程很容易受到光照强度等环境因素干扰，在没有特征的环境中匹配会失效，深度信息将无法测量

RGB-D 相机是主动测量深度的传感器，受环境的干扰会小一些。RGB-D 相机一般有 3 个镜头：中间的镜头是普通的摄像头，采集彩色图像，另外两个镜头分别用来发射红外光和接收红外光
有时候机器人上只安装了 RGB-D 相机，而没有激光雷达。如果要运行激光 SLAM，可以将 RGB-D 相机的深度图转换成激光雷达扫描图，就可以运行激光 SLAM 了。转换用的功能包是 depthimage_to_laserscan
- 转换原理：按某个高度平面截取深度图，截取出来的轮廓作为激光雷达扫描点发布
由于 RGB-D 相机的视野角度小，就激光 SLAM 建图效果看，没有 360 度扫描的激光雷达建的地图好。所以，通常不用 RGB-D 相机建图，而只做避障用

1.4 带编码器的减速电机

1.4.1 电机和编码器

按照电机的电源类型分为直流电机、交流电机，按照电机的换相方式分为有刷电机、无刷电机，按照电机转子的构造可以分为内转子电机、外转子电机
电机在实际的使用中，往往配合减速箱一起使用，来提高输出力矩和载重性能。减速箱有普通的减速箱和行星减速箱。行星减速箱的优势是在同等减速比条件下，减速箱体积可以更小，一般一些对体积敏感的场景用得多
编码器是对电机转速测量反馈的重要部件，有霍尔编码器、光电编码器、碳刷编码器，通常采用正交编码形式进行信号输出，也就是 AB 两相信号正交输出，有些还等会有 Z 相信号对转过一圈进行脉冲输出

1.4.2 电机控制主板

上、下位机主板方案对比
- 像树莓派、Jetson-tx2 等上位机主板，一般都会搭载 Linux 等操作系统，虽然操作系统中运行的程序看上去是实时的，可是硬件 I/O 这类操作要求的是绝对的实时性，并且不能出现任何差错，尤其是电机这种持续运行的硬件，带操作系统的上位机主板就不能达到这些要求
- 而 STM32、Arduino 这一类的下位机单片机原本就是为硬件控制驱动等应用而生的，运行的是裸机程序，由硬件电路直接保证绝对实时性，并且用下位机单片机来控制电机可以实现功能解耦合，更利于整个上层大系统的维护

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

PID 控制
- 如果给定一个指定的 PWM 值来控制电机以速度 V 转动，实际情况中器件偏差噪声干扰、负载变化等因素，电机并不会保持运行速度 V。为了达到控制电机稳定转动的目的，就需要引入反馈控制机制，电机如果转慢了，就加大控制量，如果转快了就减小控制量。而 PID (Proportional Integral Derivative，比例积分微分) 是首选算法
- 电机速度控制属于速度环控制，以 PWM 信号控制输人，以编码器进行速度反馈，选用增量型 PID 会更合适。等实际的电机控制系统软硬件都搭建完毕后，还需要对 PID 的 3 个参数进行整定，以使控制达到更佳的效果
通信协议
- 在单片机上实现好电机的 PID 控制后，还需要将速度控制封装成接口，通过特定的协议让单片机和上位机通信
  - 单片机反馈编码器值给上位机，上位机发送速度控制值和一些配置参数给单片机
- 这里，上位机与单片机采用串口通信
  - 基于串口通信的通信协议，可以采用 ROS 社区提供的现成的 rosserial 库中封装的协议，也可以自定义一套符合自己项目需要的协议

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

1.4.3 轮式里程计

一旦单片机按照通信协议封装好后，对于上位机来说，下位机的整个系统其实就是一个 API 接口，只需要按照协议调用就行了。有了这个 API 接口，就可以开发上位机的 ROS 驱动程序，如下图所示
在驱动程序运行中，一方面按照通信协议与下位机交换数据，另一方面向 ROS 中发布里程计话题 /odom 和订阅速度控制话题 /cmd_vel。通信协议数据的收发、解析相对容易，但根据下位机传回来的编码值求解里程计是核心与难点
利用轮式编码器反馈，经过航迹推演算法，得到底盘当前时刻位姿，整个过程就是机器人技术中常说的轮式里程计

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

2. 机器人主机

2.1 X86 与 ARM 主机对比

X86 和 ARM 是指计算机处理器的两种架构
- 个人台式电脑和笔记本电脑基本都是 X86 架构
- 而像手机、智能终端、物联网设备等一般都是 ARM 架构
X86 是通用型架构，基本上对大多数厂商的硬件、驱动程序、系统软件都有很好的支持，兼容性比较好
ARM 是精简型架构，因为具有精简指令的灵活性和低功耗等特点，广泛应用在嵌人式领域，但硬件厂商、驱动程序、系统软件这些就没有像 X86 那样好的支持性了，很多都需要自己裁剪、移植来适应具体的主板型号
这里讨论的是机器人主机，台式电脑和笔记本电脑不在讨论范围内
- 如果说要在机器人上选择 X86 主机，推荐使用 Intel-NUC，它的体积比较小，性能较好，缺点是功耗高，价格也远高于同性能的 ARM 主机
- ARM 主机非常适合在机器人上使用，推荐三款 ARM 主机，即：树莓派 3B+、RK3399、Jetson-tx2
  - 如果是像扫地机器人那样只需要运行激光 SLAM，就可以选择树莓派 3B+ 作为机器人的主机
  - 如果要运行视觉 SLAM，就需要选中高端的 RK3399 作为机器人的主机
  - 如果除了 SLAM 外，机器人还需要运行神经网络等深度学习算法来实现人脸识别、语言交互、语义地图等，就需要选择高端的 Jetson-tx2 作为机器人的主机

2.2 分布式架构主机

2.2.1 ROS 网络通信

ROS 网络通信是一种分布式的计算机通信方式，可以为运行在不同主机中的 ROS 节点间通信提供接口
- 机器人和工作台上的 ROS 节点是运行在 Ubuntu 系统上的，一般基于 roscpp 和 rospy 库来编写
- 手机上的 ROS 节点是运行在 Android 系统上的，一般基于 rosjava 库来编写
- 不管这些 ROS 节点是运行在什么设备中，只要将各个设备连接到同一台路由器组成一个局域网，并在每台设备上都配置好 ROS 网络通信的环境变量，ROS 网络通信就设置完成
ROS 网络通信是中心式结构，也就是参与 ROS 网络通信的所有主机必须指定一台主机作为 master (主节点) 负责整个 ROS 网络通信的管理工作，而参与 ROS 网络通信的所有主机都要向外声明自己的 host 身份
- 也就是每台主机中，都要设置 master 和 host 两个环境变量，master 和 host 的取值均为局域网内主机的真实 IP 地址
- 一般以机器人作为 master 即所有主机中的 ROS_MASTER_URI 环境变量取值都要设置成机器人的 IP 地址值
- 而参与 ROS 网络通信的所有主机各自都是一台 host，也就是每台主机中的 ROS_HOSTNAME 环境变量取值都设置成该主机自己的 IP 地址值

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

2.2.2 机器人程序远程开发

通过 ssh 工具从工作台远程登录到机器人主机，然后就可远程编辑和启动/停止机器人上的程序，远程登录命令如下
```
$ ssh 主机用户名@主机ip
```
远程登录到机器人后，在登录终端只能用 vim 编辑一些简单的程序代码。如果要修改的代码涉及的文件比较复杂，可以将机器人文件远程挂载到工作台，然后在工作台直接打开编辑
远程登录到机器人后，该终端下执行的所有命令其实最终都是在机器人本地运行，只不过通过 ssh 进行命令的传递。在登录后的终端下运行 rviz 这类 GUI 程序而报错，因为 ssh 只能传递文本信息到工作台端，无法传递 GUI 信息

3. 机器人底盘

3.1 两轮差速模型

两轮差速模型可以说是最简单的底盘模型，在底盘的左右两边平行安装两个由电机驱动的动力轮，考虑到至少需要 3 点才能稳定支撑，底盘上还需要安装用于支撑的万向轮

3.1.1 前向运动学

底盘中各个动力轮的速度可以由编码器值计算得到，那么通过各个轮子的速度求底盘整体的运动速度，即前向运动学。已知左动力轮的线速度为 $V_L$ ，右动力轮的线速度为 $V_R$ ，动力轮的线速度可以用编码器计数值计算，如下式所示
$V=\frac{M}{P}\cdot N\cdot2\pi\cdot R\cdot\frac{1}{\Delta t}$
- 其中 M 为采样周期内编码器的计数值，P 为编码器的线数，即编码盘旋转一圈触发的总脉冲数，N 为电机减速比（为小于 1 的分数），R 为轮子的半径， $\Delta t$ 为编码器采样周期
在很短的时间 t 内，底盘可以看成绕 O 点做圆周运动，两个圆周的半径分别为 $\rho$ 和 $\rho+d$ ，其中 d 为两轮轴距，两个轮子走过的路程分别为 $S_L$ 和 $S_R$ ，转过的角度为 $\varphi$ ，底盘速度瞬心走过的路程为 s， $S_L$ 和 $S_R$ 满足

$\begin{aligned}s_\mathrm{R}&=\varphi\cdot(\rho+d)=V_\mathrm{R}\cdot t\\s_\mathrm{L}&=\varphi\cdot\rho=V_\mathrm{L}\cdot t\end{aligned}$

$\begin{aligned} &\varphi=\frac{s_{\mathrm{R}}-s_{\mathrm{L}}}{d}=\frac{V_{\mathrm{R}}-V_{\mathrm{L}}}{d}\cdot t \\ &s=\varphi\cdot\left(\begin{array}{c}\rho+\frac{d}{2}\\\end{array}\right)=\varphi\cdot\rho+\varphi\cdot\frac{d}{2}=s_{L}+\frac{s_{R}-s_{L}}{2}=\frac{s_{R}+s_{L}}{2}=\frac{V_{R}+V_{L}}{2}\cdot t \end{aligned}$

由于运动时间 t 很小，利用极限的思想， $\varphi$ 趋近于 0，s 也趋近于 0，那么 s 可以等效为底盘速度瞬心轨迹圆弧切线方向的一小段位移，有了这个近似假设条件，用 s 和 $\varphi$ 直接除以 t 就可以求出底盘的整体速度 $v_{x}$ 与 $\omega_{z}$

$\begin{bmatrix}v_x\\\omega_z\end{bmatrix}=\begin{bmatrix}\dfrac{1}{2}&\dfrac{1}{2}\\-\dfrac{1}{d}&\dfrac{1}{d}\end{bmatrix}\begin{bmatrix}V_\mathrm{L}\\V_\mathrm{R}\end{bmatrix}$

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3.1.2 逆向运动学

$\left[\begin{matrix}V_{\mathrm{L}}\\V_{\mathrm{R}}\end{matrix}\right]=\left[\begin{matrix}1&-\dfrac{d}{2}\\1&\dfrac{d}{2}\end{matrix}\right]\left[\begin{matrix}v_{x}\\\omega_{z}\end{matrix}\right]$

3.1.3 轮式里程计

底盘在运动过程中，轮式编码器可以反馈每个轮子的转速，经过前向运动学公式解算就可以得到底盘的整体速度 $v_x$ 和 $\omega_z$ 。基于 $v_x$ 和 $\omega_z$ ，就可以由前一时刻底盘的位姿 $\boldsymbol{P}_{k-1}$ 推算出当前时刻底盘的位姿 $\boldsymbol{P}_{k}$ ，这个推算过程也叫 “航迹推演算法”，由于底盘不是严格的质点，所以底盘的位姿代表的是底盘速度瞬心在世界坐标系的位姿
利用轮式编码器反馈，经过航迹推演算法，得到底盘当前时刻位姿，整个过程就是机器人技术中常说的轮式里程计
下面具体分析一下轮式里程计的解算过程，如下图所示
- 机器人学中通常采用右手坐标系，底盘的正前方为 x 轴正方向、底盘的正左方为 y 轴正方向、底盘的正上方为 z 轴正方向、航向角 $\theta$ 以 x 轴为 0 度角并沿逆时针方向增大
- 一般在底盘上电时刻，在底盘的位姿处建立里程计坐标系 (odom)，随时间推移底盘的实时位姿 $P_0, P_1, P_2, P_3,\cdots, P_n$ 连接起来就形成了底盘的航行轨迹。同时，底盘自身的坐标系 (robot) 建立在其速度瞬心处
- 建立起 robot 坐标系中的速度量 $v_x$ 和 $\omega_z$ 与 odom 坐标系中的底盘位姿 $\boldsymbol{P}_{k}$ 之间的关系，就能解算出轮式里程计。在二维平面中移动的底盘，可以用 $\boldsymbol{P}=[X,Y,\theta]^\mathrm{T}$ 表示底盘的坐标和航向（即底盘的位姿），那么位姿 $\boldsymbol{P}$ 满足下式所示微分方程
  $\dot{\boldsymbol{P}}=\left[\begin{array}{c}\dot{X}\\\dot{Y}\\\dot{\theta}\end{array}\right]=\left[\begin{array}{ccc}\cos\theta&-sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{array}\right]\left[\begin{array}{c}v_x\\v_y\\\omega_z\end{array}\right]$
  - 其中，两轮差分底盘不发生侧向滑动，故 $v_y=0$
- 将 $\dot{\boldsymbol{P}}$ 在时间 t 上进行积分，就可以求出底盘实时的位姿，也就是里程计信息。实际情况下，需要在离散时间域上进行计算，由于底盘相邻两位姿相隔时间 $\Delta{t}$ 很小，积分运算可以用下式的累加运算来替代
  $\boldsymbol{P}_k=\boldsymbol{P}_{k-1}+\dot{\boldsymbol{P}}_{k-1}\cdot\Delta t$
- 由于里程计提供底盘在 odom 坐标系中的位姿，而 odom 坐标系与底盘上电启动时所处的地方有关，也就是说里程计中的底盘位姿是一个相对值。另外，里程计随时间的推移存在较大的累积误差，所以一般都是取不同时刻之间的里程计差值并应用于其他算法。由于轮式里程计只能提供底盘的局部位姿，并且存在较大累积误差，因此需要结合地图及 SLAM 重定位技术来获取全局位姿

3.2 四轮差速模型

四轮差速模型与两轮差速模型非常相似，主要靠左右两边轮子的转速差实现转弯，同一边的两个轮子的转速是完全一样的，即前轮和后轮的速度是同步的
实现前、后轮速度同步的方式主要有 3 种
- 履带连接方式：直接将前轮和后轮用履带进行连接，在机械上实现前后轮速度同步
  - 履带底盘因接触地面面积的增大而具有很好的越野性能，但也存在较大弊端，因此在对轮式里程计精度要求较高的应用场景，很少选用履带底盘
- 链条连接方式：用链条将前、后轮的输出轴连接在一起，在机械上实现前后轮速度同步
- 电控方式：在玩具和简易底盘等要求低成本的场景下，为了避免设计复杂的机械结构常常使用 4 个独立的电机控制 4 个轮子，并通过控制主板实现前轮和后轮的速度同步
  - 后两种方式是各个轮子单点接触地面，所以轮式里程计精度比履带底盘中的要高
在有些对底盘控制和里程计精度要求不是特别高的场景，也常常将这种四轮底盘简单等效为两轮差速模型来处理。等效的条件是轮子不发生侧向滑动，当底盘的长宽比足够大时，也就是前轮、后轮距离很近，左轮、右轮距离很远，底盘自转时才会足够顺畅，保证轮子不发生侧向滑动。但是实际四轮差速模型应用中，不可能把底盘的长宽比设计得过大，所以轮子转弯会存在侧向滑动。为了得到更高精度的里程计，就需要在四轮差速模型的侧向滑动情况下，对其前向运动学、逆向运动学和轮式里程计进行讨论

3.2.1 前向运动学

由于前、后轮的速度是同步的，可以以底盘几何中心 COG 沿 y 轴方向上的点 ICR 作为整个底盘进行圆周运动时的圆心，ICR 与 COG 的距离大小与圆周运动角速度大小有关
4 个轮子到 ICR 的距离为 $d_i$ ，轮子的实际速度 $v_i$ 是侧向滑动速度 $v_{iy}$ 和预设目标速度 $v_{ix}$ 的合成速度，这里的 i = 1,2,3,4
在侧向滑动的四轮底盘中，底盘的速度瞬心在其质心 COM 处，而 COM 与 COG 往往是不重合的。可以用 COM 位置处的线速度 $v_c$ 和角速度 $\omega_c$ 表示整个底盘的运动速度，其中 COM 到 ICR 的距离为 $d_c$ 。 $v_c$ 垂直于 ICR-COM 线段， $v_c$ 不仅有预设目标速度 $v_{cx}$ 分量，还有侧向滑动速度 $v_{cy}$ 分量
底盘中左轮、右轮轴距为c，点 COM 与底盘后端及前端的距离分别为 a 和 b

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

四轮差速底盘的前向运动学关系（推理略）
$\begin{bmatrix}v_\mathrm{cx}\\\omega_\mathrm{c}\end{bmatrix}=\begin{bmatrix}\dfrac{1}{2}&\dfrac{1}{2}\\-\dfrac{1}{c}&\dfrac{1}{c}\end{bmatrix}\begin{bmatrix}V_\mathrm{L}\\V_\mathrm{R}\end{bmatrix}$

3.2.2 逆向运动学

$\begin{bmatrix}V_\mathrm{L}\\V_\mathrm{R}\end{bmatrix}=\begin{bmatrix}1&-\dfrac{c}{2}\\1&\dfrac{c}{2}\end{bmatrix}\begin{bmatrix}v_\mathrm{cx}\\\omega_\mathrm{e}\end{bmatrix}$

3.2.3 轮式里程计

基本同两轮模型，此处不赘述

3.3 阿克曼模型

阿克曼底盘通过前轮的机械转向，让底盘中的 4 个轮子在基本不发生侧向滑动的情况下能顺畅地转弯，但缺点是阿克曼底盘不能原地旋转，也就是最小转弯半径不为 0
在直线行驶时，4 个车轮的轴线互相平行。在进行转弯时，需要让 4 个车轮只沿车轮切线方向前进而不发生侧移，以避免侧移给车轮带来的巨大磨损。即 4 个轮子转弯弧线必须处于共同的圆心 ICR 之上，此时前轮内侧转向角 $\delta_{\mathrm{in}}$ 会大于外侧转向角 $\delta_{\mathrm{out}}$ 。在阿克曼转向几何中，利用梯形四连杆的相等曲柄控制前轮的转向，以实现 $\delta_{\mathrm{in}}$ 和 $\delta_{\mathrm{out}}$ 需要的约束关系，避免车轮发生侧移，让 4 个车轮能顺畅地转弯

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3.3.1 前向运动学

在前轮转向角 $\delta_{\mathrm{in}}$ 和 $\delta_{\mathrm{out}}$ 不变的瞬时状态下，4 个轮子绕同一个点 ICR 做圆周运动。左后轮、右后轮的线速度为 $V_L$ 和 $V_R$ ，后轮中轴位置 $O_{back}$ 为底盘的速度瞬心， $O_{back}$ 位置处的线速度 $v_{back}$ 和角速度 $\omega_{back}$ 为底盘的整体运动速度
$\left[\begin{matrix}v_\mathrm{back}\\\omega_\mathrm{back}\end{matrix}\right]=\left[\begin{matrix}\frac{1}{2}&\frac{1}{2}\\-\frac{1}{d}&\frac{1}{d}\end{matrix}\right]\left[\begin{matrix}V_\mathrm{L}\\V_\mathrm{R}\end{matrix}\right]$

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3.3.2 逆向运动学

$\begin{bmatrix}V_{\mathrm{L}}\\V_{\mathrm{R}}\end{bmatrix}=\begin{bmatrix}1&-\dfrac{d}{2}\\1&\dfrac{d}{2}\end{bmatrix}\begin{bmatrix}v_{\mathrm{back}}\\\omega_{\mathrm{back}}\end{bmatrix}$

在阿克曼底盘中，除了需要求左、右后轮的速度外，还需要求出前轮的转向角。前轮的转向角可以用两个前轮中轴位置的平均转向角 $\delta$ 表示
$\text{tan}\delta=\frac lR$
其中
$R=\frac{v_{\mathrm{back}}}{\omega_{\mathrm{back}}}$
从而求出
$\delta=\arctan\left(\frac{l\cdot\omega_{\mathrm{back}}}{v_{\mathrm{back}}}\right)$
求出的转向角 $\delta$ 不是最终控制量。还要将转向角 $\delta$ 映射成舵机的输出舵量 Angle，才能最终实现底盘转向。而转向角 $\delta$ 与舵量 Angle 之间的映射关系与舵机及转向悬臂具体安装位置有关系，通常都是通过实验标定法获取到转向角 $\delta$ 与舵量 Angle 之间的映射表 $\delta$ -Angle

3.3.3 轮式里程计

基本同两轮模型，此处不赘述

3.4 全向模型

速度向量约束

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

底盘的速度受到约束的原因是普通轮子不能侧向移动，只有切向移动一个自由度。如果把轮子换成包含切向和侧向两个自由度的轮子，则底盘速度不受约束，也就成了全向底盘。在业界，把这种具有两个自由度的轮子称为麦克纳姆轮

3.4.1 前向运动学

全向底盘可以用底盘中心处的线速度 $\vec{v}$ 和角速度 $\vec{\omega}$ 描述底盘的整体运动，其中线速度 $\vec{v}$ 由分量 $\vec{v_x}$ 和 $\vec{v_y}$ 合成， $\vec{v}$ 可以是任何方向，即取值不受约束
底盘中 4 个轮子的速度分别为 $\vec{v_1}$ 、 $\vec{v_2}$ 、 $\vec{v_3}$ 和 $\vec{v_4}$ 。底盘前轮、后轮轴距为 2a，左轮、右轮轴距为 2b

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

前向运动学公式
$\begin{bmatrix}v_x\\v_y\\\omega\end{bmatrix}=\dfrac14\begin{bmatrix}1&1&1&1\\-1&1&1&-1\\-\dfrac1{a+b}&\dfrac1{a+b}&-\dfrac1{a+b}&\dfrac1{a+b}\end{bmatrix}\begin{bmatrix}V_1\\V_2\\V_3\\V_4\end{bmatrix}$

3.4.2 逆向运动学

$\begin{bmatrix}V_1\\V_2\\V_3\\V_4\end{bmatrix}=\begin{bmatrix}1&-1&-(a+b)\\1&1&(a+b)\\1&1&-(a+b)\\1&-1&(a+b)\end{bmatrix}\begin{bmatrix}v_x\\v_y\\\omega\end{bmatrix}$

3.4.3 轮式里程计

基本同两轮模型，此处不赘述

三、SLAM 篇

1. SLAM 发展简史

最初，机器人定位问题和机器人建图问题是被看成两个独立的问题来研究
- 定位问题的关键是必须事先给定环境地图
  - 机器人定位问题，是在已知全局地图的条件下，通过机器人传感器测量环境，利用测量信息与地图之间存在的关系求解机器人在地图中的位姿
  - 比如分拣仓库中地面粘贴的二维码路标，就是人为提供给机器人的环境地图路标信息，机器人只需要识别二维码并进行简单推算就能求解出当前所处的位姿
- 建图问题的关键是必须事先给定机器人观测时刻的全局位姿
  - 机器人建图问题，是在已知机器人全局位姿的条件下，通过机器人传感器测量环境，利用测量地图路标时的机器人位姿和测量距离及方位信息，求解出观测到的地图路标点坐标值
  - 比如装载了 GPS 定位的测绘飞机，飞机由 GPS 提供全局定位信息，测量设备基于 GPS 定位信息完成对地形的测绘。很显然，这种建立在环境先验基础之上的定位和建图具有很大的局限性

将机器人放置到未知环境，前面这种上帝视角般的先验信息将不复存在，机器人将陷入一种进退两难的局面，即所谓的 “先有鸡还是先有蛋” 的问题。如果没有全局地图信息机器人位姿将无法求解，没有机器人位姿，地图又将如何求解呢?

CMU-自主探索导航系统（TARE & FAR Planner）学习-All in one

1986年，Smith 和 Cheeseman 将机器人定位问题和机器人建图问题放在基于概率论理论框架之下进行统一研究，其中有两个开创性的点
- 第一是采用了基于概率论理论框架对机器人的不确定性进行讨论
- 第二是将定位和建图中的机器人位姿量与地图路标点作为统一的估计量进行整体状态估计，这算得上是 SLAM 问题研究的起源
2006年，Durrant-Whyte 和 Bailey 在其研究课题中首次使用 SLAM 这一名词进行表述，给 SLAM 问题制定了详细的概率理论分析框架，并对 SLAM 问题中的计算效率、数据关联、收敛性、一致性等进行了讨论，可以认为这是 SLAM 问题真正进入系统性研究的元年
- 古典 SLAM 时期
  - 将机器人定位和建图问题转换成状态估计问题，在概率框架之中展开研究，利用扩展卡尔曼滤波(EKF)、粒子滤波(PF)等滤波方法来求解
- 现代 SLAM 时期
  - 在贝叶斯网络中采用滤波法求解 SLAM 的方法，需要实时获取每一时刻的信息，并把信息分解到贝叶斯网络的概率分布中去，可以看出滤波方法是一种在线 SLAM 系统，计算代价非常大
  - 鉴于滤波方法计算代价昂贵这一前提，机器人只能采用基于激光等观测数据量不大的测距仪，并且只能构建小规模的地图，这是古典 SLAM 鲜明的特征
  - 为了进行大规模建图，在因子图中采用优化方法求解 SLAM 的方法被提出，优化方法的思路与滤波方法恰恰相反，它只是简单地累积获取到的信息，然后利用之前所有时刻累积到的全局性信息离线计算机器人的轨迹和路标点，即优化方法是一种完全 SLAM 系统

2. 数据关联、收敛和一致性

2.1 数据关联

SLAM 建图是一个增量的过程，随着机器人不断对环境的探测，环境路标信息需要不断被加入已构建出来的地图中。所谓数据关联，就是将传感器观测数据与已构建出来的地图进行匹配，判断传感器观测数据中哪些路标特征是新的、哪些是旧的，并将新路标特征数据有效地融入地图中

2.2 收敛

收敛性用于衡量 SLAM 系统在理论上的可行性。由于机器人观测模型和运动模型都具有不确定性，所以将 SLAM 放在概率框架下讨论，并利用状态估计去求解，即估计量都是带有不确定性的估计值。由路标特征组成的地图是实际环境特征的估计，估计出来的路标特征与实际环境特征存在偏差的原因是观测存在不充分性。换句话说，只要给机器人足够的机会对环境进行彻底的观测，所得到地图的不确定性将最终消除，即构建出来的地图路标特征收敛于实际环境特征

2.3 一致性

不仅要关注估计问题的收敛性，还需要关注收敛的一致性。收敛一致性讨论的是估计量收敛于实际数值的问题，下图展示了收敛一致性问题
- 其中， $\theta$ 是待估计参数真值， $\hat{\theta}$ 是估计量， $Z=\{z_1,z_2,\cdots,z_k\}$ 是当前能获取到的 k 个观测值
- 第一种收敛并不能保证估计量的取值一定与真值一致，而第二种收敛能保证估计量的取值一定与真值一致。考虑到机器人中参数随时间变化的动态性，比如机器人的位姿会经常移动，那么估计不仅要具有良好的一致性，还要能在少量观测值条件下快速收敛

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3. 激光 SLAM 系统

3.1 TF 系统简介

45.初识ROS的TF系统

3.2 里程计简介

46.什么是里程计

3.3 Gmapping 算法

用 RBPF 粒子滤波器来求解 SLAM 问题，也有人基于 RBPF 来研究构建栅格地图（grid map）的 SLAM 算法，它就是 ROS 中开源功能包 Gmapping 算法。它是一种基于粒子滤波的算法，但对 RBPF 的建议分布（proposal distribution）和重采样进行了改进

3.3.1 Gmapping 代码框架

Gmapping 算法用了 2 个 ROS 功能包来组织代码，分别为 slam_gmapping 功能包和 openslam_gmapping 功能包
- 其中 slam_gmapping 功能包用于实现算法的 ROS 相关接口，其实 slam_gmapping 本身没有实质性的内容，是一个元功能包，具体实现被放在其所包含的 gmapping 功能包中
- 单线激光雷达数据通过 /scan 话题输入 gmapping 功能包，里程计数据通过 /tf 关系输入 gmapping 功能包，gmapping 功能包通过调用 openslam_gmapping 功能包中的建图算法，将构建好的地图发布到 /map 等话题

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3.3.2 Gmapping 程序调用流程

程序 main() 函数很简单，就是创建了一个 SlamGMapping 类的对象 gn。然后，SlamGMapping 类的构造函数会自动调用 init() 函数执行初始化，包括创建 GridSlamProcessor 类的对象 gsp_ 和设置 Gmapping 算法参数
- SlamGMapping 类在 gmapping 功能包中实现
- GridSlamProcessor 类在 openslam_gmapping 功能包中实现，而 GridSlamProcessor 类以成员变量的形式被 SlamGMapping 类调用
- 接着，调用 SlamGMapping 类的 startLiveSlam() 函数，就可以进行在线 SLAM 建图了。startLiveSlam 函数首先对建图过程所需要的 ROS 订阅和发布话题进行了创建，然后开启双线程进行工作
  - 其中 laserCallback 线程在激光雷达数据的驱动下，对雷达数据进行处理并更新地图，其中调用到的 GridSlamProcessor 类的 processScan 函数就是改进的 RBPF 算法具体实现
  - 而 publishLoop 线程负责维护 map->odom 之间的 tf 关系

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3.3.3 安装与运行

安装 Gmapping 依赖库

# 安装 openslam-gmapping 和 slam-gmapping 功能包及其依赖
$ sudo apt install ros-melodic-openslam-gmapping ros-melodic-gmapping
# 卸载 openslam-gmapping 和 slam-gmapping 功能包，但保留其依赖
$ sudo apt remove ros-melodic-openslam-gmapping ros-melodic-gmapping

源码下载并编译安装

# 创建工作空间
$ mkdir -p gmapping_ws/src
$ cd gmapping_ws/src

# 源码下载
$ git clone https://github.com/ros-perception/slam_gmapping.git
$ cd slam_gmapping
$ git clone https://github.com/ros-perception/openslam_gmapping.git
$ cd ~/gmapping_ws/

# 编译安装
$ catkin_make -DCATKIN_WHITELIST_PACKAGES="openslam_gmapping"
$ catkin_make -DCATKIN_WHITELIST_PACKAGES="gmapping"

运行示例程序

$ roslaunch gmapping slam_gmapping_pr2.launch

# bag 下载地址 http://download.ros.org/data/amcl/?C=D;O=A
$ rosbag play basic_localization_stage_indexed.bag

# 在 rviz 中订阅地图话题 /map 即可看到地图
$ rviz

3.3.4 注意事项

订阅的话题
- tf(tf/tfMessage)：坐标系转换关系
  - 雷达坐标系（laser）→ 底盘坐标系（base_link）
    - 雷达坐标系名称要和 /scan 话题中雷达数据包的 header 里的 frame_id 一致
```
$ rostopic echo /scan --noarr
```
  - 底盘坐标系（base_link）→ 里程计坐标系（odom）
- scan(sensor_msgs/LaserScan)
  - 激光雷达数据话题
发布的话题
- map_metadata(nav_msgs/MapMetaData)
  - 地图加载时间、地图分辨率、地图的宽和高、地图原点坐标
- map(nav_msgs/OccupancyGrid)
  - 栅格地图数据
- ~entropy(std_msgs/Float64)
  - 机器人定位的置信度，值越大则机器人定位错误的可能性越大
提供的 TF 变换关系：map → odom（轮式里程计累积误差的动态修正量）
- 激光雷达 SLAM 输出的是 map → base_footprint 的 TF（base_link 是与机器人中心重合，而 base_footprint 是 base_link 在地面的投影）
- 里程计输出的是 odom → base_footprint 的 TF
- Gmapping 算法核心思想：先使用里程计推算机器人的位移，然后通过激光雷达点云配准算法用于修正里程计的误差，即 map → odom

3.3.5 离线运行

# 录制数据集
$ rosbag record -O test.bag /scan /tf /tf_static

# 启动建图（记得打开 use_sim_time 参数）
$ roslaunch gmapping slam_gmapping.launch

# 播放数据集
$ rosbag play test.bag

3.4 Cartographer 算法

Gmapping 代码实现相对简洁，但基于滤波方法的 SLAM 系统无法构建大规模的地图，而基于优化的方法可以
基于优化的方法实现的激光 SLAM 算法也有很多，比如 Hector、Karto、Cartographer 等，而 Cartographer 是其中获好评最多的算法，其开发团队来自谷歌公司，代码的工程稳定性较高，是少有的兼具建图和重定位功能的算法
基于优化方法的 SLAM 系统通常采用前端局部建图、闭环检测和后端全局优化这种经典框架

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3.4.1 Cartographer 代码框架

Cartographer 算法主要由 3 部分组成

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

1. cartographer_ros 功能包

cartographer_ros 功能包用于实现算法的 ROS 相关接口，Cartographer 算法是一个支持多激光雷达、IMU、轮式里程计、GPS、环境已知信标的传感器融合建图算法
- 激光雷达数据可以通过多种接口输入算法，用户可根据自己激光雷达的数据类型选择合适的话题 (/scan、/echoes 或/points2)进行输入，由于 Cartographer 算法支持 2D 和 3D 建图，所以支持单线激光雷达和多线激光雷达
- IMU 数据通过话题 /imu 输入算法，轮式里程计数据通过话题 /odom 输入算法，GPS 数据通过话题 /fix 输人算法，环境已知信标数据通过话题/landmarks 输人算法
- Cartographer 支持多种模式建图，既可以只采用激光雷达数据建图，也可以采用激光雷达数据 + IMU、激光雷达 + 轮式里程计、激光雷达 + IMU + 轮式里程计等模式建图，并且还可以用 GPS 和环境已知信标辅助建图过程。Cartographer 的工作模式和各种参数采用 *.lua 配置文件进行配置
- Cartographer 建图结果通过 2 个话题输出，其中话题 /scan_matched_points2 输出 scan-to-submap 匹配结果，话题 /submap_list 输出整个 Cartographer 最终地图结果
cartographer_ros 程序调用流程
- 主要涉及 Node 和 MapBuilder 这两个类
  - 其中 Node 类在 cartographer_ros 功能包中实现，Node 类通过类成员调用的方式将从 ROS 接口中获取的传感器数据传入 MapBuilder 类
  - MapBuilder 类在 cartographer 核心库中实现，MapBuilder 类主要包含传感器数据融合、局部建图和全局建图这 3 个部分

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

2. cartographer 核心库

cartographer 核心库实现建图算法的具体过程，也就是局部建图、闭环检测和全局建图的实现，下图为谷歌官方给出的流程框图。激光雷达数据、轮式里程计数据、IMU 数据和外部位姿辅助数据为整个算法的输入：其中激光雷达为必需数据，而轮式里程计、IMU 和外部位姿辅助（即 GPS 和已知信标）为可选数据

传感器数据输入
- 激光雷达数据（用于前端局部建图）
  - 先经过体素滤波(voxel filter)，体素滤波其实就是对点云降采样，一般是将点云划分到不同体素栅格内，再用体素栅格内所有点的重心表示此体素栅格
  - 经过体素滤波后的激光雷达数据有 2 个流向：一个是直接传给 Submaps 用于子图构建，另一个是经自适应体素滤波后用于扫描匹配
- 轮式里程计、IMU 等数据（用于后端全局优化）
  - 与扫描匹配得到的观测位姿进行多传感器融合，经融合后的位姿作为更高精度的初始位姿输入给扫描匹配，这样能大大提高扫描匹配的效率和精度，当前位姿（位置 position，姿态 orientation）可以由前一时刻位姿推算而来
  - 观测位姿：IMU 可用时，更信任 IMU 提供的 orientation；odom 可用时，更信任 odom 提供的 position；否则只能假设匀速模型，即上一个时刻的线速度和角速度在当前时刻依然不变，用线速度和角速度乘以时间间隔就能求出 position 与 orientation
前端局部建图
- 首先利用雷达数据和给定初始位姿进行扫描匹配得出观测位姿
- 然后进行运动滤波。运动滤波的作用是避免重复插入相同雷达帧数据，当位姿变化不明显时，新雷达帧数据将不会被插入子图
- 当构建完 1 个子图 submap(1) 后，就接着构建另一个子图 submap(2)，这样不断构建，局部建图过程会生成多个子图 submaps = {submap(m)}
后端全局建图
- 由闭环检测驱动，闭环检测是在程序后台持续运行的，传感器每输入一帧雷达数据，都要对其进行闭环检测。当闭环检测中匹配得分超过设定阈值就判定闭环，此时将闭环约束加入整个建图约束中，并对全局位姿约束进行一次全局优化，全局优化会对路径上的所有位姿及对应子图进行修正

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3. ceres-solver 非线性优化库

Cartographer 采用优化库 Ceres-Solver 来求解优化问题，主要包括局部建图中扫描匹配涉及的局部优化问题和全局建图中涉及的全局优化问题

3.4.2 安装与运行

手把手教你编译 cartographer
手把手教你运行 cartographer

3.5 LOAM 算法

3.5.1 LOAM 算法框架

LOAM 的核心思想：将 SLAM 问题拆分成独立的定位和建图分别来处理

特征提取模块（scanRegistration）
- 从雷达点云中提取特征点，对当前帧点云中的每个点计算平滑度，将平滑度小于某阙值(min)的点判断为 corner 特征点，而平滑度大于某值(max)的点判断为 surface 特征点
定位模块（laserOdometry）
- 利用 scan-to-scan 方法对相邻两雷达点云中的特征点进行匹配这种帧间特征匹配能得到较低精度的里程计，用该里程计来校正雷达特征点云的运动畸变
建图模块（laserMapping）
- 利用 scan-to-map 方法进行高精度定位，该方法以前面低精度的里程计作为位姿初始值，将校正后的雷达特征点云与地图进行匹配，这种扫描帧到地图的匹配能得到较高精度的里程计（1Hz 里程计），基于该高精度的里程计所提供的位姿将校正后的雷达特征点云加入已有地图
里程计融合（transformIntegration）
- 将低精度里程计和高精度里程计融合，融合通过以 1Hz 的高精度里程计为基准，利用 10Hz 的低精度里程计对其进行插值，从而输出更新速度和精度都较高的里程计

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

3.5.2 LOAM 源码解读

A-LOAM_Notes

3.5.3 LOAM 安装运行

依赖项安装

Ceres Solver 安装

$ sudo apt install cmake
$ sudo apt install libgoogle-glog-dev libgflags-dev
$ sudo apt install libatlas-base-dev
$ sudo apt-get install libeigen3-dev
$ sudo apt-get install libsuitesparse-dev

$ git clone https://github.com/ceres-solver/ceres-solver.git
$ cd ceres-solver
$ mkdir build
$ cd build
$ cmake ..
$ make
$ make test
$ sudo make install

PCL 安装
PCL学习一：点云与PCL基础

源码安装

$ mkdir -p a_loam_ws/src
$ cd ~/a_loam_ws/src
$ git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git
$ cd ../
$ catkin_make

NSH indoor outdoor.bag 数据集运行

$ cd ~/a_loam_ws
$ source devel/setup.bash
$ roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch

$ cd bag
$ rosbag play nsh_indoor_outdoor.bag
# nsh_indoor_outdoor.bag 下载地址
# 链接: https://pan.baidu.com/s/1zpS7rfLzVmyErVDlLdNP4g?pwd=tvfb 提取码: tvfb

4. 视觉 SLAM 系统（待更新）

4.1 ORB-SLAM2

5小时让你假装大概看懂ORB-SLAM2源码

4.2 VINS

四、自主导航篇

1. 理论概述

1.1 自主导航

自主导航问题的本质：从地点 A 自主移动到地点 B，机器人会有三个问题： “我在哪”、“我将到何处去” 和 “我该如何去”
自主导航系统分类
- 层级式体系结构则能够对高层级任务逐层进行条理清晰的逻辑推理
- 响应式体系结构能在低层级逻辑上对任务作出迅速响应(比如当障碍物突然出现时，系统可以立马触发避障任务的响应，而不需经过其他任务处理结果的层层触发)

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

1.2 环境感知

环境感知就是机器人利用传感器获取自身及环境状态信息的过程。自主导航机器人的环境感知主要包括：实时定位、环境建模（建图）、语义理解等

1.2.1 实时定位

定位其实就是在回答机器人提出 “我在哪” 问题，更确切点说应该是实时定位，因为机器人不仅要知道自身的起始位姿，还要知道导航过程中的实时位姿
实时定位可以分为被动定位和主动定位两种
- 被动定位依赖外部人工信标：如 GPS、UWB、RFID、二维码等
- 主动定位不依赖外部人工信标：以 SLAM 为代表

主动定位

所谓主动定位，就是机器人依靠自身传感器对未知环境进行感知并获取定位信息
SLAM 导航方案由建图（mapping）、定位（localization）和路径规划（path planning）3 大基本问题组成，这 3 大问题互相重叠和嵌套又组成新的问题，也就是 SLAM 问题、导航问题、探索问题等

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

大多数 SLAM 算法支持两种工作模式：SLAM 建图模式和 SLAM 重定位模式
- 商用场合通常采用 SLAM 重定位模式进行定位，即先手动遥控机器人进行 SLAM 环境扫描并将构建好的地图保存下来，然后载人事先构建好的离线地图并启动 SLAM 重定位模式获取机器人的实时位姿
- 比如，Gmapping 将构建出的地图保存为 *pgm 和 *yaml 文件，然后利用 map_server 功能包载人 *pgm 和 *yaml 文件并发布到 ROS 话题，最后利用 SLAM 重定位模式（这里通常为 AMCL 算法）及当前传感器信息与地图信息的匹配程度来估计位姿
但在外星球表面、荒野、岩洞等不便于人工手动遥控建图的情况下，机器人需要自发地进行探索、建图和定位
- CMU-自主探索导航系统（TARE & FAR Planner）学习-All in one

1.2.2 环境建模（建图）

环境建模其实就是对环境状态进行描述，也就是构建环境地图
- 地图可用于定位也可用于避障，因此定位用到的地图与避障用到的地图并不一定相同
- 环境地图有多种比如特征地图、点云地图、几何地图、栅格地图、拓扑地图等。视觉 SLAM 通常以构建特征地图为主，激光 SLAM 则以构建栅格地图（二维）或点云地图（三维）为主
- 由于导航过程中需要避开障碍物，所以特征地图或点云地图必须转换成栅格地图后才能导航

二维栅格地图

机器人通常采用二维占据栅格地图，其是对划分出来的每个栅格用一个占据概率值进行量化，概率为 1 的栅格被标记为占据状态，概率为 0 的栅格被标记为非占据状态，概率在 0 到 1 之间的栅格被标记为未知状态。机器人在导航过程中，要避开占据状态的栅格，在非占据状态的栅格中通行，通过传感器来探明未知状态的栅格的状态

三维栅格地图

将三维空间用立体栅格进行离散划分，就得到了三维栅格地图。三维占据栅格地图是对划分出来的每个立体栅格用一个占据概率值进行量化
相比于二维栅格，三维栅格的数量更大。为了提高三维栅格地图数据处理效率，通常采用八叉树（Octree）对三维栅格数据进行编码存储，这样就得到了八叉树地图（OctoMap）

1.2.3 语义理解

对环境状态的理解是多维度的
- 对于定位问题来说，环境状态被机器人理解为特征点或点云
- 对于导航避障问题来说，环境状态被机器人理解为二维或三维占据栅格
站在更高层次去理解，机器人会得到环境状态数据之间的各种复杂关系，即语义理解
- 无人驾驶汽车要学会车道识别、路障识别、交通信号灯识别、移动物体识别、地面分割等
- 室内机器人要学会电梯识别、门窗识别、玻璃墙识别、镂空物体识别、斜坡识别等

1.3 路径规划

路径规划回答机器人提出 “我该如何去” 问题。无论是在已知地图上导航还是在未知环境中一边探索地图一边导航，路径规划其实就是在地图上寻找一条从起点到目标点可行的通路
机器人自主导航通常是在给定的栅格地图上进行路径规划。如下图：通过对整个栅格地图遍历找到一条从 A 到 B 的路径，比如路径 L1 或 L2。在实际机器人导航中，不是找到一条可通行路径就行，还要考虑路径的各项性能（如长度、平滑性、碰撞风险、各种附加约束等）

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

1.3.1 常见的路径规划算法

基于图结构的路径搜索

Dijkstra 算法
- 假如 P(A, F) 是 A 到 F 的最短路径，D 是该最短路径上的某个点，那么 P(A, D) 必定也是 A 到 D 的最短路径
  $\underbrace{P(A,F)}_{\arg\min}=\underbrace{P(A,D)}_{\arg\min}+P(D,F)$
- Dijkstra 算法是按照广度优先（先在当前搜索节点上横向生长出子节点，然后从所有叶子节点中选择最优节点继续同样的生长方式）进行搜索，就是先遍历父节点周围的子节点，然后选择一个符合条件的子节点继续
- 栅格地图上表示的 Dijkstra 算法搜索过程
- Dijkstra 算法包含两个嵌套的循环，时间复杂度为 $O(n^2)$
  - 当大规模地图中包含的搜索节点数目 n 急剧增加时，Dijkstra 算法的实时性将大幅下降
  - Dikstra 算法的优点是其搜索策略是完备的，也就是说如果最短路径存在的话，只要花足够多的时间就一定能搜索到该路径
- 由上图搜索过程来看，从扩展出的叶子节点(浅灰色节点)中挑选出下一个搜索节点时，仅考虑了源节点到这些叶子节点的信息
  - 假设以这些叶子节点的代价函数 f(x) 为挑选依据，源节点到其他任意节点的实际代价用 g(x) 表示，Dijkstra 算法是以下式 ① 为依据挑选搜索方向
A* 算法
- A* 算法在代价函数 f(x) 中加估计代价(启发函数)来改进 Djkstra 算法的实时性，如上式 ② 所示。启发函数描述了途经节点到目标节点的估计代价。引入启发函数可以有效降低搜索节点的范围，从而提高整个搜索效率
- 估计代价 h(x) 越小，搜索节点的范围会越大。当估计代价 h(x) 为 0 时，将得不到任何启发信息，此时 A* 算法就退化成了 Dijkstra 算法。估计代价 h(x) 不能大于节点到目标节点的真实代价
  $h(x)\leqslant\cos t^*(x,\mathrm{target})$
- h(x) 的常见形式有曼哈顿距离、对角线距离、欧氏距离等
  - 当栅格地图中只允许向 4 个方向（上、下、左、右）移动时，选用曼哈顿距离
  - 当栅格地图中允许向 8 个方向（上、下、左、右、左上、左下、右上、右下）移动时，选用对角线距离
  - 当栅格地图中允许向任意方向移动时，选用欧氏距离

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

基于采样的路径搜索

以 PRM、RRT 等为代表的基于采样的路径搜索算法是将机器人所处的连续空间用随机采样离散化，然后在离散采样点上进行路径搜索

PRM 算法
- 第 1 步：随机采样。即在给定的地图上抛撒一定数量的随机点，利用这些随机点对地图中的连续空间进行离散化
- 第 2 步：移除无效采样点，也就是将落在障碍物上的采样点删除
- 第 3 步：连接，也就是按照最近邻规则将采样点与周围相邻点进行连接
- 第 4 步：移除无效连接，也就是将横穿障碍物的连接删除，这样就构建出了所谓的 PRM 路线图
- 第 5 步：添加导航任务的源节点和目标节点，也就是将源节点和目标节点与 PRM 路线图相连
- 第 6 步：搜索路径，在构建出来的 PRM 路线图上利用 A* 算法搜索源节点到目标节点之间的路径
- PRM 在构建出路线图后仍然采用 A* 搜索路径，PRM 在路径规划问题上是不完备的，当采样点较少时可能规划不出路径。由于采样点不能覆盖所有情况，所以 PRM 规划出的路径也不是最优的
RRT 算法

虽然 PRM 利用采样方式对连续空间进行离散化可以大大缩小搜索范围，但最后路径依然是在图结构上进行搜索
- RRT 算法通过采样方式构建树结构，并且一边构建树结构，一边进行路径搜索
- 如下图所示，RRT 工作流程为以源节点位置为树根，利用随机采样再生长出树枝，在各个树枝上继续利用随机采样再生长出树枝，最终总会有树枝抵达目标节点。那么，从源节点所在的树根位置沿着树枝生长方向有且仅有一条路径抵达目标节点，这条路径就是 RRT 规划出来的路径

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

1.4 运动控制

对于自主导航来说，目标点为外部给定的一个已知量，起始点由 SLAM 定位模块提供。寻路和控制策略则比较复杂，包括全局路径规划、局部路径规划、轨迹规划、轨迹跟踪等过程
- SLAM 主要提供全局地图和定位信息，而 SLAM 主要有两种工作模式
  - 第一种模式，SLAM 先运行建图模式构建好环境地图后将地图保存，接着载入已保存的全局地图并启动 SLAM 重定位模式提供定位信息
  - 第二种模式，SLAM 直接运行在线建图模式，建图过程中直接提供地图和定位信息
- 全局路径规划以起始点和目标点为输入，利用全局地图描述的障碍物信息规划出一条从起始点到目标点的全局路径
  - 全局地图一般为离散、静态形式，因此规划出来的全局路径也是离散、静态形式
  - 因为全局路径由一个个离散路径点连接而成，并且只考虑了静态障碍物信息，所以全局路径无法直接用于导航控制，只能作为导航的宏观参考
- 局部路径规划相当于全局路径规划的细化过程，以机器人能感知的局部边界上的全局路径点为局部目标点，以机器人能感知到的局部动态障碍物信息为基础规划出一条从机器人当前位置点到局部目标点的局部路径
  - 局部路径通常为连续、动态形式，局部路径并不与全局路径重合，而是尽量跟随着全局路径
  - 因为局部路径常常在突然出现的动态障碍影响下发生较大变化，且机器人实际控制误差使得真实行走路径偏离局部路径，局部路径也只能作为导航的宏观参考
- 轨迹规划相当于局部路径规划的细化过程，局部路径只考虑了几何约束，而在局部路径上添加运动学约束和动力学约束后就生成了机器人实际能执行的轨迹这就是所谓的轨迹规划
  - 理想情况下，可以直接取轨迹规划中各个轨迹点的速度信息作为动作量输入执行器（电机）
  - 不过，电机控制误差、路面起伏、轮胎打滑等导致这种开环控制策略很难奏效
- 轨迹跟踪相当于轨迹规划的细化过程，机器人按照轨迹规划出来的参考轨迹开始运动，运动一段时间后发现偏离到参考轨迹左边，机器人调整运动方向，以便逼近参考轨迹。但由于惯性，其在逼近参考轨迹之后立马又偏离到参考轨迹右边
  - 机器人会根据真实轨迹与参考轨迹的偏差不断调整自身运动
  - 轨迹跟踪其实就是基于误差反馈的闭环控制，真实轨迹始终跟随着参考轨迹左右摆动并不断逼近
1、全局路径规划和局部路径规划统称为路径规划
2、局部路径规划和轨迹规划统称为运动规划
3、轨迹规划和轨迹跟踪统称为运动控制，轨迹规划为轨迹跟踪器提供参考轨迹，而轨迹跟踪器生成动作量实现执行器的最终操控

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

1.4.1 基于 PID 的运动控制

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv

机器人在世界坐标系的状态量为 $q=[x,y,\theta]$ ，其中 x 和 y 为机器人的位置坐标， $\theta$ 为机器人的航向角
通常选择参考轨迹中离机器人最近的点 $q_{ref} =[x_{ref},y_{ref},\theta_{ref}]$ 作为追踪目标，以 $q$ 和 $q_{ref}$ 与之间的误差为反馈进行运动控制。最简单的方式就是：用距离误差调节两轮差速机器人的线速度，以航向角误差调节角速度
这里采用离散位置型 PID 控制算法，离散其实是指 PID 算法中的积分运算与微分运算分别用累加运算与差分运算替代，位置型是指 PID 算法的计算结果直接为控制量。利用两轮差速机器人运动学模型将 PID 算法得到的线速度量和角速度量转换成每个电机的速度控制量来实现最终控制
- $D_e$ 表示机器人相对于追踪目标的位置误差， $k_p、k_i、k_d$ 分别表示 PID 算法中的比例、积分和微分系数， $v_{ref}$ 为线速度参考量（用于修正）。计算结果为机器人的线速度量 $v$
- $\theta_e$ 表示机器人相对于追踪目标的航向角误差， $k_p^{\prime}\text{、}k_i^{\prime}\text{、}k_d^{\prime}$ 分别表示 PID 算法中的比例、积分和微分系数， $\omega_{ref}$ 为线速度参考量（用于修正）。计算结果为机器人的角速度量 $\omega$
  $v(t)=k_{p}\cdot D_{e}(t)+k_{i}\cdot\sum_{t}D_{e}(t)+k_{d}\cdot(D_{e}(t)-D_{e}(t-1))+v_{\mathrm{ref}}$
其中 $D_e(t)=\sqrt{(x-x_{\mathrm{ref}})^2+(y-y_{\mathrm{ref}})^2}$
$\omega(t)=k_{p}^{\prime}\cdot\theta_{e}(t)+k_{i}^{\prime}\cdot\sum_{t}\theta_{e}(t)+k_{d}^{\prime}\cdot(\theta_{e}(t)-\theta_{e}(t-1))+\omega_{\mathrm{ref}}$

其中 $\theta_e(t)=\theta-\theta_\mathrm{ref}$

1.4.2 基于 MPC 的运动控制

PID 算法属于典型的无模型控制系统，其缺点是控制量对被控对象的控制永远是滞后的
- 对于低速运动的室内移动机器人，这种控制滞后的影响并不明显
- 而对于高速运动的室外无人驾驶汽车，这种控制滞后的影响就很明显
解决方法是：在计算控制量时将下一个时刻的预判状态也考虑进来，也就是有模型控制系统。MPC（Model Predictive Control，模型预测控制）算法属于典型的有模型控制系统，该算法包括构建损失函数和优化求解两个步骤
$J=\mathrm{Loss}\left\{f(\boldsymbol{q}(k-1),\boldsymbol{u}(k)),\boldsymbol{q}_{\mathrm{ref}}(k)\right\}$

$\boldsymbol{u}(k)=\arg\min_{\boldsymbol{u}(k)}J$

所谓模型预测，就是控制量与状态量之间的映射关系 $f(\boldsymbol{q}(k-1),\boldsymbol{u}(k)$ 。由于控制方式不同，用于预测的模型并不唯一。比如在由电机控制的机器人中，一般通过加在电机上的电压或者 PWM 波来控制电机转速，而各电机转速到机器人实际线速度和角速度的映射借助运动学模型完成，最后由里程计模型预测出机器人的状态转移量 $\Delta \boldsymbol{q}$

2. 典型自主导航系统

2.1 ros-navigation

ROS学习7：ROS机器人导航仿真

2.1.1 原理分析

AMCL
- AMCL（AdaptiveMonte Carlo Localization，自适应蒙特卡洛定位）方案包含两种代码实现，即：用于二维地图定位的代码实现 amcl 和用于三维地图定位的代码实现 amcl3d，其中 ros-navigation 默认集成了二维地图定位的 amcl 代码包
- AMCL 是 MCL（Monte Carlo Localization，蒙特卡洛定位）的改进版本，而 MCL 属于粒子滤波的范畴，因此 AMCL 也属于粒子滤波的范畴
Costmap
- 为了解决各种复杂障碍物的度量问题，ros-navigation 采用 Costmap 代价地图对障碍物进行统一度量。Costmap 采用多个独立的栅格化图层来维护障碍物信息，每个图层可以独立维护某个来源的障碍物信息，这些图层可以根据不同需求进行叠加，形成特定的障碍描述层
ros-navigation 系统框架
- ros-navigaion 是一个功能包集，里面包含了大量的 ROS 功能包以及各种算法的具体实现节点。这些节点可以分为 3 类：
  - 必要节点：move_base（通过插件机制组织代码，使得 move_base 中的 global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors 等算法能被轻易替换和改进）
  - 可选节点：amcl（定位）和 map_server（加载静态地图）
  - 机器人平台相关节点：sensor transforms（障碍信息反馈）、odometry source（运动反馈）、sensor sources 和 base controller（执行器）

机器人SLAM导航学习-All in one,自主探索导航学习,机器人,学习,c++,笔记,opencv 文章来源地址https://www.toymoban.com/news/detail-602040.html

2.1.2 源码解读（待更新）

2.1.3 安装与运行

ROS学习7：ROS机器人导航仿真

2.1.4 路径规划改进

ros-navigation 中集成了一些不同的路径规划插件供用户在不同场景下使用，其中可选的
- 全局路径规划插件包括 navfn、global_planner 和 carrot_planner
- 局部路径规划插件包括 base_local_planner 和 dwa_local_planner
- 不过，这些插件还远不能满足研究和工程应用时的多样化需求
此时，用户可以选择其他第三方路径规划插件加载到 move_base 中来使用，或者根据 nav_core 的接口规范自己开发所需的路径规划插件
- SBPL_Lattice_Planner 和 srl_global_planner 为比较典型的第三方全局路径规划插件
- teb_local_planner 为比较典型的第三方局部路径规划插件

2.1.5 环境探索

自主导航和 SLAM 都是在受限条件下进行环境交互
- 自主导航的受限条件是环境地图必须已知
- SLAM 的受限条件是构建未知环境地图的过程需要人为操控
- 如果将自主导航与 SLAM 结合起来，就可以让机器人与环境的交互过程真正自主化，即所谓的环境探索。在机器人的环境探索过程中，SLAM 为自主导航提供实时更新的地图，自主导航则为 SLAM 提供操控，这样就能让机器人在完全未知的环境中自主构建地图并利用该地图自主导航
环境探索涉及 3 个基本问题：建图、定位和路径规划
- 从某种意义上说，自主导航问题是环境探索问题的一种特殊情况，也就是将环境探索问题、定位和路径规划中的建图部分替换成已知地图
- 另外，自主导航的导航目标点是从外部获取的某个确定值，而环境探索的导航目标点是探索算法根据当前状态临时生成的
环境探索与自主导航非常相似，在 ros-navigation 导航系统上稍做修改就能实现环境探索
- 将 map_server 部分更改成直接从外部获取实时地图数据
- 将 amcl 部分更改成由 SLAM 提供全局定位
- 将 ros-navigation 中从外部接收导航目标点的部分更改成从探索程序中自动生成导航目标点
1、frontier_exploration 探索算法
- frontier_exploration 的探索策略其实就是不断将机器人导航到地图边缘区域，这样地图边缘区域的未知环境就可以通过观测变为已知区域
- 将机器人放人未知环境后，机器人通过传感器能建立一个小范围的初始地图，利用计算机视觉方法可以从这个初始地图中提取未闭合的边界区域，并挑选其中一个边界区域作为导航目标点，在机器人导航到该边界区域的过程中，地图会不断扩大（被探明的区域可能会出现新的边界区域），机器人按照深度优先搜索的策略不断对边界区域进行探索直到所有边界区域都被探明为止
- 安装后就可以与 ros-navigation 导航系统和 SLAM 系统（比如Gmapping、Cartographer 等）结合使用了
2、rrt_exploration 探索算法
- rrt_exploration 集成了 frontier_exploration 中基于计算机视觉的边界区域探索策略，还集成了适用于多机器人探索的 RRT 探索策略
- rrt_exploration 包含5个节点
  - 全局边界检测器（Global Frontier Detector）
  - 局部边界检测器（Local Frontier Detector）
  - 过滤器（Filter）
  - 机器人任务整合（Allocator）
  - 传统边界检测器（Opencv-based Frontier Detector）

2.2 riskrrt 导航系统

riskrrt 的系统框架与 ros-navigation 基本一样，riskrrt 的特别之处体现在对动态障碍物的处理更加严格，在走廊、机场、路口等人流量大的场景中对机器人与动态障碍物发生碰撞的风险把控更加严格，主要通过基于风险的 RRT 全局路径规划实现
代码框架

2.3 autoware 导航系统

ros-navigation 和 riskrrt 导航系统主要用于机器人低速导航，并且大多基于 2D 地图
autoware 导航系统主要用于无人驾驶汽车的高速导航，并且基于 3D 地图
虽然 autoware 导航系统主要应用于无人驾驶汽车领域，但整个框架依然是基于 ROS 开发的。autoware 框架同样是由定位、感知识别、路径规划等主要模块组成的