Field D* 路径规划

D*算法

D* 算法是用于路径规划的增量式搜索算法，旨在解决在环境中有障碍物动态变化时的路径规划问题。D* 算法的主要思想是在每次环境状态变化时，只更新受影响的部分路径，而不必重新规划整个路径。

D* 算法的核心概念包括：

1. 代价函数：通常包括实际代价函数 g 和备用代价函数 rhs。g 表示从起始节点到当前节点的实际代价，而 rhs 表示备用的最小代价。

2. 启发式函数：用于估计从当前节点到目标节点的代价。常用的启发式函数包括欧几里德距离、曼哈顿距离等。

3. 更新策略：D* 算法中通常使用增量更新的方式，根据环境的变化，仅更新受到影响的部分路径，以减少重新规划的计算量。

4. 反向搜索：D* 算法通常是从目标节点向起始节点进行反向搜索，通过更新路径的代价来逐步找到最优路径。

Ｆｉｅｌｄ Ｄ＊

Ｆｉｅｌｄ Ｄ＊ 基于启发式搜索，从目标点开始依次搜索最小的估值函数ｆ（ｓ）来确定需要扩展的节点：

式中：ｆ（ｓ）表示从起点经节点ｓ到目标点路径的代价估计值，ｇ（ｓ）表示当前节点到目标点的实际代价，启发函数ｈ（ｓ）表示当前节点到起始点代价估计值．传统的基于栅格规划中，点ｓ到其邻节点只能是直线轨迹（见图１），代价值为

式中：ｎｂｒｓ（ｓ）表示节点ｓ 邻节点的集合，ｃ（ｓ，ｓｉ）为节点ｓ与ｓｉ 的路径代之间边的代价，ｇ（ｓｉ）为节点ｓｉ价值．而 Ｆｉｅｌｄ Ｄ＊ 允许节点ｓ可以直线移动到其栅格的任意边界点ｓｙ．插值点ｓｙ 的路径代价ｇ（ｓｙ）可以由其所在边的２个端节点的代价ｇ（ｓ１）与ｇ（ｓ２）由式（３）线性计算近似得到

式中：ｙ 是ｓ１ 到ｓｙ 的距离，取决于当前栅格的代价及ｇ（ｓ１）、ｇ（ｓ２）代价之差［５］．此时ｇ（ｓ）可由式（４）求解到邻边上的插值点或者端点的最小代价得到，ｂｏｕｎｄａｒｙ（ｓ）是｛ｓ１ｓ→２，ｓ２ｓ→３，ｓ３ｓ→４，ｓ４ｓ→５，ｓ５ｓ→６，ｓ６ｓ→７，ｓ７ｓ→８，ｓ８ｓ→１｝邻边上点的集合．

math.hypot

math.hypot(x, y) 函数返回欧几里德范数 sqrt(x*x + y*y)。在这个函数中，给定两个数字 x 和 y，它将返回这两个数字构成的直角三角形的斜边长（即两个数字构成的二维平面上的点到原点的距离）。

init_table

    def init_table(self):
        for i in range(1, self.Env.x_range - 1):
            for j in range(1, self.Env.y_range - 1):    # 从（1，1）到（49， 29）？
                s_neighbor = self.get_neighbor_pure((i, j))
                s_neighbor.append(s_neighbor[0])    # 为啥还要再加一个？
                for k in range(8):
                    self.cknbr[((i, j), s_neighbor[k])] = s_neighbor[k + 1]
                s_neighbor = list(reversed(s_neighbor))
                for k in range(8):
                    self.ccknbr[((i, j), s_neighbor[k])] = s_neighbor[k + 1]

这段代码看起来是在初始化一个表格，用于存储节点及其邻居节点之间的关系。具体来说，对于给定范围内的节点 (i, j)，首先获取其纯粹邻居节点，然后根据这些邻居节点的顺序构建节点和邻居节点之间的关系，并存储在 cknbr 和 ccknbr 中。

在循环中，首先获取节点 (i, j) 的纯粹邻居节点列表 s_neighbor，然后将这个列表的第一个元素再添加一次，这样做是为了循环连接邻居节点，使得最后一个邻居节点指向第一个邻居节点，形成一个闭环。

接着，根据这些邻居节点的顺序，依次构建节点和邻居节点之间的关系，并存储在 cknbr 和 ccknbr 中，以便后续路径规划算法使用这些信息。

在代码片段中，cknbr 和 ccknbr 存储了节点和邻居节点之间的关系，可能存在以下区别：

1. cknbr：可能代表“Clockwise Neighbor”（顺时针邻居）的缩写。这个表可能存储了节点的邻居节点按照顺时针的顺序排列的信息。这种排序方式可能在某些算法中很重要，比如在路径规划算法中，按顺时针或逆时针的顺序访问邻居节点可能会影响路径选择或优化。

2. ccknbr：可能代表“Counter-Clockwise Neighbor”（逆时针邻居）的缩写。这个表可能存储了节点的邻居节点按照逆时针的顺序排列的信息。同样地，在某些算法中，按逆时针的顺序访问邻居节点也可能具有特定的用途。

初始化cost

        for i in range(self.Env.x_range):
            for j in range(self.Env.y_range):
                self.rhs[(i, j)] = float("inf")
                self.g[(i, j)] = float("inf")
                self.bptr[(i, j)] = (i, j)

这段代码看起来是在初始化三个字典 rhs, g, 和 bptr。这些字典的作用可能如下：

1. rhs: 可能代表"Right Hand Side"的缩写，通常在路径规划算法中用于存储节点的备用代价值。在 D* 算法中，rhs 值表示从起始节点到节点 (i, j) 的最小代价。

2. g: 可能代表"Cost to Go"的缩写，通常在路径规划中表示从起始节点到节点 (i, j) 的实际代价值。在 D* 算法中，g 值表示从起始节点到节点 (i, j) 的当前最小代价。

3. bptr: 可能代表"Back Pointer"的缩写，通常在路径规划中用于存储节点 (i, j) 的前驱节点。在 D* 算法中，bptr 可能用于回溯路径，确定路径规划的具体路径。

通过对这些字典进行初始化，将 rhs 和 g 设置为无穷大，通常表示未知或不可达，而将 bptr 设置为节点自身，可能表示每个节点的初始前驱节点为其自身。

经典二维路径规划的局限性

考虑一个在户外环境中导航的地面车辆。我们可以将这个环境表示为一个二维遍历网格，其中单元被遍历的代价，反映在环境的各个区域导航的困难。有了这样的网格，我们可以很容易地提取一个用于路径规划的图：为每个单元中心分配一个节点，用边将节点连接到每个相邻的单元中心（节点）。每条边的代价是它所连接的两个单元格的遍历代价和该边的长度的组合。图1(a)显示了均匀二维网格中一个单元的节点和边缘提取过程。

然后，我们可以在这个图上进行规划，以生成从机器人的初始位置到一个期望的目标位置的路径，使用前面提到的任何一种算法。不幸的是，使用这个图生成的路径仅限于π4增量的标题。这意味着最终的解路径在路径代价上可能是次优的，涉及不必要的转弯，或者两者兼有。

https://github.com/MuneebBaderoen/AdaptiveFieldDStar/tree/master

荣誉项目。使用自适应网格细化进行寻路。Field D*算法在二维和2.5D三角形网格中的自适应细化实现。我们的目标是执行局部网格细化，在计算路径所经过的区域中生成精细的三角形网格，并迭代地改进使用此动态方案找到的路径的近似值。我们将研究当前寻路算法用于指导字段D*中节点扩展的启发式方法。用C++实现。