96、微分约束下的采样规划

微分约束下的采样规划

1. 高效寻找最近点

在微分约束下的 RDT（Rapidly-exploring Dense Trees）中，高效寻找最近点的问题再次出现。由于图 $G$ 中的边通常是弯曲的，这使得简单的点 - 线段距离测试无法使用。而且，状态轨迹的精确表示通常是未知的，只能通过系统模拟器进行数值近似。

为了解决这些问题，最好使用在扫描带中确定最近点的近似方法。如果应用的运动原语产生的状态轨迹在自由空间 $X_{free}$ 中移动得很远，可以插入中间顶点。当维度足够低（例如小于 20）时，可以使用高效的最近邻算法来抵消维护中间顶点的成本。

操作步骤：

1. 利用系统模拟器对状态轨迹进行数值近似。
2. 采用在扫描带中确定最近点的近似方法。
3. 根据运动原语产生的状态轨迹情况，决定是否插入中间顶点。
4. 若维度低于 20，使用高效最近邻算法。

2. 处理障碍物

当存在障碍物（$X_{obs} \neq \varnothing$）时，在微分约束下会出现两个新的复杂情况：
- 运动原语的使用 ：如果时间步长 $\Delta t$ 较小，很多情况下在到达障碍物边界之前时间就会耗尽。可以通过使用较大的 $\Delta t$，然后只取轨迹中无违规的部分来缓解这个问题。有时甚至需要提前裁剪轨迹以避免超调。
- 动量的影响 ：如果动量较大，将树尽可能靠近障碍物会增加 RDT 被困的可能性。靠近障碍物的顶点由于具有较大的 Voronoi 区域，会经常