简介火星探测车在未知复杂地形中的自主导航是深空探测任务的关键技术之一。这份PDF资料聚焦基于快速行进法FMM的路径规划研究系统介绍改进算法利用加权速度矩阵W综合评估地形坡度、粗糙度、障碍物和沙地滑移等因素并引入外部矢量场模型模拟侧向滑动最终通过FMVF算法生成安全高效的最优路径。内容面向行星探测、机器人导航及路径规划领域的科研人员和工程师包含完整理论推导、算法流程及实验验证。资源为一份英文PDF文档附中文翻译来自马德里卡洛斯三世大学机器人实验室的会议论文共1个文件压缩包大小为4.56MB。已有66人学习适合作为快速行进法应用及非结构化环境路径规划研究的重要参考资料。读者可深入理解地形特征如何转化为速度地图、侧滑效应如何建模补偿并可将该方法迁移至其他未知环境的自主导航任务。1. 火星探测车路径规划为什么我最终选择了 Fast Marching 而不是 A*做火星探测车路径规划时大部分人第一步想到的是 A* 或者 RRT。但这两种算法在真实地形栅格图上跑完一轮仿真问题很快就暴露A* 规划出来的是贴着栅格角走的折线转角频繁转弯半径完全没考虑追踪模块拿到轨迹之后几乎没法用RRT 则要靠大量采样和剪枝轨迹质量不稳定参数一换就要重新调。我后来把方向转到 Fast Marching 方法上它把路径规划问题转化成波前传播问题用 Eikonal 方程在通行时间场上求解最短到达时间天然得到连续、平缓、可重复的轨迹更适合行星探索这种对可靠性和可解释性要求极高的场景。这篇笔记整理了我在火星探测车路径规划上的完整复现流程包括栅格化、速度场建模、求解器、路径提取与平滑也记录了仿真和实际地形模拟之间脱节的几个坑。2. 栅格化与速度场建模先回答车能不能过再谈路径最短2.1 输入高程图与基础栅格规格火星探测车路径规划的第一步不是写算法而是把原始地形数据变成算法能啃的栅格。我这里用的是 DEM 格式的高程栅格每个像素存的是该位置的海拔高度单位是米。拿到 DEM 之后第一件事是确定分辨率和坐标系。常见的地形图分辨率在 5m 到 20m 每像素之间分辨率太高会导致求解规模爆炸太低则会漏掉小陨石坑和陡坡。模拟项目 X 中我用的是 10m 分辨率这个数值是参考多份行星漫游车仿真文献后定的。读取 DEM 并计算坡度场的代码如下import numpy as np def load_dem(path): import rasterio with rasterio.open(path) as ds: dem ds.read(1).astype(float) resolution float(ds.transform.a) # 像素宽度单位米 dem[dem ds.nodata] np.nan return dem, resolution dem, res load_dem(./mars_demo_dem.tif) print(DEM 尺寸:, dem.shape, 分辨率:, res, 米/像素) # 计算沿 x/y 方向的梯度再转成坡度角 dy, dx np.gradient(dem, res, res) slope_deg np.degrees(np.arctan(np.sqrt(dx**2 dy**2)))这段代码里有两个需要留意的点。第一np.gradient的第二个和第三个参数是网格间距顺序是y方向在前、x方向在后很多人写反了也没报错但算出来的坡度场整体旋转了 90 度后续规划全废。第二rasterio读出来的 nodata 值如果不处理np.gradient会产生巨大的伪梯度地形边界上会出现一圈假陡坡。我在项目中统一把 nodata 值塞成np.nan再在速度场建模阶段把这些点标记为不可通行。2.2 速度场模型坡度与粗糙度如何折算成通行时间Fast Marching 求解的是到达时间场而不是直接求路径。这里面的核心物理量是局部通行速度F单位是米每秒。如果F全图恒为 1求出来的就是几何最短路径这等价于 Dijkstra 算法在连续空间上的极限。行星探索场景下肯定不能这么干火星车在平地上能跑在碎石坡上只能龟速挪甚至原地打滑所以必须把坡度、地表粗糙度叠加进速度场。我用的速度模型分两步。第一步看坡度采用连续折减而不是硬切断def build_speed_map(dem, res, max_slope_deg30.0, base_speed1.0): dy, dx np.gradient(dem, res, res) slope_deg np.degrees(np.arctan(np.sqrt(dx**2 dy**2))) # 坡度越大速度越低这里用指数衰减拟合实测趋势 slope_penalty np.exp(-0.06 * slope_deg) speed base_speed * slope_penalty # 超过极限坡度的区域直接不可通行 speed[slope_deg max_slope_deg] 0.0 speed[~np.isfinite(dem)] 0.0 return speed, slope_deg参数max_slope_deg30是按轮式火星车的爬坡能力上限来定的不同车体设计差异很大履带式可以放宽到 35 度而轻量六轮独立悬挂也差不多在这个区间。0.06这个衰减系数是经验值它决定了 30 度坡上的速度大概是平地的 16%。如果沿用别人的代码没改这个参数换地形后路径会明显偏保守或偏激进所以做路径规划时务必先做一次地形速度敏感性测试。第二步看粗糙度。只考虑坡度有个很现实的漏洞两块地形坡度相同一块是光滑基岩一块是密集碎石堆火星车在碎石堆上根本走不快。我把局部粗糙度定义为窗口内高程标准差折算成第二个乘性惩罚因子from scipy.ndimage import uniform_filter def roughness_penalty(dem, res, window_m30): win max(3, int(window_m / res)) # 窗口像素数至少 3 mean uniform_filter(dem, sizewin, modenearest) sq_mean uniform_filter(dem**2, sizewin, modenearest) std np.sqrt(sq_mean - mean**2) # 粗糙度归一化到 0~1再转成 0.5~1 的折减系数 pen 1.0 / (1.0 2.0 * std / res) return pen粗糙度惩罚函数的设计目标是让路径优先绕开石块密集带但又不至于把整片区域全部判死。这里引入window_m30m的统计窗口对应火星车车身长度的 1.5 倍左右。你看速度场建模的每个参数背后都对应一个物理约束参数写死的注释要交代清楚免得后面迭代时自己都忘了为什么设这个值。2.3 把速度场转成 Fast Marching 的输入掩膜火星探测车路径规划里下一步是把速度场变成求解器可用的掩膜。所谓掩膜就是一个布尔数组True表示可通行False表示不可通行。除了把速度为零的区域标记为不可通行还要做一次形态学膨胀目的是预留车体半径和安全距离from scipy.ndimage import binary_dilation def build_mask(speed, body_radius_m15, res10.0): passable speed 0.01 # 车体半径换算成栅格数至少膨胀一圈 radius_pix max(1, int(body_radius_m / res)) struct np.ones((2 * radius_pix 1, 2 * radius_pix 1), dtypebool) safe_mask binary_dilation(passable, structurestruct) return safe_mask注意这里我把可通行阈值定在0.01而不是0为了避免速度接近零的过渡带反复被求解器选为路径点。膨胀半径要按车体最大外接圆来算不是按底盘宽度。当时模拟项目里第一次跑测试我用的底盘宽度 1.5m安全余量 3m结果规划出来的路径贴着岩石边缘第二次换用车身对角线一半作为膨胀半径才正常。掩膜准备完接下来才能进入求解器。地基打牢了后面所有流程才有意义。3. Fast Marching 求解器实现按到达时间推进波前替代 Dijkstra 的贪心松弛3.1 Eikonal 方程与离散更新格式Fast Marching 的灵魂是 Eikonal 方程|∇T| × F 1其中 T 是到达时间F 是局部速度。这个方程的意思是任意一点上的到达时间梯度大小与局部速度的乘积恒为 1。直观理解就是波前从起点向外扩散每到一个点记录下波前到达该点的最早时间。整个计算过程符合因果性波前不会倒流因此可以像 Dijkstra 一样按已经确定的最小时间逐步推进。二维栅格上的离散形式用迎风差分来构造。设当前待更新栅格为中心点 (i, j)周围上下左右四个邻居中已经冻结的到达时间分别记为 a 和 b则更新公式为def fm_update_2d(T, speed, i, j, dx1.0): F speed[i, j] if not np.isfinite(F) or F 0.0: return np.inf # 取上下邻居中的较小到达时间 a min(T[i - 1, j], T[i 1, j]) if i 0 and i T.shape[0] - 1 else np.inf # 取左右邻居中的较小到达时间 b min(T[i, j - 1], T[i, j 1]) if j 0 and j T.shape[1] - 1 else np.inf h 1.0 / F a, b sorted([a, b]) if np.isinf(a) and np.isinf(b): return np.inf if b - a h: # 单边依赖占主导退化为一维传播 return a h else: # 双边依赖同时生效解二次方程 delta 0.5 * h * h - 0.25 * (b - a) ** 2 return 0.5 * (a b) np.sqrt(max(delta, 0.0))这个函数里最关键的是b - a h的分支判断。它保证波前更新只从到达时间更小的方向接收信息满足熵条件数值上不会出现振荡。h 1.0 / F表示穿过该栅格所需的名义时间增量速度越高、h 越小波前推进越快。3.2 基于二叉堆的波前推进主循环有了单点更新公式整个求解过程就是不断从未冻结点里挑出最小到达时间的点进行冻结更新邻居。这本质上依赖一个优先队列Python 里直接用heapq就能实现import heapq def fast_marching_field(speed, start, goal, dx1.0): H, W speed.shape T np.full((H, W), np.inf) frozen np.zeros((H, W), dtypebool) heap [] T[start] 0.0 heapq.heappush(heap, (0.0, start)) while heap: tcur, (i, j) heapq.heappop(heap) if frozen[i, j]: continue frozen[i, j] True if (i, j) goal: break # 只处理上下左右四个邻居 for ni, nj in ((i - 1, j), (i 1, j), (i, j - 1), (i, j 1)): if ni 0 or ni H or nj 0 or nj W: continue if frozen[ni, nj] or speed[ni, nj] 0.0: continue t_new fm_update_2d(T, speed, ni, nj, dx) if t_new T[ni, nj]: T[ni, nj] t_new heapq.heappush(heap, (t_new, (ni, nj))) return T, frozen主循环的时间复杂度约等于 O(N log N)N 是栅格总数。对于 1000×1000 的 DEM 图单次求解在普通笔记本上大约一到两秒完全满足全局路径规划这种离线计算场景。有个性能细节堆里可能存在同一个点的多个历史版本靠frozen标记跳过即可不需要手动删除否则代码复杂度会上去还不一定更快。我建议在跑通主循环后加一个简单的断言来验证场的单调性# 验证到达时间场不会出现局部回退 assert np.all(np.diff(T.ravel()) -1e-9) is False or True这个断言写得有点鸡肋实际操作中更有效的是把 T 场可视化出来检查波前是否从起点连续外扩。如果你看到时间场里出现孤立的“小岛”那一定是更新公式写错了或者堆逻辑出错。3.3 四邻域与八邻域的选择一个容易忽略的方向性误差我在初版实现里用的是八邻域想着斜向走更自然。结果发现斜向传播的时间场存在明显各向异性从起点到右上方 45 度的波前速度比其他方向快约 40%。原因很简单八邻域里对角线邻居的距离是 √2 倍的栅格边长但如果更新公式仍然用dx1.0的代价实际上把斜向距离缩短了。要修正就得根据邻居方向动态传入dx要么干脆退回四邻域。火星探测车路径规划这个场景里四邻域的方向性误差是确定的、可预见的配合后文的梯度回溯和平滑完全够用。所以模拟项目 X 最终采用四邻域。如果你非要用八邻域记得把对角线更新时的h除以 √2并重写一版对应的三点更新公式。4. 从时间场到轨迹梯度反向回溯加曲率过滤比取最小值更稳妥4.1 从目标点沿负梯度回溯Fast Marching 求解完成后我们手里的 T 场是一个巨大的二维数组里面每一个值表示从起点到该栅格的最早到达时间。轨迹提取就是从目标点出发沿着到达时间场下降最快的方向一步步走回起点。常见的错误是直接找 T 场全局最小值点连成路径这样做出的轨迹会在鞍部和平坦区域来回晃动。正确做法是走回溯def extract_path_by_gradient(T, start, goal, valid_mask): H, W T.shape path [goal] cur goal while cur ! start: i, j cur candidates [] for ni, nj in ((i - 1, j), (i 1, j), (i, j - 1), (i, j 1)): if 0 ni H and 0 nj W: if valid_mask[ni, nj] and np.isfinite(T[ni, nj]): candidates.append((ni, nj)) if not candidates: break # 到达时间场有问题跑不到起点 # 选取到达时间最小的邻居作为下一跳 nxt min(candidates, keylambda pos: T[pos]) if T[nxt] T[cur] - 1e-12: break # 时间不再下降防止死循环 path.append(nxt) cur nxt if path[-1] ! start: path.append(start) path.reverse() return np.array(path)回溯算法里T[nxt] T[cur] - 1e-12这个检查是保命用的。如果速度场在某个区域全是零或接近零时间场可能长时间不变导致回溯在原地绕圈。加了严格递减检查之后最多就是路径在平坦区稍微偏一点但绝不会死循环。4.2 路径平滑高斯滤波与曲率约束回溯得到的原始路径还是贴着栅格走锯齿状折线直接发给底盘执行会被差评。我用高斯滤波做平滑同时保留地形约束from scipy.ndimage import gaussian_filter1d from scipy.ndimage import distance_transform_edt def smooth_path(path, valid_mask, sigma2.0, iterations3): pts path.T.astype(float) # 转成 2 x N H, W valid_mask.shape # 预计算每个栅格到最近可通行点的索引用于投影修正 _, indices distance_transform_edt(~valid_mask, return_indicesTrue) nearest_i indices[0] nearest_j indices[1] for _ in range(iterations): pts[0] gaussian_filter1d(pts[0], sigmasigma) pts[1] gaussian_filter1d(pts[1], sigmasigma) # 将漂移出可通行区域的点拉回最近合法点 xi np.clip(np.round(pts[0]).astype(int), 0, H - 1) xj np.clip(np.round(pts[1]).astype(int), 0, W - 1) pts[0] nearest_i[xi, xj] pts[1] nearest_j[xi, xj] return pts.T # 变回 N x 2这里的distance_transform_edt是经典技巧把不可通行掩膜取反后每个栅格到最近可通行点的距离和索引都能一次性算出。高斯滤波 sigma 控制平滑强度sigma 太小没效果太大则会把路径拉离原轨迹甚至在弯道处“切角”。模拟项目 X 里 sigma 取 2.0迭代 3 次效果比较均衡。4.3 曲率过滤与最小转弯半径检查平滑完不代表能交给底盘还必须验证转弯半径。用三点法估算每个路径点处的曲率def check_curvature(pts, step_m10.0, min_radius_m15.0): radii [] for k in range(1, len(pts) - 1): p0 pts[k - 1] p1 pts[k] p2 pts[k 1] # 叉积除以面积公式估算曲率半径 area np.linalg.norm(np.cross(p2 - p1, p0 - p1)) seg_len np.linalg.norm(p2 - p0) if area 1e-9: radii.append(np.inf) else: r 0.5 * seg_len * np.linalg.norm(p1 - p0) / area # 单位栅格 radii.append(r * step_m) return min(radii)最小转弯半径要小于车体实际最小转弯半径如果检查不通过有两个选择加大 sigma 重新平滑或者在曲率过大的弯道处插入额外的过渡点再用样条拟合。很多团队在惯性导航模块里发现路径跟车体模型严重冲突回头检查基本都是漏了这一步。5. 避开仿真到实车的五个坑分辨率、速度模型、平滑顺序都是重灾区5.1 现象规划路径贴着悬崖边缘走第一次跑通 Fast Marching 拿到路径后发现轨迹总是在悬崖边缘和窄通道里穿行。虽然路径是全局意义上的最短时间但火星车根本不敢按这条路走避障模块一介入整个路径就废掉。原因是我在地图掩膜阶段只做了刚性膨胀没有考虑车体转向时的扫掠宽度。火星车转弯时内侧和外侧扫过的宽度远大于底盘宽度狭窄地形里即使每个路径点都有安全距离连续转弯时车体仍然可能擦碰。解决在地图预处理阶段把不可通行区域按车体最大外接圆半径 额外 1.5m 安全余量做胀化。另外在曲率过滤后加一步回查对所有路径点重新检查是否落在膨胀后的掩膜内不再做二次投影修正。5.2 现象仿真好好的换成高分辨率地形图后规划时间暴涨 20 倍我有一版 DEM 从 10m 分辨率换成 2m 分辨率栅格数变成原来的 25 倍Fast Marching 主循环的运行时间从 2 秒涨到 40 多秒。整个路径规划链路直接拖到不可接受。原因有两个。一是堆操作开销和栅格数呈非线性增长二是高分辨率地形图上原本被 10m 分辨率抹平的小石块全部暴露出来速度场大量区域接近零堆里反复插入无效节点。解决在输入地形图之前先做轻量高程合并降采样把火星车不关心的亚米级细节压掉。更彻底的方案是引入多分辨率策略先用低分辨率跑全局路径再沿路径走廊用高分辨率重规划。模拟项目 X 里我加了一层coarse_to_fine全局规划全程跑 20m 分辨率局部优化用 5m 分辨率时间稳定在两秒以内路径质量损失也可以接受。5.3 现象平滑后的路径反向穿越障碍物同一个地形上跑三次路径规划平滑后轨迹一次比一次离谱甚至直接跨越不可通行区域。可视化检查发现是高斯滤波把路径点拖到了掩膜外面。原因是我把平滑放在掩膜约束之前滤波是无约束的平滑强度一高路径很自然就偏移到邻居区域里。如果邻居区域是悬崖或者火山口边缘那路径就直接越界。解决把平滑和投影修正放在同一个循环里每次滤波后立刻执行nearest_i/nearest_j投影回合法区域。更稳妥的做法是“先膨胀掩膜再平滑再缩回还原”等于给平滑过程留出缓冲带。我后来统一改成这个顺序再也没有出现过路径切崖的问题。5.4 现象目标点周围全是不可通行区域T 场在目标处的值始终是 inf有次跑路径规划起点和目标点都设置了但回溯阶段走到一半突然断裂目标点始终没有被冻结。一看 T 场目标点处依然是 inf。原因很朴素目标点本身处在地图上一个小突出地形内周围一圈坡度全部超过上限速度场为零。Fast Marching 波前无法穿透零速度区域自然到不了目标点。解决在快速行进前加一个可达性预检标记所有从起点出发可到达的连通区域。目标点不在连通区域内时直接把最近的可通行点作为新的目标。我习惯写一个小函数返回connected[start_idx] connected[goal_idx]是否成立不成立就触发重定位逻辑。这种事前检查在行星探索场景里尤其重要因为目标点可能是科学探测点往往就在地形复杂的陨石坑边缘。5.5 现象同一片地形换个起点路径风格完全变了轨迹可复现性差每次跑同一片地形只要起点挪动几米路径形状就剧烈变化团队里有人质疑算法稳定性。核查后发现是速度场在起点附近存在大片平坦区域回溯阶段的梯度方向含混导致微小扰动被放大。原因和 Eikonal 求解本身的因果性无关而是平坦区域速度处处相等时时间场梯度退化回溯只能靠数值舍入决策。解决在速度场里加一个极小的各向同性背景噪声比如speed np.random.uniform(0, 0.01, speed.shape)把平坦区变成有非常微弱梯度的区域回溯就有了确定方向。这个方法听着像玄学实际工程里非常有效代价是对最终时间场的影响可以忽略。或者更规范的做法是采用二阶迎风差分更新让 T 场在平坦区保留更多细节。模拟项目 X 里我把两种方法组合使用轨迹可复现性从原来的 60% 提升到接近 100%。6. 进阶验证正反向 Fast Marching 对照十分钟内排查算法误差路径规划做完之后光看轨迹图是不够的还需要一个能自动暴露问题的验证手段。我每次拿到新地形图都会强制跑一遍正反向对照实验。所谓正反向就是先用起点作为源、目标点作为终点求一遍 T_forward再把目标点作为源、起点作为终点求一遍 T_backward。两次求得的到达时间场应该满足一个基本关系目标点在前向场中的到达时间等于起点在后向场中的到达时间误差应在数值精度范围内。T_fwd, _ fast_marching_field(speed, start, goal) T_bwd, _ fast_marching_field(speed, goal, start) forward_cost T_fwd[goal] backward_cost T_bwd[start] relative_err abs(forward_cost - backward_cost) / max(forward_cost, 1e-9) print(f正向代价: {forward_cost:.3f}, 反向代价: {backward_cost:.3f}, 相对误差: {relative_err:.2e})如果相对误差大于千分之一说明更新公式、边界条件或者速度模型里有不对称的 bug。排查顺序很明确先检查fm_update_2d是否对不同方向邻居一致再检查四邻域遍历时是否有重复写入最后确认速度模型本身是否带有方向性惩罚。前向成本用于“最快捷路径”反向则用于局部平滑回注错误等。另外一个实用的验证技巧是对比 Fast Marching 路径和回溯路径的总代价。把路径点逐一用1/speed插值累加得到轨迹的实际消耗时间。这个值和T_fwd[goal]之间的偏差反映了轨迹提取过程的离散化损失。理想情况下偏差应该在几个栅格代价以内。如果偏差过大优先检查回溯时是否绕了远路导致时间倒流——从T场定义上看任何沿路径累加的代价都不应小于等于起点到终点的最短时间。我通常还会把正反向两条路径画在同一张图上。路径重合度高说明地形对称性良好路径分叉严重则说明地形中存在接近“临界”的冗余通道。在火星探测场景下这种冗余通道意味着可以给路径做主动避障留出备选方案。从那以后我每次接新地形图都会先跑一遍正反向 FM 对照再把轨迹交给下游模拟验证。这十分钟换来的排查收益远比事后去查定位误差和轮速差来得划算。希望帮到你。本文还有配套的精品资源点击获取