3D路径规划实战：用Python手写A*算法与避障导航

无人机要穿过一片楼宇密集的城区，机械臂要从堆满零件的料筐里取出一只螺丝，无人车要在立体车库中规划一条不会碰壁的上楼路线——这些任务背后有一个共同的计算核心：在三维空间里找出一条从起点到终点、避开所有障碍物的通路。这就是3D路径规划，而其中最经典、最适合入门的图搜索算法，就是A*。

这篇文章是《Python运动规划库》系列教程中关于3D图搜索的部分。我不会去调OMPL、MoveIt这些现成的规划库，而是纯手写一个基于numpy的3D A搜索器，把地图建模、邻居生成、启发式函数、主搜索循环、3D可视化整个走通，最后再聊一聊生产环境里真正会踩到的坑。不管你是刚接触运动规划的学生，还是准备把自主导航塞进自己项目的开发者，照着这篇文章的思路写一遍，后续再看DLite、JPS这些高级算法都会顺很多。

1. 3D路径规划的问题建模与算法选型

1.1 为什么二维规划解决不了这些问题

先说个很直接的场景。一架无人机在城市里执行巡检任务，面前横着一座50层高的写字楼。二维栅格地图只能告诉你这栋楼的“占地轮廓”是哪里，但无人机明明可以从300米高度直接飞过去，这栋楼根本挡不住它。如果硬要用2D地图去规划，路径会绕出一大圈，甚至在某些狭窄街区间直接判定“无路可走”。这不是算法的问题，是地图维度压根不够。

类似的场景还有很多。仓储AGV在货架间穿行，地面上横着一根消防管道，AGV底盘有20厘米离地间隙，它其实可以直接从管道上方碾过去，但2D地图会把它当成一个不可穿越的障碍物。机械臂的操作空间天然就是三维的，虽然真正的机械臂规划是在六维关节空间里做的，但当末端执行器需要绕过工作台、夹具这些固定障碍时，很多工程师第一步仍然会把问题投影到三维笛卡尔空间，先用3D路径搜索找一条参考路径，再交给逆运动学去细化。

所以结论很清楚：只要工作空间里存在“高度维度带来的自由度”，2D规划就会产生误判。3D图搜索解决的不只是“地图多了一个维度”，而是把“能不能走”的判断从平面扩展到了立体空间。

1.2 3D空间在计算机里怎么表示

做3D规划之前，先得解决“空间怎么存”的问题。目前主流有三种表示方式，各有各的适用场景。

**体素栅格（Voxel Grid）**是最直观的一种。直接把三维空间切成一格一格的小立方体，用三维数组存，0代表可通行，1代表被障碍物占据。我在这篇文章里用的就是这种。它的优点非常明显：随机访问是O(1)，A*的邻居扩展天然就是数组下标加减，写起来几乎不费脑筋。缺点是分辨率跟内存呈立方级增长，100米×100米×50米的空间，用1米分辨率也就50万个格子，但换成0.1米分辨率就是5亿个格子，内存直接飙到500MB起步。

**八叉树（OctoMap）**是对体素栅格的改良，空白区域用大节点表示，只在障碍物附近细分。在SLAM建图结果上做规划时，八叉树的内存效率比均匀栅格高得多，但代价是邻居节点查询要走树结构，代码量明显更大。

**点云（Point Cloud）**是激光雷达的直接输出，信息量最大，但不适合直接作为A*的输入，一般会先做体素化或者转成八叉树才能用。

我在这篇文章里选择均匀体素栅格，原因很简单：这是理解一切三维搜索的地基。等你把栅格A*吃透了，以后换到八叉树只是换一个邻居查询接口，搜索框架是完全一样的。一上来就搞复杂的数据结构，容易把核心算法淹没在无关细节里。

1.3 为什么选A*而不是Dijkstra或RRT

每次讲路径规划，都会有读者问同一个问题：为什么要用A*？Dijkstra不是也能找最短路径吗？RRT不是更适合高维空间吗？

Dijkstra确实保证最优解，但它的搜索方式像水波一样向着四周均匀扩散，在3D网格里这个“水波”的体积大得吓人。从起点出发，它会把所有比终点路径代价小的节点都扩展一遍，在100×100×100的地图里，这意味着可能要探测上百万个节点，纯Python循环跑下来基本没法用。

