MIT模式不加传感器，实现关节柔顺控制

MIT模式通过自感知外力引起的微小位移，在不依赖外部力传感器的情况下实现了关节级柔顺，可以通过动态改变虚拟弹簧的刚度 Kp，完美融合了位置和力矩控制，实现了二者间丝滑、无级的连续过渡。

MIT 模式之所以能实现不需要末端力传感器就能关节柔顺，以及能从纯位置控制平滑过渡到纯力矩控制，根本原因在于它重新定义了电机听话的方式。

一、不需要末端力传感器

普通位置模式（CSP）下的电机像一个“死心眼”的士兵，命令它到某个位置，它会不惜一切代价（输出最大力矩）到达那里。如果路上有障碍，它会直接撞上去。要让它柔顺，就必须（上位机+末端力传感器）告诉它：前面有障碍，后退一点。

而 MIT 模式相当于给这个士兵装上了自己的“触觉”和“判断力”。

它的核心控制律是：

这里的 qdes不再是必须精确到达的死命令，而是一个期望位置。

• 当没有外力时：关节会回到 q_des。
• 当有外力 τ_ext 试图推开关节时：关节会顺从地后退，直到位置偏差产生的抵抗力Kp(qdes−q) 与外力平衡。

关键洞察：MIT 模式不是通过感知力 → 改变位置的主动闭环来柔顺，而是通过设定一个虚拟的物理弹簧（刚度 Kp），让关节本身就表现得像一个弹簧。外部施加的力（τext）和产生的位移（qdes−q）严格遵循胡克定律。力传感器的作用被这个虚拟弹簧的物理特性所取代。

举例：

• 位置模式：命令到 A 点。外力推它到 B 点，电机死命反抗，电流飙升，直到推回 A 点。
• MIT 模式：命令到 A 点，弹簧刚度 100 Nm/rad。外力推它到 B 点，产生 0.1 rad 偏差，电机只是平静地输出 10 Nm 的力与之平衡。电机没感知力，它只是在忠实地扮演一个弹簧。

二、从纯位置到纯力矩的连续过渡

MIT 模式的优美之处在于，它把位置控制和力矩控制统一在了一个公式里。通过调整 K_p（刚度）和 K_d（阻尼）这两个参数，就可以连续地改变关节的物理特性，而不是切换控制模式。

状态 1：纯位置控制（高刚度）

• 参数设置：Kp设置为一个极大值（接近伺服刚度的上限），Kd相应设为临界阻尼值，τff=0。
• 原理：当 Kp极大时，任何微小的位置误差 qdes−q都会被放大成巨大的力矩指令。关节表现得极其刚硬，几乎不可推动，位置跟踪极其精确，这模拟了传统的位置模式。

状态 2：柔顺交互控制（有限刚度）

• 参数设置：Kp 设置为一个中等或较小的值，Kd 保证系统不振荡。
• 原理：关节表现出明确的弹性。当推它时，它会柔顺地后退。这是 MIT 模式最常见的应用场景，例如机器人腿着地时的缓冲、拖动示教。

状态 3：纯力矩控制（零刚度）

• 参数设置：Kp=0,Kd=0。
• 原理：此时 MIT 公式退化为 τ=τff。关节不再有任何位置闭环，它完全变成一个纯净的力矩源。它输出的力矩精确等于前馈力矩 τff，对位置没有任何约束。
• 挑战：要实现完美的零力柔顺（拖动示教），τff必须精确补偿掉关节自身的重力、摩擦力和惯性力。这就是为什么全动力学模型在这里变得至关重要。

连续过渡的实现

这种过渡不是“if-else”的开关切换，而是在同一个控制周期内，动态地修改 K_p 和 K_d 的目标值，并通过斜坡或滤波器让它们平滑变化。

工程实现流程：

1. 总线接口定义：在 EtherCAT 过程数据中，为每个关节定义以下接收对象：
   • q_des：期望位置
   • dq_des：期望速度（可选）
   • K_p：位置刚度（比例增益）
   • K_d：速度阻尼（微分增益）
   • tau_ff：前馈力矩
2. 驱动器内部实现：

// 驱动器每个电流环周期 (如50us) 执行
pos_error = q_des - q_actual;
vel_error = dq_des - dq_actual;
// MIT 控制律计算目标力矩
tau_cmd = Kp * pos_error + Kd * vel_error + tau_ff;
// 电流环执行
current_loop(tau_cmd);

3. 上位机平滑过渡逻辑

# 定义过渡时间
transition_time = 0.5 # 秒
# 假设从柔顺模式 (Kp=50) 过渡到力矩模式 (Kp=0)
start_kp = 50
target_kp = 0
start_time = current_time

while current_time - start_time < transition_time:
    # 线性插值
    kp_cmd = interpolate(start_kp, target_kp, elapsed_time)
    # 根据 kp 重新计算最优 kd，保证临界阻尼
    kd_cmd = 2 * sqrt(kp_cmd * joint_inertia)

    # 发送更新后的阻抗参数和 tau_ff
    send_mit_command(q_des, dq_des, kp_cmd, kd_cmd, tau_ff)
    sleep(0.001)

三、总结与对比