机械臂柔性控制的惯性补偿参数获取

惯性补偿的目的，是消除工具自身加速运动时产生的惯性力/力矩对六维力传感器读数的干扰。这样，传感器输出的就只剩下真实的接触力，确保导纳控制器不会把机器人自己的“动”误认为外界的“碰”。

对清洁场景，过程运动平缓，惯性力通常比重力小一个量级，但如果在快速趋近、区域切换、或者有突然的启停时，惯性项的影响就会凸显。因此，进行恰当的惯性补偿，是让整个力控系统在所有工况下都稳健运行的必要保障。

一、惯性补偿需要哪些参数

所有参数都定义在传感器坐标系下。

二、各参数原理、含义与获取方式

1. 工具惯性张量 I_sensor

物理含义：表示工具质量在传感器坐标系下抵抗转动加速的能力分布，是一个对称矩阵：

对角元素是绕各轴的转动惯量，非对角元素是惯性积，描述质量分布的不对称性。

为什么需要它：当工具发生角加速度 α时，传感器会感受到一个惯性力矩 −Iα（若转动轴不在质心还会耦合）。如果角运动极小，此项可忽略。但机构有两个俯仰轴，运动时工具会产生绕俯仰轴的角加速度，因此 I_sensor 需要被考虑。

获取方法（按推荐度排序）：

• 方法一：CAD 计算（推荐首选） 在 SolidWorks 等软件里，为所有零件赋予真实材料密度，以传感器坐标系原点为参考点，直接读取整个装配体的惯性张量，这是最直接且相对准确的方法，尤其模组结构已经数字化。
• 方法二：多体动力学辨识（高精度需求时） 类似负载辨识，但需要做一系列激励运动并记录力矩和运动状态。让机器人按特定轨迹运动（如正弦扫频、阶跃加减速），通过方程 M_iner=Iα+ω×(Iω) 建立超定线性方程组，求解 I中的 6 个独立分量。很多协作机器人内置了负载辨识功能，可同时输出惯性张量。如果需要自行实现，需要精确的角加速度信号，难度和噪声风险较高。
• 方法三：近似模型 如果暂时无法获取精确 I ，可以将工具近似为一个集中质量 m 位于质心，转动惯量仅由 m 和质心距离 r_com产生：

这会忽略工具自身的体分布惯量，但在质量集中、速度慢的情况下，作为起步是可以接受的。

2. 运动学参数：a_sensor, ω, α（基于关节编码器）

1. 通过正运动学得到传感器坐标系相对于基座的位姿 T(t)。
2. 数值微分计算速度与加速度：用高频率（≥1kHz）记录位姿的时间序列，经低通滤波后做中心差分。
   • 线速度 v=Δx/Δt
   • 线加速度 a=Δv/Δt
   • 角速度可从旋转矩阵的导数或直接由雅可比矩阵与关节速度获得：ω=Jωq˙
   • 角加速度 α=Jωq¨+J˙ωq˙，或直接对 ω 数值差分。

多次差分会显著放大噪声，须配合低通滤波（截止频率根据运动带宽设置，如 10~20 Hz）。

三、惯性补偿公式

在传感器坐标系下，工具运动产生的总惯性力和力矩为：

其中：

• a_sensor 是传感器原点的线加速度。
• α×r_com 和 ω×(ω×r_com)分别是质心因转动而产生的切向和向心加速度。
• 力矩公式中，前两项是纯旋转惯性力矩，第三项是惯性力因作用在质心而对传感器原点产生的力矩。

补偿后的净接触力/力矩即为：

四、注意事项

1. 先判断必要性： 估算运动中的最大加速度。若切向清洁时平稳（加速度 < 0.05 m/s²），惯性力相对于 3N 的接触力可忽略，但在快速趋近、回退、跨区域移动时，即使时间短，也可能造成力尖峰，此时惯性补偿很有价值，可以分段启用。
2. 滤波是核心： 加速度信号噪声是最大的敌人，对关节角进行差分得到的 α 和 a 必须经过合适的低通滤波（建议 Butterworth 二阶，截止频率 5~15 Hz）。如果补偿后力信号出现高频抖动，通常是加速度噪声引起的，应降低截止频率或加大导纳阻尼 D。
3. 避免过度补偿： 如果惯性张量 I不准，在大角加速度下可能补偿过头。先用 CAD 或近似模型，再在空载下做快速空跑测试：让机器人在不接触任何东西的情况下，跑一遍你最快的清洁或趋近轨迹，观察补偿后的力是否仍然接近零线。若有明显偏差，可微调 I或简化忽略部分项。
4. 坐标变换必须一致： 所有运动量（ω,α,a）必须已正确转换到传感器坐标系。如果雅可比和运动学输出的是末端连杆坐标系，需要再乘以一个恒定变换矩阵到传感器安装面。
5. 利用硬件特点： 小行程俯仰关节通常用于高频柔顺和力控，它本身运动幅度小，即使有角加速度也不会产生很大的惯性力矩。大行程俯仰在粗定位时运动可能剧烈，但此时不处于力控贴合阶段，可以选择在切换至导纳模式时立即启用惯性补偿，之前不补偿以减少计算。
6. 验证方法： 在不接触釉面的状态下，让机器人带着工具做清洁时会有的最大速度、最大加速度动作（如快速摆动俯仰轴、末端走圆形），打开重力+惯性补偿，观察力读数应始终在 ±0.3 N 以内，这是检验补偿质量的金标准。

惯性补偿是力控系统从“静态准确”走向“全动态准确”的关键一步。