47、运动规划中的导航函数与动力学约束

运动规划中的导航函数与动力学约束

1. 最优导航函数

在运动规划中，之前章节开发的向量场能产生可行轨迹，但不一定是最优轨迹，除非初始状态和目标状态处于同一凸 $n$ 维胞腔中。当 $X = R^2$ 时，可以使用连续版本的 Dijkstra 算法，从而得到基于欧几里得最短路径到目标点 $x_G$ 的精确代价到目标函数，该函数可作为导航函数，通过局部最速下降法定义反馈规划。

假设 $X$ 由一个简单多边形（无孔洞）界定，只允许使用归一化向量场，代价泛函为沿状态轨迹的欧几里得距离。对于最优路径规划，需使用 $X = cl(C_{free})$，并假设 $C_{free}$ 和 $cl(C_{free})$ 具有相同的连通性。

对于任意点 $x \in X$，可见多边形 $V(x)$ 指从 $x$ 可见的所有点的集合。若点 $x’$ 到 $x$ 的线段包含在 $X$ 中，则 $x’$ 属于 $V(x)$，此时从 $x’$ 到 $x$ 的代价到目标值就是它们之间的欧几里得距离。因此，在 $V(x_G)$ 上可立即定义最优导航函数为：
[
\varphi(x) = |x - x_G|
]

为计算 $X \setminus V(x_G)$ 中各点的最优代价到目标值，需引入路点的概念。对于 $V(x)$ 边界上的线段，部分边在 $X$ 的边界上，部分穿过 $X$ 的内部，将穿过内部的线段中离 $x$ 较近的顶点称为路点。$V(x_G)$ 的路点是某些最优路径必须经过的地方。

计算最优导航函数的算法步骤如下：
1. 令 $Z_i$ 表示算法第 $i$ 次迭代中已定义 $\varphi$ 的点集。第一次迭代时，$Z_1