MATLAB路径规划实战：A*、PRM与RRT算法实现与改进

简介：本资源是一套面向计算机科学与技术等相关专业本科生的移动机器人路径规划综合实践方案，适用于课程设计、期末大作业及算法实践能力提升场景。项目基于MATLAB实现，系统整合并改进了A*搜索、概率路线图（PRM）与快速探索随机树（RRT）三类主流路径规划算法，涵盖地图建模、障碍物检测、图构建、启发式搜索与树扩展等完整流程，代码结构清晰、注释充分，具备良好可读性与可扩展性。压缩包共31个文件，含20个核心MATLAB源码（.m）、3个动态演示GIF、5个备份文件（.zbak）、1个LICENSE协议及1份README说明文档，总大小6.28MB。已有42人学习下载，使用者可直接运行main.m等主入口脚本，观察不同算法在相同栅格地图下的规划效果对比，掌握算法原理、参数调优方法及MATLAB工程化实现技巧，为后续科研或竞赛打下扎实基础。

1. 项目概述与核心价值

最近在做一个移动机器人导航相关的项目，核心任务是在MATLAB里把路径规划的几个经典算法——A*、PRM和RRT——给跑通，并且针对它们各自的短板做了一些改进。这听起来像是课程大作业或者某个研究的前期验证，但实际做下来，你会发现这里面门道不少，远不是调个库、跑个demo那么简单。无论是做自动驾驶、无人机巡检，还是仓库AGV调度，路径规划都是最底层的核心能力之一。很多朋友入门时，要么被复杂的数学公式吓退，要么对着论文里的伪代码无从下手，结果就是代码跑不起来，或者跑出来的路径“鬼畜”得没法看。

这个项目的价值就在于，它提供了一个从理论到实践、从经典到改进的完整视角。我们不只满足于实现教科书上的标准算法，更要深入进去，看看它们在实际的MATLAB仿真环境里会遇到什么问题，比如A在复杂栅格地图里搜索慢、PRM在高维空间采样效率低、RRT在狭窄通道里“钻”不进去等等。然后，我们会动手尝试一些主流的改进思路，比如给A加上动态加权，用双向RRT加速收敛，或者给PRM引入启发式采样。最终的目标，是让你不仅能复现出可运行的代码，更能理解算法背后的“为什么”，以及在不同场景下“怎么选”和“怎么调”。无论你是 robotics 方向的学生，还是刚开始接触路径规划的工程师，这篇内容都能帮你绕过不少坑，直接抓住问题的要害。

2. 路径规划算法核心思想与选型逻辑

路径规划的本质，是在一个充满约束（障碍物、动力学限制）的空间里，为移动机器人找出一条从起点到终点的安全、高效通行路线。根据对环境信息的掌握程度，可以分为全局规划（环境已知）和局部规划（环境部分未知或动态）。我们这次重点搞的A*、PRM和RRT，都是全局规划里的明星算法，但它们解决问题的哲学和适用场景截然不同。

2.1 A*搜索算法：启发式引导的精准探索

A算法可以看作是Dijkstra算法的“聪明版”。Dijkstra会像水波纹一样向所有方向均匀扩散，确保找到最短路径，但效率不高。A引入了一个启发式函数h(n)，用来估计当前节点到目标点的代价。它的总代价函数是f(n) = g(n) + h(n)，其中g(n)是从起点到当前节点的实际代价。A*总是优先扩展f(n)最小的节点，相当于在探索时有了一个“指南针”，始终被拉向目标方向，从而大大减少了不必要的搜索范围。

在MATLAB里实现A*，最典型的就是基于栅格地图。我们把环境划分成均匀的网格，每个网格是一个节点。g(n)通常就是累积的移动步数（四连通或八连通），h(n)最常用的就是曼哈顿距离或欧几里得距离。它的优势是完备且最优（在启发函数满足可采纳性条件下），结果路径平滑且最短，特别适合已知的、结构化的二维栅格环境，比如室内平面图、游戏地图。

