深挖TRLC-DK1双线程控制架构:250Hz电机循环平滑运行的秘密

原创2026-10-28 02:42:321,624 阅读

深挖TRLC-DK1双线程控制架构:250Hz电机循环平滑运行的秘密

TRLC-DK1 是 The Robot Learning Company 推出的 AI 原生机器人开源开发套件,其从动臂(Follower Arm)的流畅控制背后,是一套精巧的双线程控制架构:250Hz 的电机线程独立负责所有串口 I/O,另一条服务器线程专职计算重力补偿与安全保护,两条线程通过加锁的共享缓冲区无缝协作,让 6 自由度机械臂+夹爪在 AI 推理的"卡顿间隙"里依然纹丝不动、平滑运行。本文将带你拆解这套架构的每一个关键设计。

🤖 先认识 TRLC-DK1:开源 AI 机器人开发套件

TRLC-DK1 由一条 6 自由度从动臂(Follower)和一条示教臂(Leader)组成:你手动操作 Leader,Follower 实时跟随,从而录制遥操作(Teleoperation)数据,再交给 LeRobot 生态做策略训练与推理。

TRLC-DK1 从动臂 CAD 渲染图,展示了 6 自由度机械臂与夹爪的整体结构

硬件上,手臂前三个大关节使用峰值扭矩 28 Nm 的 DM4340 电机,后三个小关节与夹爪使用 DM4310 电机,全部通过一根 921600 波特率的串口连接,底层驱动基于 lerobot_robot_trlc_dk1/motors/DM_Control_Python/DM_CAN.py。这种"单线总线挂 7 个电机"的设计,恰恰是后面控制架构要解决的核心难题。

TRLC-DK1 机械臂 X 光透视图,可以看到内部电机、线缆与关节布局

🧵 为什么必须用双线程?

如果把"读写串口"和"跑推理算法"放在同一个线程里,会有两个致命问题:

  1. 串口 I/O 慢且不可预测。每次向 7 个电机发命令、收反馈都要走串口,单次耗时几毫秒起步,主线程一旦卡在 I/O 上,控制频率就崩了。
  2. 应用逻辑节奏不规则。跑扩散策略(diffusion policy)推理时,模型去噪一步可能耗时数百毫秒——这期间没人发新指令,但机械臂绝不能"松手"下垂。

TRLC-DK1 的答案是职责彻底分离:

线程 频率 职责
电机线程(dk1-motor) 250 Hz 独占串口:发 MIT 位置指令给 6 个关节、发 EMIT 力控指令给夹爪、读回位姿/速度/力矩
服务器线程(dk1-server) 250 Hz 读状态 → 看门狗 → 重力补偿 → 安全检查 → 写入新指令

两条线程之间只有一个 threading.Lock 保护的共享缓冲区:电机线程写状态、读指令;服务器线程写指令、读状态;用户代码则通过更高层的接口读写。锁内只拷贝 numpy 数组(微秒级),绝不持锁做串口操作——这是实时系统里"短临界区"的经典做法。架构总览见 trlc_dk1_control/robot.py 的模块注释。

⚡ 电机线程:250 Hz 平滑运行的三大秘密

核心实现在 trlc_dk1_control/motor_chain.py 的 _motor_loop(),每一圈循环(4 ms)做四件事:加锁快照指令 → 发送 controlMIT(6 关节)与 control_pos_force(夹爪)→ 读回 7 路电机状态 → 写回共享缓冲区。

秘密一:5 ms 串口超时。 配置文件里写得很直白:serial_timeout: float = 0.005 # must be short for 250 Hz loop(见 trlc_dk1_control/config.py)。超时只有 5 ms 意味着 recv() 最多阻塞 5 ms 就放弃,保证 recv() 永远不会拖垮 250 Hz 的控制循环;而旧的 pos_vel 模式用的是 500 ms 超时,那是给"不追求实时"的场景准备的。

秘密二:MIT 阻抗控制 + EMIT 力控的分工。 手臂关节用 MIT 模式,控制器在 250 Hz 下发

力矩 = kp×(目标位置 − 当前位置) + kd×(目标速度 − 当前速度) + 前馈力矩

这样手臂是"有弹性"的阻抗体,既能跟随又不过硬;夹爪则用 EMIT(Torque_Pos)模式,位置可控、力矩受限(默认最大 1 Nm),实现柔顺的力控抓取。夹爪在启动时还会自动"旋转直到力矩突增"来标定张开位姿,无需手动校准。

秘密三:sleep 大部分时间 + 忙等收尾。 time.sleep() 的粒度在 macOS 上约有 1–2 ms,直接 sleep 到 4 ms 周期末会累积抖动。代码的做法是:先睡 period − elapsed − 1ms,最后 1 ms 用空循环 while time.monotonic() − t_start < period: pass 忙等到点(见 motor_chain.py)。此外线程每 5 秒打印一次实测频率与单圈耗时,方便你验证"真的是 250 Hz"。

