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

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

1. 最优导航函数

在二维空间 (X = R^2) 且 (X) 由简单多边形（无孔洞）界定的情况下，可借助连续版的 Dijkstra 算法来计算基于欧几里得最短路径的精确代价到目标函数，此函数可作为导航函数。

• 可见多边形 ：对于任意点 (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)) 中的点，可通过以下算法计算最优代价到目标值：
  1. 初始化：令 (Z_1 = V(x_G))，将 (V(x_G)) 的路点放入队列 (Q)。
  2. 迭代：每次从 (Q) 中移除一个路点 (x)，将 (\varphi) 的定义域扩展到包含从 (x) 可见的所有新点 (V(x) \setminus Z_i)，得到 (Z_{i + 1} = Z_i \cup V(x))。对于 (x’ \in Z_{i + 1} \setminus Z_i)，(\varphi(x’)) 的值定义为：
     [
     \varphi(x’) = \varphi(x) + |x’ - x|
     ]
  3. 更新队列：将 (V(