1. 项目概述与场景定位写空间机械臂双臂碰撞检测得先明白这个问题的来源和分量。单臂机械臂只需要考虑自身与环境之间的避碰到了双臂系统两个机械臂共享同一片工作空间彼此的构型、速度、负载状态还会互相影响——这时候碰撞风险就不再是简单的几何判断而是要在毫秒级的时间窗口内回答三个问题两条臂现在离得多近未来一段时间内会不会撞上如果会该怎么躲在我们做过的模拟项目X中这个问题被进一步放大。任务要求两条七自由度机械臂在同一操作台面上协同完成部件装配其中一个臂负责抓取和移动另一个臂负责定位和固定两条臂的有效工作空间重叠率超过七成。更棘手的是其中一条臂需要在另一条臂的腋下穿越进行供料这种高冗余构型下传统的主从规划策略经常出现不可预期的手臂贴脸。我刚开始接手这个模块时第一反应是抄一套成熟方案调研了一圈发现市场上的开源碰撞检测库主要面向单臂或者移动机器人场景直接拿到双臂高动态任务里要么是精度不够要么是漏检率让人不敢上线。这个项目最终让我们从零搭了一套融合几何模型、运动学约束和实时采样的碰撞检测框架整条链路跑下来的经验我觉得值得展开说说。再说得直白一点这篇文章适合谁看。如果你正在做双臂协作场景下的路径规划或者要在一个实时性要求高的机器人系统里嵌入碰撞检测模块又或者你只是好奇机械臂之间到底怎么避免打架那接下来的内容应该能给你一些参考。我会从设计思路、算法选型、工程实现到坑点排查把一个真实的双臂碰撞检测系统拆开来讲其中有代码级别的细节也有纯经验性质的判断。2. 核心设计思路与方案选型2.1 碰撞检测需求的实际边界先把需求边界画出图形。我们面对的是两条七自由度机械臂每个臂由基座、七个旋转关节、末端工具组成。碰撞源有两种一是双臂之间的互相碰撞这是主角二是臂与环境操作台、支架、其他固定设备的碰撞这个在项目里相对容易因为环境模型静态一次性建好就行。所以算法架构上双臂自碰撞检测要占到百分之八十的实时计算资源。实时性指标是我们定的硬杠杠单次检测周期不超过2毫秒整个规划周期内每100毫秒至少要完成一次完整的碰撞扫描。这个指标不是拍脑袋定的而是从任务需求倒推出来的。两条臂在对接装配时末端相对运动速度最高达到每秒0.5米如果碰撞检测延迟超过10毫秒理论上的最小安全距离就得放大好几厘米这对操作精度是有影响的。我们最终选型时将所有候选方案先过了一遍这个时间预算能达标才继续深入。还有一个重要约束是算法要稳定可解释。在航天这类高安全等级场景里算法不应该是黑盒——你需要能说清楚当前这条臂上哪一个连杆和另一条臂的哪个连杆距离最近以及这个距离是怎么算出来的。这个需求直接影响了我们后续对近似模型和精确模型的选择策略。2.2 为什么放弃纯包围盒方案最早我确实想过直接用成熟的包围盒库解决。AABB轴对齐包围盒和OBB有向包围盒的检测速度极快几百对几何体的碰撞检测在微秒级就能完成。但用起来就会发现一个问题机械臂不是一堆独立的刚体它是一个运动链每个连杆的运动都由前序关节决定。如果只是对每个连杆单独建包围盒然后做两两检测检测结果会非常保守——两条臂明明没有接触但包围盒已经互相穿透了因为包围盒无法精确贴合机械臂的细长外形和弯曲姿态。你可以想一个最简单的例子一条臂的末端手臂是个细长的圆柱体另一条臂的前臂要从它旁边不到一厘米的地方经过。如果给这个圆柱体包一个保守的OBB那OBB的宽度就已经超过了两厘米在狭小工况下这个假阳性会频繁触发避让路径重启任务根本跑不顺。当然也有更精细的包围盒树方案就是在每个连杆上构建多层次的OBB树接近检测时自上而下遍历来做剪枝。这个方案精度提升明显但实现复杂度和调试成本同步上升而且它本质上解决的还是静态几何的查询问题对运动链之间的动态约束关系利用得不够直接。我们最后采用的是一个分层混合方案这里先透露整体框架细节在后面展开第一层快速剔除层。基于关节角的粗略运动学边界把空间上明显不可能相遇的臂段组合直接剔除。第二层近似检测层。对剩下可能有交集的臂段用胶囊体模型加球体扫掠做快速距离估算。第三层精确检测层。对高速接近或者距离已经低于安全阈值的臂段启用网格级精确距离计算同时触发避让策略。这个方案的核心思想是分级响应不是每个周期都对所有臂段做最精细的计算而是用越来越贵的检测手段逐步聚焦到真正有风险的对象上。实时性因此有了保障。3. 核心算法拆解与实现路径3.1 机械臂几何模型的简化与等效碰撞检测的精度上限其实在建模阶段就决定了。对机械臂每个连杆都需要在其运动链上建立一个用于检测的等效几何体。等效模型的误差越大后面所有检测算法的意义都会被稀释。我们在项目里做了这样的处理每个连杆用一个或两个胶囊体来等效胶囊体的半径和长度根据连杆实际外包络形状保守取值关节部分用一个稍大半径的球体来等效。之所以选胶囊体和球体是因为这两种几何体之间的距离计算有解析解速度极快而且可以通过调整半径来补偿模型误差。这里有一个实用经验给胶囊体定半径时不要只量连杆本身的外径要把表面凸起、线缆绑扎带、传感器支架统统算进去。我们一开始就吃了这个亏第一版建好的模型在仿真里完全不碰撞一到实物联调就频频触发报警后来逐段核对才发现某一段肘关节附近的线缆固定环多出了将近8毫米精确模型里的最小距离只有6毫米报警阈值是10毫米自然天天报警。模型建好之后要验证一个关键指标模型误差必须小于安全距离余量的一半。我们的安全阈值是30毫米那么几何模型的误差就不能超过15毫米。用这个指标反推胶囊体方案完全够用连网格模型都不一定需要。3.2 快速剔除层让大部分计算根本不发生说一个我后来屡次验证的经验高效碰撞检测的关键不是让单次检测更快而是让绝大多数检测请求不发生。在我们的双七自由度机械臂场景下不考虑末端工具时两条臂各有7个连杆两两组合最多49对。如果每条臂还有末端工具要多加几对。49对逐对做胶囊体距离计算开销并不大但整个规划周期需要反复调用累积起来就会吃掉大量时间预算。快速剔除层做的事情就是把这个49对的规模压到个位数。原理是根据关节角范围和机械臂各连杆的最大伸展半径预计算一个整体包络球。两条臂的包络球如果圆心距离大于半径之和直接判定无碰撞风险——这个判断只有一次球心距离计算成本几乎可以忽略。更进一步每个连杆也可以预设最大伸展半径通过前向运动学逐级计算每个连杆在世界坐标系下的实际包络再用空间哈希或者简单坐标排序来剔除绝对不可能相交的臂段对。比如当两条臂都在各自工作空间的一侧一个在左一个在右时左手腕和右肘之间的距离可能超过两米而两条臂长的和都不超过这个值的三分之二这时候这一对其他都没有检测必要。3.3 胶囊体距离计算与连续碰撞检测通过快速剔除层留下来的臂段对我们进入近似检测层。这一层要做的是给出一个尽可能精确的距离值同时保证延迟极低。两个胶囊体之间的最短距离可以分解为两条线段之间的最短距离加上两个半径之和的修正。为什么不直接说胶囊体中心线距离减去两个半径因为胶囊体有端部的半球如果两段中心线异面距离小于各自半径之和真正的表面距离肯定不等于中心线距离减半径和需要计算端点处的贡献。我们在实际代码实现里用的是标准几何库但它返回的是线段到线段距离。如果你直接用这个结果减去半径和会得到一个偏小的距离相当于人为扩大了碰撞风险在某些阈值设置下会频繁误报。正确做法是先用线段距离判定两个胶囊体是否相交如果相交则直接返回零距离如果没相交再计算精确的曲面间最小距离这一步涉及对四个特征点两个线段端点对在对方线段上的投影做遍历取最小。这段逻辑很容易被忽略但直接决定了检测的准确性。我调试过程中花了不少时间在这个细节上最终确信它值得单独拿出来讲。连续碰撞检测是另一个关键点。如果你只是在每个控制周期做一次离散检测两个快速运动的物体可能在一个周期内互相穿透而且没有被发现。对于双臂协同这种场景必须考虑时间维度的碰撞风险。我们的做法是在前一个检测周期记录每根臂上关键标志点的位置和速度在当前周期预测下一周期它们可能出现的位置范围再和当前周期其他臂段的位置做扫描测试也就是在时间轴上做一次扫掠体相交判断。扫掠体检测的性能消耗比静态距离计算要高一个量级所以只有在近似层判定距离小于一个阈值我们设为80毫米时才启用避免每个控制周期都做这种重量级计算。这个阈值的设置逻辑是考虑到最高相对运动速度和检测周期一个周期内最大位移大约30毫米80毫米的安全扫描边界足够覆盖两个连续周期不会漏掉快速接近的工况。3.4 精确检测层局部网格级验证当两条臂段之间的距离已经低到接近安全阈值的时候近似模型的误差就不再可忽略此时需要启用精确检测。精确检测的做法是对涉及的局部曲面生成细粒度网格然后做网格间的最短距离计算。注意是局部网格不是对整个机械臂重新建模。因为我们已经知道是哪两段之间距离最近只需要把这两段的局部区域离散成网格就够了计算规模很小通常几千个三角形就能满足精度要求。网格间距的选择有讲究。三角形边长设成5毫米时距离计算误差大概在2到3毫米量级这个误差对30毫米的安全阈值来说还能接受。如果边长缩小到2毫米误差能控制在1毫米内但计算耗时成倍增加。从工程角度看先用5毫米三角形快速出结果如果距离结果介于安全阈值和预警阈值之间再用2毫米的三角形精算一次这样在绝大部分工况下都只需要跑一次粗网格。精确检测的结果不只是用来判断碰不碰还用来驱动避让策略。我们会计算碰撞趋势向量就是当前相对运动方向上距离在单位时间内减少的速率。这个速率超过某个限值时即使当前距离还没到安全阈值也会提前触发减速或者改道逻辑而不需要等真正逼近之后再处理。3.5 碰撞响应的分层策略检测出有碰撞风险之后怎么处理是另一道大题。项目里我们把响应策略分成三个等级一级预警距离小于80毫米但大于安全阈值30毫米。此时系统不干预当前运动只记录距离趋势和风险日志同时规划器开始搜索备选路径。这是看着不管阶段。二级干预距离进入30毫米阈值或碰撞趋势速率超标。此时系统对相关关节施加速度限制受影响臂段的最大关节速度降为正常值的50%。如果条件允许优先让负载较轻的臂做微量退让保持任务连续性。三级急停距离跌破15毫米或检测到任何网格穿透。此时双系统立即锁定所有关节进入安全保持模式。急停发生后需要人工确认并重新规划才能恢复。这个分层不是我们拍脑袋定的是从任务安全分析和末端负载能力推出来的。机械臂在正常操作中的最大速度、惯量和关节力矩决定了一个客观存在的安全反应距离。急停响应时间大约需要几十毫秒考虑到系统延迟和制动距离从触发急停到完全静止末端可能还会移动20到30毫米所以15毫米的急停阈值必须保证在触发之前没有任何真实接触风险。留出两倍余量是底线。4. 实操过程与关键环节实现4.1 初始版本的系统结构和模块划分动手写代码之前我们先把整个碰撞检测子系统划分成四个模块运动模型模块负责前向运动学运算输入关节角向量输出每个连杆标志点的位置和姿态。碰撞查询模块实现快速剔除、近似检测、精确检测三个层级的完整链路提供统一的查询接口。风险评估模块基于距离结果和运动趋势计算风险等级输出预警或干预指令。日志与可视化模块每100毫秒记录一次检测结果和关键特征数据同时将距离最短的臂段组合标注在仿真场景中。模块划分的原则是查询链路上层不信赖下层内部状态。碰撞查询模块对上层提供的结果是一个结构体里面包含距离、最近点坐标、涉及的连杆编号、检测置信度上层不需要知道这个结果是从哪一层算出来的。这样后续优化检测算法时不会影响策略模块。4.2 关键数据结构和接口设计这里直接给出我们最终使用的核心数据结构设计工程上可以直接参考struct CollisionQueryResult { double distance; // 最短距离单位米 double safety_margin; // 当前距离相对安全阈值的余量 int link_index_a; // 臂A的连杆索引 int link_index_b; // 臂B的连杆索引 Vec3d closest_point_a; // 臂A上最近点坐标 Vec3d closest_point_b; // 臂B上最近点坐标 double approach_speed; // 当前相对接近速度正值为靠近 int detection_level; // 1快速剔除2近似检测3精确检测 bool is_penetrating; // 是否发生穿透 };查询接口长这样CollisionQueryResult checkArmPairCollision( const ArmState arm_a, const ArmState arm_b, const SafetyConfig config );在这个接口内部系统会先执行快速剔除判断如果免检则直接返回一个distance为极大值的结果。这个设计的好处是上层策略代码非常简洁——拿到结果后只看is_penetrating和distance不需要关心内部走了哪条分支。4.3 前向运动学与标志点计算碰撞检测的几何计算必须基于当前真实关节角所以前向运动学是每次查询都绕不开的环节。我们用标准的DH参数建模但额外做了一个优化将前向运动学的计算结果缓存下来只有当检测到对应关节角发生变化时才重新计算。这在低速高精度任务里收益很大因为是两个控制器交替输出关节角指令每个控制周期内很多臂段的位置可能并未变化。如果不做缓存每一轮查询都要从头做一遍完整的矩阵链乘法白白浪费好几十微秒。另一件事是标志点的选取。除了每个连杆的起点和终点我们还在连杆中间增加两个特征点用于快速几何剔除。这些特征点并不参与深度计算但它们的包络位置可以在剔除阶段提供一个更紧密的边界。特征点数量越多剔除精度越高但计算量也会增加实际调试下来每根臂取7个特征点每个连杆一个即可达到满意的剔除率。4.4 胶囊体模型的参数标定与验证这一步说直白点就是量尺寸。我们在实物上逐段测量了每个连杆的有效外径和长度同时考虑关节运动时连杆之间的相互遮挡将每个胶囊体的长度比实际尺寸缩短了大约5%——为什么缩短因为关节处的球体模型已经有半径了如果胶囊体全长再按完整连杆长度设置关节附近两个模型会叠加等效半径偏大剔除层就会变得保守。标定完成之后要做一遍全构型扫描验证。具体做法是让两条臂在仿真中遍历各自关节角的上下限每隔5度采样一组姿态组合用我们的检测工具和另一套独立的高精度网格模型对比结果。网格模型的三角形边长设为1毫米作为准真值来评估检测误差。全扫描样本量大概有几十万组跑了一整天最终确认我们的近似检测层在最坏情况下的误差不超过8毫米完全满足15毫米的模型误差上限。4.5 仿真验证与单点故障注入测试系统跑通之后需要在仿真中做压力测试。我们在仿真场景里设置了一系列高风险工况两臂末端互相交叉、一臂在另一臂正上方小间隙悬停、两臂同步向共同目标点靠拢。每个工况都要跑至少10次统计报警准确率、漏报率和误报率。结果漏报率是零但误报率比预期的要高大约在4%。分析日志后发现误报主要集中在快速剔除层问题出在特征点包络半径设置过大。某些末端工具的姿态比较特殊特征点包络半径无法覆盖工具尖端导致明明还有空间余量却判定有碰撞风险。修正方案是把末端工具单独作为一个碰撞体参与检测同时在特征点包络计算中引入末端姿态信息修正之后误报率降到了0.5%以下。还需要做的是单点故障注入测试。具体做法是在连续运行过程中随机替换某个关节角为异常值或者突然增加一个外部扰动模型观察碰撞检测系统能否在下个周期正确地捕获异常状态并及时报警。这类测试的价值在于验证检测系统本身的鲁棒性而不是只验证正常工况下的精度。5. 常见问题与排查技巧实录5.1 问题一频繁误报导致任务中断这是调试中最常见的故障现象是机械臂明明距离安全阈值还有很大余量系统却频繁触发二级干预甚至急停。排查思路分三步。第一步检查几何模型误差把每个连杆的胶囊体半径和实际模型对比看看是不是哪段设置得太保守。第二步检查阈值配置是否存在单位不一致某个参数写成厘米而不是米直接让安全阈值放大了10倍。第三步检查剔除层是否有逻辑漏洞比如某两个臂段因为姿态原因其实已经解析出很近的距离但剔除层以为它们很远就会在之后的更粗计算里出错。我们踩过的坑是第二种早期版本里安全阈值用了厘米单位而检测结果用的是米两相对比差了100倍系统一度认为所有臂段都在安全距离以内。这类问题最好在配置加载时加一个合法性校验阈值必须落在合理区间并且对单位和数量级做显式标注。5.2 问题二快速接近工况下的漏检漏检是最危险的问题一旦出现必须立查。我们曾在一个特殊工况下复现过漏检两条臂以较高相对速度同步靠近同一目标点动态扫掠体检测理论上应该覆盖这个场景但实际测试中发现部分样本漏报。排查下来发现根因在扫掠体检测的步长设置。我们原本设定检测周期为固定100毫秒但两条臂在这个工况下的相对速度较高一个周期内末端位移接近40毫米超过扫掠体的扩展范围。修正方法是把扫掠体的扩展量改成动态值——根据当前相对速度动态计算下一周期最大可能位移再放30%的安全余量作为扫描边界。这个问题的教训是碰撞检测里的所有固定参数都必须回到物理场景里验算。你觉得100毫秒周期没问题但任务载荷一变可能就出问题了。把关键参数设计成自适应或者至少做最大值约束是最稳妥的做法。5.3 问题三检测结果抖动影响策略稳定性还有一类问题是检测结果本身准确但不同周期之间的距离值抖动明显。距离值在29毫米和35毫米之间来回跳导致策略层一会儿进入二级干预一会儿又取消系统动作如抽搐一般。抖动来源有两个可能。一个是几何模型在多构型下的准确性波动某些姿态下距离计算路径有歧义。另一个是最近点标识不稳定同一对臂段在相邻周期内最近点可能从肘部跳到腕部距离值本身没大变化但策略层如果信任最近点的连续轨迹就会受干扰。我们最终的解决办法是为距离结果加了一个低通滤波同时对最近点标识做了驻留判定——只有当相邻周期的最近点落在同一连杆区域时才认为距离变化趋势有效。这个方案一上线策略稳定性提升非常明显。5.4 问题四仿真与实物一致性差仿真里怎么跑都一切正常一到实物联调就报警不断这类问题在机器人项目里反复出现。核心原因通常是仿真模型和实物的几何参数存在偏差或者实物上某个传感器支架、线缆走位比仿真模型多出了一块。我们在联调阶段专门做了一次实物点云对齐验证用激光扫描设备将两条臂和操作台的实物点云扫描出来在软件里将扫描点云与几何模型做对齐逐段计算偏差。结果显示偏差最大的地方是腕关节附近的线缆固定环长出了12毫米这是仿真模型里完全没体现的。将胶囊体模型对应的部位加粗后报警频率立刻恢复正常。这里想特别提醒如果项目周期允许尽早做一次实测点云和设计模型的偏差评估。别等到联调的最后一刻才暴露问题那会儿所有模块都挤在一起排查成本是几何级增加的。5.5 问题五检测性能退化导致控制周期超时系统运行时间越长检测耗时会逐渐增加这是个典型的性能退化问题。表现是最初每轮检测耗时1.2毫秒运行半小时后逐渐涨到2毫秒以上严重影响主控制周期。我们定位到的原因是内存缓存的无界增长。标志点缓存模块在设计初期没有加容量上限系统长时间运行后缓存了海量关节角对应的历史标志点位置每次查询都要遍历一大圈找匹配项。把所有缓存改成绑定近期时间窗口的LRU结构后耗时重新回到1毫秒附近。这算是一个很典型的工程疏漏但也说明碰撞检测模块在实际系统里不是孤立运行的它和整个软件架构的资源管理能力密切相关。6. 个人经验与扩展方向前面说了不少技术细节最后夹带点个人体会。在这个项目里我学到的最重要一件事是碰撞检测算法不能孤立地追求精度必须放在整体系统里看它的角色。你算法再准如果控制策略和规划器不能及时消费它的输出系统依然会出问题。我们从一开始就把检测、策略、规划三条链路放在同一个仿真环境里联调这才保证最后联调阶段没有大规模返工。另一个实践心得是参数配置管理的问题。碰撞检测涉及的安全阈值、预警阈值、模型参数等关键参数最好集中在一个配置文件里管理并且每个参数都带上详细注释和使用场景说明。项目里的人来来去去如果没有清晰的参数文档新接手的人踩坑概率极高。我们在项目后期专门花了一天把所有参数整理成一页速查表这天的投资回报率特别高。如果你接下来也要做类似的双臂碰撞检测系统我建议从三个方向思考扩展。第一是把检测结果和路径规划器做更紧的耦合让每次避让动作不只是临时响应而是改变规划的代价图这样路径质量会好很多。第二是引入机器学习方法做预测性碰撞风险判断针对高频出现的场景训练一个快速风险分类器可以进一步降低高精度检测的调用率——这个方向已经有不错的论文了值得关注。第三是考虑多臂系统三条以上的臂协同工作场景碰撞检测的对象对数量会爆炸式增长当前的快速剔除层设计需要重新思考。最后再说一个我一直在用的习惯每次跑完一轮大规模仿真测试一定留一版完整的日志数据。这些数据是优化下一版算法最重要的依据很多看起来好像没问题的边缘工况其实都藏在日志里等着你去发现。做碰撞检测这个方向永远不要嫌手里的数据多。