🛡️ 服务器线程:重力补偿、看门狗与三重安全

服务器线程(trlc_dk1_control/robot.py 的 _server_loop())是"聪明的另一半",每 4 ms 依次执行:

① 重力补偿前馈。 通过 trlc_dk1_control/gravity_comp.py 调用 MuJoCo 的逆动力学,实时算出"抵消重力需要多少力矩",作为 tau_ff 前馈项下发——手臂悬停时不再需要 kp 硬顶重力,姿态更稳、抖动更小。模型直接复用仓库自带的 urdf/follower/TRLC-DK1-Follower.urdf,并有个巧妙技巧:把 URDF 里的 mesh 几何全部替换成 0.001 半径小球,因为逆动力学只关心质量/惯量,不需要真实网格。

TRLC-DK1 从动臂 URDF 模型预览图,用于 MuJoCo 重力补偿计算

② 看门狗:卡住也不松手。 如果超过 command_timeout_s(默认 0.5 s)没收到任何新指令,线程不会把目标重置为当前位置(那会让手臂变软、在末端负载下下垂),而是保持最后一次指令的目标位置,满刚度 + 重力补偿冻结原地——这正是应对扩散策略去噪间隙的关键设计。

③ 三重安全钳制:

  • 位置钳制:目标位置被 clip 到关节限位内,并额外留 0.05 rad 缓冲(LIMIT_BUFFER),且限位与 URDF 的 <limit> 严格同步,避免 MuJoCo 误注入约束力矩;
  • 力矩钳制:重力前馈被 clip 到各电机峰值扭矩以内;
  • 过流保护:连续 20 次采样(约 80 ms)力矩超限即进入"阻尼模式"——kp 归零、前馈归零、只保留 kd 阻尼,相当于主动"松劲"防烧电机,收到新指令后自动恢复。

📡 用户接口:任意频率,非阻塞读写

上层代码完全不需要感知线程存在。接口在 trlc_dk1_control/robot.py,只有两个加锁拷贝的操作:

robot = DK1Robot(DK1_DEFAULT_CONFIG("/dev/ttyACM0"))
robot.connect()

robot.command_joint_pos(q_des)   # 任意 Hz 下发目标位置,非阻塞
state = robot.get_joint_state()  # 任意 Hz 读取最新状态,非阻塞

无论你的推理跑在 10 Hz 还是 300 Hz,command_joint_pos() 只是往共享缓冲区写一份数据(微秒级返回),真正的执行永远由 250 Hz 电机线程按最新指令完成。这也是 LeRobot 集成的入口:lerobot_robot_trlc_dk1/follower.py 中 dk1_follower 的 impedance 控制模式就是直接包装了 DK1Robot,send_action() 一行调用即完成非阻塞下发。

📋 关键参数速查与调参建议

所有可调参数集中在 trlc_dk1_control/config.py 的 DK1RobotConfig:

参数 默认值 说明
motor_thread_hz 250.0 电机线程频率,决定控制平滑度
serial_timeout 0.005 串口超时(秒),必须 ≪ 控制周期
arm_kp / arm_kd [100,100,100,20,20,10] / [5,5,4,1,1,1] 各关节 MIT 增益(协议上限 kp≤500、kd≤5)
command_timeout_s 0.5 看门狗空闲阈值,超时后保持最后目标
overcurrent_threshold 20 连续超限次数,触发阻尼模式
gravity_comp_scale 1.0 重力补偿缩放,建议实测微调
max_gripper_torque_nm 1.0 夹爪力矩上限

调参小贴士: 手臂晃动/震荡 → 降低对应关节 kp 或提高 kd;悬停时轻微下垂 → 调大 gravity_comp_scale 至 1.05 左右;换装末端负载后务必同步更新 URDF 质量,否则重力补偿会"算错账"。

🔚 小结

TRLC-DK1 的双线程架构把"实时 I/O"与"智能决策"解耦:电机线程用 5 ms 超时、MIT+EMIT 分工和 sleep+忙等收尾三个细节守住 250 Hz 的节拍;服务器线程用重力补偿、看门狗和三重安全钳制守住姿态与安全边界;共享缓冲区 + 单锁的设计则让任意频率的用户代码都能非阻塞地"插队"下发指令。想深入源码,建议按 trlc_dk1_control/ 目录顺序读 config.py → gravity_comp.py → motor_chain.py → robot.py,配合 lerobot_robot_trlc_dk1/follower.py 的 LeRobot 封装,你就能完整复现这套"AI 原生"机器人控制栈 🚀

登录后查看全文
trlc-dk1