【路径规划论文整理(1)】Path Deformation Roadmaps(附带对PRM改进算法、同伦映射的整理)

本系列主要是对精读的一些关于路径搜索论文的整理，包括了论文所拓展的其他一些算法的改进思路。

这是本系列的第一篇文章：

Jaillet, Léonard & Siméon, Thierry. (2008). Path Deformation Roadmaps: Compact Graphs with Useful Cycles for Motion Planning. I. J. Robotic Res… 27. 1175-1188. 10.1177/0278364908098411.

1. Introduction(含对PRM改进算法的整理)

关于PRM可以参考笔者之前的博客。本节主要将传统PRM的问题以及改进方法。

PRM的问题：在狭小的通道可能很难被采样到，要高密度的采样才能在狭小通道处形成路径。这带来了内存和时间的增长。

这篇论文总结了多篇文章提到的不同解决方法，笔者将对这些方法展开说明一下。虽然这些方法比较老，但是都有很好的创新，做一个展开的总结，有利于后续研究思路的打开。

1.1 偏置采样

D. Hsu, T. Jiang, J. Reif, and Z. Sun. The bridge test for sampling narrow passages with probabilistic

roadmap planners. Proc. IEEE Int. Conf. on Robotics and Automation, 2003.

这篇文章中引入了桥梁测试这一采样方法和传统的均匀采样相结合。bridge test:在配置空间中的狭窄通道内，可以想象成一条线段，其两个端点位于障碍物内，而中点悬浮在自由空间上方，类似于一座桥梁。具体工作原理：

1. 线段选择：随机选择配置空间中的两个点作为线段的端点，这两个点应位于障碍物内部，以确保线段跨越狭窄通道；
2. 碰撞检测：对于这两个端点以及它们的中点，执行碰撞检测，如果端点发生碰撞，而终点没有碰撞，这表明中点位于自由空间内；
3. 接受新节点：如果中点无碰撞，它将被接受为路径图中的新节点，如此一来增加了狭窄通道内的新采样点。

   这种方式的采样相对于均匀采样提高了再狭窄通道区域的采样密度，同时减少了开阔区域的采样密度。

1.2 中轴线附近采样

J.-M. Lien, S.L. Thomas, and N.M. Amato. A general framework for sampling on the medial axis of the free space. Proc. IEEE Int. Conf. on Robotics and Automation, 2003.

该论文提出了一个通用框架，用于在free space的中间轴上进行配置空间的采样。(中轴线的思路应该是来自于Voronoi图)，具体流程：

1. 节点生成：在配置空间中均匀的随机采样节点p。如果采样的节点处于自由空间，计算其到最近障碍物之间的最短距离；如果采样的节点处于碰撞状态，计算到自由空间的最短距离，文中定义为障碍物的最深入程度。二者都会获得在障碍物边界上的投影点q。
2. 节点收缩到中间轴：对于自由节点，设置收缩方向为qp，起点为p；对于碰撞节点，设置搜索方向为pq，起点为q。从起点出发，沿着收缩方向移动节点到两个相邻边界的中点。
3. 连接节点

1.3 自由空间膨胀(这种方法通常用于机械臂)

H.-L. Cheng, D. Hsu, J.-C. Latombe, and G. S´anchez-Ante. Multi-level free-space dilation for sampling narrow passages in PRM planning. In Proc. IEEE Int. Conf. on Robotics & Automation, pages 1255–1260, 2006.

本文是利用收缩算法和多级膨胀规划器(MLDP)来解决传统PRM中狭窄通道采样问题。

收缩算法(通过对机器人或障碍物的几何模型进行收缩来膨胀自由空间F，从而简化狭窄通道的采样问题)：

1. 输入一个多面体模型M，例如机器人或障碍物的几何模型。
2. 对模型M进行四面体化处理，将其内部划分为四面体组。
3. 对模型M的每个表面顶点p，计算其内核(kernel，即顶点p可以移动的体积)以确保收缩后的模型M’始终包含在M内部。
4. 对于每个表面顶点p，计算一个点τp在其内核内，然后将顶点p沿着指向τp的方向移动到新的收缩位置p’。
5. 当生成新的收缩模型时，为了避免为每个版本的模型重建碰撞检测树，算法只需扩大每个叶节点的边界体积，以包含所有收缩因子下的三角形版本。

多级膨胀规划器（MLDP）：

1. 设置膨胀参数：初始化膨胀参数s的低值(slow)和高值(shigh)，并设置迭代次数上限k。
2. 二分搜索：在每次迭代中，根据当前的s值(slow和shigh的中点)，使用搜索算法生成一个新的收缩模型M’。
3. 调用PRM规划器：在新的收缩自由空间F’中调用PRM规划器，尝试从初始配置qint到目标配置qgoal的路径γ’。
4. 路径修复：如果找到了路径γ’，但由于膨胀过度，路径γ’可能在原始自由空间F中存在碰撞。因此，需要通过采样和变形来修复路径γ’，使其称为F中的有效路径。
5. 调整膨胀参数：如果路径γ’无法修复，则说明膨胀参数s过大，需要减少s ；如果路径γ’未找到，则说明s过小，需要增大s。重复迭代。

