44、运动规划与反馈运动规划相关知识解析

运动规划与反馈运动规划相关知识解析

一、波前传播算法

波前传播算法是迪杰斯特拉算法的一种特殊版本，旨在优化到达目标的阶段数。以下是该算法的具体步骤：
1. 初始化 (W_0 = X_G)，并令 (i = 0)。
2. 初始化 (W_{i + 1} = \varnothing)。
3. 对于每一个 (x \in W_i)，分配 (\varphi(x) = i)，并将 (x) 的所有未探索邻居插入到 (W_{i + 1}) 中。
4. 如果 (W_{i + 1} = \varnothing)，则终止算法；否则，令 (i := i + 1)，并返回步骤 2。

该算法能立即为从 (W_0) 中的任何状态在一个阶段内可到达的每个状态分配最优的代价 - 到 - 目标值 1，因为没有其他代价更优。接收代价 1 的状态可组成波前 (W_1)，(W_1) 的未探索邻居被分配代价 2，这也必然是最优的。这个过程从 (i) 到 (i + 1) 归纳重复，直到所有可达状态都被访问到。最终，最优代价 - 到 - 目标值在 (O(n)) 时间内计算完成，其中 (n) 是可达网格状态的数量。对于任何未到达的状态，可分配值 (\varphi(x) = \infty)。

二、最大间隙导航函数

在移动机器人领域，最优运动规划往往会使机器人过于接近障碍物。为解决这个问题，可计算最大间隙导航函数，具体步骤如下：
1. 不将 (W_0) 设为 (X_G)，而是设为所有至少在一个方向上运动受阻的状态集合，即离散化无碰撞空间边界上的状态。
2. 执行波前迭代，使代价从障碍物边界向外以波的形式传播。
3. 随着波前传播，它们大致会在原