柔性运动的惯性补偿公式解析

惯性补偿公式源自刚体动力学，其核心是：将六维力传感器之后的整个工具视为一个刚体，通过牛顿-欧拉方程，计算出它在加速运动时应该产生的惯性力和力矩，然后从传感器原始读数中减去这些量，从而得到纯净的外界接触力。

公式在传感器坐标系下表示为：

力补偿：

力矩补偿：

下面逐一拆解，为什么正好是这些项，以及它们各自的物理含义。

一、力的补偿：质心加速度的来源

工具的全部质量可视为集中在质心一点，传感器测量的是作用在它自身原点上的力，但惯性力真正产生的位置是质心。质心在空间中的绝对加速度 a_com 由传感器原点（即传感器坐标系原点）的运动和绕该原点的转动共同决定。根据运动学，有：

• a_sensor： 平动牵连加速度，这是传感器原点自身的线加速度。如果整个工具只做平移而没有转动，那么质心就和传感器原点一起平动，加速度完全等于 asensor。
• α×r_com：切向加速度，当工具绕传感器原点有角加速度 α时，质心由于不在转轴上，会受到一个垂直于 r_com和 α所在平面的切向线加速度，对应的是旋转起步或刹车时，质心被甩出去的倾向。
• ω×(ω×r_com)： 向心加速度，即使角速度恒定（α=0），只要工具在旋转，质心就有一个始终指向转轴的向心加速度。它的大小为 ω^2⋅d（d 是质心到转轴的垂直距离），方向朝向转轴。
• 乘以 -m：牛顿第二定律指出，让质心产生上述加速度的净外力是

m⋅a_com，但我们的传感器测的是工具对外界的作用力。在自由运动（无接触）时，传感器应读数为零；实际上它读到的正是工具自身惯性导致的“虚拟力”——也就是达朗贝尔惯性力，大小为 −m⋅a_com。因此，要补偿的惯性力就是 −m⋅a_com，即公式中的三项之和乘以 -m。

二、力矩的补偿：旋转惯性与力臂效应

传感器测量的力矩 M_raw 是外力对传感器原点的力矩。工具刚体绕原点转动时，其动量矩变化会产生惯性力矩，同时，作用在质心上的惯性力也会对传感器原点产生额外的力矩。

根据欧拉方程，工具对传感器原点的动量矩为 L=I_sensor·ω。其变化率（即外力矩）为：

因此，惯性力矩（需要补偿的反向力矩）就是上述两项取负。

• I_sensor · α： 角加速度惯性力矩，这是绕传感器原点的角加速度 α 直接引起的惯性阻力矩，就像旋转一个静止的哑铃，必须克服其转动惯量 I一样，质量分布越远离原点（I越大），这项力矩越大。
• ω×(I_sensor · ω)：陀螺力矩，即使角速度大小不变，只要角速度方向变化（即存在进动），动量矩矢量 L=Iω就会跟着转向，需要额外的力矩来维持。这一项在机器人动力学中常被称为陀螺力矩或科里奥利力矩。
• r_com×F_iner：惯性力的力矩贡献，第4、5项只给出了“绕传感器原点转动”所需的净力矩，但之前计算出的惯性力 Finer是作用在质心上的。这个力会对传感器原点产生一个力矩。由于传感器实际测量的力矩包含了这个力矩，因此在补偿时必须再减去它。

三、清洁场景下的实例理解

假设清洁模组正在做快速趋近后的回退动作，小俯仰关节向上抬，大俯仰关节同时微调。

• 力补偿部分：工具突然向上加速（第1项），并且开始绕传感器原点向上旋转（第2项），同时已经存在的旋转产生向心加速度（第3项）。这三项叠加，使得传感器感受到一个向下的冲击力。如果不补偿，导纳控制器会误以为是接触力突然增大，从而错误地向下压或引发振动。
• 力矩补偿部分：由于俯仰角加速度（第4项）和陀螺效应（第5项），工具会产生一个反扭力矩；同时，上面那个虚假的“向下冲击力”因为作用在质心，还会对手腕产生一个额外的弯折力矩（第6项）。补偿掉这些，才能让手腕力矩归零，保证姿态柔顺。

注意事项： 在低速清洁时，第2、3、4、5项可能都非常小，仅第1项（平动加减速）在快速趋近阶段占主导。但如果俯仰关节较灵活、转动幅度大，则第2项和第4项会变得显著。忽略它们，就会导致接触力检测瞬间出现尖峰，破坏釉面安全。

因此，惯性补偿公式的这三部分（力三项，力矩三项）完整描述了刚体在任意运动状态下，在传感器原点处应测得的惯性效应。将它们从原始读数中扣除，剩下的才是“纯粹的外界接触”，让你的机器人即使在动态运动中也拥有精准的“触觉”。