二维连杆机器人路径规划听起来像是教科书里的案例但实际做起来却是最见功底的入门级课题。这个项目用 MATLAB 实现两种采样类算法——RRT 和 RPM——在二维连杆机器人的关节空间里做路径规划不依赖商业工具箱手写核心函数同时把碰撞检测、最近邻搜索、路径搜索和后处理完整串起来。不管你是刚接触机器人规划的学生还是想快速评估算法效果的工程师这套代码和思路都能直接拿来改。下面我把整个设计拆解、核心细节、MATLAB 实现流程以及我踩过的坑一次性说清楚。1. 项目整体设计与思路拆解1.1 为什么选二维连杆机器人当试验台二维连杆机器人是两个旋转关节串联构成的平面机械臂基座固定第一个连杆绕肩关节转第二个连杆通过肘关节相对第一根杆转动。它的状态完全由两个关节角度决定所以配置空间天然是二维。这个特性非常关键二维配置空间可以可视化复杂的高维规划问题在这里被压缩成“在一片障碍区域中找一条连通路径”避开障碍物的形状也能直接画成图。项目用了“关节空间”而非“笛卡尔空间”来做规划。关节空间里的一个点就是机械臂一个姿势RRT 和 RPM 采样的都是关节角度组合。每次扩展或连接边的时候要通过正运动学把角度转成两个连杆在平面上的位置再做碰撞检测。正运动学公式很基础但却是整个项目的支点x1 L1 * cos(theta1) y1 L1 * sin(theta1) x2 x1 L2 * cos(theta1 theta2) y2 y1 L2 * sin(theta1 theta2)这里 theta1 是肩关节角theta2 是相对第一根杆的关节角L1 和 L2 分别是两段连杆长度。如果 theta2 是绝对角度公式里就要改成 cos(theta2) 和 sin(theta2)这个细节我在调试时吃过大亏后面单独说。1.2 RRT 和 RPM 两条路线的选择逻辑RRT快速扩展随机树是增量式算法从起点出发不断随机采样在树中找到最近节点再朝采样点扩展一个步长反复迭代直到接近目标点。它的特点是单查询跑一次只回答“从当前起点到目标点有没有路、走哪条”。RPM概率路线图法很多资料里写作 PRM则是两阶段方法先在整个配置空间中撒点保留自由空间的采样点将这些点的近邻连接成图形成路线图查询阶段用 Dijkstra 或 A* 在这个图上搜最短路径。它的特点是多查询只要路线图构建好任意起点和终点都能直接查所以静态环境中一旦构建完路线图多次规划的总成本反而更低。两个算法在二维连杆机器人的对比很有意思。二维配置空间只有两个自由度RRT 的快速随机扩展几乎不会遇到高维空间那种“维度灾难”很快就能打通。RPM 的优势体现在重复规划上如果障碍物不变路线图只建一次后续每次换起点和终点都只是查图速度是毫秒级。我做这个项目的第一个任务就是让同一套环境、同一组起点终点跑两种算法观察路径形态、成功率、耗时再决定哪个方案更适合后续的集成场景。1.3 功能模块划分整个项目按“采样—碰撞检测—搜索—后处理”划分模块每种算法只改搜索逻辑其他模块复用。采样模块RRT 需要随机节点和目标偏置采样RPM 需要一次性生成大规模随机样本。正运动学模块输入两个关节角输出两个连杆的端点坐标。碰撞检测模块判断单个配置是否在自由空间判断两个配置之间的线性插值是否完全安全。最近邻模块输入当前节点或样本找到距离最近的树节点/图节点。搜索模块RRT 用树的增量扩展RPM 用 Dijkstra 最短路径。后处理模块路径裁剪、平滑和重碰撞检测。把碰撞检测单独抽出来是这项目的精髓因为两种算法的正确性都依赖于它RRT 扩展一条边时要用它判断“这一段插值路径会不会碰障碍物”RPM 构建路线图时要用它判断“两个采样点之间能否直连”。如果碰撞检测不可信后面所有路径都是假路径。2. 核心细节解析与实操要点2.1 碰撞检测从“角度”到“障碍物距离”的换算二维连杆机器人的碰撞问题主要是连杆与障碍物的相交问题。我处理的是圆形障碍物圆心 c半径 r判断线段到圆心的最小距离 d 是否小于 r。用圆形障碍物有个好处距离计算稳定不需要处理多边形相交的复杂逻辑而且把障碍物半径稍微扩大一点就能给连杆留安全余量。判断“单配置是否碰撞”的流程是根据 theta1 和 theta2 计算杆件端点生成两段线段——第一段从基座到肘关节、第二段从肘关节到末端计算每条线段到每个障碍物圆心的最短距离如果任一距离小于障碍物半径该配置判为碰撞。“点到线段的最短距离”不要用“点到直线距离”代替直线距离会把线段延长线上的投影也算进去明显误判。正确做法是先求投影参数 t clamp(dot(p - a, b - a) / dot(b - a, b - a), 0, 1)再取 a t * (b - a) 与 p 的距离。这个公式简单但很多人第一次写都会漏掉 clamp。判断“两个配置之间的边是否碰撞”时不能只检查两端点必须对整条边做离散插值。因为两个关节角同时变化时连杆在笛卡尔空间会画一条曲线曲线轨迹可能扫过障碍物。我在项目里将角度差 norm(q2 - q1) 除以最大允许角度变化量 max_delta_q向上取整得到插值段数然后逐段检查。max_delta_q 我取 0.05 到 0.1 rad这个值越小越保守计算越慢取值太大细长障碍物可能被“跨过去”。额外做一个可视化辅助用 meshgrid 在配置空间生成密集网格逐格判断碰撞得到一个二维二值地图。MATLAB 里用 imagesc 画出来白色是自由区域黑色是碰撞区域。这个地图至关重要能让我一眼看出障碍物在 C-space 中的形状判断算法跑出的路径是否真的合理。调参时没有这张图基本等于盲调。2.2 采样策略和最近邻搜索如何影响效率RRT 的效率很大程度上取决于最近邻搜索。所以我把最近邻函数单独做了一个简单实现是遍历树中所有节点算欧氏距离但树节点数超过几千后遍历会很拖慢。MATLAB 里可以直接用knnsearch和KDTreeSearcher构建 KD 树每次扩展时查最近节点耗时能降一个量级。如果不想依赖工具箱也可以自己用rangesearch或直接暴力搜毕竟二维配置空间节点上千时暴力搜也就几毫秒不会成为瓶颈。RPM 的采样量要远多于 RRT。我常见配置是 500 到 2000 个样本点再加上路径起终点。采样时先随机生成关节角度然后用碰撞检测过滤只有自由空间的点才保留。这样得到的样本点其实带有随机性可能出现某些狭窄通道里一个点都没有导致路线图不连通。解决办法是提高采样密度或者在窄通道附近定向补采具体要看障碍物分布。最近邻连接中邻域半径是个需要权衡的参数。半径太短路线图稀疏容易被窄通道切断半径太长每个节点的邻域连接增多路线图边的数量平方级上升查询变慢。二维配置空间的面积是 (theta1_range * theta2_range)。我常用经验是先按均匀分布估算理论半径再实际扫描几组值用“路线图连通分量是否包含起终点所属分量”作为判据。2.3 关键参数的定量取值与调参原则RRT 需要设置的参数step 步长每次向随机点扩展的最大角度距离。对二维连杆机器人我建议 0.1 到 0.3 rad。步长太大路径粗糙甚至容易跨过窄道步长太小需要更多迭代。goal_bias 目标偏置以一定概率直接采样目标点而不是随机点。0.1 到 0.3 之间比较合适。我设 0.15因为太高会让树过于“贪婪”容易被障碍物死角卡住反而求不到解。max_iter 最大迭代次数3000 起步。二维 C-space 一般几百到一千次就能找到但为了防止意外场景保守上限设为 3000。goal_tol 目标容差判断当前节点是否到达目标点0.05 到 0.1 rad。这个值要小于窄通道宽度。RPM 需要设置的参数n_samples 采样数500 到 2000视障碍物复杂度和配置空间面积。neighbor_radius 邻域半径参考公式 r k * sqrt(area / n_samples)k 取 1.5 到 2.5。例如配置空间范围 [0, 2π] × [0, 2π]面积约 39.5采样 800 时理论半径约 0.27乘系数后得到 0.5 左右。如果障碍物多、通道窄直接把系数提高到 3.0牺牲一些查询速度换取连通性。max_edge_skip 碰撞检测插值段数同样用 max_delta_q 控制。调参原则就一句话先看 C-space 地图。如果自由区域是连通的但算法失败优先调大采样密度或步长如果路径明显绕远优先调大目标偏置或后处理裁剪次数。3. 实操过程与核心环节实现3.1 MATLAB 代码结构与数据组织我按函数文件方式组织工程主脚本控制流程功能函数单独放一个 .m 文件。结构体数组用来存 RRT 树每个元素包含q和parent两个字段parent存父节点在数组中的下标。RPM 则用一个节点矩阵 samples行坐标是角度列坐标是样本序号和一个邻接矩阵 adjadj(i,j) 存储第 i 个样本到第 j 个样本的直线距离不可达则置 inf。主流程分四步定义机器人参数L11.0、L20.8关节范围设为 [-pi, pi] 与 [-pi, pi]。定义障碍物三个圆形圆心和半径按需设置半径取 0.2 到 0.3。调用 RRT 或 RPM 得到路径节点序列。画三个图C-space 二维地图、配置空间的路径、笛卡尔空间机械臂逐帧动画。整套代码不依赖机器人工具箱只用基础函数和可能存在的统计/图函数。如果把shortestpath替换成自编 Dijkstra就完全零依赖移植到其他环境也方便。3.2 RRT 路径搜索核心实现RRT 核心函数我写成下面这样function [path, tree] rrt_plan(q_start, q_goal, obs, L, params) tree(1).q q_start; tree(1).parent 0; tree(1).cost 0; for iter 1:params.max_iter if rand params.goal_bias q_rand q_goal; else q_rand params.q_min rand(1, 2) .* (params.q_max - params.q_min); end idx_near nearest_neighbor(tree, q_rand); q_near tree(idx_near).q; q_new steer(q_near, q_rand, params.step); if is_edge_free(q_near, q_new, obs, L, params) tree(end 1).q q_new; %#okAGROW tree(end).parent idx_near; tree(end).cost tree(idx_near).cost norm(q_new - q_near); if norm(q_new - q_goal) params.goal_tol path extract_path(tree, numel(tree)); return; end end end path []; endsteer的功能是在 q_near 和 q_rand 的连线上截取一个固定步长如果两者距离本身小于步长就直接返回 q_rand。实现时用归一化方向向量乘步长function q_new steer(q_near, q_rand, step) delta q_rand - q_near; dist norm(delta); if dist step q_new q_rand; else q_new q_near delta / dist * step; end endnearest_neighbor如果用 KD 树可以写成function idx nearest_neighbor(tree, q) qs reshape([tree.q], 2, []); idx knnsearch(qs, q); end注意如果树节点数量增长很快每次调用都去构造 KD 树反而开销大。更高效是维护一棵持久化 KD 树但对二维小规模问题来说直接暴力遍历也完全可用我实测 3000 个节点内差别不大。提取路径时从目标节点沿 parent 回溯到起点然后把路径逆序因为回溯得到的是从目标到起点。function path extract_path(tree, idx) path []; while idx ~ 0 path [tree(idx).q; path]; idx tree(idx).parent; end end这段代码里有个性能隐患每次 while 循环用矩阵拼接生成 path节点多时会反复复制。二维规划路径最多几百个点影响可以忽略。如果是三维以上机器人大规模路径建议用 cell 先收集再合并。3.3 RPM 路线图构建与查询实现RPM 分两个阶段学习阶段代码function graph_data build_roadmap(q_start, q_goal, obs, L, params) samples zeros(params.n_samples, 2); count 0; while count params.n_samples q params.q_min rand(1, 2) .* (params.q_max - params.q_min); if is_config_free(q, obs, L, params) count count 1; samples(count, :) q; end end % 手动加入起终点 samples [q_start; q_goal; samples]; n size(samples, 1); adj inf(n, n); for i 1:n dists vecnorm(samples - samples(i, :), 2, 2); neighbors find(dists 0 dists params.neighbor_radius); for j neighbors if is_edge_free(samples(i, :), samples(j, :), obs, L, params) adj(i, j) dists(j); adj(j, i) dists(j); end end end graph_data.samples samples; graph_data.adj adj; end采样循环这里有个细节如果配置空间中自由区域占比很小纯随机采样可能要循环很久才能凑够 n_samples。更稳的写法是先随机生成一个大矩阵用 is_config_free 向量化过滤再用find取出前 n 个自由点这样能减少循环次数。查询阶段用 MATLAB 自带的图对象最省事function path query_roadmap(graph_data, q_start, q_goal) g graph(graph_data.adj); start_idx 1; goal_idx 2; [path_nodes, ~] shortestpath(g, start_idx, goal_idx); path graph_data.samples(path_nodes, :); end这里我默认 samples 矩阵的第一行是起点、第二行是终点。实际工程中最好改成根据距离搜索对应行避免起终点输入变化时下标写死。路线图的邻接矩阵是稀疏的因为每个节点的邻居数量远小于总样本数。直接用全矩阵当 graph 的输入MATLAB 会自动转成稀疏但如果样本上万内存会有点紧张。测试 2000 个样本没问题更大规模建议显式构造 sparse 矩阵。3.4 路径裁剪与平滑后处理RRT 原始路径相当“毛糙”因为扩展方向受随机采样影响路径包含大量冗余转折。RPM 用 Dijkstra 搜出来的路径虽然是无环最短路径但同样存在“贴着障碍物折线走”的问题。我用的第一个后处理方法是路径裁剪思路很简单对路径中任意两个节点 i 和 j如果 i、j 之间沿直线连接且整段无碰撞就用这段直线替代中间所有节点。这个过程反复迭代直到无法再裁剪为止。function simplified simplify_path(path, obs, L, params) simplified path(1, :); i 1; while i size(path, 1) for j size(path, 1):-1:(i 1) if is_edge_free(path(i, :), path(j, :), obs, L, params) simplified [simplified; path(j, :)]; %#okAGROW i j; break; end end end end这种裁剪算法能显著减少路径点数但对尖锐转角没有改善。想要平滑路径我用 MATLAB 的 pchip 插值把角度路径加密加密后再做一次碰撞检测。注意加密后的中间点可能插到障碍物边缘所以碰撞检测必不可少。平滑后的路径还要考虑机械臂的关节约束也就是关节速度限制。虽然本项目没有动力学模型但规划结果应该尽量平滑避免运动控制时出现速度尖峰。路径裁剪加插值后角度序列连续末端轨迹在笛卡尔空间一般也比较好看。4. 算法表现对比与适用场景分析4.1 相同障碍布局下的求解对比我用三组不同的障碍布局测过两种算法分别记录是否成功、路径长度、搜索耗时、路线图构建耗时。表格里放一组典型结果指标RRTRPM自由空间连通且无障碍狭窄通道成功路径约 3.2 rad成功路径约 2.8 rad中段仅有一个窄通道成功率高但路径较绕可能因采样不足断连提高采样后可成功三次重复规划每次重新搜索耗时不稳定建图一次后查询约 0.01 秒路径质量锯齿明显需裁剪平滑相对规整但仍有小折线参数敏感度步长和目标偏置敏感采样数和邻域半径敏感RPM 在多次查询场景明显占优。如果只是“从同一个固定起点到同一个固定终点”RRT 胜在写起来快。但真实项目里比如装配工作站机械臂经常需要在几个固定工位间切换任务构建一次路线图反复查询是更划算的选择。RRT 的一个优势是可以在线扩展环境变了一点也能从现有树继续长RPM 路线图在环境中添加新障碍物时必须把受影响区域的节点和边删掉或重新采样比较麻烦。4.2 扩展方向从二维连杆到更高维空间二维连杆机器人的配置空间是二维采样类算法的优势没有被完全发挥。真正体现 RRT 价值的是七自由度机械臂、移动机器人多关节系统那时配置空间动辄十维以上网格法彻底失效RRT 仍然能用随机采样探索出可行路径。RPM 在高维空间的困难更明显因为随机撒点在高维空间极其稀疏窄通道被采样点命中的概率很小所以高维场景一般会用 RRT* 或基于优化的方法。但我依然建议先从二维连杆做起。原因很简单二维 C-space 能被可视化算法的每一步都能用图检查调参时能直接看到“树为什么长不过去”“路线图为什么断掉”。把碰撞检测模块设计成可复用的函数后续换到三维机器人时只需要重写正运动学函数碰撞检测、采样和搜索框架几乎不变。从这个项目可以衍生很多扩展把 RRT 升级成 RRT*在扩展后尝试连接附近节点并重新选择父节点让路径渐进最优把 RPM 升级成 lazy PRM先不检测所有边搜索时只检测候选路径上的边节省大量碰撞检测。每种扩展都值得单独写一个实验但底子还是这套基础代码。5. 常见问题与排查技巧实录5.1 路径显示穿过障碍物这个问题十有八九出在碰撞检测的插值步长上。我一开始只检查了路径节点本身也就是离散配置点认为它们不碰撞就代表路径安全。结果发现两个节点之间的角度插值会让连杆扫过障碍物边缘路径虽然“在 C-space 里”看起来没问题动画演示时却穿模。排查方法把 is_edge_free 里的 max_delta_q 调小比如从 0.1 改成 0.02重新计算路径。如果问题解决说明是插值精度不够。还有一种隐蔽情况圆形障碍物半径用来做距离判断但机械臂的连杆本身有一定宽度真实机器人连杆不是无限细的线段需要在碰撞检测时把障碍物半径加上连杆半宽。我在代码里统一通过inflate_obs_radius参数处理这样不用改物理参数就能留安全裕量。5.2 C-space 地图正常但 RRT 长时间搜索失败先确认起点和目标点本身是自由配置然后检查树节点是否在起点附近反复扩展。最常见原因是步长设置得太短树没有能力穿越窄通道导致指数级扩展。把步长从 0.1 逐步调大到 0.4通常能解决。另一个原因是目标偏置设太高比如 0.5。树频繁被拉向目标点如果目标点附近有障碍物扩展尝试全被碰撞检测拦下树就被卡在障碍物边界。我建议目标偏置初始设 0.15观察路径生成后如果过于曲折再适当提高。还应该打印扩展成功率和迭代次数如果连续几百次迭代都没有新节点加入赶紧停下来查参数。5.3 RPM 路线图不连通查询返回空路径先可视化路线图的连通分量。把 samples 中所有节点按“是否从起点可达”染色画在地图上。如果起点所在分量和终点所在分量是两坨点说明采样或邻域半径有问题。处理优先级先增大 neighbor_radius这个是见效最快的。如果半径已经到理论值的 2 倍还不行再增加采样数量。有时窄通道恰好没有采样点增加采样数量也不能保证命中这时需要定向采样在窄通道中心区域附近额外撒点。手动实现也不复杂就是在随机采样之外额外把几个窄通道中心坐标加入待采样列表构造路线图时直接包含它们。5.4 关节角跨越 ±180° 时距离计算错误二维连杆的关节范围如果设为 [-pi, pi]那么 pi 和 -pi 在物理上是同一个角度。直接用欧氏距离算最近邻和步长会让树误以为这两个角度距离很远。这会造成靠近边界时路径突然绕一大圈或者 RRT 卡在边界附近。处理方式是写成 wrap 角度差函数function d angdiff(a, b) d mod(a - b pi, 2 * pi) - pi; end把最近邻、steer、步长判断里的 norm 换成基于 angdiff 的距离。比如两配置的“等效距离”可以写成 sqrt(angdiff(q1(1), q2(1))^2 angdiff(q1(2), q2(2))^2)。这个坑对二维连杆不明显因为角度范围本身只有 2π但对关节多、角度范围大的机器人不处理会直接导致规划失败。5.5 调试技巧把路径画在地图上最后分享一个我自己非常依赖的调试技巧把起点、终点、算法生成路径、C-space 黑白地图画在同一张图上用散点图叠上去。这样路径有没有穿过黑色区域一目了然RRT 树的扩展方向也能看到。我会在每一步迭代后暂停刷新观看树是否被障碍物引导得合理。这个技巧帮我节省了大量排错时间。做路径规划千万不要只盯着最终路径一定要多看配置空间的结构。二维连杆刚好是能让你“看见”所有细节的最佳起点。