1.4 基于可见性滤波

T. Sim´eon, J.-P. Laumond, and C. Nissoux. Visibility-based probabilistic roadmaps for motion planning. Advanced Robotics Journal, 14(6):477–494, 2000.

论文中提出了一个叫做可见性域(Visibility Domain)的概念：对于机器人R在一个工作空间W中运动，考虑机器人的配置空间CS和与之相关的无碰撞配置空间CSfree。定义一个局部方法C，它可以计算两个给定配置q和q’之间的路径L(例如直线段)。对于配置空间中的一个配置q，其可见性域Visc(q)由以下集合构成：

V i s c ( q ) = { 所有在 C S f r e e 中的 q ′ ，使得 C ( q , q ′ ) 也在 C S f r e e 中 } Visc(q)=\{所有在CS_{free}中的q'，使得C(q,q')也在CS_{free}中\} Visc(q)={所有在CSfree​中的q′，使得C(q,q′)也在CSfree​中}

换句话说，可见性域Visc(q)包含了所有可以从配置q通过局部方法C安全到达的无碰撞配置q’的集合。在这个定义中，配置q被称为守卫（guard）配置，因为它“守卫”或“监控”着其可见性域内的所有点。

构建可见性路线图：

1. 初始化一个空的图R，其中包含一组守卫节点(初始可见性域)和连接节点(相当于被两个不同的守卫节点同时"监控"到)。
2. 通过迭代过程，随机选择无碰撞点并确定它们是否被现有守卫节点可见或是否可以连接两个守卫节点。
3. 如果一个新点对于现有守卫节点不可见，它将称为一个新的守卫节点，并添加到图R中；如果新点可以连接两个不同的守卫节点，它将称为一个连接节点，并在图R中添加相应的边。
4. 迭代终止条件：每当新的守卫节点加入到路线图中，CSfree的覆盖率会增加，通过估计未覆盖体积与CSfree总体积的比例来控制结束条件。

1.5 自适应采样

S. Rodriguez, S. Shawna, R. Pearce, and N.M. Amato. Resampl: A region-sensitive adaptive motion planner. Proc. Workshop on the Algorithmic Foundations of Robotics, 2006.

这篇文章提出了一种区域敏感的自适应运动规划方法，利用局部区域信息来智能地做出关于采样、样本连接和路径寻找的决策。

具体方法如下：

1. 区域构建：从配置空间中获取一组点，这些点包括了在自由空间的和在障碍物内部的；根据初始化样本集(b)定义区域，每个区域由一个代表性点(例如区域中心)和邻近点(用于计算区域统计信息，如熵和半径)组成；区域构建算法通过随机选择未标记地点作为新的区域中心，并选择其最近的邻居样本形成区域。
2. 基于熵的区域分类：低熵表示区域中的样本完全是自由的或完全被阻塞，高熵表示样本中既有自由样本也有被阻塞的样本。
3. 区域图构建：构建一个图结构近似描述局部配置空间区域的连通性。图中的顶点对应于区域，如果两个区域重叠，则在它们之间放置一条边；区域图可以用来提取区域路径以辅助单查询规划，或细化它以辅助多查询规划；通过合并相邻的同类型区域或分割不清晰分类的区域来完善区域图。
4. 多查询规划：构建区域和区域图只保留重要样本，减少构建成本，专注于配置空间中困难/狭窄区域。
5. 单查询规划：为给定查询从起始到目标配置找到路径，使用区域图找到连接起始和结束区域的路径，区域图是加权的，优先通过未阻塞区域提取路径，仅在必要时使用阻塞区域(如果路径中发现阻塞区域，对这些区域进行重新分类，以获得一个连续的未阻塞路径区域序列)；为了提高在困难区域提取路径的能力，可以通过包括邻近未阻塞区域来扩展区域路径的体积，这样做可以在困难区域中包含更多的区域，从而改进路径；对于给定的区域路径，可以根据需要采样节点，并使用k近邻连接策略来连接这些节点。

1.6 利用搜索空间信息(类似于探索类问题中的最大信息增益)

B. Burns and O. Brock. Toward optimal configuration space sampling. Proceedings of Robotics: Science and Systems, 2005.

本文提出了一种自适应的运动规划方法——效用引导采样策略(Utility-Guided Sampling Strategy) ，利用过去的经验来指导未来的样本选择，以便更有效地探索配置空间并构建路径图。

具体实现：

