六维力传感器重量及质心坐标的参数辨识

一、辨识原理回顾（建立方程）

在工具不接触任何物体的静止状态下，传感器测得的就是工具重力产生的静力与静力矩。

• 基坐标系下重力矢量：base_g=[0,0,−g]T，其中 g≈9.81
• 当前传感器坐标系相对于基坐标系的旋转矩阵：R（由正运动学得到）
• 传感器感受到的重力：F_g=m · (R · base_g)
• 传感器感受到的重力矩：M_g=r_com×F_g

需要求解的未知数：标量质量 m，质心坐标 r_com=[x_c,y_c,z_c]T

将Mg=r_com×F_g写成关于待求向量 p=[m,mxc,myc,mzc]T的线性方程。对于每个姿态 i，有：

其实更简单的做法是只用力矩方程来求解，因为力方程已经包含在力矩方程中，方法如下：

这个同时包含 m 和 r_com 不够线性。更好的线性化方法是把 m 和 m·r_com 看作独立变量。

这样我们就得到了关于 S 的线性方程组，而质量 m 可以从力方程单独求解，或联合求解。

推荐的最稳定方法：

同时使用力和力矩方程，构造一个统一的线性系统，直接最小二乘求解 p=[m,Sx,Sy,Sz]T。

二、详细落地操作流程

步骤 1：准备工作（必须）

1. 机械安装完成：六维力传感器、转接法兰、清洁模组、管线全部固定好，与最终工作状态一致。
2. 底盘固定：移动底盘停驻，最好放下稳定支腿或锁死悬架，保证机械臂基座相对地面静止。在整个辨识过程中，底盘不能移动或晃动。
3. 传感器预热与校零：
   • 给传感器上电预热 10~15 分钟，消除温漂。
   • 让机械臂处于一个姿态，工具悬空不接触任何物体，静止 5 秒，记录当前传感器读数作为“电气零点” F_bias,M_bias，后续所有数据都要减去这个偏置。
4. 排除线缆应力干扰：
   • 确保所有工具端的线缆（供电、信号、水管）有足够的松弛度，且固定点尽可能接近传感器本体但不跨越弹性体，或者直接固定在工具架上，避免产生额外的回弹力。可用手轻拨线缆，观察力传感器读数变化，若变化超过 0.3 N，需重新走线。
5. 工具状态：清洁模组内无水、无泡沫（或处于标准待机重量状态），刷头静止。

步骤 2：设计激励姿态（关键）

由于末端可以达到多种倾斜姿态，但本身不能改变工具轴相对于世界 Z 轴的倾斜，只有两个俯仰关节能做到。因此你必须充分活动这两个俯仰关节，让工具的重力方向在传感器坐标系下产生显著变化。

姿态选择原则：

• 至少需要 4 个以上姿态（推荐 6~8 个）。
• 姿态应使重力在传感器 X, Y, Z 三个方向都有分量，避免始终只压在一个轴上。
• 覆盖清洁时的实际倾角范围，例如工具法向与竖直方向夹角从 0°~45° 或更大。
• 保持 SCARA 大臂、小臂位置固定（避免碰撞），仅调整升降和两个俯仰轴。

推荐姿态序列（传感器 Z 轴默认为工具指向）：

1. 姿态 1：两俯仰归零，工具竖直向下（清洁座圈姿态）。此时重力沿传感器 +Z 或 -Z 方向（取决于安装）。
2. 姿态 2：大俯仰前倾 30°，工具倾斜。
3. 姿态 3：大俯仰后仰 -20°，工具朝另一方向。
4. 姿态 4：小俯仰单独前倾 15°，大俯仰归零。
5. 姿态 5：大俯仰前倾 40°，小俯仰后仰 -10°（组合倾斜）。
6. 姿态 6：利用 SCARA 的 C 轴旋转 90°，再配合俯仰，使重力在传感器 X 和 Y 之间产生分量。
7. 姿态 7：复归竖直，但升降轴略下降，确保没有意外碰撞。
8. 姿态 8：再选择一种大幅倾斜组合。

注意：记录每个姿态的关节角，用于正运动学计算旋转矩阵 R。

步骤 3：数据采集

对每个姿态 i：

• 移动机械臂到目标关节角。
• 等待完全静止（可观察关节速度反馈为0后多等 1 秒）。
• 以至少 100Hz 的频率采集 2 秒钟的六维力数据（已减去步骤1 得到的偏置）和对应的关节角度。
• 对这段数据取平均，得到该姿态下的 F_raw,i,M_raw,i和关节角向量 q_i。

将数据记录成表：

步骤 4：计算旋转矩阵 Ri