但它的短板也很明显：状态空间爆炸。当地图分辨率很高或者维度增加时（比如变成三维），需要维护和探索的节点数会呈指数级增长，搜索速度急剧下降。另外，标准的A*对于动态障碍物或者代价变化的地图不太友好，每次环境变动几乎都需要重新规划。

2.2 PRM（概率路图法）：先建图，再查询

PRM的思路非常“工程化”，它把规划分成了两个阶段：学习阶段和查询阶段。学习阶段，它在自由的构型空间（C-Space）里随机撒点（采样），并尝试用简单的局部规划器（比如直线连接）把这些点连接起来，形成一个描述空间连通性的路图。查询阶段，再把起点和终点连接到这个路图上，用图搜索算法（如Dijkstra或A*）在路图上找到路径。

PRM的强大之处在于它的预处理思想。对于固定的环境，路图只需要构建一次，之后无论起点和终点怎么变，都可以在这个固定的图上快速查询。这非常适合多任务查询的场景，比如同一个仓库里多个货物的拣选点路径计算。在MATLAB中实现PRM，核心是写一个高效的碰撞检测函数，用来判断采样点和连线是否与障碍物相交。

它的主要挑战在于采样策略。完全随机采样在狭窄通道或复杂区域效率很低，可能撒了很多点都进不去关键通道，导致构建的路图无法连通起点和终点。这就引出了各种改进的采样策略，比如在障碍物边界附近增加采样密度（高斯采样）、利用桥测试找到狭窄通道等。

2.3 RRT（快速探索随机树）：面向未知空间的快速探索

如果你玩过《我的世界》里那种快速生长的树木模组，就能直观理解RRT。它从起点开始，生长一棵树。每次迭代，先在空间里随机采样一个点，然后在当前的树上找到离这个随机点最近的节点，朝着随机点的方向生长一个固定步长，生成一个新的节点。如果这条生长边没有碰撞，就把新节点加入树中。如此反复，直到树扩展到目标点附近。

RRT的核心优势是搜索速度快，特别适合高维空间（如机械臂的关节空间）和复杂障碍物环境。它不试图构建整个空间的图，而是以一种偏向随机的方式快速探索，因此能较快地找到一条可行路径（但不一定是最优的）。在MATLAB里仿真RRT，看着树结构在障碍物中蜿蜒生长，最终抵达目标，过程非常直观。

当然，它的缺点也很突出：路径质量随机。由于采样随机，生成的路径往往曲折、冗长，且每次运行结果都可能不同。它也不是最优的，甚至不是渐近最优的（除非使用RRT*等变种）。标准RRT在狭窄通道前容易“卡住”，因为随机点很难恰好落在通道内，导致树在通道口徘徊。

2.4 算法选型决策矩阵

面对具体问题，该怎么选？这里有个简单的决策逻辑：

场景特征	推荐算法	核心理由
低维（2D/3D栅格）、已知静态环境、要求最短路径	A*	结果最优，路径平滑，实现简单直观。
固定环境、需要频繁为不同起终点规划	PRM	一次建图，多次查询，长期效率高。
高维空间（>3D）、复杂几何障碍、只需可行解	RRT	搜索速度快，能处理复杂约束和微分约束。
环境部分未知或动态变化	通常不单独使用	需结合局部规划器（如DWA、TEB）或在线重规划。
狭窄通道环境	改进的PRM或RRT	需采用针对性采样策略（桥测试、双向RRT）。

注意：没有“银弹”算法。在实际系统中，经常是分层规划：上层用A*或PRM做全局粗略规划，下层用RRT或动态窗口法（DWA）做局部避障和轨迹优化。我们这个项目分别实现它们，正是为了理解各自的特性，为后续的融合打下基础。

3. MATLAB实现基础：环境建模与碰撞检测

在写算法之前，我们必须先把舞台搭好，也就是在MATLAB里构建一个能让机器人“跑起来”的仿真环境。这主要包括两部分：如何用数据表示地图（环境建模），以及如何判断机器人的位姿是否会撞上障碍物（碰撞检测）。

3.1 环境建模：栅格地图与几何地图

