3基于采样方法路径规划

Sample-based path finding

Preliminaries

在空间中随机撒点，连成树或者图，利用算法寻找路径

characters

不需要明确构建构型空间及其边界，只需要知道一个机器人位形是否处于碰撞中，在对空间有充分认识下进行简单的碰撞测试，碰撞检测是一个单独的模块,可以根据应用程序进行调整，随着碰撞检测的改善,进而实现这些算法，有单查询和多查询不同的方法

Notion of completeness in planning

complete planner: 在有限时间内正确查找到路径（A* , JPS)
Probabilistic Complete Planner: 概率完备解；如果存在解，规划器能够使用随机采样找到解，也就是如果存在一个可行解，当样本数趋于无穷大时，找到的概率为1。
Resolution Complete Planner: 在grid中寻求解，如果存在解，规划器使用确定采样找到解
解：
寻找函数的解方法:梯度下降法，牛顿法等
如何探索解空间的连续性：在连续的构型空间采样出离散的构型空间样本，来构建图或者树的结构来表征解空间的连续性，采样一般有批次或增量式进行，

Pobabilistic Road Map（PRM）

概率路图

T

• a graph structure

• 2阶段

  • Learning phase
    在位形空间中随机采n个点
    删除有碰撞的点
    连接点，起始点和终点需要包含，连接与点接近的点（虚线不能够连接），但是连线不能经过障碍物，并且不考虑机器人的位形约束
    
    撒点
    

  • Query phase
    在图中搜索路径,采用A*或者Dijistra算法，可以看做使用路线图代替了grid map，可以反复利用这个图，而无需得到整个图
    

• Pro and Cons
  Pro: probabilistically complete
  cons: 在状态空间上构建图，但不特别关注生成路径,不够有效率，机器人的运动学约束也会容易不满足, 而且需要解决2点边界值问题

Improve efficiency

Reason

• Lazy collision-checking
  只关注距离准则，然后找到路径，如果有障碍物，则删去，重新构建图，
  
  
  
  这样避免了learning phase中碰撞检测（碰撞检测费时）

Rapidly-exploring Random Tree（RRT）

与PRM不同之处在于，
快速搜索随机树大致步骤:采样点，连接构成树，检测无误，然后插入新点，进行迭代，特点是增量式构建
相较于PRM:构建的不是图，而是树，不需要使用A*算法，加入终点便结束了

pseudocode

Input : M,xInit,xGoal
Result: a path from xInit to xGoal
T.init();//2 red point
for i =1 to n do
	xRand<- Sample(M);//获得xrand点
	xNear<-Near(xRand,T);//寻找距离xrand最近的点
	xNew<-Steer(xRand,xNear,StepSize);//找到xnew, stepsize步长
	Ei<-Edge(xNew,xNear);//
	if CollisionFree(M,Ei) then//碰撞检测，无障碍物则
		T.addNode(xNew);//xnew被添加到树中
		T.addEdge(Ei);
	if xNew=xGoal then
		success();

演示图

发生碰撞则不添加到树中

可以参考视频描述

地址

首先确定空间中起点和终点的位置，在空间中随机撒点,得到x_rand;寻找距离最近的x_near(一开始时x_start),然后以一定的步长扩展x_new，如果x_new不在障碍物上，那么就扩展，知道x_new与终点的距离小于一个阈值，便生成目标点，得到最终路径，反向查询，沿终点回溯到起点。最终的父节点是起点。

Pros

• 具有目标导向性，计算复杂度低
• 能在高维空间中相对快速找到满意解
• 适应能力强

Cons

• 并非最优的路径
• 对整个空间采样，效率不高
• 在狭窄空间难以找到路径

improving

• 使用kd-tree加速搜索（对点进行划分）
  左图展示RRT树；中间得到一个采样点rand，利用kd-tree能够找到距离最近的节点
  
  在空间中的划分
  
  右边为划分依据，先找横向x中位数，再找纵向y中位数，进行划分，如上图，4.0为所有节点x的中位数，进行划分，可取2.2和4.2,这里左边2.2取为中位数，右边4.3，依次类推，具体可以参考链接
