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

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

1. 最优导航函数

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

1.1 基本假设与定义

假设 $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|$

1.2 计算 $X \setminus V(x_G)$ 中各点的最优代价到目标值

对于 $V(x)$ 边界上的线段，有些边在 $X$ 的边界上，有些则穿过 $X$ 的内部。对于穿过内部的线段，离 $x$ 较近的顶点称为路点。$V(x_G)$ 的路点是某些最优路径必须经过的地方。

计算 $X \setminus V(x_G)$ 中各点的最优代价到目标值的算法步骤如下：
1. 令 $Z