先聊一个很常见的误区给二连杆机械臂做路径规划很多人第一反应是“把末端点从A挪到B不就行了吗”。但实际调一版程序你就会发现末端点明明能过去中间那根杆子却撞上了障碍物仿真画面直接穿模。这正是二维连杆机器人路径规划有意思的地方——你规划的不是末端点在物理空间里的轨迹而是两个关节角在构型空间里的一条路径。这篇文章把我做这个项目时整理的完整思路放出来基于RRT快速扩展随机树和RPM概率路图法两种经典采样算法从运动学建模、碰撞检测到MATLAB实现代码一条龙讲清楚。文章会尽量照顾不同基础的读者不会只丢一堆代码而是解释每个模块为什么这样写。正在做机械臂路径规划仿真、入门采样规划算法或者想快速复现一套可用代码的同学这篇文章应该能帮你少走不少弯路。1. 先把问题讲清楚二维连杆机器人到底在规划什么1.1 末端能到不等于机械臂能到为什么路径规划要做在关节空间二连杆机器人的位形由一个二维向量 (θ1, θ2) 唯一确定θ1 是第一个关节角θ2 是第二个关节相对第一个关节的转角。整个机械臂在二维平面上的可达区域其实是一个由 L1 和 L2 两根杆长决定的圆环。很多初学者习惯在“工作空间”里规划也就是直接画一条末端从起点走到终点的直线然后反解关节角。这种做法在无障碍物时没问题一旦环境里存在障碍物就立刻露馅末端路径是直的但中间连杆扫过的面积可能把障碍物包进去了。路径规划必须放在关节空间也叫构型空间C-space里做。C-space 里的一个点不代表机械臂末端的坐标而是代表“两个关节各转了多少度”。在这个空间里机械臂被抽象成一个点障碍物则被映射成一片形状非常不规则的禁区。采样算法的核心思想很直白我不去解析障碍物的精确边界而是不断在 C-space 里随机撒点只保留不在禁区里的点然后用它们拼出一条从起点位形到终点位形的通路。1.2 正运动学建模与障碍场景定义先定义两根连杆的长度我习惯用 L1 1.0、L2 0.8单位不必纠结建模时保持一致即可。第二个关节角 θ2 我采用“相对转角”而非“绝对转角”也就是第二个连杆的方向角是 θ1 θ2。这样正运动学公式写出来很对称关节1位置固定在原点 (0, 0)关节2位置p1 (L1·cosθ1, L1·sinθ1)末端位置p2 (L1·cosθ1 L2·cos(θ1θ2), L1·sinθ1 L2·sin(θ1θ2))对应的MATLAB函数很简单function [p0, p1, p2] forward_kinematics(q, L) % q [theta1, theta2] % L [L1, L2] L1 L(1); L2 L(2); t1 q(1); t2 q(2); p0 [0, 0]; p1 [L1 * cos(t1), L1 * sin(t1)]; p2 [p1(1) L2 * cos(t1 t2), ... p1(2) L2 * sin(t1 t2)]; end注意整个规划过程中我们最主要的碰撞检测对象是线段 p0-p1 和 p1-p2末端点 p2 反而不是唯一需要关心的。这是连杆机器人和移动机器人最本质的区别。1.3 构型空间与角度周期性的第一个大坑θ1 和 θ2 的取值范围我统一用 [-π, π]。这里藏着一个新手必踩的坑角度的周期性。θ1 179° 和 θ1 -179° 在物理上是几乎同一个位形两个关节角只差 2°但如果你直接用欧氏距离算它们的距离是 358°这会导致最近邻搜索和步长扩展全部出错。RRT 和 RPM 都依赖“在 C-space 里找最近点”这个操作角度回绕问题不解决算法会在边界处疯狂震荡。处理办法是用 MATLAB 的 wrapToPi 函数或者自己写一个角度差计算function d angle_dist(a, b) d abs(wrapToPi(a - b)); end后面所有涉及角度距离的地方都必须走这个函数不能用直接减法。2. 碰撞检测模块两种算法共同的性能命门2.1 把连杆简化成线段简化几何的边界二维连杆在平面里扫过的区域可以近似看成两根带厚度的线段但常规做法是先忽略杆的粗细直接用线段 p0-p1 和 p1-p2 做碰撞检测。这么做的好处是计算量小坏处是过于乐观——实际机械臂有物理宽度贴着障碍物规划的路径执行起来极可能刮蹭。我的习惯是在碰撞判断里增加一个安全余量比如障碍物半径统一加 0.02用膨胀障碍物的方式补偿连杆宽度。这个技巧很简单但很有效。2.2 圆形障碍物的碰撞判断点到线段的距离障碍物先做最简单的情况一个圆心在 c、半径为 r 的圆。要判断连杆是否撞上它本质是求“线段上离圆心最近的那个点”再算距离是否小于 r。点到线段的距离公式和点到直线的距离不一样因为垂足可能落在线段延长线上这时候最近点就是线段的某个端点。function dist dist_point_to_segment(P, A, B) % P 是圆心A/B 是线段端点 v B - A; w P - A; t dot(w, v) / dot(v, v); t max(0, min(1, t)); % 限制投影系数范围 closest A t * v; dist norm(P - closest); end有了这个函数圆障碍物的碰撞检测就变成分别对两根连杆线段调用一次任何一根的距离小于 r当前位形就判定为碰撞。2.3 矩形障碍物与线段相交检测矩形障碍物在二维地图里也很常见。判断线段是否与矩形相交常规做法是把矩形拆成四条边分别做线段相交测试。两个线段相交的判定用“跨立实验”最简洁线段 AB 和 CD 相交当且仅当 A、B 分别在 CD 两侧且 C、D 分别在 AB 两侧。MATLAB实现如下function inter line_segment_intersect(p1, p2, p3, p4) d1 cross2d(p4 - p3, p1 - p3); d2 cross2d(p4 - p3, p2 - p3); d3 cross2d(p2 - p1, p3 - p1); d4 cross2d(p2 - p1, p4 - p1); inter (d1 * d2 0) (d3 * d4 0); end function c cross2d(u, v) c u(1) * v(2) - u(2) * v(1); end注意这里的边界条件我用了 0也就是线段恰好擦过矩形顶点也算相交。在实际规划中擦边而过意味着安全余量为零我宁可多判几次碰撞也不要放进去一个贴边位形。2.4 边采样大步长扩展时的分段碰撞检查RRT 和 RPM 在连接两个位形节点时判断的是“这两个位形之间的整条边是否都在自由空间里”。如果两个位形本身都是自由的但连接它们的直线穿过了障碍区怎么办必须对这条边做细分采样在两点之间插值出一串中间点逐点检查碰撞。我一般把插值步长设为 0.02~0.05 弧度具体取决于障碍物尺寸。障碍物越小插值间隔要越细否则细杆障碍可能被整段跳过。这里有个性能经验碰撞检测是整个算法最耗时的模块占比能到 70% 以上。优化方法是先做粗检测比如判断整根连杆的包围盒是否离障碍物很远距离超过阈值就跳过精细计算。3. RRT快速扩展随机树从随机采样到一条可执行路径3.1 算法流程与树结构RRT 的逻辑很像“在黑暗里往各个方向扔绳子”从起点出发每次随机选一个方向扔一段固定长度的绳子如果扔出去的那段没有碰到障碍物就把新端点记下来重复这个过程直到有绳子离目标足够近。本质上它是在 C-space 里从起点开始生长一棵树树不断向随机方向探索。代码层面我习惯用三个数组维护这棵树nodesN×2 的矩阵每一行是一个位形 [θ1, θ2]parentsN×1 的数组记录每个节点的父节点索引costsN×1 的数组记录从起点到当前节点的累计代价3.2 扩展步长、目标偏置和最近邻搜索三个关键参数直接决定RRT的表现。第一个是扩展步长 step_size每次向着随机点前进多少弧度。步长太大会导致边采样时频繁撞墙步长太小则收敛极慢。我习惯按 C-space 对角线长度的一定比例来设比如在 [-π, π]² 空间里对角线约 4.44 弧度取 0.1~0.2 比较稳。第二个是目标偏置概率 p_goal每次采样有 5%~10% 的概率直接以目标点作为采样点而不是随机点。这样能避免树漫无目的地乱长。但偏置概率不能太大太大会让树“太急”在复杂环境里容易卡死。第三个是最近邻搜索因为维度只有两维直接暴力遍历所有节点完全没问题没必要上KD树。3.3 主循环与路径回溯核心主循环代码不长但信息密度高function path rrt_plan(q_start, q_goal, map, param) nodes q_start; parents 0; costs 0; step param.step_size; p_goal param.p_goal; max_iter param.max_iter; tol param.tol; for iter 1:max_iter if rand p_goal q_rand q_goal; else q_rand [rand_range(-pi, pi), rand_range(-pi, pi)]; end [dist_min, idx_min] min(dist_cspace(nodes, q_rand)); q_near nodes(idx_min, :); direction q_rand - q_near; direction direction / norm(direction); q_new q_near step * direction; q_new wrapToPi(q_new); if is_edge_free(q_near, q_new, map) nodes(end1, :) q_new; parents(end1) idx_min; costs(end1) costs(idx_min) cost_between(q_near, q_new); if norm(wrapToPi(q_new - q_goal)) tol path extract_path(nodes, parents); return; end end end error(RRT reached max iteration without finding a path); end其中 dist_cspace 是我封装好的“带角度回绕处理的C空间距离”is_edge_free 则调用边采样碰撞检测。路径回溯是从最后一个节点沿着 parents 链条一直找到起点然后把节点顺序翻转就得到从起点到终点的位形序列。3.4 路径平滑从锯齿线到可执行轨迹RRT 的原始路径通常是锯齿状的直接在真实机械臂上执行会带来严重的运动冲击。我常用的处理是贪心平滑尝试把路径中的某两个节点直接用直线连接在关节空间里如果这段直线完全自由就删除中间所有节点。从起点开始不断向后试探最远能一笔连到哪个节点。贪心平滑的实现逻辑非常直白function smooth_path greedy_smooth(path, map) smooth_path path(1, :); i 1; while i size(path, 1) for j size(path, 1):-1:(i1) if is_edge_free(path(i, :), path(j, :), map) smooth_path(end1, :) path(j, :); i j; break; end end end end平滑完的路径节点数通常会减少 50% 以上代价函数也会明显下降。注意每次裁剪完都要过一遍碰撞检测贪心算法有时会把一条看似安全但实际擦边的短路径剪出来。4. RPM概率路图撒点建图一次投资多次查询4.1 构建阶段与查询阶段RPM 的思路和 RRT 完全不同。RRT 是为“单次规划”设计的从起点长树RPM 则先把整个 C-space 的自由区域“摸一遍底”建立一张连通图之后随便给哪对起点终点都能在图上快速查路。我当初第一次跑通 RPM 的代码时最大的感受是构建阶段花了半天查询阶段秒出结果。它特别适合那种“环境不变但任务点经常换”的机器人应用场景。构建阶段分三步第一步在 C-space 里均匀撒 N 个随机点第二步剔除落在障碍区域的点第三步对剩下的自由点把彼此距离小于连接半径的点用边连起来并检查这些边是否自由。查询阶段则是把起点和终点分别连接到图上最近的两个自由节点然后调用图搜索算法找路径。4.2 邻域连接策略半径固定还是k近邻连接半径的选择是 RPM 构建阶段最大的技术活。半径设大了计算量成平方上涨而且容易跨过狭窄通道把本不该相连的区域连起来半径设小了图会碎成好几块连通性不足。我可以给一个经验起点在 [-π, π]² 空间里如果采样 N 500 个点半径取 0.6~0.8 弧度比较稳。比固定半径更稳的方式是用 k 近邻对每个节点只连接最近的 k 个点我常用 k 8~12。这样能让密度低的区域不会因为固定半径过小变成孤立点密度高的区域也不会产生冗余稠密边。MATLAB 里可以直接调 knnsearch不需要自己写。4.3 用MATLAB图对象做最短路径查询构建完路图后MATLAB 自带的 graph 对象可以直接派上用场function [path, ok] rpm_query(roadmap, nodes, q_start, q_goal) % 连接起点终点到路图 [~, idx_s] min(dist_cspace(nodes, q_start)); [~, idx_g] min(dist_cspace(nodes, q_goal)); all_nodes [q_start; nodes; q_goal]; s_edges []; t_edges []; w_edges []; if is_edge_free(q_start, nodes(idx_s, :)) s_edges(end1) 1; t_edges(end1) idx_s 1; w_edges(end1) dist_cspace(q_start, nodes(idx_s, :)); end if is_edge_free(q_goal, nodes(idx_g, :)) s_edges(end1) size(all_nodes, 1); t_edges(end1) idx_g 1; w_edges(end1) dist_cspace(q_goal, nodes(idx_g, :)); end G graph(s_edges, t_edges, w_edges); % 再叠加 roadmap 的边…… [path_indices, cost] shortestpath(G, 1, size(all_nodes, 1)); path all_nodes(path_indices, :); ok ~isinf(cost); end这里有个容易忽略的细节如果起点或终点本身处在碰撞区域内需要先做本地修复比如在其邻域里搜索最近的自由点否则查询阶段必失败。我在早期版本里忽略过这个结果同一个起点在不同的随机种子下时好时坏后来发现是起点的自由状态没有做校验。5. 实测对比成功率、耗时与路径质量的取舍5.1 测试场景与统计方法为了让对比不那么主观我设置了两个典型场景。场景A是一个空旷环境中间只有一个圆形障碍物场景B是一个复杂环境里面有多个矩形和一个环形障碍物围出的狭窄通道。固定 L11.0、L20.8起点 q_start [-1.5, 0.8]弧度目标 q_goal [1.2, -0.6]。每个场景下 RRT 和 RPM 各跑 30 次。RRT 参数step_size 0.15p_goal 0.1max_iter 5000。RPM 参数N 500连接半径 0.7k 10。统计三个指标规划成功率、平均规划耗时、最终路径的关节空间总长度。5.2 结果数据与原因分析在场景A空旷环境中RRT 平均耗时约 0.8 秒成功率 100%路径长度均值约 5.2RPM 构建阶段平均耗时 3.5 秒查询阶段 0.02 秒成功率 100%路径长度均值约 4.1。RPM 路径更短的原因很简单路图上运行的是 Dijktra 全局最优搜索而 RRT 的树扩展带有较大随机性即使做了平滑也很难追上路图搜索的质量。在场景B狭窄通道中两张算法的差异被明显放大。RRT 在 30 次里有 8 次达到迭代上限失败成功率约 73%平均耗时升到 3.1 秒路径质量也明显下降。RPM 构建路图时N500 的采样点很难密集覆盖狭窄通道导致路图频繁断裂成功率只有 47%但把 N 提升到 1500 后成功率升到 83%构建耗时就到了 15 秒量级。这说明RPM的性能瓶颈在采样密度而RRT的瓶颈在随机性的运气。5.3 该怎么选一句话判断标准我的选择标准非常朴素如果只规划一次或几次果断用 RRT它从起点出发生长天然更聚焦不需要花时间探索没用的区域如果环境固定、任务点经常变化用 RPM因为建图成本可以摊薄到无数次查询里如果环境里有极窄通道把 RRT 的 p_goal 调到 0.05 左右或者直接用带双向扩展的 RRT-Connect 变体比硬调 RPM 采样密度更划算。6. 调参与踩坑随机种子、角度回绕和邻域半径6.1 步长和目标偏置率的敏感度实验我做过一组很粗糙的敏感度测试固定其他参数只改 step_size0.05 时RRT在2000次迭代内几乎找不到路径0.15 时候规划又快又稳0.3 时碰撞率飙升很多边在细分检查时被拦住。结论是步长应该和障碍物尺寸挂钩而不要迷信某个固定值。目标偏置率 p_goal 0.2 时中段节点明显更密但也更容易被目标点“吸”进死胡同降到 0.08 后探索性更好在复杂环境下成功率反而更高。一个经验是宁可随机性大一点也别让树太早被目标牵着鼻子走。6.2 随机种子毁掉一次仿真的真实经历之前某次调试我在同一个场景下反复跑代码有时候能出路径有时候报错“Reached max iteration”。一开始我以为算法不够健壮后来把随机种子固定下来逐帧跟踪发现所有失败案例都指向同一个问题碰撞检测里用了 0 的边界判定导致某些恰好擦边的线段被判为碰撞而这些擦边位形恰恰是狭窄通道里唯一的通路。把判定改成 0并给障碍物加了安全半径成功率一下子从 60% 拉到了 95% 以上。这件事给我的教训是采样算法本身有随机性单次成功不代表代码没问题单次失败也不代表算法不行一定要跑多次统计再下结论。同时碰撞检测的边界条件必须单独拎出来做单元测试因为它同时是 RRT 和 RPM 的公共地基地基一歪楼全歪。6.3 角度回绕带来的“伪最近点”还有一个很隐蔽但杀伤力极大的坑就是我在第 1.3 节提到的角度周期性问题。如果你偷懒直接用默认的欧氏距离在 θ 边界附近会出现“伪最近点”。比如当前节点是 [179°, 0°]随机采样点是 [-179°, 0°]实际只差 2°却被算成 358°扩展方向完全反了。这种情况在关节型机器人里太常见了因为关节没有“边界”转过一圈就回到原位。所有涉及角度距离的地方包括最近邻搜索、步长方向计算、路径代价累加都必须统一用 wrapToPi 处理后的角度差。6.4 失败模式对照表我把调试中遇到过的典型失败模式整理成了一张表新手可以直接对着排查现象可能原因排查方向迭代到上限始终无路径碰撞检测边界条件过严检查 与 的使用场景路径在边界处剧烈抖动角度回绕未处理全面检索 angle_dist 是否遗漏同一参数每次结果差异巨大随机种子/采样密度问题固定seed做多次重复实验RPM路图连通率低连接半径过小检查孤立点数量适当增大半径规划合法但执行时碰撞未考虑杆厚安全余量障碍物膨胀处理路径平滑后变碰撞贪心直连跳过了中间约束平滑后重新整体碰撞校验最后再分享一点个人体会如果要在真实项目里用这两套算法我一般的做法是先用 RPM 快速评估环境自由空间的连通性了解大概的可行路径在哪里再用带目标偏置的 RRT 或 RRT-Connect 去跑一条真正可执行的路径。这样既避开了 RPM 在窄通道里疯狂堆采样点的成本也让 RRT 的随机性有了大方向参照。还有一个经常被忽略的小细节把碰撞检测模块写成一个独立函数输入两个位形、输出布尔值不要混在任何主循环里。这个函数值得花最多时间打磨、加单元测试因为整个工程里它是被调用次数最多的代码没有之一。我见过太多人把碰撞检测草草写在脚本里最后带着一个隐性 bug 调了两天参数白白消耗热情。先磨好这个地基RRT 和 RPM 的上层逻辑反而都很好落地。