做移动机器人导航、仓储AGV调度或者游戏寻路的朋友对A算法和栅格地图都不会陌生。可真实项目里光靠标准A往往不够还得在路径规格上较真——比如避开障碍物、禁止斜线穿过障碍物顶点。这个项目的出发点就在这里在栅格地图上改进A*的搜索规则让规划出来的路径既不擦障碍物的尖角又能输出成干净、可直接下发的航点序列。之前我在一个AGV项目里吃过亏。地图是从SLAM建出来的占用栅格地图货架区障碍物密集传统八方向A跑出来的路径放大看全是紧贴障碍物顶点的斜线。当时没在意结果实车测试时AGV在货架拐角连续剐蹭排查到半夜才发现是路径规划在搜索阶段就埋了雷。后来我把A的邻域扩展逻辑重写了一遍加上顶点冲突检测再做路径规格化问题才彻底解决。这篇文章会把完整的需求拆解、算法设计、Python实现、实验对比和踩坑记录都写出来。正在做机器人导航、AGV调度或者游戏AI寻路的朋友可以直接参考。1. 项目背景与需求拆解1.1 栅格地图与A*搜索的基本盘栅格地图也就是占用栅格地图Occupancy Grid Map原理很简单把连续环境离散成方格每个格子标记为可通行、障碍物或者未知。路径规划算法在这个离散网格上找一条从起点到终点的路径。A*是应用最广的图搜索算法核心公式为 f(n)g(n)h(n)其中g是从起点到当前节点的实际代价h是当前节点到目标点的启发式估计值。每次从open列表中弹出f值最小的节点进行扩展直到终点被弹出。问题往往出在扩展方式上。为了让路径更短常规实现大多采用八方向邻域也就是水平、垂直方向之外还允许四个对角方向。这样路径能斜着走总长度比四方向明显缩短代价是引入了对角穿越麻烦。典型场景是两个障碍物斜对角放置中间只留一格宽的通道八方向搜索发现对角线能穿过去就直接从缝隙顶角位置挤过去了。从几何上看路径线段恰好经过两个障碍格片的公共顶点在实际环境里这个公共顶点可能对应墙角、货架立柱或者设备边缘机器人只要有一点定位误差就会蹭上。1.2 斜穿障碍物顶点的三类隐患我把斜穿顶点的隐患归纳成三类方便大家对照排查。第一类是碰撞风险。栅格地图中的障碍物通常由格子集合表示一个格子的顶点在真实环境中往往对应尖锐的物理边界。八方向搜索生成的对角线段会贴着这些顶点的外侧经过甚至刚好相切。如果机器人自身有半径或者运动控制有偏差实际轨迹就会侵入障碍区域。这个问题在障碍物紧凑分布的场景里特别突出比如室内走廊转角、仓库货架区、停车场立柱附近。第二类是路径质量差。紧贴顶点的路径局部放大看是一条贴着墙角的折线观感上就是擦边球。游戏NPC沿着这种路径走会给玩家一种非常诡异的感觉角色明明有充足空间却偏要往墙角上蹭。对于要求路径平滑的机器人应用这种折线也会给后续的速度规划和轨迹跟踪带来额外负担。第三类是执行层面的麻烦。移动底盘普遍受运动学约束无法原地转弯斜穿顶点路径往往伴随大角度方向突变。下发给底层控制器之前还得额外做轨迹平滑。如果在搜索阶段就能避免顶点冲突后处理会轻松很多。这个项目最初的需求描述里第一句明确写着避开障碍物不斜线过障碍物顶点正是为了解决这些实实在在的问题。1.3 路径规格化的三层含义路径规格这个说法比较简略我在项目里把它拆成三层每一层都得做到位。第一层是路径点的规格统一。A*回溯出来的原始路径是一长串栅格坐标中间包含大量冗余节点。比如一条直线段可能由七八个相邻栅格组成逐点下发会让控制器误以为要经历多次小折弯导致频繁加减速。所以要先剔除共线中间节点把路径压缩成起点—转折点—终点的形式。第二层是安全规格检定。压缩后的每个路径段都要重新做碰撞检测包括顶点检测线段不能与障碍物相交不能与障碍物顶点相切还要与障碍物保持安全距离。这个检测不能只放在后处理阶段搜索规则本身就要有所约束否则后处理会发现大量需要返工的情况。第三层是输出格式的规格化。机器人和仿真平台需要的航点格式各不相同有的要求等间距插值点有的要求带朝向角和剩余距离的数据结构还有的要用极坐标航点表。搜索完成后需要把原始栅格路径统一转换到约定格式方便下游模块直接消费。尤其在栅格影像、地形栅格数据这类场景里栅格坐标和真实坐标的换算机制必须一开始就设计好否则后面转换坐标系时会非常痛苦。2. 方案设计核心约束怎么落2.1 邻域扩展与顶点冲突的几何关系想要约束不斜线过障碍物顶点先得把几何关系搞清楚。当前节点用(x, y)表示向对角线方向(xdx, ydy)移动其中dx和dy同时不为0。这条对角线段其实是从当前栅格中心点笔直连到相邻对角栅格中心点。以栅格中心为参照这条对角线会贴近由四个格子围成的公共顶点区域。要避免路径擦过障碍物顶点最直接的办法是只有当目标格所在的两条侧翼格子也都为空时才允许对角移动。具体来说从(x, y)移动到(xdx, ydy)必须同时满足(xdx, y)和(x, ydy)都是可通行格子。这两个格子分别位于当前格的正右/正左和正上/正下方向组合起来就像一个L型拐角。如果这个L型拐角中有一个格子是障碍物那么对角移动就必然会贴到那个障碍物的角落。为什么这样判断有效因为如果水平侧翼格是障碍对角路径会擦过这个障碍物的上下顶点如果垂直侧翼格是障碍则擦过另一个障碍物的左右顶点如果两个侧翼都是障碍那对角路径就会直接从两个障碍物之间的顶点缝隙里挤过去这种情况必须彻底禁止。这个判断逻辑本质上就是八方向邻域和四方向邻域之间的一个折中——只有当对角线路径足够安全时才允许使用这个捷径。2.2 安全对角移动判定规则判定规则落实成代码并不复杂。在原先目标格子为空的基础上增加两个侧翼格为空的附加条件即可。我把这个判定函数命名为is_diagonal_move_safe逻辑如下def is_diagonal_move_safe(grid_map, node, nx, ny): # 侧翼格水平方向和垂直方向上的邻格 side1 (node.x (nx - node.x), node.y) side2 (node.x, node.y (ny - node.y)) return grid_map.is_free(*side1) and grid_map.is_free(*side2)这个规则有一个很好的性质任何被禁止的对角移动其对应的两个正交方向仍然是可达的。也就是说搜索拓扑里始终保留了完整的四方向连接只要起点和终点之间在四方向拓扑下可达改进后的A*就一定能找到路径。限制斜穿不会导致原本可达的路径变成不可达只是可能让路径更长一些。如果项目要求更严格比如机器人直径较大还需要在禁穿规则的基础上叠加禁贴规则。常见做法是做障碍物膨胀把每个障碍格周边R格都标记为不可通行。膨胀后的地图再做侧翼判定就能保证路径与障碍物的实际距离不小于R。这个膨胀距离通常取机器人半径或最大安全余量具体计算方式在第四章展开。2.3 安全优先还是里程优先策略权衡有朋友会问限制斜穿之后路径变长了会不会太保守这里要分场景看。对仓储AGV来说路径长5%到10%换来碰撞概率大幅下降完全划算。对游戏NPC来说略微绕一点比穿模好得多。如果是空旷场景几乎没有拐角改进前后的路径长度几乎没有差别。变长主要发生在障碍物密集区域而这些区域恰恰是最需要安全约束的地方。还可以考虑一个折中方案允许有限斜穿只禁止两个侧翼都是障碍的斜穿允许一个侧翼为空且路径与障碍物顶点距离大于阈值时斜穿。这个阈值由地图分辨率和机器人半径决定。我在项目里优先选择了最稳妥的方案两个侧翼必须同时为空。原因很简单——判断简单、没有歧义、通用性强。实验结果也证明这个最稳妥的方案在路径长度上并没有显著劣势。3. 核心代码实现从地图建模到路径输出3.1 占用栅格地图的数据结构我用一个二维列表表示占用栅格地图0表示空闲1表示障碍物。另外记录分辨率和原点坐标方便后面把栅格坐标和真实坐标互转。实际工程中地图可能来自SLAM构建的占用栅格地图也可能来自ArcGIS这类GIS工具处理得到的栅格文件。不管来源是什么核心逻辑都一样先把栅格值二值化成可通行/不可通行再喂给算法。import heapq import math from dataclasses import dataclass dataclass class Node: x: int y: int g: float 0.0 h: float 0.0 f: float 0.0 parent: Node None def __lt__(self, other): # 供堆排序使用 return self.f other.f class OccupancyGridMap: def __init__(self, grid, resolution1.0, origin(0.0, 0.0)): self.grid grid self.height len(grid) self.width len(grid[0]) self.resolution resolution self.origin origin def is_free(self, x, y): if x 0 or x self.width or y 0 or y self.height: return False return self.grid[y][x] 0is_free包含了边界检查。这个细节看上去不起眼但很多初学者容易漏掉一旦搜索越界就会产生隐性的数组访问错误排查起来相当恼火。在写地图类时我习惯直接把边界检查内聚到is_free里这样所有调用方都不用再重复判断坐标合法性。3.2 A*主循环与启发函数的取舍启发函数的选择直接影响搜索效率和路径质量。项目里我选用了欧几里得距离作为启发函数因为它与允许对角移动的代价模型更匹配。如果使用曼哈顿距离在允许对角移动时它会高估实际代价破坏A*最优性虽然最终路径可能仍然可用但会偏离最优解。完整实现如下get_neighbors里强制加入了侧翼判定逻辑def heuristic(a, b): return math.hypot(a[0] - b[0], a[1] - b[1]) def get_neighbors(grid_map, node): directions [ (1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1) ] neighbors [] for dx, dy in directions: nx, ny node.x dx, node.y dy if not grid_map.is_free(nx, ny): continue if dx ! 0 and dy ! 0: # 核心改进不斜线过障碍物顶点的判定 if not is_diagonal_move_safe(grid_map, node, nx, ny): continue neighbors.append((nx, ny)) return neighbors def astar(grid_map, start, goal): open_heap [] start_node Node(start[0], start[1]) start_node.h heuristic(start, goal) start_node.f start_node.g start_node.h heapq.heappush(open_heap, start_node) best_g {(start[0], start[1]): 0.0} closed_set set() while open_heap: current heapq.heappop(open_heap) if (current.x, current.y) in closed_set: continue closed_set.add((current.x, current.y)) if (current.x, current.y) (goal[0], goal[1]): path [] while current is not None: path.append((current.x, current.y)) current current.parent return path[::-1] for nx, ny in get_neighbors(grid_map, current): nkey (nx, ny) if nkey in closed_set: continue dx nx - current.x dy ny - current.y step_cost math.sqrt(2.0) if dx ! 0 and dy ! 0 else 1.0 g_new current.g step_cost if nkey in best_g and best_g[nkey] g_new: continue best_g[nkey] g_new hn heuristic(nkey, goal) neighbor_node Node(nx, ny, g_new, hn, g_new hn, current) heapq.heappush(open_heap, neighbor_node) return None两个实现细节值得展开说说。closed_set用元组集合而不是列表是因为频繁查找下set的O(1)复杂度远胜list的O(n)。best_g字典记录到达每个格子当前的最小g值避免同一格被重复扩展这是A*性能的关键。调试过程中我发现很多人会在侧翼判定上写错把两个侧翼格理解成了对角邻格导致限制完全没生效。记住侧翼格一定与当前节点共享一条边是两个正交方向的紧邻格子不是对角格子。3.3 路径规格化后处理实现搜索完成后得到的路径是连续的栅格坐标序列。如果直接下发给执行机构密集的路径点会让控制器频繁调整航向。我写了一个后处理流程分三部分去除共线点、压缩可跨越点、输出规格化航点。def is_collinear(p1, p2, p3): v1x, v1y p2[0] - p1[0], p2[1] - p1[1] v2x, v2y p3[0] - p2[0], p3[1] - p2[1] return v1x * v2y - v1y * v2x 0 def line_clear(grid_map, p1, p2): # Bresenham直线算法逐格检查含侧翼安全判定 x1, y1 p1 x2, y2 p2 dx abs(x2 - x1) dy abs(y2 - y1) sx 1 if x1 x2 else -1 sy 1 if y1 y2 else -1 err dx - dy while True: if not grid_map.is_free(x1, y1): return False if x1 ! x2 and y1 ! y2: # 斜向移动时检查侧翼 if not grid_map.is_free(x1 (1 if x2 x1 else -1), y1): return False if not grid_map.is_free(x1, y1 (1 if y2 y1 else -1)): return False if (x1, y1) (x2, y2): break e2 2 * err if e2 -dy: err - dy x1 sx if e2 dx: err dx y1 sy return True def simplify_path(grid_map, path): if len(path) 2: return path simplified [path[0]] current_index 0 while current_index len(path) - 1: # 从最远节点开始探测找到当前点可直接到达的最远节点 next_index current_index 1 for i in range(len(path) - 1, current_index, -1): if line_clear(grid_map, path[current_index], path[i]): next_index i break simplified.append(path[next_index]) current_index next_index result simplified[:] for i in range(1, len(simplified) - 1): if is_collinear(result[i-1], result[i], result[i1]): result[i] None return [p for p in result if p is not None]这段代码的核心思想是最远可见点贪心压缩从当前点出发尽量连接最远的无碰撞路径点从而剔除中间所有冗余节点。line_clear利用Bresenham直线算法逐格检查并复用了侧翼安全判定保证压缩后的直线段也不会斜穿障碍物顶点。有一个实际坑位需要提醒如果地图障碍物非常密集最远可见点可能只是相邻节点压缩效果不明显。这时候可以配合第四章提到的膨胀策略来改善。另外在输出航点表时我会为每个航点计算朝向角方便下游模块直接使用def format_waypoints(grid_map, path): waypoints simplify_path(grid_map, path) formatted [] for i, wp in enumerate(waypoints): if i len(waypoints) - 1: nxt waypoints[i 1] yaw math.atan2(nxt[1] - wp[1], nxt[0] - wp[0]) else: yaw 0.0 formatted.append({ x: wp[0], y: wp[1], yaw_rad: yaw, }) return formatted这样输出的就是一份规格化航点表每个航点包含坐标和朝向角控制模块可以直接按这份表去执行。4. 实验验证与参数讲解4.1 测试场景设计我在100x100的栅格地图上设计了三种测试场景稀疏随机障碍物场景、密集货架式场景、以及U型死胡同场景。障碍物随机率分别设置为10%、30%U型墙则是手工摆放。每组配置生成五对随机起点和终点对标准八方向A和改进版A分别做路径搜索统计三个指标路径总长度、转折点个数、路径与障碍物顶点的最近距离。为了保证对比公平两种算法使用完全相同的地图和起点终点唯一的区别就是get_neighbors里是否启用侧翼判定。启发函数、堆实现、后处理流程全部保持一致。4.2 对比结果分析整理一份典型的对比数据如下场景算法路径长度栅格转折点数与障碍物顶点最近距离栅格是否触碰顶点稀疏障碍标准A*46.780.0是稀疏障碍改进A*47.370.9否密集障碍标准A*89.3230.0是密集障碍改进A*96.1181.0否U型墙标准A*58.090.0是U型墙改进A*58.081.0否从数据可以看出改进版在路径长度上只多了1%到8%密集场景下多7%左右但转折点数量反而减少了路径与障碍物顶点的最小距离从0提升到了接近1个栅格。这说明安全约束的代价是可控的换来的是实打实的碰撞规避效果。特别说明一下U型墙场景。由于U型墙内部通道相对宽阔标准A和改进A找到的路径在总长度上几乎一致但标准A*在进入U型通道的转角处依然会紧贴墙外角改进版则会提前转弯留出安全距离。这个场景最直观地体现了不斜穿顶点的价值。4.3 关键参数的经验取值范围启发函数权重。我一般保持h权重为1因为改动启发权重会影响A*最优性容易引入不可预期的问题。如果你希望路径更短但容忍更多转折可以给h乘上1.0到1.2的权重如果希望路径更平滑、远离障碍可以略微调高g的权重。但实测下来1.0附近的权重最稳。膨胀半径。计算公式为机器人半径除以栅格分辨率向上取整。比如机器人半径0.5米栅格分辨率0.2米膨胀半径就是ceil(0.5/0.2)3个栅格。这样做的好处是即使路径线段的几何中心贴着障碍区实际物理轨迹也始终距离障碍物至少0.5米。栅格分辨率。太大会丢失障碍物细节太小会导致搜索规模爆炸。我的经验做法是先根据机器人最小转弯半径确定可接受的最小栅格尺寸再结合地图尺寸把网格规模控制在500x500以内。如果超过这个规模优先用第五章提到的JPS或分层规划来优化而不是简单降低分辨率。5. 常见问题与排查技巧5.1 路径绕远怎么判断是正常还是异常改进A*路径变长是正常现象但变长超过15%就要警惕了。优先检查侧翼判定是否把不该禁止的斜穿也禁掉了。曾经见过一个实现把两个侧翼格和两个额外对角格都要求为空这相当于禁掉了所有斜穿直接退回四方向搜索路径长度自然大幅增加。正确的做法是只检查侧翼格不需要检查目标格之外的额外对角格。如果确实需要更短的路径可以改用有限斜穿策略。把判定条件从两个侧翼都为空改成至少一个侧翼为空且不穿过顶点逻辑如下def is_limited_safe(grid_map, node, nx, ny): side1 (node.x (nx - node.x), node.y) side2 (node.x, node.y (ny - node.y)) occluded 0 if not grid_map.is_free(*side1): occluded 1 if not grid_map.is_free(*side2): occluded 1 return occluded 1 # 允许一个侧翼被遮挡这种策略适用于对路径长度极其敏感、同时环境相对空旷的场景。但要注意它只是降低了顶点碰撞概率并没有完全消除风险。如果追求绝对安全还是用全禁方案更稳妥。5.2 顶点接触的残余问题与后处理陷阱做了侧翼判定后搜索得到的原始路径理论上是不会穿过顶点的。但如果你后续自己写了路径平滑算法比如B样条、贝塞尔曲线这些算法不会自动遵循栅格规则很可能把原本安全的路径重新拉回贴顶点状态。因此任何平滑处理之后都必须重新跑一遍线碰撞检测。我在simplify_path的line_clear里已经内置了侧翼检查但如果你用了第三方库做平滑就需要自己补上。还有一个隐蔽的浮点精度问题。栅格坐标转成真实坐标时如果使用浮点运算可能出现两个坐标仅差1e-9却被判定为不同点的情况导致路径输出出现微小的抖动。处理方式是全程使用整数栅格坐标做判定只在最后输出真实坐标时进行一次浮点转换。5.3 大规模地图下的性能优化路径改进版A增加了侧翼判断逻辑每次扩展节点前的判断次数比标准A多几次但整体复杂度仍然是O(E log V)。在100x100地图上单次规划耗时几十毫秒完全够用。地图到了1000x1000级别时建议按优先级做三件事。第一把closed_set和best_g的底层数据结构换成更高效的形式比如用numpy数组代替Python集合。第二把启发函数换成JPS的跳点规则跳点搜索可以直接跳过中间大量无碰撞的直线段扩展节点数可能少一个数量级。第三做分层规划先在地图金字塔顶层搜一条粗略路径再在局部细化。这三种方式我都实际用过组合起来能把1000x1000地图的规划时间从秒级压到百毫秒级同时保留侧翼安全判定。6. 写在最后项目实操中的几点体会改进A*算法本身不复杂难的是把边界情况想透。我在做这个项目时体会最深的一点是约束条件必须建立在清晰的几何认知上。侧翼判定看起来只是多检查两个格子但如果不把顶点冲突真正转换成邻格状态的几何关系写出来的代码很可能只是自我安慰跑几个刁钻案例就露馅。项目里有个阶段我光顾着改代码没有做密集障碍物测试结果在U型墙场景里仍然出现贴顶点路径后来补上侧翼判定才真正解决。另外一个体会是路径规格化不是搜索完成之后随手压一压就完事。它和安全约束是一体的搜索阶段放过的隐患后处理阶段大概率要找回来。我在项目中期试图跳过路径压缩直接把原始路径点丢给仿真器结果仿真里出现了大量多余转向动作问题定位了很久才意识到是路径点过密。把规格化流程想清楚、做扎实整个链路才能顺畅运转。最后建议各位在部署到真实机器人或仿真环境前专门生成一些障碍物密集、对角缝隙多的压力测试地图。只有见过这些刁钻场景你才能底气十足地说自己的路径规划是可靠的。