（6-4-01）D* Lite 算法：D* Lite路径规划器(1)

6.4  D* Lite路径规划器

本项目是一个基于 A* 和 D* Lite 路径规划算法的可视化工具，旨在模拟自动驾驶系统中的路径规划过程。通过在网格地图上放置起点、终点和障碍物，用户可以观察算法如何找到最优路径并实时调整路线以应对动态环境变化。项目不仅提供了路径搜索和执行阶段的视觉反馈，还允许用户手动干预和监控路径规划过程，是理解和测试自动驾驶车辆路径规划算法的有益工具。

实例6-3：D* Lite路径规划系统（codes/6/dynamic-a-star/）

6.4.1  项目介绍

随着自动驾驶技术的快速发展和普及，路径规划成为了自动驾驶系统中至关重要的一环。自动驾驶车辆需要能够安全、高效地从起点到目的地，同时应对各种复杂的道路条件和动态环境变化。路径规划算法在此过程中发挥着关键作用，帮助车辆在不确定和变化的环境中做出最佳决策。传统的路径规划算法如 A* 算法（A-star）以及更为适应动态环境的增量式算法 D* Lite，通过在离散状态空间中进行搜索和更新，能够有效地计算出最短路径或最优路径，并在需要时进行实时调整和优化。这些算法基于启发式搜索和图论等数学原理，通过在网格地图或连续空间中模拟车辆移动，为自动驾驶系统提供了基础的决策支持。

本项目旨在通过可视化工具和交互式界面，帮助用户深入理解和研究路径规划算法的原理和应用。用户可以通过简单的操作，创建虚拟的道路网络，放置起点、终点和障碍物，并选择不同的算法来计算和观察最优路径的生成过程。这些功能不仅有助于学术研究和算法开发，也为自动驾驶系统的开发和优化提供了实验平台和测试工具。

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

（1）网格设计和编辑模块

1. 用户可以通过在网格地图上点击鼠标来放置起点、终点和障碍物，定义路径规划的起始条件和环境约束。
2. 提供网格绘制工具，显示出路径规划的可视化效果，包括起点、终点、障碍物和可通行路径。

（2）路径规划模块

1. 支持 A* 算法和 D* Lite 算法两种路径规划方法。
2. 用户可以选择执行不同的算法来计算从起点到终点的最优路径。
3. 提供实时的路径更新和重新规划功能，以适应动态环境变化或用户手动调整。

（3）路径执行和仿真模块

1. 在路径规划完成后，用户可以切换到执行模式，观察自动驾驶车辆如何沿着计算出的路径移动。
2. 支持用户手动干预，例如在执行过程中添加障碍物或调整路径。

（4）交互式控制和监视

1. 提供交互式控制界面，包括模式选择、算法选择和执行控制。
2. 实时显示路径规划过程和执行状态，包括起点、终点、当前位置以及路径的动态变化。

（5）用户体验优化

1. 设计简洁直观的用户界面，使用户能够轻松理解和操作路径规划过程。
2. 提供实时反馈和视觉效果，帮助用户更好地理解和分析不同算法在不同场景下的表现和效率。

通过这些功能模块，本项目旨在帮助用户深入理解和测试自动驾驶车辆的路径规划算法，为研究人员和开发者提供一个实验和验证算法效果的工具。

6.4.2  实现路径规划算法

文件path_finding.py实现了两个路径搜索算法：D* Lite和A*（A-star），主要功能是在给定的网格地图上寻找从起点到终点的最短路径，考虑到可能的障碍物和障碍物的动态变化。其中，D* Lite是一种增量式路径规划算法，适合在环境动态变化时重新计算路径；A*算法则是经典的静态路径搜索算法。代码中实现了路径的计算、障碍物的检测与处理，以及在Pygame中实时显示和交互。

（1）定义启发式函数 h，用于计算两个点 p1 和 p2 之间的曼哈顿距离（Manhattan distance）。曼哈顿距离是两点在一个网格中沿着网格边缘行走的距离，计算方法为两点在水平和垂直方向上坐标差的绝对值之和。这段代码是路径搜索算法中用于估计节点之间距离的一部分。

def h(p1, p2):
	x1, y1 = p1
	x2, y2 = p2
	return abs(x1 - x2) + abs(y1 - y2)

（2）定义计算节点 spot 的关键值的函数 calculate_key，关键值由两部分组成：k1 和 k2。其中，k1 是基于节点的当前路径代价、启发式函数值和一个额外参数 k_m 的组合计算而来；k2 则是节点的路径代价 g 和路径代价边界 rhs 中较小的一个。该函数在D* Lite路径搜索算法中用于评估节点在优先队列中的优先级，以便实现增量式路径更新和动态障碍物处理的功能。

def calculate_key(spot, current, k_m):
    k1 = min(spot.g, spot.rhs) + h(spot.get_pos(), current.get_pos()) + k_m
    k2 = min(spot.g, spot.rhs)
    return (k1, k2)

