尧图网页设计 
1. 项目概述从寻路到动态规划两种算法的核心分野在机器人导航、游戏AI、物流路径规划乃至我们日常使用的地图App里路径搜索算法是让机器“认识路”的基石。从业十多年从早期的RTS游戏寻路到如今的自动驾驶局部规划我反复与各种搜索算法打交道。其中A*A-Star和D*Dynamic A-Star是两座绕不开的里程碑。它们名字相似都带个“星号”但解决的问题场景和设计哲学却截然不同。A算法更像是你出发前用手机地图规划好的一条从家到公司的最优路线它基于一张已知、静态的地图进行计算而D算法则像是一个老司机在拥堵的早高峰里开车他最初也有个计划路线但当前方突然出现事故封路环境动态变化时他能立刻、高效地重新规划出一条新路线而不是傻傻地退回起点重新导航。简单来说A是静态环境下的最优路径规划器而D是动态或部分未知环境下的增量式重规划器。理解这个根本区别是掌握它们的关键。网络上关于A和D的讨论很多但往往要么过于理论化充斥着数学公式让人望而生畏要么过于零散只讲操作不讲背后的“为什么”。这篇文章我将结合自己踩过的坑和实战经验为你彻底拆解这两种算法的原理、实现细节以及最重要的——它们各自的应用场景和选择依据。无论你是算法初学者还是需要在项目中做技术选型的工程师希望这篇深度解读能给你带来直接可用的洞见。2. 核心原理深度拆解启发式搜索与增量式智慧要理解A和D不能只停留在“怎么用”的层面必须深入其设计哲学和数学模型。这就像学开车不仅要会踩油门刹车还得懂点发动机原理遇到故障时才知道从哪排查。2.1 A*算法启发式引导的全局最优搜索A*算法的核心思想非常直观它不想像无头苍蝇般的广度优先搜索BFS那样盲目探索也不想像深度优先搜索DFS那样一条路走到黑。它很“聪明”在每一步选择下一个要探索的节点时都会做一个综合评估。这个评估由一个代价函数f(n) g(n) h(n)决定。这是A*的灵魂公式务必吃透g(n)从起点到当前节点n的实际代价。这是确切的、已经发生的成本。h(n)从当前节点n到目标点的估计代价。这就是“启发式”Heuristic函数是算法“聪明”与否的关键。f(n)通过当前节点n到达目标点的估计总代价。A*总是优先探索f(n)值最小的节点认为这条路最有希望最快到达终点。这里的关键在于启发式函数h(n)的选择。它必须满足一个黄金法则可采纳性Admissibility即h(n)永远不能高估从节点n到目标的实际代价。在网格地图中最常用的就是曼哈顿距离只允许上下左右移动或欧几里得距离允许斜向移动。因为它们都是两点间的直线距离在实际有障碍物的环境中最短路径不可能比直线更短所以绝不会高估。注意如果h(n)高估了实际代价A*就无法保证找到的路径是最优的。但一个更紧贴实际、但绝不超出的h(n)能极大地提升搜索效率。这就是为什么在游戏中对于允许斜向移动的场景欧几里得距离通常比曼哈顿距离效果更好因为它更接近真实代价引导性更强。A*的工作流程可以概括为以下几步我习惯用两个集合来思考开放列表Open List和关闭列表Closed List。初始化将起点加入开放列表。主循环 a. 从开放列表中取出f(n)值最小的节点n作为当前节点。 b. 如果节点n就是目标点则路径找到反向回溯即可。 c. 将节点n从开放列表移至关闭列表表示已探索过。 d. 遍历节点n的所有邻居节点。 e. 对于每个邻居节点m * 如果m在关闭列表中忽略它。 * 计算从起点经过n到m的临时g_temp(m) g(n) cost(n, m)cost是移动代价。 * 如果m不在开放列表中或者这个新的g_temp(m)比它之前记录的g(m)更小发现了一条到m的更优路径那么 * 更新m的g(m)为g_temp(m)。 * 计算m的f(m) g(m) h(m)。 * 记录m的父节点为n用于最终路径回溯。 * 如果m是新节点将其加入开放列表。如果开放列表为空说明没有可行路径。这个过程保证了A*第一次扩展到目标节点时找到的路径一定是全局最优的在启发函数可采纳的前提下。它的搜索过程像是一滴有智慧的墨水从起点开始优先朝着目标的大致方向“渗透”过去。2.2 D*算法面对变化的从容应对者现在考虑一个动态场景你正按照A规划的最优路径走向目标突然发现前方原本通畅的路被一堵新出现的墙挡住了。一个朴素的想法是在原地重新运行一次A。但这意味着抛弃之前所有的计算成果从头开始搜索在大型地图或实时性要求高的场景如机器人中这是无法接受的性能开销。D算法这里主要指经典的DLite算法它是目前最实用高效的版本就是为了解决这个问题而生。它的核心智慧是增量式重规划和反向搜索。1. 反向搜索与“乐观估计”D* Lite的第一个反直觉设计是它从目标点Goal开始向起点Start搜索。为什么因为在动态环境中障碍物变化通常发生在机器人周围当前位置附近。从目标点开始搜索我们可以预先计算好从地图上任何一点到目标点的“估计代价”这个代价被记为rhs(s)right-hand side。同时每个节点还有一个键值Key用于优先级排序。rhs(s)的计算基于一个“乐观假设”假设节点s的所有后继节点即s的邻居中能到达目标的方向到目标的最优路径代价已知那么rhs(s)就等于从s到这些后继节点的代价加上后继节点本身的代价中的最小值。初始化时只有目标点的rhs(goal)0。2. 键值Key与优先级队列D* Lite不像A*那样直接比较f值。它使用一个二维的键值K(s) [k1(s), k2(s)]来对节点进行优先级排序。其中k1(s) min(g(s), rhs(s)) h(s, start)。注意这里的启发式是到当前起点的距离因为我们是反向搜索。k2(s) min(g(s), rhs(s))。优先级队列按照k1主序、k2次序进行排序。这个设计精妙地统一了对新路径的探索和对旧路径的更新。3. 增量式更新当变化发生时这是D*最精彩的部分。当机器人移动并发现某条边如从节点u到节点v的代价发生变化时例如成本增加表示出现障碍算法会更新受影响的节点这里是节点u的rhs值。然后将节点u可能还有其前驱节点以新的键值重新插入优先级队列。接着算法运行一个主循环不断从队列中取出键值最小的节点进行处理局部地传播这个代价变化的影响直到队列中所有节点的状态都“一致”即对于所有节点s有g(s) rhs(s)并且当前机器人所在节点的键值不比队列中任何节点的键值更优。这意味着局部重规划完成找到了从当前位置到目标的新最优路径。这个过程只重新计算了受障碍影响的那一小部分地图的代价而不是全图因此效率极高。机器人可以几乎不间断地沿着新路径继续前进。个人解读你可以把A看作一个完美的离线规划师而D则是一个老练的在线执行者。A在行动前给你一张完美的蓝图D则是一边行动一边手握橡皮擦和铅笔随时根据眼前看到的新情况修改蓝图而且修改得又快又省力。在真实、开放、动态的世界里D*所代表的增量式重规划思想其价值往往远超一次性的最优规划。3. 算法实现与关键细节剖析理解了原理我们来看看如何把它们变成代码。这里我会用一些伪代码和关键代码段来说明并指出那些容易踩坑的细节。3.1 A*算法的实现要点与优化技巧一个基础的A*实现并不复杂但工业级的应用需要考虑很多优化。1. 数据结构的选择开放列表Open List需要频繁取出f值最小的节点因此一个优先队列最小堆是必然选择。在Python中可以用heapq模块在C中用std::priority_queue。关闭列表Closed List需要快速判断节点是否已被探索。一个哈希集合HashSet是最佳选择如Python的set()C的std::unordered_set。记录父节点和g值通常用一个字典/哈希表node - parent_node和另一个字典node - g_value来存储。2. 启发式函数h(n)的实现# 假设节点用(x, y)坐标表示 def manhattan_distance(node, goal): return abs(node.x - goal.x) abs(node.y - goal.y) def euclidean_distance(node, goal): return math.sqrt((node.x - goal.x)**2 (node.y - goal.y)**2) # 对于允许斜向移动且代价为1四方向和1.414斜方向的网格 # 一个更好的启发式是切比雪夫距离或对角线距离 def diagonal_distance(node, goal): dx abs(node.x - goal.x) dy abs(node.y - goal.y) # D是直线移动代价D2是对角线移动代价 D 1 D2 math.sqrt(2) return D * (dx dy) (D2 - 2 * D) * min(dx, dy)选择哪种距离直接影响到算法扩展节点的数量和“方向感”。3. 处理平局Tie-Breaking当开放列表中有多个节点f值相同时先处理哪一个这会影响搜索效率和最终路径的“美观度”。一个常见的技巧是修改启发式函数使其具有轻微的倾向性。例如给h(n)乘以一个略大于1的因子(1.0 p)其中p是一个很小的数如0.001。或者在优先队列中当f值相同时比较节点的h值或g值。这能引导算法更倾向于探索离目标更近的节点减少不必要的扩展。4. 路径回溯找到目标后通过不断访问每个节点的“父节点”指针从目标反向追溯到起点再将序列反转就得到了从起点到目标的路径。实操心得在游戏开发中我们经常对A做“后处理”。比如对找出的路径进行拉直String Pulling或平滑Smoothing处理让角色移动轨迹更自然而不是僵硬的网格折线。此外对于大地图直接应用A可能很慢常采用分层路径规划Hierarchical Pathfinding比如先在地图区块Room级别规划再在每个区块内详细规划。3.2 D* Lite算法的实现框架D* Lite的实现比A*复杂因为它要维护节点的两种代价g和rhs以及一个复杂的键值比较逻辑。以下是其最核心的流程框架1. 数据结构优先队列U存储待处理的节点按键值K排序。计数值k_m用于在机器人移动后调整键值计算中的启发式部分这是实现“反向搜索”随起点移动而调整的关键。映射g和rhs存储每个节点的g值和rhs值。2. 关键函数CalculateKey(s): 计算节点s的键值。def CalculateKey(s): # k_m 是起点移动后的全局偏移量 return [min(g[s], rhs[s]) h(s, start) k_m, min(g[s], rhs[s])]UpdateVertex(u): 当节点u的rhs或邻居发生变化时调用更新u在队列中的状态。def UpdateVertex(u): if g[u] ! rhs[u]: # 状态不一致 if u in U: U.update(u, CalculateKey(u)) else: U.insert(u, CalculateKey(u)) else: if u in U: U.remove(u)ComputeShortestPath(): 核心的主循环直到队列为空或满足终止条件。def ComputeShortestPath(): while U.topKey() CalculateKey(start) or rhs[start] ! g[start]: u U.pop() if g[u] rhs[u]: # 过一致状态Overconsistent g[u] rhs[u] for s in predecessors(u): # 更新所有前驱节点 rhs[s] min(rhs[s], cost(s, u) g[u]) UpdateVertex(s) else: # 欠一致状态Underconsistent g[u] INFINITY for s in predecessors(u) [u]: # 更新前驱和自身 if rhs[s] cost(s, u) g[u]: # 重新计算rhs[s]因为其最优后继可能变了 rhs[s] min over s in successors(s) of (cost(s, s) g[s]) UpdateVertex(s)3. 主流程初始化设置所有节点的g和rhs为无穷大rhs[goal]0将目标点加入队列U然后运行ComputeShortestPath()。此时算法计算出了从各点到目标的初始路径代价。机器人移动与重规划 a. 机器人沿当前路径根据g值梯度下降移动一步。 b. 如果检测到边代价变化如发现新障碍更新该边的代价c(u, v)。 c. 更新受影响的节点如节点u的rhs值。 d. 调用UpdateVertex(u)及其可能的前驱。 e. 如果机器人位置新的起点的代价需要更新则增加k_mk_m h(old_start, new_start)这相当于将所有键值中的启发式部分进行平移以适应起点的移动。 f. 再次调用ComputeShortestPath()进行高效的局部重规划。踩坑记录实现D* Lite时最容易出错的地方在于键值的比较逻辑和节点状态的维护。必须严格保证优先队列的排序顺序与论文定义一致。另一个坑是启发式函数的一致性。D* Lite要求启发式函数满足一致性即三角不等式否则可能无法保证正确性。曼哈顿距离和欧氏距离都满足。在动态更新k_m时务必使用上一次的起点和新的起点来计算启发式差值这是保证算法在起点移动后仍能正确工作的关键。4. 应用场景对比与选型指南了解了原理和实现我们最终要回答一个实际问题我的项目到底该用A还是D选择不当要么是杀鸡用牛刀带来不必要的复杂度要么是牛刀杀鸡系统根本无法应对真实环境。4.1 A*算法的典型应用场景A*适用于环境信息完全已知、静态不变的场合。它的优势是原理简单、实现直观、易于优化且能保证找到最优路径。游戏AI离线寻路在大多数策略游戏如《星际争霸》、《文明》中地图是固定的单位在移动前进行一次性路径规划。A*是绝对的主流配合地图预处理如导航网格NavMesh和空间划分效率极高。物流仓储静态路径规划在仓库管理系统WMS中为AGV自动导引车规划从货架A到拣货点B的固定路线地图格局已知A*非常适合。地图导航应用路线规划当你输入起点和终点后服务器端在完整的道路网络数据上运行A或其变种如考虑实时交通的加权A规划出一条静态路线。虽然交通状况动态变化但基础路网是静态的一次规划足以应对。拼图游戏如八数码求解将游戏状态视为节点状态转移视为边A*可以高效地找到最短解序列。选型信号如果你的地图在规划前后不会改变或者变化频率极低以分钟、小时计且重新进行全局规划的成本可以接受那么A*通常是更简单、更高效的选择。4.2 D*算法的典型应用场景D*适用于环境部分未知、动态变化且需要高频、实时重规划的场合。它的核心价值在于增量更新的效率。移动机器人实时导航这是D的经典战场。机器人在未知环境中探索SLAM需要一边建图一边规划。当传感器激光雷达、摄像头发现前方有未知障碍如突然出现的人、移开的椅子时D可以快速局部调整路径让机器人绕行而不是停下来全图重算。无人机在复杂空域的避障飞行空域中可能存在突然出现的其他飞行器或临时禁飞区。D*能让无人机快速、平滑地调整航迹。实时战略游戏RTS中的单位微操当一大群单位集体移动时如果某个单位被卡住或路线被建筑、其他单位临时阻挡使用D*思想可以为该单位快速计算一条局部绕行路径而不影响整个队伍的移动指令。自动驾驶汽车的局部轨迹重规划在跟踪全局路由的同时车辆需要应对突然切入的车辆、行人或道路施工。D或其变种如DLite的衍生算法可用于在极短时间内生成一条安全、舒适的新局部轨迹。选型信号如果你的系统需要持续应对高频、局部的环境变化并且对重规划的计算延迟有严格要求例如要求毫秒级响应那么D或其现代变种如DLite, Field D*几乎是必选项。4.3 性能与复杂度对比特性维度A* 算法D* (D* Lite) 算法规划类型一次性全局规划增量式重规划环境假设完全已知、静态部分未知、动态搜索方向前向起点 - 目标反向目标 - 起点随起点调整时间复杂度O(b^d)b为分支因子d为深度初始规划同A*重规划通常远快于重新运行A*空间复杂度需要存储开放列表和关闭列表除A*所需外还需维护每个节点的rhs值和更复杂的队列状态最优性保证是启发函数可采纳是在动态变化后重规划结果对当前已知信息最优实现难度相对简单相对复杂状态管理和键值逻辑容易出错适用场景地图编辑、游戏寻路、离线导航机器人实时导航、无人机避障、游戏动态避障个人经验法则静态环境无脑A*。它的生态成熟优化技巧多如JPS跳点搜索能极大加速网格寻路。动态环境但变化是全局的、低频的比如一天变几次可以用A*定期重跑配合简单的局部碰撞反应如势场法避让通常就够了。动态环境且变化是局部的、高频的比如机器人每秒都在感知新障碍必须上D*或其变种。虽然实现复杂但它是保证系统实时性和鲁棒性的基石。如果环境完全未知D需要与SLAM同步定位与建图结合。通常的模式是SLAM不断更新全局/局部地图D基于最新的地图进行规划。此时地图更新的频率和规划频率需要仔细权衡。5. 常见问题、调试技巧与进阶思考即使理解了算法在实际编码和调试中也会遇到各种问题。这里分享一些我积累的实战经验和排查思路。5.1 A*算法常见问题排查问题算法永远找不到路径即使路径明显存在。检查启发函数首先确认你的启发函数h(n)是否可采纳。如果h(n)可能高估真实代价A*就可能错过最优路径甚至找不到路径。确保你使用的距离度量对于你的移动方式是合理的例如在允许斜向移动的网格中使用曼哈顿距离会严重高估可能导致问题。检查障碍物处理确认你的“邻居节点”生成逻辑是否正确过滤了障碍物。一个常见的错误是在遍历邻居时没有检查该邻居节点本身是否是障碍物或者从当前节点移动到邻居节点的边是否被障碍物阻挡对于斜向移动需要检查对角线方向的两个相邻格子是否同时可通行。检查数据结构确保你的开放列表优先队列能正确更新节点的f值。如果你发现一个已在开放列表中的节点其g值被更新得更小你必须调整该节点在优先队列中的位置这需要支持降低键值操作的优先队列或采用“延迟删除”策略将新节点重复插入当从队列中取出时检查其g值是否最新。问题算法能找到路径但路径看起来“很傻”或者不是最短的。平局打破策略这通常是由于f值相同时的处理顺序导致的。尝试实现一个次要比较器比如当f值相同时优先选择h值更小的节点更靠近目标这样搜索会更“贪婪”路径往往更直接。移动代价不对等检查你的cost(n, m)函数。如果对角线移动的代价不是sqrt(2)而是1那么A*找到的“最短路径”在欧氏距离意义上可能就不是真正的直线最短。确保移动代价与你期望的路径最优性定义一致。后处理A在网格上找到的是网格中心的连线。对于可视化或角色移动这条路径可能有很多不必要的拐角。可以在A之后运行一个简单的路径平滑算法比如检查路径中非相邻的三点如果中间点可以去掉即起点和终点直线可达则去掉中间点。问题算法在大地图上运行非常慢。优化启发式使用更紧贴真实代价的启发式如对角线距离代替曼哈顿距离可以显著减少扩展的节点数量。使用JPSJump Point Search对于均匀网格地图JPS是A*的革命性优化它通过“跳点”跳过大量不必要的节点能将搜索速度提升一个数量级甚至更多。分层路径规划将大地图划分为多个区域如房间、街区。先在高层次进行区域间的A规划再在每个区域内进行详细的A规划。这牺牲了绝对最优性但换来了巨大的性能提升。5.2 D* Lite算法调试心得调试D* Lite比A*更具挑战性因为它的状态更多样。问题机器人停滞不前或者路径在障碍物变化后更新异常。键值计算与比较这是最可能出错的地方。务必用单元测试验证你的CalculateKey(s)函数和优先队列的比较器确保它们与论文中的定义完全一致。特别是k_m的更新逻辑k_m h(last_start, current_start)这里的h必须是一致的启发式函数。节点状态一致性理解g(s) rhs(s)一致、g(s) rhs(s)过一致、g(s) rhs(s)欠一致这三种状态。在ComputeShortestPath循环中对过一致和欠一致节点的处理逻辑完全不同必须严格实现。可以在关键节点打印出g和rhs值观察其变化是否符合预期。邻居与代价更新范围当一条边(u, v)的代价增加出现障碍时不仅需要更新节点u还需要更新所有将u作为最优后继的节点即那些满足rhs(s) cost(s, u) g(u)的节点s。漏掉这些节点的更新会导致算法“不知道”某些路径已经失效。问题初始规划就失败了。首先将环境设置为完全静态并关闭机器人的移动和动态更新。此时D* Lite应该退化为一个从目标到起点的反向A*。用A算法从起点到目标正向运行的结果与DLite规划出的路径代价应该完全一致。这是一个非常重要的交叉验证手段。检查目标点的初始化rhs[goal]必须设置为0并且UpdateVertex(goal)后目标点应被加入优先队列。5.3 进阶思考与扩展在实际项目中纯粹的A或DLite可能还需要与其他技术结合。与势场法/向量场直方图VFH结合在密集动态障碍物环境中如人潮涌动的广场仅靠路径规划可能不够。可以先用D*规划一条宏观路径再结合局部避障算法如动态窗口法DWA、人工势场法进行实时微调处理突然逼近的移动障碍物。考虑动力学约束对于汽车、无人机等非全向移动的机器人规划出的路径必须是运动学可行的。这催生了诸如Hybrid A*在状态空间x, y, θ中搜索、State Lattice Planning等算法。D也可以扩展到状态空间成为Kinodynamic D但复杂度会急剧上升。非最优但更快的选择有时我们并不需要绝对最优路径而是需要非常快的规划速度。这时可以牺牲最优性来换取速度例如使用Weighted A*给启发式函数加一个大于1的权重使其更“贪婪”或者RRT快速随机搜索树系列算法。在动态环境中也有ADAnytime D** 这样的算法它先快速找到一个可行解然后利用剩余时间不断优化。最后我想强调的是没有“最好”的算法只有“最合适”的算法。A和D代表了路径规划中两种核心思想全局最优规划和增量式重规划。掌握它们的原理和实现细节就像工具箱里有了两把得心应手的扳手。当你面对一个新的路径规划问题时首先要问的不是“我用哪个算法”而是“我的环境是静态还是动态”、“我的计算资源有多少”、“我对路径的最优性要求有多高”。想清楚了这些问题技术选型自然就清晰了。在我经历的项目中往往是多种算法的混合体在起作用例如用A做全局粗规划用DLite做局部细规划和重规划再用一些简单的规则处理突发碰撞。这种分层、混合的策略往往是工程实践中最稳健、最有效的解决方案。 A*与D*算法深度解析：从静态寻路到动态重规划的核心差异与应用
1. 项目概述从寻路到动态规划两种算法的核心分野在机器人导航、游戏AI、物流路径规划乃至我们日常使用的地图App里路径搜索算法是让机器“认识路”的基石。从业十多年从早期的RTS游戏寻路到如今的自动驾驶局部规划我反复与各种搜索算法打交道。其中A*A-Star和D*Dynamic A-Star是两座绕不开的里程碑。它们名字相似都带个“星号”但解决的问题场景和设计哲学却截然不同。A算法更像是你出发前用手机地图规划好的一条从家到公司的最优路线它基于一张已知、静态的地图进行计算而D算法则像是一个老司机在拥堵的早高峰里开车他最初也有个计划路线但当前方突然出现事故封路环境动态变化时他能立刻、高效地重新规划出一条新路线而不是傻傻地退回起点重新导航。简单来说A是静态环境下的最优路径规划器而D是动态或部分未知环境下的增量式重规划器。理解这个根本区别是掌握它们的关键。网络上关于A和D的讨论很多但往往要么过于理论化充斥着数学公式让人望而生畏要么过于零散只讲操作不讲背后的“为什么”。这篇文章我将结合自己踩过的坑和实战经验为你彻底拆解这两种算法的原理、实现细节以及最重要的——它们各自的应用场景和选择依据。无论你是算法初学者还是需要在项目中做技术选型的工程师希望这篇深度解读能给你带来直接可用的洞见。2. 核心原理深度拆解启发式搜索与增量式智慧要理解A和D不能只停留在“怎么用”的层面必须深入其设计哲学和数学模型。这就像学开车不仅要会踩油门刹车还得懂点发动机原理遇到故障时才知道从哪排查。2.1 A*算法启发式引导的全局最优搜索A*算法的核心思想非常直观它不想像无头苍蝇般的广度优先搜索BFS那样盲目探索也不想像深度优先搜索DFS那样一条路走到黑。它很“聪明”在每一步选择下一个要探索的节点时都会做一个综合评估。这个评估由一个代价函数f(n) g(n) h(n)决定。这是A*的灵魂公式务必吃透g(n)从起点到当前节点n的实际代价。这是确切的、已经发生的成本。h(n)从当前节点n到目标点的估计代价。这就是“启发式”Heuristic函数是算法“聪明”与否的关键。f(n)通过当前节点n到达目标点的估计总代价。A*总是优先探索f(n)值最小的节点认为这条路最有希望最快到达终点。这里的关键在于启发式函数h(n)的选择。它必须满足一个黄金法则可采纳性Admissibility即h(n)永远不能高估从节点n到目标的实际代价。在网格地图中最常用的就是曼哈顿距离只允许上下左右移动或欧几里得距离允许斜向移动。因为它们都是两点间的直线距离在实际有障碍物的环境中最短路径不可能比直线更短所以绝不会高估。注意如果h(n)高估了实际代价A*就无法保证找到的路径是最优的。但一个更紧贴实际、但绝不超出的h(n)能极大地提升搜索效率。这就是为什么在游戏中对于允许斜向移动的场景欧几里得距离通常比曼哈顿距离效果更好因为它更接近真实代价引导性更强。A*的工作流程可以概括为以下几步我习惯用两个集合来思考开放列表Open List和关闭列表Closed List。初始化将起点加入开放列表。主循环 a. 从开放列表中取出f(n)值最小的节点n作为当前节点。 b. 如果节点n就是目标点则路径找到反向回溯即可。 c. 将节点n从开放列表移至关闭列表表示已探索过。 d. 遍历节点n的所有邻居节点。 e. 对于每个邻居节点m * 如果m在关闭列表中忽略它。 * 计算从起点经过n到m的临时g_temp(m) g(n) cost(n, m)cost是移动代价。 * 如果m不在开放列表中或者这个新的g_temp(m)比它之前记录的g(m)更小发现了一条到m的更优路径那么 * 更新m的g(m)为g_temp(m)。 * 计算m的f(m) g(m) h(m)。 * 记录m的父节点为n用于最终路径回溯。 * 如果m是新节点将其加入开放列表。如果开放列表为空说明没有可行路径。这个过程保证了A*第一次扩展到目标节点时找到的路径一定是全局最优的在启发函数可采纳的前提下。它的搜索过程像是一滴有智慧的墨水从起点开始优先朝着目标的大致方向“渗透”过去。2.2 D*算法面对变化的从容应对者现在考虑一个动态场景你正按照A规划的最优路径走向目标突然发现前方原本通畅的路被一堵新出现的墙挡住了。一个朴素的想法是在原地重新运行一次A。但这意味着抛弃之前所有的计算成果从头开始搜索在大型地图或实时性要求高的场景如机器人中这是无法接受的性能开销。D算法这里主要指经典的DLite算法它是目前最实用高效的版本就是为了解决这个问题而生。它的核心智慧是增量式重规划和反向搜索。1. 反向搜索与“乐观估计”D* Lite的第一个反直觉设计是它从目标点Goal开始向起点Start搜索。为什么因为在动态环境中障碍物变化通常发生在机器人周围当前位置附近。从目标点开始搜索我们可以预先计算好从地图上任何一点到目标点的“估计代价”这个代价被记为rhs(s)right-hand side。同时每个节点还有一个键值Key用于优先级排序。rhs(s)的计算基于一个“乐观假设”假设节点s的所有后继节点即s的邻居中能到达目标的方向到目标的最优路径代价已知那么rhs(s)就等于从s到这些后继节点的代价加上后继节点本身的代价中的最小值。初始化时只有目标点的rhs(goal)0。2. 键值Key与优先级队列D* Lite不像A*那样直接比较f值。它使用一个二维的键值K(s) [k1(s), k2(s)]来对节点进行优先级排序。其中k1(s) min(g(s), rhs(s)) h(s, start)。注意这里的启发式是到当前起点的距离因为我们是反向搜索。k2(s) min(g(s), rhs(s))。优先级队列按照k1主序、k2次序进行排序。这个设计精妙地统一了对新路径的探索和对旧路径的更新。3. 增量式更新当变化发生时这是D*最精彩的部分。当机器人移动并发现某条边如从节点u到节点v的代价发生变化时例如成本增加表示出现障碍算法会更新受影响的节点这里是节点u的rhs值。然后将节点u可能还有其前驱节点以新的键值重新插入优先级队列。接着算法运行一个主循环不断从队列中取出键值最小的节点进行处理局部地传播这个代价变化的影响直到队列中所有节点的状态都“一致”即对于所有节点s有g(s) rhs(s)并且当前机器人所在节点的键值不比队列中任何节点的键值更优。这意味着局部重规划完成找到了从当前位置到目标的新最优路径。这个过程只重新计算了受障碍影响的那一小部分地图的代价而不是全图因此效率极高。机器人可以几乎不间断地沿着新路径继续前进。个人解读你可以把A看作一个完美的离线规划师而D则是一个老练的在线执行者。A在行动前给你一张完美的蓝图D则是一边行动一边手握橡皮擦和铅笔随时根据眼前看到的新情况修改蓝图而且修改得又快又省力。在真实、开放、动态的世界里D*所代表的增量式重规划思想其价值往往远超一次性的最优规划。3. 算法实现与关键细节剖析理解了原理我们来看看如何把它们变成代码。这里我会用一些伪代码和关键代码段来说明并指出那些容易踩坑的细节。3.1 A*算法的实现要点与优化技巧一个基础的A*实现并不复杂但工业级的应用需要考虑很多优化。1. 数据结构的选择开放列表Open List需要频繁取出f值最小的节点因此一个优先队列最小堆是必然选择。在Python中可以用heapq模块在C中用std::priority_queue。关闭列表Closed List需要快速判断节点是否已被探索。一个哈希集合HashSet是最佳选择如Python的set()C的std::unordered_set。记录父节点和g值通常用一个字典/哈希表node - parent_node和另一个字典node - g_value来存储。2. 启发式函数h(n)的实现# 假设节点用(x, y)坐标表示 def manhattan_distance(node, goal): return abs(node.x - goal.x) abs(node.y - goal.y) def euclidean_distance(node, goal): return math.sqrt((node.x - goal.x)**2 (node.y - goal.y)**2) # 对于允许斜向移动且代价为1四方向和1.414斜方向的网格 # 一个更好的启发式是切比雪夫距离或对角线距离 def diagonal_distance(node, goal): dx abs(node.x - goal.x) dy abs(node.y - goal.y) # D是直线移动代价D2是对角线移动代价 D 1 D2 math.sqrt(2) return D * (dx dy) (D2 - 2 * D) * min(dx, dy)选择哪种距离直接影响到算法扩展节点的数量和“方向感”。3. 处理平局Tie-Breaking当开放列表中有多个节点f值相同时先处理哪一个这会影响搜索效率和最终路径的“美观度”。一个常见的技巧是修改启发式函数使其具有轻微的倾向性。例如给h(n)乘以一个略大于1的因子(1.0 p)其中p是一个很小的数如0.001。或者在优先队列中当f值相同时比较节点的h值或g值。这能引导算法更倾向于探索离目标更近的节点减少不必要的扩展。4. 路径回溯找到目标后通过不断访问每个节点的“父节点”指针从目标反向追溯到起点再将序列反转就得到了从起点到目标的路径。实操心得在游戏开发中我们经常对A做“后处理”。比如对找出的路径进行拉直String Pulling或平滑Smoothing处理让角色移动轨迹更自然而不是僵硬的网格折线。此外对于大地图直接应用A可能很慢常采用分层路径规划Hierarchical Pathfinding比如先在地图区块Room级别规划再在每个区块内详细规划。3.2 D* Lite算法的实现框架D* Lite的实现比A*复杂因为它要维护节点的两种代价g和rhs以及一个复杂的键值比较逻辑。以下是其最核心的流程框架1. 数据结构优先队列U存储待处理的节点按键值K排序。计数值k_m用于在机器人移动后调整键值计算中的启发式部分这是实现“反向搜索”随起点移动而调整的关键。映射g和rhs存储每个节点的g值和rhs值。2. 关键函数CalculateKey(s): 计算节点s的键值。def CalculateKey(s): # k_m 是起点移动后的全局偏移量 return [min(g[s], rhs[s]) h(s, start) k_m, min(g[s], rhs[s])]UpdateVertex(u): 当节点u的rhs或邻居发生变化时调用更新u在队列中的状态。def UpdateVertex(u): if g[u] ! rhs[u]: # 状态不一致 if u in U: U.update(u, CalculateKey(u)) else: U.insert(u, CalculateKey(u)) else: if u in U: U.remove(u)ComputeShortestPath(): 核心的主循环直到队列为空或满足终止条件。def ComputeShortestPath(): while U.topKey() CalculateKey(start) or rhs[start] ! g[start]: u U.pop() if g[u] rhs[u]: # 过一致状态Overconsistent g[u] rhs[u] for s in predecessors(u): # 更新所有前驱节点 rhs[s] min(rhs[s], cost(s, u) g[u]) UpdateVertex(s) else: # 欠一致状态Underconsistent g[u] INFINITY for s in predecessors(u) [u]: # 更新前驱和自身 if rhs[s] cost(s, u) g[u]: # 重新计算rhs[s]因为其最优后继可能变了 rhs[s] min over s in successors(s) of (cost(s, s) g[s]) UpdateVertex(s)3. 主流程初始化设置所有节点的g和rhs为无穷大rhs[goal]0将目标点加入队列U然后运行ComputeShortestPath()。此时算法计算出了从各点到目标的初始路径代价。机器人移动与重规划 a. 机器人沿当前路径根据g值梯度下降移动一步。 b. 如果检测到边代价变化如发现新障碍更新该边的代价c(u, v)。 c. 更新受影响的节点如节点u的rhs值。 d. 调用UpdateVertex(u)及其可能的前驱。 e. 如果机器人位置新的起点的代价需要更新则增加k_mk_m h(old_start, new_start)这相当于将所有键值中的启发式部分进行平移以适应起点的移动。 f. 再次调用ComputeShortestPath()进行高效的局部重规划。踩坑记录实现D* Lite时最容易出错的地方在于键值的比较逻辑和节点状态的维护。必须严格保证优先队列的排序顺序与论文定义一致。另一个坑是启发式函数的一致性。D* Lite要求启发式函数满足一致性即三角不等式否则可能无法保证正确性。曼哈顿距离和欧氏距离都满足。在动态更新k_m时务必使用上一次的起点和新的起点来计算启发式差值这是保证算法在起点移动后仍能正确工作的关键。4. 应用场景对比与选型指南了解了原理和实现我们最终要回答一个实际问题我的项目到底该用A还是D选择不当要么是杀鸡用牛刀带来不必要的复杂度要么是牛刀杀鸡系统根本无法应对真实环境。4.1 A*算法的典型应用场景A*适用于环境信息完全已知、静态不变的场合。它的优势是原理简单、实现直观、易于优化且能保证找到最优路径。游戏AI离线寻路在大多数策略游戏如《星际争霸》、《文明》中地图是固定的单位在移动前进行一次性路径规划。A*是绝对的主流配合地图预处理如导航网格NavMesh和空间划分效率极高。物流仓储静态路径规划在仓库管理系统WMS中为AGV自动导引车规划从货架A到拣货点B的固定路线地图格局已知A*非常适合。地图导航应用路线规划当你输入起点和终点后服务器端在完整的道路网络数据上运行A或其变种如考虑实时交通的加权A规划出一条静态路线。虽然交通状况动态变化但基础路网是静态的一次规划足以应对。拼图游戏如八数码求解将游戏状态视为节点状态转移视为边A*可以高效地找到最短解序列。选型信号如果你的地图在规划前后不会改变或者变化频率极低以分钟、小时计且重新进行全局规划的成本可以接受那么A*通常是更简单、更高效的选择。4.2 D*算法的典型应用场景D*适用于环境部分未知、动态变化且需要高频、实时重规划的场合。它的核心价值在于增量更新的效率。移动机器人实时导航这是D的经典战场。机器人在未知环境中探索SLAM需要一边建图一边规划。当传感器激光雷达、摄像头发现前方有未知障碍如突然出现的人、移开的椅子时D可以快速局部调整路径让机器人绕行而不是停下来全图重算。无人机在复杂空域的避障飞行空域中可能存在突然出现的其他飞行器或临时禁飞区。D*能让无人机快速、平滑地调整航迹。实时战略游戏RTS中的单位微操当一大群单位集体移动时如果某个单位被卡住或路线被建筑、其他单位临时阻挡使用D*思想可以为该单位快速计算一条局部绕行路径而不影响整个队伍的移动指令。自动驾驶汽车的局部轨迹重规划在跟踪全局路由的同时车辆需要应对突然切入的车辆、行人或道路施工。D或其变种如DLite的衍生算法可用于在极短时间内生成一条安全、舒适的新局部轨迹。选型信号如果你的系统需要持续应对高频、局部的环境变化并且对重规划的计算延迟有严格要求例如要求毫秒级响应那么D或其现代变种如DLite, Field D*几乎是必选项。4.3 性能与复杂度对比特性维度A* 算法D* (D* Lite) 算法规划类型一次性全局规划增量式重规划环境假设完全已知、静态部分未知、动态搜索方向前向起点 - 目标反向目标 - 起点随起点调整时间复杂度O(b^d)b为分支因子d为深度初始规划同A*重规划通常远快于重新运行A*空间复杂度需要存储开放列表和关闭列表除A*所需外还需维护每个节点的rhs值和更复杂的队列状态最优性保证是启发函数可采纳是在动态变化后重规划结果对当前已知信息最优实现难度相对简单相对复杂状态管理和键值逻辑容易出错适用场景地图编辑、游戏寻路、离线导航机器人实时导航、无人机避障、游戏动态避障个人经验法则静态环境无脑A*。它的生态成熟优化技巧多如JPS跳点搜索能极大加速网格寻路。动态环境但变化是全局的、低频的比如一天变几次可以用A*定期重跑配合简单的局部碰撞反应如势场法避让通常就够了。动态环境且变化是局部的、高频的比如机器人每秒都在感知新障碍必须上D*或其变种。虽然实现复杂但它是保证系统实时性和鲁棒性的基石。如果环境完全未知D需要与SLAM同步定位与建图结合。通常的模式是SLAM不断更新全局/局部地图D基于最新的地图进行规划。此时地图更新的频率和规划频率需要仔细权衡。5. 常见问题、调试技巧与进阶思考即使理解了算法在实际编码和调试中也会遇到各种问题。这里分享一些我积累的实战经验和排查思路。5.1 A*算法常见问题排查问题算法永远找不到路径即使路径明显存在。检查启发函数首先确认你的启发函数h(n)是否可采纳。如果h(n)可能高估真实代价A*就可能错过最优路径甚至找不到路径。确保你使用的距离度量对于你的移动方式是合理的例如在允许斜向移动的网格中使用曼哈顿距离会严重高估可能导致问题。检查障碍物处理确认你的“邻居节点”生成逻辑是否正确过滤了障碍物。一个常见的错误是在遍历邻居时没有检查该邻居节点本身是否是障碍物或者从当前节点移动到邻居节点的边是否被障碍物阻挡对于斜向移动需要检查对角线方向的两个相邻格子是否同时可通行。检查数据结构确保你的开放列表优先队列能正确更新节点的f值。如果你发现一个已在开放列表中的节点其g值被更新得更小你必须调整该节点在优先队列中的位置这需要支持降低键值操作的优先队列或采用“延迟删除”策略将新节点重复插入当从队列中取出时检查其g值是否最新。问题算法能找到路径但路径看起来“很傻”或者不是最短的。平局打破策略这通常是由于f值相同时的处理顺序导致的。尝试实现一个次要比较器比如当f值相同时优先选择h值更小的节点更靠近目标这样搜索会更“贪婪”路径往往更直接。移动代价不对等检查你的cost(n, m)函数。如果对角线移动的代价不是sqrt(2)而是1那么A*找到的“最短路径”在欧氏距离意义上可能就不是真正的直线最短。确保移动代价与你期望的路径最优性定义一致。后处理A在网格上找到的是网格中心的连线。对于可视化或角色移动这条路径可能有很多不必要的拐角。可以在A之后运行一个简单的路径平滑算法比如检查路径中非相邻的三点如果中间点可以去掉即起点和终点直线可达则去掉中间点。问题算法在大地图上运行非常慢。优化启发式使用更紧贴真实代价的启发式如对角线距离代替曼哈顿距离可以显著减少扩展的节点数量。使用JPSJump Point Search对于均匀网格地图JPS是A*的革命性优化它通过“跳点”跳过大量不必要的节点能将搜索速度提升一个数量级甚至更多。分层路径规划将大地图划分为多个区域如房间、街区。先在高层次进行区域间的A规划再在每个区域内进行详细的A规划。这牺牲了绝对最优性但换来了巨大的性能提升。5.2 D* Lite算法调试心得调试D* Lite比A*更具挑战性因为它的状态更多样。问题机器人停滞不前或者路径在障碍物变化后更新异常。键值计算与比较这是最可能出错的地方。务必用单元测试验证你的CalculateKey(s)函数和优先队列的比较器确保它们与论文中的定义完全一致。特别是k_m的更新逻辑k_m h(last_start, current_start)这里的h必须是一致的启发式函数。节点状态一致性理解g(s) rhs(s)一致、g(s) rhs(s)过一致、g(s) rhs(s)欠一致这三种状态。在ComputeShortestPath循环中对过一致和欠一致节点的处理逻辑完全不同必须严格实现。可以在关键节点打印出g和rhs值观察其变化是否符合预期。邻居与代价更新范围当一条边(u, v)的代价增加出现障碍时不仅需要更新节点u还需要更新所有将u作为最优后继的节点即那些满足rhs(s) cost(s, u) g(u)的节点s。漏掉这些节点的更新会导致算法“不知道”某些路径已经失效。问题初始规划就失败了。首先将环境设置为完全静态并关闭机器人的移动和动态更新。此时D* Lite应该退化为一个从目标到起点的反向A*。用A算法从起点到目标正向运行的结果与DLite规划出的路径代价应该完全一致。这是一个非常重要的交叉验证手段。检查目标点的初始化rhs[goal]必须设置为0并且UpdateVertex(goal)后目标点应被加入优先队列。5.3 进阶思考与扩展在实际项目中纯粹的A或DLite可能还需要与其他技术结合。与势场法/向量场直方图VFH结合在密集动态障碍物环境中如人潮涌动的广场仅靠路径规划可能不够。可以先用D*规划一条宏观路径再结合局部避障算法如动态窗口法DWA、人工势场法进行实时微调处理突然逼近的移动障碍物。考虑动力学约束对于汽车、无人机等非全向移动的机器人规划出的路径必须是运动学可行的。这催生了诸如Hybrid A*在状态空间x, y, θ中搜索、State Lattice Planning等算法。D也可以扩展到状态空间成为Kinodynamic D但复杂度会急剧上升。非最优但更快的选择有时我们并不需要绝对最优路径而是需要非常快的规划速度。这时可以牺牲最优性来换取速度例如使用Weighted A*给启发式函数加一个大于1的权重使其更“贪婪”或者RRT快速随机搜索树系列算法。在动态环境中也有ADAnytime D** 这样的算法它先快速找到一个可行解然后利用剩余时间不断优化。最后我想强调的是没有“最好”的算法只有“最合适”的算法。A和D代表了路径规划中两种核心思想全局最优规划和增量式重规划。掌握它们的原理和实现细节就像工具箱里有了两把得心应手的扳手。当你面对一个新的路径规划问题时首先要问的不是“我用哪个算法”而是“我的环境是静态还是动态”、“我的计算资源有多少”、“我对路径的最优性要求有多高”。想清楚了这些问题技术选型自然就清晰了。在我经历的项目中往往是多种算法的混合体在起作用例如用A做全局粗规划用DLite做局部细规划和重规划再用一些简单的规则处理突发碰撞。这种分层、混合的策略往往是工程实践中最稳健、最有效的解决方案。