贪心最佳优先搜索只盯着启发式h来判断方向，确实快，但容易被局部障碍物误导，可能绕一大圈远路，极端情况下甚至找不到路径。

A把两者结合起来：g是已经付出的代价，h是到终点的预估代价，两者相加，每一次都优先扩展“最有希望通向目标”的节点。在静态栅格地图上，A是“既能保证最优、又兼顾效率”的最优解。

至于RRT，它用在连续高维空间里确实很香，比如六自由度机械臂的关节空间规划，那里面没有现成的栅格可用。但如果我们手上已经有一张体素化的3D地图，图搜索的路径质量、确定性和复现性都优于RRT。RRT给出的路径是随机采样出来的折线，往往还需要额外的平滑处理。

一句话总结：A*是静态栅格地图上的默认解，也是后续所有高级图搜索算法的起点。

2. A*算法核心原理：f、g、h是怎么协同工作的

2.1 从Dijkstra到A*：多出来的“启发式”

A*的核心公式短得可以写在一张便签上：

f(n) = g(n) + h(n)

其中：

• g(n)：从起点到当前节点n，已经花掉的实际代价。
• h(n)：从当前节点n到终点，还需要花掉的代价的估计值。
• f(n)：经过节点n这条路，从起点到终点的总代价估计。

打个比方。你在一个陌生城市里从城南去城北，g是你已经走过的公里数，h是你根据地图上两点直线距离估算的“还差多少公里”，f就是“走你当前这条路，全程大概多少公里”。正常人都会优先尝试那些“走得不算远、离终点也不远”的路线，而不是一条已经绕了30公里、据说只剩2公里的岔路。

A*每一步做的事情就是：从待扩展列表（open list）里取出f值最小的节点，扩展它，更新邻居的g值，再把邻居放进待扩展列表。重复这个过程，直到弹出终点节点，或者待扩展列表耗尽。

2.2 为什么h必须是“不乐观”的

A*能保证找到最优路径，有一个非常关键的前提：h(n)必须小于等于从n到终点的真实最短距离。这个性质在算法导论里叫可采纳性（Admissible）。

如果h高估了剩余代价，会发生什么？假设最优路径上某个节点的真实代价是100，但你把h估成了200，它的f值被抬高到300，排到了另一个真实代价是150的路径后面。搜索就会先走那条看起来“近”但实际上不是最短的路线，最终得到的路径就不是全局最优的了。

如果h恰好等于真实代价，那A*的效率会达到理论最高——它几乎直奔目标，每个节点只扩展一次。但现实里，h等于真实代价意味着你已经提前知道了最短路径，这通常是不可能的。

如果h等于0，A*就退化成了Dijkstra，保证最优但速度最慢。

所以设计启发式函数的策略很清晰：**在保证不高于真实代价的前提下，让h尽量贴近真实值。**这也是为什么启发式选型在3D搜索里如此重要的原因。

2.3 open list和closed list到底在干嘛

很多教材把A*描述成维护两个表：open list放待扩展节点，closed list放已经扩展完的节点。理论上没错，但工程实现上，我习惯的做法是用一个字典记录当前已知的最小g值，外加一个二叉堆（heap）来维护待扩展节点，不显式维护closed list。

流程是这样的：

1. 把起点塞进堆，g(start) = 0。
2. 从堆里弹出f值最小的节点current。
3. 如果堆里记录的这条g值大于当前已知的g_score[current]，说明这是一条过期记录，直接跳过。
4. 扩展current的所有邻居，计算新的g值，比已知的更小就更新parent指针并推入堆。
5. 重复2-4，直到堆空或者弹出goal节点。

这里允许同一个节点多次入堆，初看有点浪费，但实际是聪明的做法。因为二叉堆从中间删除一个节点的复杂度是O(N)，而重复入堆只是多存一条记录，堆弹出的复杂度始终是O(log m)。空间上多花了一点，时间上却避免了最麻烦的堆内删除操作。这个细节，笔试面试和工程实践里都特别常见。

3. 3D图搜索的邻居定义、移动代价与启发式函数

3.1 邻居定义直接决定路径形态

2D网格里大家很熟悉4邻居和8邻居的说法。到了3D，邻居数量变成了三个档位：

• 6邻居：上下左右前后，只能沿坐标轴移动。
• 18邻居：在6邻居基础上加上12个“边斜向”，即两个坐标方向同时变化的移动。
• 26邻居：在18邻居基础上再补上8个“角斜向”，即三个坐标方向同时变化的移动。