最常用的是二值栅格地图。用一个二维矩阵map表示，比如0代表自由空间（白色），1代表障碍物（黑色）。分辨率决定了规划精度和计算量。在MATLAB中生成一个简单地图：

% 创建一个100x100的空白地图（全0） map = zeros(100, 100); % 添加一个矩形障碍物 (行20-40， 列30-60) map(20:40, 30:60) = 1; % 添加一些随机障碍物斑点 for i = 1:20 x = randi([1,100]); y = randi([1,100]); map(max(1,x-2):min(100,x+2), max(1,y-2):min(100,y+2)) = 1; end % 可视化 figure; imagesc(map); colormap(gray); axis equal; axis tight; hold on; plot(start(2), start(1), 'go', 'MarkerSize', 10, 'LineWidth', 3); % 起点（注意MATLAB是行-列索引） plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 3); % 终点

对于PRM和RRT，我们有时需要更灵活的几何地图，即用多边形（Polygon）的顶点列表来定义障碍物。这更接近真实CAD环境。碰撞检测时，需要判断点或线段是否在多边形内部或相交。

3.2 碰撞检测：路径规划的安全底线

这是所有规划算法的基石，必须又快又准。对于栅格地图，检测一个点(x, y)是否碰撞很简单：if map(round(y), round(x)) == 1。但机器人有尺寸，不能看作一个点。通常采用膨胀障碍物的方法：在规划前，根据机器人的半径（或外接圆半径）将障碍物区域向外膨胀一圈，这样规划时就可以把机器人视为一个点，在膨胀后的地图上运动。MATLAB中可以用imdilate函数实现：

robot_radius = 5; % 假设机器人半径5个像素 se = strel('disk', robot_radius); % 创建圆形结构元素 inflated_map = imdilate(map, se); % 膨胀障碍物

对于PRM中“连接两点是否碰撞”，需要检测线段是否穿过障碍物。一个简单有效的方法是采样法：在线段上等间距取多个点，判断每个点是否在膨胀后的障碍物内。只要有一个点碰撞，就认为整条线段不可行。

function collision = checkLineCollision(p1, p2, inflated_map) % p1, p2: 线段端点坐标 [x, y] numPoints = 20; % 采样点数量，根据精度调整 collision = false; for t = linspace(0, 1, numPoints) point = p1 + t * (p2 - p1); x = round(point(1)); y = round(point(2)); % 检查边界 if x < 1 || x > size(inflated_map, 2) || y < 1 || y > size(inflated_map, 1) collision = true; break; end if inflated_map(y, x) == 1 % 注意行列索引 collision = true; break; end end end

实操心得：碰撞检测是性能瓶颈。在MATLAB中，应尽量避免在循环内进行大量的边界检查和矩阵访问。对于性能要求高的场景，可以考虑将地图预处理成更高效的数据结构（如距离场），或者用MEX文件调用C/C++代码。但在项目初期，采样法足够清晰且易于调试。

4. 经典A*算法实现与加权改进

我们先从最结构化的A开始。实现一个基础的栅格A是理解图搜索的最佳起点。

4.1 标准A*的MATLAB实现要点

A*需要维护两个列表：开放列表（OpenList）和关闭列表（ClosedList）。开放列表存放待考察的节点，关闭列表存放已考察过的节点。每个节点需要记录：坐标、父节点、g值、f值。在MATLAB中，我们可以用结构体数组或更高效的方式（如用矩阵并行存储）来管理这些信息。

一个清晰但非最优的实现框架如下：

1. 初始化：将起点加入OpenList，其g=0，f=h(start, goal)。
2. 主循环： a. 从OpenList中找出f值最小的节点，作为当前节点。 b. 如果当前节点是目标点（或足够接近），则回溯路径，规划成功。 c. 将当前节点移入ClosedList。 d. 遍历当前节点的所有邻居（四方向或八方向）。 e. 如果邻居在ClosedList中或不可通行（碰撞），则跳过。 f. 计算从起点经过当前节点到该邻居的临时g值tentative_g。 g. 如果邻居不在OpenList中，或者新的tentative_g比它原有的g值更小，则更新该邻居的父节点为当前节点，更新其g和f值，并将其加入/更新到OpenList中。
3. 循环结束：如果OpenList为空仍未找到目标，则规划失败。

