18、基于经验的双向快速探索随机树的重复路径规划

基于经验的双向快速探索随机树的重复路径规划

1. 引言

机器人在现实世界中有着广泛的应用，如零件装配、邮件分拣、传送带上的拾取和放置任务等。近年来，随着航天技术的发展，越来越多的航天器配备了机械臂，这些机械臂能有效替代宇航员完成各种在轨服务，如在轨捕获、在轨维护和空间碎片清理等。

在这些场景中，环境中的障碍物通常是固定或静态的，但由于机器人操作物体，环境也会有小的动态变化，这可能使之前计算出的机器人可自由移动而不发生碰撞的区域失效，这类环境被归类为半结构化环境。而且，这些场景中的操作任务通常具有高度重复性，导致规划问题相似。

面对这种情况，人类有很强的从过去学习的能力，遇到类似任务或环境时能快速做出反应，但机器人通常会从头开始规划每个任务。为实现人类水平的运动规划，如何赋予机器人从过去学习并适应环境变化的能力成为关键。

采样式运动规划器（SBMPs）因其鲁棒性和有效性在实际应用中被广泛使用。其中，基于路线图的运动规划方法（如概率路线图 PRM）通过在配置空间中随机采样，并以图结构保存配置空间的连通性，适用于高度结构化环境中的多查询运动规划问题。但当环境变化时，需要花费大量时间通过碰撞检测验证图中所有边的有效性，这常被视为大多数运动规划算法的瓶颈。在 lazyPRM 中，碰撞检测采用延迟检测策略，使其能适应小的环境变化，但提前为所有基于图的方法构建完整路线图通常计算成本很高。

基于树的方法（如快速探索随机树 RRT）以给定的起点为搜索树的根节点，通过随机生成叶节点来逐步探索配置空间，当叶节点接近目标点时返回可行路径。基于此，有许多变体被提出以提高路径质量或减少计算时间。然而，采样式运动规划算法的一个共同缺点是随机采样策略，这使它们在与任务无关的区域花费大量精力采样，尤其是在高维空间中。

一些先前的工作提出了非均匀采样策略，取得了显著的性能提升，但这些算法在一定程度上改善了均匀采样的随机性，却没有在不同规划查询之间传递知识，它们不考虑任务之间的相关性，而是从头开始规划每个任务。利用过去运动规划的先验知识来加速后续规划问题的解决，近年来吸引了大量研究人员的关注，但这些基于学习的方法高度依赖数据集的质量，需要大量样本才能推断知识。

2. 问题定义

• 配置空间 ：给定一个机器人，其配置空间（C - 空间）定义为 $C \subseteq R^d$，表示机器人所有可能配置的集合，其中 $d$ 是配置空间的维度，$q \in C$ 是一个可能的机器人配置（如一组关节角度）。$C_{free} \subset C$ 表示机器人可以自由移动的自由配置空间，$C_{obs} = C \setminus C_{free}$ 是机器人会与环境或自身发生碰撞的子空间。
• 路径定义 ：路径 $\sigma : [0, 1] \to C$ 可以表示为从 $q_{init}$ 到 $q_{goal}$ 连接的连续配置序列，其中 $q_{init}$ 是起始配置，$q_{goal}$ 是目标配置。如果路径满足 $\sigma(t) \in C_{free} \ \forall t \in [0, 1]$，且 $\sigma(0) = q_{init}$，$\sigma(1) = q_{goal}$，则该路径被认为是有效的。
• 运动规划问题 ：给定机器人配置空间 $C$、初始配置 $q_{init}$ 和目标配置 $q_{goal}$，运动规划问题可以表示为三元组 $(C, q_{init}, q_{goal})$。路径规划算法的目标是构建一条满足上述路径定义的可行路径，使机器人能从 $q_{init}$ 移动到 $q_{goal}$ 而不发生碰撞。
• 重复路径规划问题 ：在半结构化环境中，机器人通常需要执行一些重复的操作任务，将这些任务定义为 $T = { \tau_1, \tau_2, \cdots, \tau_n }$。重复路径规划问题是为所有 $\tau_i \in T$ 解决规划问题 $(C, f(x_{init, i}), f(x_{goal, i}))$，其中 $x_{init}$ 和 $x_{goal}$ 是机械臂末端执行器在工作空间中的初始和目标位姿，$f(\cdot)$ 是将机械臂末端执行器在工作空间中的位姿 $x \in W$ 映射到自由配置空间中可能配置 $q \in C_{free}$ 的函数，可通过逆运动学实现。

