基于RTAB-Map和RRT的自主导航方案

REF：基于毫米波雷达的人机共融机器人路径规划

1. 导航相关硬件模型

• 激光雷达： 激光雷达通过激光发射器向环境中发射红外光束，当与环境中的物体相遇后，激光光束会被反射回来，被激光雷达接收器接收反射的激光光束，在激光雷达的测距方法中，核心原理包括三角测距与飞行时间（TOF）测距两类

cdc747ee7a3a97d5b205ca62af3a8571.png

• a. 三角测距原理：激光束被不同的物体反射后，由入射光和反射光形成不同的三角形，通过计算这些三角形的信息，便可获得所在环境的相关的距离位置信息

4ad2e329f9d1c417c2874788f88fb3a4.png

• b. 测距原理：计算检测激光脉冲的飞行往返时间差 (t2-t1 )，可得到被检测物体的距离

e409adeef45b53c2934d4ff795be03ca.png

• 相机： 机器人视觉感知的传感器

355686424aaa92f62f15a5cad4ae70cb.png

• c1. 结构光相机：对投射光源进行结构化与特征化编码，将编码后的图案投射至物体表面会因物体的形状特征而产生相应的变化，通过相机接收捕捉调制的结构光，结构光形变的程度可以确定出物体的三维形态和深度信息，结构光主要可以分为线扫描结构光和面阵结构光两类。

ae92e0b384aaeff30d70470b89e56eb1.png

dafc61256395ade04ed2f05b1e33773d.png

• c2. TOF 深度相机：与激光雷达的 TOF 技术相同，主要分为脉冲调制和连续波调制两种，脉冲调制直接根据脉冲发射和接收的时间差来测算距离，连续波调制采用调制光对场景进行照射，并测量该调制光经过场景中物体反射后返回光的相位延迟，运用正交采样对延迟测量，间接得出物体与测量装置之间的距离
• a. 单目相机：由镜头和图像传感器构成，原理为小孔成像，成像信息由图像传感器转换为数字图像输出

2335b6db351cc65ed3616aa99fbfb3ba.png

• b. 双目相机：借助同步采集左右相机的图像，运用立体匹配算法计算图像间的视差，并依据三角几何关系估计每个像素的深度值

8f5c7cfda932f32d14d23e2659e93b9a.png

• c. RGB-D 深度相机：同时输出两个数据流：一个是普通的彩色图像(RGB 图像)，另一个是深度图(Depth Map)，包含了场景中每个点的深度信息，可以通过结构光、飞行时间(TOF)等技术实现

8615c5edc4e8d94f63b62c1927c1f150.png

• 毫米波雷达： 连续不断地向检测区域发射固定周期的线性调频脉冲信号，对发射和反射回来的信号进行频率和相位分析，就能确定检测目标的方位、距离以及速度

fcc018d45283909c39945c11e89411c5.png

e21822f54b708e2c87a780e284a5fb22.png

0a042d3cf7b6c819c32190f83a6b0a40.png

2. 融合视觉、激光及IMU的SLAM建图

• 相机标定： 通过张正友标定法，建立相机所拍摄的图像像素坐标系与真实世界三维坐标系之间的精确转换关系，利用棋盘格平面上的点在不同姿态下的投影关系，通过最小二乘法求解相机的内外参数（ROS 中 camera-calibration 功能包）

2fc7c157f700123c51b32a0b44ae6b2a.png

• 多传感器融合建图： 融合视觉相机、激光雷达、IMU传感器，使用激光雷达的精度、相机的丰富视觉信息和IMU相结合，通过RTAB-Map限制局部地图的大小，实现低成本的环境地图效果。

336958ad5fdb43604b25a03590d93303.png

1. 视觉里程计：

c31da29274d8e95e9430947ee6195bc2.png

1. 激光里程计：

a72ab025c663e7f2017ccd2351e2ca24.png

1. 局部地图：局部地图基于机器人自身坐标系构建，全局地图根据世界坐标系构建，两者通过机器人坐标系的变换关系来进行相互转换。

a2c624c555415f2655c7b1455f5caa34.png

1. 闭环检测：包括视觉闭环检测和激光相似检测，视觉闭环基于视觉词袋和离散贝叶斯滤波器，其中视觉词袋用于快速计算当前位姿节点与侯选节点的相似度，离散贝叶斯滤波器用于维护所有节点相似度的概率分布
2. 全局图优化：将 WM 中所有节点和约束边（相邻边、激光相似边、视觉闭环边）送入图优化模块进行全局优化，优化过程会对所有节点的里程计位姿进行校正
3. 全局地图构建：优化完成后，根据节点中的相关里程计位姿信息，将生成的多个局部地图逐步连接起来组成完整的环境地图，根据需求选择不同的地图拼接方法生成地图类型

eecbb55478501f69d0c7911034fba6e9.png

• RTAB-Map数据管理： 将地图中的节点分为短期内存(Short-Term Memory,STM)、工作内存(Working Memory, WM)、长期内存(Long-Term Memory, LTM)三个层级，通过对构建地图的过程中产生大量的数据节点来进行分级管理

ef408e95a250b6eeae42fcc6fb80a68d.png

3. 基于RRT的全局路径规划

• 传统 RRT 路径规划算法： 通过状态空间的随机采样点，以起点 start 作为根节点，把搜索导向空白区域，不断生长随机采样生成节点为随机扩展树，当最新搜索节点到达终点 goal 区域，就得到了从起点到终点的连续路径
• 父节点重选优化路径： 为了便于找到路径代价更小的路径，对当前已有的多个路径进行筛选，仅保留最小代价的父路径节点。

3ffd9b657f5e8755d2f97bf665a98715.png

-节点重布线删减冗余节点： 基于最新节点，优化当前最新路径，使得路径具有渐近最优性，在每次采样过程中保持最优节点的选取。

b73f5d3b593b425c909871f8aa684469.png

• 双向搜索和目标偏置采样提高搜索效率： 从起点和终点同时建立搜索树T1和T2 ，两棵搜索树分别向对方生长（两棵搜索树交替生长），在生长过程中使用目标偏置策略，提高路径规划采样的导向性和采样速度
• 路径边界值提高搜索效率： 对在路径生长过程中的冗余路径进行修剪，有效减少不必要的路径生成，降低计算成本，提高路径搜索过程的效率。

699c90dd232317de3efa82c36062a674.png

• 节点组检测连接确定结束条件： 检测T1和T2 搜索树，在各自趋向于对方搜索树将会在t 时刻的某一点相遇并确定路径

592726d25d90e27ed47aef5117d2e6f1.png

• 路径平滑优化： 采用三次 B 样条曲线对所得到的路径进行平滑处理，样条曲线表达式

1e3e2de3506e1f1bde143345ed7d8184.png

2bfa8a9d2f1c771903de7e5419c8e119.png