8.3 实战案例：D* Lite路径规划系统
本项目实现了D* Lite路径规划算法的可视化演示，使用Pygame库创建了一个网格世界环境，包括起点、终点和障碍物。通过交互式操作，用户可以观察机器人在环境中移动并重新规划路径，展示了路径规划在实时环境中的应用。
实例8-3：D* Lite路径规划系统（codes/8/d-star-lite/）
8.3.1 项目介绍
本项目实现了D* Lite路径规划算法的可视化系统，首先使用Pygame库创建了一个网格世界环境，包括可自定义的起点、终点和障碍物。然后，利用D* Lite算法在此环境中规划路径，并通过交互式操作实时更新机器人的移动情况和重新规划的路径。用户可以观察机器人在网格世界中的行进过程，并随时通过键盘输入触发重新规划，以展示路径规划在动态环境中的应用。
本项目的功能模块如下所示：
- 网格世界环境创建模块：使用Pygame库创建了一个网格世界的可视化环境，包括网格单元格的绘制、起点、终点和障碍物的设置等功能。
- 路径规划模块：实现了D* Lite路径规划算法，通过该算法在网格世界中进行路径规划，并能够实时更新路径以适应环境的变化。
- 交互式控制模块：通过键盘输入和鼠标点击等交互方式，实现了对机器人移动和环境操作的控制，包括手动触发路径重新规划、设置障碍物等功能。
- 可视化模块：利用Pygame库将网格世界、机器人位置、路径规划结果等实时可视化展示在屏幕上，提供直观的视觉反馈，帮助用户理解路径规划过程和结果。
上述功能模块共同构成了一个完整的路径规划演示系统，用户可以通过交互式操作在网格世界中观察路径规划的过程和结果，从而更好地理解和掌握路径规划算法的原理和应用。
8.3.2 构建图
在 D* Lite 算法中，图起着关键作用。D* Lite 是一种用于路径规划的增量式搜索算法，它通过在图中进行搜索来找到从起始点到目标点的最短路径。
（1）文件graph.py定义了两个类：Node 和 Graph，用于表示图结构以及节点。
class Node: def __init__(self, id): self.id = id # 父节点的 ID 字典 self.parents = {} # 子节点的 ID 字典 self.children = {} # g 的近似值 self.g = float('inf') # rhs 值 self.rhs = float('inf') def __str__(self): return 'Node: ' + self.id + ' g: ' + str(self.g) + ' rhs: ' + str(self.rhs) def __repr__(self): return self.__str__() def update_parents(self, parents): self.parents = parents class Graph: def __init__(self): self.graph = {} def __str__(self): msg = 'Graph:' for i in self.graph: msg += '\n node: ' + i + ' g: ' + \ str(self.graph[i].g) + ' rhs: ' + str(self.graph[i].rhs) return msg def __repr__(self): return self.__str__() def setStart(self, id): if(self.graph[id]): self.start = id else: raise ValueError('start id not in graph') def setGoal(self, id): if(self.graph[id]): self.goal = id else: raise ValueError('goal id not in graph') def addNodeToGraph(graph, id, neighbors, edge=1): node = Node(id) for i in neighbors: # print(i) node.parents[i] = edge node.children[i] = edge graph[id] = node return graph graph = addNodeToGraph(graph, 'x1y1', ['x1y2', 'x2y1']) graph = addNodeToGraph(graph, 'x2y1', ['x1y1', 'x3y1', 'x2y2']) graph = addNodeToGraph(graph, 'x1y2', ['x1y1', 'x2y2']) graph = addNodeToGraph(graph, 'x2y2', ['x1y2', 'x2y1', 'x3y2']) graph = addNodeToGraph(graph, 'x3y1', ['x3y2', 'x2y1']) graph = addNodeToGraph(graph, 'x3y2', ['x3y1', 'x2y2']) g = GridWorld(X_DIM, Y_DIM) return g类Node表示图中的节点，具有如下所示的属性和方法。
- id：节点的标识符。
- parents：字典，存储节点的父节点及对应的边权重。
- children：字典，存储节点的子节点及对应的边权重。
- g：表示节点的 g 值。
- rhs：表示节点的 rhs 值。
- __init__ 方法：用于初始化节点对象。
- __str__ 和 __repr__ 方法：用于返回节点对象的字符串表示。
- update_parents 方法：用于更新节点的父节点。
类Graph表示整个图，具有如下所示的属性和方法。
- graph字典：：存储图中的所有节点。
- setStart 方法：用于设置起始节点。
- setGoal 方法：用于设置目标节点。
- __init__ 方法：用于初始化图对象。
- __str__ 和 __repr__ 方法：用于返回图对象的字符串表示。
（2）文件grid.py定义了类GridWorld，它是类Graph的子类，用于表示一个网格世界。在初始化的过程中，通过传入的参数 x_dim 和 y_dim 来创建一个指定大小的网格，然后根据连接方式（8 连通或 4 连通），生成对应的图结构。
from graph import Node, Graph class GridWorld(Graph): def __init__(self, x_dim, y_dim, connect8=True): self.x_dim = x_dim # 网格的 x 维度 self.y_dim = y_dim # 网格的 y 维度 # 为每一行创建一个元素（网格的高度） self.cells = [0] * y_dim # 遍历每个元素，并用一行来替换（网格的宽度） for i in range(y_dim): self.cells[i] = [0] * x_dim # 是否为 8 连通图，若为 True 则表示是，否则为 4 连通图 self.connect8 = connect8 self.graph = {} # 图的节点和边 self.generateGraphFromGrid() # 从网格生成图 # self.printGrid() def __str__(self): msg = 'Graph:' #初始化一个字符串变量 msg for i in self.graph: msg += '\n node: ' + i + ' g: ' + \ str(self.graph[i].g) + ' rhs: ' + str(self.graph[i].rhs) + \ ' neighbors: ' + str(self.graph[i].children) return msg def __repr__(self): return self.__str__() def printGrid(self): print('** GridWorld **') # 网格世界 for row in self.cells: print(row) def printGValues(self): for j in range(self.y_dim): str_msg = "" for i in range(self.x_dim): node_id = 'x' + str(i) + 'y' + str(j) # 节点 ID node = self.graph[node_id] if node.g == float('inf'): str_msg += ' - ' # 无穷大 else: str_msg += ' ' + str(node.g) + ' ' # 节点的 g 值 print(str_msg) def generateGraphFromGrid(self): edge = 1 for i in range(len(self.cells)): row = self.cells[i] for j in range(len(row)): # print('graph node ' + str(i) + ',' + str(j)) node = Node('x' + str(i) + 'y' + str(j)) # 节点 if i > 0: # 不是顶部行 node.parents['x' + str(i - 1) + 'y' + str(j)] = edge node.children['x' + str(i - 1) + 'y' + str(j)] = edge if i + 1 < self.y_dim: # 不是底部行 node.parents['x' + str(i + 1) + 'y' + str(j)] = edge node.children['x' + str(i + 1) + 'y' + str(j)] = edge if j > 0: # 不是左侧列 node.parents['x' + str(i) + 'y' + str(j - 1)] = edge node.children['x' + str(i) + 'y' + str(j - 1)] = edge if j + 1 < self.x_dim: # 不是右侧列 node.parents['x' + str(i) + 'y' + str(j + 1)] = edge node.children['x' + str(i) + 'y' + str(j + 1)] = edge self.graph['x' + str(i) + 'y' + str(j)] = node # 添加到图中通过上述代码可知，类GridWorld包含了打印网格、打印节点 g 值、从网格生成图的成员方法。其中，方法printGrid()用于将网格结构打印到控制台，方法printGValues()用于打印图中节点的 g 值，而方法generateGraphFromGrid()则根据网格结构生成图的节点和边，并存储在 self.graph 属性中。最终，类GridWorld中的 __str__ 和 __repr__ 方法被重载，以提供对该对象的字符串表示。