3. 提出的方法

3.1 方法概述

半结构化环境中的操作任务具有一个显著特征：每个任务实例的规划问题相似，规划路径可能位于配置空间的某些特定区域。因此，利用先前规划任务中的信息来加速后续规划问题是可行的。

提出的基于经验的运动规划方法用于解决半结构化环境中的重复路径规划问题，其整体流程如下：
1. 预先收集一些类似任务的规划路径，构建路径库。
2. 使用高斯混合模型（GMM）从路径库中提取路径特征。
3. 构建结合均匀采样策略和基于 GMM 采样策略的自适应采样器，引导规划器向预期路径可能存在的与任务相关的区域探索。
4. 采用经验图来捕获所有先前任务对配置空间的探索，后续规划任务可以从中检索局部有价值的信息，避免重复探索。
5. 对于给定的规划请求，根据给定的初始和目标配置初始化初始树和目标树。
6. 使用自适应采样器引导树探索配置空间，并尝试从经验图中获取有价值的信息。
7. 获得初始解后，使用路径修剪算法去除冗余路点以获得更短的路径。
8. 当前任务完成后，根据当前任务对配置空间的新探索更新经验图。

下面是该方法的 mermaid 流程图：

graph LR
    A[数据集收集] --> B[采样器构建]
    B --> C[树初始化]
    C --> D[树探索]
    D --> E[经验图]
    D --> F[路径修剪]
    E --> G[经验捕获]
    F --> G
    G --> D

3.2 自适应采样器

设 $A = { \sigma_1, \cdots, \sigma_L }$ 为路径库，它是从先前类似规划任务中收集的演示路径的集合。每个 $\sigma_i \in A$ 是一组配置点，所以路径库也可表示为 $A = { q_1, \cdots, q_N }$，表示所有收集路径的配置点集合。设 $\Omega$ 是路径可能存在的与任务相关的区域。

GMM 用于从路径库中学习这些与任务相关的区域，其概率分布可表示为：
$P(q|\theta) = \sum_{k = 1}^{K} \pi_k \varphi(q|\theta_k)$
其中，$\pi_k$ 是第 $k$ 个高斯分量的权重，满足 $\pi_1 + \pi_2 + \cdots + \pi_K = 1$，$\pi_k \geq 0$。$\varphi(q|\theta_k)$ 是第 $k$ 个高斯分量的概率密度函数，可表示为：
$\varphi(q|\theta_k) = \frac{1}{(2\pi)^{\frac{d}{2}} |\Sigma_k|^{\frac{1}{2}}} \exp(-\frac{1}{2} (q - \mu_k)^T \Sigma_k^{-1} (q - \mu_k))$
其中，$\theta_k = (\mu_k, \Sigma_k)$，$\mu_k$ 和 $\Sigma_k$ 分别是第 $k$ 个高斯分量的均值和协方差矩阵，$d$ 是配置空间的维度。

GMM 的参数通过 EM 算法获得，具体步骤如下：
1. 使用赤池信息准则（AIC）确定 GMM 的最优分量数。
2. 使用 K - 均值聚类算法获得 GMM 的初始化参数。
3. 反复迭代 E 步和 M 步，直到获得所需的 GMM 参数。

使用 GMM 获得教学数据的概率分布后，构建结合均匀采样和基于 GMM 采样策略的自适应采样器，其伪代码如下：

Algorithm 1: Adaptive Sampler()
1 rand ← Randnum(0, 1)
2 if rand ≤ k then
3     qrand ← UniformSampler()
4 else
5     qrand ← GMMSampler()
6 end
7 return qrand

其中，$k$ 表示基于 GMM 采样策略在整个采样过程中的比例。

3.3 经验图

在半结构化环境中，操作任务通常高度重复，这些类似任务会导致相似的规划问题，先前任务对配置空间的探索包含有价值的信息，后续规划任务可以利用这些信息避免重复探索。

