【移动机器人运动规划】02 —— 基于采样的规划算法

前言

本文部分内容参考了深蓝学院的移动机器人运动规划，依此做相关的笔记与整理。之前的文章也有对基于采样的算法进行过介绍，所以本文并不着重介绍这类算法的基本概念，主要是对之前文章的一些补充。

相关代码整理:

1. https://gitee.com/lxyclara/motion-plan-homework/
2. https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/blob/master
3. https://gitee.com/aries-wu/Motion-plan/blob/main/
4. 链接: https://pan.baidu.com/s/1UtVHRxDq771LfSGK_21wgQ?pwd=rhtp 提取码: rhtp

相关文章：

自动驾驶路径规划——基于概率采样的路径规划算法（PRM）https://blog.csdn.net/sinat_52032317/article/details/127177278
自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）https://blog.csdn.net/sinat_52032317/article/details/127197120

基本概念

规划完备性概念

• Complete Planner: always answers a path planning query correctly in bounded time
• Probabilistic Complete Planner: if a solution exists, planner will eventually find it, using random sampling (e.g. Monte Carlo sampling)
• Resolution Complete Planner: same as above but based on a deterministic sampling (e.g sampling on a fixed grid).【采样更确定】

概率路线图（Probabilistic Road Map）

之前的这篇博客已经有过介绍以及代码示例：自动驾驶路径规划——基于概率采样的路径规划算法（PRM）

基本流程

一般可以分为两个阶段：预处理阶段（Learing phase/preprocess phase）和查询阶段(query phase)。

预处理阶段

• 初始化。设$G(V,E)$为一个无向图，其中顶点集 $V$代表无碰撞的顶点集，连线集$E$代表无碰撞路径。初始状态为空。
• 构型采样。从构型空间中采样一个无碰撞的点$a(i)$并加入到顶点集 $V$ 中。
• 领域计算。定义距离 $\rho$，对于已经存在于顶点集$V$中的点，如果它与$a(i)$ 的距离小于 $\rho$，则将其称作点$a(i)$的邻域点。
• 边线连接。将点$a(i)$与其领域点相连，生成连线$\tau$ 。
• 碰撞检测。检测连线 $\tau$ 是否与障碍物发生碰撞，如果无碰撞，则将其加入到连线集 $E$ 中。
• 结束条件。当所有采样点（满足采样数量要求）均已完成上述步骤后结束，否则重复2-5。
  

查询阶段

采用AStar或Dijkstra等算法从起点到终点进行搜索。

优缺点（pros&cons）

更详细的特点总结在之前的博客中已经阐述过了，这里只列出几点关键的。

优点：

• 概率完备性
• 应对高维空间规划效率高
• 不易陷入局部最小值

缺点：

• 还未考虑边界值问题（运动学约束）
• 分为两阶段式的算法冗长。

一些改进算法

Lazy collision-checking

改进点：
在采点建图时不做碰撞检测处理，在后续Search阶段才进行碰撞检测处理。若检测到碰撞，删除路径中碰撞的点与边，重构路线图，再次进行搜索，直到找到一条路径。

以下是示意图

Rapidly-exploring Random Tree

之前的这篇博客已经有过介绍以及代码示例：自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）

算法伪代码

一些改进算法

KD-tree

参考：https://blog.csdn.net/junshen1314/article/details/51121582

利用kd-tree查找最近的节点（每次找中位数）

Bidirectional RRT / RRT Connect

Bidirectional RRT / RRT Connect之前的这篇博客已经有过介绍：自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）

Optimal sampling-based path planning methods

Rapidly-exploring Random Tree*

这部分同样可以参考自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）

算法伪代码：

Kinodynamic-RRT*

考虑机器人的运动学约束

论文：Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints
https://arxiv.org/abs/1205.5088

Anytime-RRT*

在机器人运动过程中，一直在更新RRT*

Anytime Motion Planning using the RRT*https://ieeexplore.ieee.org/document/5980479

Advanced Sampling-based Methods

Informed RRT*

Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic
https://ieeexplore.ieee.org/abstract/document/6942976

流程

当生成路径之后，以红色的点到绿色的点这段路径的长度（红色部分）为半长轴的两倍（2a），以红色点和绿色点作为焦点，生成椭圆。在椭圆的范围内进行采样与规划，重新生成路径后再次重复以上步骤。informed RRT*提升了规划速度，减少CPU运算时间，同时路径更为平滑。