• Bidirectional RRT / RRT Connect
  一次撒点，两树构建，针对终点在narrow passage
  

Optimal Criterion

采用动态规划思想，离散形式如下：

B(j)表示j的前驱
D(i,j)表示从i状态到j状态的代价。

在运动规划中，状态图(或网格)几乎肯定是成环的。
每个状态可以在任何级别(步长)，并且只能通过范围查询或k-NN查询访问状态。
基于抽样的方法在增量或批量生成隐式随机几何图(random geometric graph RGG)时引入了一些随机性。它仍然可以被看作是图形搜索。

Optimal sampling-based path planning methods

RRT*

• pseudocode
  
  
  初始化部分与RRT相同
  与RRT不同之处在于树的扩展，Xnew没有直接选择最近的点作为父节点，而是搜索多个领域（蓝色圈）节点，从而选择父节点Xnear，如X2原来的父节点是X1，但是通过搜索发现Xnew节点作为父节点更近，那么通过rewire()剪枝操作，删去X1-X2将其父节点更新为Xnew。
  视频演示
  RRT生成路径之后便停止，RRT*生成路径之后还在不断优化。

关于蓝圈的大小

$\eta$:扩展节点的步长
card(V):集合的势，也就是集合元素的个数，这里表示节点的个数
d表示构型空间的维数
$\gamma$:为了达到渐进最优的系数

$\xi$单位球的体积
考虑时间维度的话可以参考 Revised in Kiril Solovey et al., “Revisiting the Asymptotic Optimality of RRT*", ICRA, 2020

工程实现优化方法

优化数据结构，如采用KD树，
对树进行剪枝；
延缓碰撞检测，对生成的父节点进行排序；
使用双向树，起点终点各生成一颗树。
重复利用数据，可以记录碰撞检测，一般代价要对称。

RRT* flaw

过度探索，"重连"无希望的顶点。


探索不足，“Rewire”仅在新节点被添加到生成树之后发生，并且只在其“Near”节点上起作用。
•它不能保证任何中间迭代的中间路径是最优的

RRT*到这里便结束了，但其实还可以在范围内的其他节点添加

RRT#

改正了上面RRT*的两大缺陷，生成渐进最优路径更快了

Kinodynamic-RRT*

采用曲线取代线段连接，更好的符合机器人的运动学约束

视频

Anytiem-RRT*

在机器人跟随路径运动时，实时更新树，优化路径

视频

Advanced Sampling-based Methods

Informed Sampling

Informed RRT*

可以不用在整个空间在撒点，当路径找到，在路径处撒点，在一个以起点和终点为焦点椭圆内撒点，以红色点和绿色点为焦点画圆
L2 Informed set

以欧式距离为代价评判，

缺点：除了L2范数代价(欧氏距离)，几乎不能显式地应用于其他代价，对于间隙很长的地图，搜寻慢

GuILD

Guided Incremental Local Densification
虽然迭代可能没有找到一个更短的路径到达目标，新的更短的路径到搜索树中的其他顶点可以立即改进采样策略。

informed集保持不变。但是，GuILD 利用搜索树中改进的成本来更新本地子集。
初始信标集缩小为进一步的焦点采样，信标和目标之间剩余的松弛用来扩展信标-目标集
在信标和目标之间，可以花费更大的路径长度预算，但总的来说仍然可以产生更短的路径。

Cross-entropy motion planning

首先生成路径（可以借助RRT or RRT*),在轨迹节点周围采样，这些节点构成多高斯模型，得到路径；以节点为均值，均值化后的节点重新进行采样，继续得到更多路径。方差则是通过指定，如图，通过两次采样，得到两条路径，共三条路径，多个轨迹的多个节点求均值，作为新多高斯模型参数，然后再进行采样，不断优化，从而找到路径。

Other variants

• 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

Implementation

open motion planning Library
Moveit
Tutorials

Ros Navigation stick

move_base

常见

• Global planner
  A*,D*,RRT,etc
• Local planner
  DWA,Eband,Teb,etc

ref

https://www.shenlanxueyuan.com/course/633