FA_规划和控制(PC)-D*规划

FA：formulas and algorithm, PC：planning and control

D*（D-Star）是一种经典的增量式启发式路径规划算法，专门针对动态环境设计 ------ 也就是环境中障碍物可能随时变化的场景，这也是它和 A最核心的区别（A更适合静态环境）。下面我会从基础概念到代码实现，一步步帮你彻底理解 D*。

一、D * 算法核心解析

1. 核心定位与优势

• 本质：反向搜索的增量式路径规划算法（从终点向起点搜索）
• 核心思想：先在初始环境中规划一条从起点到终点的路径；当环境变化（发现新障碍物）时，不重新全局搜索，而是仅更新受影响的路径段，大幅提升效率
• 对比 A*：

特性	A*	D*
搜索方向	起点→终点	终点→起点（反向）
环境适应性	静态环境	动态环境（增量更新）
核心优势	静态环境下效率高	动态环境下重规划快
适用场景	一次性路径规划	实时避障、移动机器人导航

2. 核心概念

• 状态（State） ：节点的三种状态（区分待处理节点）：
  NEW：新节点，未处理
  OPEN：待评估节点（优先级队列）
  CLOSED：已处理节点
• 优先级（Priority）：每个节点的优先级由两个值决定 (k1, k2)，用于优先级队列排序
• 代价更新（Cost Update）：当环境变化时，仅更新受影响节点的代价值，而非重新计算全局代价

3. 典型使用场景

• 移动机器人自主导航（如扫地机器人、无人机、AGV）
• 自动驾驶车辆的实时避障（道路突发障碍物）
• 游戏 AI 的动态路径规划（游戏中敌人 / 道具位置变化）
• 物流机器人在动态仓库中的路径规划

表格

提出者	核心思想	适用场景	工程推荐	经典应用	学习建议
Anthony Stentz (1994)	反向搜索 + 增量更新	动态/未知环境中的路径规划	优先使用 D Lite	NASA 火星车、军用无人平台	先掌握 A，再学 D Lite

二、D * 算法具体操作步骤

D * 的核心流程分为初始规划 和增量重规划两个阶段，步骤如下：

阶段 1：初始反向搜索（终点→起点）

• 初始化：
  • 设定终点为起始节点，初始化其代价值 g(goal)=0g(goal) = 0g(goal)=0
  • 将终点加入优先级队列（OPEN）
  • 所有其他节点的代价值初始化为无穷大