Cross-entropy motion planning

Cross-entropy motion planninghttps://ieeexplore.ieee.org/document/6301069

首先得到一个路径
然后以路径中的每个点作为一个高斯模型的中心，在多高斯模型中采样，得到多条路径。
然后对多条路径做均值，重新构建多高斯模型。

其他变种

• Lower Bound Tree RRT (LBTRRT)[a]
• Sparse Stable RRT[b]
• Transition-based RRT (T-RRT)[c]
• Vector Field RRT[d]
• Parallel RRT (pRRT)[e]
• Etc.[f]

[1] An Overview of the Class of Rapidly-Exploring Random Trees
[2] http://msl.cs.uiuc.edu/rrt/
[a] https://arxiv.org/pdf/1308.0189.pdf
[b] http://pracsyslab.org/sst_software
[c] http://homepages.laas.fr/jcortes/Papers/jaillet_aaaiWS08.pdf
[d] https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6606360
[e] https://robotics.cs.unc.edu/publications/Ichnowski2012_IROS.pdf
[f] https://github.com/zychaoqun

实践

[1] https://ompl.kavrakilab.org/
[2] https://moveit.ros.org/
[3] https://industrial-training-master.readthedocs.io/en/melodic/_source/session4/Motion-Planning-CPP.html

代码链接

链接: https://pan.baidu.com/s/1UtVHRxDq771LfSGK_21wgQ?pwd=rhtp 提取码: rhtp

作业思路

[1] 第3章作业思路讲解1
[2] 第3章作业思路讲解2

MATLAB

RRT

RRT*

Goal-bias RRT*

粗略计算RRT、RRTStar、Goal-bias RRTStar的15次规划平均规划时间(s)、平均规划路径的代价、平均访问的节点数以及树的节点树。MAP1、MAP2、MAP3是三种地图，MAP1是作业提供的地图，MAP2是narrow space的测试地图，MAP3则是更严格的narrow space测试地图。RRT、RRTStar、Goal-bias RRTStar三种算法采用的步长均为20，其中RRTStar、Goal-bias RRTStar的choose parent部分的筛选范围为50，Goal-bias RRTStar对目标点的偏置概率为0.3。

由于RRT通过随机采样点来扩展树的结构，从而在树的结构上找到相对应的到达目标点的可行路径。理论上RRT算法是概率完备的，随看采样点的增多，找到路径的概率趋近于1，因此采样点的分布很大程度上决定了RRT算法找到路径的快速性。

RRT的问题在于算法并不是asymptotically optimal的，而RRTStar通过重选父节点和重布线，使得算法可以达到asymptotically optimal的特性，因此可以使路径更优，代价值更小，这从三种地图的平均路径规划的代价值可以看出。与此同时，RRTstar相比RRT树的平均节点数要少得多，树的结构更简洁。

Goal-bias RRTStar简单地采用启发式目标点偏置的方法即:在一定概率上尝试连接树与目标点，使得树更加朝向目标点进行生长，对比上面左图与右图，加入启发式目标点偏置，RRT扩展的点数更少，并且更快地找到解。从表中的数据可以得到，Goal-bias RRTStar的平均规划时间要比RRT、RRTStar小得多，即使在narrow space 的情况下，其规划速度也更快。

ROS

OMPL

OMPL，一个开放的运动规划库，由许多基于采样的先进的运动规划算法组成。
OMPL本身不包含任何与碰撞检查或可视化相关的代码。这是一个有意的设计选择，因此OMPL不会绑定到特定的碰撞检查程序或可视化前端。该库的设计使其可以轻松集成到提供其他所需组件的系统中。

OMPL官网https://ompl.kavrakilab.org
相关文档https://ompl.kavrakilab.org/geometricPlanningSE3.html

OMPL调用流程

1. 构建状态空间 RealVectorStateSpace(3)；
2. 设置状态空间边界 realVectorBounds(3)；
3. 构建状态信息 SpaceInformation；
4. 构建问题实例 ProblemDefinition；
5. 构造起点和终点并设置值ScopedState；
6. 设置优化目标，这里用路径长度；
7. 构建规划器 Planner，例如RRTstar；
8. 规划器求解 Planner->solve()；
9. 若求解成功，则调用getSolutionPath()获取路径点

结果