柔顺关节补偿的参数和工程步骤

一、悬空摆动

目标：确定每个关节的 MIT 参数 Kp 和 Kd，使关节在没有外部接触时能快速、平滑地跟踪目标位置，不产生振荡或过冲。

准备

• 关节处于空载状态（该关节之后的所有连杆都已安装，但末端不接触任何物体）。
• 驱动器已配置为 MIT 模式，Kp=0,Kd=0,τff=0初始。
• 上位机能以固定周期（如 2 ms）发送目标位置 qdes，并记录实际位置 qact

设置Kp

① 设定测试轨迹 使用一个小幅度、低频率的正弦摆动作为目标位置，这样既能激励动态响应，又不会触及机械限位，例如：

• 幅度：±5° 到 ±10°（0.1~0.2 rad）
• 频率：1~2 Hz

② 粗调 Kp

• 从很小的 Kp（如 10 Nm/rad）开始，运行摆动程序，观察实际位置曲线。
• 逐渐增大 Kp（每次增加 20%~50%），直到实际位置出现明显振荡或超调（即位置曲线在目标值上下反复波动，或反向冲过目标值再回来）。
• 记录此时临界振荡的 Kp,osc

③ 确定工作Kp 取 Kp=0.6×Kp,osc∼0.7×Kp,osc，此时通常响应较快且无振荡。

设置 Kd

取70%后已经不振荡了，本身没有超调，可以先设 Kd=0 运行，然后观察两个关键指标：

• 抗扰动能力：用手轻轻拍打该关节的连杆，看位置是否迅速恢复、有无余振，如果出现抖动或多次回摆，就增加 Kd
• 停止时的平稳性：摆动指令突然停止，观察关节是否立刻停稳，若在停止点附近出现小幅来回摆动，也需要 Kd。

从 0 开始，以 Kp×0.01为步长逐步增加，直到扰动后无余振、停止平稳。Kd 的物理效果就像粘稠的油，会让运动更柔顺，但过大会使响应变迟钝。最终标准是：阶跃响应无超调，扰动后2~3个周期内平稳。

如果追求极致简单，也可以设定 Kd=0，只要 Kp足够安全，工程上通常保留一定的 Kd 以增强鲁棒性。

最终目标

• 正弦跟踪最大位置误差 < 0.5°。
• 位置曲线平滑，无肉眼可见的锯齿或振荡。
• 用手敲击关节连杆，位置扰动在 0.2 秒内恢复且不振荡。

二、前馈力矩τ_ff

连杆质量、质心和摩擦参数全部用于计算前馈力矩 τff，它们让电机提前输出一个抵消自身重量和摩擦的力矩，这样 PD 控制器就只需要处理微小的动态误差，而不用扛着重力干活。

• 连杆质量 mi与质心 ci：结合当前关节角度，通过运动学计算出该连杆重力对每个关节产生的力矩，将其作为 τff 的一部分，可使机械臂在任意姿态下都能悬浮，不会因为重力而下垂。
• 摩擦参数 fc,fv：库仑摩擦和粘滞摩擦，用于预测并抵消关节运动时的摩擦阻力，使低速运动更平滑，避免粘滑现象。

不做这些补偿的风险：

• 机械臂在垂直方向运动时会产生较大的位置静差，甚至完全撑不住自身重量。
• 导纳力控贴合时，位置修正会带有额外的重力偏置，导致实际接触力不准。

三、重力与摩擦参数标定

1. 重力参数（连杆质量与质心）标定

原理：在不同姿态下，通过手动调节各关节 τff 使机械臂悬浮，记录下这些力矩值，然后反推出连杆的重力参数，本质是一次静态负载辨识。

• 准备：将整臂装好（含清洁模组），所有关节均设为 MIT 模式，但 Kp=0,Kd=0，只留 τff通道。
• 选取姿态：在机械臂工作空间内选择 6~8 个差别较大的姿态，要求各关节角度变化明显，尤其是俯仰关节 J5、J6。
• 手动悬浮：在每个姿态下，用手扶住末端连杆，然后从 J1 到 J6 依次调节每个关节的 τff值，直到该关节在这个姿态下刚好能静止悬浮（手松开后不下掉、不反弹）。记录下此时的关节角度 q 和对应的 τff 向量（6 维）。
• 数据拟合：根据机械臂的刚体静力学模型，关节重力矩向量 τg 可表示为各连杆质量 mi 和质心坐标 ci的线性函数，将 6~8 组数据作为样本，用最小二乘法即可辨识出这些惯性参数。这一步可以写成简单的 Python 脚本，调用 numpy.linalg.lstsq。
• 验证：用辨识出的参数在线计算重力前馈，让机械臂在多个新姿态下 Kp=0 悬浮，若无需手动调整即可静止，说明标定成功。

验收标准：

• 在任意姿态下，仅靠重力前馈（Kp=0）机械臂能静止悬浮，手动轻推后无重力性下垂。
• 或更简单的：在位置模式（Kp 正常）下运行，实际位置与目标位置的静差 < 0.1°。

2. 摩擦参数标定

原理：让单关节以不同恒定速度运动，由于无加速度和重力变化（其他关节锁死或补偿），电机力矩主要用于克服摩擦。记录速度-力矩关系，线性拟合得 fc,fv

• 隔离关节：将其他关节固定（或由重力补偿支撑），被测试关节可自由运动。
• 匀速运动：让该关节分别以一系列恒定速度（如 ±1°/s, ±5°/s, ±10°/s, ±20°/s）做正反转运动，记录整个行程中 τact 的平均值。
• 数据拟合：绘制速度-力矩散点图，用直线 τ=fc⋅sign(q˙)+fvq˙拟合，得到 fc,fv。
• 写入前馈：实时根据关节速度计算摩擦前馈，加入到 τffτff 中。

验收标准：

• 加入摩擦补偿后，关节在低速（<1°/s）运动时无粘滑跳动，力矩指令平稳。
• 来回换向时力矩过渡平滑，无突然的尖峰。

四、参数使用

假设已经完成了以下标定（按极简流程，不一定全做，但知道怎么用）：

每个周期 F = F_raw - F_bias

控制循环（每 2ms）：

1. 六维力传感器零漂及工具负载辨识（第1步）。
2. 单关节 MIT 参数整定（悬空摆动实验确定 Kp,Kd）。
3. 连杆重力/质心参数整定（静态多姿态悬浮辨识）。
4. 摩擦参数整定（匀速实验）。
5. 整机空载轨迹验证：跑螺旋线，检查跟踪精度和传感器力归零。
6. 运动学零位与工具标定（纸笔测试等）。
7. 导纳力控参数调试。

# 1. 读取实际关节位置并修正零位
q = read_joint_positions() - offset
dq = read_joint_velocities()
# 2. 正运动学，获取末端姿态
T_tool = forward_kinematics(q)
R = T_tool[:3,:3]
# 3. 力传感器补偿
F_raw, M_raw = read_ft_sensor()
F_contact = F_raw - F_bias - m * R.T @ [0,0,-9.81]
# 4. 导纳控制，计算末端修正量ΔX
# …（见之前伪代码）
# 5. 修正末端目标位姿，逆运动学得 q_des
q_des = ik(T_ref @ T_corr)
# 6. 计算关节前馈力矩
tau_ff = gravity_compensation(q) + friction_compensation(dq)
# 7. 发送 MIT 指令
for i in range(6):
    send_MIT_command(i, q_des[i], 0, Kp[i], Kd[i], tau_ff[i])