【MATLAB源码-第448期】基于MATLAB的复杂山地无人车路径规划Dijkstra，A星，RRT，RRT星对比仿真

操作环境：

MATLAB 2024a

1、算法描述****

摘要

复杂山地环境下的无人车路径规划不能只追求路径最短。车辆在越野场景中会同时受到坡度、地形起伏、路面通行性、摩擦条件和打滑风险的影响。若路径规划只把环境看成普通二维障碍物地图，算法可能会得到一条几何距离较短但实际难以通行的路径。本文基于 MATLAB 2024a 搭建了复杂山地无人车路径规划仿真系统。系统首先生成带有高程变化、弯曲可通行区域、局部起伏和摩擦差异的复杂地形环境，然后计算坡度层、起伏层、通行性层和打滑风险层，并进一步构造综合代价场。系统在同一地图、同一起终点和同一评价指标下，对 Dijkstra、A*、RRT 和 RRT* 四种路径规划方法进行对比。仿真结果通过三维地形路径图、坡度约束层路径图、通行性层路径图、打滑风险层路径图、地形起伏层路径图、综合代价场路径图、归一化指标柱状图和沿程剖面图进行展示。结果表明，图搜索类算法在固定栅格代价场中具有较好的稳定性，采样类算法能够体现复杂空间随机探索能力，而 RRT* 相比基础 RRT 具有更明显的路径优化效果。该系统适合用于无人车路径规划学习、复杂环境代价建模、算法横向对比和 MATLAB 可视化仿真分析。

关键词： MATLAB 2024a；复杂山地环境；无人车路径规划；Dijkstra算法；A算法；RRT算法；RRT算法；综合代价场；打滑风险；路径质量评价

1 引言

路径规划是移动机器人和无人车自主导航系统中的关键环节。一般路径规划任务要求车辆或机器人从起点到达目标点，同时避开障碍区域，并尽量降低距离、时间或能耗等代价。近年的综述研究通常把移动机器人路径规划分为全局规划和局部规划两类，全局规划依赖已有环境信息，局部规划更强调实时感知和动态避障。本文研究的内容属于已知地图下的全局路径规划仿真，但它并不是简单的平面避障问题，而是面向复杂山地越野环境的多因素代价规划问题。

在结构化道路中，车辆可以借助车道线、道路边界和交通规则形成较稳定的规划约束。在山地、矿区、救援现场、野外巡检和非铺装道路中，车辆面对的环境要复杂得多。地面可能存在坡度过大、起伏剧烈、摩擦不足和可通行性差等问题。无人车如果只按最短距离规划，很容易穿过高风险区域。近年关于非结构化环境无人地面车辆的研究也强调，越野路径规划需要关注地形通行性、车辆与地形之间的相互作用、代价估计和地形约束。

本文的 MATLAB 仿真系统围绕“复杂山地无人车越野路径规划”展开。系统不把地图简单处理成黑白障碍物区域，而是构造带有高程、坡度、粗糙度、通行性、摩擦系数和打滑风险的多层环境。随后，系统将这些环境信息融合为综合代价场，并让 Dijkstra、A*、RRT 和 RRT* 四种算法在统一条件下搜索路径。这样的设计有两个价值。第一，它能展示不同算法在复杂代价地图中的路径差异。第二，它能让路径评价从“是否到达终点”提升到“是否适合车辆实际通行”。

2 复杂山地环境建模

系统首先通过 mkMap 函数生成山地环境。该函数构造的地图不是普通随机矩阵，而是由多个高程峰值、低谷、周期性地形变化和平滑扰动共同形成。这样可以模拟山地环境中常见的山脊、缓坡、沟谷和局部起伏。系统还设置了一条弯曲的相对易通行区域，用来模拟野外道路、车辙、巡检通道或较平整的可通过带。这样，规划算法不会只沿直线搜索，而是需要在距离和地形风险之间进行权衡。

