空间机械臂双臂碰撞检测算法，这个话题一看就是给在轨服务、空间装配这类任务做铺垫的。单臂操作时碰撞问题相对可控，一旦上双臂，两条臂协同抓取、拆装、对接，工作空间高度重叠，碰撞检测就不是“加一个判断函数”那么简单了。我的实际经验是，这东西本质上是一个实时几何求交问题，嵌在运动学、动力学和路径规划之间，既要算得快，又要算得准，还得能处理柔性关节和热变形带来的不确定性。
这篇文章就围绕双臂碰撞检测的完整技术链路展开，从碰撞模型怎么建、检测算法怎么选，到工程实现时怎么分配算力、怎么处理误报漏报，尽量把我在实际项目里踩过的坑和验证过的方案都讲清楚。适合正在做空间机械臂控制、或者做双臂机器人仿真避碰的开发者参考。
1. 为什么说双臂碰撞检测是个“三维空间里的高速判交”问题
1.1 碰撞检测的本质：在一个高维空间里做实时求交
很多人第一反应是，碰撞检测不就是判断两个物体有没有重叠吗？用包围盒一包，每帧算一下相交就行。这话在桌面级工业机器人上勉强成立，但放到空间机械臂双臂场景里就远远不够了。
空间机械臂的特点是：关节多，自由度普遍在7自由度以上，双臂就是14个甚至更多自由度；杆件长，有的臂展超过十米；载荷大，末端要抓好几吨的设备。两条臂在空中协同作业时，工作空间绝大部分都是重叠的。这意味着不只是臂杆和臂杆可能碰，臂杆和载荷可能碰，载荷和载荷可能碰，甚至连关节罩、线缆支架、相机支架这些“非结构件”都可能成为碰撞点。
从数学上看，每个时刻每条臂的构型就是一组关节角向量，双臂联合构型则是几十维空间里的一个点。碰撞检测要回答的问题是：这个构型下，臂A的哪一段和臂B的哪一段距离最近，距离值是多少，是否低于安全阈值。如果把它看成一个连续问题，那就是两族三维线段/圆柱体在关节角变化过程中的最小距离追踪。所以核心不是一个简单的“是否相交”布尔判断，而是一个几何求交算法，需要对几十对图元做距离计算。
1.2 空间环境和地面环境的核心差异：微重力、刚柔耦合、延时
在地面做双臂碰撞检测，底座是固定在地基上的，运动学模型相对干净。空间站上的机械臂底座是浮动基座，两条臂装在舱体和桁架上，而舱体本身没有固定支撑，微重力下整个系统处于自由漂浮状态。操作时机械臂动作产生的反作用力会推动基座漂移，基座一漂移，所有臂杆的绝对位置又都变了。这就是自由漂浮空间机器人的动力学耦合问题。
这种耦合带来的麻烦是：碰撞检测必须使用当前时刻的真实基座位姿，而不能只用名义上的安装位置。如果按地面习惯，把两条臂的基座当成固定约束来算，不用多久就会出现明显的预报误差。我在某次地面仿真验证时，对比固定基座模型和浮动基座模型下的最小距离曲线，在同样的关节运动轨迹下，最大偏差能到几十毫米量级。而机械臂末端的定位精度本身追求的就是毫米级，这个偏差足以造成碰撞漏报。
更麻烦的是空间机械臂普遍存在柔性。长臂杆的模态频率很低，动作时臂杆会产生弹性振动，名义位置和实际位置之间有偏差，而且这个偏差是动态的，难以用确定性模型完全覆盖。碰撞检测系统必须有冗余策略——几何计算出的距离值只作为参考，还要叠加安全裕度，并根据实时传感器信息修正。
另一个被低估的问题是延时。空间站上机械臂和载荷相机、测距传感器之间的数据链路由遥测通道组成，信号从采集到进入控制器再回来，总延时往往有几百毫秒。碰撞检测本质上用的是上一时刻的构型，检测到碰撞点的时候，真实位置可能已经往前走了一段。这决定了算法不能只做瞬时检测，最好能做连续检测或者速度预测补偿，否则高速挥臂时很容易“检测到了但已经晚了”。
1.3 双臂避碰的三个层次：互碰、自碰、器碰
设计双臂碰撞检测逻辑前，先要明确防护对象。碰撞风险其实分三层：第一层是两臂之间的互碰，也就是臂A的任意杆件对臂B的任意杆件；第二层是每条臂自身的自碰，比如某个关节转到极限后，前臂和上臂罩子挨在一起；第三层是机械臂和周围环境、舱体、载荷设施之间的碰撞，这在舱外任务里尤其重要，机械臂带着载荷大范围回转时，很容易扫到舱壁太阳翼之类的结构。
有人觉得自碰好处理，反正单臂运动学自己做一下关节限位就行。但这个思路在自由度高的机械臂上不够可靠。7自由度机械臂的自碰点远不止相邻关节的杆件，非相邻杆件在有些姿态下也会靠得很近。我做过一次高维构型空间的搜索统计，在随机采样的大量构型中，非相邻杆件的最小距离低于安全阈值的比例并不低，某些特定姿态下肘部关节会和基座侧壁贴近，单靠关节限位根本防不住。
器碰检测则需要环境模型。空间站舱外结构、对接机构、天线等设备都是有包络模型的，但这些模型往往来自设计图纸，在轨后会有微小的形变和安装偏差。碰撞检测系统里如果直接使用名义模型而不做标定修正，就会出现一个尴尬的情况：仿真里显示距离还有几十毫米，实际上已经刮上了。我在实际项目中养成了一个习惯：环境模型一定要加外扩偏置，偏置量至少覆盖安装误差和热变形量。
2. 碰撞模型的建立：从几何包络到运动学同步
2.1 杆件包络是关键：球体、胶囊体和OBB的选择
碰撞检测的精度和速度，很大程度上取决于用什么形状去包络机械臂的杆件。常用的包络有三类：球体包络、胶囊体包络、方向包围盒（OBB）包络。
球体最简单，每个杆件用一串球覆盖。计算两杆间距时，就是计算两组球之间的最近距离，而两球最近距离就是球心距减半径，这个计算量很小。缺点是包络效率低，细长杆件要用很多球才能贴紧，否则冗余空间巨大。空间机械臂杆件又长又细，纯球体包络会导致严重的“虚拟碰撞”——模型显示碰了，实际还差得远，逼得操作员放大安全阈值，结果反而降低了真实防碰能力。
胶囊体是球体加圆柱的组合，数学上就是两个端点加一个半径。胶囊体之间的最近距离计算，等价于空间两条线段之间的最短距离再减去两个半径之和，计算量适中，包络贴合度远好于球串。我实际项目里首选胶囊体方案来包络臂杆，因为空间机械臂的主臂杆形状相对规则，用一到两个胶囊就能包得不错。
OBB包络贴合度高，但相交测试计算量明显更大，而且当杆件形状不规则（比如带有关节驱动器和线缆支架）时，OBB的轴方向选取很讲究，选不好包络就松了。我更推荐混合方案：主臂杆用胶囊体，关节罩和末端执行器用多球簇，不规则附件用OBB，最后统一归到一个碰撞树里。这样既保证了计算效率，又能照顾到那些形状古怪的部件。
2.2 碰撞树的组织方式：为什么说层次包围盒树是标准解法
机械臂是一个链式结构，天然适合用树来组织图元。每条臂按运动学链建立一棵碰撞树：根节点对应基座，往下是各关节的罩体、臂杆、末端工具。检测双臂碰撞时，不是把所有图元拉平做两两相交测试，而是从两棵树的根节点开始，做层次化的遍历。
层次包围盒树的基本思路是：每个节点存有该节点下所有子图元的包围盒，先在粗粒度上判断两棵树的节点包围盒是否相交，如果不相交，就整棵子树剪枝；如果相交，才继续下探到子节点。这样大部分构型下，需要精确计算的图元对非常少。
我常用的是AABB（轴对齐包围盒）做树的粗粒度节点，因为相交测试极其简洁，就是三个轴向上判断是否重叠。叶子节点放胶囊体、球体或OBB。整个双臂场景下，两棵树每帧做一次同步遍历，正常姿态下单帧求交的图元对数量往往不超过二十对，计算量完全可控。
2.3 运动学同步：坐标系和时戳是碰撞检测的命门
碰撞检测必须基于统一坐标系下的真实位置。这里最容易被忽略的是坐标系时戳问题。两条臂的控制周期可能不同步，有的关节控制器以200Hz反馈状态，有的以100Hz反馈，而且遥测通道还可能抖动。如果直接把两组带有不同时戳的状态拿来算距离，就会把时间上不对齐的两个构型强行合成到一起。一旦某条臂正在快速运动，这个合成结果会出现很大的位置偏差，造成误报或漏报。
我的处理方式是在碰撞检测模块内部维护一个状态缓存池，记录最近若干周期内每条臂的关节角和末端位姿及对应的时戳。做碰撞检测前，先做时间对齐，以当前检测周期为基准，对各臂状态做线性插值或外推，统一到同一时刻后再送入几何求交器。
这个细节看起来不起眼，但我见过不止一个项目因忽略时戳对齐而在一致性测试中被判为不通过。只要系统里各传感器节点的时间基准不一致，碰撞检测的可靠性就无从谈起。所以设计阶段就要把时间同步方案定下来，常见的做法是采用分布式时钟同步协议，在全链路打上统一时间戳。
2.4 安全阈值的设定：一个需要综合考虑的工程决策
安全阈值设多大，直接影响任务效率和安全性。阈值设大了，机械臂一动就报警，操作员只能减速慢行，任务效率极低；设小了，真碰上了系统来不及响应。这个问题没有标准答案，只能根据系统特性来标定。
我一般把安全距离阈值分解成三部分：名义规划安全距离、控制不确定度、机构包络误差。名义规划安全距离由轨迹规划器保证，一般给到100到150毫米；控制不确定度来自关节跟踪误差和基座漂移误差，需要根据历史遥测数据统计得出，比如末端最大跟踪偏差30毫米；机构包络误差是模型化偏差，一般按20到30毫米估。
所以碰撞检测触发阈值就不是一个单一值，而是分级阈值。当最小距离低于第一级阈值（比如200毫米）时，系统进入警戒状态，记录并提示操作员；低于第二级阈值（比如100毫米）时，触发减速；低于第三级阈值（比如50毫米）时，紧急停止。实测下来，这种分级策略比单一阈值好用得多，既给了控制器反应时间，又不会因为瞬时抖动就频繁停机。
3. 核心检测算法拆解：图元求交到连续检测
3.1 两两图元的解析求交公式
碰撞检测落到最底层，就是若干组图元的距离求解。空间机械臂场景中主要涉及的图元对是：球-球、球-胶囊、胶囊-胶囊、球-平面、胶囊-平面。
球-球的最近距离最简单，直接算球心距减半径和。球-胶囊等价于球心到线段的最短距离再减半径。胶囊-胶囊则分两个阶段：先求两条中心线段之间的最短距离和最近点，再减去两个胶囊半径之和。空间两线段距离计算有现成的解析公式，核心是构造以参数u和v表示的线段点，求二元二次函数的最小值，并对u、v做[0,1]区间夹取。这些公式都是闭式解，单次计算耗时在微秒级，完全能满足实时性要求。
这里有一个工程细节：胶囊体如果用于覆盖关节区域，由于关节轴附近的几何形状并不完全是圆柱，直接用标准胶囊会漏掉一些凸起。我的做法是在关节罩附近额外叠加几个小球，球心落在关节轴线上，半径取罩体最大回转半径。这样既不用把整个罩体都做大包络，又能覆盖转动过程中的最大轮廓。
3.2 包围盒树加速：实际遍历策略和剪枝条件
层次包围盒树的加速效果取决于剪枝效率。在双臂碰撞检测场景中，初始状态下两条臂的距离通常较远，粗粒度节点在很浅的层次就能判定不相交，剪枝比例非常高。随着两条臂靠近，树遍历才逐渐深入到叶子节点。
遍历策略我优先用深度优先，因为实现简单且栈开销可控。对每个节点，先做AABB相交测试，再根据测试结果决定是否遍历子节点。为了进一步加速，还可以对两条臂的碰撞树分别建立各自的局部坐标系，遍历时先从末端执行器向基座方向回溯，因为绝大多数任务中，末端和载荷进入重叠区域的概率远大于臂根。
我在仿真平台上做过对比实验：不加速情况下，双臂各16个图元，每帧需要256次两两测试；用层次包围盒树后，平均只需要十几对精确测试，在中低配置的计算平台上单帧检测耗时可以压到1毫秒以内。这个性能指标对于碰撞检测来说完全是富余的，可以在同一颗处理器上同时跑多个避碰检查任务。
3.3 连续碰撞检测：为什么离散检测可能“漏”掉问题
如果只在每个控制周期做一次瞬时检测，理论上会存在一个致命漏洞：两条臂可能在一个控制周期内从不相交状态跨越到相交状态，而中间的所有中间构型都没被采样到。这在低速操作时问题不大，但空间机械臂在做大范围转移时末端速度可以到每秒几百毫米，一个40毫秒的控制周期里末端已经走了十几毫米，如果原本距离就接近阈值，完全可能直接穿越。
解决这个问题有两类思路。一类是子步进细分法：把当前周期内的运动轨迹按时间细分成若干小步，对每个小步做瞬时检测。这个方法实现简单，但会给控制回路增加额外延迟，而且细分步长的选取没有通用准则。
另一类是解析连续检测法：对运动中的图元，根据其起点和终点构型构造扫描体，把时间维度上的连续问题转化为空间维度上的几何问题。两个运动胶囊体之间的最近距离，可以转化为四个关键时间点的距离比较加上中间极值点的判断。这个做法的计算量比细分法小，而且能严格保证不遗漏碰撞区间。
我在实际项目中的方案是混合使用：常规状态下用瞬时检测加细分两步验证，当最小距离进入警戒阈值时切换到连续检测模式，用解析法精确计算最近距离和对应时间。这样既能保证常规状态的算力开销低，又能在高风险阶段提供严格保障。
3.4 距离梯度计算：给避碰规划提供方向信息
碰撞检测通常被认为是给安全系统提供“是否碰撞”的判断，但在双臂协同操作中，它还有一个更重要的角色：为避碰轨迹调整提供距离梯度。所谓距离梯度，就是当前最近距离对各个关节角度的偏导数。它告诉规划器：如果想增大两条臂之间的最小距离，最优的方向是让哪个关节转动、往哪个方向转动。
距离梯度的计算可以用解析法也可以用数值法。解析法需要对距离函数做链式求导，因为距离函数往往是不平滑的，求导过程麻烦且容易在最近点切换时产生跳变。数值法则简单粗暴，对每个关节角加一个小扰动，重新计算距离，再用差分近似偏导。缺点是计算量随关节数线性增长，14自由度就是14次额外距离计算。
好在碰撞检测只在高风险阶段才需要梯度，此时计算频率本来就可以适当降低。以100ms为周期计算一次梯度，对应的额外计算量完全在承受范围内。获得梯度后，避碰规划模块就可以沿着梯度反方向生成一个名为“排斥向量场”的修正量，叠加到原规划轨迹上，实现实时避让。
4. 双臂协同避碰的策略层设计
4.1 先规划后检测，还是边检测边调整
双臂避碰策略有两种流派。一种是完全离线规划——轨迹规划器在任务执行前就穷举搜索出一条无碰轨迹，运行时执行开环轨迹，碰撞检测只做监控，不参与在线修正。另一种是实时修正——碰撞检测模块在线输出距离和梯度，轨迹规划器根据这些信息实时调整轨迹。
空间机械臂操作任务对可靠性的要求极高，纯开环执行不现实，因为环境模型和实际状态总有偏差，离线规划出来的无碰轨迹可能因为基座漂移或柔性变形而失效。但完全实时修正也面临挑战：修正算法必须稳定收敛，不能让机械臂出现抖动甚至发散。
我采用的方案是离线规划和在线修正并用。离线阶段生成名义轨迹，并做全面的碰撞筛查，确保名义轨迹本身没有碰撞风险。在线阶段，碰撞检测模块持续监控实际构型与名义轨迹的偏差，一旦最小距离低于警戒阈值，就启动修正。修正幅度限制在小范围内，比如关节角修正量不超过一个限幅值，确保修正不会引发动力学稳定性问题。
4.2 主从协调策略：让一条臂主动避让
双臂协同操作必须明确角色分工，否则会出现两臂互相让、反而让出问题的场面。我习惯设计为主从结构：主臂负责完成核心操作任务，从臂负责避让配合。也就是说，碰撞检测算出的距离梯度只作用于从臂，主臂的轨迹不因碰撞检测结果而改变。
这样设计的好处是主臂的操作精度不受影响。比如双臂配合拆卸某设备时，主臂抓持着设备进行精密位移，如果因为碰撞报警就调整主臂轨迹，可能破坏整个操作过程。而从臂作为辅助臂，本来就在做陪护、支撑、观测类工作，稍微调整轨迹影响不大。
但主从分配不是永远固定的。不同任务阶段，主从角色可以动态切换。比如某阶段臂A在抓取，臂B在旁边待命，那臂A是主；下一阶段臂B在推进器前做精确操作，臂A退到安全距离，那臂B是主。切换时要注意过渡平滑，不能直接在运动中改主从标志，否则可能产生指令跳变。
4.3 多级响应策略：警戒、减速、急停的三级联动
实际工程中，碰撞检测的响应不是“检测到碰撞就停”，那样太粗糙。空间机械臂的惯性很大，急停本身也有风险，操作过程中的剧烈停机可能造成载荷晃动甚至损坏。所以我设计了一套多级响应策略，映射到控制系统的不同层级。
第一级是报警提示。当最小距离低于警戒阈值时，系统在操作界面上给出警告，并记录时间戳、构型快照和距离值，供地面人员分析。这一级不干预控制，因为可能只是瞬时波动。
第二级是速度限制。当最小距离低于减速阈值时，控制系统的速度指令上限被动态收紧。比如原本允许末端速度300mm/s，此时强制降到80mm/s。速度一降，机械臂在单位时间内走过的路径变短，即使后续还有接触风险，控制器也有更多反应时间。
第三级是紧急制动。当最小距离低于急停阈值时，控制器沿当前运动方向的反方向施加安全制动力，让机械臂在最短距离内停下来。急停策略的设计要特别注意，不能直接对关节施加阶跃扭矩，而要按规划的减速曲线执行，否则柔性臂会产生大幅振荡，碰得更狠。
4.4 同步控制周期：碰撞检测输出如何与控制器融合
碰撞检测模块的输出频率和控制器的执行频率不一定一致。我的设计是让碰撞检测以独立的周期运行，比如200Hz，而控制器以500Hz运行。两者之间通过一个共享内存接口通信，检测模块写距离值和标志位，控制器读这些值并做平滑处理。
这里有个关键注意点：控制器不能直接用原始的检测距离值做控制，因为这个值可能每帧跳变。距离跳变的来源很多，比如最近点从臂A的杆1切换到杆2，距离函数本身不连续。正确的做法是先对距离值做低通滤波或中值滤波，再用滤波后的值去驱动速度限制逻辑。
我在实际测试中发现，如果不对距离值做平滑，机械臂在接近过程中会出现明显的速度抖动，因为控制器看到的距离一会儿大一会儿小，速度限制一会儿松一会儿紧。加了滤波后，速度曲线变得平滑，操作体验和安全性都明显改善。
5. 工程实现中的性能优化与验证方法
5.1 算力预算：碰撞检测到底需要多少计算量
很多团队在做系统设计时会把碰撞检测的算力估得很高，导致硬件选型超标。实际上，合理的检测算法是可以在中端嵌入式平台上跑的。我以自己的实现为例：双臂各10个胶囊体和若干辅助球，加上3个环境OBB障碍，总计不到50个图元。采用层次包围盒树加速后，单帧的平均精确求交图元对大约12对，每对用时约2微秒，加上树遍历的包围盒测试开销，单帧总耗时约50微秒。
这意味着即使碰撞检测模块以200Hz运行，实时计算负载也只是占用了很小的一块CPU时间。真正费算力的是连续检测模式下的解析求交和距离梯度计算，但这两个功能只在高风险阶段启用，且此时对实时性的要求可以适当放宽。
5.2 数值稳定性：坐标变换和浮点误差的陷阱
空间机械臂的碰撞检测涉及大量坐标变换，尤其是把图元从局部坐标系变换到基座坐标系甚至全局坐标系时。变换矩阵的数值误差会累积，如果两条臂用了不同的变换路径，末端位置的数值误差可能达到厘米级。
我在实现中有一个习惯：几何图元采用双精度浮点数存储，变换时尽量使用单位四元数配合平移向量，而不是直接用旋转矩阵连乘。四元数的归一化特性可以避免矩阵连乘导致的数值漂移。另外，所有图元的坐标变换在进入碰撞检测模块前统一完成，模块内部不再做二次变换，减少误差入口。
还有一个常见陷阱：当胶囊体的半径设得比两个图元的距离精度还小时，距离计算结果可能因为浮点误差出现负值。程序里要对负距离做处理，不能直接拿去做控制。我的做法是在距离计算后统一加一个下限钳位，并且保留符号信息用于判断是否已经侵入包络。
5.3 仿真验证：从单元测试到全数字样机
碰撞检测算法的验证分为四个层次。第一层是单元测试，对每个求交函数的解析公式做大量随机输入验证，并与高精度的几何库结果对比。这一层主要验证数学正确性。
第二层是静态构型测试，把机械臂置于一系列关键构型下，人工检查检测结果是否合理。这些关键构型通常来自任务场景中的典型姿态，比如双臂交汇、末端交错、载荷贴近等。
第三层是动态场景测试，在仿真软件中运行动态任务流程，把碰撞检测模块接入闭环控制回路，观察整个过程中距离曲线和响应动作是否合理。这一层主要验证逻辑正确性和时序正确性。
第四层是硬件在环测试，把碰撞检测代码部署到实际控制器硬件上，接入真实传感器数据，测试实时性能和稳定性。我强烈建议这一层不能省，因为仿真环境中的数据都是干净的，硬件在环才能暴露时序抖动、缓存冲突、接口带宽等问题。
5.4 误报与漏报的权衡：参数标定的实际案例
碰撞检测系统的工程调试，本质上是误报率和漏报率的权衡。误报太多，操作团队会产生“狼来了”心理，甚至主动关闭报警系统；漏报一次，代价可能就是设备损坏。我在某次调试中，初始参数下系统频繁误报，排查后发现原因是环境模型中的舱体包络偏置量设得过小，而舱体表面实际有安装支架凸起，导致机械臂在正常操作中多次触发警戒。
解决方式是重新核对了所有环境障碍物的实际安装尺寸，把偏置量从20毫米调整到35毫米，同时把警戒阈值从200毫米放宽到180毫米。调整后误报率大幅下降，而留出的安全裕度仍然足够。这个案例说明：参数标定一定要结合真实工况，不能纯靠理论推算。
漏报问题的排查就复杂得多。有一次仿真测试中，两条臂明明很接近了，检测结果却显示距离正常，查了半天发现是碰撞树更新逻辑有个bug：臂B的某两个图元在关节转动超过一定范围后，局部坐标更新频率低于检测频率，导致检测用的是过期图元位置。这个bug在低速运行时完全看不出来，只有高速运动时才会暴露。从那以后，我坚持在代码里加入图元时间戳校验功能，一旦发现图元数据时效性不合格，就直接置为最高风险等级。
6. 常见问题与排查技巧实录
6.1 距离值跳变怎么办
距离跳变最常见的原因是最近点在图元之间切换。比如原本最近的图元对是臂A的杆3对臂B的杆5，某个关节稍微转了一下，最近图元对变成了臂A的杆4对臂B的杆6，距离值就会发生不连续的跳变。这种跳变在物理上完全合理，但会让下游控制器很困惑。
排查思路是先确认跳变是否来自最近图元切换。方法是在检测模块里输出当前最近图元的身份标识，对比跳变前后图元是否改变。如果是图元切换，就在控制器侧加中值滤波；如果不是，就要怀疑坐标变换或时间对齐出了问题。
6.2 漏检和误检怎么系统性排查
漏检的排查顺序通常是：先检查模型正确性——碰撞树里的图元尺寸和安装位置是否和实际一致；再检查时间同步——两条臂的状态是否对齐到同一时戳；然后检查检测频率——信号采样率是否足够覆盖机械臂最大运动速度；最后检查阈值——报警阈值是否设得比实际安全距离还小。
误检的排查顺序则是反过来的：先看阈值是否太敏感，再看环境模型中是否有不必要的包络偏置，然后检查是否有图元本身包络得过大。我总是建议把碰撞检测模块做成可回放模式，记录所有输入状态和检测结果，出问题时可以逐帧回看，快速定位是哪一个环节导致异常。
6.3 双臂基座漂移带来的虚假报警怎么处理
微重力环境下基座漂移是稀疏但持续的，末端位置误差因此持续积累。碰撞检测模块如果完全依赖运动学正解，基座漂移会直接导致距离计算偏移，产生虚假报警。
处理方案有两种。一种是引入基座位置估计器，用加速度计和光学测距数据实时估计基座位姿，再把估计值修正到运动学链里。另一种是允许一定的位置不确定度，在报警逻辑中加入“接触趋势判断”——不仅要距离低于阈值，还要距离的变化趋势持续缩小才触发报警，如果距离虽然小但变化率在零附近波动，就只记日志不触发。
我实际使用下来，两种方案结合的效果最好。基座漂移补偿能把系统模型修正得更准，趋势判断能滤掉尖峰干扰。单独用任一方案，总会有边界情况处理不好。
6.4 工具和调试手段推荐
碰撞检测模块的调试，可视化是最重要的辅助手段。我在仿真环境中总是把两条臂的碰撞图元以半透明形式叠加显示，距离最近的两点用高亮连线标出。这样操作员和开发人员一眼就能看出检测结果是否符合真实几何关系，比看数表效率高得多。
此外，日志系统要记录足够丰富的上下文信息。不仅要记录最小距离值，还要记录检测时间、图元对身份、坐标变换链路过版本、当前阈值状态、滤波前后对比值。这些信息在故障复盘中必不可少。我见过有团队为了节省存储空间，只记距离值不记图元身份，出问题时根本无从下手，最后只能靠猜。
7. 我的几点实战心得
做了几轮空间机械臂双臂碰撞检测的开发验证，有几个体会特别深。
第一，碰撞检测不是一个独立的算法模块，它和运动学、控制、传感、规划都耦合在一起。如果只把注意力放在数学求交公式上，忽略时戳同步、基座漂移补偿和参数标定，系统整体可靠性一定上不去。
第二，算力不是主要瓶颈，模型正确性和边界条件才是。与其花大力气优化求交函数的指令级性能，不如先把碰撞树的图元配置、环境模型的外扩偏置核清楚。很多团队的碰撞检测跑得飞快，但结果不对，就是因为模型的某个尺寸写错了。
第三，验证工作要高于实现工作。我统计过自己项目的时间分配，碰撞检测模块本身的编码只占三分之一，剩下三分之二全在测试、回放、调参和故障复盘上。这不是低效，而是这种安全关键系统的必然要求。
第四，给操作员留出人工确认的接口。再好的算法也有判定不了的局面，比如柔性臂振动导致距离值持续波动，或者环境模型里根本没有的临时障碍物。系统应该允许操作员通过人工指令切换检测模式、调整阈值或临时屏蔽某些风险等级，但这个操作必须留下审计记录。
如果后续有条件，可以考虑把碰撞检测从“反馈式”升级为“预测式”——用动力学模型预测未来几百毫秒内的系统状态，提前执行避碰动作，而不是等距离已经开始缩小才反应。这个方向的难点在于动力学预测的精度，但一旦突破，双臂协同操作的效率和安全性都会再上一个台阶。
