13、基于运动描述符的机器人运动规划

基于运动描述符的机器人运动规划

1. 引言

在机器人运动规划中，如何找到符合特定运动描述符（MoDs）特性的运动序列是一个关键问题。例如，我们可能希望机器人的运动顺应人流方向，避免拥堵区域，或者根据不同的任务需求表现出不同的行为。如图所示，机器人（蓝白方块）需要到达走廊远端的目标位置（黄色方块），同时要避开人员（橙蓝标记）和障碍物（棕色标记代表柱子和雕塑，绿色标记代表植物，黑灰结构代表传送带）。当机器人规划走左侧时，虽然运动强度较低，但由于人们的运动方向相反，机器人需要多次执行避障操作；而当机器人能够获取人流方向信息时，它会选择右侧路线，尽管动态强度较高，但能顺应人流，无需执行避障操作。

为了实现这些不同的行为，我们可以在运动规划算法中引入与 MoDs 相关的额外成本组件。在详细介绍这些额外成本组件的公式之前，我们先介绍用于运动规划的 RRT* 算法及其优势，然后讨论两种不同的成本函数，接着引入基于采样的运动规划中的信息采样函数，最后评估使用这些成本函数的规划器，并讨论这些考虑 MoDs 的规划器相对于不考虑 MoDs 的规划器的各种定性和定量优势。

2. 基于 RRT* 的采样式运动规划

在运动规划领域，存在多种规划方法，总体可分为组合式和采样式两类。组合式方法需要对环境有明确的了解，并对状态空间进行详尽的探索，因此在高维系统中扩展性不佳。而采样式方法通过仅在采样状态下探测环境来解决规划问题，只需要一个碰撞检测单元来探索状态空间。在高维空间和具有复杂微分约束的系统中，采样式方法在路径质量和规划效率方面表现优于组合式方法。

在大多数实际情况下，机器人的运动规划通常涉及微分约束，这是由机器人的运动学和动力学特性引起的。因此，在运动规划过程中考虑这些微分约束是很有必要的，这样才能确保生成的路径是可行的。同时，我们也希望运动规划算法是最优且完备的。基于这些原因，采样式运动规划（SMP）算法成为实际应用中运动规划的理想选择。常见的 SMP 算法包括快速探索随机树（RRT）、概率路图（PRM）及其最优变体 RRT 和 PRM 。在本文中，我们将重点关注 RRT* 算法。

2.1 RRT 和 RRT*

如今，单查询采样式运动规划器已成为解决涉及复杂微分约束的规划问题的事实上的标准。下面我们简要介绍两种这样的规划器：RRT 及其最优变体 RRT*。

• RRT ：RRT 算法是一种用于解决运动规划问题的增量采样和搜索技术。给定起始状态和目标区域，该算法会返回一个代表解决方案的运动序列。算法通过采样方案随机选择无碰撞状态，并使用局部规划方法将这些状态连接到搜索树中（前提是连接路径无碰撞）。采样和连接步骤会重复进行，直到采样到目标区域中的一个配置并能将其连接到搜索树为止。RRT 算法具有概率完备性，即如果存在解决方案，随着采样状态数量趋于无穷大，找到解决方案的概率趋于 1。然而，RRT 算法返回的是第一个找到的可行解决方案，可能不是最优解决方案。
• RRT ：RRT 算法是 RRT 算法的扩展，它在找到初步解决方案后会不断改进。与 RRT 不同，RRT 具有渐近最优性，即随着采样次数的增加，最终得到的路径会趋近于最优路径。RRT 确保只保留搜索树中每个顶点的最小成本路径。当一个新采样的状态连接到树中时，如果通过该顶点的路径更便宜，则会对现有路径进行重连。下面是 RRT* 算法的伪代码：

Data: A start state xstart and a goal region Xgoal
Result: A trajectory from start state to goal region
τ.AddNode(xstart)
c(xstart) ← 0
t ← 1
while t ≤ T do
    xrand ← Sampling()
    xnear ← NearestSearch(τ, xrand)
    σ, xnew ← Extend(xnear, xrand)
    if σ ∈ Xobs then
        continue
    end
    τ.AddNode(xnew)
    τ.AddEdge(xnear, xnew, σ)
    cost(xnew) ← cost(xnear) + cost(σ)
    τ.Rewire(xnew, xnear)
    if xnew ∈ Xgoal then
        solution ← Path(xstart, xnew)
    end
    t ← t + 1
end
if solution = φ then
    return failure;
else
    return solution;
end

