【路径生成--采样的方法】Rapidly exploring Random Trees(RRT)类算法

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

在项目和平时的学习中，我对机器人/无人驾驶的决策规划模块进行了划分，当然划分的方法有很多，我的划分方式仅供参考
（1）动态障碍物行为预测模块（Behavior prediction）–结合感知和高精度地图信息，估计周围障碍物未来运动状态
（2）执行机构的轨迹规划模块（Trajectory_planning）–执行机构如机器人载体上的机械臂、串行云台等的运动轨迹规划
（3）任务决策模块（Mission_planning）–任务决策模块比较偏业务层了，处理机器人/无人驾驶的各种任务，主要分为三个方面：车底盘航线业务决策（交规、横向换道等等）、执行应用机构业务决策（机械臂、人机交互等等）、不同场景的导航方案切换决策（组合导航、融合导航）
（4）前端路径探索模块（path_finding）–全局路径规划算法难度不算复杂，找到一条可通行的（必须满足）、考虑动力学的（尽可能满足）、可以是稀疏的路径base_waypoints【由于其只考虑了环境几何信息，往往忽略了无人机本身的运动学与动力学模型。因此，其得到的轨迹往往显得比较“突兀”，并不适合直接作为无人机的控制指令】
（5）后端轨迹处理模块（motion_planning）–我主要归纳整理为三个方向：（1）对base_waypoints进行简单处理及生成方向、（2）对base_waypoints轨迹优化方向【一般是二次优化，这里用的较多的事优化方面的知识】、（3）进行对应功能的replan方向（replan之前的预处理、进入replan的条件、停障replan、避障replan、纠偏replan、换道replan、自动泊车replan、穿过狭窄道路replan等等），这部分内容使用的方法比较专
（6）路径跟踪模块（trajectory_following）–这个模块就得针对机器人载体了，如无人驾驶使用得阿克曼模型可以采用几何的pure pursuit纯追踪算法，更好的可以用模型预测控制MPC方法，还有强化学习做的（效果怎样我就没验证过了）；当然也可能事麦克纳姆轮车、差速车PID、还有无人机的三维轨迹跟踪等等
（7）碰撞检测模块
（8）集群多机器人规划模块

当然，这种划分方式是我权衡了原理和功能粗略划分的，在实际产品研发过程中，需要理解了各个算法的功能和定位的基础上融汇贯通，不能生搬硬套，如全段通过hybrid A探索出来的路径与A、RRT*探索出来的路径更平滑，后端轨迹优化的任务就不用这么重了；又如，机器人/无人驾驶项目研发的需求业务还没发展到能响应很多功能阶段，任务决策使用简单的状态机（fsm）就可以对现有任务进行状态转移了

本文先对Rapidly exploring Random Trees(RRT)算法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

一、基于采样的普通路径规划算法–RRT（Rapidly Exploring Random Tree）

1.RRT图例

2.RRT原理

这是一种搜索算法，其实在采集样本的基础上进行路径规划，【防盗标记–盒子君hzj】一般适用于三维空间的目标搜寻。其设计原理是将空间中的某点作为起始点，然后规定一定原则，在原则约束下以起始点来确定一个新的节点，重复直至找到目标终止点。
点到直线的距离：

采样的方法和PRM类似，但是RRT的回溯搜索的方法是（盲目–改进版本）从自身节点起进行扩展的

3.RRT伪代码

（1）输入：地图、起始点、终止点
（2）输出：从起始点到终止点的路径
（3）核心关键函数实现
1）采样函数
2）Nearest函数
3）Collision-checking函数

算法规程演示动图（大概能看出RRT效果）
https://www.youtube.com/watch?v=pOFtvZ_qVsA

4.RRT总结

（1）优点
快速随机树算法是一种较为迅捷搜索的方式，增量式的构建树其优点明显，主要体现在针对性的找一条从起点到终点的路径其较强较快的搜索能力，【防盗标记–盒子君hzj】并且运用快速拓展随机树的路径规划对路径运行环境没有限制要求。

