（11）定义方法CollisionFree，目的是检查从状态 s1 到状态 s2 的路径是否可行，即是否存在障碍物。方法CollisionFree首先注释掉了一段代码，该代码原本的意图是在两个状态之间的中点位置取一个区域，然后检查这个区域是否有障碍物。如果存在障碍物（即区域像素值求和非零），则返回 False 表示路径不可行。
def CollisionFree(self, s1, s2):
'''
检查以s1->s2是否可行，必要时增加s1,s2之间的栅格检查
'''
# half_grid_size = int(self.grid_size / 2)
# row_start = int((s1.pos[0] + s2.pos[0]) * self.grid_size / 2)
# col_start = int((s1.pos[1] + s2.pos[1]) * self.grid_size / 2)
# area = self.map[row_start: row_start +
# self.grid_size, col_start: col_start + self.grid_size]
# if np.sum(area):
# return False
return s1.is_obs == False and s2.is_obs == False（12）定义方法ChebyshevDistance，用于计算二维空间中两点 p1 和 p2 之间的切比雪夫距离（Chebyshev distance）。切比雪夫距离是一种度量两个点之间距离的方法，它等于两点在所有坐标轴上的绝对差值的最大值。
def ChebyshevDistance(self, p1, p2):
return self.grid_size * max(abs(p1[0] - p2[0]), abs(p1[1] - p2[1]))（13）定义方法EuclideanDistance，用于计算的是两点之间的直线距离，即根据勾股定理，两点之间的距离是它们坐标差的平方和的平方根。这个方法首先计算两个点在 x 和 y 坐标上差的平方和，然后取平方根，并乘以每个网格的大小 grid_size 来得到实际的欧几里得距离。
def Cost(self, s1, s2):
if self.CollisionFree(s1, s2):
return self.DiagonalDistance(s1.pos, s2.pos)
else:
return float('inf')（14）定义方法DiagonalDistance，用于计算两个点 p1 和 p2 之间的对角线距离。对角线距离是考虑了网格中可以对角移动的情况，通常用于那些允许斜向移动的路径搜索算法中。
def Cost(self, s1, s2):
if self.CollisionFree(s1, s2):
return self.DiagonalDistance(s1.pos, s2.pos)
else:
return float('inf')（15）定义弧成本方法Cost ，用于计算从状态 s1 到状态 s2 的移动成本。首先，使用 CollisionFree 方法检查从 s1 到 s2 是否存在障碍物，如果路径是可行的（即没有障碍物），则使用 DiagonalDistance 方法计算两点之间的对角线距离作为移动成本。如果路径不可行，即存在障碍物，则返回无穷大（float('inf')），表示从 s1 到 s2 是不可到达的。
def Cost(self, s1, s2):
if self.CollisionFree(s1, s2):
return self.DiagonalDistance(s1.pos, s2.pos)
else:
return float('inf')（16）定义方法Insert，其功能是将一个状态对象 x 插入到搜索算法的开放列表（open_list）中，并根据状态的当前状态（t 属性）和新的启发式成本（h_new）来更新其 k 值和 h 值。方法Insert首先检查状态 x 的当前状态类型：
- 如果状态是 'NEW'（即尚未被考虑过），则直接将其 k 值设置为 h_new。
- 如果状态已经在 'OPEN' 列表中，那么将 k 值更新为当前 k 值和 h_new 中的较小者，以确保不会增加其成本估计。
- 如果状态在 'CLOSED' 列表中，意味着它已经被探索过，此时将 k 值更新为当前 h 值和 h_new 中的较小者。
无论状态之前的状态如何，都会将其 h 值更新为 h_new，并将状态类型设置为 'OPEN'，表示它现在是待探索的状态。然后，将状态对象添加到开放列表中。
def Insert(self, x, h_new):
if x.t == 'NEW':
x.k = h_new
elif x.t == 'OPEN':
x.k = min(x.k, h_new)
elif x.t == 'CLOSED':
x.k = min(x.h, h_new)
x.h = h_new
x.t = 'OPEN'
self.open_list.add(x)
return（17）定义函数ProcessState，这是一个用于处理状态的算法函数，通常用于启发式搜索算法中，比如A*算法。该函数的主要功能包括：
- 从状态集合中找到具有最小启发式成本（h）的状态，如果找不到则返回-1。
- 记录这个状态的启发式成本（k_old）。
- 从集合中删除这个状态。
- 根据启发式成本和实际成本更新相邻状态的启发式成本和回溯指针（b），以优化搜索路径。
- 根据相邻状态的类型（NEW, CLOSED）和它们的启发式成本，决定是否更新它们的状态，并重新插入到状态集合中。
- 最终返回具有最小启发式成本的状态和对应的状态对象。
def ProcessState(self):
x = self.MinState()
if x is None:
return -1
k_old = self.GetKMin()
self.Delete(x)
if k_old < x.h:
for y in self.Neighbors(x):
if y.h <= k_old and x.h > y.h + self.Cost(y, x):
x.b = y
x.h = y.h + self.Cost(y, x)
if k_old == x.h:
for y in self.Neighbors(x):
if y.t == 'NEW' or \
(y.b is x and y.h != x.h + self.Cost(x, y)) or \
(y.b is not x and y.h > x.h + self.Cost(x, y)):
y.b = x
self.Insert(y, x.h + self.Cost(x, y))
else:
for y in self.Neighbors(x):
if y.t == 'NEW' or \
(y.b is x and y.h != x.h + self.Cost(x, y)):
y.b = x
self.Insert(y, x.h + self.Cost(x, y))
else:
if y.b is not x and y.h > x.h + self.Cost(x, y):
self.Insert(x, x.h)
else:
if y.b is not x and x.h > y.h + self.Cost(y, x) and \
y.t == 'CLOSED' and y.h > k_old:
self.Insert(y, y.h)
return self.GetKMin(), x（18）定义方法ProcessState，功能是在路径搜索算法中处理当前状态 x 并更新其邻居状态。方法ProcessState执行以下步骤：
- 从开放列表中找到具有最小 k 值的状态 x，如果开放列表为空，则返回 -1 表示没有更多状态可以处理。
- 记录下 x 的 k 值之前的状态 k_old。
- 从开放列表中删除状态 x，将其标记为已处理。
- 如果 x 的旧 k 值小于其启发式成本 h，这意味着 x 的成本估计已经更新，需要重新评估其邻居状态 y。如果邻居状态 y 的启发式成本小于或等于 k_old 并且通过 x 到 y 的成本加上 x 的启发式成本小于 y 当前的启发式成本，则更新 y 的最佳路径和成本。
- 如果 x 的旧 k 值等于其启发式成本，说明 x 是通过相同成本到达的，需要更新所有邻居状态 y 的最佳路径和成本，如果邻居状态 y 是新状态或通过 x 到达的成本更低，则更新 y 的父节点和启发式成本。
- 如果 x 的旧 k 值不等于其启发式成本，这表示 x 的成本估计发生了变化，需要更新所有受影响的邻居状态 y。如果 y 是新状态或通过 x 到达的成本更低，更新 y 的父节点和启发式成本。此外，如果 y 之前不是通过 x 到达的，并且 x 的启发式成本加上到 y 的成本小于 y 当前的启发式成本，则需要重新插入 x 或 y 到开放列表中。
最后，方法ProcessState返回更新后的最小 k 值和当前处理的状态 x。这个方法是路径搜索算法中更新和维护状态列表的关键部分，确保始终选择成本最低的状态进行扩展，并及时更新邻居状态的成本估计。
def ModifyCost(self, x, y, cval):
if x.t == 'CLOSED':
# self.Insert(x, x.b.h + self.Cost(x, x.b))
self.Insert(x, y.h + self.Cost(x, y))
return self.GetKMin()（19）定义方法DrawPath，功能是在图像界面上绘制从起点到终点的路径。方法DrawPath首先检查是否提供了颜色参数 color，如果没有提供，则随机生成一个颜色。方法DrawPath使用 mid_node 变量来追踪当前节点，从起点 self.qstart 开始，沿着路径逐步前进，直到到达终点 self.qgoal。对于路径上的每个节点，方法计算出该节点中心点的像素坐标 pt1，以及其父节点（即路径上的前一个节点）的中心点坐标 pt2。然后，使用 cv2.line 函数在图像 self.src_map 上绘制一条线段，连接这两个点，表示路径的一部分。每绘制完一条线段后，使用 cv2.imshow 显示图像，并使用 cv2.waitKey 暂停50毫秒，以便观察路径的绘制过程。
def DrawPath(self, color=None):
'''
路径结果， 这里坐标和cv的图片坐标相反的
'''
if color is None:
color = (random.randint(0, 255),
random.randint(0, 255),
random.randint(0, 255))
mid_node = self.qstart
while mid_node != self.qgoal:
# 画出网格连线
pt1 = (int(mid_node.pos[1] * self.grid_size + self.grid_size / 2),
int(mid_node.pos[0] * self.grid_size + self.grid_size / 2))
pt2 = (int(mid_node.b.pos[1] * self.grid_size + self.grid_size / 2),
int(mid_node.b.pos[0] * self.grid_size + self.grid_size / 2))
cv2.line(self.src_map, pt1, pt2, color, 2)
cv2.imshow('D_STAR', self.src_map)
cv2.waitKey(50)
mid_node = mid_node.b
return（20）定义方法Planning，功能是实现一个交互式的路径规划过程，其中包括障碍物的添加、路径搜索、动态障碍物的处理以及路径的重新规划。总的来说，方法Planning提供了一个动态环境下的路径规划解决方案，允许用户交互式地添加障碍物，并在遇到新的障碍物时重新规划路径。
if __name__ == "__main__":
map_path = 'map/map500-500.png'
qstart = [10, 10]
qgoal = [490, 490]
max_steps = 1000
grid_size = 20
d_star = D_STAR(map_path, qstart, qgoal, grid_size)
input('press any key to start planning:')
d_star.Planning()
input('press any key to quit:')上述代码的实现流程如下所示：
- 首先提示用户输入障碍物的数量，然后尝试将输入转换为整数并使用 AddObstacle 方法添加障碍物。
- 初始化开放列表 open_list 并设置目标点 qgoal 的启发式成本为 0，然后将其插入开放列表。
- 调用 ProcessState方法开始路径搜索过程，直到找到路径或无法找到路径。
- 如果找到路径，调用 DrawPath 方法绘制并显示路径；如果没有找到路径，则输出 "Not Found"。
- 接下来，方法进入一个循环，提示用户是否需要添加更多障碍物。如果是则再次调用 AddObstacle 方法。
- 当动态障碍物出现时，使用两种不同的方法来处理。其中方法一被注释掉了，它涉及到将障碍物设置为无穷成本并重新规划路径。方法二被采用，它将遇到障碍物的节点的成本设置为无穷大，修改通过该节点的成本，并重新进行路径搜索直到该节点不再是障碍物或搜索失败。
- 最后，如果从起点 qstart 到终点 qgoal 的路径上的每个节点都成功访问且没有遇到障碍物，则绘制并显示新路径；如果路径搜索失败，则输出 "Not Found" 并结束。
（21）下面的代码是一个 Python 脚本的主入口点，定义了一个简单的使用场景，使用 D_STAR 类（尽管这个类在代码片段中没有给出定义）来执行路径规划。
if __name__ == "__main__":
map_path = 'map/map500-500.png'
qstart = [10, 10]
qgoal = [490, 490]
max_steps = 1000
grid_size = 20
d_star = D_STAR(map_path, qstart, qgoal, grid_size)
input('press any key to start planning:')
d_star.Planning()
input('press any key to quit:')上述代码的实现流程如下所示：
- 首先，设置地图的路径 map_path，起点坐标 qstart，终点坐标 qgoal，搜索的最大步数 max_steps，以及每个网格单元的大小 grid_size。
- 然后，创建了 D_STAR 类的一个实例 d_star，将地图路径、起点、终点和网格大小作为参数传递。
- 脚本使用 input 函数等待用户按下任意键以开始规划过程，这是一种简单的交互方式，用于在继续之前给用户一个明确的开始信号。
- 一旦用户按下键，d_star.Planning()方法被调用，这将启动路径规划过程，包括障碍物的添加、路径搜索和动态障碍物处理等。
- 最后，脚本再次使用 input函数等待用户按下任意键以退出程序。
运行程序，例如输入障碍物数量为50后会打印输出下面的内容，并绘制找到的最短路径可视化图，如图5-4所示。
press any key to start planning:
input obstacle numbers: 50
Found
add obstacle numbers:
图5-4 最短路径可视化图
860
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