算法开始时，搜索树 τ 只包含起始状态 xstart。Sampling 函数负责从配置空间中随机选择无碰撞状态，NearestSearch 函数用于在树中找到距离新采样状态最近的顶点，Extend 函数（也称为转向函数）是一种局部规划方法，用于尝试连接 xnear 和 xrand 并返回连接路径 σ。碰撞检测通过检查路径 σ 是否与障碍物空间 Xobs 相交来实现。AddNode 和 AddEdge 函数分别用于向树中添加节点和边，cost 函数用于计算特定状态或路径的成本，Rewire 函数用于确保树中每个顶点的路径成本最小。

在存在微分约束的情况下，Extend 函数还会考虑运动学和/或动力学约束，并返回控制输入序列（如速度、加速度）。为了充分利用 MoDs 信息，我们将考虑依赖于速度信息的成本函数。

2.2 转向函数

接下来，我们简要介绍与两种轮式移动机器人相关的转向函数：Dubins 汽车和差分驱动机器人。

• POSQ ：对于差分驱动机器人，我们使用状态 x = (x, y, θ, v) 进行配置，其中 (x, y) 是笛卡尔坐标，θ 是航向角，v 是平移速度。经过笛卡尔坐标到极坐标的转换后，运动方程如下：
• $\dot{\rho} = -\cos\alpha v$
• $\dot{\alpha} = \frac{\sin\alpha}{\rho} v - \omega$
• $\dot{\varphi} = -\omega$
  其中，$\rho$ 是机器人位姿 (x, y, θ) 与目标状态的笛卡尔坐标之间的欧几里得距离，$\varphi$ 是机器人参考系 {Xr} 的 x 轴与目标状态参考系 {Xg} 的 x 轴之间的夹角，$\alpha$ 是机器人参考系的 y 轴与连接机器人和目标位置的向量之间的夹角，v 是平移速度，$\omega$ 是角速度。

为了精确、平滑且高效地连接任意两个状态，我们使用并扩展了 POSQ 转向函数。当在算法中调用该转向函数连接两个采样状态 x1 = (x1, y1, θ1, v1) 和 x2 = (x2, y2, θ2, v2) 时，我们首先使用 POSQ 进行初始扩展，然后修改速度曲线，使初始速度等于 v1，最终速度等于 v2。速度曲线使用高效的三阶多项式时间律生成。

• Dubins 汽车 ：Dubins 汽车的运动学模型如下：
• $\dot{x} = v \cos\theta$
• $\dot{y} = v \sin\theta$
• $\dot{\theta} = \omega$
  其中，x 和 y 是机器人相对于固定世界坐标系的欧几里得坐标，$\theta$ 是机器人的航向角，v 是切向速度，$\omega$ 是角速度。Dubins 等人假设汽车以恒定速度 v = 1（即向前行驶），其最优路径由不超过三个运动基元组成：直行（S，$\omega$ = 0）、左转（L，$\omega$ = 1）、右转（R，$\omega$ = -1）。具体来说，最优路径由以下几种曲线组合而成：LSR（左转、直行、右转）、LSL、RSR、RSL、RLR、LRL。

3. 基于运动描述符的运动规划成本函数

在使用 RRT* 进行规划时，成本函数是定义机器人最终行为的关键组件。我们的目标是设计能够平衡机器人不同可能行为的成本函数，使机器人既能高效、成功地完成任务（如到达目标），又能表现出期望的行为（如顺应人流、逆人流、避免拥堵、趋向拥堵等）。

假设一条轨迹 ς1a 由一系列点 σ1, σ2, …, σa 组成。对于类似汽车的机器人，每个轨迹点包含机器人的位置、航向和速度（速度向量的大小）。传统的、不考虑 MoDs 的运动规划通常使用最小化路径长度或转向所需努力的成本函数。在这种情况下，传统的路径长度成本项 cd 和方向成本项 cq 分别为：
- $c_d(\varsigma_1^a) = \sum_{i=1}^{a} ||x_i - x_{i-1}||$
- $c_q(\varsigma_1^a) = \sum_{i=1}^{a} |(1 - |q_i \cdot q_{i-1}|)|$
其中，xi 和 qi 分别是与轨迹点 σ(i) 相关的位置向量和四元数。第一个求和项中的 ||xi - xi - 1|| 是欧几里得距离，四元数点积 qi · qj 得到两个单位四元数之间夹角的余弦值，因此 |(1 - |qi · qi - 1|)| 被称为四元数距离，它倾向于选择转弯较少的路径。

对于考虑 MoDs 的规划，除了 cd 和 cq 成本组件外，我们还引入了一个额外的组件 cc，用于利用 MoD 信息对轨迹进行惩罚。从起始节点到搜索树中某个节点的总成本可以表示为：
$c_{tot}(\varsigma_1^a) = w_d c_d(\varsigma_1^a) + w_q c_q(\varsigma_1^a) + w_c c_c(\varsigma_1^a)$
其中，wd、wq 和 wc 分别是欧几里得距离、四元数距离和 MoD 成本组件的权重参数。如果 wd 和 wq 设为 0，则忽略欧几里得和四元数距离度量，生成的轨迹将严格顺应（或逆）人流方向（取决于 cc 的具体定义）；如果 wc 设为 0，则规划器的行为与传统的、不考虑 MoDs 的 RRT* 相同。