这里有一个关键技巧：如何高效地从OpenList中找f最小的节点？如果每次都用min函数遍历，在节点多时极慢。更优的做法是使用优先队列（Min-Heap）。MATLAB没有内置的堆，但我们可以用containers.Map配合自定义排序，或者利用sort函数，但需要注意性能。一个折中方案是维护一个已排序的节点列表，但插入/删除成本高。对于教学和中小地图，直接遍历查找是可以接受的。

4.2 动态加权A*：在速度与最优间权衡

标准A的启发函数h(n)是“可采纳的”，意味着它从不高估实际代价，这保证了找到的路径是最短的。但有时我们为了更快地找到一条“还不错”的路径，可以接受轻微的非最优。这就是加权A的思想：给启发函数加一个大于1的权重w，即f(n) = g(n) + w * h(n)。

当w > 1时，算法会更“贪婪”，更倾向于朝目标前进，从而更快地找到一条路径（搜索的节点数更少）。但权重过大，路径可能会变得迂回，甚至在某些情况下找不到路径（如果w * h(n)严重高估，可能破坏可采纳性）。在动态环境中，我们可以根据情况调整w：在开阔地增大w加速搜索，在复杂区域减小w保证最优性。

在MATLAB中实现，只需修改代价计算部分：

function f = calculateF(g, h, w) % w: 启发函数权重， w=1为标准A*， w>1为加权A* f = g + w * h; end

注意事项：加权A找到的路径长度与最优路径长度的比值是有理论上界的。它是一种次优但快速的折中方案。在实际项目中，我经常先用加权A（w=1.5~2）快速得到一条初始路径，如果时间允许，再用标准A*（w=1）或任何时间规划算法（Anytime A*）对其进行优化。

4.3 打破对称性与Tie-Breaker

在均匀的栅格地图中，经常会出现多个节点具有相同f值的情况。标准A*的实现（比如使用默认的min函数）会任意选择一个，这可能导致搜索范围像正方形一样向外扩散，而不是更偏向目标。一个简单的改进是引入一个微小的**打破对称性（Tie-Breaker）**因子，让算法在f相同时，优先选择h值更小的节点（即更靠近目标的）。

一种常见方法是给f值加上一个极小的扰动：f = f + h * p，其中p是一个非常小的常数（如1e-6）。这样，当两个节点f的主值相等时，h更小的节点其f值会略小，从而被优先扩展。这个技巧能显著改善A*的搜索效率，使搜索方向更“尖锐”地指向目标。

5. PRM构建与查询的工程化实现

PRM的实现比A*更模块化，清晰地分为学习和查询两个阶段。

5.1 学习阶段：高效采样与连接策略

学习阶段的目标是构建一个能反映自由空间连通性的无向图。关键参数有两个：采样点数量N和连接距离d_max。

function roadmap = buildPRM(map, N, d_max) % map: 膨胀后的二值栅格地图 % N: 采样点数量 % d_max: 最大连接距离 roadmap.vertices = []; % 顶点坐标 [x, y] roadmap.edges = {}; % 邻接表， cell数组，每个元素是该顶点连接的顶点索引列表 % 1. 随机采样 dims = size(map); vertices = []; while size(vertices, 1) < N % 在自由空间内随机采样 p = [randi(dims(2)), randi(dims(1))]; % [x, y] if map(p(2), p(1)) == 0 % 检查是否碰撞 vertices = [vertices; p]; end end roadmap.vertices = vertices; numVertices = N; roadmap.edges = cell(numVertices, 1); % 2. 连接邻近节点 for i = 1:numVertices for j = i+1:numVertices p1 = vertices(i, :); p2 = vertices(j, :); dist = norm(p1 - p2); if dist <= d_max % 检查连线是否碰撞 if ~checkLineCollision(p1, p2, map) % 无碰撞，添加双向边 roadmap.edges{i} = [roadmap.edges{i}, j]; roadmap.edges{j} = [roadmap.edges{j}, i]; end end end end end