• 反向搜索：
  • 从优先级队列中取出优先级最高的节点 n
  • 遍历节点 n 的所有邻居节点 n'
  • 计算邻居节点的新代价值 gnew=g(n)+cost(n,n′)g_new = g(n) + cost(n, n')gnew=g(n)+cost(n,n′)（cost为两节点间的移动代价）
  • 如果 gnew<g(n′)g_new < g(n')gnew<g(n′)，更新 g(n′)g(n')g(n′)，并将 n' 加入 / 更新到优先级队列
  • 将节点 n 标记为 CLOSED
  • 重复直到起点被标记为 CLOSED（初始路径规划完成）
• 生成初始路径：从起点出发，每一步选择相邻节点中代价值最小的节点，直到终点

阶段 2：增量重规划（环境变化时）

• 检测环境变化：机器人移动过程中发现新障碍物（某条边 / 节点不可通行）
• 更新代价：将障碍物节点的代价值设为无穷大，并加入优先级队列
• 重新评估 ：
  • 从优先级队列中取出节点，更新其邻居节点的代价值
  • 仅更新受障碍物影响的节点（增量更新）
• 重新生成路径：从当前机器人位置出发，基于更新后的代价值，重新选择最优路径

三、D * 算法代码实例

下面实现一个简化版的 D * 算法，包含核心的初始规划和增量重规划功能，并附带动态可视化：

import math
import heapq
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import numpy as np

# 定义优先级队列元素（用于D*的优先级排序）
class PriorityQueueItem:
    def __init__(self, priority, node):
        self.priority = priority  # (k1, k2) 优先级
        self.node = node          # 节点坐标 (x, y)
    
    def __lt__(self, other):
        # 优先级排序规则：先比k1，再比k2
        if self.priority[0] != other.priority[0]:
            return self.priority[0] < other.priority[0]
        return self.priority[1] < other.priority[1]

# D*算法核心类
class DStarPlanner:
    def __init__(self, grid_size=0.5, obstacle_radius=1.0):
        self.grid_size = grid_size          # 网格步长
        self.obstacle_radius = obstacle_radius  # 障碍物半径
        self.g = {}  # 节点到终点的实际代价
        self.rhs = {} # 节点的局部一致代价（D*核心）
        self.open_list = []  # 优先级队列
        self.obstacles = set()  # 障碍物集合
        self.goal = None        # 终点
        self.start = None       # 起点
        self.current = None     # 当前位置
        
        # 可视化相关
        self.fig, self.ax = None, None
        self.path_line = None
        self.obstacle_patches = []
        self.node_scatter = None

    # 初始化节点代价
    def init_node(self, node):
        if node not in self.g:
            self.g[node] = float('inf')
            self.rhs[node] = float('inf')

    # 计算两节点间的欧几里得距离
    def distance(self, node1, node2):
        return math.hypot(node1[0]-node2[0], node1[1]-node2[1])

    # 计算节点的优先级
    def calculate_key(self, node):
        min_val = min(self.g[node], self.rhs[node])
        k1 = min_val + self.heuristic(node, self.current)
        k2 = min_val
        return (k1, k2)

    # 启发函数：到当前位置的欧几里得距离
    def heuristic(self, node1, node2):
        if node2 is None:
            return 0
        return self.distance(node1, node2)

    # 向优先级队列添加节点
    def insert(self, node):
        key = self.calculate_key(node)
        heapq.heappush(self.open_list, PriorityQueueItem(key, node))

    # 更新节点的rhs值
    def update_vertex(self, node):
        if node != self.goal:
            min_rhs = float('inf')
            # 遍历所有邻居节点
            for neighbor in self.get_neighbors(node):
                if not self.is_collision(neighbor):
                    min_rhs = min(min_rhs, self.g[neighbor] + self.distance(node, neighbor))
            self.rhs[node] = min_rhs
        
        # 从开放列表中移除旧的节点项
        self.open_list = [item for item in self.open_list if item.node != node]
        heapq.heapify(self.open_list)
        
        # 如果g和rhs不一致，加入开放列表
        if self.g[node] != self.rhs[node]:
            self.insert(node)

    # 获取节点的邻居（8方向）
    def get_neighbors(self, node):
        x, y = node
        directions = [
            (self.grid_size, 0), (-self.grid_size, 0),
            (0, self.grid_size), (0, -self.grid_size),
            (self.grid_size, self.grid_size), (-self.grid_size, -self.grid_size),
            (self.grid_size, -self.grid_size), (-self.grid_size, self.grid_size)
        ]
        neighbors = []
        for dx, dy in directions:
            nx, ny = x + dx, y + dy
            # 限制坐标范围
            if 0 <= nx <= 10 and 0 <= ny <= 10:
                neighbors.append((nx, ny))
        return neighbors

    # 检查节点是否碰撞
    def is_collision(self, node):
        for (ox, oy) in self.obstacles:
            if self.distance(node, (ox, oy)) < self.obstacle_radius:
                return True
        return False

    # 初始化D*算法
    def initialize(self, start, goal, initial_obstacles):
        self.start = start
        self.goal = goal
        self.current = start
        self.obstacles = set(initial_obstacles)
        
        # 初始化代价
        for x in np.arange(0, 10.1, self.grid_size):
            for y in np.arange(0, 10.1, self.grid_size):
                node = (round(x, 1), round(y, 1))
                self.init_node(node)
        
        # 终点的rhs值为0
        self.rhs[self.goal] = 0
        self.insert(self.goal)
        
        # 初始化可视化
        self.init_visualization()

    # 主规划循环（计算最短路径）
    def compute_shortest_path(self):
        while True:
            if not self.open_list:
                break  # 开放列表为空
            
            # 获取优先级最高的节点
            top_item = heapq.heappop(self.open_list)
            current_node = top_item.node
            current_key = top_item.priority
            new_key = self.calculate_key(current_node)
            
            # 如果当前节点的key已过期，跳过
            if current_key > new_key:
                self.insert(current_node)
                continue
            
            # 如果g > rhs（需要更新g值）
            if self.g[current_node] > self.rhs[current_node]:
                self.g[current_node] = self.rhs[current_node]
                # 更新所有邻居
                for neighbor in self.get_neighbors(current_node):
                    self.update_vertex(neighbor)
            # 如果g < rhs（需要提高代价）
            else:
                self.g[current_node] = float('inf')
                self.update_vertex(current_node)
                for neighbor in self.get_neighbors(current_node):
                    self.update_vertex(neighbor)
            
            # 检查起点是否已收敛
            if self.current == self.start and self.g[self.start] == self.rhs[self.start]:
                break

    # 检测新障碍物（模拟机器人感知）
    def detect_new_obstacle(self, obstacle):
        if obstacle not in self.obstacles:
            self.obstacles.add(obstacle)
            # 更新受影响的节点
            for x in np.arange(obstacle[0]-2, obstacle[0]+2.1, self.grid_size):
                for y in np.arange(obstacle[1]-2, obstacle[1]+2.1, self.grid_size):
                    node = (round(x, 1), round(y, 1))
                    if 0 <= x <= 10 and 0 <= y <= 10:
                        self.update_vertex(node)
            # 重新规划路径
            self.compute_shortest_path()
            # 更新可视化
            self.add_obstacle_visualization(obstacle)

    # 生成当前路径
    def get_path(self):
        path = [self.current]
        current_node = self.current
        
        while current_node != self.goal:
            min_cost = float('inf')
            next_node = None
            # 选择代价最小的邻居
            for neighbor in self.get_neighbors(current_node):
                if not self.is_collision(neighbor):
                    cost = self.g[neighbor] + self.distance(current_node, neighbor)
                    if cost < min_cost:
                        min_cost = cost
                        next_node = neighbor
            
            if next_node is None or next_node in path:
                break  # 无可行路径
            
            path.append(next_node)
            current_node = next_node
        
        return path

    # 初始化可视化
    def init_visualization(self):
        plt.ion()
        self.fig, self.ax = plt.subplots(figsize=(8, 8))
        
        # 绘制初始障碍物
        for (ox, oy) in self.obstacles:
            patch = patches.Circle((ox, oy), self.obstacle_radius, color='r', alpha=0.3)
            self.ax.add_patch(patch)
            self.obstacle_patches.append(patch)
        
        # 绘制起点和终点
        self.ax.scatter(self.start[0], self.start[1], color='green', s=100, label='Start')
        self.ax.scatter(self.goal[0], self.goal[1], color='red', s=100, label='Goal')
        
        # 初始化路径线条
        self.path_line = self.ax.plot([], [], 'b-', linewidth=2, label='D* Path')[0]
        self.node_scatter = self.ax.scatter([], [], color='cyan', s=20, alpha=0.5, label='Explored Nodes')
        
        # 图表设置
        self.ax.set_xlabel('X (m)')
        self.ax.set_ylabel('Y (m)')
        self.ax.set_title('D* Path Planning (Dynamic Environment)')
        self.ax.legend(loc='upper left')
        self.ax.grid(True)
        self.ax.axis('equal')
        self.ax.set_xlim(0, 10)
        self.ax.set_ylim(0, 10)
        
        plt.draw()
        plt.pause(0.1)

    # 添加新障碍物的可视化
    def add_obstacle_visualization(self, obstacle):
        patch = patches.Circle((obstacle[0], obstacle[1]), self.obstacle_radius, color='r', alpha=0.3)
        self.ax.add_patch(patch)
        self.obstacle_patches.append(patch)
        plt.draw()
        plt.pause(0.5)

    # 更新路径可视化
    def update_visualization(self, path):
        if path:
            path_x = [p[0] for p in path]
            path_y = [p[1] for p in path]
            self.path_line.set_data(path_x, path_y)
        
        # 绘制探索过的节点
        explored_x = [node[0] for node in self.g if self.g[node] < float('inf')]
        explored_y = [node[1] for node in self.g if self.g[node] < float('inf')]
        self.node_scatter.set_offsets(np.c_[explored_x, explored_y])
        
        plt.draw()
        plt.pause(0.1)

# -------------------------- 测试代码 --------------------------
if __name__ == "__main__":
    # 1. 初始化D*规划器
    dstar = DStarPlanner(grid_size=0.5, obstacle_radius=1.0)
    
    # 2. 设置起点、终点和初始障碍物
    start = (0.0, 0.0)
    goal = (8.0, 8.0)
    initial_obstacles = [(3.0, 3.0), (5.0, 5.0)]
    
    # 3. 初始化并执行初始规划
    dstar.initialize(start, goal, initial_obstacles)
    dstar.compute_shortest_path()
    
    # 4. 获取初始路径并可视化
    path = dstar.get_path()
    dstar.update_visualization(path)
    print("初始路径长度:", len(path))
    print("初始路径前5个节点:", path[:5])
    
    # 5. 模拟动态环境：发现新障碍物
    print("\n检测到新障碍物 (7.0, 3.0)，开始重规划...")
    dstar.detect_new_obstacle((7.0, 3.0))
    
    # 6. 获取重规划后的路径并可视化
    new_path = dstar.get_path()
    dstar.update_visualization(new_path)
    print("重规划后路径长度:", len(new_path))
    print("重规划后路径前5个节点:", new_path[:5])
    
    # 保持可视化窗口
    plt.ioff()
    plt.show()

代码核心说明

1. 核心数据结构：

• ggg：节点到终点的实际代价（全局代价）
• rhsrhsrhs：节点的局部一致代价（D * 的核心，用于增量更新）
• 优先级队列：基于(k1, k2)排序，保证每次处理最优节点

2. 关键方法：

• update_vertex()：更新节点的局部代价，是增量更新的核心
• compute_shortest_path()：主规划循环，计算最短路径
• detect_new_obstacle()：模拟动态环境，检测新障碍物并触发重规划
• get_path()：从当前位置生成最优路径

3. 可视化效果：

• 初始路径：蓝色线条（从起点到终点）
• 障碍物：红色半透明圆形
• 探索节点：青色小点
• 动态效果：添加新障碍物后，路径会自动绕开

四、运行代码的前置条件

1. 安装依赖库：

pip install numpy matplotlib

2. 运行环境：Python 3.6+
3. 无需额外配置，直接运行即可看到动态效果

五、结束语

1. D * 核心特性：

• 反向搜索（终点→起点），增量式更新，适合动态环境
• 通过g和rhs两个代价函数实现局部更新，避免全局重算
• 优先级队列保证每次处理最关键的节点，效率远高于重新规划

2. 使用要点：

• 适合机器人导航、自动驾驶等需要实时避障的场景
• 核心优势是环境变化后的重规划效率极高
• 实现关键是正确维护g/rhs代价和优先级队列

3. 与 A * 的选择原则：

• 静态环境选 A*（实现简单、效率高）
• 动态环境选 D*（增量更新，重规划快）