40、运动规划的扩展方法与应用

运动规划的扩展方法与应用

1. 沿闭合构形空间（Cclo）步进

在运动规划中，对于每个样本 $\alpha(i)$，首先要找到 $C$ 中距离它最近的点 $q_n$，然后局部规划器会生成一个位于局部切平面内的运动。这个运动是从 $q_n$ 到 $\alpha(i)$ 的向量在切平面上的投影。

1.1 RDT 算法的调整

原本的 RDT 算法可以直接应用，但需要修改第 4 行的停止构形函数，以同时考虑障碍物和定义 $C_{clo}$ 的约束条件。

1.2 局部运动的计算

具体步骤如下：
1. 计算 $v = \alpha(i) - q$，它表示在没有约束的情况下从 $q$ 出发要形成边的方向。
2. 将 $v$ 投影到 $C_{clo}$ 在点 $q$ 处的切平面上，计算局部运动。由于 $C_{clo}$ 通常是非线性的，局部运动产生的点可能并不精确在 $C_{clo}$ 上，因此需要接受一定的数值容差，并采取足够小的步长来维持这个容差。
3. 迭代过程：根据新的点重新计算 $v$，并将其投影到新的切平面上进行移动。如果误差超过阈值，则需要在法向方向上执行运动以回到 $C_{clo}$。当由于切平面的对齐（几乎垂直于 $v$）或遇到障碍物而无法继续前进时，该过程终止，最终得到停止构形 $q_s$。

1.3 切平面的计算

在每次迭代中，切平面的计算基于约束条件 $f_i(q) = 0$ 的微分构形向量 $dq$。若满足以下条件，则 $dq$ 位于约束的切空间内：
$\frac{\partial f_i(q)}{\partial q_1} dq_1 + \