1. 初始化：从配置空间中随机选择初始点，并标记他们的状态(阻塞或自由)。
2. 模型构建：使用K-NN来构建一个能够预测新样本状态的近似模型，来估计配置的状态概率。
3. 效用计算：定义一个效用函数，该函数基于当前路径图的结构和样本可能带来的信息增益(新样本减少路径图的不确定性的贡献)，效用函数考虑当前路径图的结构，以及新样本可能对路径图造成的改变。
4. 样本选择与评估：对于每个新的样本候选，使用近似模型和效用函数来计算其期望效用。
5. 迭代更新。

1.7 延迟障碍检测

R. Bohlin and L.E. Kavraki. Path planning using lazy prm. IEEE Int. Conf. on Robotics and Automation, pages 521–528, 2000.

该论文旨在最小化路径规划过程中碰撞检查的数量，从而减少规划器的运行时间。Lazy PRM算法构建了一个假设无碰撞的路径图，该路径图由用户定义的初始和目标配置以及随机生成的节点组成。算法的目标是在路径图中找到初始和目标配置之间的最短可行路径。与标准的PRM算法不同，Lazy PRM最初假设所有节点和边都是无碰撞的，只在必要时才进行碰撞检查。

算法步骤：

1. 构建初始路径图：算法开始时，首先在配置空间中随机生成一组节点，并将初始点和目标点加入到这些节点中，这些节点通过边连接起来，边代表节点之间的直接路线。
2. 选择邻居节点：对于每个节点，算法确定一组邻居节点，这些节点与该节点足够接近，从而可能通过无碰撞的直接路径连接。
3. 搜索最短路径：A*
4. 检查路径碰撞：一旦找到路径，算法将检查路径上的节点和边是否碰撞；发现碰撞，将从路径途中移除相应的节点和边，并重新搜索路径。
5. 节点增强：如果在路径图中找不到可行路径，算法将通过生成新节点并将其添加到路径图中来增强路径图，新节点可以均匀分布，也可以基于路径图中已有信息集中在困难区域。
6. 多次查询：一旦找到无碰撞路径，算法将终止并返回路径。

   Lazy PRM算法是一种高效的路径规划方法，特别适合于那些碰撞检查成本较高的场景。通过减少不必要的碰撞检查，算法能够快速地为机器人找到可行路径，同时保持对路径质量的考虑。此外，算法的自适应学习能力使其在处理多次查询时更加高效。

1.8 回到正文内容的Introduction

论文的目标是提出一种方法，能够在保持路线图紧凑性的同时，捕捉配置空间中多种自由路径的多样性。

2. 一些概念

2.1 同伦(Homotopy)

同伦这个概念来自于拓扑数学，两个拓扑空间如果可通过一系列连续的形变从一个变成另一个，那么就称这两个拓扑空间同伦。参考博客

如下图，f和g就是同伦映射关系。

2.2 直纹曲面(Ruled Surface)

在几何学中，如果一个曲面上的任意一点上均有至少一条直线经过，则称该曲面为直纹曲面。另一种常见的说法是，如果一个曲面可以由一条直线通过连续运动构成，则可称其为直纹曲面。参考博客

2.3 K阶变形

一种特殊的同伦变形，使得将τ点转换为τ’点的每条曲线都是K段的角线，即由K个连续直线段形成的分段线性曲线。一阶变形表面表示直纹表面；并且通过K个直纹表面接连获得K阶变形。

2.4 路径可见性图(Visibility Diagram of Paths)

下图是计算一对路径之间可视性图的例子，白色表示可视，蓝色表示不可视。两条路径之间的可见性(一阶)变形可以表示如下：当且仅当在它们的可见性图中存在可以连接坐标(0,0)和(1,1)的路径，此时两条具有相同端点的路径是可见性变形的，即d图。

3. K阶变形路径图(K-order Deformation Roadmap)

Def：R是K阶变形路径图当且仅当对任何自由空间的路径τ可以在R中提取出与τ两端相同的K阶变形路径τ’。

3.1 RCPV Roadmaps

Def: RCVP即对于自由空间中的任何点，可见的子路线图都是连接的。

下图左图视点qv的子路线图是断开的，右图视点qv的子路线图是连接的。

4. 主要算法(构建变形路径图)

算法分为两个步骤：

1. 构建一个小的树，获得空间中的连通性。
2. 初始树被增强为具有多重连通性所需要的有用循环。

第一步采用Visibility-PRM(见Introcution部分)；第二步在节点和边上丰富第一步生成的树，创建路径变形路线图的有用循环，具体如下：

5. 实验结果和总结

• 实验表明，该技术能够可靠地捕捉复杂空间中多重连通性的多连通性，适用于自由飞行和关节机器人在2D和3D环境中的问题。
• PDR在保持紧凑结构的同时，能够有效地捕捉到不同路径的多样性，提高了路径规划的质量和效率。