上面的双循环连接策略复杂度是O(N^2)，当N很大时非常慢。工程优化：使用空间数据结构加速邻近搜索，如k-d树（MATLAB自带knnsearch或rangesearch）。我们可以为每个节点只连接其最近的K个邻居，或者一定距离d_max内的邻居。

% 使用k-d树优化连接（需要Statistics and Machine Learning Toolbox） [Idx, D] = knnsearch(vertices, vertices, 'K', 20); % 找每个点的最近20个邻居 for i = 1:numVertices neighbors = Idx(i, 2:end); % 排除自身 distances = D(i, 2:end); for k = 1:length(neighbors) j = neighbors(k); if distances(k) <= d_max p1 = vertices(i, :); p2 = vertices(j, :); if ~checkLineCollision(p1, p2, map) roadmap.edges{i} = [roadmap.edges{i}, j]; % 注意：这里只添加了单向，需要在循环j时也会处理，或最后统一处理成无向图 end end end end

5.2 查询阶段：将起终点融入路图

路图建好后，对于新的起点start和终点goal：

1. 分别将start和goal作为临时节点加入图中。
2. 为它们各自寻找在路图中一定距离d_max内、且能无碰撞连接的邻居节点，建立连接。
3. 在扩展后的图上，使用图搜索算法（如Dijkstra或A*）寻找从start到goal的路径。

function path = queryPRM(roadmap, start, goal, map, d_max) vertices = roadmap.vertices; edges = roadmap.edges; % 将起点和终点作为最后两个顶点加入 newVertices = [vertices; start; goal]; newEdges = [edges; cell(2,1)]; numV = size(newVertices, 1); startIdx = numV - 1; goalIdx = numV; % 为起点和终点连接邻居 for idx = [startIdx, goalIdx] point = newVertices(idx, :); % 寻找所有现有顶点中距离小于d_max的 for i = 1:size(vertices, 1) dist = norm(point - vertices(i, :)); if dist <= d_max if ~checkLineCollision(point, vertices(i, :), map) % 添加双向边 newEdges{idx} = [newEdges{idx}, i]; newEdges{i} = [newEdges{i}, idx]; end end end end % 在扩展后的图上运行Dijkstra算法 pathIndices = dijkstraGraph(newVertices, newEdges, startIdx, goalIdx); path = newVertices(pathIndices, :); end

踩坑记录：d_max是个关键参数。设得太小，起点/终点可能连接不上任何路图节点，导致查询失败。设得太大，连接检查的计算量会增加，且可能试图穿过障碍物连接远处的点。一个经验法则是，根据自由空间的特征尺寸来设置，通常可以先设为地图对角线长度的5%~10%，再根据实际情况调整。

5.3 启发式采样改进：让采样点更“聪明”

完全随机采样在狭窄通道场景是灾难。高斯采样是一种改进：它以一定的概率在障碍物边界附近采样。具体做法是，先随机选择一个点，如果它在障碍物内，则在其附近（按高斯分布）再采一个点。这样采样点对更容易落在狭窄通道的两侧。桥测试是另一种更强力的方法：在障碍物内取一个点，在其附近小范围内取两个点，如果这两个点都在自由空间，且它们的连线穿过障碍物区域，那么它们很可能位于狭窄通道的两侧，此时取它们的中点作为采样点，这个点有很大概率落在通道内。

在MATLAB中实现桥测试采样可以显著提升在迷宫类环境中的建图成功率。这体现了PRM的核心思想：采样策略决定了路图的质量。好的采样能用更少的点构建出连通性更好的路图。

6. RRT算法核心实现与双向扩展优化

RRT的实现充满了随机性，代码比A*和PRM更简洁，但调试起来也更有趣（或者说更令人抓狂）。

6.1 标准RRT生长过程详解

RRT维护一棵树，树中的每个节点除了坐标，还需要记录其父节点索引，以便最后回溯路径。