（3）定义函数 top_key，用于从优先队列 queue 中获取优先级最高的节点的关键值。如果队列不为空，则对队列进行排序并返回第一个元素的前两个值（通常是计算关键值时返回的 k1 和 k2）。如果队列为空，则返回一个表示无穷大的元组 (float('inf'), float('inf'))。这个函数在D* Lite路径搜索算法中用于确定下一个要处理的节点。

def top_key(queue):
    queue.sort()
    if len(queue) > 0:
        return queue[0][:2]
    else:
        return (float('inf'), float('inf'))  

（4）下面代码实现了路径搜索算法中的节点更新操作函数 update_vertex，用于在D* Lite路径搜索算法中负责更新节点状态，并根据新的路径代价重新安排节点在优先队列中的位置。首先，更新节点 spot 的路径代价边界 rhs，考虑其所有邻居节点的路径代价。然后，检查优先队列 queue 中是否包含节点 spot 的条目，并确保只有一个条目存在；如果存在多个，则引发异常。接着，如果节点 spot 的 rhs 值不等于 g 值，则将其更新后的关键值重新加入优先队列，并标记节点为开放状态。最后，函数调用 draw()，用于更新路径可视化。

def update_vertex(draw, queue, spot, current, end, k_m):
    s_goal = end
    if spot != s_goal:
        min_rhs = float('inf')
        for neighbor in spot.neighbors:
            min_rhs = min(min_rhs, neighbor.g + h(spot.get_pos(),neighbor.get_pos()))
        spot.rhs = min_rhs
    id_in_queue = [item for item in queue if spot in item]
    if id_in_queue != []:
        if len(id_in_queue) != 1:
            raise ValueError('more than one spot (' + spot.get_pos() + ') in the queue!')
        queue.remove(id_in_queue[0])
    if spot.rhs != spot.g:
        heapq.heappush(queue, calculate_key(spot, current, k_m) + (spot,))
        spot.make_open()
    draw()

（5）定义函数 next_in_shortest_path，用于确定当前节点 current 在最短路径中的下一个节点。首先，检查当前节点的路径代价边界 rhs 是否为无穷大，如果是，则打印消息表示“陷入困境”。否则，遍历当前节点的所有邻居节点，计算每个邻居节点到当前节点的路径代价，并找到其中路径代价最小的节点作为下一个节点。如果找到了合适的节点，则返回该节点；如果未找到合适的节点，则引发异常，表示无法进行有效的状态转换。

def next_in_shortest_path(current):
    min_rhs = float('inf')
    next = None
    if current.rhs == float('inf'):
        print('You are stuck!')
    else:
        for neighbor in current.neighbors:
            # print(i)
            child_cost = neighbor.g + h(current.get_pos(),neighbor.get_pos())
            # print(child_cost)
            if (child_cost) < min_rhs:
                min_rhs = child_cost
                next = neighbor
        if next:
            return next
        else:
            raise ValueError('No suitable child for transition!')

（6）定义函数scan_obstacles，用于在给定的搜索范围内扫描障碍物并更新路径的功能。首先，根据指定的扫描范围收集当前节点 current 的所有邻居节点，并将其添加到待更新的节点列表 spots_to_update 中。接着，使用一个循环逐步扩展扫描范围，通过检查每个节点的邻居，逐层将新发现的节点添加到待更新列表中，直到达到指定的扫描范围。然后，将待更新列表转换为唯一的节点集合，以避免重复更新相同的节点。最后，它遍历所有待更新的节点，如果发现其中有障碍物或对象，就更新路径信息，并返回一个指示是否发现新障碍物的标志。

def scan_obstacles(draw, queue, current, end, scan_range, k_m):
    spots_to_update = []
    range_checked = 0
    if scan_range >= 1:
        for neighbor in current.neighbors:  
            print(f"adding {neighbor.get_pos()} to spots to update")          
            spots_to_update.append(neighbor)
        range_checked = 1
    # print(states_to_update)

    while range_checked < scan_range:
        new_set = []
        for spot in spots_to_update:
            new_set.append(spot)
            print(f"adding {spot.get_pos()} to spots to update")       
            for neighbor in spot.neighbors:
                if neighbor not in new_set:
                    new_set.append(neighbor)
                    print(f"adding {neighbor.get_pos()} to spots to update")       
        range_checked += 1
        spots_to_update = new_set

    spots_to_update = list(set(spots_to_update))

    new_obstacle = False
    for spot in spots_to_update:
        if spot.is_barrier() or spot.is_object():  # found cell with obstacle
            print('found obstacle in ', spot.get_pos())
            for neighbor in spot.neighbors:
                # 第一次观察到有障碍物的单元格，在此之前没有障碍物
                # 如果图中的状态子图.children[邻居] != float('inf'):
                update_vertex(draw, queue, spot, current, end, k_m)
                new_obstacle = True
        # elif states_to_update[state] == 0: # 没有障碍物的单元格
            # for neighbor in graph.graph[state].children:
                # 如果图中的状态子图.children[邻居] != float('inf'):
    # print(graph)
    return new_obstacle

