6.3 底层控制与逆动力学：从关节指令到电机力矩

文档摘要

6.3 底层控制与逆动力学：从关节指令到电机力矩轨迹规划给了「关节角度随时间变化」的目标曲线，但电机不直接吃角度——它吃电流（力矩）。怎么让关节精确跟踪轨迹？这一层就是底层控制。 6.3.1 控制问题的形式化轨迹跟踪控制问题：给定：期望关节轨迹 $qd(t), \dot{q}d(t), \ddot{q}d(t)$。当前：实际关节状态 $q(t), \dot{q}(t)$（编码器读）。输出：电机力矩 $\tau(t)$。目标：让 $q(t) \to qd(t)$，跟踪误差 $\to 0$。这是经典控制理论的核心问题。 6.3.

6.3 底层控制与逆动力学：从关节指令到电机力矩

轨迹规划给了「关节角度随时间变化」的目标曲线，但电机不直接吃角度——它吃电流（力矩）。怎么让关节精确跟踪轨迹？这一层就是底层控制。

6.3.1 控制问题的形式化

轨迹跟踪控制问题：

给定：期望关节轨迹 q_d(t), \dot{q}_d(t), \ddot{q}_d(t)。
当前：实际关节状态 q(t), \dot{q}(t)（编码器读）。
输出：电机力矩 \tau(t)。
目标：让 q(t) \to q_d(t)，跟踪误差 \to 0。

这是经典控制理论的核心问题。

6.3.2 PID 控制：最简单也最常用

PID（Proportional-Integral-Derivative） 是工业最常用的控制器：