function [tree, path] = buildRRT(start, goal, map, maxIter, stepSize) % start, goal: 起点终点坐标 % map: 膨胀地图 % maxIter: 最大迭代次数 % stepSize: 生长步长 tree.vertices = start; % 顶点列表，每行是一个点[x, y] tree.parent = 0; % 父节点索引列表，根节点父节点为0 goalReached = false; for iter = 1:maxIter % 1. 随机采样 if rand < 0.05 % 以一定概率（如5%）直接采样目标点，加速收敛 randPoint = goal; else randPoint = [randi(size(map,2)), randi(size(map,1))]; end % 2. 寻找树上最近点 [nearestIdx, nearestPt] = findNearestVertex(tree.vertices, randPoint); % 3. 朝随机点方向生长一步 direction = randPoint - nearestPt; dist = norm(direction); if dist > 0 direction = direction / dist; % 单位化 newPt = nearestPt + stepSize * direction; newPt = round(newPt); % 对齐到栅格 % 边界检查 if newPt(1)<1 || newPt(1)>size(map,2) || newPt(2)<1 || newPt(2)>size(map,1) continue; end % 4. 碰撞检测（检查生长边） if ~checkLineCollision(nearestPt, newPt, map) % 5. 添加新节点 tree.vertices = [tree.vertices; newPt]; tree.parent = [tree.parent; nearestIdx]; % 6. 检查是否到达目标区域 if norm(newPt - goal) < stepSize % 尝试直接连接新节点到目标 if ~checkLineCollision(newPt, goal, map) tree.vertices = [tree.vertices; goal]; tree.parent = [tree.parent; size(tree.vertices,1)-1]; goalReached = true; break; end end end end end % 回溯路径 path = []; if goalReached idx = size(tree.vertices, 1); % 最后一个节点（目标点）索引 while idx ~= 0 path = [tree.vertices(idx,:); path]; idx = tree.parent(idx); end end end function [idx, pt] = findNearestVertex(vertices, point) % 简单线性搜索，对于大树可用k-d树优化 distances = sum((vertices - point).^2, 2); [~, idx] = min(distances); pt = vertices(idx, :); end

6.2 双向RRT（RRT-Connect）：从两端“夹击”

标准RRT从起点开始单向生长，在狭窄通道前容易陷入局部“挣扎”。双向RRT的思想是同时从起点和终点生长两棵树（Tree A和Tree B），每次迭代中，一棵树正常生长，然后尝试让另一棵树朝这棵树的新节点方向生长。如果两棵树成功连接，则路径找到。

它的核心优势是收敛速度显著加快。因为两棵树相向生长，搜索空间被有效压缩。在MATLAB中实现，需要维护两套顶点和父节点列表，并在每次迭代中决定是扩展Tree A还是Tree B（可以交替进行）。关键步骤是“连接尝试”：当Tree A生长出一个新节点newNodeA后，不是让Tree B也随机生长，而是让Tree B尝试朝着newNodeA的方向进行贪婪扩展——即不断以步长向newNodeA生长，直到发生碰撞或到达newNodeA。如果两棵树在中间相遇，则规划成功。

% 在双向RRT主循环中的关键片段 if extendTree(TreeA, TreeB.newestNode, stepSize, map) == 'reached' % 如果TreeA成功扩展并到达了TreeB的最新节点 % 则两棵树连接成功 path = extractPath(TreeA, TreeB); break; end % 交换两棵树角色，继续迭代 [TreeA, TreeB] = deal(TreeB, TreeA);

实操心得：双向RRT的性能提升非常明显，尤其是在起点和终点之间障碍物较多的场景。但实现时要注意“连接”的判断条件。由于浮点精度和碰撞检测的误差，两棵树可能无法精确到达同一个点。通常设定一个容差，当两棵树最近节点的距离小于步长或某个阈值时，即认为连接成功。步长stepSize的选择也很关键，太大可能“穿墙”，太小则生长缓慢。一般设为环境尺度的1%~5%。

6.3 路径后处理：从“随机枝干”到“可通行路径”

