机械臂柔性运动的重力和惯性补偿

六维力传感器直接测到的是施加在它安装面上的所有广义力，这里面混合了真实接触力、工具重力、自身重力、运动惯性力以及振动等干扰。直接使用这个原始信号，会让导纳控制器产生错误的位置修正，轻则贴合不准，重则误判碰撞或引起振荡。

要解决这个问题，必须系统地做力信号净化，核心是重力补偿和惯性补偿。

一、干扰源拆解

把传感器测量值 F_s 拆解开：

F_s=F_contact+F_gravity+F_inertia+F_disturbance

• Fcontact：想要提取的真实接触力（清洁头与釉面）。
• Fgravity：末端工具（含清洁模组、安装件）的重力在传感器坐标系下的分量，这个力随机械臂姿态变化而变化，量级恒定且不小。
• Finertia：工具质量在加速时产生的惯性力（m⋅a）以及转动惯量带来的力矩，清洁时通常运动速度不高，但若快速启停或摆动，此项不可忽略。
• Fdisturbance：其他，如刷头旋转产生的周期性扰动力、线缆拉扯、水液晃动、传感器温漂、零漂等。

二、核心补偿方法

1. 重力补偿（基础且必须）

重力补偿就是根据当前机械臂姿态，实时算出工具重力在传感器坐标系下的值，并减去。

• 事前标定（一次性）
  1. 将机器人安装好工具，不接触任何物体，操纵末端到3个以上不同的姿态（如传感器轴分别竖直向上、水平、倾斜45°等）。
  2. 记录每个姿态下传感器的力/力矩读数及对应的末端姿态（旋转矩阵 Rtoolbase）。
  3. 利用最小二乘法，解算出工具在传感器坐标系下的质量中心坐标 r_com （三维向量），以及工具总重力大小 G=mg
  4. 很多协作机器人厂商提供负载辨识工具，可自动完成该步骤。
• 在线实时计算 已知标定结果 m 和质心在传感器坐标系的位置 r_com，实时获取当前末端相对于基座的姿态 R_sensorbase。
  • 重力在基坐标系下始终为 g_base=[0,0,−g]T。
  • 转换到传感器坐标系： F_g=R_basesensor x g_base x m（力部分）
  • 同时重力对传感器原点产生力矩： M_g=r_com×F_g
  • 从传感器原始读数中减去这两个分量即可。

实现要点：

• 需要实时获取准确的末端位姿（通过正运动学）。
• 由于机构有SCARA+双俯仰，姿态计算直接使用关节角度和正运动学即可，精度足够。

2. 惯性补偿（动态运动时必要）

当末端存在加速度时，工具质量会产生惯性力 −m⋅a。

• 获取加速度 a： 可通过关节编码器二次差分得到，也可在末端加装低成本的IMU（更直接，但需注意防水）。对于清洁这种低速场景，关节编码器差分通常够用，但需低通滤波处理噪声。
• 计算惯性力/力矩：
  • 平动惯性力： F_iner=−m⋅a_sensor（a_sensor 是传感器坐标系下的线加速度）
  • 转动惯性力矩（如果转动较大）： M_iner=−I⋅α−ω×(I⋅ω) 一般清洁时转动很慢，此项可忽略，或仅用简单模型。
• 从传感器信号中扣除 Finer（和 Miner）。

注意：惯性补偿对加速度信号噪声敏感，要配合低通滤波。如果清洁速度确实很慢（比如切向80mm/s，且无剧烈急停），惯性项可能远小于接触力，为简化可暂忽略，但重力补偿绝不可省。

3. 刷头旋转等周期性干扰抑制

若清洁模组自带旋转刷或超声波振动，会引入高频周期力。

• 机械隔离：设计上确保刷头转动系统与传感器安装面之间有软连接，尽量不传到传感器。
• 软件滤波：使用陷波滤波器锁定旋转频率（比如刷头转速600rpm即10Hz），从力信号中精确去除该频率成分，同时保留低频的接触力信息。

4. 零漂与温度补偿

每次启动或进入清洁前，让机器人末端悬空静止，记录几秒传感器读数取均值作为当前零点。若传感器温漂明显，可每隔一定时间自动执行一次空载校零。

三、落地实施步骤（针对清洁系统）

1. 安装完成后的首次标定
   • 在无接触、静止状态下，用机器人内置负载辨识功能或手动采点，求出末端工具（包括清洁模组、支架、传感器本身负载）的质量 m 和质心坐标 rcom。
   • 验证：让机械臂摆出几个姿态，观察补偿后的力值是否接近零（<0.2N）。
2. 实时运行回路
   • 读取六维力传感器 raw → 减去实时重力项（用当前姿态计算） → 减去实时惯性项（用当前加速度计算，如低速可省略） → 陷波滤波去除刷头周期干扰 → 低通滤波（如100Hz，避免力控内环振动） → 得到纯净的 F_contact → 送入导纳控制器。
3. 双重保险 即使做了以上补偿，导纳控制器本身的过阻尼参数（大D）也对残余高频误差不敏感，但为提高接触检测精度和恒力稳定性，力信号前置净化是必须的。

四、一句话总结

重力补偿是底限，惯性补偿看速度，周期干扰用陷波，线缆走线要自由。 通过这些处理，才能得到一个只反映“釉面接触力”的干净信号，让导纳控制器的虚拟弹簧阻尼系统真正准确地感知和反应。