下面我们介绍两种用于实现不同机器人行为的 cc 成本函数。

• Down - The - CLiFF 成本（DTC） ：使用 CLiFF 地图作为 MoD 的底层表示，我们可以定义一个考虑机器人速度与 CLiFF 地图中半包裹正态分布（SWNDs）偏差的 cc 成本函数，即 DTC 成本。

设 Vi 是与轨迹点相关的包含航向和速度的向量。假设在与轨迹点 σi 对应的 CLiFF 地图位置有 Ji 个 SWNDs，每个 SWND 的均值为 μj，协方差为 Σj。与该轨迹点相关的马氏距离为：
$D_i = \sum_{j=1}^{J_i} \sqrt{(V_i - \mu_j)^T \Sigma_j^{-1} (V_i - \mu_j)}$
马氏距离反映了速度实例与底层分布的偏离程度，偏离越大，马氏距离越高。

轨迹的 DTC 成本是所有轨迹点的马氏距离的加权和：
$c_c(\varsigma_1^a) = \sum_{i=1}^{a} (p_i q_i D_i)$
其中，pi 和 qi 分别是与轨迹点 σi 对应的 CLiFF 地图位置的观测比率和运动比率。这种 pq 加权方式使得运动较少或观测较少的位置成本较低，因此使用该成本函数的规划器会生成沿着运动较少（由于 q 避免拥堵）和观测较少（由于 p 具有探索性）区域的轨迹，同时尝试匹配底层人流的速度和方向（由于马氏距离顺应人流）。

我们可以通过调整公式来改变机器人的行为。例如：
- $c_c(\varsigma_1^a) = \sum_{i=1}^{a} (q_i D_i)$：假设成本与位置的观测时间无关（即观测比率 p = 1），使用该成本函数的机器人行为可以描述为自信的，因为它假设对人流有完美的了解。
- $c_c(\varsigma_1^a) = \sum_{i=1}^{a} (\frac{q_i}{p_i} D_i)$：这种情况下，机器人会更倾向于观测较多的区域，表现出谨慎的行为。此外，选择合适的 Di 替换项可以帮助规划器生成逆人流的轨迹。

• 上游成本 ：另一种用于 Eq. (5.3) 中 MoD 成本 cc 的替代方案是 Palmieri 等人提出的扩展上游准则。该成本是 Ko 等人提出的上游准则的扩展，为了简单起见，我们将上游成本和扩展上游准则（EUC）互换使用。与 Eq. (5.4) 类似，与轨迹点 σi 相关的扩展上游准则可以表示为：
  $U_i = \sum_{j=1}^{J_i} (||\overrightarrow{\mu_j}|| - \langle\overrightarrow{\mu_j}, \hat{x}_i\rangle)$
  其中，μi 是与轨迹点 σi 相关的位置向量，$\hat{x}_i$ 是轨迹点处轨迹方向的单位切向量。注意，Eq. (5.8) 中使用的 CLiFF 地图均值已转换为笛卡尔坐标。

轨迹的上游成本可以表示为：
$c_c(\varsigma_1^a) = \sum_{i=1}^{a} U_i$

下面是 RRT* 算法的流程 mermaid 图：

graph TD;
    A[开始] --> B[初始化树 τ 包含起始状态 xstart];
    B --> C[设置 c(xstart) = 0, t = 1];
    C --> D{是否 t ≤ T};
    D -- 是 --> E[随机采样 xrand];
    E --> F[在树中找到最近的 xnear];
    F --> G[尝试连接 xnear 和 xrand 得到 σ 和 xnew];
    G --> H{σ 是否在障碍物空间 Xobs 中};
    H -- 是 --> D;
    H -- 否 --> I[将 xnew 添加到树 τ 中];
    I --> J[添加边 (xnear, xnew, σ) 到树 τ 中];
    J --> K[计算 cost(xnew) = cost(xnear) + cost(σ)];
    K --> L[对树进行重连操作];
    L --> M{xnew 是否在目标区域 Xgoal 中};
    M -- 是 --> N[记录解决方案];
    M -- 否 --> O[t = t + 1];
    O --> D;
    D -- 否 --> P{是否找到解决方案};
    P -- 是 --> Q[返回解决方案];
    P -- 否 --> R[返回失败];

我们可以用表格总结不同成本函数的特点：
| 成本函数 | 特点 |
| ---- | ---- |
| 传统 cd 和 cq | 最小化路径长度和转向努力，不考虑 MoDs |
| DTC 成本 | 考虑运动和观测比率，实现避免拥堵和探索功能，可调整行为 |
| 上游成本 | 基于扩展上游准则，利用 CLiFF 地图均值信息 |

