A星学习记录

1、A星总体思路把起点放入开集初始化g(start)0h(start)heuristic(start, goal)f(start)gh当开集非空时从开集中取出f最小的点作为当前点。如果当前点就是终点停止搜索并通过父节点回溯整条路径。否则把当前点加入闭集表示这个点已经完成扩展。遍历当前点的所有邻居对每个邻居分类处理如果邻居是障碍物或者在闭集中跳过如果邻居不在开集中说明第一次发现它记录父节点为当前点计算g/h/f加入开集如果邻居已经在开集中但这条新路径更优更新它的父节点更新g/f重复上述过程直到找到终点或开集为空表示无路可达2、核心代价函数深入理解 g 与 h在 A* 算法中决定下一步往哪走的关键在于一个极其精妙的公式f g h f g hfgh用最通俗的话来说这就是“已经花了多少代价”加上“看起来还要花多少代价”。为1. 真实历史代价g的计算g代表的是已经走过来的真实代价。它不是凭空算出来的而是从起点开始一步一步累加出来的确定值。起点初始化g(start) 0移动累加如果当前点current走到相邻的neighbor那么到达邻居的新代价为tentative_g g(current) cost(current, neighbor)这里的cost取决于你如何定义机器人在地图上的移动。在最常见的8 邻域栅格地图允许上下左右和对角线移动中直走一步的代价通常设为1斜走一步对角线的代价根据勾股定理通常设为2 ≈ 1.414 \sqrt{2} \approx 1.4142​≈1.414举个例子从起点向右直走一个格子此时g 1。接着再往右上角斜走一步此时g 1 1.414 2.414。所以g就是“从起点一路累加过来的真实路径长度”。2. 未知预估代价h的计算启发函数h代表的是从当前点到终点的估计代价Heuristic。因为我们还没真正走到终点中间可能隔着千难万险的障碍物所以我们只能“猜”一下还有多远。常用的启发函数有以下几种欧几里得距离 (Euclidean Distance)最常用的“直线距离”非常适合 8 邻域地图。公式h ( x − x g o a l ) 2 ( y − y g o a l ) 2 h \sqrt{(x - x_{goal})^2 (y - y_{goal})^2}h(x−xgoal​)2(y−ygoal​)2​在 Python 中你可以用一行代码实现h math.hypot(goal_x - x, goal_y - y)曼哈顿距离 (Manhattan Distance)如果你在一个只能“横平竖直”移动的 4 邻域地图里不能斜着走那曼哈顿距离最合适。公式h ∣ x − x g o a l ∣ ∣ y − y g o a l ∣ h |x - x_{goal}| |y - y_{goal}|h∣x−xgoal​∣∣y−ygoal​∣对角线距离 (Octile Distance)同样适合 8 邻域但它比纯欧氏距离更贴近栅格地图的实际移动步数计算h ( d x d y ) ( 2 − 2 ) × min ⁡ ( d x , d y ) h (dx dy) (\sqrt{2} - 2) \times \min(dx, dy)h(dxdy)(2​−2)×min(dx,dy)(注对于初学者练手直接使用欧几里得距离已经足够好用。)3. 为什么g是“真实”而h是“估计”理解这两者的本质区别也就理解了 A* 的灵魂g是你已经一步一步踩出来的确定无疑。h是你站在当前位置眺望终点时的理想化预测。在实际寻路中当前点到终点的连线上可能横亘着一堵墙真实路径必须绕远路。但h通常只是按完美的几何距离估算它无法“未卜先知”所有的绕路细节。因此g掌控着严谨的底线而h赋予了算法快速向目标靠拢的方向感。4. 一个例子假设在一个网格地图中起点坐标为(0, 0)终点坐标为(3, 4)如果你当前走到了(1, 1)并且你是通过“先向右移一次再向上移一次”到达这里的两次直走你的真实花费g 1 1 2如果此时我们使用欧几里得距离来计算hh ( 3 − 1 ) 2 ( 4 − 1 ) 2 4 9 13 ≈ 3.606 h \sqrt{(3-1)^2 (4-1)^2} \sqrt{4 9} \sqrt{13} \approx 3.606h(3−1)2(4−1)2​49​13​≈3.606那么当前点(1, 1)的总预估代价就是f g h 2 3.606 5.606 f g h 2 3.606 5.606fgh23.6065.606A* 算法的底层逻辑就是不断计算出每一个备选点的f值永远优先去探索那个f值最小看起来总代价最低的格子3、图搜索算法主要参考该篇知乎文章路径规划 | 图搜索算法DFS、BFS、GBFS、Dijkstra、A*3.1、配置空间在机器人的工作空间中机器人是有大小和形状的在配置空间中将机器人视为质点而障碍物按照机器人的体积膨胀。3.2、深度优先搜索DFS核心思想只要前方还有路它就会一直往深处钻一条路走到黑。直到撞到死胡同它才会退回上一个岔路口尝试另一条路。数据结构栈 (Stack)先进后出 (LIFO)。工作流程在下面的无权图中找到从节点a到节点i的路径为例说明一下DFS算法的工作流程过程如下从i回溯得到路径a-b-c-g-i优点内存占用较小只需记住当前路径上的节点。缺点绝对不保证最短路径 它找到的第一条路可能是在地图上绕了十万八千里才绕到终点的路。总结适合用来回答“能不能走到”绝对不能用来回答“怎么走最近”。3.3、广度优先搜索BFS核心思想它会先把距离起点 1 步的所有格子走完再走距离 2 步的格子层层递进绝不冒进。数据结构队列 (Queue)先进先出 (FIFO)。工作流程以上面的无权图中找到从节点a到节点i的路径为例不断地弹出、扩展节点直到找到节点i为止完整流程如下图在i加入队列的时刻就已经找到目标点了此时回溯。从终点回溯i的父节点为ff的父节点为ee的父节点为a这样就可以得到a到i的最短路径为a-e-f-i如下入队的时候同时出队的元素就是它的父节点然后不断回溯直到找到起点。优点在“无权图”即每走一步代价都一样中BFS 找到的第一条路径绝对是最短路径。缺点非常消耗算力。它像个没有方向感的盲人即使终点在东边它也会花同样的时间去探索西边、南边和北边。总结求无权图最短路径的基石算法。3.4 Dijkstra 算法核心思想Dijkstra算的代价函数定义为f ( n ) g ( n ) f(n) g(n)f(n)g(n)其中g ( n ) g(n)g(n)表示从起点到当前点的移动代价。工作流程以下图为例计算起点a到终点i的最短路径箭头上的数值表示两个节点间的距离下图中绿色节点 (未发现)表示还没有探索到的区域目前的距离暂定为无穷大。粉红色节点 (队列中 / 候选区)表示已经发现了这条路并计算出了一个“暂定最短距离”节点上的数字。这些节点构成了我们的优先队列 (Priority Queue)。深蓝色节点 (已扩展 / 终局)表示这个节点已经完成了考察。它身上的数字就是从起点到它的“绝对最短真实距离”以后再也不会改变了。问题1 怎么进入队列变成粉红色已扩展深蓝色点-找其邻居-邻居是绿色算出“当前深蓝节点的距离 连线上的权重”把这个数字写在邻居身上邻居变成粉红色邻居已经是粉红色比较“原有的数字”和“新路线算出来的数字”问题2怎么成为已扩展节点变成深蓝色所有粉红色节点队列中找出其中数字最小的那一个。把它从队列中拿出来颜色涂成深蓝色已扩展。问题3怎么回溯我们去找父节点得到了一条倒序的链条i ← f ← h ← d ← e ← c ← b ← 0优点带权图且没有负权边中最短路径的绝对霸主。 只要图的代价是非负的它百分之百能找到总代价最小的路线。缺点它有g ( n ) g(n)g(n)保证底线但没有h ( n ) h(n)h(n)提供方向。所以它在带权图中依然是向四周盲目扩散的计算极其缓慢。总结地图导航如早期 Google Maps的底层逻辑极其严谨但不够聪明。3.5、启发式搜索算法(Heuristic Algorithm)3.5.1、贪婪最佳优先算法(Greedy Best First Search, GBFS)核心思想GBFS使用启发函数h ( n ) h(n)h(n)来作为代价函数也就是f ( n ) h ( n ) f(n) h(n)f(n)h(n)其中h ( n ) h(n)h(n)是当前节点到终点的代价它可以指引搜索算法往终点靠近主要用欧氏距离(Euclidean Distance)或者曼哈顿距离(Manhattan Distance)来表示它们的区别如下图数据结构优先队列 (Priority Queue / Min-Heap)普通队列是一种先进先出的数据结构而在优先队列中元素被赋予了优先级最高优先级元素优先删除。每次弹出h ( n ) h(n)h(n)最小的节点。优点在没有障碍物的空地上速度极快直奔目标。缺点极其容易陷入“局部最优”。遇到一个“U”型障碍物它会一头扎进去然后在里面疯狂打转或直接报错因为它不愿意往回走往回走意味着h hh值变大。总结速度极快但容易翻车不保证最短路径。3.5.2、A* 算法核心思想A* 完美结合了 Dijkstra 的严谨实际代价g gg和贪心算法的聪明预估代价h hh。它既朝着目标走又时刻盯着脚下的花费防止走偏。核心公式f ( n ) g ( n ) h ( n ) f(n) g(n) h(n)f(n)g(n)h(n)数据结构优先队列 (Priority Queue) 配合开集/闭集 (Open/Closed Set)。每次弹出f ( n ) f(n)f(n)最小的节点。核心性质只要你的启发函数h ( n ) h(n)h(n)是可采纳的 (Admissible) —— 即h ( n ) h(n)h(n)永远小于等于真实的剩余距离A* 就绝对能保证找到最短路径。如果h ( n ) h(n)h(n)估高了A* 可能会错过最短路径退化向贪心。怎么优化1Tie Breaker (打破对称性)当地图上有很多f ( n ) f(n)f(n)相同的格子时给h ( n ) h(n)h(n)乘上一个极小的常数如 1.001打破平衡让算法果断选择一条路减少无意义的搜索。2 加权 A (Weighted A)令f g W × h ( W 1 ) f g W \times h\ (W 1)fgW×h(W1)。它就不再保证全局最优牺牲一点点路径的最优性换取计算速度的几十倍提升。纯贪心算法 (GBFS) 遇到 U 型障碍物会死在局部最优加粗样式里出不来。A* 算法同样有启发函数它为什么不会永远卡死在里面它逃离陷阱的底层机制和代价是什么1当算法被诱导进入 U 型死胡同后虽然前方的h ( n ) h(n)h(n)看起来很近但随着它在死胡同里越走越深g ( n ) g(n)g(n)实际累积步数会不断飙升。 当死胡同内部所有备选节点的总代价f ( n ) f(n)f(n)被g ( n ) g(n)g(n)撑得极其巨大甚至超过了当初在墙外面那些被冷落的绕路节点的f ( n ) f(n)f(n)时A* 的优先队列就会自动切回外围节点“溢出”陷阱重新从外面绕路。2付出的代价 算力灾难。为了逃离陷阱A* 必须把 U 型障碍物内部的几乎所有网格都计算并扩展一遍俗称“填坑”这会消耗海量的计算时间和内存。3.5.3、JPS 算法 (Jump Point Search)核心思想A* 在大型游戏地图中依然有个痛点在大平原上它还是得一格一格地算太慢了。JPS 的核心思想是“跳过不需要做决定的对称格子”。只要前面没障碍我就一直沿直线飞过去直到遇到障碍物的拐角强制邻居或者必须转弯的地方我才停下来计算。核心规则寻找“跳点 (Jump Point)”。跳点通常出现在障碍物的边缘拐角处。算法在扩展时不再把所有相邻格子加入开集而是沿着直线或对角线“射出射线”只把射线碰到的“跳点”加入开集。优点在均匀网格Uniform Grid地图中速度完爆 A*因为开集里保存的节点数量被极其恐怖地压缩了。局限极其挑食。JPS 只能运行在每走一格代价都相同的网格地图上。如果你的地图有不同的权重比如草地代价 2公路代价 1JPS 瞬间瘫痪无法使用。3.5.4、混合 A* 算法 (Hybrid A*)核心思想前文提到的所有算法包括 A* 和 JPS都有一个致命假设它们把移动物体当成了“可以原地 360 度转弯的质点”。但在现实物理世界中汽车阿克曼转向结构是不能原地掉头的核心改进1状态空间升维引入车头朝向基础 A* 的状态只有坐标( x , y ) (x, y)(x,y)。而 Hybrid A* 的状态是三维的( x , y , θ ) (x, y, \theta)(x,y,θ)加入了航向角θ \thetaθ。2模拟真实驾驶打方向盘在扩展邻居节点时它不再是“上下左右”走格子而是模拟真实的车辆运动模型例如方向盘左打死、直行、右打死来推演下一段弧线轨迹。3连续的坐标空间虽然它底层依然依赖栅格地图来进行碰撞检测但车辆的实际坐标( x , y , θ ) (x, y, \theta)(x,y,θ)是连续的浮点数不再被强制对齐到网格的中心点。4双重启发函数因为维度增加导致计算量爆炸它通常同时运行两个h hh来引导方向h 1 h_1h1​(无视车辆约束只看障碍物)其实就是跑一个基础的 2D A*。h 2 h_2h2​(无视障碍物只看车辆约束)利用数学曲线如 Reed-Shepp 曲线算出空地上考虑转弯半径的最短距离。最终取两者最大值h max ⁡ ( h 1 , h 2 ) h \max(h_1, h_2)hmax(h1​,h2​)优点生成的路径极其平滑天然满足非完整运动学约束Non-holonomic constraints真车或轮式机器人可以直接照着开。局限维度灾难计算量庞大。因为引入了角度维度搜索空间呈指数级爆炸。如果启发函数设计不好算法极易超时。3.6、总结找有没有路 - DFS迷宫最短路无权重 - BFS带权重最短路求稳 - Dijkstra带权重最短路求快 - A* (最优解与速度的平衡)超大空旷网格极致速度 - JPS真实物理世界车辆/机器人导航 - Hybrid A* (混合 A*)