（8-3-01）自动驾驶中的无地图环境路径探索：D* Lite路径规划系统（1）

8.3 实战案例：D* Lite路径规划系统

本项目实现了D* Lite路径规划算法的可视化演示，使用Pygame库创建了一个网格世界环境，包括起点、终点和障碍物。通过交互式操作，用户可以观察机器人在环境中移动并重新规划路径，展示了路径规划在实时环境中的应用。

实例 8-3 ： D* Lite 路径规划系统 （ codes/8/d-star-lite/ ）

8.3.1 项目介绍

本项目实现了D* Lite路径规划算法的可视化系统，首先使用Pygame库创建了一个网格世界环境，包括可自定义的起点、终点和障碍物。然后，利用D* Lite算法在此环境中规划路径，并通过交互式操作实时更新机器人的移动情况和重新规划的路径。用户可以观察机器人在网格世界中的行进过程，并随时通过键盘输入触发重新规划，以展示路径规划在动态环境中的应用。

本项目的功能模块如下所示：

1. 网格世界环境创建模块：使用Pygame库创建了一个网格世界的可视化环境，包括网格单元格的绘制、起点、终点和障碍物的设置等功能。
2. 路径规划模块：实现了D* Lite路径规划算法，通过该算法在网格世界中进行路径规划，并能够实时更新路径以适应环境的变化。
3. 交互式控制模块：通过键盘输入和鼠标点击等交互方式，实现了对机器人移动和环境操作的控制，包括手动触发路径重新规划、设置障碍物等功能。
4. 可视化模块：利用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表示图中的节点，具有如下所示的属性和方法。

1. id：节点的标识符。
2. parents：字典，存储节点的父节点及对应的边权重。
3. children：字典，存储节点的子节点及对应的边权重。
4. g：表示节点的 g 值。
5. rhs：表示节点的 rhs 值。
6. init 方法：用于初始化节点对象。
7. str 和 repr 方法：用于返回节点对象的字符串表示。
8. update_parents 方法：用于更新节点的父节点。

类Graph表示整个图，具有如下所示的属性和方法。

1. graph字典：：存储图中的所有节点。
2. setStart 方法：用于设置起始节点。
3. setGoal 方法：用于设置目标节点。
4. init 方法：用于初始化图对象。
5. 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 方法被重载，以提供对该对象的字符串表示。