高程矩阵是整个系统的基础。系统根据高程变化计算坡度，根据局部高程差异估计地形起伏程度。坡度越大，车辆爬坡难度越高，轮胎附着裕量越小。地形起伏越明显，车辆姿态变化越剧烈，行驶舒适性和稳定性越差。对于无人车而言，这些因素都会影响路径是否可执行。三维地形路径规划研究通常会把坡度、地形梯度和地形可通行性作为重要因素，因为它们直接影响车辆通过安全和能耗。

系统还生成了路面通行性矩阵和摩擦系数矩阵。通行性用于表达地面是否适合车辆通过。通行性高的区域可以理解为较平整、较坚实、风险较低的路面；通行性低的区域可以理解为泥泞、碎石、坑洼或其他不利地面。摩擦系数则影响打滑风险。低摩擦区域即使坡度并不极端，也可能让车辆发生滑移。因此，越野路径规划不能只看坡度，也不能只看障碍物，它必须把多类环境因素合成起来判断。

3 风险层与综合代价场构造

系统在 getLayer 函数中从原始地图进一步提取坡度层、粗糙度层、通行性层和打滑风险层。坡度层用于描述地形倾斜程度。粗糙度层用于描述局部高程变化强度。打滑风险层由坡度、摩擦条件、通行性和地形起伏共同影响。这个设计比较贴近越野车辆的实际逻辑，因为车辆是否容易打滑，往往不是由单个因素决定，而是由坡度、附着条件和地面状态共同决定。近年的通行风险综述也强调，移动机器人需要沿路径评估通行风险，并常使用通行性栅格地图表达潜在危险。

系统在 costMap 函数中构造综合代价场。综合代价场把坡度、地形起伏、低通行性和打滑风险加权融合。每个栅格都有一个代价值。代价值越高，说明车辆通过该区域的综合成本越高。系统还设置了硬约束。当坡度超过最大阈值、通行性低于最小阈值，或者打滑风险超过允许上限时，该网格会被视为不可通行。这样可以避免算法为了缩短距离而穿过明显危险区域。

这种“软代价加硬约束”的方法比单一障碍物建模更实用。软代价用于引导算法选择更优区域，但不会把所有不理想区域直接封死。硬约束用于排除明显不可接受区域，保证路径的基本安全边界。如果只使用软代价，算法可能仍会穿越高风险区域。如果只使用硬约束，算法又无法区分不同可通行区域之间的质量差异。本文系统把两者结合起来，使路径结果更容易解释，也更适合复杂山地场景。

系统还考虑了上坡爬升惩罚。路径从一个节点移动到另一个节点时，如果目标节点高程更高，系统会增加上坡代价。这个处理有现实意义。两条路径的水平距离可能接近，但爬升量较大的路径对车辆动力、电池能耗和通过稳定性都有更高要求。路径规划如果忽略爬升代价，就可能得到一条表面距离较短但实际执行代价较高的路线。

4 路径规划算法设计

本文系统采用四种路径规划算法进行对比，分别是 Dijkstra、A*、RRT 和 RRT*。Dijkstra 和 A* 属于图搜索类方法，适合在栅格地图或代价图中搜索路径。RRT 和 RRT* 属于采样类方法，适合表达随机扩展和复杂空间探索能力。MATLAB 官方路径规划介绍中也把 A*、RRT 和 RRT* 作为常见路径规划方法进行说明，并强调路径规划可以基于地图、状态空间或拓扑图完成。

Dijkstra 算法从起点开始扩展累计代价最小的节点，直到找到终点。它的优点是稳定、直观、结果可解释。在固定代价场中，Dijkstra 可以作为全局搜索基准。它的缺点是搜索范围较大，计算效率容易受到地图规模影响。在本系统中，Dijkstra 使用八邻域扩展，并把相邻节点之间的综合代价作为路径累计成本的一部分。这样，算法搜索的不再是普通几何最短路，而是综合风险更低的路径。