RRT生成的路径通常像一根多节的树枝，有很多不必要的转折。直接让机器人跟踪这样的路径是低效甚至不稳定的。因此，路径后处理必不可少。最常用的方法是剪枝（Path Pruning）：从起点开始，尝试连接后续的非相邻节点，如果连线无碰撞，则跳过中间的所有节点。这类似于在原始路径上拉紧一条橡皮筋。

function smoothedPath = smoothPath(path, map) smoothedPath = path(1, :); % 从起点开始 currentIdx = 1; while currentIdx < size(path, 1) nextIdx = size(path, 1); % 从最远的点开始尝试 for i = size(path,1):-1:currentIdx+1 if ~checkLineCollision(path(currentIdx,:), path(i,:), map) nextIdx = i; break; end end smoothedPath = [smoothedPath; path(nextIdx, :)]; currentIdx = nextIdx; end end

经过剪枝，路径的转折点会大大减少，更接近一条“捷径”。对于更高质量的要求，还可以在剪枝的基础上进行B样条或贝塞尔曲线拟合，使路径不仅短，而且曲率连续，更适合机器人运动控制。

7. 算法性能对比与场景化测试

实现完算法，我们必须把它们放在同一个擂台上比一比。在MATLAB中设计一个综合测试场景非常有助于直观理解。

7.1 设计对比实验

我们可以创建三个有代表性的地图：

1. 简单空旷地图：主要测试算法的基础功能和路径最优性。
2. 迷宫地图：布满狭窄通道，考验算法在复杂几何空间中的探索能力。
3. 多障碍物随机地图：模拟现实中的杂乱环境，测试算法的鲁棒性和效率。

评价指标应该包括：

• 成功率：在最大迭代次数/时间内找到路径的概率。
• 路径长度：找到的路径总长（像素或米）。
• 规划时间：从调用函数到返回路径所花的CPU时间（使用tic/toc）。
• 搜索节点数/采样点数：反映算法的计算开销和内存占用。
• 路径平滑度：可以用路径的总转向角或曲率来粗略衡量。

在MATLAB中编写一个自动测试脚本，批量运行每个算法在不同地图上的表现，并将结果汇总到表格中。

7.2 结果分析与算法选择指南

根据我的测试经验，通常会出现以下结论：

• A*：在简单和迷宫地图中，只要分辨率足够，都能找到最短路径。但在大尺寸高分辨率地图上，时间开销和内存占用会急剧上升，甚至因OpenList过大而导致内存不足。加权A*（w=1.5）能大幅减少搜索节点（减少30%-50%），路径长度仅增加约5%-10%，是很好的折中。
• PRM：在固定环境多任务查询时优势巨大。建图时间可能较长（尤其是N很大时），但一旦建好，查询速度极快（接近Dijkstra在图上的搜索）。在迷宫地图中，基础随机采样PRM成功率可能很低，而桥测试采样PRM能有效解决此问题。d_max和N需要仔细调参。
• RRT：在高维或非常复杂的随机地图中，最快找到可行解。但路径长度最长，且结果不稳定（每次运行长度差异大）。双向RRT能显著提高成功率和收敛速度，路径质量也有所改善。后处理剪枝对RRT至关重要，能将路径长度缩短20%-40%。

场景化选择总结：

• 机器人全局导航（已知室内地图）：首选加权A*。地图可预先栅格化，要求路径最优且平滑。对于非常大的楼层地图，可以考虑采用分层A或*跳点搜索（JPS）**进行优化。
• 机械臂运动规划（6维以上关节空间）：首选双向RRT或其优化变种（如RRT*）。PRM在高维空间所需采样点呈指数增长，难以构建连通路图。
• 游戏NPC寻路或动态障碍物：A* 配合动态重规划或D* Lite算法。PRM和RRT在动态更新障碍物时开销较大。
• 采样类算法参数调优：PRM的N和d_max， RRT的stepSize和maxIter，都没有银弹值。务必进行参数扫描，画出成功率/时间/路径长度随参数变化的曲线，找到“拐点”作为最佳参数。

8. 常见问题排查与MATLAB调试技巧