4. 信息采样函数的引入

为了充分发挥基于采样的运动规划器的效用，我们引入信息采样函数。信息采样函数在采样过程中能够结合运动描述符（MoDs）的信息，使得采样更有针对性，从而提高规划的效率和质量。

在 RRT* 算法中，传统的采样方式是随机采样，这种方式虽然具有概率完备性，但在某些情况下可能会导致采样效率低下。通过引入信息采样函数，我们可以根据 MoDs 提供的信息，如人流方向、拥堵程度等，有目的地选择采样点。

例如，在一个存在人流的环境中，如果我们知道某个区域的人流方向和强度，信息采样函数可以优先在人流方向一致且强度较低的区域进行采样，这样可以减少不必要的采样，更快地找到符合要求的路径。

具体实现时，信息采样函数可以根据 CLiFF 地图中的信息进行设计。CLiFF 地图中包含了每个区域的运动信息和观测信息，我们可以根据这些信息计算每个区域的采样优先级。例如，对于运动强度较低且观测次数较少的区域，赋予较高的采样优先级，这样规划器可以更多地探索这些区域，找到更优的路径。

5. 考虑 MoDs 的规划器评估

在这部分，我们将评估使用上述成本函数的规划器，并与不考虑 MoDs 的规划器进行比较，讨论它们的各种定性和定量优势。

5.1 评估指标

为了评估规划器的性能，我们可以使用以下几个指标：
- 路径长度 ：规划得到的路径的总长度，较短的路径通常意味着更高效的运动。
- 规划时间 ：规划器找到路径所需的时间，较短的规划时间表示规划效率更高。
- 路径质量 ：可以通过路径的平滑度、转弯次数等指标来衡量，高质量的路径可以使机器人更稳定地运动。
- 符合期望行为的程度 ：例如，路径是否顺应人流、是否避免了拥堵区域等。

5.2 评估实验

我们进行了一系列实验，比较了两种考虑 MoDs 的规划器（DTC - RRT 和 CLiFF - RRT ）和传统的不考虑 MoDs 的 RRT* 规划器的性能。

实验设置如下：
- 环境 ：模拟一个包含人员流动和障碍物的室内环境，使用 CLiFF 地图来表示环境中的运动信息。
- 任务 ：机器人需要从起始位置移动到目标位置。

实验结果如下表所示：
| 规划器 | 路径长度 | 规划时间 | 路径质量 | 符合期望行为程度 |
| ---- | ---- | ---- | ---- | ---- |
| 传统 RRT | 较长 | 较长 | 一般 | 低 |
| DTC - RRT | 较短 | 较短 | 较高 | 高 |
| CLiFF - RRT* | 较短 | 较短 | 较高 | 高 |

从实验结果可以看出，考虑 MoDs 的规划器在路径长度、规划时间、路径质量和符合期望行为程度等方面都表现出明显的优势。DTC - RRT 和 CLiFF - RRT 规划器能够生成更短的路径，规划时间更短，路径质量更高，并且更符合我们期望的行为，如顺应人流、避免拥堵等。

下面是不同规划器性能比较的 mermaid 柱状图：

graph LR;
    classDef bar fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px;
    A(传统 RRT*):::bar --> B(路径长度);
    A --> C(规划时间);
    A --> D(路径质量);
    A --> E(符合期望行为程度);
    F(DTC - RRT*):::bar --> B;
    F --> C;
    F --> D;
    F --> E;
    G(CLiFF - RRT*):::bar --> B;
    G --> C;
    G --> D;
    G --> E;

6. 总结与展望

通过本文的研究，我们深入探讨了基于运动描述符（MoDs）的机器人运动规划问题。我们介绍了 RRT* 算法及其在运动规划中的应用，讨论了不同的转向函数和成本函数，并引入了信息采样函数来提高规划效率。通过实验评估，我们证明了考虑 MoDs 的规划器在性能上优于传统的不考虑 MoDs 的规划器。

未来的研究方向可以包括：
- 更复杂环境的适应 ：目前的研究主要集中在室内环境，未来可以考虑将算法扩展到更复杂的室外环境，如城市街道、工业园区等。
- 多机器人协同规划 ：研究多个机器人在共享环境中的协同运动规划问题，如何避免机器人之间的碰撞和冲突，提高整体效率。
- 实时更新 MoDs 信息 ：在实际应用中，环境的运动信息是动态变化的，如何实时更新 MoDs 信息并及时调整规划路径是一个重要的研究方向。

总之，基于 MoDs 的机器人运动规划是一个具有广阔应用前景的研究领域，未来还有很多问题值得我们深入探索。