这个选择直接决定路径长什么样。6邻居走出来的路径全是直角折线，转弯极其僵硬；26邻居可以斜着穿空间，路径明显更短也更平滑，但代价是每个节点要检查26个方向，计算量是6邻居的四倍还多。

实际项目中怎么选？地面机器人跑2D平面，8邻居最常见；无人机和机械臂的运动自由度高，26邻居是标配。我在文章后面的实现里默认开了26邻居，但代码里用allow_diagonal这个开关可以随时切回6邻居，方便大家对比效果。

3.2 移动代价必须精确匹配邻居集合

这是入门阶段最容易踩的一个坑。3D栅格里，不同方向的移动距离是不同的：

• 沿轴走一步：代价1。
• 二维对角线（比如dx=1，dy=1，dz=0）：代价sqrt(2)约等于1.414。
• 三维对角线（dx=1，dy=1，dz=1）：代价sqrt(3)约等于1.732。

如果图省事，把所有移动代价都设成1，A*会认为斜着走和直着走一样“便宜”。这时候它就会疯狂偏爱对角线移动，因为“反正代价一样，我斜着一口气窜过去还省了中间节点”。等你拿真实欧几里得距离一量，会发现它规划出来的路径明显偏长，而且路径形态很诡异。

正确的做法是每次生成邻居时实时算一遍移动距离，三行代码的事：

move_cost = np.sqrt(dx * dx + dy * dy + dz * dz)

不要图省事写死。这个代价直接参与g值计算，一旦错了，后面所有最优性证明都白搭。

3.3 3D启发式函数选型对比

3D空间里最常用的启发式有三种，适用场景完全不同：

曼哈顿距离在26邻居下会严重高估剩余代价，因为它默认所有移动都必须沿轴走，可一旦允许斜穿，真实距离比它算出来的小，启发式就不再可采纳，搜索结果就失去了最优性保证。

欧几里得距离永远小于或等于真实最短距离，所以一定可采纳，是新手最稳妥的选择。代价是它比真实代价矮一截，搜索会多探索一些节点。

3D对角距离在26邻居下精确匹配了移动代价，它先把三轴差值的最大值、中间值、最小值拆出来，分别乘以对应的单位代价，加起来。这种情况下启发式非常贴近真实代价，搜索节点数最少，效率最高。代码也不复杂，就是把三个坐标差排个序的事。

4. Python实现一个完整的3D A*搜索器

4.1 地图与数据结构

下面这段是我在实际项目里整理出来的一个精简版本，完全可以直接运行。整篇文章的代码都用同一个核心类，方便大家对照。

import heapq import numpy as np from typing import List, Tuple, Optional class AStar3D: """3D体素栅格地图上的A*搜索器""" def __init__(self, grid: np.ndarray, allow_diagonal: bool = True): """ Parameters ---------- grid : 三维numpy数组，0=可通行，1=障碍物 allow_diagonal : 是否允许斜向移动，默认True（26邻居） """ if grid.ndim != 3: raise ValueError("grid must be a 3D numpy array") self.grid = grid.astype(np.int8) self.allow_diagonal = allow_diagonal self.shape = grid.shape def _get_neighbors(self, node): x, y, z = node neighbors = [] if self.allow_diagonal: for dx in (-1, 0, 1): for dy in (-1, 0, 1): for dz in (-1, 0, 1): if dx == 0 and dy == 0 and dz == 0: continue nx, ny, nz = x + dx, y + dy, z + dz if not (0 <= nx < self.shape[0] and 0 <= ny < self.shape[1] and 0 <= nz < self.shape[2]): continue if self.grid[nx, ny, nz] == 1: continue move_cost = np.sqrt(dx * dx + dy * dy + dz * dz) neighbors.append(((nx, ny, nz), float(move_cost))) else: for dx, dy, dz in ((1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0), (0, 0, 1), (0, 0, -1)): nx, ny, nz = x + dx, y + dy, z + dz if not (0 <= nx < self.shape[0] and 0 <= ny < self.shape[1] and 0 <= nz < self.shape[2]): continue if self.grid[nx, ny, nz] == 1: continue neighbors.append(((nx, ny, nz), 1.0)) return neighbors