A* 算法可以看作在 Dijkstra 的基础上加入启发式估计。它不仅考虑从起点到当前节点的累计代价，也考虑当前节点到终点的估计距离。这样，搜索会更有方向性。本文系统中的 A* 使用八邻域扩展，并用最小可行代价缩放启发距离，避免启发项过强导致路径质量下降。与 Dijkstra 相比，A* 更适合体现搜索效率和路径质量之间的平衡。

RRT 算法通过随机采样扩展树结构。它从起点开始，不断向随机目标点扩展新节点，并检查新路径段是否可通行。RRT 的优势是探索能力强，不需要像图搜索一样完整遍历大量栅格。它的不足是路径质量受随机性影响较大，基础 RRT 得到的路径可能绕行明显，也可能不够平滑。本项目设置了目标点偏置采样，使随机树有一定概率直接向终点方向扩展，从而提高找到可行路径的效率。

RRT* 是 RRT 的优化版本。它在添加新节点时，会在邻域范围内寻找代价更低的父节点，并尝试对已有节点进行重连。RRT* 的核心价值在于路径质量会随着采样迭代逐步改善。Karaman 和 Frazzoli 的经典研究提出了 RRT*，并给出了其渐近最优性的理论分析。本项目中的 RRT* 使用重连半径控制局部优化范围，并通过祖先节点标记避免树结构形成闭环。

5 仿真流程与指标设计

系统主程序为 main.m。程序开头设置随机种子，以保证地形和路径结果具有可复现性。随后，程序创建结果图文件夹，并清理旧图片。接着，系统设置地图尺寸、网格分辨率、坡度阈值、打滑阈值、通行性阈值、各类代价权重、RRT 采样次数、随机树扩展步长、目标点偏置概率和 RRT* 重连半径。所有关键参数集中在 cfg 结构体中，便于读者修改和对比。

系统运行流程比较清晰。第一步是生成地图，包括高程、通行性、摩擦系数和起终点。第二步是计算风险图层，包括坡度、起伏和打滑风险。第三步是构造综合代价场。第四步是在同一综合代价场下运行四种路径规划算法。第五步是对路径进行平滑处理。第六步是计算路径长度、爬升量、平均坡度、平均打滑风险、平均通行性和总代价。第七步是输出命令行统计信息和八张结果图。

路径评价不能只看一项指标。若只看路径长度，算法可能倾向于穿过高坡度或低摩擦区域。若只看平均坡度，又可能忽略路径距离和绕行程度。若只看总代价，读者不容易知道总代价由哪些因素造成。因此，系统同时输出距离、爬升、坡度、打滑、通行性和综合代价。这样的指标组合能够更完整地说明路径质量。

6 仿真结果分析

系统输出八张结果图。第一张图是三维山地多算法路径对比图。该图把高程地形、综合代价着色和四种算法路径放在同一三维坐标中。它可以直观看出不同算法在山地空间中的整体走向。第二张图是坡度约束层与路径。该图用于观察算法是否避开高坡区域。第三张图是路面可通行性层与路径。该图可以判断路径是否倾向于经过更适合车辆行驶的区域。第四张图是轮胎打滑风险层与路径。该图对越野无人车尤其重要，因为高坡度和低摩擦组合会显著增加车辆失稳概率。

第五张图是地形起伏风险层与路径。它反映路径是否避开局部高低变化剧烈区域。第六张图是综合代价场路径对比。它是多层风险因素融合后的整体表达。第七张图是多算法路径质量归一化柱状图。它把路径距离、爬升量、平均坡度、平均打滑风险、低通行性和总代价放在同一尺度下比较。第八张图是沿程剖面图，分别展示路径上的高程、坡度和打滑风险变化。近年复杂地形路径规划研究也常使用路径长度、平滑性、平坦性、规划时间等 terrain-aware 指标来评价规划器表现。

从结果解释角度看，单张图不能完整说明算法优劣。三维路径图适合展示整体视觉效果，但不够量化。风险层路径图适合解释路径为什么绕行。综合代价图适合说明算法面对的统一优化目标。归一化指标图适合做算法横向比较。沿程剖面图适合分析车辆执行路径时的连续风险变化。因此，八张图组合起来形成了比较完整的结果体系。

