Cell Decomposition系列路径规划算法——PCD

做个正直的人

思想

PCD是目前Cell Decomposition算法中，我认为表现最好的。
基于概率的算法（比如RRT、PCD）和确定性算法（比如A*、Dijkstra）相比，不需要显式的表达，因而在高维问题中具有相当大的优势。可以大大节省计算量和空间需求。

• PCD虽然是基于ACD的，也是对各个cell进行不同粒度的分解，也是采用固定的形状，但是PCD中的cell是free还是occupied并不是确定的，而是概率的。PCD在一个cell中进行采样，如果所有的采样都是free，那么，这一个cell就被认为是 possibly free cell；如果所有的采样都是occupied，那么，这一个cell就被认为是possibly occupied cell；如果采样中，有free，有occupied，那么这一个cell被认为是known to be mixed，也就是需要被分解成前两个类型。
• 在一轮分解完成之后，采用一个图搜索算法去搜索一个连接init和goal的cell队列，称为通道channel。
• 搜索到一个通道之后，就会检查相应的path是不是安全，不安全就分解那一个不安全的possibly free cell；若是没有搜索到这样一个通道，那么说明当前的所有possibly free cell还没有连接成1个连通图，这肯定是因为有possibly occupied cell把这两个子连通图给隔开了，那么PCD就去这些possibly occupied cell采样并分解。

这一些步骤迭代下去，就能找到一个连接init和goal的path。下面具体的讲解一下PCD算法的细节。

1、表示

我们把一个cell $k_i$划分为以下三类之一

• possibly free cell，if 该cell内的采样都是free
• possibly occupied cell，if 该cell内的采样都是occupied
• known to be mixed，if 该cell内的采样有free，有occupied

包含了起始位置$q_{init}$和$q_{goal}$的cell分别记为$K_{init}$和$K_{goal}$。

在图搜索阶段，我们把每一个possibly free cell都看作是一个node，如果两个possibly free cell是相邻的（有公共边），那么我们就把这两个cell对应的node用edge给连接起来。最后我们就可以得到一些个连通图。

包含了$K_{init}$的连通图记为开始区域$R_{init}$。

包含了$K_{goal}$的连通图记为目标区域$R_{goal}$。

搜索阶段我们找到的那一系列连接$K_{init}$和$K_{goal}$的cells称为一个通道channel。

2、算法步骤

1. 初始化：把整个地图看做一个cell，这个cell中有两个采样点$q_{init}$和$q_{goal}$