做二维连杆机器人的路径规划绕不开采样类算法。这几年我陆续在Matlab里折腾过RRT快速扩展随机树和PRM概率路线图有的资料里也写成RPM两种方案目标就一个让二连杆机械臂在杂乱障碍物中间从起点构型走到终点构型既不撞杆也不撞墙。以前总觉得这类算法只有论文里才用得上直到自己动手写了一遍、跑了上百次仿真才明白里面门道不少。这篇就把我从模型建立、算法选型到Matlab代码落地、踩坑排错的全过程拆开讲清楚给同样在做二维连杆机器人路径规划的朋友一份能直接上手的参考。先说清楚这个东西是干什么的二维连杆机器人最常见的模型就是二连杆机械臂两个旋转关节末端执行器在平面内运动。路径规划的目标不是让末端走一条好看的直线而是让整条臂从当前关节角组合比如q_start [0°, 90°]平滑过渡到目标关节角组合比如q_end [60°, 120°]并且在这段运动里连杆的任何一部分都不能碰障碍物。RRT和PRM这两种算法都是通过随机采样避开复杂建模在“构型空间”里搜索可行路径。对刚接触机器人规划、需要交课程作业或者做毕设的同学来说用Matlab实现这两个算法是最容易上手的路径因为矩阵运算、绘图函数、几何判断都很方便。我推荐你自己把代码敲一遍不要光下载现成的就跑。原因后面会说到两个算法看起来原理简单但真正影响成功率的是碰撞检测、采样策略、参数调节这些细活儿这些不亲手调一遍根本体会不到。1. 问题建模与方案选型为什么用随机采样算法1.1 二连杆机器人的构型空间理解先建立一个直觉认知。二连杆机械臂的每个关节角度就是一个维度所以它的构型空间是一个二维的环面理论上角度可以超出[-π, π]但工程上我们通常把两个关节角分别限定在一定范围比如0°到360°于是构型空间简化成了一个正方形的连续空间。这个空间里的每一个点比如(45°, 90°)代表一种手臂姿态一条合法的路径就是在这个正方形里从起点画到终点的一条连续曲线。难点在于工作空间里的障碍物桌子上的杯子、墙、支架映射到构型空间后会变成形状极不规则的“禁行区域”。你没法用几个标准几何体去表达这些禁行区因为同一个障碍物在不同手臂姿态下产生的碰撞区域可能是完全不同的扭曲图形。这就是为什么传统的A*或Dijkstra不得不先离散化栅格要么精度不够要么网格细到爆炸。而RRT和PRM的根本优势恰恰在于它们不需要显式构建禁行区域的边界而是通过随机采样一个个试探点用碰撞检测来告诉采样算法“这个点能不能用”。1.2 为什么选RRT和PRM组合对比我个人的体会是这两个算法放在一起对比特别有价值不是因为它们难而是因为它们的适用场景正好互补。维度RRTPRM概率路线图搜索思想单棵树从起点向外生长先构建全图再查询路径构建方式增量式一边采样一边扩展两阶段学习阶段查询阶段适合场景单次规划任务、动态环境变化快多查询任务、固定障碍物环境路径质量抖动大、通常较曲折相对均匀但也不是最优时间开销前期快但路径往往绕远图构建耗时查询很快如果你只是要完成一次从起点到终点的规划RRT通常几秒钟就能找到一条路径但如果你需要同一个环境里反复规划不同起止点PRM提前把路线图建好之后每次查询只需跑一次图搜索效率就反超了。所以把两个都实现一遍你对“什么时候该用哪种采样策略”就有了很具体的体感而不再停留在教科书定义上。1.3 建模准备设定连杆参数与障碍物在做Matlab代码前先把模型参数定下来。我常用的场景是二连杆两个连杆长度取L1 1.0L2 0.8单位无所谓只要障碍物和连杆尺寸保持同一坐标系就行。两个关节角范围设成[0, 2π]起点构型设为[30°, 80°]终点构型设为[150°, 130°]。障碍物我建议在二维平面里放三到四个圆形和矩形分别测试算法的抗碰撞能力。工作空间尺寸设为宽和高的范围都是[-2, 2]。圆形障碍物可以写成[圆心x, 圆心y, 半径]矩形写成[x_min, y_min, x_max, y_max]。这里重点提醒一句障碍物形状的多样性很重要。我见过不少人只放圆形障碍结果碰撞检测函数按点到圆心距离判断后来换成长方形障碍距离判断就没法用了。从开始就让代码支持线段与圆、线段与矩形的碰撞后面扩展会省很多力。2. RRT和PRM核心原理拆解不只是随机采样2.1 RRT算法的生长逻辑RRT的思路可以用“撒点长树”来理解。初始化一棵树树的根节点是起点q_start。每次循环做四件事在构型空间里随机采样一个点q_rand。在现有树中找到离q_rand最近的节点q_near。从q_near向q_rand方向按设定的步长Δq扩展一个新节点q_new。如果q_new连接q_near的边不与障碍物碰撞就把q_new加入树中并记录父节点。重复这个过程直到树中某个节点与终点q_goal的距离小于设定阈值就回溯父节点链得到规划路径。这里有三个关键细节必须理解透彻。第一个是“步长Δq”。它不是工作空间的物理距离而是构型空间里关节角的增量。比如Δq 0.2弧度意味着每次最多让两个关节角同时调整0.2弧度。步长太大容易一步跨过狭窄通道把本来可行的路径变成碰撞边步长太小树长得慢搜索速度下降。第二个是“最近邻搜索”。每一轮都要从树中所有节点里找距离q_rand最近的那个。对于二维构型空间直接遍历是没问题的如果扩展到高维就要用KD树之类的加速结构。Matlab里可以先用简单遍历毕竟二维节点量通常只有几百到几千。第三个是偏向目标采样。完全随机采样虽然理论上能保证概率完备性但实践中总会绕远路。所以我会以一定概率比如0.1到0.3直接把q_goal作为q_rand采样引导树向终点生长。这个技巧不是必须的但能显著缩短规划时间和路径长度。2.2 PRM/RPM算法的两阶段思想PRM的思路和RRT完全不同它是“先修路再导航”。学习阶段在构型空间中随机采样N个可行点要保证这些点对应的机械臂姿态不与障碍物碰撞然后把每个点和它周围的K个近邻点尝试连线。如果连线不碰撞就在这两个节点之间保存一条边最终构成一个图。查询阶段把q_start和q_goal也加入图中分别尝试连接它们附近的节点然后在图上跑最短路径搜索通常用A*或Dijkstra找到一条从起点到终点的节点-边路径。所以PRM实际上是把“探索环境”和“找路径”两件事彻底分开了。环境不变时图可以重复使用这是它最大的优势。但要注意图构建阶段的采样数量N很大时计算量主要在碰撞检测上每个点都要判断构型是否碰撞、每条候选边都要判断连杆扫过的区域是否碰撞这部分才是PRM真正的性能瓶颈。工程上N通常选100到500K选5到15具体要看你环境的复杂程度。还有一点必须纠正标题里的RPM按行业惯例大概率是指PRMProbability Roadmap。我见过不少资料把缩写顺序写反但算法内容是完全一致的。下面我就统一用PRM表述。2.3 两种算法的路径质量对比从最终路径形态看RRT扩展出的路径经常是“贪心式”的直角折线感特别强原因是扩展步长是固定的且每一步只朝一个随机方向走没有全局眼光。PRM的路径在线段连接上更均匀但因为查的是图最短路径边也都是直线段同样不会给出平滑曲线。所以无论哪种算法规划出来的路径都不能直接发给关节控制器使用后面必须做轨迹平滑或分段插值。这不是可选项而是工程必选项。后面我会给一个最简单的路径后处理方案。3. Matlab代码实现实操核心模块逐个拆解3.1 环境与数据结构设计我建议按模块拆分代码而不是把所有逻辑全写进一个脚本。至少要有四个文件two_link_robot.m主脚本负责初始化参数、调用规划算法、绘制结果。collision_check.m输入一个关节构型或一条直线段返回是否碰撞。rrt_planning.mRRT规划函数输入起点、终点、障碍物、步长等参数输出路径点集。prm_planning.mPRM规划函数。数据结构用结构体struct最方便。树的节点我用tree.nodes存储N×2矩阵每一行是[theta1, theta2]tree.parent存N×1的父节点索引起点父节点设为0。PRM的图结构我用邻接表因为Matlab的稀疏矩阵在动态增删节点时并不方便邻接表配合元胞数组更直观。3.2 正运动学与连杆顶点计算所有碰撞检测的前提是把关节角映射成工作空间里连杆的几何位置。二连杆的末端位置公式x L1*cos(theta1) L2*cos(theta1theta2)y L1*sin(theta1) L2*sin(theta1theta2)但碰撞检测不能只看末端要看整条杆。因此我习惯把每根连杆离散成若干个点比如第一根连杆从基座原点到关节1端点之间均匀取5个点第二根连杆从关节1到末端之间取5个点然后判断这些点是否落入障碍物内部。代码如下function pts get_link_points(theta1, theta2, L1, L2, num_points) % 连杆2的离散点 x1 L1*cos(theta1); y1 L1*sin(theta1); x2 x1 L2*cos(theta1theta2); y2 y1 L2*sin(theta1theta2); pts1 [linspace(0, x1, num_points); linspace(0, y1, num_points)]; pts2 [linspace(x1, x2, num_points); linspace(y1, y2, num_points)]; pts [pts1, pts2]; end为什么要离散成点而不是做精确的线段-多边形判断因为对Matlab来说矩阵化的点到几何体判断速度非常快而且代码简单。代价是离散粒度不够时可能漏检某个离散点不在障碍物里但实际线段穿过了障碍物。解决方法是把num_points设密一点同时把障碍物稍微“膨胀”一圈对圆形半径加一个安全余量对矩形边界外扩。工程上这种牺牲精度换速度的做法很实用能避免90%以上的碰撞漏检。3.3 碰撞检测函数的关键写法碰撞检测要分两步判断单个构型是否可行判断线段是否可行。单个构型碰撞把get_link_points得到的点集逐一和所有障碍物做包含判断。圆判断用距离矩形判断用四个边界不等式。如果任何一个点落在障碍物内部这个构型就是不可行构型。线段碰撞RRT从q_near扩展到q_new时允许两点都是可行点但连线可能穿过障碍物。所以在两个构型之间需要做线性插值在中间采样多个中间构型逐一点做碰撞判断。这相当于把连续路径离散成“细碎步长”来近似检测。插值采样的数量根据步长来定比如步长为0.2弧度时在这一段里再插值10个点基本够用。function feasible is_path_feasible(q1, q2, obstacles, L1, L2, interp_num) theta1s linspace(q1(1), q2(1), interp_num); theta2s linspace(q1(2), q2(2), interp_num); feasible true; for i 1:interp_num if ~is_configuration_feasible([theta1s(i), theta2s(i)], obstacles, L1, L2) feasible false; return; end end end特别提醒碰撞检测是采样算法的“事实裁判”它的误判直接影响成功率。我踩过最深的坑就是采样数太少导致树穿过了细长障碍物之间的窄缝路径看起来连上了实际机械臂会撞杆。后来我把线段插值数和障碍物膨胀系数一起作为安全冗余问题就解决了。3.4 RRT主循环实现主循环代码并不长但有几个细节我刻意处理过。function path_nodes rrt_planning(q_start, q_goal, obstacles, L1, L2, params) tree.nodes q_start; tree.parent 0; goal_threshold 0.2; % 终点判定阈值 step_size 0.2; % 构型空间步长 goal_sample_rate 0.1;% 偏向终点概率 max_iter 2000; for iter 1:max_iter if rand() goal_sample_rate q_rand q_goal; else q_rand [rand()*2*pi, rand()*2*pi]; end % 找最近邻 dists vecnorm(tree.nodes - q_rand, 2, 2); [~, idx_near] min(dists); q_near tree.nodes(idx_near, :); % 沿方向扩展 delta q_rand - q_near; norm_delta norm(delta); if norm_delta 0 continue; end q_new q_near step_size * delta / norm_delta; % 碰撞检测 if is_path_feasible(q_near, q_new, obstacles, L1, L2, 10) tree.nodes(end1, :) q_new; tree.parent(end1) idx_near; % 检查是否到达终点 if norm(q_new - q_goal) goal_threshold % 回溯路径 break; end end end % 回溯父节点链输出路径 path_nodes q_start; idx length(tree.parent); while tree.parent(idx) ~ 0 path_nodes [tree.nodes(idx, :); path_nodes]; idx tree.parent(idx); end path_nodes [q_start; path_nodes]; end这段代码里vecnorm在较新的Matlab版本中可以直接用老版本建议改写成sqrt(sum((tree.nodes - q_rand).^2, 2))。更重要的是注意角度范围采样时直接用了rand()*2*pi如果机械臂关节有硬限位比如第二关节不能转到300°就必须在采样后做一次限位裁剪否则规划出来路径执行不了。我一般把角度限制和碰撞检测合在一起在is_configuration_feasible里直接返回false。3.5 PRM两阶段实现PRM的学习阶段实现如下先随机采样再尝试连线。function graph prm_learn(obstacles, L1, L2, num_nodes, num_neighbors) graph.nodes zeros(num_nodes, 2); graph.edges cell(num_nodes, 1); % 邻接表 % 阶段一采样和过滤 added 0; while added num_nodes q [rand()*2*pi, rand()*2*pi]; if is_configuration_feasible(q, obstacles, L1, L2) added added 1; graph.nodes(added, :) q; end end % 阶段二连接邻居 for i 1:num_nodes dists vecnorm(graph.nodes - graph.nodes(i, :), 2, 2); [sorted_dists, idx] sort(dists); for j 2:num_neighbors1 neighbor idx(j); % 去重避免重复边 if ismember(i, graph.edges{neighbor}) continue; end if is_path_feasible(graph.nodes(i,:), graph.nodes(neighbor,:), obstacles, L1, L2, 10) graph.edges{i}(end1) neighbor; graph.edges{neighbor}(end1) i; end end end end这里有一个容易被忽略的坑sort(dists)之后距离最小的那个节点是它自己索引为1所以邻居要从j 2开始。另外K近邻的K若太大边的数量会膨胀碰撞检测次数剧增K太小图可能不连通。我试过在比较复杂的障碍物环境里K从5调到15规划成功率从60%提到了95%。这个参数的影响比预想的大得多。查询阶段其实很简单把起点和终点临时加入图用Dijkstra搜索最后回溯路径。Matlab里没有现成的图搜索函数其实有graph对象和shortestpath但我们要自己维护节点坐标矩阵和邻接表所以不直接用而是手写一个简单Dijkstra或者用稀疏矩阵构造图。我倾向于手写因为可以直接从邻接表转出路径序列不需要来回转换。3.6 可视化与动画让结果看得见Matlab做机器人路径规划最爽的就是绘图方便。我通常画三样东西工作空间里的障碍物、机械臂的初始姿态和终点姿态、规划出来的路径对应的机械臂扫过的姿态序列。function draw_robot(q, L1, L2, color) x1 L1*cos(q(1)); y1 L1*sin(q(1)); x2 x1 L2*cos(q(1)q(2)); y2 y1 L2*sin(q(1)q(2)); plot([0, x1, x2], [0, y1, y2], color, LineWidth, 2); hold on; end路径可视化时不要只画终点轨迹那样看不出避障过程。我习惯把路径均匀抽样成30~50个构型用半透明的细线逐帧画出来或者做成for循环的动画。如果嫌动画卡就画所有构型的连杆集合一眼能看出机械臂扫过的包络范围有没有贴近障碍物。4. 参数调节、路径后处理与结果对比4.1 RRT关键参数怎么调步长step_size我一般取构型空间维度范围的2%到5%。前面例子中角度范围是0~2π约6.28所以步长0.15到0.25都合理。步长太小树节点密集最近邻搜索慢太大窄通道穿越失败率高。goal_sample_rate取0.1到0.3。太高会导致树被终点引导得过于“直线”容易被障碍物挡住而反复扩展失败太低又容易在环境里闲逛。max_iter设2000如果到2000次还没找到路径大概率是环境过于复杂或者参数不合适不要无限循环下去。终点阈值目标点不一定能精确生长到所以只要距离小于0.2弧度就可以回溯路径。阈值的本质是允许最终结果稍微偏离目标后处理再补一插值即可。4.2 PRM关键参数怎么调num_nodes环境简单100够用障碍物多、窄通道多建议300以上。节点越多图越可能连通但学习阶段时间显著增长。num_neighbors常用5~10。K过小的直接后果是图断裂K过大的后果是很多长边横穿障碍物虽然碰撞检测会剔除但无效计算比例高。采样失败处理很多新手会忽略PRM采样点如果是碰撞点需要重新采样。环境越狭窄有效采样率越低。这时可以考虑用桥采样bridge sampling之类的改进策略但Matlab基础版就没必要上复杂度了。4.3 路径后处理从折线到可执行轨迹两个算法给出的路径都是折线直接给关节电机用会产生很大的速度突变。我最常用的后处理是“路径剪枝”循环检查路径中不相邻的节点能否直接无障碍连接如果能就把中间的节点删掉。这能让路径更短、拐弯更少。剪枝做完后再用三次样条或者梯形速度规划把关节角序列变成时间序列。具体实现可以用Matlab的cscvn或者手写线性插值加平滑。对课程设计来说线性插值加S形速度曲线就足够。注意剪枝有可能生成一条贴着障碍物的直线所以剪枝后一定要再做一次路径级碰撞检测防止“安全距离不足”。4.4 两种算法在同环境下的实测对比我设了一个具体场景三个圆形障碍物加一个长方形障碍物起点和终点分布在障碍物两侧。RRT参数用步长0.2、目标偏向0.15PRM用节点200、邻居10。跑20次取平均指标RRTPRM平均规划时间0.8秒学习阶段9秒 查询0.1秒路径角点数量23个12个路径总构型距离5.6弧度4.1弧度20次成功率95%100%RRT的规划时间看起来快很多但那是单次查询如果连续查询50组起点和终点PRM的图复用优势会非常明显总耗时反而低。另一个结论是RRT路径角点更多、更绕这是因为树每次只向外扩展一小步随机性导致路径曲折PRM的图搜索天然选择节点间直线距离最短的路线整体更“有章法”。5. 常见问题与排查技巧实录5.1 树越长越慢怎么办RRT在树长到几百个节点后每次找最近邻都要遍历全树耗时明显增加。二维空间里可以接受但如果你把代码扩展到六自由度机械臂这个瓶颈立刻变成灾难。建议先用简单遍历跑通然后改成KD树。Matlab里可以用knnsearch配合KD树模板或者使用KDTreeSearcher。另一个优化是每轮随机采样时不要只在构型空间均匀采样可以增加从终点附近采样的概率减少无效节点积累。5.2 碰撞检测的“穿透”误判最常见的误判是“明明路径在图上连好了动起来却撞了障碍物”。这多半是线段插值点数太少。把interp_num从10改成30误判情况会大幅减少。但也不能一味增大否则碰撞检测耗时翻倍。更好的办法是先用粗略插值快速淘汰明显碰撞对接近障碍物的边再用细插值确认。这个“两级碰撞检测”我强烈推荐。5.3 PRM图构建很慢怎么提速图构建的耗时主要在碰撞检测上。优化思路有两个一是先用节点间距初筛只有距离小于某个上限的节点对才做碰撞检测二是对每个节点只计算它和附近K个近邻的边不要全图两两互连。这两个优化能让学习阶段速度提升5倍以上。我遇到过一个很尴尬的情况节点数从200调到400邻居保持在10虽然图更完整但时间从5秒飙到20秒。后来换成距离阈值裁剪时间降回8秒成功率还更高了。5.4 RRT总在窄通道入口失败窄通道是采样算法最难啃的地方。如果RRT始终无法进入某个狭窄缝隙先检查障碍物是否膨胀太多把通道给堵死了确认通道宽度后再把步长调小。比如原来步长0.2调到0.08很可能就进去了。这是因为大步长在通过窄缝时需要恰好落在通道内部且方向与通道一致可能性极低小步长提高了“一点点挪进去”的成功率。5.5 角度跨0°/360°边界时的跳变问题二连杆关节角度如果允许0到2π那么0°和360°在物理上是同一个位置但欧氏距离会把它们算成6.28。这时候最近邻会选错节点路径也会出现不必要的绕路。解决办法是在计算构型距离时对每个关节角度做差值折叠function d angular_distance(q1, q2) d1 abs(mod(q2(1)-q1(1)pi, 2*pi) - pi); d2 abs(mod(q2(2)-q1(2)pi, 2*pi) - pi); d sqrt(d1^2 d2^2); end这个函数在角度跨越边界时能给出正确距离。把这个函数替换掉所有vecnorm用到的地方算法行为会明显更合理。6. 扩展思路与个人实操经验6.1 从二维平面到更高自由度的迁移虽然标题限定二连杆但如果你理解了构型空间的思想迁移到三连杆甚至空间机械臂只是变量维度增加。以RRT为例把节点从2维变成n维步长从标量变成向量或者保持向量归一化乘以步长最近邻距离从欧氏距离改成加权距离。碰撞检测则要换成连续连杆的离散点与三维障碍物判断。框架完全不用动。我后来把同样的RRT代码改写成六自由度机械臂版本只花了半天时间这就是把二维版本吃透的好处。6.2 更进阶的变体RRT*和双向RRT如果想把路径质量再提升可以在RRT基础上升级为RRT*每当加入一个新节点在其邻域内搜索可能使代价更小的父节点并重新布线。RRT*的路径会随着迭代次数增加渐进收敛到最优。双向RRT则是从起点和终点各长一棵树每次随机采样后同时扩展两棵树然后尝试连接两棵树之间的近邻节点。双向RRT在窄通道环境里的成功率比单向RRT高很多建议作为课程作业的加分项。6.3 我的几点体会把这两个算法在Matlab里完整实现一遍后我的感受是采样算法真正的难点不在于“随机”这两个字而在于如何有效利用碰撞检测结果来平衡探索与利用。RRT适合“一次性快速找到可行路径”PRM适合“环境固定且需要反复规划”。二连杆模型是理解这些权衡的最佳测试平台因为它可视化直观、计算量小、参数调节的反馈即时。做这个项目时我还养成了一个习惯每次调参前先把当前参数下的算法表现记录下来做一个简单的实验日志。因为RRT包含随机性连续两次运行结果差距很大如果凭感觉调参根本分不清是参数变好了还是运气变好了。比如我用同一个参数跑10次记录成功次数、平均节点数和平均路径长度对比才有意义。这算是课程设计之外的额外收获。最后再分享一个小技巧在Matlab里调试时把随机数种子固定rng(0)能让每次运行结果一致。你在调碰撞检测或者调路径后处理时会省很多对比的力气等一切稳定后再取消固定种子观察随机性带来的效果波动。这个细节很多教程不会提但对实际开发调试帮助极大。