地图就是用numpy三维数组表示的体素栅格。这里我用了int8而不是bool，一方面numpy的bool底层也是uint8，但int8在很多索引场景下更直观；另一方面网格数据后续如果要做膨胀（inflate obstacles）、多分辨率金字塔，int8的空间足够扩展。

_get_neighbors这个方法承担了三个责任：遍历方向集合、做边界检查、做障碍物检查。26邻居的实现就是三层循环遍历-1、0、1的所有组合，把零向量跳过。每次找到合法邻居，顺手算一个真实的欧几里得移动代价。这个方法会被主循环调用无数次，所以我把边界检查和障碍物检查放在同一个if里提前continue，尽量减少无效迭代。

4.2 启发式函数与主搜索循环

下面继续看核心搜索逻辑。

@staticmethod def _heuristic(a, b, h_type="euclidean"): """启发式函数，支持 euclidean / manhattan / diagonal 三种""" dx = abs(a[0] - b[0]) dy = abs(a[1] - b[1]) dz = abs(a[2] - b[2]) if h_type == "euclidean": return float(np.sqrt(dx * dx + dy * dy + dz * dz)) elif h_type == "manhattan": return float(dx + dy + dz) elif h_type == "diagonal": dmax = max(dx, dy, dz) dmin = min(dx, dy, dz) dmid = dx + dy + dz - dmax - dmin return float((dmin * np.sqrt(3)) + (dmid - dmin) * np.sqrt(2) + (dmax - dmid) * 1.0) else: raise ValueError(f"unknown h_type: {h_type}") def search(self, start, goal, h_type="euclidean"): """A*主搜索，返回路径点列表，找不到返回None""" for pt, name in ((start, "start"), (goal, "goal")): if not (0 <= pt[0] < self.shape[0] and 0 <= pt[1] < self.shape[1] and 0 <= pt[2] < self.shape[2]): raise ValueError(f"{name} point out of grid: {pt}") if self.grid[pt] == 1: raise ValueError(f"{name} point is inside obstacle: {pt}") open_heap = [] # 堆元素: (f_score, g_score, node) g_score = {start: 0.0} came_from = {} heapq.heappush(open_heap, (self._heuristic(start, goal, h_type), 0.0, start)) while open_heap: f, g, current = heapq.heappop(open_heap) if g > g_score.get(current, float("inf")): continue if current == goal: path = [] node = current while node is not None: path.append(node) node = came_from.get(node) path.reverse() return path for neighbor, move_cost in self._get_neighbors(current): tentative_g = g_score[current] + move_cost if tentative_g < g_score.get(neighbor, float("inf")): came_from[neighbor] = current g_score[neighbor] = tentative_g f_next = tentative_g + self._heuristic(neighbor, goal, h_type) heapq.heappush(open_heap, (f_next, tentative_g, neighbor)) return None

这里有几个实现细节值得单独说一下。

第一个是堆里存的是(f_score, g_score, node)三元组。这样做的好处是，即使同一个节点因为发现了更短路径而重复入堆，堆也能根据f值正确排序。弹出时用if g > g_score.get(current, float("inf"))判断当前这条记录是不是“过期版本”，如果是就跳过。这个判断极其关键，没有它，过期记录会导致路径回溯出错，甚至死循环。

第二个细节是came_from字典。它记录每个节点是从哪个节点走过来的。找到目标后，沿着came_from一路回溯到起点，再把列表反转，就是一条从起点到终点的完整路径。这里要注意回溯循环的终止条件：起点的parent不存在，所以node = came_from.get(node)拿到None时循环自然结束。

第三个细节是搜索结束返回None的情况。如果堆弹空了还没碰到goal，说明起点和终点之间根本不存在通路，可能是地图被障碍物隔断了。这是非常常见的失败模式，后面会专门讲怎么排查。

4.3 生成测试地图与3D可视化

算法写完了，光看代码跑不出结果总觉得不踏实。我写了一个生成测试地图的函数，让场景可视化出来才有说服力。

def make_test_grid(size=(24, 24, 24), seed=7): """生成一个带'空中墙体'的测试地图""" np.random.seed(seed) grid = np.zeros(size, dtype=np.int8) # 随机障碍物，密度约8% grid[np.random.random(size) < 0.08] = 1 # 人工加一堵'空中墙体'，放在x=8..14, y=6..18, z=4..10 # 这堵墙只挡到第10层，从第11层以上可以翻越 grid[8:15, 6:19, 4:11] = 1 start = (2, 2, 2) goal = (21, 21, 21) grid[start] = 0 grid[goal] = 0 return grid, start, goal

