简介火星探测车在复杂地形中的路径规划是行星探索的关键技术。这份PDF研究资料聚焦基于快速行进法FMM的改进算法面向机器人导航、行星探测及自动驾驶领域的学习者与研究人员系统阐述如何综合坡度、粗糙度、障碍物和沙地滑移等要素构建加权速度矩阵W并引入外部矢量场模型模拟侧向滑动最终通过FMVF算法生成安全高效的最优路径。资源为单篇PDF文档文件约4.56MB便于下载后随时翻阅或打印内容包含问题建模、算法推导与实景验证案例适合作为相关课题的算法参考与写作依据。已有66人学习该资源足见其在行星探测路径规划方向具有一定参考价值。通过对FMM改进思路的完整呈现读者可快速掌握复杂地形下自主导航的建模方法为后续深空探测或地面机器人在未知环境中的路径优化提供可直接借鉴的技术路径。1. 火星探测车路径规划先别急着做避障想清楚“这条路能不能让车活下去”火星探测车路径规划说起来是“从 A 点走到 B 点找条路”实际干起来是另一码事。地球上做机器人导航地图是激光雷达厘米级扫出来的路面是柏油或水泥规划器只需要考虑“别撞”。到了火星地图来自轨道器拍的高程数据精度按米算地面可能是松软沙地、碎石坡、岩脊规划出来的路径不仅要“能走”还得保证车轮不打滑、车体不侧翻、悬架不被卡死。换句话说这是一套把地形可通行性、车体运动学、感知不确定性一起塞进搜索代价的决策系统。本文适合正在做移动机器人导航、想把方案往野外或行星探测场景迁移的开发者也适合想搞懂规划器在地面场景之外怎么改的从业者。我会从模型假设讲到全局搜索、局部避障再给一份能跑通的参考实现和踩坑记录。2. 车体与地形建模规划器眼里的火星不是一张平整栅格图2.1 六轮摇臂悬架的运动学别拿阿克曼模型硬套火星车的底盘结构和地面机器人差异很大。常见的是六轮摇臂悬架六个轮子分布在车体两侧每个轮子通过摇臂与车体连接左右摇臂再通过差速器耦合。这种结构的核心优势是在跨越岩石或沟槽时车体俯仰变化远小于单轮抬升量六个轮子始终尽可能贴合地面。但这给运动学建模带来一个麻烦——它既不是两轮差分模型也不是阿克曼转向模型。规划器需要的是一个“足够准但不过度复杂”的车体模型。我的做法是把六轮摇臂悬架简化为一个带独立轮速和前后轮转向的单体模型忽略摇臂的动态摆动只保留两个关键约束转向角限位和悬架行程。转向角限位决定车能不能原地转悬架行程决定车体在多大地形落差下不会“架空”。这两条直接写进代价函数比建一套完整的悬架动力学更有性价比。import math def evaluate_pose(dem, x, y, yaw, rover): # dem: 2D 高程数组, 单位米 # rover.wheel_offsets: 六个轮子在车体系下的局部坐标 [(lx, ly), ...] # rover.track_width: 左右轮距, 单位米 wheel_z [] for lx, ly in rover.wheel_offsets: wx x lx * math.cos(yaw) - ly * math.sin(yaw) wy y lx * math.sin(yaw) ly * math.cos(yaw) wz sample_height(dem, wx, wy) # 双线性插值查高程 wheel_z.append(wz) left_z sum(wheel_z[::2]) / 3.0 # 左侧三个轮 right_z sum(wheel_z[1::2]) / 3.0 # 右侧三个轮 roll math.atan2(left_z - right_z, rover.track_width) # 硬约束侧倾超过阈值, 直接判不可通行 if abs(roll) math.radians(15): return float(inf) # 硬约束轮间高差超过悬架行程, 车体可能被架空 if max(wheel_z) - min(wheel_z) rover.suspension_travel: return float(inf) # 软惩罚侧倾越大代价越高, 但不到不可通行的程度 return abs(roll) / math.radians(15)这段代码把运动学问题变成了“六个轮子的高度采样”。sample_height用双线性插值从 DEM 里取高度避免栅格跳变造成代价抖动。侧倾阈值 15° 不是随便拍的它对应车体静态翻覆角的 60% 左右——规划路径时按 15° 限制留给局部避障和动态扰动至少 10° 的余量。suspension_travel取悬架最大行程的 70%因为实际地形里轮子可能同时压到凸起和凹坑行程余量太小会让代价函数过度敏感。这里有一个容易被忽略的点六个轮子的高度方差不能只看最大值和最小值。火星车越过一块 0.3 米高的岩石时单轮抬升 0.3 米可能没事但如果对角线两个轮子同时抬高车体就会产生扭转载荷。所以我会在代价函数里额外加一项“对角轮高差”超过阈值同样判不可通行。这个细节在仿真里可能看不出来实车底盘设计时会非常在意。2.2 从地形到代价地图坡度、粗糙度、岩石密度怎么“翻译”成数字轨道器给的 DEM 分辨率通常在 1~5 米每像素这个精度做全局规划够用做局部规划远远不够。所以地形代价要分层处理全局层用 DEM 提取坡度、粗糙度、岩石密度合成一张栅格代价地图局部层用车载立体相机生成 0.1~0.2 米分辨率的局部高程图在线更新。两层地图的分辨率不同但坐标系必须严格对齐否则全局路径和局部轨迹会在切换点打架。坡度计算不要直接用中心差分。DEM 本身有测量噪声直接对原始栅格做梯度运算会把单个像素的噪声放大成几度的伪坡规划器就会绕开一段实际很平缓的区域。我习惯先做梯度再做窗口平滑顺序不能反过来——先平滑会磨掉真实的地形脊线规划出的路径可能贴着悬崖边走。import numpy as np from scipy.ndimage import uniform_filter def compute_slope(dem, cell_size, window5): # dem: 2D numpy array, 单位米 # cell_size: 单像素对应地面尺寸, 单位米 dzdy, dzdx np.gradient(dem, cell_size, cell_size) gx uniform_filter(dzdx, sizewindow) gy uniform_filter(dzdy, sizewindow) slope np.arctan(np.sqrt(gx**2 gy**2)) return slopecell_size是栅格的实际尺寸决定梯度数值。window5 对 1 米分辨率的 DEM 相当于 5 米尺度上的平均坡度能滤掉单块岩石造成的伪坡但保留坡体趋势。如果你用的 DEM 分辨率是 2 米window 可以缩到 3因为物理尺度已经翻倍了。粗糙度我用另一个指标窗口内高程残差的标准差。先对 DEM 做窗口均值用原始高程减均值得到残差再统计残差的 RMS。粗糙度代价值和高程方差成正比但要注意别把岩石密集区域直接标成不可通行——火星车恰恰需要利用岩石之间的空隙通过粗糙度很高但缝隙足够宽时应该只增加时间代价而不是硬堵死。岩石密度检测依赖能否从高程数据里分辨出凸起物。我的做法是对 DEM 做形态学顶帽变换提取高于周围地面的局部凸起再统计每百平方米的岩石数量和平均尺寸。这块信息在全局层面不精确但可以作为全局路径的“绕行倾向”岩石多的区域代价上浮引导 A* 优先走开阔地带把精细避让交给局部规划器处理。2.3 不确定性的代价DEM 误差和感知盲区不该被当成“平整”DEM 不是真值。轨道器影像生成的 DEM 在坡度大的区域有系统性误差在阴影区域甚至有大片空洞。很多人在做代价地图时直接把空洞插值填上当成普通地形。这是我在项目里踩过最典型的坑规划器会穿过一条 DEM 上看起来平坦、实际上是悬崖阴影的“假山谷”。我现在的做法是给每个栅格额外存一份“置信度”值。置信度来源于三个渠道DEM 原始数据是否有值、是否有相邻像素高程突变、是否在感知盲区内。置信度低的栅格不会简单标成障碍而是给一个上浮的代价——比如普通地形的 3 倍。这样全局规划器在距离相同的情况下会优先选择置信度高的走廊而不是穿越大片未知区域。这个思路和局部规划里的未知区域处理是一致的我只是把同一套逻辑一直延伸到全局代价层。另一个工程细节DEM 空洞插值要标注来源。如果某一片区域是插值出来的在路径平滑阶段要把它当作“软障碍”路径尽量从边缘绕开而不是穿过去。你可以用 scipy.ndimage 的距离变换把空洞区域扩张一圈让规划路径和空洞边缘保持至少一个栅格的距离。这个安全距离在全局规划里看起来微不足道但到了局部规划器接手时它决定了车会不会被迫在悬崖边急转弯。3. 全局路径规划从 DEM 代价地图到无碰撞走廊的搜索实现3.1 A* 搜索8 邻域与 16 邻域、启发函数与上坡代价的取舍全局规划要回答的问题是给定起点和终点在代价地图上找一条总代价最小的路径。候选方案里有 Dijkstra、A*、RRT、RRT*、Theta*、Field D*。我在室内仓库场景常用 RRT 系列因为障碍物是任意多边形栅格化损失大。但火星车场景不同地形是连续栅格场代价是各向异性的往下坡走和往上坡走的代价不一样A* 配合栅格邻域展开最直接也最容易把坡度代价塞进 g(n)。A* 的核心是 f(n) g(n) h(n)。g(n) 是起点到当前格子的累积代价h(n) 是启发函数。很多人直接用欧氏距离但我们的代价函数里包含高度变化启发函数也必须考虑高程差。我用的是“欧氏距离 高度差惩罚”的加权启发h_weight 取 1.1~1.3。权重等于 1 时保证最优解但路径会贴着不可通行区域边缘走稍大于 1路径会偏向直行方向反而留出安全距离。import heapq def a_star(cost_map, start, goal, h_weight1.2): # cost_map: 2D, 每个格子是通行代价, inf 表示不可通行 # start / goal: (row, col) open_heap [(0.0, start)] came_from {} g_score {start: 0.0} rows, cols cost_map.shape while open_heap: _, current heapq.heappop(open_heap) if current goal: return reconstruct_path(came_from, current) for dr, dc in neighbors_8(): nr, nc current[0] dr, current[1] dc if not (0 nr rows and 0 nc cols): continue if cost_map[nr, nc] float(inf): continue move_cost cost_map[nr, nc] # 上坡惩罚目标点高于当前点时, 代价放大 if dem[nr, nc] dem[current[0], current[1]]: move_cost * uphill_factor tentative_g g_score[current] move_cost if tentative_g g_score.get((nr, nc), float(inf)): came_from[(nr, nc)] current g_score[(nr, nc)] tentative_g f tentative_g h_weight * heuristic((nr, nc), goal) heapq.heappush(open_heap, (f, (nr, nc))) return None邻域展开我默认用 8 邻域。16 邻域能减少路径的锯齿感但 A* 的搜索空间几乎翻倍而且 16 邻域里斜对角步长和直线步长不一致距离系数容易写错。我的习惯是全局地图超过 500×500 就只用 8 邻域把平滑交给后面的路径平滑模块而不是靠加密邻域硬凑。上坡惩罚系数uphill_factor的取值要看你规划的车型。实验室里的小型四轮探测车电机扭矩余量大系数可以设 1.2大型六轮车在松软沙地上的牵引力裕度很小系数要到 2.0 甚至 3.0。这个系数有一个直观的等价关系它代表“上 1 米坡”的代价相当于平地上走几米。规划器最终选择的多是缓坡绕行路线而不是最短直线。3.2 路径平滑与曲率约束折线变轨迹但别把障碍压回去A* 出来的路径是一串栅格中心点连成的折线带 45° 拐角直接下发给底盘跟踪车会频繁启停转向对功耗是灾难对轮地附着力也是考验。所以后面必须接平滑模块。常见做法有两类一是对折线做三次样条插值二是用梯度下降把路径推向障碍物反方向。我推荐后者。样条插值的问题是它只拟合几何形状不感知代价地图插值结果可能重新压到高代价区域甚至障碍上梯度下降则直接把代价地图梯度纳入优化每条路径点都知道“哪边是高坡、哪边是悬崖”。def smooth_path(path, cost_map, alpha0.1, beta0.4, iters200): new_path path.copy() for _ in range(iters): for i in range(1, len(new_path) - 1): prev, curr, nxt new_path[i-1], new_path[i], new_path[i1] # 拉直项: 让三个点尽量共线 straight prev nxt - 2 * curr # 斥力项: 沿代价地图梯度反方向推离高代价区 grad grad_of_cost(cost_map, curr) new_path[i] curr alpha * straight - beta * grad return new_path拉直项里的alpha控制路径张力太大路径会被拉成直线但可能再次贴近障碍物太小平滑效果不明显。我一般先固定 alpha0.1再调 beta 看路径与不可通行区的最小距离。每迭代 50 次打印一次“路径与障碍最小距离”如果收敛后这个距离小于车体半宽加 0.5 米安全余量就把 beta 调大 30% 重跑。注意grad_of_cost要在代价地图的平滑版本上取梯度直接用原始代价地图会产生剧烈梯度把路径推得扭曲。3.3 全局路径的刷新策略什么时候该重新搜、什么时候局部绕一下全局路径规划不是只跑一次就结束。火星车驶过一段区域后车载感知会更新局部代价地图可能发现全局路径上有一块 DEM 没识别出来的大岩石。这时候如果立刻触发全局重规划A* 会搜出一条完全不同的大绕行路线车就得大幅转向耗时耗能如果不重规划局部规划器又没有能力绕过太大的障碍区域。我的策略是分级处理局部障碍的覆盖范围小于全局路径长度的 30% 时不触发全局重规划只修改局部代价层让局部规划器绕完再接回原全局路径超过 30%说明原路径走廊已经失效才清空全局代价层重新搜索。这个 30% 阈值来自我的实测经验低于它时绕行的累计代价小于重搜的代价高于它时旧路径已经失去参考价值。刷新全局路径还需要一个“防抖动”机制。火星车的 DEM 是静态先验全局代价层不会频繁变化但如果你的系统里加入了实时更新的全局地形层A* 每次重搜的结果会有差异车头会来回调整方向。我加了一个最小重规划间隔——两次全局重规划之间至少间隔 60 秒且终点方向偏差不超过 15° 时保持原路径不变。这两个参数非固定值视车速而定但“重规划要滞后、要带死区”这个原则通用。4. 局部路径规划动态窗口避障与未知区域的速度决策4.1 DWA 的速度采样与轨迹推演在算力有限时怎么做实时规划全局路径规划基于先验地图但火星车实际行驶时轨道器地图里一个 1 米分辨率的像素可能是一块 0.6 米高的岩石。车载立体相机要到 10 米距离才能看清这个尺度的障碍再远就只能靠猜。所以局部规划必须基于实时感知每几百毫秒重新决策一次。局部规划器我倾向用 DWA动态窗口法的变体。DWA 的思路很直接在当前速度空间里采样一组线速度和角速度对每个速度组合推演一小段轨迹用代价函数打分选分数最高的轨迹执行。“动态窗口”指的是每个时刻可达到的速度不是全空间而是受电机加减速能力限制的一个窗口——车不可能瞬间从前进变成后退这是物理约束。def dwa_plan(global_path, local_costmap, state, speed_window, dt0.5): best_traj, best_score None, -float(inf) for vx in linspace(speed_window.vx_min, speed_window.vx_max, 8): for wz in linspace(speed_window.wz_min, speed_window.wz_max, 8): if not accel_feasible(state, vx, wz): continue traj simulate_trajectory(state, (vx, wz), horizon2.0, dtdt) if in_collision(traj, local_costmap): continue score ( - distance_to_goal(traj[-1], global_path) * 0.4 - curvature_cost(traj) * 0.3 - clearance_cost(traj, local_costmap) * 0.3 ) if score best_score: best_score, best_traj score, traj return best_traj轨迹推演用简单的前向运动学给定线速度、角速度和起点姿态按时间积分。dt 取 0.5 秒推演时长 2 秒对应火星车 0.1~0.2 m/s 的巡航速度正好是 0.2~0.4 米的距离尺度——这个尺度匹配立体相机的有效测距范围。速度采样密度 8×8 已经够用因为 DWA 的动态窗口本身就窄采样太密对效果提升有限只会增加计算压力。curvature_cost我用来惩罚频繁换向。火星车的转向机构是轮组独立转向理论上可以原地转但频繁转向会大量消耗电量也会扰动车上的科学载荷。这里罚函数不需要太强只要让规划器在路径平滑时优先选择大半径弧线即可。4.2 权重调参距离项、避障项、方向保持之间的平衡DWA 打分函数里的三个权重决定了车的“性格”。距离项权重太大车会沿着全局路径硬切来不及躲开局部障碍最后靠刹车停住避障项权重太大车会在两块石头之间来回试探甚至原地打转。我在项目里的习惯是先固定距离权重为 1然后把避障和方向保持权重调到让车能顺利穿过一个“宽度等于车体宽度 1.2 倍的窄缝”。窄缝能过说明避障项不会过度保守不会反复横跳说明方向罚函数起了作用。这三项权重的绝对值远没有比例重要。你要记录的是不同地形下的行为差异松软沙地里距离项权重应该下调因为急停急转更容易刨坑岩石区里避障项要上调但要注意别把车逼到悬崖一边。这些参数必须放到仿真里用同一张地图反复跑再把轨迹投影到 DEM 上看与地形的关系光看数值曲线很难判断问题出在哪。一个血泪经验DWA 的轨迹推演用的是理想运动学火星车在沙地上存在明显滑转实际转角小于命令转角。如果你发现仿真里路径跟踪很顺、实车上车头却总往一边偏大概率是滑转率没建模。临时办法是在打分函数里加一个“方向保持”项减小换向次数减少滑转累积根本办法是等视觉里程计数据回来用实际位移反推滑转率再补偿到轨迹推演里。4.3 未知区域的速度决策感知盲区怎么处理才不会让车“闭眼冲”局部代价地图里有的区域相机没看到有的是阴影匹配失败导致高程无效。如果你把未知区域当成障碍车会频繁刹停在复杂地形里根本走不动如果把未知区域当成自由空间车可能直直冲进一个沙坑。正确的处理是第三类把未知栅格的代价设成一个“中等偏高”的值并限制这段区域内的最大速度。# 代价标记约定 # 0: 已知可通行 # 1: 未知区域 # 2: 已知障碍 if cell 2: cost float(inf) elif cell 1: cost 5.0 # 比普通地形高, 但允许通行 max_speed_limit 0.05 # 巡航速度的 30% 左右 else: cost 1.0未知区域的速限是关键参数。设太保守车每走几米就停下来等感知刷新整体效率反而低设太激进车会在视觉盲区里全速冲过一块可能翻车的凸起。我通常取巡航速度的 30%~50%并且在感知距离内没有发现大型障碍时才允许恢复到正常巡航速度。这样的行为表现是车在接近未知区域时主动减速看清了再提速而不是猛踩刹车。另一个被很多人忽略的点局部规划器的目标点不能是整个全局路径。如果把全局路径从起点到终点整条塞给局部规划器距离项会被远处的路径点主导车会走出奇怪的大弧线。我通常只取全局路径上距离车体 3~5 米内的路径点作为局部目标。这个前瞻距离也不是越大越好——太大局部规划器对突然出现的障碍反应迟钝太小车在蜿蜒地形里会走“蛇形线”。我一般用“巡航速度 × 3 秒”做基准再按地形复杂度上下调整。5. 火星车路径规划避坑指南仿真过关、上路就翻车的 5 个常见问题5.1 现象仿真里每 3 米一次急停实车电机过热规划器每几百毫秒收到一次新的局部代价图每次代价图的变化都会触发轨迹重选车就频繁急停再启动。原因出在两层地图的分辨率不匹配全局 DEM 是 1 米分辨率局部相机生成的是 0.2 米分辨率同一个位置在两套图上的可通行性判断不一致规划器反复改主意表现出来就是走走停停。解决方法是把全局代价图插值到 0.5 米局部感知作为增量叠加在底图之上并且给规划器加一个最小重规划间隔——两次轨迹替换至少隔 2 秒。算力允许的话我还会对局部代价图做时间上的一致性滤波单帧出现的“假障碍”不会立刻触发刹车。5.2 现象规划路径贴着悬崖边缘走看着近但车不敢走A* 的代价地图只编码了“这个格子能不能走”没编码“这个格子离危险区有多远”。就算坡度阈值设得合理搜索算法也会倾向于贴着不可通行区的边缘挤过去因为那里的距离最近。事后看代价地图路径与悬崖边缘近到只有一个栅格。解决办法是对代价地图做一次距离变换把每个格子到最近不可通行区的距离作为一项乘法因子写入代价。距离小于车体半宽的 1.5 倍时代价指数增长。scipy 的distance_transform_edt一次就能算完别自己在栅格上写 BFS慢且容易出错。5.3 现象车在松软沙地原地刨坑规划器却认为“在前进”规划器用的是理想运动学模型车轮打滑时实际位移小于命令位移但规划器不知道还在按轮速积分估算位置轨迹推演与实际严重偏差。朝一个方向开十分钟后车在栅格地图上的位置可能已经偏出去好几米。解决方法是接入视觉里程计对比实际位移与规划期望位移。偏差超过 20% 时把当前地形栅格的摩擦系数下调重新计算该区域的通行代价并触发局部重规划。这块在整个路径规划里最接近玄学——同一个坡度干沙和湿沙的通行性差一倍你只能让规划器从实测数据里反推地形属性而不是预设一个不变的摩擦系数。5.4 现象翻越一块岩石后全局路径被重置车绕了一大圈原因是我前面提到过的重规划策略不对。局部障碍信息被写进了全局代价层A* 重搜时把整条旧路径都否定了新路径与旧路径差异巨大车体大幅转向。解决方法是把障碍物信息只写入局部代价层全局层保持静态只有当局部不可通行的范围超过全局路径长度的 30% 时才触发全局重规划。你可以把这个 30% 打印到日志里每次全局重规划时记录触发原因连续出现三次“单点障碍触发”就说明策略写错了。5.5 现象太阳角度变化导致相机在斜坡上产生阴影误检为障碍立体相机在低角度强光下阴影区域的匹配会失败生成的深度图在这些区域出现空洞或错误高程。规划器如果把阴影当成障碍车会绕开实际上可以通过的斜坡甚至因为路径被阻断而原地转圈。解决方法是把阴影像素在局部代价图里标记为“未知”而不是“障碍”并按 4.3 节的速限规则减速通过。同时把“未知区域占比”作为健康度指标记录到遥测里某片区域未知占比超过 30% 时自动降低车速等相机多角度观测后再恢复。这类问题没有一次调参就能杜绝的答案我的习惯是凡是在阴影区域出现过的规划偏差全部留档下次遇到类似光照条件直接套用经验参数而不是临时再调。6. 用一张合成 DEM 验证规划闭环最小复现流程与归档习惯到这里原理和坑都讲完了最后给一条能在一台普通笔记本上跑完的验证路径。我不建议上来就搭完整仿真环境——我自己第一次搭就花了一周时间其中一半在折腾软件版本兼容。先用一张合成 DEM 把算法链路打通再决定要不要上物理仿真。最小闭环是三段用 NumPy 生成一张 200×200 的合成 DEM加入噪声模拟岩石把 DEM 转成代价地图跑 A* 得到全局路径再用 DWA 在相同栅格图上做局部跟踪。整个过程不涉及真实车体但能验证“代价地图设计 → 全局搜索 → 局部避障”这条主链路是否自洽。# 生成合成 DEM 并跑通 A* DWA 的最小骨架 python generate_dem.py --size 200 --noise 0.15 python build_costmap.py --dem output/dem.npy --cell-size 1.0 python run_astar.py --costmap output/cost.npy --start 10,10 --goal 180,150 python run_dwa.py --costmap output/cost.npy --global-path output/path.npy验证时重点看三件事。第一换不同的随机种子生成 DEM跑 A* 看路径是否保持一致。如果路径每次差异很大说明代价地图里坡度计算不够平滑先把第 2 章的窗口调大。第二在 DEM 上手工挖一条宽度等于车体宽度 1.2 倍的窄缝看 DWA 能不能挤过去。如果总是绕路说明避障权重太高我期望的结果是“减速但穿过去”而不是“永远绕路”。第三把终点放进不可通行区域验证 A* 返回空路径时局部规划器进入“原地旋转找路”而不是“无限直线前进”。这一步最容易漏漏了实车就会朝着看不见的目标猛冲。最后说一个我自己的工作习惯每调一个参数都把规划前后的代价地图、路径、速度曲线存成 CSV 或图片归档。火星车路径规划的调试90% 的时间在回答“为什么路径从这个格子走而不是那个格子”没有归档就是在裸调。我吃过一次亏调了三天参数觉得效果变好了但没记录改了哪个值最后只能对着代码 diff 一点点猜。从那以后每次跑完实验我都会把参数文件、代价地图快照、路径轨迹打包成一个带时间戳的目录留在本地。这个方向没有银弹多留一份记录下次翻车能省半天。希望帮到你。本文还有配套的精品资源点击获取