使用正运动学，根据 q_i 计算传感器坐标系相对于机器人基坐标系的齐次变换矩阵。提取旋转部分 R_i。确保基坐标系 Z 轴竖直向上（或向下，需与重力方向定义一致）。一般设定基坐标系为Z 轴向上，重力方向为base_g=[0,0,−g]T

步骤 5：构造方程组并求解

使用 Python或 MATLAB 编写辨识脚本。

import numpy as np

g = 9.81  # 可更精确
# 采集数据：列表 forces_raw (Nx3), moments_raw (Nx3), R_list (Nx3x3)
N = len(forces_raw)

A = np.zeros((N*6, 4))
b = np.zeros(N*6)

for i in range(N):
    R = R_list[i]
    # 重力在传感器系下的单位方向 * g
    v = g * R.T @ np.array([0, 0, -1])  # 若基座标系Z向上
    # v = g * R.T @ np.array([0, 0, 1]) # 若Z向下，根据你的定义调整

    # 力方程
    A[6*i:6*i+3, 0] = v   # 对应质量 m
    b[6*i:6*i+3] = forces_raw[i]

    # 力矩方程 (S × v = -[v]_× S)
    V_cross = np.array([[0, -v[2], v[1]],
                        [v[2], 0, -v[0]],
                        [-v[1], v[0], 0]])  # 这是 -[v]_×
    A[6*i+3:6*i+6, 1:4] = V_cross
    b[6*i+3:6*i+6] = moments_raw[i]

# 最小二乘求解
p, residuals, rank, s = np.linalg.lstsq(A, b, rcond=None)
m = p[0]
S = p[1:4]
r_com = S / m

print(f"辨识质量 m: {m:.3f} kg")
print(f"辨识质心 r_com: [{r_com[0]:.4f}, {r_com[1]:.4f}, {r_com[2]:.4f}] m")

注意重力方向：如果实际安装传感器后，在工具竖直向下时，力读数 Z 分量约为 +mg，说明传感器坐标系 Z 轴向下，此时重力在基坐标系的表示应为 [0, 0, g]（若基座Z向上）或需调整符号。可通过一个已知姿态手动验证符号，否则辨识结果会出错。简单验证：让工具竖直向下，传感器力读数应接近 [0, 0, m*g] 或 [0, 0, -m*g]，据此决定 u_g 符号。

步骤 6：参数验证

1. 静态补偿验证：
   • 机器人运行在线重力补偿程序：实时读取传感器，减去根据 R 计算的重力 F_g=m⋅(RT⋅base_g)和力矩 M_g=r_com×F_g。
   • 让机械臂在多个新姿态（不同于辨识用姿态）静止，观察补偿后的力/力矩值。
   • 合格标准：各方向力波动在 ±0.2 N 以内，力矩在 ±0.05 Nm 以内，且无明显偏置。如果偏置较大，说明辨识有误或零漂变化，需重做。
2. 动态轻推验证：
   • 在补偿开启下，用手轻轻推动清洁模组，传感器读数应只反映你推的力，一松手力回归零线。证明重力补偿完全。
3. 记录质心合理性：
   • 辨识出的 rcom 长度（距离传感器中心的距离）应在合理范围，例如几十到一百多毫米，且方向大致朝模组重心方向。这可用目测粗略判断。

三、注意事项

• 传感器零漂：温漂、上电初期不稳定是最大误差源。务必预热，并在辨识前校零。如果长时间工作后零漂变化明显，可设置定时自动校零（收回工具悬空）。
• 工具内液体：若清洁模组有水箱或泡沫发生器，液体晃动会造成力波动。辨识时应排空或注满并静止，确保晃动停止。波动大时难以获得稳定平均值，可延长采样时间并做低通滤波。
• 线缆和管道的应力：任何牵拉都会产生额外力，且姿态变化时这个力非线性变化。必须做到“零应力”走线，这是力控成功的一半。
• 运动学精度：正运动学计算中，关节零位偏差、连杆长度误差都会导致姿态矩阵 R 不准，从而重力分解错误。务必确保机器人运动学参数标定准确。
• 避免奇异姿态：在辨识数据采集中，避免俯仰关节使工具轴线接近竖直且没有其他轴变化，虽然这没问题，但最好让重力方向在传感器 XY 平面有投影，以保证 S_x, S_y 可观。
• 多次求解取平均：为了提高鲁棒性，可以重复做 3 次辨识（重新移动姿态），结果取平均。若结果偏差大，说明存在未建模干扰。
• 不要在接触状态下辨识：绝对确保工具四周悬空，无触碰。

完成上述流程后，就得到了精确的 mm 和 rcom。将它们写入重力补偿模块，从此六维力传感器将成为一个纯净的“接触力感知器官”，为你后续的导纳贴合、接触检测打下坚实基础。