这个场景特意模拟了一个“二维地图无法表达”的困境：一堵立在空间中的高墙，但它的顶部没封死，上方的通道是畅通的。2D规划遇到这堵墙只能绕到两侧，而3D规划可以大摇大摆从墙顶跨过去。这就是三维搜索真正的价值所在。

可视化用matplotlib的三维散点就够了。障碍物全部画成小灰点，路径画成蓝色折线，起点和终点用特殊标记标出来。

import matplotlib.pyplot as plt def visualize(grid, path=None, start=None, goal=None): fig = plt.figure(figsize=(10, 8)) ax = fig.add_subplot(111, projection="3d") obs = np.argwhere(grid == 1) if len(obs): ax.scatter(obs[:, 0], obs[:, 1], obs[:, 2], c="#aaaaaa", marker="s", s=2, alpha=0.4, label="obstacle") if path is not None: path = np.array(path) ax.plot(path[:, 0], path[:, 1], path[:, 2], c="blue", linewidth=3, label="A* path") ax.scatter(path[:, 0], path[:, 1], path[:, 2], c="blue", s=10) if start is not None: ax.scatter([start[0]], [start[1]], [start[2]], c="green", s=80, marker="o", label="start") if goal is not None: ax.scatter([goal[0]], [goal[1]], [goal[2]], c="red", s=80, marker="*", label="goal") ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") ax.legend() plt.tight_layout() plt.show()

4.4 跑起来看效果

主程序调用非常简单：

grid, start, goal = make_test_grid() planner = AStar3D(grid, allow_diagonal=True) path = planner.search(start, goal, h_type="euclidean") print(f"Path found: {path is not None}, nodes: {len(path) if path else 0}") visualize(grid, path, start, goal)

我在自己的电脑上跑这个24×24×24的地图，A*几乎是在几十毫秒内返回结果。路径节点数通常在六七十个左右，路线会从起点出发，绕到那堵“空中墙体”的侧面，然后从墙顶上方翻过去，再落到终点那一侧。把可视化窗口转个角度，一眼就能看出那条路径比2D规划“聪明”在哪。

这里也提一句：如果地图尺寸涨到100×100×100，纯Python版本的A*可能要跑几秒甚至十几秒，这是正常的。性能问题我们在第5章细聊。

5. 常见问题与性能调优实战

5.1 搜索失败的第一时间排查

A*返回None，通常逃不出这几个原因：

1. 起点或终点本身就在障碍物里。这个我在search函数里已经做了显式检查，但现实项目中从传感器读到的起点坐标经常有噪点，很可能“卡”在障碍物边界上。建议在调用前对外层坐标做一次合法性校验，或者做一层腐蚀处理，把靠近障碍物边缘的点自动挪开。

2. 起点和终点不连通。在稀疏障碍物地图里很少发生，但如果障碍物密度超过一定阈值，空间会被隔成几个孤立的子区域。快速判断方法：先跑一次BFS或者使用并查集做连通性分析，如果起点和终点不连通，直接返回“无解”，不用浪费A*的时间。

3. 地图太大，open list被撑爆导致程序卡死。这个不是逻辑错误，而是资源耗尽的工程问题，解法见下面性能调优部分。

5.2 路径不是最优的常见原因

有人跑完A*后发现路径长度跟真实最短路径差了那么一截，往往不是代码写错，而是下面几种情况：

第一种，启发式高估了代价。最常见的是26邻居下用了曼哈顿距离。我见过有人拿着2D代码直接改成3D，只把坐标系换成三个轴，启发式仍然用dx+dy+dz，结果路径走出来的确“看起来合理”，但一测长度明显偏大。这种情况把启发式换成欧几里得距离就可以解决。

第二种，移动代价没对准。比如26邻居下所有移动代价都写了1，搜索会偏爱斜穿，导致路径整体歪向对角线方向。

第三种，地图边界值问题。比如地图原点不是(0,0,0)，而是(100,200,50)，代码里的边界判断用错了参考系，虽然不会报错，但搜索会错过一些本该能走的节点。建议所有坐标都以地图索引为准，统一转换。

5.3 性能优化的三板斧

3D搜索最大的敌人就是维度灾难。100×100×100就是一百万个网格，200×200×200就是八百万个，纯Python的A*在这个规模下会慢得让人怀疑人生。实际项目中我推荐按顺序尝试下面三个方案。