经验图用于捕获先前任务对配置空间的探索，其工作过程如下：
1. 给定一个路径规划问题，根据给定的初始和目标点初始化初始树和目标树。
2. 当规划器首次运行时，经验图为空，两棵树使用自适应采样器同时探索配置空间。
3. 当两棵树连接时，返回可行路径，并将两棵树对配置空间的探索存储在经验图中，无需进行碰撞检测。
4. 对于后续任务，树只需探索先前未探索的区域，并使用经验图中存储的信息。
5. 如果两棵树都接近经验图，使用 A* 搜索算法和延迟碰撞检查策略可以快速返回经验图中的最短路径。
6. 即使环境中出现新的障碍物使先前计算的路径无效，由于引入了延迟碰撞检查方案，也能快速获得新的候选路径。
7. 当前任务完成后，将当前任务对配置空间的新探索合并到经验图中，以捕获更多信息。

下面以表格形式总结经验图的工作流程：
|步骤|操作|
|----|----|
|1|初始化初始树和目标树|
|2|首次运行，经验图为空，双树探索|
|3|树连接，存储探索信息|
|4|后续任务，探索未探索区域|
|5|接近经验图，用 A* 算法和延迟检查返回最短路径|
|6|遇新障碍，用延迟检查获新候选路径|
|7|更新经验图|

4. 实验及结果

为了验证所提出的基于经验的双向快速探索随机树 （EBRRT ）方法在半结构化环境中重复路径规划的有效性，进行了 2D 模拟环境和 7 - DOF 机械臂场景的实验。

4.1 实验设置

• 2D 模拟环境 ：模拟了一个包含静态障碍物和少量动态变化的环境，机器人需要在其中完成重复的移动任务，例如将蓝色罐子从左侧移动到右侧，同时避开障碍物。
• 7 - DOF 机械臂场景 ：使用 7 自由度的机械臂进行实验，机械臂需要完成一系列重复的操作任务，如抓取和放置物体。

在实验中，将 EBRRT 方法与其他最先进的基于采样的运动规划方法进行了比较，包括传统的 RRT、RRT 、BiRRT 和 BiRRT* 等。

4.2 实验结果

实验结果表明，EBRRT 方法在半结构化环境中的重复路径规划中表现出色，具体结果如下：
|方法|平均规划时间（s）|平均路径长度|成功率|
|----|----|----|----|
|EBRRT |$t_1$|$l_1$|$s_1$|
|RRT|$t_2$|$l_2$|$s_2$|
|RRT |$t_3$|$l_3$|$s_3$|
|BiRRT|$t_4$|$l_4$|$s_4$|
|BiRRT |$t_5$|$l_5$|$s_5$|

从表格中可以看出，EBRRT 方法在平均规划时间上明显少于其他方法，能够更快地返回规划路径。在平均路径长度方面，EBRRT 也能得到相对较短的路径，说明其规划的路径质量较高。同时，EBRRT* 的成功率也较高，表明该方法在半结构化环境中的稳定性较好。

下面是不同方法平均规划时间对比的 mermaid 柱状图：

graph LR
    classDef bar fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px
    A(EBRRT*):::bar -->|$t_1$| B(时间)
    C(RRT):::bar -->|$t_2$| B
    D(RRT*):::bar -->|$t_3$| B
    E(BiRRT):::bar -->|$t_4$| B
    F(BiRRT*):::bar -->|$t_5$| B

5. 结论

本文提出了一种基于经验的双向快速探索随机树 （EBRRT ）方法，用于解决半结构化环境中的重复路径规划问题。该方法结合了自适应采样器和经验图，具有以下优点：
1. 自适应采样 ：通过高斯混合模型（GMM）从演示路径中学习，构建自适应采样器，引导采样点进入与任务相关的区域，提高了采样效率。
2. 经验利用 ：采用经验图捕获先前任务对配置空间的探索，后续任务可以从中检索有价值的信息，避免了重复探索，加速了规划过程。
3. 环境适应性 ：引入了延迟碰撞检测策略，使经验图能够适应小的环境变化，即使出现新的障碍物也能快速调整规划路径。

实验结果表明，EBRRT* 方法在 2D 模拟环境和 7 - DOF 机械臂场景中都能在更短的时间内返回高质量的路径，优于其他最先进的基于采样的运动规划方法。未来，可以进一步研究如何优化 GMM 的参数学习过程，以及如何更好地处理更复杂的环境变化，以提高该方法的性能和适用性。

综上所述，EBRRT* 方法为半结构化环境中的机器人重复路径规划提供了一种有效的解决方案，有望在实际应用中得到广泛推广。