简介面向月球车地形建模与自主导航需求这份Matlab资源覆盖DEM高程生成、局部路径规划和全局路径规划完整流程适合计算机、电子信息、数学等专业学生用于课程设计、期末大作业或毕业设计也便于研究者快速搭建仿真基线。代码采用参数化编程关键参数如坡度、阴影代价及边界条件均可灵活调整注释详细运行门槛低。资源包共81个文件其中42个m脚本对应地形生成、代价地图构建和两类路径规划算法29个png图展示不同工况下的规划结果另有pdf总结报告、html说明与markdown笔记辅助理解整体仅5.28MB轻量易部署。目前已有180人学习下载配套多个Matlab版本2014/2019a/2021a兼容支持作者为资深算法工程师可私信交流运行问题能有效降低复现与二次开发成本。1. 月球车 DEM 与路径规划先建图再走路月球车驶下着陆器后第一件事不是踩油门而是回答两个问题周围地形长什么样、往哪条路走最安全。前者靠数字高程模型DEM把三维地形栅格化每个格子存一个高程值后者在 DEM 派生出的代价地图上做搜索分出两个时间尺度——全局规划用低分辨率 DEM 找宏观最优路线局部规划靠实时感知在行驶途中修正偏差。在 MATLAB 里复现这套流程核心链路是点云 → 插值 → DEM → 代价地图 → A* 全局搜索 → 势场或动态窗口局部修正。按这条链路展开每步给出可运行的最小脚本和参数边界适合正在搭移动机器人或无人车路径规划框架的工程师参考新手也能照着把整个工程串起来。2. DEM 生成从点云到栅格高程图的 MATLAB 最小实现2.1 数据来源与点云预处理月球车感知地形的主要手段是激光雷达和双目立体视觉。激光雷达直接输出无序 3D 点云双目视觉通过视差图恢复深度再反投影到三维空间最终都落到一组 (x, y, z) 坐标上。MATLAB 的pointCloud对象可以统一装载这两种来源的数据。拿到点云不能直接插值噪声和点位密度不均会让 DEM 表面出现大量毛刺。第一步是离群点去除和体素降采样% 读取 PCD 格式点云并做预处理 ptCloud pcread(lunar_site.pcd); % 统计滤波参考 10 个邻居标准差阈值 1.5 ptCloudClean pcdenoise(ptCloud, NumNeighbors, 10, Threshold, 1.5); % 体素降采样gridStep 为 0.1 米 gridStep 0.1; ptCloudDown pcdownsample(ptCloudClean, gridAverage, gridStep);pcdenoise对每个点统计其邻域距离分布超过Threshold倍标准差的点判为离群点并剔除。pcdownsample按 0.1 米的体素格子把空间划分成小立方体每个立方体内所有点取平均得到一个代表性点。gridStep就是后续 DEM 的水平分辨率0.1 米适合模拟月球车周围的精细地形如果处理的是大范围着陆区 DEM这个值可以放大到 1 米甚至 5 米以控制计算量。2.2 栅格化散点插值为规则网格DEM 的数学本质是一个规则网格上的高程函数 z f(x, y)。MATLAB 的griddata是散点插值的主力函数支持 linear、nearest、cubic 三种方法。线性插值在点云密度不均时会产生三角面片棱线三次插值更平滑但可能在空洞处过冲。做路径规划场景建议先用线性插值后续验证阶段再对比三次插值的效果差异。% 根据降采样后的点云范围生成规则网格 xRange min(ptCloudDown.Location(:,1)):gridStep:max(ptCloudDown.Location(:,1)); yRange min(ptCloudDown.Location(:,2)):gridStep:max(ptCloudDown.Location(:,2)); [X, Y] meshgrid(xRange, yRange); % 线性插值得到高程栅格矩阵 Z griddata(ptCloudDown.Location(:,1), ... ptCloudDown.Location(:,2), ... ptCloudDown.Location(:,3), X, Y, linear);插值完成后必然出现NaN空洞原因是局部区域没有激光回波比如陡坡阴影处或在传感器盲区内。直接填 0 是错误的会在路径规划中制造出不存在的低洼陷阱。常见做法是用fillmissing做最近邻填充Z fillmissing(Z, nearest); % 再用 3x3 中值滤波去除孤立异常像素 Z medfilt2(Z, [3 3], symmetric);medfilt2的 3×3 窗口对单点噪声很有效symmetric参数让边界处的像素按镜像方式补全避免边缘被滤波后出现偏移。2.3 保存与可视化检查生成的X、Y、Z三个矩阵和gridStep一起存成 MAT 文件后续路径规划脚本直接复用避免每次运行都重新插值save(rover_dem.mat, X, Y, Z, gridStep); figure; imagesc(xRange, yRange, Z); axis xy; axis equal; colorbar; colormap(parula); xlabel(X / m); ylabel(Y / m); title(Lunar DEM after interpolation);这里用imagesc把高程矩阵画成伪彩色图axis xy修正纵轴方向否则图像会上下翻转。保存的文件里同时包含坐标向量和分辨率后续做路径规划时索引换算不会错位。DEM 生成环节的三个关键参数汇总如下参数推荐值影响gridStep / 体素大小0.1 ~ 1.0 m决定 DEM 分辨率越小细节越丰富计算量越大pcdenoise 邻居数8 ~ 15越大对离群点越不敏感可能漏掉真噪点pcdenoise 阈值1.0 ~ 2.5越小剔除越激进可能把真实地形特征当噪声删掉滤波器窗口3×3 ~ 5×5窗口越大表面越平滑但会磨掉小型障碍物提示DEM 生成后一定要做可视化检查。路径规划里最麻烦的穿地问题多半是 DEM 边界处xRange和yRange没有对齐导致索引查错或者空洞区域被填成了极小值。3. 全局路径规划A* 搜索与栅格代价地图搭建3.1 从 DEM 到代价地图的换算逻辑DEM 记录的是绝对高程路径规划真正关心的是这个格子能不能走、走起来有多费力。直接从高程值判断可通行性是新手最常见的误区——一块 20 度的斜坡在 DEM 上看高程变化不大但月球车爬上去会严重打滑。因此要把 DEM 加工成栅格代价地图costmap。常规做法是叠加三层代价信息。第一层是坡度根据中心像素与周围像素的高程差计算梯度角超过车辆最大爬坡角月球车通常取 20~25 度直接置为障碍。第二层是粗糙度用邻域内高程标准差表示标准差大的区域视为碎石堆或岩石区。第三层是自身高度差模拟车体宽度对两侧地形差异的容忍度。function costmap dem2costmap(Z, gridStep, maxSlope, vehicleWidth) [rows, cols] size(Z); % 计算梯度转成坡度角度 [dzx, dzy] gradient(Z, gridStep, gridStep); slopeAngle atan(sqrt(dzx.^2 dzy.^2)) * 180 / pi; % 坡度代价超过阈值标记为 255不可通行否则按比例归一化 costmap zeros(rows, cols); costmap(slopeAngle maxSlope) 255; costmap(slopeAngle maxSlope) uint8(slopeAngle(slopeAngle maxSlope) ... / maxSlope * 254 1); % 粗糙度代价3x3 邻域标准差的归一化值叠加到坡度代价上 rough stdfilt(Z, ones(3, 3)); roughNorm rough / (max(rough(:)) eps) * 50; costmap min(costmap uint8(roughNorm), 255); % 车体宽度约束对代价图做形态学膨胀 se strel(disk, round(vehicleWidth / gridStep), 0); costmap imdilate(costmap, se); end这里stdfilt计算每个 3×3 邻域内高程值的标准差天然反映地表粗糙程度。imdilate用圆形结构元素把障碍区向外扩张半个车体宽度防止规划出来的路径贴着岩石边缘走。结构元素的半径参数vehicleWidth / gridStep是车宽除以 DEM 分辨率比如车宽 1 米、分辨率 0.1 米半径就是 5 个像素。3.2 A* 算法的 MATLAB 实现A* 是栅格全局规划的默认选择在 8 连通栅格上按 f g h 扩展节点g 代表从起点到当前节点的实际代价h 代表从当前节点到目标的启发式估计。MATLAB 实现的核心是 open 集合的维护和路径回溯。function [path, cost] astar_grid(costmap, start, goal) [rows, cols] size(costmap); % 8 连通邻居偏移 neighbors [-1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1]; gScore inf(rows, cols); fScore inf(rows, cols); cameFrom zeros(rows, cols, 2); gScore(start(1), start(2)) 0; fScore(start(1), start(2)) heuristic(start, goal); openSet [start, fScore(start(1), start(2))]; while ~isempty(openSet) [~, idx] min(openSet(:, 3)); current openSet(idx, 1:2); openSet(idx, :) []; if isequal(current, goal) break; end for k 1:size(neighbors, 1) n current neighbors(k, :); if n(1) 1 || n(1) rows || n(2) 1 || n(2) cols continue; end if costmap(n(1), n(2)) 255 continue; end % 距离代价 1 归一化地形代价 tentative_g gScore(current(1), current(2)) ... 1 double(costmap(n(1), n(2))) / 255; if tentative_g gScore(n(1), n(2)) cameFrom(n(1), n(2), :) current; gScore(n(1), n(2)) tentative_g; fScore(n(1), n(2)) tentative_g heuristic(n, goal); openSet [openSet; n, fScore(n(1), n(2))]; end end end % 回溯路径 path []; cur goal; while ~isequal(cur, start) path [cur; path]; cur squeeze(cameFrom(cur(1), cur(2), :)); end path [start; path]; cost gScore(goal(1), goal(2)); end function h heuristic(a, b) h sqrt((a(1)-b(1))^2 (a(2)-b(2))^2); end代价计算里1 costmap / 255这个式子很关键。走一步的基础距离代价是 1再加上归一化的地形代价坡度大的格子代价高、平坦区域代价低。这样规划出的路径会自然绕开陡坡而不是贴着陡坡边缘走也不会在多种路径长度相近时随机选择。启发函数heuristic用欧氏距离8 连通栅格下始终满足可采纳性admissible保证 A* 找到最优解。3.3 A* 参数调节与性能权衡参数起始值调参方向邻域类型4 连通 / 8 连通8 连通路径更短但斜穿障碍角4 连通更保守启发函数欧氏距离换曼哈顿距离可提速但可能非最优地形代价权重1/255权重加大倾向于绕远路走平缓区域障碍阈值255可降到 200 提前排除高成本区域open 集合的维护方式直接影响运行速度。小地图上用一个普通数组排序即可尺寸超过 500×500 的栅格建议换成java.util.PriorityQueue或 C 语言风格的最小堆。另一个提速手段是把costmap从 uint8 换成逻辑数组把坡度判断提前到 A* 主循环之外。提示A* 结束后要检查cost是否为inf。如果是大概率是起点或终点被imdilate后的障碍区覆盖了把目标点向最近的非障碍像素移动一格就能解决。4. 局部路径规划动态窗口法与人工势场法的 MATLAB 对比4.1 全局路径为什么不能直接执行全局路径在离线 DEM 上搜出来假设地形完全静态。但月球车行驶途中会遇到低分辨率 DEM 里看不出来的小型岩石或者车轮打滑导致偏离全局路径。此时需要一个局部规划器在几十米的感知窗口内重新计算一段短轨迹保持车体朝向同时绕开突发障碍。全局规划和局部规划的职责边界是全局负责宏观路线频率低每 10 秒级局部负责微观避障频率高每 0.1~0.5 秒级。在 MATLAB 里做联合仿真通常用一个主循环先调全局 A*得到完整路径点序列每一步先沿全局路径锁定一个局部目标点再交给局部规划器计算控制指令。4.2 人工势场法实现简单但容易陷入局部最优人工势场法把目标点设计为引力源、障碍物设计为斥力源合力方向就是运动方向。MATLAB 实现非常简洁function [vx, vy] potential_field(current, goal, obstacles, k_att, k_rep, rho0) % 引力正比于车到目标点的距离 f_att k_att * (goal - current); f_rep [0, 0]; % 斥力只计算感知窗口内的障碍物 for i 1:size(obstacles, 1) diff current - obstacles(i, :); rho norm(diff); if rho rho0 f_rep f_rep k_rep * (1/rho - 1/rho0) * diff / rho^3 ... / (rho^2 eps); end end force f_att f_rep; vx force(1); vy force(2); endk_att是引力增益k_rep是斥力增益rho0是斥力影响半径。这个算法的最大问题有两个一是障碍物附近容易陷入局部极小值二是在狭窄通道中来回震荡。缓解办法是给引力加一个饱和阈值距离超过一定范围后引力不再增大避免目标点太远时斥力完全被压过。4.3 动态窗口法考虑运动学约束的更优解动态窗口法DWA直接在速度空间采样把线速度 v 和角速度 ω 的可行组合投影到短时间窗口内模拟出一组弧线轨迹用评价函数打分选最优。DWA 天然适合差速驱动的月球车模型function [v_best, w_best] dwa(v, w, pose, goal, costmap, param) % 采样速度窗口 v_samples v(1):param.v_step:v(2); w_samples w(1):param.w_step:w(2); best_score -inf; v_best 0; w_best 0; dt param.dt; simT param.simT; for vi v_samples for wi w_samples % 模拟运动轨迹 x pose(1); y pose(2); theta pose(3); traj [x, y]; for t 0:dt:simT x x vi * cos(theta) * dt; y y vi * sin(theta) * dt; theta theta wi * dt; traj [traj; x, y]; %#okAGROW end % 检查轨迹是否碰障碍 if ~check_collision(traj, costmap) % 评价函数heading distance velocity score heading_score(traj(end,:), goal) ... distance_score(traj, costmap) ... velocity_score(vi); if score best_score best_score score; v_best vi; w_best wi; end end end end endDWA 的三个评价项权重需要按实际需求调节。heading_score鼓励朝目标方向走distance_score鼓励远离障碍物velocity_score鼓励保持一定速度避免停车。在月球车场景建议降低velocity_score的权重因为月面行驶求稳不求快低速通行的安全性远高于快速到达。算法计算量局部最优问题运动学约束适用场景人工势场法低严重无开阔地形、教学演示动态窗口法中不易有窄通道、动态避障快速随机扩展树高无需额外处理复杂构型空间提示DWA 的check_collision不要把整条轨迹铺到全局代价图上查只查车辆周围 5~10 米的局部窗口。全局图分辨率低直接查会把一条本来安全的轨迹误判为穿越障碍。5. 从 DEM 到路径闭环坡度代价与仿真验证技巧5.1 沿路径提取高程剖面路径规划完成后把路径点映射回 DEM提取出沿路径的高程剖面验证坡度是否都在车辆限值内。这一步成本很低但能提前暴露路径穿越陡坡这类问题。% 假设 path 是 A* 返回的 N×2 坐标数组单位与 DEM 一致 demIdxX round((path(:,1) - min(xRange)) / gridStep) 1; demIdxY round((path(:,2) - min(yRange)) / gridStep) 1; % 防止越界 demIdxX max(1, min(size(Z,2), demIdxX)); demIdxY max(1, min(size(Z,1), demIdxY)); elevProfile Z(sub2ind(size(Z), demIdxY, demIdxX)); % 计算沿路径的坡度 pathLen [0; cumsum(sqrt(diff(path(:,1)).^2 diff(path(:,2)).^2))]; slope atan(abs(diff(elevProfile)) ./ (diff(pathLen) eps)) * 180 / pi; figure; subplot(2,1,1); plot(pathLen, elevProfile); xlabel(路径长度/m); ylabel(高程/m); title(DEM 高程剖面); subplot(2,1,2); plot(pathLen(2:end), slope); xlabel(路径长度/m); ylabel(坡度/deg); title(沿路径坡度); yline(20, r--, 爬坡限值);sub2ind把行列索引换算成线性索引避免双重索引性能浪费。把yline(20)画在坡度图上可以直接看到哪些路段超过车辆爬坡能力再针对性地调整这些路段的代价权重重新跑 A*。5.2 让路径平滑可执行A* 输出的是栅格折线直接交给底层运动控制器会出现频繁转向的抖动。常见做法是 Douglas-Peucker 抽稀后做三次样条插值但这样处理可能把轨迹推向障碍物。更稳妥的做法是加一个最大曲率约束的路径平滑器function [smoothPath] smooth_path(path, maxCurvature) % 用三次样条参数化路径再按曲率约束重采样 t 1:size(path,1); ppX csaps(t, path(:,1), 0.3); ppY csaps(t, path(:,2), 0.3); tt linspace(1, size(path,1), size(path,1)*10); px fnval(ppX, tt); py fnval(ppY, tt); % 计算曲率 dx gradient(px); dy gradient(py); ddx gradient(dx); ddy gradient(dy); k abs(dx .* ddy - dy .* ddx) ./ (dx.^2 dy.^2).^(3/2); % 丢弃曲率超限的中间点再做一次样条 valid k maxCurvature; smoothPath [px(valid), py(valid)]; endcsaps是 MATLAB 的平滑样条拟合函数平滑系数 0 到 1 之间越接近 0 曲线越光滑但偏离原始路径越远。曲率计算中梯度用gradient函数完成不需要符号运算。最后一步检查曲率超限点是防止平滑后路径压到障碍物的保险措施。5.3 仿真验证的完整流程与常见错误完整的验证流程分四步第一步把 DEM 重采样到不同分辨率对比各分辨率下的路径差异确认全局路径对分辨率不敏感第二步在固定全局路径上施加不同初始朝向和速度测试局部规划器的鲁棒性第三步人为在 DEM 中叠加随机小障碍观察局部规划器是否能成功绕过第四步统计各阶段耗时确定全局规划和局部规划的调用频率。一个容易忽略的细节是 DEM 坐标系的单位。某些开源月球 DEM 数据比如 LRO 的分米级产品使用经纬度坐标直接换算到平面距离时会失真。常见的做法是把经纬度投影到极方位立体投影或简单的等距圆柱投影然后才进入griddata插值环节。坐标系不统一的典型症状是路径长度和坡度计算结果偏差 10% 以上却找不到原因。gridStep选择还影响全局规划的计算量。1000×1000 的栅格上 A* 单次搜索约需数秒而 200×200 栅格可以在毫秒级完成。多轮仿真时建议先用低分辨率 DEM 粗筛路径再在高分辨率 DEM 上只对粗筛路径周围的窄带状区域做细化搜索能把整体耗时压缩一个数量级。本文还有配套的精品资源点击获取