**第一板斧：把g_score和came_from换成更高效的数据结构。**Python的字典很灵活，但开销不小。对于尺寸固定的3D网格，完全可以用三个numpy数组来存：g_score_map = np.full(shape, np.inf)，parent_map = np.full(shape + (3,), -1)。这样索引和更新都是O(1)的numpy操作，比字典快一个数量级。堆还是用heapq，但节点可以用整数索引表示，比如把三维坐标(x,y,z)编码成x * ny * nz + y * nz + z，入堆时只压一个整数，内存和比较开销都小很多。

**第二板斧：加权A让搜索更快。**把启发式函数乘上一个大于1的系数，比如h(n) * 1.2，A会更快地向目标方向收敛，代价是不再保证全局最优。这个方法在实际工程里非常常用，尤其是无人机路径规划，很多时候“不错的路径”远好于“理论最短但晚三秒才算出来的路径”。具体做法：给A*加一个weight参数，默认1.0，设置为1.2或者1.5时搜索节点数往往能降三分之一以上。

**第三板斧：换算法。**如果地图是均匀代价网格，JPS（Jump Point Search）是一个极其高效的剪枝算法，它能在保持A最优性的前提下，把大量“中间节点”整段跳过，搜索速度提升一个数量级。JPS在2D网格上相当成熟，3D场景也有对应的扩展版本，但实现复杂度明显上升，适合在掌握了基础A之后再研究。

另外一个非常实用的方向是双向A*：从起点和终点同时开始搜索，两边交替扩展，当两边边界相遇时合并路径。在3D大规模地图上，双向搜索能把搜索空间砍掉一大半，实现的改动也不大，只需要两个堆和两套came_from。

5.4 生产环境中的避坑建议

写代码这关过了，真正落地时还会遇到几件破事，挑重点说几个。

**地图分辨率选多大真是够用。**0.5米分辨率和0.1米分辨率，搜索耗时的差距不是5倍而是几十倍，因为网格数量是立方级增长的。做大型户外无人机巡检，我一般先用1米或2米的低分辨率地图搜出一条粗糙路径，然后只在这条路径的通道内用高分辨率做局部细化。这个“由粗到细”的策略比从头到尾用高分辨率地图高效太多。

*动态环境不要用纯A反复重规划。**如果地图里的障碍物会移动，每秒钟重新跑一遍A非常浪费。这类场景更适合DLite，它能在上一次搜索结果的基础上做增量修复，只更新受影响的那部分路径，耗时通常只有全量重规划的十分之一。

**斜穿障碍物角落的问题。**3D栅格中，允许斜着走之后，可能会让路径“擦着”障碍物角落偷过去。对于无人机这种有实际体积的机器人，物理尺寸不能理想化为一个点，路径与障碍物之间必须保留安全距离。工程上通常先把障碍物地图做膨胀处理（把障碍物周围的格子标记为占用），再用A*搜索。膨胀半径根据机器人最大尺寸设定，这一步是必备的，不做的话，算法层面“找到了路”，真机实测一头撞上去。

**纯Python的性能天花板。**如果你的地图长期稳定在300×300×300，纯Python A*基本没法实时跑。我试过用numba给邻居生成函数加@jit装饰器，性能提升非常明显，但要注意numpy数组的类型签名写对。也可以把核心搜索循环改成C++写一个Python扩展，但工作量直线上升。对绝大多数学习和中小型项目而言，numba加速加结构优化已经够用了。

最后分享一点个人体会

3D A看起来只需要把2D代码加一个维度，但真正写完之后你会发现问题都藏在细节里：邻居怎么定义、代价怎么算、启发式选哪一种、堆里面的过期记录怎么处理，每一个都直接决定结果对不对、效率高不高。我的建议是，拿到代码之后一定亲手改几个参数跑一遍。把allow_diagonal关掉，对比路径形态；把h_type换成manhattan，观察路径长度变化；把地图换成全空，看看A能不能走出一条接近直线的路径。这种“破坏性实验”比照着代码抄十遍更管用。

后续如果想继续深入，建议沿着两个方向走：一是把算法换成JPS和D* Lite解决大地图和动态环境的性能问题；二是把栅格搜索的路径输出到真实运动规划链路里，跟样条平滑、速度规划对接，让路径真正可执行。那才是运动规划最出成果的地方。