4. 碰撞检测数学基础

在双臂机器人系统中，碰撞检测并不是“把两个模型丢进引擎里跑一下”这么简单。它背后依赖一套稳定的数学语言：用向量描述方向与位置，用矩阵描述姿态与变换，用坐标系把不同部件统一到同一参考框架，再通过距离计算与几何求交判断“是否接触、何时接触、接触在哪里”。本节把这些基础工具串成一条可落地的知识链，为后续包围盒、GJK、分离轴、连续碰撞检测等内容打底。

一、核心原理：从“点”到“体”的数学表达

碰撞检测的第一件事是回答：物体在空间中的位置和姿态如何表示？在三维空间中，一个点用三维向量 p = [x, y, z]^T 表示；一个方向用单位向量 u 表示；一个刚体的位姿则用“位置 + 旋转”描述。旋转常用旋转矩阵 R（3×3，正交且行列式为 1）或四元数表示。若某点在物体局部坐标系下为 p_local，则它在世界坐标系下为：

p_world = R * p_local + t
其中 R 为旋转矩阵，t 为平移向量。

双臂机器人有两个机械臂，各自有基座坐标系、关节坐标系、末端工具坐标系，甚至还有视觉相机坐标系。碰撞检测必须在统一坐标系下进行，否则“距离”没有意义。因此，坐标系变换是碰撞检测的“语法”，向量与矩阵是“词汇”，距离与求交是“句子”。

二、方法流程：向量、矩阵与坐标系变换

向量运算中，点积用于求夹角与投影，叉积用于求法向量与判断方向。例如，判断两个方向是否接近平行，可用 |u·v| ≈ 1；求点到直线的距离，可用投影长度。矩阵则用于组合变换：若先旋转再平移，可写成齐次变换矩阵 T：

T = [ R  t ]
    [ 0  1 ]

p_h = T * p_h_local
p_h = [x, y, z, 1]^T

齐次坐标的好处是：旋转、平移、缩放可以统一为矩阵乘法，多个变换可以连乘。对于双臂系统，常见链路是：世界坐标系 → 左臂基座 → 左臂末端 → 左臂工具；世界坐标系 → 右臂基座 → 右臂末端 → 右臂工具。任意两个部件之间的相对位姿可表示为：

T_AB = T_AW^{-1} * T_BW
其中 T_AW 表示 A 到世界系的变换。

这一步是碰撞检测的“对齐”操作。只有把两个物体的几何体放到同一坐标系下，才能计算距离或求交。

三、距离计算与几何求交：判断“碰没碰”

距离计算是碰撞检测的连续量基础。常见对象包括点-点、点-线段、点-三角形、线段-线段、球-球、球-平面、AABB-AABB 等。以点-线段距离为例，设线段端点为 A、B，点为 P，则：

AB = B - A
t = clamp( dot(P - A, AB) / dot(AB, AB), 0, 1 )
Q = A + t * AB
distance = ||P - Q||

几何求交则回答“是否相交”。例如，两个球体相交的条件是：

||c1 - c2|| <= r1 + r2

两个 AABB 相交的条件是三个轴上的区间都重叠：

overlap_x = (min_x1 <= max_x2) && (min_x2 <= max_x1)
overlap_y = ...
overlap_z = ...

对于更复杂的凸体，常用分离轴定理（SAT）或 GJK。SAT 的核心思想是：若存在一个轴，使两个物体在该轴上的投影不重叠，则它们不相交。GJK 则通过迭代寻找两个凸集之间的最近点，若距离为零则相交。这些算法都建立在向量投影、支撑函数和距离计算之上。

四、案例/场景：双臂协作中的“擦碰”判断

假设左臂末端工具与右臂末端工具在协作搬运中靠近。第一步，通过正运动学得到左工具在世界系下的位姿 T_WL_tool，右工具为 T_WR_tool。第二步，将两个工具的几何模型（如圆柱、球、AABB）变换到世界系。第三步，计算两者之间的最近距离。若距离小于安全阈值 d_safe，则触发减速；若距离小于接触阈值 d_contact，则判定为碰撞。若使用球体近似，则直接比较球心距与半径和；若使用 AABB，则检查三轴区间重叠；若使用凸包，则用 GJK 求最近距离。

五、常见误区与复盘

• 误区一：忽略坐标系变换。直接拿局部坐标算距离，结果完全错误。必须统一到同一参考系。
• 误区二：把旋转矩阵当普通矩阵。旋转矩阵必须正交，数值误差累积后需重新正交化，否则变换会“扭曲”。
• 误区三：只用距离阈值代替求交。距离小于阈值不等于相交，需结合几何求交或连续检测。
• 误区四：忽略连续碰撞。高速运动时，离散采样可能跳过碰撞瞬间，需用扫掠体或时间步进。

复盘时，建议把每次碰撞或接近事件记录为：时间戳、左右臂关节角、末端位姿、最近距离、使用的几何表示、判定结果。这样既能验证数学模型的正确性，也能为后续优化提供数据支撑。掌握向量、矩阵、坐标系变换、距离计算与几何求交，是双臂机器人碰撞检测从“能跑”走向“可靠”的必经之路。