（2）缺点
1）轨迹不是平滑的，不能够满足无人车运动的需求
2）规划出来的路径不是最短甚至不是最优的，只是达到了目标而已（折线路径不太满足动力学）
3）偏向于处理静态障碍物的问题，在动态环境中会有一定局限性。【防盗标记–盒子君hzj】
4）狭窄的环境下，RRT效果不太好

5.RRT实现的改进【节点搜索改进、节点连接改进】

（1）kd-tree算法(这个方法提高了RRT的节点搜索的效率)

节点搜索方向,Xrand -> Xnear -> Xnew

（2）Bidirection RRT/RRT Connect算法【防盗标记–盒子君hzj】(这个方法提高了RRT的节点连接的效率)

二、基于采样的最优路径规划算法–RRT*（Rapidly Exploring Random Tree*）

1.RRT*的目的

RRT*解决了RRT生成的路径不是最优的问题，找到一条最优的路径

2.RRT*算法原理

通过把连续的空间离散化，在离散空间中撒一些点（Xrand），建立一个从起始点到目标点的树，在起始点开始，向目标点的方向，去盲目探索，【防盗标记–盒子君hzj】从树上找到离这些点（Xrand）最近的一些点（Xnear），确定下一个采样点（Xnew），只要不碰到障碍物就行，每次探索一小步，直到到达目标点，再对该路径进行改进(rewrite()函数剪枝)【好像一个盲人，靠着气味寻找目标一样】
（1）当前节点重新选择父节点

（2）范围内的节点重新连接（rewire）

3.RRT*伪代码

4.RRT算法演示动图（大概能看出RRT效果）

https://www.youtube.com/watch?v=YKiQTJpPFkA

三、RRT算法 VS RRT*算法

一张对比图说明问题

RRT、RRT*、informed RRT*是一个不断改进的路径搜索算法，【防盗标记–盒子君hzj】RRT算法的基本原理，包括Sampling、Nearest、Collision-Checking

四、偏重于得到路径后的优化–informed RRT*算法

1.informed RRT*目的

informed RRT*得到可行路径后，把轨迹优化的范围限制在了一个椭圆内，【防盗标记–盒子君hzj】从而减少了很多优化的没必要的算力,改进采样方式提高路径搜索的效率（树的拓展效率）

2.informed RRT*算法原理

3.informed RRT*伪代码

4.informed RRT*缺点

（1）只能处理静态障碍物的问题
（2）轨迹还是不够平滑，扭来扭曲的，导数是平滑了但是数值变化也很大

5.RRT* VS informed RRT*

一张图说明优化的效果

五、Cross-entropy motion planning算法

1.算法步骤

当一个路径（折线）生成之后，在路径的节点处进行采样【防盗标记–盒子君hzj】，经过采样之后就能得到很多路径，对每条路径对应的节点取均值作为新路径的节点，进而得到新的更平滑的路径；重复迭代上诉过程，直到得到符合要求的优化路径

2.论文参考

Cross-entropy motion planning、

3.算法效果

六、RRT算法其他的变种

综述

[1] An Overview of the Class of Rapidly-Exploring Random Trees
[2]http://msl.cs.uiuc.edu/rrt/

（0）Anytime-RRT*算法

论文Anytime Motion Planning using the RRT∗

（1）Lower Bound Tree RRT (LBTRRT)

https://arxiv.org/pdf/1308.0189.pdf

（2）Sparse Stable RRT

http://pracsyslab.org/sst_software

（3）Transition-based RRT (T-RRT)

http://homepages.laas.fr/jcortes/Papers/jaillet_aaaiWS08.pdf

（4）Vector Field RRT

https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6606360

（5）Parallel RRT (pRRT)

https://robotics.cs.unc.edu/publications/Ichnowski2012_IROS.pdf

（6）Dynamic RRT

（7）RRT-Connect

（8）RRT-smart

更多的算法github

https://github.com/zychaoqun

总结（缺点）

（1）轨迹不是平滑的，扭来扭曲的，导数是平滑了但是数值变化也很大，不能够满足无人车运动的需求，需要后端的轨迹优化
（2）大多数规划出来的路径不是最短甚至是最优的，只是达到了目标而已【防盗标记–盒子君hzj】
（3）大多数只能处理静态障碍物的问题

但是搜索的速度是非常快的！！

---

参考资料

待补充