✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
一、引言：路径规划的 “痛点” 与算法融合的破局思路
1.1 路径规划：智能系统的 “导航大脑”
在科技飞速发展的当下，机器人、自动驾驶车辆等智能设备如雨后春笋般涌现，逐渐融入人们生活与工业生产的各个角落 。它们能够自主运行的核心支撑，便是路径规划技术，这项技术堪称智能系统的 “导航大脑”。简单来说，路径规划的目标明确而关键，就是要在复杂多变的环境里，为智能设备生成一条从起点顺利抵达终点的路径，且这条路径必须满足无碰撞的基本安全要求，同时尽可能做到高效可行，以提升运行效率、降低能耗等 。
在机器人领域，无论是工业生产线上精准作业的机械臂，还是服务于日常生活的家用清洁机器人，都依赖路径规划来合理规划运动轨迹。比如，工业机械臂需要在有限空间内，避开周围设备与障碍物，精准抓取和放置零件；家用清洁机器人则要在家具、杂物等复杂布局的室内环境中，规划出全面且高效的清洁路线 。在自动驾驶领域，车辆需要依据实时路况、交通规则以及周围障碍物分布，规划出安全、快速的行驶路径，保障乘客的出行安全与高效 。
然而，随着应用场景朝着高维、动态、复杂的方向不断演进，传统路径规划算法渐渐显得力不从心 。在高维空间中，计算量会呈指数级增长，这就是所谓的维度灾难，使得算法的运行效率大幅降低；同时，算法还容易陷入局部最优解，无法找到全局最优路径 。在动态环境里，如自动驾驶场景中突然出现的障碍物、交通状况的实时变化等，传统算法的实时性不足问题凸显，难以快速做出路径调整，可能导致危险发生 。因此，迫切需要探索更高效、更智能的算法组合方案，以应对这些严峻挑战。
1.2 单算法的 “短板”：RRT 与 PRM 的各自局限
在众多路径规划算法中，快速探索随机树（RRT）和概率路网（PRM）作为两种经典的采样式路径规划算法，备受关注 。但它们各自也存在明显的局限性。
RRT 算法以其独特的随机采样和树扩展策略，在高维空间中展现出快速探索的能力 。它从起始点开始，通过不断随机采样空间中的点，并向采样点扩展树的节点，逐步构建起一棵覆盖大部分可达空间的随机树 。只要有足够的时间，它就能以概率 1 找到一条从起始点到目标点的可行路径，这就是所谓的概率完备性 。在机器人运动规划中，RRT 可以快速找到绕过障碍物的大致路径，使机器人能够在复杂环境中快速行动起来 。
RRT 算法也有其短板。它生成的路径往往比较曲折，不是最优路径，这是因为它的首要目标是快速找到可行路径，而非追求路径的最优性 。在实际应用中，这样的路径可能会增加机器人的运动时间和能耗 。RRT 算法对采样点的分布十分敏感，如果采样点分布不合理，可能导致规划失败，或者生成大量冗余路径，白白浪费计算资源 。在一些对路径精度和效率要求较高的场景中，RRT 算法的这些缺陷就显得尤为突出 。
PRM 算法则采用了不同的策略，它通过离线预处理来构建一个表示自由空间连通性的概率路图 。在离线阶段，PRM 在自由空间中大量随机生成节点，然后尝试连接这些节点 。若连接线段不与障碍物相交，就在路图中添加一条边 。在线查询时，只需将起始点和目标点连接到已构建好的路图中，再通过图搜索算法（如 A * 算法）就能快速找到一条路径 。这种离线建图、在线查询的方式，使得 PRM 在面对重复查询相似路径的场景时，具有极高的效率 。在工厂中，机器人需要反复执行相同或相似的搬运任务时，PRM 算法可以快速规划出路径，大大提高生产效率 。
PRM 算法也并非完美无缺。在构建路图时，它需要大量的采样点才能确保自由空间的连通性，这无疑增加了计算成本和时间开销 。PRM 算法对于狭窄通道或 “瓶颈” 区域的探索能力较弱 。这些特殊区域在复杂环境中很常见，如室内环境中的狭窄过道、工业场景中的设备间隙等 。由于采样的随机性，PRM 可能无法在这些狭窄区域生成足够的节点并建立有效的连接，导致路图断裂，最终难以找到通过这些区域的路径 。
正是由于 RRT 和 PRM 算法各自存在上述局限，单独使用它们时，往往难以满足复杂多变的路径规划需求 。但换个角度看，它们的局限性恰好形成了互补关系 。由此引出本文的核心观点：通过巧妙地将 RRT 和 PRM 算法进行串联融合，充分发挥两者的优势，就有可能实现 “1 + 1> 2” 的效果，为路径规划难题提供更优的解决方案 。在接下来的内容中，将深入探讨这种串联运动规划器的具体实现方式、优势以及实际应用效果 。
二、核心算法拆解：读懂 RRT 与 PRM 的 “独门秘籍”
在深入探讨串联运动规划器之前，先分别剖析 RRT 和 PRM 这两种核心算法的工作原理、执行流程以及它们各自的优势与局限，这将为后续理解两者的融合机制奠定坚实基础 。
2.1 快速探索随机树（RRT）：随机生长的 “空间探索者”
快速探索随机树（RRT）算法自诞生以来，在路径规划领域掀起了一阵革新的浪潮，凭借其独特的随机采样与树状扩展策略，为解决复杂空间中的路径规划难题提供了高效方案 。
2.1.1 RRT 的基本原理与诞生背景
RRT 算法由 Steven M. LaValle 于 1998 年提出，彼时，传统路径规划算法在面对高维空间时，由于维度灾难的困扰，计算量呈指数级增长，效率急剧下降 。RRT 算法的出现，犹如一道曙光，打破了这一困境 。它基于蒙特卡洛方法，通过在配置空间中随机采样点，逐步构建一棵不断扩展的树状结构 。这棵树从起始点开始生长，每个新节点都是通过在随机方向上扩展一定步长而生成的 。只要有足够的时间，RRT 算法就能够以概率 1 找到一条从起始点到目标点的可行路径，这种特性被称为概率完备性 。在一个复杂的室内环境中，机器人需要从房间的一角移动到另一角，RRT 算法可以快速探索周围空间，绕过各种家具、墙壁等障碍物，找到一条可行的移动路径 。
2.1.2 RRT 的核心执行流程
Image
⛳️ 运行结果
Image
Image
📣 部分代码
% Input: A 2D Segment, S, and a 2D convex polygon Q
% Output: Boolean b, where b is true if interesection
function b = isintersect_linepolygon(S,Q)
Q = CCW(Q);
Q = [Q,Q(:,1)];
p0 = S(:,1);
p1 = S(:,2);
% If line segment is single point, check if point is in polygon
if isequal(p0,p1)
b = inpolygon(p0(1),p0(2),Q(1,:),Q(2,:));
return;
end
tE = 0;
tL = 1;
ds = p1 - p0;
for i=1:size(Q,2)-1
% Calculate edge of Q
qi = Q(:,i);
qi1 = Q(:,i+1);
ei = qi - qi1;
% Compute the outward normal vector
e_hat = ei / norm(ei);
ni = [-e_hat(2); e_hat(1)];
N = -(p0 - qi)'*ni;
D = ds'*ni;
if D == 0
if N < 0
b = false;
return;
end
end
t = N/D;
if D < 0
tE = max(tE,t);
if tE > tL
b = false;
return;
end
elseif D > 0
tL = min(tL,t);
if tL < tE
b = false;
return;
end
end
end
if tE <= tL
b = true;
return;
else
b = false;
return;
end
end
% Function to get Counter Clockwise Order for Polygon
function ccw_poly = CCW(poly)
k = length(poly);
center_x = sum(poly(1,:))/k;
center_y = sum(poly(2,:))/k;
for i = 1:k
dx = poly(1,i)-center_x;
dy = poly(2,i)- center_y;
angles(i) = wrapTo2Pi(atan2(dy, dx));
end
% Sort by angles to get CCW Order
[~, order] = sort(angles);
ccw_poly = poly(:, order);
% Sort vertices so we start with lowest y value verex in CCW order
[~, lowest_y_index] = min(ccw_poly(2, :));
ccw_poly = circshift(ccw_poly, [0, -lowest_y_index + 1]);
end
🔗 参考文献
图片
🏆团队擅长辅导定制多种科研领域MATLAB仿真，助力科研梦：