（7）定义函数 move_and_rescan，其功能是在当前节点 current 向终点 end 移动并进行重新扫描的过程。如果当前节点已经等于终点 end，则返回 'goal' 和当前的 k_m 值。否则，获取当前节点的下一个最短路径节点 new。

def move_and_rescan(draw, queue, current, end, scan_range, k_m):
    if(current == end):
        return 'goal', k_m
    else:
        last = current
        new = next_in_shortest_path(current)        

        if(new.is_object() or new.is_barrier()):  # just ran into new obstacle
            print("obstacle")
            new = current  # need to hold tight and scan/replan first

        results = scan_obstacles(draw, queue, new, end, scan_range, k_m)
        # print(graph)
        k_m += h(last.get_pos(), new.get_pos())
        calc_shortest_path(draw, queue, current, end, k_m)
        return new, k_m 

（8）定义函数 calc_shortest_path，用于计算从起点 start 到终点 end 的最短路径。函数通过迭代处理优先队列 queue 中的节点，直到满足以下两个条件之一：起点节点 start 的路径代价边界 rhs 等于路径代价 g，或者优先队列中顶部节点的关键值小于使用当前 k_m 值计算的起点节点的关键值。

def calc_shortest_path(draw, queue, start, end, k_m):
    while (start.rhs != start.g) or \
          (top_key(queue) < calculate_key(start, start, k_m)):
        # 更新旧的关键值
        k_old = top_key(queue)
        u = heapq.heappop(queue)[2]        
        if k_old < calculate_key(u, start, k_m):
            heapq.heappush(queue, calculate_key(u, start, k_m) + (u,))
            u.make_open()
        elif u.g > u.rhs:
            u.g = u.rhs
            for neighbor in u.neighbors:
                update_vertex(draw, queue, neighbor, start, end, k_m)
        else:
            u.g = float('inf')
            update_vertex(draw, queue, u, start, end, k_m)
            for neighbor in u.neighbors:
                update_vertex(draw, queue, neighbor, start, end, k_m)   
        u.make_closed()
        draw() 

（9）定义函数d_star_lite，实现了D* Lite算法的主要逻辑。首先，通过遍历网格中的每个位置，将所有节点的路径代价 g 和路径代价边界 rhs 初始化为无穷大。然后，将终点 end 的路径代价 g 设置为0，路径代价边界 rhs 也设置为0。接下来，使用函数calculate_key计算终点 end 的关键值，并将其与终点 end 一起推入优先队列 queue 中。然后调用 calc_shortest_path 函数，利用D* Lite算法计算从起点 start 到终点 end 的最短路径。最后，返回更新后的优先队列 queue 和更新后的 k_m 值，用于后续路径规划和更新。

def d_star_lite(draw, grid, queue, start, end, k_m):
    for row in grid:
        for spot in row:
            spot.g = float("inf")
            spot.rhs = float("inf")
    end.g = 0
    end.rhs = 0
    heapq.heappush(queue, calculate_key(end, start, k_m) + (end,))
    calc_shortest_path(draw, queue, start, end, k_m)
    return queue, k_m

（10）函数a_star实现了A*算法，用于在给定的网格 grid 上从起点 start 到终点 end 的最短路径搜索。函数a_star使用优先级队列 open_set 存储待处理的节点，并初始化了路径记录 came_from、路径代价 g_score 和启发式代价 f_score。在主循环中，如果优先级队列不为空，会从中取出当前优先级最高的节点 current 进行处理。如果当前节点 current 等于终点 end，则通过 came_from 返回路径记录。

def a_star(draw, grid, start, end):
	count = 0
	open_set = PriorityQueue()
	open_set.put((0, count, start))
	came_from = {}
	g_score = {spot: float("inf") for row in grid for spot in row}
	g_score[start] = 0
	f_score = {spot: float("inf") for row in grid for spot in row}
	f_score[start] = h(start.get_pos(), end.get_pos())
	open_set_hash = {start}
	while not open_set.empty():
		for event in pygame.event.get():
			if event.type == pygame.QUIT:
				pygame.quit()
		current = open_set.get()[2]
		open_set_hash.remove(current)
		if current == end:
			# 重建路径(came_from, end, draw)
			# end.make_end()
			return came_from
		for neighbor in current.neighbors:
			temp_g_score = g_score[current] + 1
			if temp_g_score < g_score[neighbor]:
				came_from[neighbor] = current
				g_score[neighbor] = temp_g_score
				f_score[neighbor] = temp_g_score + h(neighbor.get_pos(), end.get_pos())
				if neighbor not in open_set_hash:
					count += 1
					open_set.put((f_score[neighbor], count, neighbor))
					open_set_hash.add(neighbor)
					neighbor.make_open()
		if current != start:
			current.make_closed()
		draw()
	return None

在上述代码中，对于当前节点的每个邻居节点，计算临时路径代价 temp_g_score，如果计算得到的路径代价比当前记录的路径代价 g_score 更优，则更新路径记录，并更新启发式代价 f_score。如果邻居节点不在优先级队列中，将其加入队列，并标记为开放状态。如果当前节点不是起点 start，则将其标记为关闭状态，并通过 draw() 函数更新路径的可视化。