\tau(t) = K_p e(t) + K_i \int_0^t e(t') dt' + K_d \dot{e}(t)

其中 e(t) = q_d(t) - q(t) 是跟踪误差。

项	作用	增大效果
P（比例）	误差大就用力大	响应快但易振荡
I（积分）	消除稳态误差	消除静差但可能超调
D（微分）	抑制误差变化	阻尼振荡但对噪声敏感

PID 简单到无需模型，调试到「能用」很容易。但高性能跟踪需要更精细的方法——因为 PID 忽略了机器人动力学。

6.3.3 机器人动力学：控制的基础

机械臂的动力学方程（拉格朗日方程）：

M(q) \ddot{q} + C(q, \dot{q}) \dot{q} + G(q) = \tau

各项含义：

M(q)：惯性矩阵（n×n），关节角的函数。
C(q, \dot{q}) \dot{q}：科氏力与离心力。
G(q)：重力项。
\tau：关节力矩。
q, \dot{q}, \ddot{q}：关节角、角速度、角加速度。

这是正向动力学——给定力矩求运动。控制的反向问题是逆动力学——给定期望运动求所需力矩：

\tau = M(q) \ddot{q}_d + C(q, \dot{q}) \dot{q} + G(q)

逆动力学是计算力矩控制的基础。

6.3.4 计算力矩控制（Computed Torque）

计算力矩控制（也叫反馈线性化、inverse dynamics control）用逆动力学 + 反馈修正：

\tau = M(q) [\ddot{q}_d + K_p e + K_d \dot{e}] + C(q, \dot{q}) \dot{q} + G(q)

把这个 \tau 代入动力学方程：

M(q) \ddot{q} + C \dot{q} + G = M(q) [\ddot{q}_d + K_p e + K_d \dot{e}] + C \dot{q} + G

两边消去，得：

\ddot{q} = \ddot{q}_d + K_p e + K_d \dot{e} \Rightarrow \ddot{e} + K_d \dot{e} + K_p e = 0

误差动态是一个线性二阶系统！只要选 K_p, K_d 让这个二阶系统稳定，跟踪误差指数收敛到零。


def computed_torque(q, dq, q_d, dq_d, ddq_d, K_p, K_d, model):
    e = q_d - q
    de = dq_d - dq
    # 逆动力学 + 反馈
    ddq_ref = ddq_d + K_p * e + K_d * de
    tau = model.M(q) @ ddq_ref + model.C(q, dq) @ dq + model.G(q)
    return tau

计算力矩的优势

理论精确：完美模型下，跟踪误差指数收敛。
解耦：每个关节的误差动态独立（被 M 解耦）。
可调性能：K_p, K_d 决定收敛速度和阻尼。

计算力矩的局限

依赖模型：模型不准时性能下降。
鲁棒性：未建模的摩擦、外扰会让跟踪误差不为零。

💡 工业现实：纯计算力矩在模型完美时极好，但实际机器人有摩擦、间隙、负载变化，模型不可能完美。所以实际系统常用「计算力矩 + 鲁棒项」或「自适应控制」补偿模型误差。

6.3.5 自适应控制：应对模型不确定性

自适应控制（Adaptive Control）在模型参数不确定时，在线估计参数：

假设机器人质量参数未知。
设计参数估计律，根据跟踪误差在线更新估计。
用估计参数计算力矩。

代表方法如 Slotine-Li 自适应控制，能保证跟踪误差和参数估计误差都有界。

自适应控制适合负载变化大的场景（机械臂抓不同重量物体）。

6.3.6 模型预测控制（MPC）

模型预测控制（Model Predictive Control, MPC） 是更现代的方法：

工作流程：

当前状态 x_t 已知。
用模型预测未来 H 步的状态（horizon）。
求解最优化问题：让预测状态轨迹最优（跟踪误差、能量、约束）。
只执行最优控制序列的第一个 u_t。
下一时刻重复。

MPC 优化问题

\min_{u_{0:H-1}} \sum_{k=0}^{H-1} \|x_k - x_k^d\|_Q^2 + \|u_k\|_R^2

约束：

动力学约束 x_{k+1} = f(x_k, u_k)。
状态约束（关节限制、碰撞）。
输入约束（力矩限制）。

MPC vs 计算力矩 vs PID

方法	模型	约束处理	计算成本	性能
PID	不需要	不处理	极低	中（无模型限制）
计算力矩	需要精确模型	不处理	中	高（模型准时）
MPC	需要	显式处理	高（每步优化）	高 + 可处理约束

💡 MPC 的崛起：MPC 之所以成为高端机器人（人形、自动驾驶）的主流，核心是它显式处理约束——力矩不超限、关节不超限、不撞障碍、保持任务约束，这些都能写进 MPC 的优化问题。计算力矩做不到。

实时 MPC 的工程

MPC 的挑战是计算成本——每步都要解一个优化。实时 MPC（100-1000Hz）需要：

短 horizon（H = 5-20）。
高效求解器（OSQP、HPIPM、acados）。
GPU/FPGA 加速。
显式 MPC（预先离线解出 lookup table）。

现代工具如 acados、CasADi、HPIPM 让实时 MPC 在 1kHz 以下频率成为可能。

6.3.7 Whole-Body Control：人形机器人的核心

人形机器人有 30+ 自由度，且要保持平衡——传统单关节控制不够，需要全身控制（Whole-Body Control, WBC）。

WBC 把整个机器人建模为一个浮基系统：

基座 6 DoF（位置 + 姿态）。
关节 DoF（如 30）。
接触约束（脚与地面的接触力）。

WBC 的优化问题：

\min \quad \text{tracking error of tasks} + \text{effort cost}

约束：

浮基动力学。
接触互补约束（脚要么接触要么离开，不能中间）。
摩擦锥约束（接触力在摩擦锥内）。
关节、力矩限制。

任务（task）可以叠加：

主任务：保持平衡（质心加速度目标）。
次任务：手部跟踪轨迹。
再次任务：关节靠近中立位。

代表实现如 OSC（Operational Space Control）、Hierarchical WBC。波士顿动力 Atlas、Tesla Optimus 都用 WBC 做底层控制。

⚠️ WBC 的难度：浮基系统维度高、约束复杂、非线性强，实时求解 WBC 是当前机器人控制最前沿的工程难题之一。很多团队用 QP-based WBC + MPC 高层规划的层次架构。

6.3.8 雅可比与操作空间控制

机械臂末端在工作空间（笛卡尔空间）的运动与关节空间的运动通过雅可比矩阵（Jacobian） 关联：

\dot{x}_{end} = J(q) \dot{q}

雅可比的逆可用于操作空间控制（Operational Space Control, OSC）：

直接控制末端位置/力（而非关节角）。
在任务空间定义控制律，通过逆雅可比转换为关节力矩。
自然处理冗余机械臂（n 关节 > 6 任务自由度）。

OSC 是接触丰富任务（擦拭、抛光、与人协作）的常用方法，第 6.4 节的阻抗控制就是 OSC 的特例。

6.3.9 控制方法的选择决策树

6.3.10 学习型控制

最近几年，学习型控制兴起：

强化学习控制：用 RL 在仿真中学底层控制策略（如四足、人形平衡）。
模仿学习控制：从专家轨迹学控制器。
Neural-MPC：用神经网络替代 MPC 部分模块（成本预测、动力学模型）。
Diffusion Policy 直接出力矩：第 5 章 VLA 直接输出动作，本质绕过传统控制。

学习型控制在高维、非线性、难以建模的系统（双足人形、柔性机器人）上有优势，但牺牲了安全保证。当前趋势是传统控制 + 学习增强的混合方案。

6.3.11 控制频率分层

最后强调频率分层的重要性：

频率	任务	方法
1-10 Hz	全局规划	RRT*、LLM 规划
10-30 Hz	策略生成	VLA、扩散策略
30-100 Hz	轨迹优化	MPC、TOPP
100-1000 Hz	底层控制	PID、计算力矩
1k-10k Hz	电流环	FOC、电流控制

每一层为下一层提供参考，下一层向上一层反馈状态。这是现代机器人系统的标准架构。

本节小结

轨迹跟踪控制从「期望关节角」到「电机力矩」，是闭环的最后一环。
PID 简单但忽略动力学；计算力矩用逆动力学 + 反馈线性化，理论精确。
自适应控制处理模型不确定性，适合负载变化大的场景。
MPC 用滚动优化显式处理约束，是高端机器人主流。
Whole-Body Control 处理浮基人形机器人，是控制工程前沿。
雅可比与操作空间控制用于直接控制末端，是接触任务基础。
学习型控制在高维难建模系统有优势，但牺牲安全保证。
现代机器人系统按 1Hz-10kHz 频率分层：规划→策略→轨迹→控制→电流。

下一节《6.4 精细操作与力控》将讨论接触丰富任务的特殊控制——阻抗、导纳、力位混合控制。