自己动手实现时，肯定会遇到各种“坑”。这里记录几个最常见的问题和解决思路。

8.1 算法运行失败或路径不合理

1. 问题：A*找不到路径，但明明有路。

   • 检查1：碰撞检测膨胀半径。机器人半径设置是否过大，导致膨胀后起点或终点被障碍物覆盖？inflated_map是否正确生成并可视化确认。
   • 检查2：启发函数过估计。如果h(n)高估了真实代价，A*可能找不到最优路径，甚至失败。确保使用的是可采纳的启发函数（如曼哈顿距离对于四连通移动是可采纳的）。
   • 检查3：OpenList/ClosedList逻辑错误。最常见的是节点重复扩展或父节点更新逻辑有误。可以在小地图上单步调试，打印每个节点的状态。

2. 问题：PRM路图构建成功，但查询失败。

   • 检查1：起终点连接。queryPRM函数中，起终点是否成功连接到路图？检查d_max是否太小，或者起终点附近障碍物太密导致没有可连接的邻居。可视化连接后的图看看。
   • 检查2：路图连通性。构建的路图本身可能就是不连通的多个组件。增加采样点N或连接距离d_max。更有效的是改用启发式采样（如桥测试）。

3. 问题：RRT迭代了很久也找不到路径。

   • 检查1：步长stepSize。步长太大，可能每次生长都撞上障碍物；步长太小，生长太慢，尤其在空旷区域。尝试调整步长为环境尺度的1%~5%。
   • 检查2：目标偏置采样概率。代码中是否有if rand < goalBias直接采样目标点的逻辑？适当提高这个概率（如0.05到0.1）可以加速收敛。
   • 检查3：狭窄通道。标准RRT在狭窄通道前极其低效。考虑换用双向RRT，或者引入采样偏向性（当树长时间未扩展时，在未探索区域增加采样概率）。

8.2 MATLAB编程与性能优化

1. 性能瓶颈：算法在稍大的地图上就跑得很慢。

   • 向量化操作：避免在大的for循环中进行逐元素的矩阵访问。例如，计算所有节点到某点的距离，用sum((vertices - point).^2, 2)而不是循环。
   • 预分配数组：在扩展tree.vertices或path时，如果知道大致规模，先用zeros(N, 2)预分配内存，避免MATLAB频繁调整数组大小。
   • 使用更高效的数据结构：A*的OpenList用优先队列（可以搜索MATLAB File Exchange上的Min-Heap实现）。最近邻搜索用knnsearch。
   • 碰撞检测优化：这是最耗时的部分。确保checkLineCollision函数高效。可以考虑使用距离变换地图（bwdist），预先计算每个自由空间点到最近障碍物的距离，这样碰撞检测可以简化为判断线段上各点的距离值是否大于机器人半径。

2. 可视化调试：路径规划非常依赖可视化来理解算法行为。

   • 实时绘制：在算法主循环中，加入简单的绘图命令（如plot新节点、边），可以动态观察树/图的生长过程。使用drawnow limitrate来更新图形而不至于太慢。
   • 分步调试：在循环内设置条件断点，当扩展到特定节点或迭代次数时暂停，检查当前状态。

3. 路径不平滑或抖动：

   • 栅格对齐误差：在栅格地图中，路径节点被限制在格点上，导致路径呈锯齿状。A*使用八连通邻域比四连通更平滑。对于更高要求，可以考虑在连续坐标空间实现算法（PRM和RRT本就是连续的），或者对栅格路径进行插值平滑。
   • RRT随机性：RRT的原始路径必然抖动。后处理剪枝和曲线拟合是必须的步骤。

最后，分享一个我调试时的小习惯：为每个算法写一个独立的测试脚本，并保存每次运行的关键结果（路径、时间、节点数）和对应的参数。这样，当你调整一个参数时，能清晰地看到它带来的影响，而不是凭感觉。路径规划算法是理论和实践的结合，多动手实现，多观察可视化结果，才能真正领悟其精妙之处。希望这些代码片段和经验能帮你更快地上手，少走些弯路。

本文还有配套的精品资源，点击获取