6.3.2 实现路径规划算法
文件d_star_lite.py实现了D* Lite算法，用于路径规划，适用于动态环境中的自动驾驶场景。首先读取地图图像并进行网格化处理，将地图分成若干小网格。每个网格对应一个节点对象，并标识是否为障碍物。起始节点和目标节点初始化后，通过优先队列和一系列更新规则计算出最短路径。当有新障碍物出现时，算法会实时更新路径，保证路径的动态最优性。最终，计算出的路径在地图上显示出来，适用于自动驾驶中对障碍物和路径的实时更新与避让。文件d_star_lite.py的具体实现流程如下所示。
（1）定义类Node，代表路径规划中的一个节点。每个节点具有以下属性：位置pos，到起点的估计成本g，从起点到节点的最小成本rhs，节点的键值k，指向前驱节点的指针p，以及一个标志位is_obs表示是否为障碍物。该类用于在路径规划算法中存储和处理节点的信息。
class Node(object):
def __init__(self, pos):
self.pos = pos
self.g = float('inf')
self.rhs = float('inf')
self.k = [float('inf'), float('inf')]
self.p = None
self.is_obs = False（2）定义类D_STAR_LITE的构造方法 __init__，该方法初始化了D* Lite算法的方向列表 self.directions，表示八个可能的移动方向。接着，它调用了 self.MapGridding 方法，将给定的地图路径 map_path、起始点 qstart、目标点 qgoal 和网格大小 grid_size 进行网格化处理，准备路径规划所需的数据结构。
class D_STAR_LITE(object):
def __init__(self, map_path, qstart, qgoal, grid_size):
self.directions = [[-1, -1], [-1, 0], [-1, 1],
[0, -1], [0, 1],
[1, -1], [1, 0], [1, 1]]
self.MapGridding(map_path, qstart, qgoal, grid_size)
return（3）定义方法MapGridding，用于将输入的地图图像进行网格化处理。该方法首先读取并转换地图图像为灰度图，然后通过二值化处理得到二值图像。接着，计算地图的网格尺寸，将地图分成若干小网格，并为每个网格初始化一个 Node 对象，存储在二维数组 self.S 中。起始点和目标点也在此过程中被初始化。随后，该方法检查每个网格是否包含障碍物，并绘制网格线将地图可视化显示出来。
def MapGridding(self, map_path, qstart, qgoal, grid_size, color=(0, 0, 0)):
'''
grid map 网格化地图
'''
self.src_map = cv2.imread(map_path)
self.map = cv2.cvtColor(self.src_map, cv2.COLOR_BGR2GRAY)
_, self.map = cv2.threshold(
self.map, 0, 255, cv2.THRESH_BINARY_INV)
self.map_shape = np.shape(self.map)
self.grid_size = grid_size
self.rows = int(self.map_shape[0] / grid_size)
self.cols = int(self.map_shape[1] / grid_size)
# 地图栅格, 初始化地图所有路径点
self.S = []
for row in range(self.rows):
items_rows = []
for col in range(self.cols):
items_rows.append(Node([row, col]))
self.S.append(items_rows)
self.qstart = self.S[int(qstart[0] / grid_size)][int(qstart[1] / grid_size)]
self.qgoal = self.S[int(qgoal[0] / grid_size)][int(qgoal[1] / grid_size)]
for row in range(self.rows):
for col in range(self.cols):
if self.IsObstacle(self.S[row][col]):
self.S[row][col].is_obs = True
# 绘制地图栅格
for row in range(self.rows + 1):
pt1 = (0, int(row * self.grid_size))
pt2 = (int(self.cols * self.grid_size), int(row * self.grid_size))
cv2.line(self.src_map, pt1, pt2, color)
cv2.imshow('D_STAR_LITE', self.src_map)
cv2.waitKey(2)
for col in range(self.cols + 1):
pt1 = (int(col * self.grid_size), 0)
pt2 = (int(col * self.grid_size), int(self.rows * self.grid_size))
cv2.line(self.src_map, pt1, pt2, color)
cv2.imshow('D_STAR_LITE', self.src_map)
cv2.waitKey(2)
cv2.imshow('D_STAR_LITE', self.src_map)
cv2.waitKey(50)
return（4）定义方法IsObstacle，用于判断节点 s 所在的网格是否有障碍物。该方法通过计算节点所在网格区域的像素值总和，来确定该区域是否存在障碍物。如果区域内的像素值总和不为零，说明有障碍物，返回 True；否则返回 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（5）定义方法CollisionFree，用于检查从节点 s1 到节点 s2 的路径是否无障碍物。该方法计算两节点之间区域的像素值总和，如果总和不为零，说明存在障碍物，返回 False。此外，它还检查 s1 和 s2 节点本身是否为障碍物。如果两节点之间区域无障碍物且两节点本身不是障碍物，则返回 True。
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（6）下面的代码定义了三个方法，用于计算两点之间的不同距离度量：
- ChebyshevDistance：计算并返回两点之间的切比雪夫距离，即网格中两点在水平或垂直方向上的最大距离，乘以网格大小。
- EuclideanDistance：计算并返回两点之间的欧几里得距离，即两点间直线距离的整数值，乘以网格大小。
- DiagonalDistance：计算并返回两点之间的对角距离，即两点水平和垂直方向距离之和，加上对角移动的额外代价，乘以网格大小。
def ChebyshevDistance(self, p1, p2):
return self.grid_size * max(abs(p1[0] - p2[0]), abs(p1[1] - p2[1]))
def EuclideanDistance(self, p1, p2):
return self.grid_size * int(math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2))
def DiagonalDistance(self, p1, p2):
dx = abs(p1[0] - p2[0])
dy = abs(p1[1] - p2[1])
return self.grid_size * (dx + dy) + int((math.sqrt(2) - 2) * self.grid_size) * min(dx, dy)（7）定义方法Cost，用于估计两个节点 s1 和 s2 之间的移动代价。首先，调用 CollisionFree 方法检查从 s1 到 s2 的路径是否无障碍物。如果路径无障碍，则返回两节点之间的对角距离（使用 DiagonalDistance 计算）；如果路径有障碍，则返回无穷大（表示不可达）。
def Cost(self, s1, s2):
'''
节点 s1 和 s2 估计距离
'''
if self.CollisionFree(s1, s2):
return self.DiagonalDistance(s1.pos, s2.pos)
else:
return float('inf')（8）下面的代码定义了三个方法，用于路径规划中的关键值计算和优先队列操作：
- HScore：计算并返回节点 s 到起始点的估计距离，使用对角距离（DiagonalDistance）度量。
- CalculateKey：计算并返回节点 s 的键值。键值是一个包含两个元素的列表，其中第一个元素是 s 的估计成本 g 和最小成本 rhs 中较小者与 HScore(s) 之和，第二个元素是较小的 g 或 rhs 值。这个键值用于优先队列中的排序。
- TopKey：返回优先队列 U 中键值最小的键值，即队列中的顶键值，用于确定当前最优节点。
def HScore(self, s):
'''
节点 s 到起始点的估计距离
'''
return self.DiagonalDistance(self.qstart.pos, s.pos)
def CalculateKey(self, s):
'''
计算节点 Key值
'''
key2 = min(s.g, s.rhs)
return [key2 + self.HScore(s), key2]
def TopKey(self):
'''
返回有限队列 topkey
'''
return min([X.k for X in self.U])（9）定义方法Successors，用于寻找与节点 s 相邻且无障碍物的节点列表。首先遍历预先定义的八个方向（包括水平、垂直和对角线方向），计算每个方向上的相邻节点位置。然后检查这些相邻节点是否在地图范围内，并且是否没有障碍物。符合条件的相邻节点会被添加到 succ_list 列表中，并最终返回。这个方法在路径规划中用于获取当前节点 s 的可行邻接节点列表。
def Successors(self, s):
'''
8 邻接节点：与 s 可以联通（s和succ均无障碍物）
'''
succ_list = []
for direction in self.directions:
row = s.pos[0] + direction[0]
if row < 0 or row >= self.rows:
continue
col = s.pos[1] + direction[1]
if col < 0 or col >= self.cols:
continue
neighbor = self.S[row][col]
if neighbor.is_obs == False:
succ_list.append(neighbor)
return succ_list（10）定义方法Predessors，它实际上是 Successors 方法的一个别名。该方法的功能是返回与给定节点 s 相邻且无障碍物的节点列表，与 Successors 方法完全一致。在某些路径规划算法中，包括D* Lite算法，节点的前驱（predecessor）与后继（successor）具有相同的意义，因此这里使用 Predessors 方法来直接调用 Successors 方法以简化代码逻辑。
def Predessors(self, s):
return self.Successors(s)
170
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
