简介这是一份基于C与SSS框架的机械臂路径规划算法实现工程适合机器人算法学习者、C开发人员及机械臂路径规划研究者参考。资源包围绕 Translational_ArticulateRobots_SSSFramework 展开涵盖刚体模拟、障碍物建模、UnionFind 与 Painter 等模块并用迷宫与狭窄通道两个场景的 mp4 录屏直观展示规划效果便于对照源码梳理可通行轨迹的生成逻辑。包体共 58 个文件以 30 个 cpp、14 个 hpp、3 个 h 源码文件为主辅以 plist、pbxproj 等 Xcode 工程配置以及 README 使用说明压缩包整体约 35.71MB目录结构清晰。目前已有 164 人学习下载。读者可获得可直接编译的运行工程、环境配置文件、演示录像与模块化代码对理解机械臂在受限空间内的路径搜索、碰撞检测与框架集成有较大帮助。1. 基于 C/SSS 的机械臂路径规划从采样到路径收敛一条机械臂要在布满矩形障碍物的平面里从起点挪到终点这可能是机器人学里最容易被低估的问题。平移关节机械臂的工作看似只是把几根连杆“推过去”但每根连杆都在扫出体积末端位置又依赖前面所有连杆的位移叠加配置空间一旦超过四维暴力栅格搜索就彻底失效。这个用 C 写的 SSS 框架把问题收敛成三件事对配置空间采样、用碰撞检测筛掉非法点、再借助 UnionFind 把零散采样点连成通路。仓库里自带的狭窄通道和迷宫两个演示场景正好覆盖了采样规划最容易翻车的两类地形。如果你需要一个能跑通、能改参数、能出演示视频的机械臂路径规划算法实现这套代码比从零写 RRT 要省事得多。2. 配置空间建模RodSimulator 与 Box 扫掠体的碰撞判定2.1 平移关节机械臂的配置空间为什么是 2N 维平移关节机械臂Translational Articulate Robot的每根连杆只做平移、不做旋转机械结构上类似多级伸缩臂。假设机械臂有 N 根连杆每个连杆在位形空间里对应一个独立的平移量那么一个完整的配置就是一组 N 个位移向量的叠加。在二维工作空间里每个连杆的平动用两个分量表示所以配置空间的维度是 2N 而不是 N。很多刚接触路径规划的人在这里容易踩坑以为工作空间是二维的配置空间就可以像栅格地图一样直接画出来。实际上只有 N1 时配置空间才等于工作空间一旦连杆数量增加配置空间就是高维流形样本点无法直接可视化。这也解释了为什么这类问题普遍采用基于采样的规划方法而不是 A* 这类确定性搜索。A* 需要把配置空间离散成栅格维度一高内存和计算量按指数增长。采样法只在配置空间里撒点每个点都代表一组完整的连杆平移量再用碰撞检测判断这个点是否合法。撒点数量与维度没有直接关系只要参数设置合理经验上几千个采样点就能覆盖大多数平面机械臂场景。2.2 连杆运动中的扫掠体与外接 Box碰撞检测的第一步是计算每根连杆在采样配置下的空间占用。项目里的 RodSimulator 负责这件事给定一组平移量它逐根计算连杆的位置并生成扫掠体。扫掠体是连杆从上一时刻位置移动到当前位置过程中划过的所有空间区域的并集在平面场景里一根线段平移后扫出的形状是一个凸四边形。代码里通常不会直接用扫掠体做精确碰撞检测因为凸四边形做相交测试本身成本不高但场景里障碍物一多整体复杂度就上去了。常见的做法也是这个仓库里采用的方案先用 Box 对每根连杆和每个障碍物做 AABB 粗略筛选排除掉距离明显过远的组合剩下的候选对再交给 Geo 模块里的函数做精确判定。这里 Box 不仅是障碍物的表示方式也是连杆扫掠体的外接包围盒两层套用能让碰撞检测的耗时降一个数量级。2.3 分离轴定理障碍物与连杆的具体判定精确碰撞判定用的分离轴定理SAT对凸多边形非常高效。两个凸多边形不相交当且仅当存在一条分离轴使得两个多边形在这条轴上的投影区间互不重叠。这条轴一定垂直于某个多边形的某条边所以只需枚举所有边法线做投影比较即可。下面这段代码是 SAT 判定的核心流程项目中 Geo 模块的isCollided函数和它的逻辑基本一致。// 分离轴检测两个凸多边形是否相交 bool satIntersect(const std::vectorVec2 polyA, const std::vectorVec2 polyB) { std::vectorVec2 axes; collectAxes(polyA, axes); // 收集 A 的所有边法线 collectAxes(polyB, axes); // 收集 B 的所有边法线 for (const Vec2 n : axes) { Projection projA project(polyA, n); Projection projB project(polyB, n); if (projA.max projB.min || projB.max projA.min) return false; // 找到一个分离轴说明不相交 } return true; // 所有轴投影都重叠说明相交 }collectAxes收集所有边的法线project负责把一个多边形投影到指定轴上并返回投影区间的最大最小值。只要有一根轴能把两个多边形分开就可以提前返回。SAT 的优点是精度高且没有漏判计算量随顶点数线性增长对于 Box 这类只有四个顶点的障碍物开销几乎可以忽略。碰撞检测的边界情况需要额外处理连杆恰好贴住障碍物边缘时投影区间会出现刚好相等的情况。此时 SAT 返回相交但实际运动中机械臂贴着障碍物滑过去是允许的。常见的处理方式是给连杆的 Box 加一个很小的COLLISION_PADDING外扩量把“恰好接触”判成“未碰撞”留出安全余量。这个值一般在 0.01 到 0.05 之间设置太小起不到缓冲作用设置太大又会把狭窄通道判成不可通过属于需要根据场景尺度反复调的参数。3. SSS 采样与 UnionFind 连通性搜索核心算法剖析3.1 SSS 的“采样—合成—搜索”三步流程SSS 是这套框架自定义的采样规划策略名字里的三个 S 对应 Sample、Synthesize、Search。它和 PRM 的总体思路接近但实现上更强调用局部合成来降低连接阶段的计算量。整体流程分三步采样阶段在配置空间内随机生成大量配置点每个配置点通过碰撞检测筛选合法的留下非法的丢弃。合成阶段对每个合法采样点在其邻域半径内寻找若干邻近点尝试在两点之间生成一条直线路径。这条直线路径同样需要做连续碰撞检测只有整条路径无碰才会在两点之间建立一条边。搜索阶段借助 UnionFind 维护所有节点的连通关系最后从起点配置和终点配置分别查询判断二者是否在同一个连通分量里。合成阶段是 SSS 区别于 RRT 的关键。RRT 是增量式地从起点生长出一棵树每次只扩展一个最近节点SSS 则是先并行撒点再批量连接更适合一次性离线规划任务。而且合成阶段是可以并行的每个节点只负责处理自己邻域内的连接互不依赖。项目里如果没有用多线程把这一阶段的循环体拆给 OpenMP 也能收获明显加速。3.2 UnionFind 如何维护采样点连通性合成阶段产生大量边但这些边只表示“相邻点可达”。要判断起点和终点之间是否存在通路最简单的办法是维护一个并查集。UnionFind 在路径规划里用得不多但在这个场景下非常合适不需要像 BFS 那样遍历整个图每次查询起点和终点是否连通只需要两次find。class UnionFind { public: explicit UnionFind(int n) : parent(n), rank(n, 0) { for (int i 0; i n; i) parent[i] i; } int find(int x) { while (parent[x] ! x) { parent[x] parent[parent[x]]; // 路径压缩 x parent[x]; } return x; } void unite(int a, int b) { int ra find(a), rb find(b); if (ra rb) return; if (rank[ra] rank[rb]) std::swap(ra, rb); parent[rb] ra; if (rank[ra] rank[rb]) rank[ra]; // 按秩合并 } private: std::vectorint parent, rank; };路径压缩让find的均摊复杂度接近常数按秩合并保证树的高度可控。在采样点数量达到数万时这两项优化缺一不可否则最坏情况下会退化成链表遍历。unite只在两点之间的路径通过碰撞检测时调用这就保证了 UnionFind 里任意两个连通节点之间都存在一条无碰路径。搜索阶段的核心逻辑是先让起点和终点加入这个连通结构。具体做法是把起点当作一个新节点尝试连接它邻域内的合法采样点终点同理。连接成功后只要find(startIdx) find(goalIdx)就说明存在一条可行路径。路径的具体节点序列可以在合成阶段额外记录每条边对应节点的父子关系查询时逆序回溯即可。3.3 与 PRM、RRT 的实际对比规划方法节点组织方式连通性维护手段典型瓶颈PRM无向图邻接表 BFS邻域搜索参数敏感图构建开销大RRT树父指针回溯需要调目标偏置概率狭窄通道表现差SSS离散采样集合UnionFind采样密度与连接半径的配合PRM 虽然和 SSS 一样是采样加图搜索但 PRM 构建图之后用 BFS 或 Dijkstra 查询最短路径图的邻接表内存占用和查询耗时都比 UnionFind 高RRT 的树形结构天然适合在线规划但在狭窄通道场景下随机树基本撞不进细长入口。SSS 的优势在于把“图搜索”简化成了“集合合并”建图过程本身就是校准的 —— 合不进去的边直接丢弃最后只需要检查起点终点是否同属一个集合。4. main.cpp 工程落地CONFIG.h 参数与运行流程4.1 CONFIG.h 里值得关注的关键参数整套代码的运行参数集中在 CONFIG.h 里。文件名带后缀.h本质是一组宏定义规划前根据场景调整编译后参数即固定。下面是一份符合最常见设置的参数表和代码仓库中的默认值保持同组量级。// CONFIG.h #define SAMPLE_COUNT 5000 // 采样点数量 #define LINK_COUNT 3 // 机械臂连杆数 #define CONNECTION_RADIUS 0.35f // 合成连接的最大欧氏距离 #define COLLISION_PADDING 0.02f // 碰撞检测外扩余量 #define RANDOM_SEED 42 // 随机数种子 #define OUTPUT_PATH path.txtSAMPLE_COUNT采样点总数决定了对配置空间的覆盖程度。迷宫场景建议提到 8000 以上简单空旷场景 2000 就够。CONNECTION_RADIUS决定每个节点的连接范围。这个值如果小于狭窄通道的宽度即使采样点落在通道里也连不成通路。COLLISION_PADDING如前文所说控制安全余量。RANDOM_SEED固定种子才能保证每次运行结果一致否则调参时无法判断效果变化来自参数还是运气。4.2 main.cpp 的核心流程从采样到输出路径main.cpp 的组织方式和一个标准的离线规划器一致加载场景、初始化机械臂模型、采样、建边、查询、输出。int main() { auto env LoadObstacles(obstacles.txt); // 读取场景障碍物 RobotArm arm(LINK_COUNT); // 初始化连杆参数 auto samples SSS::sample(env, arm, SAMPLE_COUNT); // 配置空间采样 SSS::buildConnections(samples, CONNECTION_RADIUS); // 合成阶段生成候选边 UnionFind uf(samples.size()); for (const auto e : samples.edges()) { if (SSS::edgeFree(e, env, arm)) { // 边级碰撞检测 uf.unite(e.u, e.v); // 无碰边合并 } } auto path SSS::query(samples, uf, start, goal); // 判断连通并回溯路径 WritePath(path, OUTPUT_PATH); // 写出 path 文件 return 0; }SSS::sample返回的samples里包含采样配置点和它们之间的候选边。buildConnections只生成边不做碰撞判定真正的碰撞检测在循环里统一执行这样做的目的是把碰撞判定集中到一处便于统计耗时和调试。SSS::query的作用是尝试把起点和终点接入 UnionFind 的连通分量如果接入成功就从内部记录的父子关系回溯出完整的路径节点序列最后写入path.txt。4.3 编译与运行演示场景仓库附带的是 Xcode 工程方便在 macOS 直接打开运行。做批量实验时我一般改用命令行编译效率更高g -O2 -stdc17 main.cpp Geo.cpp Box.cpp RodSimulator.cpp SSS.cpp UnionFind.cpp -o sss_planner ./sss_planner ./scenes/maze.txt-O2优化对碰撞检测循环影响显著不建议去掉-stdc17是为了使用较新的标准库特性。程序运行结束后path.txt里每行记录一个配置即每根连杆的平移量可以用仓库里的回放脚本逐帧画出机械臂的移动过程输出成视频。狭窄通道和迷宫两个演示场景都是这么生成的先用narrow_channel.txt跑出路径再按帧渲染机械臂位姿最终合成 mp4。运行过程中如果发现程序卡住或路径为空优先怀疑CONNECTION_RADIUS和SAMPLE_COUNT这两个参数。先打印采样点的合法数量如果合法采样点数量远小于SAMPLE_COUNT说明碰撞检测过严或采样空间太小如果合法点足够但还是连不通问题大概率出在连接半径上。5. 在狭窄通道和迷宫场景中调参验证与进阶技巧5.1 通道宽度与采样密度如何联动狭窄通道场景里机械臂必须穿过一条明显窄于活动空间的通道。采样落在通道内的概率正比于通道面积占整个配置空间的比例这个比例一旦低于 5%纯随机采样基本不可能连出通路。我会把SAMPLE_COUNT提高到一万以上并适度缩小CONNECTION_RADIUS。缩小半径看似会让连接变难但通道内采样点密集后小半径反而可以避免出现跨越障碍物的“假边” —— 这是使用大半径时最常见的错误两点直线距离很近但实际线段在某个中间位置穿过了障碍物边界。迷宫场景则相反。迷宫的特点是通道多且长但单段通道本身并不窄。这时把CONNECTION_RADIUS调大一些能快速连接远处的采样点减少通道内孤立节点数量规划成功率提升明显。一个经验法是让连接半径略大于通道宽度的 1.5 倍既能覆盖通道内部又不容易让边越过障碍物。参数组合可以这样设置场景类型SAMPLE_COUNTCONNECTION_RADIUSCOLLISION_PADDING空旷场景2000–30000.5–0.60.02狭窄通道8000–120000.15–0.250.01–0.02迷宫5000–80000.4–0.50.02–0.03COLLISION_PADDING在狭窄通道里要调小因为通道本身的空隙已经很紧凑外扩量稍大就会把合法区域全部吞掉。代价是机械臂容易贴着障碍物走实际应用中需要再叠加一个距离保持层的控制项。5.2 验证路径的有效性与质量运行结束后不能只看有没有路径输出还要对路径做一次独立验证。下面的函数回放整条路径逐段检查边级碰撞并统计总长度用于评估路径质量。// 路径验证返回路径总长度存在碰撞返回 NaN float evaluatePath(const std::vectorConfig path, const Env env, const RobotArm arm) { float len 0.0f; for (size_t i 1; i path.size(); i) { if (!SSS::edgeFree(path[i-1], path[i], env, arm)) return std::numeric_limitsfloat::quiet_NaN(); len dist(path[i-1], path[i]); } return len; }路径长度可以作为调参的第二指标同样的场景下更短的路径通常意味着连接边更直冗余回绕少。如果发现路径总长度偶尔出现异常大的值多半是某条边在合成阶段被错误连接优先提高edgeFree的采样分辨率 —— 也就是把直线路径离散成更多小段逐段做碰撞检测。5.3 随机种子与重复实验的工程细节调参时必须固定RANDOM_SEED。同样的参数配合不同的种子成功率波动有时能达到 30% 以上这就是采样规划的随机性。正确的做法是固定种子做参数对比选出最优组合后再换 5 到 10 个种子跑统计成功率。另外main.cpp里采样阶段输出的合法节点数量是个很关键的中间指标运行时打印出来能快速确认是采样不足还是连接半径的问题。把这一行日志默认打开比以后反复加打印语句省事得多。本文还有配套的精品资源点击获取