本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。
第 62 章：腿足硬件栈——驱动器 / 通信总线 / 传感器 / Unitree SDK / ros2_control 硬件接口¶
难度：⭐⭐~⭐⭐⭐ | 前置：Ch31（ROS2 集成）、Ch57（腿足状态估计）、Ch61（实时 C++ 工程与 ros2_control）
核心参考文献： - Katz B., Di Carlo J., Kim S. (2019) "Mini Cheetah: A Platform for Pushing the Limits of Dynamic Quadruped Control", ICRA - Seok S. et al. (2015) "Design Principles for Energy-Efficient Legged Locomotion and Implementation on the MIT Cheetah Robot", IEEE/ASME T-Mech - Wensing P. M. et al. (2017) "Proprioceptive Actuator Design in the MIT Cheetah: Impact Mitigation and High-Bandwidth Physical Interaction for Dynamic Legged Robots", IEEE T-RO - Hutter M. et al. (2016) "ANYmal - a Highly Mobile and Dynamic Quadrupedal Robot", IROS - EtherCAT Technology Group (2023) "EtherCAT — the Ethernet Fieldbus", EtherCAT.org
建议学时：1.5 周（25--30 小时）
前置自测¶
📋 前置自测（答不出 \(\geq 2\) 题 → 先回对应章节复习）
-
[Ch61] PREEMPT_RT 内核的核心作用是什么？为什么普通 Linux 内核不满足腿足 1 kHz 控制的实时性要求？
-
[Ch61] ros2_control 中
SystemInterface的read()和write()方法分别承担什么职责？它们在实时循环中的调用顺序是什么？ -
[Ch61] 实时循环的"四大禁忌"是什么？为什么在 1 kHz 控制循环内调用
malloc会导致灾难性后果？ -
[Ch57] IMU 加速度计测量的是什么物理量？为什么静止时读数不为零？
-
[基础电学] 直流电机的扭矩与电流之间是什么关系？扭矩常数 \(K_t\) 的物理含义？
如果第 5 题答不出——本章涉及电机控制基础，建议先了解 BLDC 电机的基本工作原理。
本章目标¶
学完本章后，你应该能够：
- **画出**腿足机器人完整硬件架构图，说清从计算平台到关节电机的每一层通信路径
- **解释**准直驱（QDD）电机为什么是腿足机器人的主流选择——从力控带宽和反向驱动性的角度
- 理解 FOC（磁场定向控制）的基本原理，知道电流环如何实现精确扭矩控制
- 对比 CAN 总线、EtherCAT、UDP 三种通信方案的带宽、延迟和适用场景
- 使用 Unitree SDK2 的 LowLevel 接口控制 Go2 的 12 个关节电机
- 实现 ros2_control 的
HardwareInterface将 Unitree SDK 接入 ros2_control 框架 - **选择**合适的 IMU、力传感器和编码器，理解各传感器的精度指标和标定方法
62.1 腿足机器人硬件全景 ⭐¶
动机：从算法到电机的完整链路¶
本节解决的问题：腿足机器人从算法计算出一个关节扭矩值到电机实际输出该扭矩，中间经历了哪些环节？每一层的延迟和带宽限制是什么？
当你在 Ch53 的 WBC 中计算出 \(\boldsymbol{\tau}_{\text{WBC}} \in \mathbb{R}^{12}\)，或在 Ch64 的 RL 策略中推理出关节位置目标 \(\boldsymbol{q}_{\text{des}} \in \mathbb{R}^{12}\) 后，这些"数字"要经过一条复杂的链路才能变成真实的物理运动。如果你不理解这条链路中每一层的工作方式和延迟特性，就无法调试真机上出现的种种问题——"为什么仿真中完美的控制器在真机上就抖动？""为什么电机偶尔不响应？""为什么关节温度升高后扭矩就不准了？"
这些问题的答案都藏在硬件栈里。
控制循环层次结构¶
腿足机器人的控制循环不是一个单一循环，而是**多层嵌套**的层次结构——好比交响乐团的组织架构：指挥（规划层）给出乐章结构和节奏意图，声部首席（MPC 层）将其细化为乐句的强弱和速度，每位演奏者（电机伺服层）则以极高频率精确执行每一个音符。每一层有不同的频率、不同的延迟要求、不同的计算平台：
┌────────────────────────────────────────────────────────┐
│ 规划层 (Planning) │
│ 频率: 1-10 Hz | 延迟容忍: 100-1000 ms │
│ 内容: 步态规划、路径规划、行为决策 │
│ 平台: CPU (非实时) │
└───────────────────────┬────────────────────────────────┘
│ 参考轨迹 / 步态相位
▼
┌────────────────────────────────────────────────────────┐
│ MPC 层 (Model Predictive Control) │
│ 频率: 20-100 Hz | 延迟容忍: 10-50 ms (软实时) │
│ 内容: 预测未来 0.5-1s 轨迹, 优化 GRF │
│ 平台: CPU (PREEMPT_RT 推荐) │
└───────────────────────┬────────────────────────────────┘
│ 期望 GRF / 质心轨迹
▼
┌────────────────────────────────────────────────────────┐
│ WBC 层 (Whole-Body Control) │
│ 频率: 200-1000 Hz | 延迟容忍: 1-5 ms (硬实时) │
│ 内容: 逆动力学, 约束 QP, 输出关节扭矩 │
│ 平台: CPU (PREEMPT_RT, isolated CPU) │
└───────────────────────┬────────────────────────────────┘
│ τ_des / q_des / kp / kd
▼
┌────────────────────────────────────────────────────────┐
│ 通信层 (Communication Bus) │
│ 延迟: 10-500 μs | 协议: CAN / EtherCAT / UDP │
│ 内容: 打包指令发送、接收反馈 │
│ 平台: 主控板 + 总线控制器 │
└───────────────────────┬────────────────────────────────┘
│ 电机指令帧
▼
┌────────────────────────────────────────────────────────┐
│ 电机伺服层 (Motor Servo) │
│ 频率: 10-40 kHz | 延迟: < 100 μs │
│ 内容: FOC 电流环, PD 位置环, 安全保护 │
│ 平台: 电机驱动器 MCU (STM32 / 专用 DSP) │
└───────────────────────┬────────────────────────────────┘
│ PWM 信号
▼
┌────────────────────────────────────────────────────────┐
│ 物理层 (Physical Plant) │
│ BLDC 电机 + 行星齿轮 + 关节结构 │
│ 编码器 + 电流传感器 → 反馈回伺服层 │
└────────────────────────────────────────────────────────┘
以 Unitree Go2 为例的硬件全景¶
Unitree Go2 是目前学术界和工业界常用的四足平台之一。下表用于说明主流消费级四足的典型硬件分层；具体计算板、电机内部结构和传感器配置会随 Air/Pro/EDU、批次和固件版本变化，上机前必须以所持版本的官方资料、SDK 消息和实物标定为准。
| 层次 | Go2 具体实现 | 说明 |
|---|---|---|
| 计算平台 | EDU 常见 NVIDIA Jetson Orin NX / 其他版本定制主板 | 算力和可开放接口随版本变化 |
| 中间件 | CycloneDDS (unitree_sdk2) | DDS 是 ROS2 默认中间件的同族协议 |
| 主控板 | 专用 Motion Control Board | 负责电机协调、IMU 数据融合、安全保护 |
| 通信总线 | CAN 总线 (4 条，每腿 1 条) | 每条 CAN 连接 3 个电机驱动器 |
| 关节电机 | QDD BLDC 外转子电机 | 槽极数、绕组和驱动细节以具体拆解/规格为准；Go2 常见减速比约 6.22:1 |
| 关节编码器 | 绝对式磁编码器 | 通常集成在电机模组内；安装位置和字段含义需以实物/SDK 为准 |
| IMU | MEMS IMU（集成在主控板） | 6 轴（加速度计 + 陀螺仪） |
| 足端传感器 | SDK 暴露 foot_force / foot_force_est 字段 |
不同版本可能来自足端传感器或估计量，不能不经标定直接当作精确力传感器 |
| 相机 | 双目立体 + 鱼眼 | 感知层，非本章重点 |
四条腿 \(\times\) 3 DOF/腿 = 12 个电机：
- 髋侧摆关节（hip abduction/adduction）：控制腿的左右摆动
- 髋前后关节（hip flexion/extension）：控制腿的前后摆动
- 膝关节（knee flexion/extension）：控制小腿的弯曲
如果不理解硬件栈会怎样¶
场景 1：仿真完美但真机抖动
你在 MuJoCo 中调好了 WBC 的 PD 增益，机器人走得很稳。部署到 Go2 真机时，关节剧烈抖动。原因：真实电机有 ~2 ms 的通信延迟和 FOC 环路延迟（仿真是零延迟），PD 增益需要降低 30-50% 来补偿这个延迟。如果你不知道硬件栈中存在这些延迟，就会在 PD 调参上浪费大量时间。
场景 2：机器人偶尔"失控"一下
控制器大部分时间工作正常，但偶尔机器人会突然抖一下或者某条腿松软一下。原因可能是 CAN 总线丢帧——当总线负载过高时，低优先级的消息可能延迟甚至丢失。如果你不理解 CAN 的优先级仲裁机制，根本不知道该从哪里排查。
场景 3：电机过热保护触发
长时间站立时电机温度升高，突然进入过热保护降额模式，机器人随即倒下。原因：准直驱电机的静态负载全靠电流维持（不像谐波减速器有自锁），站立时持续消耗大量电流产生热量。理解电机热模型后，可以通过优化站立姿态（让关节接近力矩最小构型）来减少发热。
历史演进¶
| 年代 | 代表平台 | 通信 | 电机类型 | 关键特点 |
|---|---|---|---|---|
| 2005 | BigDog (Boston Dynamics) | 专用液压控制 | 液压驱动 | 液压力控，功率密度极高但效率低 |
| 2013 | MIT Cheetah 1 | 自研 SPI 板 | 自研 QDD | 首次证明电动 QDD 可做高动态运动 |
| 2016 | ANYmal (ETH) | EtherCAT | SEA (ANYdrive) | 工业级 EtherCAT，串联弹性驱动 |
| 2018 | MIT Mini Cheetah | 自研 SPI 板 | 自研 QDD | 9 kg 做后空翻，QDD 路线确立 |
| 2019 | Unitree A1 | CAN | 商用 QDD | 消费级四足的开创者 |
| 2023 | Unitree Go2 | CAN + DDS | 改进 QDD | SDK2 开放生态，学术界大量采用 |
| 2024 | Unitree B2 / G1 | CAN / EtherCAT | 高功率 QDD | 工业/人形平台 |
⚠️ 概念误区：认为"硬件栈只是工程问题，不需要理论"
新手想法："我只要调好算法，硬件层面的东西交给硬件工程师就好。"
实际上：腿足控制的算法设计**深度依赖**硬件特性。WBC 的 QP 约束中包含关节扭矩上限（来自电机规格）、关节速度上限（来自减速比和电机 KV 值）；MPC 的预测模型需要考虑电机动力学延迟；RL 策略的 observation space 包含电机温度和电流等硬件信号。不理解硬件的算法工程师写出来的控制器，永远只能在仿真里跑。
正确认知：硬件栈是算法部署的"最后一公里"。MIT Cheetah 团队在其 2019 ICRA 论文中明确指出："The actuator design is not separable from the control design"（驱动器设计和控制设计不可分割）。
🧠 思维陷阱：认为"仿真和真机的差距主要来自动力学建模不准"
新手想法："Sim-to-Real gap 是因为 MuJoCo/Isaac Gym 的物理引擎不够准确。"
实际上：Sim-to-Real gap 的最大来源之一是**通信延迟和电机动力学**。真实系统中：(1) 通信有 1-5 ms 延迟；(2) 电机 FOC 环不是理想的扭矩源——有带宽限制、扭矩波纹、温度依赖的 \(K_t\) 变化；(3) 齿轮有背隙和摩擦。这些在仿真中通常被忽略或大幅简化。
正确思维：提升 Sim-to-Real 迁移效果，既要改善物理仿真精度，也要改善硬件模型——用 actuator network（Ch64）来学习真实电机的输入输出映射就是一种方案。
练习¶
练习 62.1.1：画出你自己实验室中一台腿足机器人（或你计划使用的平台）的完整硬件架构图。标注每一层的通信协议、频率和延迟。如果没有真机，以 Unitree Go2 为例，参考本节的架构图。
练习 62.1.2：估算从 WBC 输出 \(\boldsymbol{\tau}_{\text{des}}\) 到电机实际输出该扭矩的**总端到端延迟**。假设：WBC 计算 0.5 ms，CAN 通信 1 ms，FOC 电流环 0.1 ms（40 kHz 频率下约 1 个周期）。总延迟对 1 kHz 控制循环意味着什么？PD 增益需要如何调整？
练习 62.1.3（思考题）：为什么 Boston Dynamics 早期选择液压驱动而非电驱动？液压的功率密度优势在什么条件下被电动 QDD 超越？提示：考虑系统效率、控制带宽和维护成本。
62.2 驱动器与电机 ⭐⭐¶
动机：为什么腿足机器人不用工业机械臂的电机¶
本节解决的问题：工业机械臂和腿足机器人都需要精确的关节控制，但为什么它们使用完全不同类型的电机？"准直驱"电机的核心设计思想是什么？它如何实现精确力控和高速运动？
工业机械臂（如 KUKA、ABB）使用的电机方案是**高速小扭矩电机 + 谐波减速器（减速比 100:1-160:1）。这种方案的位置精度极高（重复定位 \(\pm 0.02\) mm），但有一个致命缺陷：**减速器的高摩擦和高惯量使得反向驱动极其困难。当外力作用于关节时（比如腿着地时的冲击力），电机几乎感受不到——因为减速器把力"隔离"了。
对于腿足机器人，这是灾难性的。腿足运动的本质是**与环境持续进行力交互**——每一步着地都是一次冲击，机器人必须能感知这个冲击力并做出反应。如果电机"感受不到"外力，力控就无法实现。
如果用谐波减速器会怎样¶
假设你把 KUKA 的谐波减速器电机装到一台四足机器人上：
- 力控精度极差：谐波减速器的效率约 80%，反向效率更低（~50%）。通过电流估计的扭矩误差可达 30-50%。WBC 计算出 \(\tau = 10\) Nm，实际输出可能是 5-15 Nm——力控基本不可用
- 着地冲击损坏减速器：腿足着地的冲击载荷是稳态的 3-5 倍。谐波减速器的柔轮（Flexspline）在反复冲击下容易疲劳断裂
- 不可反向驱动：机器人被推倒时，关节无法随外力旋转（自锁），导致关节和结构件承受全部冲击——结构损坏风险极高
历史：从液压到串联弹性到准直驱¶
腿足机器人的驱动方案经历了三个主要阶段：
阶段 1：液压驱动（2000s-2010s）
Boston Dynamics 的 BigDog（2005）和 Atlas 早期版本使用液压驱动。液压的功率密度极高（~2 kW/kg），远超当时的电机。但液压系统噪声大、效率低（通常 <30%）、维护困难、需要外部液压泵。这限制了液压驱动在实验室之外的应用。
阶段 2：串联弹性驱动器 SEA（2010s）
Pratt 和 Williamson（1995）提出的 Series Elastic Actuator（SEA）在电机输出端串联一个弹性元件（弹簧）。弹簧的变形量通过编码器测量，根据胡克定律 \(F = k \cdot \Delta x\) 精确计算输出力。ETH Zurich 的 ANYmal（2016）使用自研的 ANYdrive SEA，实现了高精度的力控。
SEA 的优势： - 力测量精度高（通过弹簧形变测量，不依赖电流） - 被动抗冲击（弹簧吸收着地冲击能量，类似人类的跟腱 Achilles tendon） - 能量存储和释放（跑步时弹簧储存动能并在蹬地时释放，研究表明弹簧可贡献 ~50% 的跑步机械功）
SEA 的劣势： - 力控带宽受限——弹簧引入了一个共振频率，高于该频率的力控指令被衰减 - 机械复杂度高——额外的弹簧、弹簧壳体和形变传感器增加了重量和成本 - 弹簧刚度选择困难——刚度太低响应慢，刚度太高失去弹性优势
阶段 3：准直驱 QDD（2013-至今）
MIT Cheetah 团队（Seok et al., 2015; Wensing et al., 2017）提出了一条完全不同的技术路线：用大扭矩密度的 BLDC 电机 + 极低减速比（5-10:1）的行星齿轮，直接实现力控。这就是准直驱（Quasi-Direct-Drive, QDD）。
QDD 的核心设计原理¶
QDD 的设计哲学是**让电机尽可能"透明"——外部施加的力能几乎无损地传递到电机轴，电机输出的力也能几乎无损地传递到关节。这种"透明度"在力控术语中称为**反向驱动性（Backdrivability）。
反向驱动性的物理含义：
考虑一个齿轮传动系统。设电机侧扭矩为 \(\tau_m\)，关节侧扭矩为 \(\tau_j\)，减速比为 \(N\)，传动效率为 \(\eta\)：
正向传动（电机驱动关节）： $\(\tau_j = N \cdot \eta \cdot \tau_m\)$
反向传动（外力驱动电机）： $\(\tau_m = \frac{\tau_j}{N \cdot \eta_{\text{back}}}\)$
其中 \(\eta_{\text{back}}\) 是反向效率。对于高减速比系统：
| 参数 | 谐波减速器 (N=100) | QDD 行星齿轮 (N=6) |
|---|---|---|
| 正向效率 \(\eta\) | ~80% | ~95% |
| 反向效率 \(\eta_{\text{back}}\) | ~30-50% (甚至自锁) | ~90% |
| 等效关节侧惯量 \(I_{\text{eff}} = N^2 I_m\) | 10000 \(I_m\) | 36 \(I_m\) |
| 反向驱动力 \(\tau_{\text{backdrive}}\) | 极大 | 极小 |
关键公式：等效关节侧转动惯量 \(I_{\text{eff}} = N^2 I_m\)（反映惯量 reflected inertia）。减速比 \(N\) 的平方意味着：100:1 减速器让等效惯量放大 10000 倍！这就是高减速比系统难以反向驱动的根本原因——要推动关节，外力需要"翻越"巨大的等效惯量。
QDD 的 \(N = 6\) 意味着等效惯量只放大 36 倍——这足够低，使得外部冲击力可以自然地传递到电机。电机侧的电流传感器能直接感受到这个力，实现**本体感受（Proprioceptive）力控**。
Unitree Go2 的电机设计¶
Unitree Go2 的关节电机可按以下方式理解。表中的内部结构来自公开拆解和常见版本经验，用于教学建模；不应当写进控制器作为跨版本硬编码事实：
| 参数 | Go2 电机规格 |
|---|---|
| 电机类型 | BLDC 外转子 QDD 模组 |
| 绕线方式 | 以具体电机版本为准 |
| 减速器类型 | 单级行星齿轮 |
| 减速比 | ~6.22:1 |
| 峰值关节扭矩 | 45 Nm |
| 编码器类型 | 绝对式磁编码器 |
| 电流传感器 | 三相电流检测 |
| 控制 MCU | 以具体驱动板版本为准 |
为什么用外转子（Outrunner）设计：外转子电机的转子在外、定子在内，相比内转子电机具有更大的磁极半径。根据扭矩公式 \(\tau = B \cdot I \cdot L \cdot r\)（\(B\) 磁场强度，\(I\) 电流，\(L\) 导体长度，\(r\) 半径），更大的半径意味着**相同电流下更大的扭矩**。这正是腿足机器人需要的——大扭矩密度。
主流 QDD 电机对比¶
| 电机 | 减速比 | 峰值扭矩 | 峰值扭矩密度 | 用于 |
|---|---|---|---|---|
| MIT Mini Cheetah 自研 | 6:1 | 约 17 Nm 级别 | 高扭矩密度 | MIT Mini Cheetah |
| 大型 MIT Cheetah 平台执行器 | 低减速比 QDD | 百 Nm 级别 | 高扭矩密度 | Cheetah 3 等大型平台 |
| Unitree Go2 内置 | 6.22:1 | 45 Nm | — | Unitree Go2 |
| T-Motor/CubeMars AK80-6 | 6:1 | 12 Nm (峰值) | ~24.7 Nm/kg (电机+减速器 0.485 kg) | 开源四足 (Solo12) |
| T-Motor AKE90-8 | 8:1 | — | ~121 Nm/kg | 高性能四足/人形 |
| ANYdrive (SEA) | 50:1 | 40 Nm | — | ANYmal C/D |
| ODRI/Solo 自研 | 9:1 | ~2.7 Nm | — | 开源研究平台 |
⚠️ 编程陷阱：混淆电机侧扭矩和关节侧扭矩
错误做法：SDK 返回
tau_est = 2.0，你以为关节侧扭矩是 2.0 Nm，直接用于 WBC 的逆动力学计算。现象：动力学方程求解出的接触力总是偏小或偏大，控制器表现异常。
根本原因：Unitree SDK 的
tau_est字段返回的是**关节侧**扭矩（已经考虑了减速比），但如果你使用其他电机驱动器（如 T-Motor AK 系列），返回值可能是**电机侧**扭矩，需要乘以减速比 \(N\) 才是关节侧扭矩。正确做法：阅读驱动器文档，明确
tau字段的定义。在代码中显式标注：double tau_joint = driver.getTorque(); // Check: motor-side or joint-side? // If motor-side: tau_joint = tau_motor * gear_ratio * efficiency;💡 概念误区：认为"减速比越低越好"
新手想法："QDD 用 6:1 减速比效果好，那直驱（1:1）不是更好？"
实际上：纯直驱（Direct Drive, DD）确实有最佳的反向驱动性，但代价是**扭矩太小**。以 Go2 为例，其电机在 6.22:1 减速下峰值扭矩为 45 Nm。如果去掉减速器，峰值扭矩只有 ~7 Nm——这不足以支撑一只腿站立。
设计权衡：减速比 \(N\) 是一个关键的设计权衡参数。\(N\) 增大：扭矩 \(\uparrow\)，惯量 \(N^2 \uparrow\)，反向驱动性 \(\downarrow\)。MIT Cheetah 团队通过系统分析（Seok 2015）确定了 6:1 是"力控带宽"和"扭矩能力"之间的最佳折衷点。
驱动方案选型决策¶
| 驱动方式 | 功率密度 | 力控带宽 | 静止功耗 | 成本 | 适用场景 |
|---|---|---|---|---|---|
| 液压 | 极高 | 低 | 近零 | 高 | 重载/低速（建筑机器人、军事后勤） |
| 高减速比电机 | 中 | 极低 | 中 | 低 | 位控应用（工业机械臂） |
| QDD（准直驱） | 中高 | 高 | 中 | 中 | 高动态足式（四足/人形跑跳） |
| SEA（串联弹性） | 中 | 中 | 中 | 中高 | 需被动柔顺（人机交互、步行） |
| 电液混合（静液支撑） | 高 | 中 | 低 | 高 | 大负载+长续航（野外巡检、搬运） |
💡 前沿方向：电液混合驱动
电液混合路线结合了液压的"静止时低功耗"特性和电机的"高动态响应"优势。典型应用场景是大负载四足的野外长距离运动——纯电驱动在持续负重站立时持续消耗电能维持关节力矩，而静液支撑可以通过液压锁定实现近零静态功耗。浙江大学等团队已在该方向取得进展。
参考文献：Load-Carrying Assistance of Articulated Legged Robots Based on Hydrostatic Support
⚠️ 严谨说明：液压/静液支撑可显著降低维持载荷的能耗，但泵、泄漏、阀控损耗仍然存在，并非字面意义上的"零功耗"。
💡 硬件设计→控制假设的因果链
四足机器人普遍采用**膝电机近端布置**（将膝关节电机安装在髋部，通过连杆传动）而非直接安装在膝关节上。这一设计决策的核心目的是降低腿部转动惯量，使得： 1. 腿部质量集中在躯干附近，SRBD（单刚体动力学）的"质量集中在躯干"假设更好地成立 2. Centroidal 动力学中忽略关节运动引起的惯性张量变化（\(\dot{I}_G \approx 0\)）更加合理 3. 摆动腿的惯性力对躯干的扰动更小，MPC 预测更准确
换言之，膝电机近端布置不仅是机械优化，更是让简化控制模型成立的前提条件。如果腿部惯量过大（如电机直接装在膝关节），SRBD 模型精度会显著下降，MPC 控制效果变差。
练习¶
练习 62.2.1：计算一个 QDD 电机（\(N = 6\)，电机转子惯量 \(I_m = 0.001\) kg\(\cdot\)m\(^2\)）和一个谐波减速器电机（\(N = 100\)，\(I_m = 0.0001\) kg\(\cdot\)m\(^2\)）的等效关节侧惯量。讨论：为什么虽然谐波减速器电机本身更小更轻（\(I_m\) 更小），但等效关节侧惯量反而更大？
练习 62.2.2：ANYmal 选择了 50:1 减速比的 SEA，而 MIT Cheetah 选择了 6:1 的 QDD。从以下三个维度比较这两种方案的权衡：(a) 力控精度 (b) 最大关节速度 (c) 抗冲击能力。如果你要设计一个新的四足机器人，你会选择哪种方案？为什么？
练习 62.2.3（编程）：写一个 Python 脚本，绘制减速比 \(N\) 从 1 到 100 变化时以下量的变化曲线：(a) 关节侧扭矩 \(\tau_j = N \cdot \eta \cdot K_t \cdot I_{\max}\)（假设 \(\eta = 0.95 - 0.005N\)，\(K_t = 0.1\) Nm/A，\(I_{\max} = 20\) A）(b) 等效关节侧惯量 \(I_{\text{eff}} = N^2 I_m\)（\(I_m = 0.001\)）(c) 力控带宽的近似值 \(\omega_{\text{bw}} = \sqrt{K_p / I_{\text{eff}}}\)（\(K_p = 100\)）。找到三者最优折衷的 \(N\) 值范围。
62.3 电机控制基础 ⭐⭐¶
动机：从"给电机一个扭矩指令"到"电机实际输出该扭矩"¶
本节解决的问题：当你在 WBC 中算出 \(\tau = 10\) Nm 并发送给电机驱动器时，驱动器内部发生了什么？它是如何把一个"扭矩数字"变成三相电流信号来驱动 BLDC 电机旋转的？这个过程的带宽和延迟是多少？
这是腿足硬件栈中对算法工程师来说最"黑箱"的一层。大多数人把电机驱动器当作一个理想的"扭矩源"——输入扭矩指令，输出精确扭矩。但真实的驱动器有带宽限制、扭矩波纹、温度漂移等非理想特性。理解这些特性对调试真机控制器至关重要。
如果把电机当作理想扭矩源会怎样¶
场景：你在 WBC 中设计了一个 200 Hz 带宽的力控制器（即扭矩指令中包含 200 Hz 的分量）。在仿真中一切正常——因为仿真器中电机是理想扭矩源。部署到真机后：
- 电机 FOC 电流环的带宽约 2-4 kHz
- 但从 WBC 发出指令到电机执行的总链路延迟约 2-3 ms
- 2-3 ms 纯延迟不会形成“截止频率”，但会在 200 Hz 处引入约 \(144^\circ\)--\(216^\circ\) 的相位滞后
你的 200 Hz 力控指令会因为链路延迟消耗掉大部分相位裕度，即使电流环本身足够快，也可能在闭环中振荡。解决方案：将外层力控带宽限制在通信延迟允许的范围内，或使用 Smith predictor、相位超前/前馈补偿等延迟补偿，并通过实机频响测试验证。
FOC（磁场定向控制）的基本原理¶
为什么需要 FOC：
BLDC（Brushless DC）电机有三个相位绕组（U/V/W）。要产生恒定扭矩，需要让三相电流**精确跟踪转子的旋转位置**。这就像在旋转木马上推一个人——你必须在恰当的位置和方向施力，否则用力无效甚至反作用。
FOC 的核心思想是**坐标变换**——把实验室坐标系（三相 UVW）中复杂的时变交流控制问题，变换到**与转子同步旋转**的坐标系（d-q 坐标系）中的简单直流控制问题。
FOC 的核心步骤：
- Clark 变换（3 相 → \(\alpha\beta\) 坐标系）：把三相电流 \((i_U, i_V, i_W)\) 变换为两个正交分量 \((i_\alpha, i_\beta)\)
- Park 变换（\(\alpha\beta\) → \(dq\) 坐标系）：用转子角度 \(\theta_e\)（电角度）旋转到与转子同步的坐标系
- 在 \(dq\) 坐标系中做 PI 控制：
- \(i_d\) 是磁通分量（flux）：通常设为 0（MTPA 控制，最大扭矩电流比）
- \(i_q\) 是扭矩分量（torque）：直接正比于输出扭矩
其中 \(p\) 是极对数，\(\psi_m\) 是永磁体磁链，\(K_t\) 是扭矩常数。
- 逆 Park 变换 + 逆 Clark 变换：把 PI 控制器输出的 \((v_d, v_q)\) 变回三相电压 \((v_U, v_V, v_W)\)
- SVPWM（空间矢量 PWM）：生成实际的 PWM 信号驱动功率管
关键理解：通过 FOC，控制 BLDC 电机的扭矩**等价于控制 \(i_q\) 电流**。这是一个经典的 PI 控制问题，带宽可达 2-10 kHz。这就是为什么 QDD 电机可以做精确力控——控制电流就是控制扭矩。
电机驱动器内部的控制环路¶
一个典型的 QDD 电机驱动器内部有**三层嵌套控制环**：
外环: 位置/扭矩环 (由用户指令驱动, ~1 kHz)
│
│ ┌─── 用户可配置 ───┐
│ │ τ_des = kp*(q_des - q) + kd*(dq_des - dq) + τ_ff │
│ └──────────────────┘
│ │
│ ▼ i_q_des = τ_des / K_t
│
├── 中环: 电流环 / FOC (驱动器固件, ~20-40 kHz)
│ │
│ │ PI controller: i_q → v_q → SVPWM → PWM duty
│ │
│ └── 内环: PWM + 电流采样 (~40-100 kHz)
│ │
│ └── 功率管 H桥驱动 → BLDC 三相绕组
│
└── 反馈: 编码器(位置) + 电流传感器(三相电流)
对于腿足控制，关键理解是：
- 用户（WBC/RL）操作的是最外环——发送 \((q_{\text{des}}, \dot{q}_{\text{des}}, k_p, k_d, \tau_{\text{ff}})\) 五个参数
- 电流环（FOC）是驱动器固件实现的——用户通常不能修改，也不需要修改
- 扭矩控制 = 设 \(k_p = k_d = 0\)，只发 \(\tau_{\text{ff}}\)：此时外环退化为直接的扭矩指令 \(i_q = \tau_{\text{ff}} / K_t\)
Unitree SDK 中的关节控制命令完美体现了这个三层结构：
// Unitree SDK2: joint command structure
cmd.motor_cmd()[i].q() = q_des; // Position target
cmd.motor_cmd()[i].dq() = dq_des; // Velocity target
cmd.motor_cmd()[i].kp() = kp; // Position gain
cmd.motor_cmd()[i].kd() = kd; // Velocity gain (damping)
cmd.motor_cmd()[i].tau() = tau_ff; // Feedforward torque
// WBC pure torque mode:
cmd.motor_cmd()[i].q() = 0.0; // Ignored when kp=0
cmd.motor_cmd()[i].dq() = 0.0; // Ignored when kd=0
cmd.motor_cmd()[i].kp() = 0.0; // No position tracking
cmd.motor_cmd()[i].kd() = 0.0; // No damping
cmd.motor_cmd()[i].tau() = tau_wbc; // Direct torque command
扭矩常数 \(K_t\) 的温度依赖性¶
\(K_t\) 不是一个恒定值。永磁体的磁场强度随温度升高而下降（钕铁硼磁体的温度系数约 \(-0.12\%/°C\)）。这意味着：
| 温度 | \(K_t\) 变化 | 对 10 Nm 指令的实际输出 |
|---|---|---|
| 25°C（标称） | 基准 | 10.0 Nm |
| 60°C | \(-4.2\%\) | 9.58 Nm |
| 80°C | \(-6.6\%\) | 9.34 Nm |
| 100°C（过热保护附近） | \(-9.0\%\) | 9.10 Nm |
工程影响：长时间运动后电机温度升至 80°C，WBC 命令的 10 Nm 实际只输出 9.34 Nm——这 6.6% 的误差足以让状态估计器（Ch57）计算的接触力出现偏差。高性能系统需要**在线温度补偿**：\(\tau_{\text{cmd}} = \tau_{\text{des}} / (1 - \alpha_T \cdot \Delta T)\)。
⚠️ 编程陷阱：WBC 纯扭矩模式下不设阻尼
错误做法：WBC 输出扭矩时设 \(k_p = 0, k_d = 0\)，纯依赖 \(\tau_{\text{ff}}\)。
现象：关节在不受力时自由摆动，WBC 命令的微小误差会导致关节振荡。如果 WBC 计算失败（返回零扭矩），关节完全失去控制。
根本原因：纯扭矩模式下没有任何低层阻尼来抑制振荡或提供安全保护。
正确做法：即使在 WBC 模式下，也保留一个小的 \(k_d\)（如 0.5-2.0）作为关节阻尼。这不会显著影响 WBC 的力控精度，但能提供关键的稳定性裕度：
cmd.motor_cmd()[i].kp() = 0.0; cmd.motor_cmd()[i].kd() = 1.0; // Small damping for stability cmd.motor_cmd()[i].tau() = tau_wbc;💡 概念误区：认为"FOC 电流环和 WBC 频率一样"
新手想法："WBC 跑 1 kHz，电机也是 1 kHz 更新，所以两者频率匹配。"
实际上：FOC 电流环跑在 20-40 kHz（是 WBC 的 20-40 倍），每个 WBC 周期内电流环已经执行了 20-40 次迭代。这意味着电流环的响应远快于 WBC 的更新——从 WBC 的视角看，电机确实近似一个"理想扭矩源"。但从通信链路的视角看，WBC 的指令要经过 CAN/DDS 传输才能到达驱动器，这个延迟（~1-2 ms）是主要瓶颈。
关键洞察：真正限制力控带宽的不是 FOC 环，而是**通信延迟**。
练习¶
练习 62.3.1：解释为什么 FOC 的 Park 变换本质上是一个"坐标旋转"。提示：\(dq\) 坐标系是以电角度 \(\theta_e\) 旋转的。如果转子有 \(p\) 个极对，机械角度 \(\theta_m\) 和电角度 \(\theta_e\) 之间是什么关系？Go2 的 42 极电机有多少极对？
练习 62.3.2：一个 QDD 电机的扭矩常数 \(K_t = 0.08\) Nm/A，减速比 \(N = 6\)，减速器效率 \(\eta = 0.95\)。如果最大电流 \(I_{\max} = 30\) A：(a) 关节侧峰值扭矩是多少？(b) 如果温度从 25°C 升到 80°C（\(K_t\) 下降 6.6%），峰值扭矩变为多少？(c) 为维持 45 Nm 关节扭矩，需要多大的电流？电机热损耗 \(P = I^2 R\)（\(R = 0.1\ \Omega\)）是多少？
练习 62.3.3（思考题）：RL 策略部署到真机时，有些团队选择在驱动器层面保留 \(k_p, k_d > 0\)（PD + feedforward 模式），而不是纯扭矩模式。讨论这种做法的优缺点。从 Sim-to-Real 的角度，哪种模式更容易迁移？
62.4 通信总线 ⭐⭐¶
动机：指令怎样从计算机到达电机¶
本节解决的问题：WBC/RL 的输出是一个 12 维向量（12 个关节的扭矩/位置指令），这些数字如何**可靠地、低延迟地**从计算平台传送到 12 个分布在机器人四肢的电机驱动器？
通信总线是硬件栈中**连接软件与硬件的关键桥梁**。选择错误的总线或配置不当，会导致控制延迟不确定、丢帧、同步误差——这些问题在仿真中完全不存在，但在真机上是常见的故障源。
如果通信不可靠会怎样¶
场景：CAN 总线因为接触不良，偶尔丢失一帧电机反馈。状态估计器收到的关节角度突然跳变（上一帧的旧值 → 丢帧 → 下一帧的新值，中间缺了一帧），导致数值微分计算的关节速度出现尖峰。这个速度尖峰传播到 WBC 的阻尼项 \(k_d \cdot \dot{q}\)，产生一个巨大的扭矩脉冲——机器人"抖"了一下。
三种主流通信方案对比¶
| 特性 | CAN 总线 | EtherCAT | UDP (Unitree DDS) |
|---|---|---|---|
| 物理层 | 差分信号线 (CAN_H/CAN_L) | 100BASE-TX 以太网 | 标准以太网 |
| 带宽 | 1 Mbps (经典) / 5 Mbps (CAN-FD) | 100 Mbps | 100 Mbps - 1 Gbps |
| 拓扑 | 总线型（多主） | 菊花链（主从） | 星型/点对点 |
| 确定性 | 中（优先级仲裁） | 高（主站轮询） | 低（无确定性保证） |
| 典型周期 | 1-10 ms | 0.05-1 ms | 1-10 ms |
| 最大节点数 | ~127 | 65535 | 无限制 |
| 成本 | 低 | 中-高 | 低 |
| 代表平台 | Unitree Go2/A1, Solo12 | ANYmal, Talos, iCub | Unitree SDK2 (上层) |
CAN 总线详解¶
**CAN（Controller Area Network）**由 Bosch 于 1986 年为汽车行业发明。它的设计目标是在恶劣的电磁环境中提供**可靠的**多节点通信。
CAN 的工作原理：
- 差分信号：CAN_H 和 CAN_L 两根线的电压差表示数据。差分信号对共模噪声有强抑制能力，适合电机驱动器附近的电磁干扰环境
- 非破坏性仲裁：多个节点同时发送时，ID 值较小的消息优先（ID 中的 0 电平"赢"过 1 电平）。输了仲裁的节点自动退让，不需要重发
- CRC 校验：每帧数据附带 15 位 CRC，检测传输错误
CAN 帧格式：
┌─────┬──────────┬─────┬──────────────────┬──────┬──────┬──────┐
│ SOF │ ID(11b) │ RTR │ Data (0-8 bytes) │ CRC │ ACK │ EOF │
└─────┴──────────┴─────┴──────────────────┘──────┴──────┴──────┘
经典 CAN 的数据段最大 8 字节。一个电机指令通常包含 \((q_{\text{des}}, \dot{q}_{\text{des}}, k_p, k_d, \tau_{\text{ff}})\) 五个参数，每个用 16 位定点数编码——刚好 10 字节，需要用 CAN-FD（64 字节数据段）或拆成两帧。
腿足机器人的 CAN 总线配置：
主控板 MCU
├── CAN0 ─── 电机0(Hip_Ab) ─── 电机1(Hip_Fe) ─── 电机2(Knee) [前左腿]
├── CAN1 ─── 电机3(Hip_Ab) ─── 电机4(Hip_Fe) ─── 电机5(Knee) [前右腿]
├── CAN2 ─── 电机6(Hip_Ab) ─── 电机7(Hip_Fe) ─── 电机8(Knee) [后左腿]
└── CAN3 ─── 电机9(Hip_Ab) ─── 电机10(Hip_Fe)─── 电机11(Knee)[后右腿]
每条 CAN 总线连接 3 个电机。在 1 Mbps 带宽下，每帧（含开销）约 130 bits，3 个电机的指令 + 反馈共 6 帧 \(\times\) 130 bits = 780 bits，理论耗时 0.78 ms。加上仲裁和总线空闲时间，实际约 1-2 ms——满足 1 kHz 控制频率的要求。
Linux SocketCAN 编程：
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <unistd.h>
#include <algorithm>
#include <cstring>
class CANInterface {
public:
bool init(const std::string& interface_name) {
// Create raw CAN socket
sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (sock_ < 0) return false;
// Bind to specific interface (e.g., "can0")
struct ifreq ifr;
std::strncpy(ifr.ifr_name, interface_name.c_str(), IFNAMSIZ);
ioctl(sock_, SIOCGIFINDEX, &ifr);
struct sockaddr_can addr;
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
bind(sock_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
return true;
}
bool sendMotorCommand(uint32_t motor_id,
float q_des, float dq_des,
float kp, float kd, float tau_ff) {
struct can_frame frame;
frame.can_id = 0x100 + motor_id; // Command CAN ID
frame.can_dlc = 8;
// Classic CAN has only 8 payload bytes. This example uses 16-bit
// q/dq/tau and coarse 8-bit kp/kd. If kp/kd need full precision,
// send them in another frame, configure them out-of-band, or use
// CAN-FD with a larger payload.
int16_t q_raw = static_cast<int16_t>(q_des * 1000.0f);
int16_t dq_raw = static_cast<int16_t>(dq_des * 100.0f);
int16_t tau_raw = static_cast<int16_t>(tau_ff * 100.0f);
std::memcpy(&frame.data[0], &q_raw, sizeof(q_raw));
std::memcpy(&frame.data[2], &dq_raw, sizeof(dq_raw));
std::memcpy(&frame.data[4], &tau_raw, sizeof(tau_raw));
frame.data[6] = static_cast<uint8_t>(std::clamp(kp, 0.0f, 255.0f));
frame.data[7] = static_cast<uint8_t>(std::clamp(kd, 0.0f, 255.0f));
return write(sock_, &frame, sizeof(frame)) == sizeof(frame);
}
private:
int sock_ = -1;
};
Unitree Go2 的通信架构¶
Unitree Go2 的通信架构比纯 CAN 更复杂，它是一个**分层通信系统**：
┌──────────────────────────────────┐
│ 用户计算平台 (Jetson Orin NX) │
│ unitree_sdk2 / ROS2 │
└────────────┬─────────────────────┘
│ CycloneDDS (UDP multicast)
│ 以太网 / WiFi
▼
┌──────────────────────────────────┐
│ 主控板 (Motion Control Board) │
│ 低级别安全控制 + 电机协调 │
└────────────┬─────────────────────┘
│ CAN 总线 (4 条)
▼
┌──────┐ ┌──────┐ ┌──────┐ ┌──────┐
│ 前左 │ │ 前右 │ │ 后左 │ │ 后右 │
│3电机 │ │3电机 │ │3电机 │ │3电机 │
└──────┘ └──────┘ └──────┘ └──────┘
上层（用户→主控板）使用 CycloneDDS（一种 DDS 实现，与 ROS2 的默认 DDS 兼容）通过 UDP 传输。下层（主控板→电机）使用 CAN 总线。用户不直接操作 CAN——而是通过 unitree_sdk2 的 API 间接控制。
⚠️ 编程陷阱：DDS 的 QoS 配置不当导致丢帧
错误做法：使用默认的 DDS QoS（Reliability = RELIABLE）发送高频控制指令。
现象：当网络出现瞬时拥塞时，RELIABLE 模式会重传丢失的帧，导致后续帧排队等待。控制指令到达电机时已经过时，产生延迟尖峰。
根本原因：实时控制需要的是"最新的数据"，不是"所有数据都不丢"。一个 2 ms 前的扭矩指令没有任何价值——电机状态已经变了。
正确做法：使用 BEST_EFFORT + 小队列深度（History Depth = 1）：
// DDS QoS for real-time control rclcpp::QoS qos(1); // History depth = 1 qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);🧠 思维陷阱：认为"UDP 没有确定性所以不能用于实时控制"
新手想法："UDP 不保证送达，所以不能用于机器人控制。应该用 TCP 或 EtherCAT。"
实际上：Unitree 的所有产品都在用 UDP (DDS over UDP) 做上层通信，MIT Cheetah 用 LCM（也是基于 UDP multicast）。关键不在于 UDP 本身是否"确定性"，而在于**整体链路的延迟是否满足要求**。在局域网环境下（计算平台和主控板之间只隔一根网线），UDP 的延迟非常稳定（<0.5 ms），丢包率极低（<0.001%）。偶尔的丢包通过**控制器的鲁棒性**来应对——WBC 使用上一帧的状态、电机驱动器有内置安全超时。
正确思维：通信协议的选择要看**具体部署环境和延迟需求**，而不是教条地比较理论特性。
练习¶
练习 62.4.1：计算在 1 Mbps CAN 总线上传输以下数据需要的时间：(a) 1 帧标准 CAN 帧（11 位 ID + 8 字节数据 + 开销 ≈ 130 bits）(b) 12 个电机的命令和反馈（24 帧）。讨论：CAN 1 Mbps 的带宽是否足够支撑 1 kHz 控制频率？
练习 62.4.2（编程）：使用虚拟 CAN 接口测试 SocketCAN 编程。在 Linux 上执行以下步骤：
然后编写一个 C++ 程序：(a) 每 1 ms 发送一帧 CAN 数据 (b) 在另一个线程中接收并打印 (c) 统计往返延迟。练习 62.4.3（思考题）：如果你要设计一个新的 12 自由度四足机器人，在以下三种场景下你会选择什么通信方案？(a) 低成本研究平台（预算 <5 万元）(b) 高性能工业巡检机器人（预算 <50 万元）(c) 人形机器人（30+ 自由度，预算无限制）
62.5 EtherCAT 详解 ⭐⭐⭐¶
动机：为什么工业级腿足机器人选择 EtherCAT¶
本节解决的问题：CAN 总线够用，为什么 ANYmal、Talos、iCub 等工业级平台还要用更复杂、更昂贵的 EtherCAT？EtherCAT 相比 CAN 有哪些本质优势？作为算法工程师，需要了解 EtherCAT 到什么程度？
当机器人的自由度增加（如人形机器人 30+ DOF）或控制频率要求提升到 2 kHz 以上时，CAN 总线的 1 Mbps 带宽成为瓶颈。12 个电机 \(\times\) 2 帧(cmd+fb) \(\times\) 130 bits \(\times\) 2 kHz = 6.24 Mbps——已经超过经典 CAN 的极限。即使使用 CAN-FD（5 Mbps），30 自由度人形机器人也难以满足。
EtherCAT 以 100 Mbps 带宽和极低延迟（<50 \(\mu\)s）解决了这个问题。EtherCAT 的"飞越处理"机制好比高速公路的 ETC 收费站——车辆（数据帧）不需要停下来缴费，以全速通过时射频天线自动完成扣款（从站读写数据），每个收费站仅引入约 1 微秒的延迟。相比之下，CAN 总线更像传统人工收费站，每辆车都必须停车、缴费、再启动。
如果用 CAN 做 30 自由度人形会怎样¶
30 自由度人形机器人需要每个控制周期传输：30 电机 \(\times\) (cmd 10 bytes + fb 10 bytes) = 600 bytes = 4800 bits。在 1 Mbps CAN 上，纯数据传输就需要 4.8 ms——加上仲裁和开销，总延迟超过 6 ms。1 kHz 控制（1 ms 周期）完全不可能。即使分 6 条 CAN 总线，每条 5 个电机，仍然紧张。而 EtherCAT 传输 600 bytes 只需约 50 \(\mu\)s——比 CAN 快 100 倍。
EtherCAT 的核心架构¶
EtherCAT（Ethernet for Control Automation Technology）由 Beckhoff 于 2003 年发明。它使用标准以太网物理层，但**完全重新定义了协议层**。
EtherCAT 的核心创新——"飞越处理"（Processing on the fly）：
在传统以太网中，每个节点接收完整的帧 → 处理 → 发送新帧。EtherCAT 的从站不这样做——它们在帧**经过时**直接从帧中读取属于自己的数据，并把自己的数据写入帧。帧从头到尾只经过一次，不需要在每个节点停留。
主站 →─┬─→ 从站1 →─→ 从站2 →─→ ... →─→ 从站N →─┐
│ (读取+写入) (读取+写入) (读取+写入)│
└──────────────────────────────────────────┘
(帧返回主站)
每个从站引入的延迟: ~1 μs (硬件级)
N 个从站的总延迟: N × 1 μs + 线缆传输延迟
PDO（Process Data Object）——实时数据：
PDO 是**每个周期必须交换**的实时数据。在腿足机器人中：
| PDO 类型 | 方向 | 内容 | 大小 |
|---|---|---|---|
| TxPDO (从站→主站) | 反馈 | 位置、速度、扭矩、状态字、温度 | ~20 bytes/电机 |
| RxPDO (主站→从站) | 指令 | 目标位置/扭矩、控制字、PD增益 | ~16 bytes/电机 |
所有从站的 PDO 被打包在**一个以太网帧**中（最大 ~1500 bytes），一帧搞定所有电机的通信——这是 EtherCAT 高效的核心原因。
SDO（Service Data Object）——配置数据：
SDO 用于**非实时的参数配置**（如设置 PID 增益、读取电机序列号、修改运行模式）。在 EtherCAT CoE 中，SDO 通常走 mailbox 通道，不是把数据塞进 PDO 帧的“空闲带宽”。因此它不应放进 1 kHz 实时控制周期；实时循环只交换 PDO，配置和诊断放在初始化或低频管理线程。
分布式时钟（Distributed Clocks, DC）：
EtherCAT 的另一个关键特性是**硬件级时钟同步**。所有从站的时钟通过硬件自动同步到参考从站（通常是第一个从站），同步精度 <1 \(\mu\)s。这意味着所有电机可以在**同一时刻**执行新的指令——对于多关节协调运动至关重要。
CAN 总线没有硬件时钟同步——每个电机收到指令的时间取决于它在总线上的位置，可能相差几百微秒。
SOEM（Simple Open EtherCAT Master）¶
SOEM 是目前机器人领域最常用的开源 EtherCAT 主站库。它运行在用户空间（不需要内核模块），适合快速开发和调试。
#include "ethercat.h" // SOEM header
// SOEM initialization sequence
int main() {
// 1. Initialize SOEM on the network interface
if (!ec_init("eth0")) {
printf("SOEM init failed\n");
return -1;
}
// 2. Scan for slaves (auto-detect all EtherCAT devices)
int slave_count = ec_config_init(FALSE);
printf("Found %d slaves\n", slave_count);
// 3. Map PDOs (configure process data layout)
ec_config_map(&IOmap);
// 4. Configure Distributed Clocks
ec_configdc();
// 5. Switch all slaves to Operational state
ec_slave[0].state = EC_STATE_OPERATIONAL;
ec_writestate(0);
// 6. Real-time cyclic loop
while (running) {
// Send and receive PDO data in one operation
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
// Read motor feedback from IOmap
for (int i = 0; i < slave_count; i++) {
int32_t position = *(int32_t*)(ec_slave[i+1].inputs);
int16_t torque = *(int16_t*)(ec_slave[i+1].inputs + 4);
// ... process feedback
}
// Write motor commands to IOmap
for (int i = 0; i < slave_count; i++) {
*(int32_t*)(ec_slave[i+1].outputs) = target_position[i];
*(int16_t*)(ec_slave[i+1].outputs + 4) = target_torque[i];
}
// Wait for next cycle (e.g., 1 ms)
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_cycle, NULL);
}
ec_close();
return 0;
}
ethercat_driver_ros2¶
ethercat_driver_ros2（ICube-Robotics 维护）是将 EtherCAT 与 ros2_control 集成的开源框架。它提供了一个通用的 EthercatDriver 基类，让你通过 YAML 配置文件定义 PDO 映射，而不需要直接写 SOEM 代码。
架构：
ros2_control controller_manager
│
▼
EthercatDriver (HardwareInterface)
│
▼
SOEM (EtherCAT Master)
│
▼
EtherCAT 从站 (电机驱动器)
该项目基于 SOEM 的 vendored package，支持 ROS2 Humble/Iron/Jazzy，截至 2025 年仍在活跃维护中。
⚠️ 编程陷阱：SOEM 的
ec_receive_processdata超时导致控制中断错误做法：使用默认超时参数
EC_TIMEOUTRET = 2000（2000 \(\mu\)s）。现象：偶尔出现超时（线缆接触不良、电磁干扰），
ec_receive_processdata阻塞 2 ms，控制周期被打乱。根本原因：EtherCAT 帧的往返时间通常 <100 \(\mu\)s，2000 \(\mu\)s 的超时太长。如果帧真的丢了，等 2 ms 也没用——应该立即使用上一帧的数据继续控制。
正确做法：将超时设为合理值（如 500 \(\mu\)s），并在超时时使用上一帧数据 + 报警：
int wkc = ec_receive_processdata(500); // 500 us timeout if (wkc < expected_wkc) { // Use previous cycle's data, increment error counter comms_error_count_++; if (comms_error_count_ > 10) { triggerSafeStop(); // Too many consecutive errors } } else { comms_error_count_ = 0; // Reset on success }💡 概念误区：认为"EtherCAT 就是更快的以太网"
新手想法："EtherCAT 用以太网线，所以和普通以太网差不多，只是更快。"
实际上：EtherCAT 只借用了以太网的**物理层**（线缆和插头）。协议层完全不同——你不能用普通以太网交换机连 EtherCAT 设备，不能用 TCP/IP 和 EtherCAT 从站通信，不能把 EtherCAT 设备插到你的笔电网口上当普通网络设备用。EtherCAT 需要专用的**主站软件**（SOEM / IgH / acontis）和支持 EtherCAT 的**从站硬件**。
正确理解：EtherCAT = 以太网物理层 + 完全自定义的实时协议层。
练习¶
练习 62.5.1：计算在 100 Mbps EtherCAT 网络上传输 30 个电机的 PDO 数据需要的时间。假设每个电机 RxPDO 16 bytes + TxPDO 20 bytes，以太网帧开销 42 bytes（前导码+帧头+CRC+帧间隙），所有 PDO 打包在一帧中。与 CAN 的结果对比。
练习 62.5.2（代码阅读）：阅读 ethercat_driver_ros2 的 GitHub 仓库（https://github.com/ICube-Robotics/ethercat_driver_ros2），理解其 YAML 配置文件如何定义 PDO 映射。写一个配置文件描述一个假想的 3 自由度机器人腿（每个关节一个 EtherCAT 从站）。
练习 62.5.3（思考题）：EtherCAT 的分布式时钟（DC）如何帮助改善腿足控制的性能？考虑以下场景：4 条腿的 12 个电机需要在同一时刻执行新指令。如果没有 DC（如 CAN 总线），各电机执行指令的时间可能相差多少？这对步态同步有什么影响？
62.6 传感器栈 ⭐⭐¶
动机：控制器需要知道"现在发生了什么"¶
本节解决的问题：腿足机器人上有哪些传感器？每种传感器的精度、噪声特性和标定方法是什么？如何选择合适的传感器组合？
Ch57 从状态估计算法的角度讨论了传感器——本节从**硬件角度**深入讨论传感器的选型、规格和标定。两个视角互补：Ch57 告诉你"传感器数据怎么用"，本节告诉你"传感器数据从哪来、有多准、怎么保证它准"。
IMU 选型¶
IMU 是腿足状态估计的核心传感器。选择合适的 IMU 需要理解以下关键指标：
| 指标 | 含义 | 影响 |
|---|---|---|
| 噪声密度（Noise Density） | 单位带宽内的噪声 RMS | 直接影响短期测量精度 |
| 偏差稳定性（Bias Stability） | 偏差的最小 Allan 方差 | 影响长期漂移速度 |
| 偏差不稳定性（Bias Instability） | 静止时偏差的随机游走速率 | Kalman 滤波器 Q 矩阵的关键参数 |
| 量程（Full Scale Range） | 最大可测量范围 | 腿足冲击加速度可达 20-50g |
| 带宽/输出数据率 | 输出频率 | 影响状态估计的预测步频率 |
三款代表性 IMU 的对比：
| 参数 | ICM-42688-P (TDK) | BMI088 (Bosch) | ADIS16470 (ADI) |
|---|---|---|---|
| 定位 | 消费级 / 无人机 | 无人机 / 机器人 | 工业级 |
| 陀螺仪噪声密度 | 2.8 mdps/\(\sqrt{\text{Hz}}\) | 12 mdps/\(\sqrt{\text{Hz}}\) | 1.6 mdps/\(\sqrt{\text{Hz}}\) |
| 陀螺仪偏差稳定性 | ~3 deg/hr | ~8 deg/hr | ~8 deg/hr |
| 加速度计噪声密度 | 60 \(\mu\)g/\(\sqrt{\text{Hz}}\) | 175 \(\mu\)g/\(\sqrt{\text{Hz}}\) | 26 \(\mu\)g/\(\sqrt{\text{Hz}}\) |
| 加速度计量程 | \(\pm\)16g | \(\pm\)24g | \(\pm\)40g |
| 最大输出率 | 32 kHz | 2 kHz | 2 kHz |
| 接口 | SPI (24 MHz) / I2C | SPI / I2C | SPI |
| 封装 | 2.5\(\times\)3\(\times\)0.91 mm LGA | 3\(\times\)4.5\(\times\)0.95 mm LGA | 14.6 mm\(^2\) 模块 |
| 价格（量产） | ~2-3 USD | ~5-8 USD | ~80-120 USD |
| 典型应用 | 消费级四足、手机 | Unitree Go2、PX4 | ANYmal、工业 AMR |
选型建议：
- 低成本研究平台：ICM-42688-P 是性价比最高的选择。陀螺仪噪声密度 2.8 mdps/\(\sqrt{\text{Hz}}\) 对于有腿部运动学校正的状态估计器（Ch57）足够。Go2 level 的四足大多使用类似级别的 MEMS IMU
- 高性能工业平台：ADIS16470 的加速度计量程 \(\pm\)40g 能覆盖腿足着地冲击，陀螺仪噪声更低。适合精度要求高的工业巡检机器人
- 人形机器人：可能需要多 IMU 融合（胸部 + 骨盆 + 脚部各一个），单个 IMU 使用 ICM-42688 或 BMI088 即可
关节编码器¶
关节编码器测量关节的旋转角度。腿足机器人常用两种类型：
增量式编码器（Incremental Encoder）： - 输出脉冲信号，需要上电时归零（homing） - 精度高：可达 4096-65536 CPR（每圈脉冲数） - 成本低 - 缺点：上电后不知道当前位置，断电后丢失位置信息
绝对式编码器（Absolute Encoder）： - 直接输出当前绝对角度（通常 12-14 位，分辨率 ~0.02-0.09 deg） - 上电即知位置，无需归零 - 缺点：精度略低于高端增量式，成本较高
为什么腿足机器人偏好绝对式编码器：
腿足机器人需要**上电后立即知道当前关节位置**——不像工业机械臂可以慢慢做 homing 运动。四足机器人可能在任意姿态下启动（侧躺、趴着、被翻倒），必须在启动瞬间就知道 12 个关节的精确位置，才能计算安全的站立轨迹。
磁编码器（如 AS5048A、AS5600）是腿足领域最常用的绝对式编码器：
| 参数 | AS5048A | AS5600 |
|---|---|---|
| 分辨率 | 14 位 (16384 步/圈) | 12 位 (4096 步/圈) |
| 精度 | \(\pm\)0.05 deg | \(\pm\)0.15 deg |
| 接口 | SPI / PWM | I2C / PWM |
| 特点 | 可测绝对多圈 | 低成本 |
| 价格 | ~$5 | ~$1.5 |
Go2 使用集成在电机模组内部的磁编码器，通过检测永磁体的磁场方向来确定角度。编码器是在电机侧、减速器输出侧，还是经过驱动板融合后的关节角字段，取决于具体模组和 SDK 定义；控制代码应把 SDK 的 q 明确标注为“关节侧角度字段”，并通过零位标定和外部量具验证，而不是仅凭编码器安装位置推断精度。
接触传感器¶
接触检测是腿足控制的关键环节——Ch57 已经讨论了接触检测算法，本节关注硬件层面。
方案 1：足端力传感器
- 原理：压阻式或电容式力传感器安装在脚底
- 精度：法向力精度 ~1 N
- 优点：直接测量，响应快
- 缺点：增加重量和复杂度，可能被碎石损坏
- 代表：Unitree Go2 SDK 暴露
foot_force/foot_force_est字段；字段来源和量纲需要按机型标定确认
方案 2：本体感受接触检测（Proprioceptive Contact Detection）
- 原理：通过电机电流/扭矩和关节运动学反推接触力
- 方法：\(\hat{F}_{\text{contact}} = J^{-T}(\tau_{\text{measured}} - \tau_{\text{gravity}} - \tau_{\text{inertia}})\)
- 优点：无需额外传感器，利用 QDD 电机的高反向驱动性
- 缺点：精度受限于电机扭矩估计精度（~5-10%）
- 代表：MIT Mini Cheetah、ANYmal（主要方法）
方案 3：六维力/力矩传感器（6-axis F/T sensor）
- 原理：测量完整的 6 维力/力矩（\(F_x, F_y, F_z, M_x, M_y, M_z\)）
- 精度：极高（<0.1% FS）
- 缺点：昂贵（3000-10000 USD/个）、重、脆弱
- 应用：人形机器人脚踝（如 Atlas）、学术研究
IMU 标定¶
IMU 标定是确保状态估计精度的关键步骤。Ch57 已经讨论了标定的滤波层面，本节补充硬件层面的标定方法。
静态标定（Static Calibration）——最简单也最常用：
struct IMUCalibration {
Eigen::Vector3d gyro_bias; // Gyroscope bias (rad/s)
Eigen::Vector3d accel_bias; // Accelerometer bias (m/s^2)
static IMUCalibration calibrateStatic(
const std::vector<Eigen::Vector3d>& gyro_samples,
const std::vector<Eigen::Vector3d>& accel_samples,
int num_samples = 3000) { // 30 seconds at 100 Hz
IMUCalibration cal;
// Gyro bias: average of readings when stationary
// (should be zero, any nonzero is bias)
cal.gyro_bias = Eigen::Vector3d::Zero();
for (const auto& g : gyro_samples) {
cal.gyro_bias += g;
}
cal.gyro_bias /= static_cast<double>(gyro_samples.size());
// Accel bias: average minus gravity
// When stationary, accel should read [0, 0, 9.81] in body frame
// (assuming robot is level)
Eigen::Vector3d accel_mean = Eigen::Vector3d::Zero();
for (const auto& a : accel_samples) {
accel_mean += a;
}
accel_mean /= static_cast<double>(accel_samples.size());
Eigen::Vector3d gravity_body(0.0, 0.0, 9.81); // Assuming level
cal.accel_bias = accel_mean - gravity_body;
return cal;
}
};
温度标定——高精度应用需要：
IMU 的偏差随温度变化（温度系数 ~0.01 deg/s/degC for 陀螺仪）。方法是在不同温度下（通过加热/冷却）重复静态标定，建立偏差-温度曲线：
在运行时根据当前温度实时补偿偏差。
⚠️ 编程陷阱：标定时机器人没有放水平
错误做法：在不平的桌面上做 IMU 静态标定。
现象：标定后的加速度计偏差明显偏大，状态估计器的 roll/pitch 初值有几度的误差。
根本原因：静态标定假设重力方向沿 body frame 的 \(z\) 轴（\(\boldsymbol{g}_b = [0, 0, 9.81]^T\)）。如果机器人倾斜 \(\alpha\) 度，重力在 \(x/y\) 轴上有分量 \(9.81 \sin\alpha\)，这个分量被错误地计入了 accel_bias。
正确做法：(a) 使用水平仪确保机器人水平（\(< 0.5\) deg）；(b) 或者不假设水平，而是用加速度计读数**估计初始 roll/pitch**，然后计算重力在 body frame 中的投影。
💡 概念误区：认为"编码器精度就是关节角度精度"
新手想法："磁编码器精度 \(\pm\)0.05 deg，所以关节角度精度也是 \(\pm\)0.05 deg。"
实际上：编码器测量的是**编码器安装位置**的角度。如果编码器在减速器输出端，直接测量关节角度——精度确实是 \(\pm\)0.05 deg。但如果编码器在电机侧（减速器输入端），需要除以减速比才是关节角度，此时分辨率提高 \(N\) 倍，但**减速器背隙**成为主要误差源。行星齿轮的背隙约 0.5-2 deg（关节侧），远大于编码器精度。
关键：关节角度精度 = min(编码器精度, 减速器背隙)。高端 QDD 电机（如 T-Motor AKE80）使用高精度行星减速器，背隙可控制在 ~9 arcmin (0.15 deg)。
练习¶
练习 62.6.1：使用 Allan 方差分析 IMU 噪声特性。给定一段 IMU 静止录制数据（或自行录制），用 Python 计算陀螺仪的 Allan 偏差曲线，从中提取角度随机游走（ARW）和偏差不稳定性（Bias Instability）。
练习 62.6.2：比较有足端力传感器和无足端力传感器两种接触检测方案。分析：(a) 检测延迟（力传感器 vs 电流反推）(b) 误检率（在粗糙地面上）(c) 对状态估计精度的影响。如果预算有限，你会选择装力传感器还是用本体感受方案？
练习 62.6.3（编程）：写一个 IMU 静态标定程序。输入：30 秒的 IMU 原始数据（陀螺仪 + 加速度计）。输出：陀螺仪三轴偏差、加速度计三轴偏差、初始 roll/pitch 角度。用你自己的手机 IMU 数据测试。
62.7 Unitree SDK 剖析 ⭐⭐¶
动机：如何用代码控制一台真实的四足机器人¶
本节解决的问题：Unitree SDK2 的架构是什么？HighLevel 和 LowLevel 模式有什么区别？如何安全地用 SDK 控制 Go2 的 12 个关节？有哪些安全机制防止你把机器人弄坏？
unitree_sdk2 是 Unitree 为 Go2/B2/H1/G1 等机器人提供的官方开发工具包。它基于 CycloneDDS 实现通信，提供 C++ 和 Python 两种接口。理解 SDK 的架构和安全机制是从仿真到真机部署的必经之路。
如果不了解 SDK 安全机制会怎样¶
真实案例：一位研究生第一次用 unitree_sdk2 控制 Go2，他写了一个程序让所有 12 个关节同时移到目标位置。但他不知道 SDK 有**指令超时保护**——当 SDK 在 250 ms 内没有收到新指令时，电机会自动切换到阻尼模式。他的程序在初始化阶段花了 500 ms 建立 DDS 连接，结果电机先收到第一帧指令后，在等待第二帧的 250 ms 内触发了超时保护，四条腿的电机突然变软，机器人从桌子上"坐"了下来。
SDK2 架构¶
unitree_sdk2
├── unitree_sdk2/ # Core C++ library
│ ├── include/
│ │ ├── unitree/robot/channel/ # DDS communication
│ │ │ ├── channel_publisher.hpp
│ │ │ ├── channel_subscriber.hpp
│ │ │ └── channel_factory.hpp
│ │ ├── unitree/idl/ # IDL message definitions
│ │ │ ├── go2/
│ │ │ │ ├── LowCmd_.hpp # Low-level motor commands
│ │ │ │ ├── LowState_.hpp # Low-level motor feedback
│ │ │ │ ├── HighCmd_.hpp # High-level body commands
│ │ │ │ └── HighState_.hpp # High-level body state
│ │ │ └── hg/ # Humanoid (H1/G1) messages
│ │ └── unitree/common/ # Utilities
│ └── example/
│ ├── go2/
│ │ ├── low_level/ # Direct motor control
│ │ └── high_level/ # Body-level control
│ └── b2/
└── unitree_sdk2_python/ # Python bindings (pybind11)
HighLevel vs LowLevel 模式¶
| 特性 | HighLevel 模式 | LowLevel 模式 |
|---|---|---|
| 控制粒度 | 整体运动（速度、方向） | 单个关节电机 |
| 输入 | 线速度 \(v_x, v_y\)，角速度 \(\omega_z\) | 12 个 \((q, \dot{q}, k_p, k_d, \tau)\) |
| 频率 | ~100-500 Hz | ~500-1000 Hz |
| 内置算法 | 步态、平衡、状态估计 | 无（用户全权控制） |
| 安全性 | 高（Unitree 内置保护） | 低（用户负责安全） |
| 用途 | 导航、遥控、简单行为 | WBC/RL 部署、研究 |
| DDS Topic | rt/api/sport/request |
rt/lowcmd / rt/lowstate |
对于部署 WBC 或 RL 控制器，必须使用 LowLevel 模式——因为你需要直接控制每个关节的扭矩/位置。
LowLevel 控制完整示例¶
下面代码采用 Unitree SDK2 当前 Go2 示例中的 unitree_go::msg::dds_ 命名空间。示例保留了一个刻意标出的并发简化：回调直接写数组，生产实现应改为后文 ros2_control 示例中的快照/三缓冲方案。
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/idl/go2/LowState_.hpp>
#include <unitree/dds_wrapper/common/crc.h>
#include <chrono>
#include <thread>
#include <atomic>
#include <cmath>
#include <iostream>
#include <functional>
using namespace unitree::robot;
// Joint indices for Go2 (4 legs x 3 joints)
// FR=FrontRight, FL=FrontLeft, RR=RearRight, RL=RearLeft
// 0: FR_hip_ab, 1: FR_hip_fe, 2: FR_knee
// 3: FL_hip_ab, 4: FL_hip_fe, 5: FL_knee
// 6: RR_hip_ab, 7: RR_hip_fe, 8: RR_knee
// 9: RL_hip_ab, 10: RL_hip_fe, 11: RL_knee
class Go2LowLevelController {
public:
Go2LowLevelController() {
// Initialize DDS communication
ChannelFactory::Instance()->Init(0, "eth0");
// Create publisher and subscriber
cmd_pub_.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(
"rt/lowcmd"));
state_sub_.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(
"rt/lowstate"));
cmd_pub_->InitChannel();
state_sub_->InitChannel(
std::bind(&Go2LowLevelController::stateCallback, this,
std::placeholders::_1),
1); // History depth = 1
}
void stateCallback(const void* msg) {
const auto& state =
*static_cast<const unitree_go::msg::dds_::LowState_*>(msg);
// Teaching shortcut: this direct write has a data race with run().
// Production code should use a snapshot, SPSC queue, or triple buffer.
for (int i = 0; i < 12; ++i) {
joint_pos_[i] = state.motor_state()[i].q();
joint_vel_[i] = state.motor_state()[i].dq();
joint_tau_[i] = state.motor_state()[i].tau_est();
}
// IMU data
for (int i = 0; i < 4; ++i)
imu_quat_[i] = state.imu_state().quaternion()[i];
for (int i = 0; i < 3; ++i) {
imu_gyro_[i] = state.imu_state().gyroscope()[i];
imu_accel_[i] = state.imu_state().accelerometer()[i];
}
}
void run() {
running_ = true;
// Safety: first read current position and hold it
std::this_thread::sleep_for(std::chrono::milliseconds(500));
double init_pos[12];
for (int i = 0; i < 12; ++i) {
init_pos[i] = joint_pos_[i];
}
auto next_time = std::chrono::steady_clock::now();
const auto period = std::chrono::microseconds(2000); // 500 Hz
while (running_) {
unitree_go::msg::dds_::LowCmd_ cmd{};
for (int i = 0; i < 12; ++i) {
// Hold initial position with moderate PD gains
cmd.motor_cmd()[i].mode() = 0x01; // FOC mode
cmd.motor_cmd()[i].q() = init_pos[i];
cmd.motor_cmd()[i].dq() = 0.0;
cmd.motor_cmd()[i].kp() = 30.0; // Moderate stiffness
cmd.motor_cmd()[i].kd() = 5.0; // Moderate damping
cmd.motor_cmd()[i].tau() = 0.0; // No feedforward
}
// CRC protection (required by Go2)
cmd.crc() = computeCRC(cmd);
cmd_pub_->Write(cmd);
next_time += period;
std::this_thread::sleep_until(next_time);
}
}
void stop() { running_ = false; }
private:
uint32_t computeCRC(unitree_go::msg::dds_::LowCmd_& cmd) {
// SDK2 also provides wrappers that fill CRC internally. If using raw
// LowCmd_, keep this calculation exactly consistent with your SDK.
return crc32_core(reinterpret_cast<uint32_t*>(&cmd),
(sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
}
std::unique_ptr<ChannelPublisher<unitree_go::msg::dds_::LowCmd_>> cmd_pub_;
std::unique_ptr<ChannelSubscriber<unitree_go::msg::dds_::LowState_>> state_sub_;
double joint_pos_[12]{}, joint_vel_[12]{}, joint_tau_[12]{};
double imu_quat_[4]{}, imu_gyro_[3]{}, imu_accel_[3]{};
std::atomic<bool> running_{false};
};
⚠️ 并发陷阱：上述回调直接写入共享数组，控制线程同时读取——这是**数据竞争（data race，C++ 标准下的未定义行为）**。正确做法是使用双缓冲/三缓冲、SPSC 队列，或在非实时线程中做 DDS 接收后把快照发布给实时循环。不要把
std::mutex直接放进 1 kHz 实时路径。
安全机制¶
Unitree SDK 内置了多层安全保护，理解这些机制对于避免损坏机器人至关重要：
| 安全机制 | 触发条件 | 行为 | 注意 |
|---|---|---|---|
| 指令超时 | ~250 ms 无新指令 | 电机切入阻尼模式 | 确保控制循环稳定运行 |
| CRC 校验 | CRC 字段与数据不匹配 | 指令被丢弃 | 每帧必须正确计算 CRC |
| 扭矩限制 | \(\|\tau\| >\) 扭矩上限 | 截断到上限 | 上限因关节位置不同 |
| 位置限制 | \(q\) 超出关节运动范围 | 电机进入保护模式 | 膝关节负角会导致结构碰撞 |
| 温度保护 | 温度 > 阈值（~80 degC） | 降额或关闭电机 | 长时间站立要注意 |
| 电流保护 | 电流超过安全阈值 | 立即切断 | 不可恢复，需重启 |
从 HighLevel 切换到 LowLevel 的安全流程：
- 首先在 HighLevel 模式下让机器人站稳
- 读取当前所有关节角度（通过
rt/lowstate） - 切换到 LowLevel 模式时，第一帧指令必须是当前角度 + 适中的 PD 增益（"hold position"）
- 确认控制稳定后，再逐渐过渡到你的控制器输出
⚠️ 编程陷阱：LowLevel 模式下忘记设 CRC
错误做法：发送 LowCmd 时 CRC 字段留零。
现象：指令全部被主控板丢弃，电机没有任何响应，像是"SDK 连接失败"。
根本原因：Go2 的主控板对每帧指令做 CRC 校验。CRC 不正确的帧直接丢弃，不会有任何错误提示。
正确做法：使用 SDK 提供的 CRC 计算函数。不同 SDK 版本的 CRC 算法可能不同，务必使用对应版本的实现。
💡 概念误区：认为"Python SDK 和 C++ SDK 性能一样"
新手想法："unitree_sdk2_python 用起来更方便，RL 策略也是 Python，全用 Python 就好。"
实际上：Python SDK 通过 pybind11 封装 C++ 库，DDS 通信本身性能相同。但 Python 的 GIL 和垃圾回收会引入不确定的延迟——在 500 Hz 控制循环中，Python 线程可能因 GC 暂停 1-5 ms。对于 HighLevel 控制和实验原型，Python 够用；对于 LowLevel 部署，生产环境用 C++。
折中方案：RL 推理用 C++ (LibTorch/ONNX Runtime，Ch64 详述)，控制循环用 C++，只在数据记录和可视化时用 Python。
练习¶
练习 62.7.1（代码阅读）：阅读 unitree_sdk2 的 example/go2/low_level/ 目录下的示例代码。回答：(a) 控制循环的频率是多少？(b) 哪些安全检查在发送指令前执行？(c) 如果断开网络连接，电机会怎么反应？
练习 62.7.2（编程）：基于上面的示例代码，修改 run() 方法，让 Go2 的前右腿膝关节（index 2）做正弦运动：\(q_{\text{des}}(t) = q_0 + 0.3 \sin(2\pi \cdot 0.5 \cdot t)\)。注意：(a) 幅度不能太大（避免自碰撞）(b) 必须先检查关节运动范围 (c) 必须平滑过渡（不要从静止突然跳到正弦）。
练习 62.7.3（思考题）：比较 Unitree SDK2（基于 DDS）和 MIT Cheetah 的 LCM 通信。从以下角度分析：(a) 开发便捷性 (b) 跨平台兼容性 (c) 与 ROS2 的集成难度 (d) 通信延迟。为什么 MIT 当时选择 LCM 而不是 DDS？
62.8 ros2_control 硬件接口 ⭐⭐¶
动机：让你的控制器适配任意硬件¶
本节解决的问题：你在 Ch53 实现的 WBC 控制器可以在仿真中运行。但如何让**同一个控制器代码**既能在 Gazebo 仿真中运行，也能在 Unitree Go2 真机上运行，还能在未来切换到另一个四足平台时无需修改？答案是 ros2_control 的硬件抽象层。
Ch61 介绍了 ros2_control 的框架结构。本节深入实现一个完整的 HardwareInterface，将 Unitree SDK 接入 ros2_control 框架。
如果不做硬件抽象会怎样¶
场景：你的 WBC 代码直接调用 Unitree SDK 读写电机数据。一年后，实验室购入了新的机器人平台（比如 Agility Robotics 的 Digit），使用完全不同的 SDK。你需要：
- 在 WBC 代码的所有位置查找并替换 Unitree SDK 调用 → 容易遗漏
- 重新测试所有代码路径 → 工作量巨大
- 如果要同时支持两个平台 → 代码充满
#ifdef UNITREE/#ifdef DIGIT条件编译 → 可维护性灾难
ros2_control 的解决方案：WBC 只和 StateInterface / CommandInterface 交互——这是统一的抽象接口。每个硬件平台提供自己的 HardwareInterface 实现（UnitreeHW、DigitHW 等），负责将统一接口映射到具体的 SDK 调用。切换平台只需更换一个 YAML 配置文件。
HardwareInterface 实现¶
以下是一个简化但完整的 Unitree Go2 硬件接口实现：
// unitree_hw.hpp
#pragma once
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/state.hpp>
// Unitree SDK headers
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/idl/go2/LowState_.hpp>
namespace unitree_hw {
class UnitreeHW : public hardware_interface::SystemInterface {
public:
// ---- Lifecycle callbacks ----
hardware_interface::CallbackReturn on_init(
const hardware_interface::HardwareInfo& info) override {
if (hardware_interface::SystemInterface::on_init(info) !=
hardware_interface::CallbackReturn::SUCCESS) {
return hardware_interface::CallbackReturn::ERROR;
}
// Parse URDF parameters
num_joints_ = info_.joints.size(); // Should be 12
joint_positions_.resize(num_joints_, 0.0);
joint_velocities_.resize(num_joints_, 0.0);
joint_efforts_.resize(num_joints_, 0.0);
joint_position_cmds_.resize(num_joints_, 0.0);
joint_velocity_cmds_.resize(num_joints_, 0.0);
joint_effort_cmds_.resize(num_joints_, 0.0);
joint_kp_.resize(num_joints_, 0.0);
joint_kd_.resize(num_joints_, 0.0);
// 预分配 shadow buffers（DDS callback 写入，read() 读取）
shadow_positions_.resize(num_joints_, 0.0);
shadow_velocities_.resize(num_joints_, 0.0);
shadow_efforts_.resize(num_joints_, 0.0);
return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State&) override {
// Initialize Unitree SDK
unitree::robot::ChannelFactory::Instance()->Init(0, "eth0");
cmd_pub_ = std::make_unique<
unitree::robot::ChannelPublisher<
unitree_go::msg::dds_::LowCmd_>>("rt/lowcmd");
state_sub_ = std::make_unique<
unitree::robot::ChannelSubscriber<
unitree_go::msg::dds_::LowState_>>("rt/lowstate");
cmd_pub_->InitChannel();
state_sub_->InitChannel(
[this](const void* msg) { this->onStateReceived(msg); }, 1);
return hardware_interface::CallbackReturn::SUCCESS;
}
// ---- Export interfaces ----
std::vector<hardware_interface::StateInterface>
export_state_interfaces() override {
std::vector<hardware_interface::StateInterface> interfaces;
for (size_t i = 0; i < num_joints_; ++i) {
interfaces.emplace_back(info_.joints[i].name,
hardware_interface::HW_IF_POSITION, &joint_positions_[i]);
interfaces.emplace_back(info_.joints[i].name,
hardware_interface::HW_IF_VELOCITY, &joint_velocities_[i]);
interfaces.emplace_back(info_.joints[i].name,
hardware_interface::HW_IF_EFFORT, &joint_efforts_[i]);
}
return interfaces;
}
std::vector<hardware_interface::CommandInterface>
export_command_interfaces() override {
std::vector<hardware_interface::CommandInterface> interfaces;
for (size_t i = 0; i < num_joints_; ++i) {
interfaces.emplace_back(info_.joints[i].name,
hardware_interface::HW_IF_POSITION, &joint_position_cmds_[i]);
interfaces.emplace_back(info_.joints[i].name,
hardware_interface::HW_IF_VELOCITY, &joint_velocity_cmds_[i]);
interfaces.emplace_back(info_.joints[i].name,
hardware_interface::HW_IF_EFFORT, &joint_effort_cmds_[i]);
}
return interfaces;
}
// ---- Real-time loop: read / write ----
hardware_interface::return_type read(
const rclcpp::Time&, const rclcpp::Duration&) override {
// ⚠️ 生产代码必须使用 RT-safe 同步！
// onStateReceived() 在 DDS 线程写 joint 数组，
// read() 在 RT 线程读同一数组 → 无同步 = data race (UB)
//
// RT-safe 同步方案（禁止 std::mutex，会导致优先级反转）：
// 方案1: std::atomic_flag + memcpy snapshot（下方演示）
// ⚠️ 注意：atomic_flag 的 test_and_set 循环是忙等待(spin-lock)，
// 在竞争激烈时会产生无界自旋和抖动。仅适用于临界区极短
// （<1 us）且竞争线程数<=2的场景。生产环境优先考虑方案2。
// 方案2: triple buffer（Ch55.7 的 MRT 架构：三槽位原子交换，写不阻塞读取最新值，最优方案）
// 方案3: lock-free SPSC queue
// 方案1 示例：原子标志 + snapshot（忙等待，见上方注意事项）
while (state_writing_.test_and_set(std::memory_order_acquire)) {}
for (size_t i = 0; i < num_joints_; ++i) {
joint_positions_[i] = shadow_positions_[i];
joint_velocities_[i] = shadow_velocities_[i];
joint_efforts_[i] = shadow_efforts_[i];
}
state_writing_.clear(std::memory_order_release);
return hardware_interface::return_type::OK;
}
hardware_interface::return_type write(
const rclcpp::Time&, const rclcpp::Duration&) override {
unitree_go::msg::dds_::LowCmd_ cmd{};
for (size_t i = 0; i < num_joints_; ++i) {
cmd.motor_cmd()[i].mode() = 0x01;
cmd.motor_cmd()[i].q() = joint_position_cmds_[i];
cmd.motor_cmd()[i].dq() = joint_velocity_cmds_[i];
cmd.motor_cmd()[i].kp() = joint_kp_[i];
cmd.motor_cmd()[i].kd() = joint_kd_[i];
cmd.motor_cmd()[i].tau() = joint_effort_cmds_[i];
}
cmd.crc() = computeCRC(cmd);
cmd_pub_->Write(cmd);
// Teaching shortcut: DDS Write may allocate or enter the kernel.
// A production ros2_control plugin should hand off this command to
// a non-real-time publisher thread instead of sending here directly.
return hardware_interface::return_type::OK;
}
private:
void onStateReceived(const void* msg) {
const auto& state =
*static_cast<const unitree_go::msg::dds_::LowState_*>(msg);
// 写入 shadow buffer，由 read() 在 RT 线程中拷贝到 state interfaces
// ⚠️ 忙等待：若 read() 临界区耗时过长，此处 DDS 线程会自旋等待，
// 增加 CPU 占用和抖动。生产环境建议改用 triple buffer 消除互斥。
while (state_writing_.test_and_set(std::memory_order_acquire)) {}
for (size_t i = 0; i < num_joints_; ++i) {
shadow_positions_[i] = state.motor_state()[i].q();
shadow_velocities_[i] = state.motor_state()[i].dq();
shadow_efforts_[i] = state.motor_state()[i].tau_est();
}
state_writing_.clear(std::memory_order_release);
}
uint32_t computeCRC(unitree_go::msg::dds_::LowCmd_& cmd) {
return crc32_core(
reinterpret_cast<uint32_t*>(&cmd),
(sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
}
size_t num_joints_ = 12;
std::vector<double> joint_positions_, joint_velocities_, joint_efforts_;
std::vector<double> joint_position_cmds_, joint_velocity_cmds_;
std::vector<double> joint_effort_cmds_;
std::vector<double> joint_kp_, joint_kd_;
// Shadow buffers + atomic flag for RT-safe DDS→read() 同步
std::vector<double> shadow_positions_, shadow_velocities_, shadow_efforts_;
std::atomic_flag state_writing_ = ATOMIC_FLAG_INIT;
std::unique_ptr<unitree::robot::ChannelPublisher<
unitree_go::msg::dds_::LowCmd_>> cmd_pub_;
std::unique_ptr<unitree::robot::ChannelSubscriber<
unitree_go::msg::dds_::LowState_>> state_sub_;
};
} // namespace unitree_hw
⚠️ 实时性陷阱：示例中的
cmd_pub_->Write()仍在write()路径里执行 DDS 序列化和网络发送，生产实现应通过非实时线程异步发布：实时循环只写入预分配命令快照，普通线程负责 DDS 发布。这里保留同步写法是为了让硬件接口结构更容易阅读。
对应的 URDF 配置¶
<!-- go2.ros2_control.xacro -->
<ros2_control name="Go2System" type="system">
<hardware>
<plugin>unitree_hw/UnitreeHW</plugin>
<param name="network_interface">eth0</param>
</hardware>
<!-- Front Right Leg -->
<joint name="FR_hip_ab">
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
</joint>
<joint name="FR_hip_fe">
<!-- ... same interfaces ... -->
</joint>
<joint name="FR_knee">
<!-- ... same interfaces ... -->
</joint>
<!-- ... repeat for FL, RR, RL legs ... -->
</ros2_control>
与 legged_control 的关系¶
开源项目 legged_control（qm_control 的后继）正是这种架构的实战范例。它包含：
legged_controllers/：WBC 控制器（ros2_control Controller 插件）legged_unitree_hw/：Unitree 硬件接口（UnitreeHW）legged_gazebo_hw/：Gazebo 仿真硬件接口
同一个 WBC 控制器，通过 YAML 配置切换不同的硬件接口：
# Real robot
controller_manager:
ros__parameters:
hardware_interface: "unitree_hw/UnitreeHW"
update_rate: 500 # Hz
# Simulation
controller_manager:
ros__parameters:
hardware_interface: "gazebo_ros2_control/GazeboSystem"
update_rate: 500 # Hz
⚠️ 编程陷阱：
read()和write()中做内存分配错误做法：在
read()或write()中创建临时std::vector或std::string。现象：控制循环偶尔出现 1-5 ms 的延迟尖峰（由
malloc引起）。根本原因：
read()和write()在 controller_manager 的实时循环中被调用（Ch61）。任何内存分配都违反了"四大禁忌"。正确做法：所有缓冲区在
on_init()或on_configure()中预分配。read()和write()只操作预分配的成员变量。使用EIGEN_RUNTIME_NO_MALLOC（Ch53）在开发期检测违规。🧠 思维陷阱：认为"ros2_control 的抽象会引入性能开销"
新手想法："多了一层抽象，性能肯定变差。直接调 SDK 更快。"
实际上：ros2_control 的
StateInterface/CommandInterface本质上只是一个double*指针——指向预分配的double变量。读写一个double指针的开销 < 1 ns，完全可忽略。真正的开销在 DDS 通信和电机驱动器的处理时间，这与是否使用 ros2_control 无关。ros2_control 的实际价值：(1) 代码复用（同一个 WBC 适配多平台）(2) 仿真/真机无缝切换 (3) controller_manager 管理多个控制器的生命周期。这些工程价值远超任何微小的性能代价。
练习¶
练习 62.8.1（代码阅读）：精读 legged_control 的 legged_unitree_hw/src/UnitreeHW.cpp。回答：(a) on_activate() 中做了哪些初始化？(b) read() 如何处理 DDS 回调和实时循环之间的线程同步？(c) 是否使用了无锁数据结构？
练习 62.8.2（编程）：扩展上面的 UnitreeHW 实现，添加 IMU 数据的 StateInterface。需要导出以下接口：imu/orientation.x/y/z/w（四元数）、imu/angular_velocity.x/y/z、imu/linear_acceleration.x/y/z。提示：ros2_control 支持 sensor 类型的硬件组件。
练习 62.8.3（对比练习）：对比 legged_control/UnitreeHW.cpp 和 franka_ros2/franka_hardware.cpp（Franka Emika 的 ros2_control 硬件接口）。分析：(a) 两者的 read()/write() 实现有何不同？(b) Franka 使用 libfranka（实时以太网），Unitree 使用 DDS——这对硬件接口的设计有何影响？(c) 哪一个更适合作为你自己 HW 接口的参考模板？
62.9 安全与保护 ⭐¶
动机：如何确保你不会把机器人弄坏¶
本节解决的问题：在开发和部署腿足控制器的过程中，有哪些可能损坏机器人或造成人身危险的操作？如何设计多层安全保护机制来防止这些事故？
腿足机器人不同于工业机械臂——机械臂有固定基座、工作空间受限、安全围栏保护。腿足机器人在开放空间中运动，随时可能倒下、碰撞或失控。安全保护不是可选的附加功能，而是**系统设计的基础要求**。
安全保护的层次结构¶
安全保护应该是**多层冗余**的——每一层独立工作，任何一层失效都有下一层兜底：
┌──────────────────────────────────────────┐
│ 第 5 层：物理安全 (人工干预) │
│ E-Stop 急停按钮, 安全绳, 测试架 │
├──────────────────────────────────────────┤
│ 第 4 层：主控板安全 (固件级) │
│ 通信超时保护, 过流保护, 过温保护 │
├──────────────────────────────────────────┤
│ 第 3 层：驱动器安全 (电机驱动器固件) │
│ 关节限位, 电流限制, 速度限制 │
├──────────────────────────────────────────┤
│ 第 2 层：软件安全 (你的控制器代码) │
│ 指令合理性检查, 状态异常检测, 软限位 │
├──────────────────────────────────────────┤
│ 第 1 层：算法安全 (WBC/MPC 约束) │
│ 扭矩约束, 接触力约束, 质心约束 │
└──────────────────────────────────────────┘
软件层安全实现¶
以下是一个生产级的安全检查模块：
class SafetyChecker {
public:
struct Limits {
double q_min, q_max; // Joint position limits (rad)
double dq_max; // Joint velocity limit (rad/s)
double tau_max; // Joint torque limit (Nm)
double temp_warning; // Temperature warning threshold (degC)
double temp_critical; // Temperature critical threshold (degC)
};
SafetyChecker(const std::vector<Limits>& limits) : limits_(limits) {}
enum class Status { OK, WARNING, CRITICAL };
struct CheckedCommand {
double q;
double dq;
double tau;
Status status;
};
CheckedCommand checkCommand(int joint_idx,
double q_cmd, double dq_cmd, double tau_cmd,
double q_cur, double temp_cur) {
const auto& lim = limits_[joint_idx];
Status status = Status::OK;
// 1. Position limit check (with margin)
double margin = 0.05; // 0.05 rad (~3 degrees) safety margin
if (q_cmd < lim.q_min + margin || q_cmd > lim.q_max - margin) {
RCLCPP_WARN(rclcpp::get_logger("safety"),
"Joint %d: position cmd %.3f near limit [%.3f, %.3f]",
joint_idx, q_cmd, lim.q_min, lim.q_max);
q_cmd = std::clamp(q_cmd, lim.q_min + margin, lim.q_max - margin);
}
// 2. Velocity limit check
if (std::abs(dq_cmd) > lim.dq_max) {
RCLCPP_WARN(rclcpp::get_logger("safety"),
"Joint %d: velocity cmd %.3f exceeds limit %.3f",
joint_idx, dq_cmd, lim.dq_max);
dq_cmd = std::clamp(dq_cmd, -lim.dq_max, lim.dq_max);
status = Status::WARNING;
}
// 3. Torque limit check
if (std::abs(tau_cmd) > lim.tau_max) {
RCLCPP_WARN(rclcpp::get_logger("safety"),
"Joint %d: torque cmd %.3f exceeds limit %.3f",
joint_idx, tau_cmd, lim.tau_max);
tau_cmd = std::clamp(tau_cmd, -lim.tau_max, lim.tau_max);
status = Status::WARNING;
}
// 4. Temperature check
if (temp_cur > lim.temp_critical) {
RCLCPP_ERROR(rclcpp::get_logger("safety"),
"Joint %d: temperature %.1f degC CRITICAL (limit %.1f degC)",
joint_idx, temp_cur, lim.temp_critical);
return {q_cmd, 0.0, 0.0, Status::CRITICAL};
}
if (temp_cur > lim.temp_warning) {
RCLCPP_WARN(rclcpp::get_logger("safety"),
"Joint %d: temperature %.1f degC warning (limit %.1f degC)",
joint_idx, temp_cur, lim.temp_warning);
status = Status::WARNING;
}
// 5. Sudden jump detection
double dq_estimated = (q_cmd - q_cur) * 500.0; // at 500 Hz
if (std::abs(dq_estimated) > lim.dq_max * 2.0) {
RCLCPP_ERROR(rclcpp::get_logger("safety"),
"Joint %d: cmd jump too large (%.3f rad in one step)",
joint_idx, q_cmd - q_cur);
return {q_cur, 0.0, 0.0, Status::CRITICAL};
}
return {q_cmd, dq_cmd, tau_cmd, status};
}
// Emergency soft stop: gradually reduce gains to zero
void triggerSoftStop(std::vector<double>& kp, std::vector<double>& kd,
std::vector<double>& tau_ff, int ramp_steps = 100) {
for (size_t i = 0; i < kp.size(); ++i) {
kp[i] *= (1.0 - 1.0 / ramp_steps); // Decay over ramp_steps
kd[i] *= (1.0 - 0.5 / ramp_steps); // Decay slower (keep damping)
tau_ff[i] *= (1.0 - 1.0 / ramp_steps);
}
}
private:
std::vector<Limits> limits_;
};
常见硬件故障排查¶
| 现象 | 可能原因 | 排查方法 | 解决方案 |
|---|---|---|---|
| 机器人完全不动 | 电池没电 / SDK 未连接 / CRC 错误 | 检查电池电量，ping 主控板，检查 CRC |
充电 / 检查网络 / 修复 CRC |
| 单个关节不动 | CAN 线松动 / 电机故障 / 超出限位 | candump 检查 CAN 帧 / 读 error code |
检查线缆 / 更换电机 / 调整限位 |
| 关节抖动 | PD 增益过高 / 通信延迟 / 编码器噪声 | 降低 \(k_p\)，用示波器测 CAN 延迟 | 调参 / 检查总线负载 |
| 机器人倒下 | IMU 漂移 / 状态估计发散 / 足底打滑 | 录制数据回放分析 | 重新标定 / 调整 KF 参数 |
| 电机过热 | 长时间大电流 / 散热不良 / 站姿不佳 | 监控温度话题 | 优化站姿 / 增加休息间隔 |
| 偶尔"抖一下" | CAN 丢帧 / DDS QoS 不当 | 统计通信丢帧率 | 降低总线负载 / 调 QoS |
⚠️ 编程陷阱：安全检查放在控制器外面（不在实时循环内）
错误做法：安全检查在一个独立的低频线程中运行（如 10 Hz），而控制循环以 500 Hz 运行。
现象：控制器输出了一个危险指令，安全线程可能在 100 ms 后才发现——此时电机已经执行了 50 个危险指令。
根本原因：安全检查的频率必须**不低于**控制器的频率。
正确做法：安全检查是
write()函数的**第一步**——在发送任何指令之前先检查。确保安全检查代码不做内存分配（满足实时约束）。🧠 思维陷阱：认为"有了 Unitree 的内置保护就不需要自己做安全检查"
新手想法："Unitree 主控板已经有扭矩限制和温度保护了，我的代码不需要额外检查。"
实际上：Unitree 的内置保护是**最后一道防线**，不是唯一防线。内置保护触发时通常意味着**已经出了问题**——电流保护触发会直接切断电机，机器人瞬间倒下；温度保护触发会降额，运动能力骤降。你的代码应该在问题**即将发生**时就干预（如温度 70 degC 时就降低负载，而不是等到 80 degC 触发硬件保护）。
正确思维：安全保护是多层冗余设计——你的软件层保护让 99% 的危险情况在到达硬件保护之前就被处理。
练习¶
练习 62.9.1：设计一个 Go2 的完整安全参数表。对 12 个关节分别设定：关节位置上下限、最大速度、最大扭矩、温度警告/危险阈值。参考 Go2 的 URDF 和官方文档。
练习 62.9.2（编程）：实现一个 Watchdog Timer。规则：如果控制器在连续 3 个周期内没有发送新指令（可能是计算超时或崩溃），自动触发 soft stop。提示：用一个计数器记录"已经多少个周期没收到新指令"。
练习 62.9.3（思考题）：RL 策略有时会输出"看起来正常但实际危险"的指令——比如让两条腿交叉、让膝关节反向弯曲。这些指令不违反单关节的限位，但会导致结构自碰撞。如何检测和防止这类问题？提示：self-collision checking（用碰撞几何体做前向运动学后检查）。
62.10 本章小结¶
知识点总结¶
| 节 | 主题 | 核心概念 | 难度 |
|---|---|---|---|
| 62.1 | 硬件全景 | 控制循环层次结构，从规划到电机的完整链路 | ⭐ |
| 62.2 | 驱动器与电机 | QDD vs SEA vs 谐波减速器，反向驱动性，\(I_{\text{eff}} = N^2 I_m\) | ⭐⭐ |
| 62.3 | 电机控制基础 | FOC 原理，Clark/Park 变换，\(\tau = K_t \cdot i_q\)，三层控制环 | ⭐⭐ |
| 62.4 | 通信总线 | CAN vs EtherCAT vs UDP，带宽/延迟/确定性对比 | ⭐⭐ |
| 62.5 | EtherCAT 详解 | 飞越处理，PDO/SDO，分布式时钟，SOEM API | ⭐⭐⭐ |
| 62.6 | 传感器栈 | IMU 选型（ICM-42688/BMI088/ADIS16470），编码器，接触传感器 | ⭐⭐ |
| 62.7 | Unitree SDK | SDK2 架构，HighLevel/LowLevel 模式，安全机制 | ⭐⭐ |
| 62.8 | ros2_control HW | HardwareInterface 实现，仿真/真机无缝切换 | ⭐⭐ |
| 62.9 | 安全与保护 | 多层安全，故障排查，soft stop | ⭐ |
核心公式回顾¶
| 公式 | 含义 | 所在节 |
|---|---|---|
| \(I_{\text{eff}} = N^2 I_m\) | 等效关节侧转动惯量（reflected inertia） | 62.2 |
| \(\tau = K_t \cdot i_q\) | 扭矩-电流关系（FOC 核心） | 62.3 |
| \(\tau = k_p(q_d - q) + k_d(\dot{q}_d - \dot{q}) + \tau_{\text{ff}}\) | Unitree 关节控制律 | 62.3/62.7 |
累积项目：本章新增模块¶
项目：从零构建四足站立控制器
本章新增**硬件接口层**：
Ch47: 加载 URDF → Pinocchio 正/逆运动学
Ch49: 浮动基座动力学模型
Ch53: WBC 控制器（QP 求解）
Ch57: 状态估计器（KF/InEKF）
Ch61: 实时 C++ 框架 + ros2_control Controller 插件
Ch62: ★ ros2_control HardwareInterface (UnitreeHW) ← 本章
★ 安全检查模块 (SafetyChecker) ← 本章
★ IMU 标定工具 ← 本章
本章完成后，你的累积项目已经具备了从算法到硬件的完整链路——理论上可以在 Go2 真机上运行。下一步 Ch63-64 将添加 RL 策略训练和部署模块。
延伸阅读¶
必读¶
| 资料 | 类型 | 难度 | 说明 |
|---|---|---|---|
| Katz et al. (2019) "Mini Cheetah", ICRA | 论文 | ⭐⭐ | QDD 驱动器设计理念，四足硬件架构的经典范例 |
| Wensing et al. (2017) "Proprioceptive Actuator Design in the MIT Cheetah", T-RO | 论文 | ⭐⭐⭐ | QDD 本体感受力控的理论基础，反向驱动性分析 |
| Unitree SDK2 官方文档 (support.unitree.com) | 文档 | ⭐ | Go2/B2 开发必备，API 参考 |
推荐¶
| 资料 | 类型 | 难度 | 说明 |
|---|---|---|---|
| Seok et al. (2015) "Design Principles for Energy-Efficient Legged Locomotion", T-Mech | 论文 | ⭐⭐⭐ | 电机设计优化的系统方法论 |
| Hutter et al. (2016) "ANYmal - a Highly Mobile and Dynamic Quadrupedal Robot", IROS | 论文 | ⭐⭐ | SEA 方案的代表，与 QDD 形成对比 |
| ODRI "Open Dynamic Robot Initiative" (github.com/open-dynamic-robot-initiative) | 开源 | ⭐⭐ | 开源硬件设计，含 solo8/solo12 电机驱动器 |
| ethercat_driver_ros2 (github.com/ICube-Robotics) | 开源 | ⭐⭐⭐ | EtherCAT + ros2_control 集成的实战参考 |
| SimpleFOC (simplefoc.com) | 开源 | ⭐⭐ | 开源 FOC 库，适合理解 FOC 底层实现 |
进阶¶
| 资料 | 类型 | 难度 | 说明 |
|---|---|---|---|
| EtherCAT Technology Group 官方文档 (ethercat.org) | 文档 | ⭐⭐⭐ | EtherCAT 协议的权威规范 |
| MJBots moteus 驱动器 (github.com/mjbots/moteus) | 开源 | ⭐⭐⭐ | 开源 QDD 电机驱动器，T-Motor 的开源替代 |
| "Cycloidal QDD Actuator Designs with Learning-based Torque Estimation" (arXiv 2410.16591) | 论文 | ⭐⭐⭐⭐ | 用 ML 学习电机扭矩模型来补偿传动非理想特性 |
与其他章节的衔接¶
向前承接： - Ch57（IMU + 状态估计）→ 62.6 传感器栈从硬件角度补充了 Ch57 的传感器模型 - Ch61（实时 C++ + ros2_control）→ 62.8 的 HardwareInterface 是 Ch61 Controller 的"下层对接" - Ch53（WBC）→ 62.3 的电机控制律是 WBC 扭矩输出的"最后一站"
向后指向： - 62.7 Unitree SDK → Ch68 legged_control 精读（完整的 SDK 集成实例） - 62.8 ros2_control HW → Ch69 Mini-Legged 综合实战（真机部署） - 62.2/62.3 电机特性 → Ch64 RL 部署中的 actuator network（学习电机的非理想特性）
上一章（Ch61）搭好了实时 C++ 和 ros2_control 的"软件地基"。本章把硬件栈的每一层——从 BLDC 电机的 FOC 控制到 CAN/EtherCAT 通信，从 IMU 传感器选型到 Unitree SDK 的安全机制——逐一拆解。到这里，算法（Ch47-60）到硬件（Ch61-62）的完整链路已经建立。下一步 Ch63-64 将进入强化学习在腿足上的训练与部署——这是当前腿足控制最活跃的研究方向。Ch63 侧重 RL 策略训练（Python + IsaacLab），Ch64 侧重部署（C++ + LibTorch/ONNX），两章的衔接正是"Sim-to-Real"的核心工程挑战——而本章 62.2-62.3 讲的电机动力学，恰恰是 Sim-to-Real gap 的重要来源之一。