路径规划算法实战：Dijkstra、A*与D*的Matlab实现

1. 路径规划算法入门实战：从原理到Matlab实现

路径规划是机器人导航、游戏AI和物流优化的核心技术，就像在超市里寻找最短路线避开拥挤区域一样实用。本文将手把手带你实现三种经典算法：Dijkstra、A和D，用Matlab代码演示如何让智能体在复杂环境中找到最优路径。

提示：本文所有代码均采用模块化设计，每个函数不超过30行，方便新手理解和调试。建议边阅读边在Matlab中实践。

1.1 算法选型指南

三种算法各有适用场景：

• Dijkstra：当不知道目标位置时（如火灾逃生场景），它会均匀扩散搜索整个地图
• A*：已知目标位置时的最优选择，通过启发式函数加速搜索
• D*：动态环境的首选，当障碍物突然出现时能快速调整路径

2. A*算法深度解析与实现

2.1 算法核心原理

A*算法可以理解为"有方向性的Dijkstra"，其核心代价函数为：

code复制f(n) = g(n) + h(n)

其中：

• g(n)：从起点到节点n的实际成本
• h(n)：从节点n到终点的预估成本（启发函数）

2.1.1 启发函数设计技巧

曼哈顿距离是最常用的启发函数：

matlab复制function h = heuristic(a, b)
    % 适用于只能四方向移动的网格
    h = abs(a(1)-b(1)) + abs(a(2)-b(2)); 
end

如果允许斜向移动，建议使用对角距离：

matlab复制dx = abs(a(1)-b(1));
dy = abs(a(2)-b(2));
h = (dx + dy) + (sqrt(2)-2)*min(dx,dy);  % 对角线成本为√2≈1.414

注意：启发函数必须满足可采纳性（永远不高估实际成本），否则可能找到次优路径。

2.2 完整Matlab实现

matlab复制function path = Astar(start, goal, grid)
    % 开放列表存储[行,列,实际成本