超级会员免费看
基于运动分布（MoDs）的运动规划技术解析
在机器人运动规划领域，利用运动分布（MoDs）可以显著提升规划效率和质量。本文将深入探讨基于MoDs的运动规划，包括成本函数、采样函数以及相关的评估实验。
成本函数差异分析
在运动规划中，成本函数起着关键作用，不同的成本函数会导致机器人表现出不同的行为。这里主要分析上游成本（upstream cost）和DTC成本（Down-The-CLiFF cost）的差异。
上游成本在计算时既不考虑运动和观察比率，也不利用CLiFF-map中的协方差信息。而DTC成本则考虑了CLiFF-map中的协方差信息，这使得机器人的运动方向在一定程度上可以偏离平均方向，只要这种偏离在构建CLiFF-map时被观察到。
以一维分布为例，DTC成本利用了马氏距离（Mahalanobis distance）。马氏距离是指一个点到分布均值的欧几里得距离除以分布的标准差，即该点与均值之间的标准差数量。因此，当分布的标准差增大时，马氏距离会减小。这意味着对于方差较大的分布，即使方向与平均方向不太一致，马氏距离也可能较小。相比之下，上游准则不考虑协方差，因此在使用两个协方差差异较大的CLiFF-map时，生成的轨迹可能看起来相似。
采样函数设计
传统的RRT*算法采用从整个状态空间进行均匀采样的方式，这种方法在复杂环境中效率较低。而基于MoDs的信息，我们可以设计更有效的采样函数，以提高规划时间。
最小信任因子方法
最小信任因子采样方法与DTC成本公式紧密相关。该方法采用从有偏分布中进行拒绝采样的方式，引导RRT*对配置空间进行探索。
采样偏差的设计直接受DTC成本函数的启发，在采样过程中使用运动和观察比率，优先选择探索性和避免拥堵的行为，引导探索向观察较少或运动较少的区域进行，同时尽量匹配已学习到的流动模式。
以下是采样的伪代码：
Result: Newly sampled pose xnew
Generate 3 random numbers r1, r2 and r3 from a uniform distribution;
if r1 < 0.05 then
xnew ← UniformSample(Xgoal);
return xnew;
end
xnew ← UniformSample(Xfree);
if r1 > 0.80 then
return xnew;
end
while True do
q ← cliffmap(xnew).q
p ← cliffmap(xnew).p
if r2 > q then
return xnew;
end
else
if r3 < pq then
Reject the sample;
end
else
xnew.heading ← cliffmap(xnew).heading;
return xnew;
end
end
end
采样策略如下：
1. 以5%的概率从目标区域选择一个状态。
2. 否则，从整个状态空间均匀采样一个状态。
3. 以20%的概率无条件接受该样本，以确保概率完备性。
4. 否则，根据运动和观察比率进行判断：
- 如果运动比率低，则接受样本，因为成本可能较低或该区域运动较少。
- 如果运动比率高但观察比率低，则尝试跟随流动。
- 如果运动和观察比率都高，则拒绝样本，因为成本可能较高。
Dijkstra图方法
Dijkstra图方法旨在使RRT
实现跟随流动的行为，称为CLiFF-RRT
。该方法分两步进行：
1.
SelectMixtures
：选择允许系统从起始点移动到目标点的半包裹混合模型（semi-wrapped mixtures）。首先构建一个图G，每个节点与CLiFF-map中的半包裹高斯混合模型（SWGMM）及其对应位置相关联。为每个地图单元格计算其SWGMM组件与相邻单元格组件之间的边，并为每条边分配一个成本：
- (c(e(n_{G,i}, n_{G,j})) = d(e(n_{G,i}, n_{G,j})) + U(\hat{x}
{n
{G,j}}, \xi_{n_{G,j}}))
- 其中，(d(e(n_{G,i}, n_{G,j})))是节点之间的欧几里得距离平方，(\hat{x}
{n
{G,j}})是单位向量，(\xi_{n_{G,j}})是与节点(n_{G,j})相关联的SWGMM，(U(\hat{x}
{n
{G,j}}, \xi_{n_{G,j}}))是扩展的上游准则（EUC）。
- 通过Dijkstra搜索生成一个从起始点到目标点的CLiFF-map元组路径(P_D)，并将其传递给采样单元。
2.
CLiFFSampling
：根据参数(\alpha \in [0, 1])，从整个状态空间的均匀分布或(P_D)中的CLiFF-map分布中抽取样本：
- (x_{rand} \sim \sum_{i=0}^{N_{PD}-1} \sum_{j=1}^{J_i} \pi_{j}^{N_{SW}} \xi_i(\mu_{j}^{\xi_i}, \Sigma_{j}^{\xi_i}))
评估实验
为了验证基于MoDs的运动规划方法的有效性，进行了两组实验，分别评估CLiFF-RRT 和DTC-RRT 的性能。
CLiFF-RRT*评估
实验目的是评估CLiFF-RRT 算法相对于常规RRT、RRT 和All-Mixtures-RRT*的性能。所有方法使用相同的成本函数，即MoD成本（EUC加上欧几里得和四元数距离成本），并使用POSQ转向函数。
实验在不同复杂度的环境中进行，包括L、P、迷宫和交叉场景。环境通过离线学习的行人交通运动模型生成，CLiFF-map的网格单元大小设置为1m。
评估指标包括规划时间、轨迹长度、最终成本、粗糙度以及在规划时间限制内找到轨迹的百分比。
实验结果表明：
1. CLiFF-RRT
能够更快地找到初始解决方案，并且平均收敛到较低成本的解决方案的速度也比基线方法快。这是因为它利用了环境动态的先验知识，减少了对整个状态空间的探索。
2. CLiFF-RRT
找到的解决方案成本更低。通过Dijkstra搜索选择的混合模型引导树向状态空间中轨迹长度和控制努力之间能达到较好平衡的区域移动。
3. CLiFF-RRT*的采样策略生成的轨迹更平滑。离线学习的混合模型使树的扩展连接具有较少的速度不连续性，而均匀采样则缺乏对特定区域常见运动的先验知识。
| 规划器 | MoD成本组件（(c_c)） | 采样函数 |
|---|---|---|
| RRT | EUC (Eq.(5.5)) | 均匀采样 |
| RRT* | EUC | 均匀采样 |
| All-Mixtures-RRT* | EUC | 所有混合模型 |
| CLiFF-RRT* | EUC | Dijkstra图 |
以下是CLiFF-RRT
在不同环境中的实验结果：
| 规划器 | 环境 | (C_s) | (T_s) (s) | (l_p) (m) | (R) | 60s内解决率 (%) |
| ---- | ---- | ---- | ---- | ---- | ---- | ---- |
| CLiFF-RRT
| L | (111.42 \pm 5.08) | (5.30 \pm 8.08) | (33.59 \pm 0.78) | (0.00007 \pm 0.00004) | 100 |
| All-Mixtures-RRT
| L | (130.89 \pm 32.92) | (14.97 \pm 17.65) | (35.48 \pm 2.31) | (0.00009 \pm 0.0001) | 48 |
| RRT | L | (784.82 \pm 618.5) | (15.83 \pm 17.16) | (40.59 \pm 7.36) | (0.0023 \pm 0.0048) | 36 |
| RRT
| L | (212.26 \pm 193.4) | (28.15 \pm 14.91) | (37.31 \pm 3.35) | (0.00043 \pm 0.0013) | 34 |
| CLiFF-RRT
| 迷宫 | (151.51 \pm 12.62) | (31.13 \pm 32.66) | (123.23 \pm 1.02) | (0.000022 \pm 0.00004) | 90 |
| All-Mixtures-RRT
| 迷宫 | (180.52 \pm 54.83) | (36.74 \pm 38.12) | (126.43 \pm 3.83) | (0.000038 \pm 0.000025) | 10 |
| RRT | 迷宫 | (1260.54 \pm 1278.96) | (41.74 \pm 17.87) | (169.94 \pm 25.93) | (0.00058 \pm 0.00064) | 10 |
| RRT
| 迷宫 | (560.78 \pm 397.98) | (64.29 \pm 35.93) | (176.68 \pm 43.67) | (0.00053 \pm 0.00062) | 14 |
| CLiFF-RRT
| P | (1125.16 \pm 659.0) | (11.42 \pm 14.11) | (59.13 \pm 4.48) | (0.000098 \pm 0.00025) | 56 |
| All-Mixtures-RRT
| P | (1688.09 \pm 27.48) | (12.38 \pm 7.68) | (103.54 \pm 31.1) | (0.0007 \pm 0.0024) | 2 |
| RRT | P | (2532.96 \pm 798.46) | (21.16 \pm 19.8) | (82.87 \pm 23.04) | (0.0007 \pm 0.0014) | 16 |
| RRT
| P | (1128.06 \pm 454.64) | (25.43 \pm 18.90) | (122.61 \pm 34.72) | (0.00057 \pm 0.00007) | 16 |
| CLiFF-RRT
| 交叉 | (182.52 \pm 28.77) | (24.96 \pm 17.29) | (34.71 \pm 1.00) | (0.013 \pm 0.019) | 76 |
| All-Mixtures-RRT
| 交叉 | (307.67 \pm 56.25) | (29.4 \pm 18.70) | (51.77 \pm 20.76) | (0.013 \pm 0.0104) | 14 |
| RRT | 交叉 | (722.15 \pm 373.35) | (27.78 \pm 25.55) | (41.97 \pm 10.14) | (0.0196 \pm 0.012) | 10 |
| RRT* | 交叉 | (298.75 \pm 69.42) | (27.16 \pm 15.77) | (47.61 \pm 16.39) | (0.087 \pm 0.0069) | 24 |
通过以上实验和分析，我们可以看到基于MoDs的运动规划方法在提高规划效率和质量方面具有显著优势。后续我们将继续探讨DTC-RRT*的评估结果，进一步了解不同成本和采样函数组合的性能表现。
flowchart TD
classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px
classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px
classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px
A([开始]):::startend --> B{是否 r1 < 0.05}:::decision
B -- 是 --> C(从目标区域采样 xnew):::process
C --> D([返回 xnew]):::startend
B -- 否 --> E(从自由空间均匀采样 xnew):::process
E --> F{是否 r1 > 0.80}:::decision
F -- 是 --> D
F -- 否 --> G(获取 q 和 p):::process
G --> H{是否 r2 > q}:::decision
H -- 是 --> D
H -- 否 --> I{是否 r3 < pq}:::decision
I -- 是 --> J(拒绝样本):::process
J --> E
I -- 否 --> K(设置 xnew 航向):::process
K --> D
这个流程图展示了最小信任因子方法的采样过程，从开始到最终返回新采样的姿态，包含了多个条件判断和处理步骤，清晰地呈现了算法的逻辑。
基于运动分布（MoDs）的运动规划技术解析
DTC - RRT*评估
前面我们对比了使用相同成本函数但不同采样函数的RRT 规划器，接下来将评估使用不同成本和采样函数的RRT 规划器，重点关注DTC - RRT*的性能。
评估设置
在定性实验中，我们将DTC - RRT 生成的轨迹与Upstream - RRT 生成的轨迹进行比较。在定量实验中，我们测量规划时间、距离成本（(c_d)）、上游成本（(U)）和DTC成本（(c_{tot})），并对无信息RRT 、Upstream - RRT 、无偏DTC - RRT 和使用最小信任因子采样函数的DTC - RRT 进行比较。所有这些量都是在60秒的规划时间限制内的最新解决方案。
评估使用的环境如图所示，在环境的占用网格上覆盖了一个CLiFF - map网格，标记了起始和目标位置。我们构建了各种CLiFF - map，在地图中央障碍物的两侧设置不同的参数，以观察规划器在从起始点到目标点时会选择哪一侧。通过指定矩形区域的速度、航向、样本数量等参数生成虚拟速度观测值。
构建的八个地图如下：
1.
DiffDirections
：有两个平均方向相反的流。
2.
DiffSpeeds
：有两个平均速度不同的流。
3.
DiffQ
：左侧有流，右侧无流，两个区域运动比率不同。
4.
DiffVarSpeed
：有两个速度方差不同的流。
5.
DiffVarDir
：有两个方向方差不同的流。
6.
DiffIntensity
：两个区域运动比率不同的流。
7.
DiffPQ1
：两个流的运动和观察比率不同，左侧平均(pq)值约为0.17，右侧约为0.21。
8.
DiffPQ2
：两个流的运动和观察比率不同，左侧平均(pq)值约为0.32，右侧约为0.11。
| 地图ID | 速度（左） | 航向（左） | 速度（右） | 航向（右） | 样本（左） | 样本（右） |
|---|---|---|---|---|---|---|
| DiffDirections | (1.0 \pm 0.45) | (-90 \pm 3.35) | (1.0 \pm 0.45) | (90 \pm 3.35) | 10k | 10k |
| DiffSpeeds | (1.0 \pm 0.45) | (-90 \pm 3.35) | (0.5 \pm 0.45) | (-90 \pm 3.35) | 10k | 10k |
| DiffQ | (1.0 \pm 0.45) | (-90 \pm 3.35) | – | – | 10k | 0 |
| DiffVarSpeed | (1.0 \pm 0.32) | (-90 \pm 3.35) | (1.0 \pm 0.78) | (-90 \pm 3.35) | 10k | 10k |
| DiffVarDir | (1.0 \pm 0.45) | (-90 \pm 3.35) | (1.0 \pm 0.45) | (-90 \pm 4.75) | 10k | 10k |
| DiffIntensity | (1.0 \pm 0.45) | (-90 \pm 3.35) | (1.0 \pm 0.45) | (-90 \pm 3.35) | 20k | 10k |
| DiffPQ1 - 2 | (1.0 \pm 0.45) | (-90 \pm 3.35) | (1.0 \pm 0.45) | (-90 \pm 3.35) | 20k | 20k |
结果和观察
每个算法在每个地图上运行25次，成功定义为在60秒时间限制内找到解决方案的试验百分比。结果总结如下：
1. 无偏DTC - RRT
相比RRT
和Upstream - RRT
，改进解决方案所需的时间要长得多。
2. 有偏采样的DTC - RRT
最多与无偏版本花费相同的时间，并且其中位规划时间与RRT
和Upstream - RRT
的规划时间非常接近。
3. DTC - RRT
在所有三种成本方面都比基线方法产生成本更低的解决方案。
4. 有采样偏差的DTC - RRT
比Upstream - RRT
产生的解决方案上游成本更低。当马氏距离最小时，车辆在每个轨迹点的速度和航向更接近底层流的均值，从而自动最小化上游成本。由于DTC - RRT
使用有偏采样，它比无偏的DTC - RRT*更好地最小化了马氏距离，因此更好地最小化了上游准则。
| 规划器 | 地图 | 规划时间 | DTC成本 | 上游成本（(U)） | 距离成本（(c_d)） | 成功率（%） |
|---|---|---|---|---|---|---|
| 无信息RRT* | DiffDirections | (9.92 \pm 12.74) | (7793.50 \pm 4064.84) | (53.33 \pm 41.2) | (33.48 \pm 1.89) | 90 |
| 无信息RRT* | DiffSpeeds | (7.50 \pm 10.62) | (7782.19 \pm 3320.10) | (4.56 \pm 2.62) | (33.65 \pm 1.63) | - |
| 无信息RRT* | DiffQ | (8.60 \pm 8.35) | (4422.69 \pm 3881.85) | (4.44 \pm 3.45) | (33.57 \pm 1.30) | - |
| 无信息RRT* | DiffVarSpeed | (7.24 \pm 8.64) | (7768.74 \pm 6740.81) | (3.83 \pm 2.65) | (33.30 \pm 1.17) | - |
| 无信息RRT* | DiffVarDir | (6.66 \pm 10.10) | (7470.19 \pm 4566.96) | (5.13 \pm 4.30) | (33.69 \pm 1.54) | - |
| 无信息RRT* | DiffIntensity | (7.65 \pm 8.81) | (5122.64 \pm 3820.58) | (3.90 \pm 2.94) | (33.34 \pm 1.55) | - |
| 无信息RRT* | DiffPQ1 | (6.86 \pm 10.21) | (132.14 \pm 437.01) | (3.67 \pm 2.23) | (33.47 \pm 1.65) | - |
| 无信息RRT* | DiffPQ2 | (8.50 \pm 9.43) | (804.95 \pm 2232.86) | (9.81 \pm 17.27) | (34.33 \pm 1.83) | - |
| Upstream - RRT* | DiffDirections | (10.74 \pm 12.44) | (7408.48 \pm 4157.70) | (4.43 \pm 2.40) | (34.83 \pm 1.87) | 89.5 |
| Upstream - RRT* | DiffSpeeds | (7.80 \pm 7.95) | (6904.24 \pm 5197.57) | (1.46 \pm 2.62) | (34.95 \pm 1.87) | - |
| Upstream - RRT* | DiffQ | (4.03 \pm 5.24) | (893.27 \pm 1741.86) | (0.68 \pm 2.38) | (34.60 \pm 1.83) | - |
| Upstream - RRT* | DiffVarSpeed | (8.60 \pm 7.40) | (4898.31 \pm 7498.97) | (2.41 \pm 1.54) | (34.72 \pm 1.60) | - |
| Upstream - RRT* | DiffVarDir | (9.80 \pm 11.15) | (4301.20 \pm 3682.80) | (2.41 \pm 1.52) | (35.04 \pm 1.78) | - |
| Upstream - RRT* | DiffIntensity | (8.51 \pm 7.57) | (3082.43 \pm 2618.13) | (2.50 \pm 1.36) | (34.14 \pm 1.42) | - |
| Upstream - RRT* | DiffPQ1 | (5.77 \pm 5.36) | (124.13 \pm 416.92) | (2.34 \pm 1.48) | (34.54 \pm 1.66) | - |
| Upstream - RRT* | DiffPQ2 | (8.05 \pm 10.18) | (1156.52 \pm 1640.28) | (2.12 \pm 1.85) | (34.46 \pm 1.89) | - |
| DTC - RRT*（均匀采样） | DiffDirections | (21.86 \pm 14.06) | (49.14 \pm 3.64) | (5.34 \pm 3.13) | (38.22 \pm 2.75) | 92.5 |
| DTC - RRT*（均匀采样） | DiffSpeeds | (18.80 \pm 16.19) | (47.64 \pm 2.28) | (3.81 \pm 3.77) | (36.27 \pm 1.94) | - |
| DTC - RRT*（均匀采样） | DiffQ | (8.70 \pm 8.13) | (35.47 \pm 2.39) | (0.00 \pm 0.00) | (35.47 \pm 2.39) | - |
| DTC - RRT*（均匀采样） | DiffVarSpeed | (22.56 \pm 13.36) | (42.59 \pm 1.69) | (1.87 \pm 1.29) | (36.76 \pm 1.44) | - |
| DTC - RRT*（均匀采样） | DiffVarDir | (19.30 \pm 15.85) | (41.67 \pm 2.81) | (2.80 \pm 2.11) | (36.33 \pm 1.97) | - |
| DTC - RRT*（均匀采样） | DiffIntensity | (20.17 \pm 14.59) | (40.77 \pm 3.13) | (2.50 \pm 2.13) | (36.60 \pm 2.44) | - |
| DTC - RRT*（均匀采样） | DiffPQ1 | (7.95 \pm 9.79) | (36.91 \pm 1.81) | (2.62 \pm 2.21) | (34.35 \pm 1.92) | - |
| DTC - RRT*（均匀采样） | DiffPQ2 | (13.27 \pm 11.54) | (36.53 \pm 1.90) | (2.39 \pm 2.07) | (34.68 \pm 1.83) | - |
| DTC - RRT* | DiffDirections | (13.02 \pm 14.47) | (44.99 \pm 3.20) | (3.81 \pm 2.54) | (36.87 \pm 2.15) | 87.5 |
| DTC - RRT* | DiffSpeeds | (9.88 \pm 11.80) | (41.29 \pm 2.27) | (2.69 \pm 2.07) | (35.09 \pm 2.08) | - |
| DTC - RRT* | DiffQ | (5.83 \pm 5.42) | (34.27 \pm 1.55) | (0.03 \pm 1.53) | (34.16 \pm 1.61) | - |
| DTC - RRT* | DiffVarSpeed | (21.22 \pm 14.38) | (38.58 \pm 1.48) | (0.97 \pm 1.72) | (34.76 \pm 1.93) | - |
| DTC - RRT* | DiffVarDir | (14.61 \pm 14.97) | (37.55 \pm 1.13) | (1.68 \pm 1.27) | (34.03 \pm 1.18) | - |
| DTC - RRT* | DiffIntensity | (14.79 \pm 15.04) | (36.85 \pm 1.84) | (1.84 \pm 1.82) | (33.80 \pm 1.95) | - |
| DTC - RRT* | DiffPQ1 | (13.62 \pm 14.61) | (34.84 \pm 1.05) | (1.59 \pm 1.09) | (32.95 \pm 1.14) | - |
| DTC - RRT* | DiffPQ2 | (15.84 \pm 16.37) | (34.70 \pm 1.29) | (1.65 \pm 1.81) | (33.39 \pm 1.30) | - |
定性结果以DTC - RRT
和Upstream - RRT
规划器获得的解决方案的热图形式呈现。热图通过累积60秒后的最新解决方案并计算落在网格地图每个单元格上的轨迹点数来构建。从热图中可以得出以下定性观察结果：
- DTC - RRT
比Upstream - RRT
实现了更好的流一致性。它更喜欢车辆速度和方向与底层分布匹配的区域。由于Dubins车辆的恒定速度为1米/秒，DTC - RRT
选择左侧速度更接近1米/秒的轨迹。而Upstream - RRT
的成本函数在机器人航向与CLiFF - map的平均航向一致时不考虑平均速度。
flowchart TD
classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px
classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px
classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px
A([开始评估]):::startend --> B(运行各规划器25次):::process
B --> C(计算规划时间、成本等指标):::process
C --> D{指标比较}:::decision
D -- DTC - RRT*成本低 --> E(记录DTC - RRT*优势):::process
D -- 其他规划器优势 --> F(记录其他规划器情况):::process
E --> G(生成热图):::process
F --> G
G --> H(分析热图结果):::process
H --> I([结束评估]):::startend
这个流程图展示了DTC - RRT*评估的整体过程，从开始评估到最终得出结论，涵盖了运行规划器、计算指标、比较指标、生成热图和分析结果等步骤。
综上所述，基于运动分布（MoDs）的运动规划方法，无论是CLiFF - RRT 还是DTC - RRT ，在提高规划效率、降低解决方案成本和生成平滑轨迹方面都表现出显著的优势。不同的成本函数和采样函数组合各有特点，在实际应用中可以根据具体需求进行选择。
扫一扫
1895
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