7 系统特点

本系统的第一个特点是环境建模更完整。系统没有停留在二维障碍物避障，而是把高程、坡度、起伏、通行性、摩擦系数和打滑风险都纳入路径规划框架。第二个特点是算法类型覆盖较合理。Dijkstra 和 A* 代表确定性图搜索方法，RRT 和 RRT* 代表采样类规划方法，四者放在同一代价场下比较，能够体现不同算法的路径差异。第三个特点是评价指标丰富。系统不只输出路径图，还给出距离、爬升、坡度、打滑、通行性和总代价等指标。第四个特点是可视化结果完整。八张图分别从三维地形、风险层、综合代价、指标对比和沿程剖面展示结果，适合讲解和演示。

系统的第五个特点是代码结构清楚。主程序 main.m 负责总体流程，辅助函数集中放在 function 文件夹中。每个函数职责相对明确，便于阅读和修改。第六个特点是参数可调。地图大小、坡度阈值、打滑阈值、代价权重、采样次数、目标偏置概率和重连半径都可以直接修改。第七个特点是工程解释性较强。用户不仅能看到路径从哪里走，还能解释路径为什么绕开某些区域，为什么某些算法路径更短但风险更高，为什么某些路径距离略长但综合代价更低。

8 结论

本文基于 MATLAB 2024a 实现了复杂山地无人车多算法路径规划仿真系统。系统通过高程地图、通行性层、摩擦系数层、坡度层、粗糙度层和打滑风险层建立复杂地形模型，并在综合代价场下对 Dijkstra、A*、RRT 和 RRT* 四种算法进行统一对比。仿真结果表明，图搜索类算法在固定栅格代价场中具有较好的稳定性，A* 通过启发式搜索提高了方向性，RRT 展示了随机探索能力，RRT* 通过邻域择优和重连机制提升了路径质量。

本项目最有价值的地方不是简单实现某一个算法，而是把“复杂越野环境建模、多因素代价融合、多算法公平对比、多指标可视化评价”放在同一个 MATLAB 工程中。这样的结构更适合路径规划学习、算法演示和工程思路分析。对于后续扩展，可以继续加入车辆最小转弯半径、速度约束、动态障碍物、能耗模型和局部避障策略，使系统进一步接近真实无人车导航任务。

参考文献

Karur, Karthik, et al. “A Survey of Path Planning Algorithms for Mobile Robots.” Vehicles, 2021.
Liu, Lijuan, et al. “Path Planning Techniques for Mobile Robots: Review and Prospect.” Expert Systems with Applications, 2023.
Yang, Li, et al. “Path Planning Technique for Mobile Robots: A Review.” Machines, vol. 11, no. 10, 2023, article 980.
Wang, Nan, et al. “A Survey on Path Planning for Autonomous Ground Vehicles in Unstructured Environments.” Machines, vol. 12, no. 1, 2024, article 31.
Benrabah, Mohamed, et al. “A Review on Traversability Risk Assessments for Autonomous Mobile Robots.” Sensors, vol. 24, no. 6, 2024, article 1909.
Wang, Nan, et al. “Traversability Analysis and Path Planning for Autonomous Wheeled Vehicles on Rigid Terrains.” Drones, vol. 8, no. 9, 2024, article 419.
Reda, Mohamed, et al. “Path Planning Algorithms in the Autonomous Driving System.” Robotics and Autonomous Systems, 2024.
Tang, Yong, et al. “Path Planning Trends for Autonomous Mobile Robot Navigation.” 2025.
Swinton, S., et al. “Improving Rover Path Planning in Challenging Terrains.” Robotics, vol. 14, no. 10, 2025, article 135.
Karaman, Sertac, and Emilio Frazzoli. “Sampling-Based Algorithms for Optimal Motion Planning.” The International Journal of Robotics Research, 2011.