第11章：全身控制基础

开篇导言

全身控制（Whole-Body Control, WBC）是现代机器人控制的核心技术，它将复杂的多自由度机器人系统视为一个整体，通过统一的优化框架同时处理运动生成、力控制和约束满足。与传统的分层控制不同，WBC能够充分利用机器人的所有自由度，实现更加自然、高效的运动。本章将深入探讨WBC的理论基础，包括任务空间控制、优先级管理和优化框架，并通过HRP-2人形机器人的案例展示这些理论的实际应用。

11.1 任务空间控制理论

11.1.1 从关节空间到任务空间

传统的机器人控制通常在关节空间进行，但实际任务往往定义在笛卡尔空间或其他任务相关的空间中。任务空间控制的核心思想是直接在任务空间中指定期望行为，然后映射到关节空间执行。这种方法的优势在于：任务目标直观明确、便于人机交互、自然处理约束。

任务空间的选择取决于具体应用。常见的任务空间包括：

• 笛卡尔空间：末端执行器的位置和姿态（SE(3)）
• 质心空间：机器人整体质心的位置和动量
• 接触空间：接触点的位置和作用力
• 视觉空间：图像特征或视觉伺服误差
• 混合空间：结合多个任务描述的增广空间

考虑一个n自由度机器人，其关节配置为 $\mathbf{q} \in \mathbb{R}^n$，任务空间变量为 $\mathbf{x} \in \mathbb{R}^m$，两者通过前向运动学关联：

$$\mathbf{x} = \mathbf{f}(\mathbf{q})$$ 对时间求导可得速度级运动学： $$\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}$$ 其中 $\mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}}{\partial \mathbf{q}}$ 是任务雅可比矩阵。

进一步对时间求导得到加速度级运动学： $$\ddot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\ddot{\mathbf{q}} + \dot{\mathbf{J}}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}$$ 这里 $\dot{\mathbf{J}}\dot{\mathbf{q}}$ 项表示由于雅可比矩阵时变引起的加速度，在高速运动时不可忽略。

雅可比矩阵的几何意义深刻：

• 列空间：表示可达的任务空间速度方向
• 零空间：表示不影响任务的内部运动
• 奇异值：反映各方向的运动传递效率
• 条件数：衡量运动学的各向同性

11.1.2 操作空间动力学

为了在任务空间中进行力控制，我们需要推导操作空间动力学。这种表述由Khatib在1987年提出，成为现代机器人控制的基础。操作空间动力学的核心贡献是将复杂的关节空间动力学转换为任务空间的简洁形式，使控制器设计更加直观。

从机器人的关节空间动力学出发： $$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}^T\mathbf{F}_{\text{ext}}$$ 其中：

• $\mathbf{M}(\mathbf{q})$：质量矩阵
• $\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})$：科氏力和离心力矩阵
• $\mathbf{g}(\mathbf{q})$：重力向量
• $\boldsymbol{\tau}$：关节力矩
• $\mathbf{F}_{\text{ext}}$：外部作用力

通过运动学约束 $\ddot{\mathbf{x}} = \mathbf{J}\ddot{\mathbf{q}} + \dot{\mathbf{J}}\dot{\mathbf{q}}$，并利用动力学方程求解 $\ddot{\mathbf{q}}$，可以推导出操作空间动力学。

推导过程的关键步骤：

1. 从动力学方程解出：$\ddot{\mathbf{q}} = \mathbf{M}^{-1}(\boldsymbol{\tau} + \mathbf{J}^T\mathbf{F}_{\text{ext}} - \mathbf{C}\dot{\mathbf{q}} - \mathbf{g})$
2. 代入运动学约束：$\ddot{\mathbf{x}} = \mathbf{J}\mathbf{M}^{-1}(\boldsymbol{\tau} + \mathbf{J}^T\mathbf{F}_{\text{ext}} - \mathbf{C}\dot{\mathbf{q}} - \mathbf{g}) + \dot{\mathbf{J}}\dot{\mathbf{q}}$
3. 定义任务空间控制力：$\mathbf{F} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}\mathbf{J}\mathbf{M}^{-1}\boldsymbol{\tau}$
4. 整理得到操作空间动力学： $$\boldsymbol{\Lambda}(\mathbf{q})\ddot{\mathbf{x}} + \boldsymbol{\mu}(\mathbf{q}, \dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}) = \mathbf{F} + \mathbf{F}_{\text{ext}}$$ 其中：

• $\boldsymbol{\Lambda} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$：操作空间惯性矩阵，反映任务空间的等效惯性
• $\boldsymbol{\mu} = \boldsymbol{\Lambda}\mathbf{J}\mathbf{M}^{-1}\mathbf{C}\dot{\mathbf{q}} - \boldsymbol{\Lambda}\dot{\mathbf{J}}\dot{\mathbf{q}}$：操作空间科氏力和离心力
• $\mathbf{p} = \boldsymbol{\Lambda}\mathbf{J}\mathbf{M}^{-1}\mathbf{g}$：操作空间重力补偿项
• $\mathbf{F}$：任务空间控制力

操作空间惯性矩阵 $\boldsymbol{\Lambda}$ 具有重要的物理意义：

• 它是配置相关的，在奇异配置附近会急剧增大
• 其特征值反映了各方向的惯性特性
• 可用于设计解耦的任务空间控制器

关节力矩与任务空间力的映射关系： $$\boldsymbol{\tau} = \mathbf{J}^T\mathbf{F} + \mathbf{N}^T\boldsymbol{\tau}_0$$ 其中第二项是零空间力矩，不产生任务空间力但影响内部配置。

11.1.3 运动学冗余与零空间

当 $m < n$ 时，机器人具有运动学冗余，存在无穷多组关节配置可以实现相同的任务空间位置。零空间投影是处理冗余的关键工具。冗余自由度为机器人提供了额外的灵活性：避障能力、关节限位回避、优化次要目标、提高容错性。

冗余度的定量描述：

• 局部冗余度：$r = n - \text{rank}(\mathbf{J})$
• 任务冗余度：$r_t = n - m$（当雅可比满秩时）
• 有效冗余度：考虑关节限位和障碍物后的实际可用自由度

雅可比矩阵的Moore-Penrose伪逆为： $$\mathbf{J}^{\dagger} = \mathbf{M}^{-1}\mathbf{J}^T(\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$$ 这是动力学一致的伪逆，相比于运动学伪逆 $\mathbf{J}^{\dagger}_{\text{kin}} = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T)^{-1}$，它考虑了惯性分布，产生的运动在动力学意义下是最优的。

不同伪逆的选择准则：

• 运动学伪逆：最小化关节速度范数 $|\dot{\mathbf{q}}|^2$
• 动力学伪逆：最小化瞬时动能 $\frac{1}{2}\dot{\mathbf{q}}^T\mathbf{M}\dot{\mathbf{q}}$
• 加权伪逆：最小化 $\dot{\mathbf{q}}^T\mathbf{W}\dot{\mathbf{q}}$，可根据任务调整权重

零空间投影矩阵为： $$\mathbf{N} = \mathbf{I} - \mathbf{J}^{\dagger}\mathbf{J}$$ 任何投影到零空间的运动都不会影响任务空间行为： $$\dot{\mathbf{q}} = \mathbf{J}^{\dagger}\dot{\mathbf{x}} + \mathbf{N}\dot{\mathbf{q}}_0$$ 其中 $\dot{\mathbf{q}}_0$ 是任意的关节速度，可用于优化次要目标。

零空间运动的典型应用：

1. 奇异性回避：$\dot{\mathbf{q}}_0 = k_s\nabla_q\text{det}(\mathbf{J}\mathbf{J}^T)$
2. 关节限位回避：$\dot{\mathbf{q}}_0 = -k_l\nabla_q H(\mathbf{q})$，其中$H$是限位势函数
3. 姿态优化：$\dot{\mathbf{q}}_0 = k_p(\mathbf{q}_{\text{nominal}} - \mathbf{q})$
4. 能量最小化：$\dot{\mathbf{q}}_0 = -k_e\nabla_q V(\mathbf{q})$，其中$V$是势能

零空间的动态特性也很重要。零空间动力学可表示为： $$\mathbf{M}\mathbf{N}\ddot{\mathbf{q}} + (\mathbf{C}\mathbf{N} + \mathbf{M}\dot{\mathbf{N}})\dot{\mathbf{q}} = \mathbf{N}^T\boldsymbol{\tau}_0$$ 这表明零空间运动需要考虑惯性和科氏效应。

        任务空间
           ↑
           | J
           |
      关节空间 ←------ N -------- 零空间
                              (冗余自由度)

11.2 运动与力控制的统一框架

11.2.1 混合运动/力控制

实际任务通常需要同时控制位置和力。例如，擦拭表面需要在法向维持接触力，在切向跟踪轨迹。统一框架通过选择矩阵实现这种混合控制。Mason在1981年提出的“顺应性选择矩阵”(Compliance Selection Matrix)成为该领域的奥基石。

混合控制的基本原理是将任务空间分解为两个正交子空间：

• 运动控制子空间：要求精确的位置/速度跟踪
• 力控制子空间：要求精确的力/力矩控制

这两个子空间必须满足正交性约束：$\mathbf{S}\overline{\mathbf{S}} = \mathbf{0}$，确保没有方向同时进行位置和力控制。

定义选择矩阵 $\mathbf{S}$ 和其补 $\overline{\mathbf{S}} = \mathbf{I} - \mathbf{S}$，分别选择运动控制和力控制的方向。

对于简单任务，选择矩阵通常是对角矩阵： $$\mathbf{S} = \text{diag}(s_1, s_2, ..., s_m), \quad s_i \in \{0, 1\}$$ 对于复杂的接触任务，可能需要旋转到适当的坐标系： $$\mathbf{S} = \mathbf{R}\mathbf{S}_{\text{diag}}\mathbf{R}^T$$ 其中$\mathbf{R}$是旋转矩阵，将任务坐标系与接触坐标系对齐。

控制律为： $$\mathbf{F} = \mathbf{S}(\mathbf{F}_{\text{motion}} + \mathbf{F}_{\text{feedforward}}) + \overline{\mathbf{S}}\mathbf{F}_{\text{force}}$$ 其中：

• $\mathbf{F}_{\text{motion}} = \boldsymbol{\Lambda}(\ddot{\mathbf{x}}_d + \mathbf{K}_p\mathbf{e}_p + \mathbf{K}_d\mathbf{e}_v)$：运动控制项
• $\mathbf{F}_{\text{force}} = \mathbf{F}_d + \mathbf{K}_f(\mathbf{F}_d - \mathbf{F}_{\text{measured}})$：力控制项
• $\mathbf{F}_{\text{feedforward}} = \boldsymbol{\mu} + \mathbf{p}$：动力学补偿

实际应用中的关键问题：

1. 力传感器噪声：需要低通滤波，但会引入相位滞后
2. 接触刚度估计：环境刚度影响力控制增益选择
3. 切换瞬态：运动/力控制模式切换时的平滑过渡
4. 稳定性保证：高刚度环境下的力控制稳定性

典型应用场景：

插孔任务：      研磨任务：      装配任务：
   Z(力)          Z(力)         Z(力)
    |              |             |
    v              v             v
  [peg]        [tool]        [part]
    |           ~~~~          ====
  [hole]       surface       fixture
  X,Y(位置)     X,Y(位置)     X,Y,Rz(位置)

11.2.2 阻抗控制

阻抗控制提供了另一种统一运动和力控制的方法，通过指定期望的动力学行为。与混合控制不同，阻抗控制不需要显式地分离运动和力控制方向，而是通过调节机器人的“柔顺性”来适应环境。

阻抗控制的核心思想是将机器人表现为一个虚拟的机械系统（质量-弹簧-阻尼系统）： $$\mathbf{M}_d(\ddot{\mathbf{x}} - \ddot{\mathbf{x}}_d) + \mathbf{D}_d(\dot{\mathbf{x}} - \dot{\mathbf{x}}_d) + \mathbf{K}_d(\mathbf{x} - \mathbf{x}_d) = \mathbf{F}_{\text{ext}}$$ 其中 $\mathbf{M}_d$、$\mathbf{D}_d$、$\mathbf{K}_d$ 分别是期望的惯性、阻尼和刚度矩阵。

阻抗参数的选择准则：

• 惯性矩阵 $\mathbf{M}_d$：通常选择为对角矩阵，决定系统的惯性响应
• 阻尼矩阵 $\mathbf{D}_d$：根据临界阻尼条件：$\mathbf{D}_d = 2\sqrt{\mathbf{M}_d\mathbf{K}_d}$
• 刚度矩阵 $\mathbf{K}_d$：根据任务需求，低刚度提高柔顺性，高刚度提高精度

不同阻抗模式的特点：

• 纯刚度控制：$\mathbf{M}_d = \mathbf{D}_d = \mathbf{0}$，适用于静态任务
• 纯阻尼控制：$\mathbf{M}_d = \mathbf{K}_d = \mathbf{0}$，适用于速度控制
• 惯性解耦：$\mathbf{M}_d = \boldsymbol{\Lambda}$，消除惯性耦合

控制律为： $$\mathbf{F} = \boldsymbol{\Lambda}\mathbf{M}_d^{-1}[\mathbf{K}_d(\mathbf{x}_d - \mathbf{x}) + \mathbf{D}_d(\dot{\mathbf{x}}_d - \dot{\mathbf{x}}) - \mathbf{F}_{\text{ext}}] + \boldsymbol{\Lambda}\ddot{\mathbf{x}}_d + \mathbf{F}_{\text{feedforward}}$$ 这个控制律确保闭环系统具有期望的阻抗行为。在实际应用中，可以通过变阻抗控制动态调整参数： $$\mathbf{K}_d = \mathbf{K}_{d,0} + \alpha(t)\mathbf{K}_{d,\Delta}$$ 其中$\alpha(t)$是根据任务阶段或环境感知调整的自适应参数。

11.2.3 接触约束处理

浮动基座机器人（如人形机器人）的全身控制必须考虑接触约束。与固定基座机器人不同，浮动基座机器人的基座本身是可动的，必须通过接触力来维持平衡和产生运动。

接触约束的数学表述包括运动学和动力学两个方面。接触点的加速度约束为： $$\mathbf{J}_c\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c\dot{\mathbf{q}} = \mathbf{0}$$ 这表示接触点必须保持静止（粘性接触）或按指定轨迹运动（滑动接触）。

通过拉格朗日乘数法，可以导出约束力： $$\mathbf{F}_c = \boldsymbol{\Lambda}_c(\mathbf{J}_c\mathbf{M}^{-1}\boldsymbol{\tau} + \dot{\mathbf{J}}_c\dot{\mathbf{q}} - \mathbf{J}_c\mathbf{M}^{-1}(\mathbf{C}\dot{\mathbf{q}} + \mathbf{g}))$$ 其中 $\boldsymbol{\Lambda}_c = (\mathbf{J}_c\mathbf{M}^{-1}\mathbf{J}_c^T)^{-1}$ 是接触空间的有效惯性。

接触约束的分类：

1. 单侧约束：只能产生推力，不能产生拉力
2. 双侧约束：可产生推拉力（如抓取）
3. 滑动约束：允许切向滑动，需要摩擦模型

接触力必须满足物理约束（如摩擦锥）： $$\mathbf{F}_c \in \mathcal{K}$$ 其中 $\mathcal{K}$ 是可行接触力集合。

对于点接触，库仑摩擦锥约束为： $$\sqrt{f_x^2 + f_y^2} \leq \mu f_z, \quad f_z \geq 0$$ 对于面接触（如脚掌），还需考虑压心（CoP）约束： $$\mathbf{p}_{\text{CoP}} \in \mathcal{S}_{\text{support}}$$ 其中$\mathcal{S}_{\text{support}}$是支撑面。这些约束可以线性化为： $$\mathbf{A}_{\text{friction}}\mathbf{F}_c \leq \mathbf{b}_{\text{friction}}$$ 使得在QP框架中求解。

11.3 优先级任务管理

机器人系统通常需要同时满足多个任务目标，这些任务可能相互冲突。优先级任务管理提供了系统化的方法来处理这些冲突，确保关键任务（如安全约束）始终得到满足，同时尽可能实现次要目标。

优先级管理的基本原则：

1. 安全优先：物理约束和安全限制始终最高优先级
2. 任务分层：根据重要性将任务分成不同层次
3. 冗余利用：利用剩余自由度实现低优先级任务
4. 冲突解决：明确定义冲突时的处理策略

11.3.1 严格优先级方法

当多个任务存在冲突时，优先级管理确保高优先级任务得到满足。严格优先级通过零空间投影实现，保证低优先级任务不会干扰高优先级任务：

第1级任务（最高优先级）： $$\dot{\mathbf{q}}_1 = \mathbf{J}_1^{\dagger}\dot{\mathbf{x}}_1$$ 这里使用伪逆确保最小范数解。

第2级任务（在第1级零空间中）： $$\dot{\mathbf{q}}_2 = \dot{\mathbf{q}}_1 + (\mathbf{J}_2\mathbf{N}_1)^{\dagger}(\dot{\mathbf{x}}_2 - \mathbf{J}_2\dot{\mathbf{q}}_1)$$ 其中 $\mathbf{N}_1 = \mathbf{I} - \mathbf{J}_1^{\dagger}\mathbf{J}_1$ 是第1级任务的零空间投影矩阵。

第二项 $(\mathbf{J}_2\mathbf{N}_1)^{\dagger}(\dot{\mathbf{x}}_2 - \mathbf{J}_2\dot{\mathbf{q}}_1)$ 的意义：

• $\mathbf{J}_2\mathbf{N}_1$：任务2在任务1零空间中的有效雅可比
• $\dot{\mathbf{x}}_2 - \mathbf{J}_2\dot{\mathbf{q}}_1$：任务2的剩余误差
• 整体：在不影响任务1的前提下最大程度满足任务2

一般地，第k级任务： $$\dot{\mathbf{q}}_k = \dot{\mathbf{q}}_{k-1} + (\mathbf{J}_k\mathbf{N}_{k-1})^{\dagger}(\dot{\mathbf{x}}_k - \mathbf{J}_k\dot{\mathbf{q}}_{k-1})$$ 其中累积零空间投影矩阵递归计算： $$\mathbf{N}_{k-1} = \mathbf{N}_{k-2} - (\mathbf{J}_{k-1}\mathbf{N}_{k-2})^{\dagger}(\mathbf{J}_{k-1}\mathbf{N}_{k-2})$$ 初始条件：$\mathbf{N}_0 = \mathbf{I}$

这个递归公式确保：

• $\mathbf{J}_i\mathbf{N}_{k-1} = \mathbf{0}, \forall i < k$（不影响高优先级任务）
• $\text{rank}(\mathbf{N}_{k-1}) = n - \sum_{i=1}^{k-1}\text{rank}(\mathbf{J}_i\mathbf{N}_{i-1})$（剩余自由度）

严格优先级的优缺点：

• 优点：保证高优先级任务完全满足，明确的层次结构
• 缺点：不连续性（任务激活/停用时），计算复杂，可能导致低优先级任务完全无法满足

优先级层次结构：
┌─────────────────┐
│   任务1（最高）   │ ← 平衡控制       │ 安全关键
├─────────────────┤                  │
│      任务2       │ ← 末端执行器位置  │ 主要任务
├─────────────────┤                  │
│      任务3       │ ← 姿态调整       │ 辅助任务
├─────────────────┤                  │
│   任务4（最低）   │ ← 关节限位回避   │ 优化目标
└─────────────────┘

零空间维度变化：
层次  任务维度  剩余自由度
 0      -        n
 1      m₁       n-m₁
 2      m₂       n-m₁-m₂
 3      m₃       n-m₁-m₂-m₃
 ...

11.3.2 加权优先级方法

加权方法通过优化问题软性处理优先级，避免了严格优先级的不连续性问题： $$\min_{\dot{\mathbf{q}}} \sum_{i=1}^{n_t} w_i |\mathbf{J}_i\dot{\mathbf{q}} - \dot{\mathbf{x}}_{i,d}|^2 + w_r|\dot{\mathbf{q}}|^2$$ 其中：

• $w_i$ 是任务权重，通常按数量级递减：$w_1 \gg w_2 \gg ... \gg w_{n_t}$
• $w_r$ 是正则化权重，防止数值不稳定

权重选择策略：

1. 指数递减：$w_i = \alpha^{i-1}w_0$，其中$\alpha \in (0, 1)$
2. 自适应权重：$w_i = f(|\mathbf{e}_i|)$，根据任务误差调整
3. 切换权重：平滑过渡任务激活/停用

这导致闭式解： $$\dot{\mathbf{q}} = (\sum_{i=1}^{n_t} w_i\mathbf{J}_i^T\mathbf{J}_i + w_r\mathbf{I})^{-1}(\sum_{i=1}^{n_t} w_i\mathbf{J}_i^T\dot{\mathbf{x}}_{i,d})$$ 这可以理解为加权伪逆： $$\dot{\mathbf{q}} = \mathbf{J}_w^{\dagger}\dot{\mathbf{x}}_w$$ 其中： $$\mathbf{J}_w = \begin{bmatrix} \sqrt{w_1}\mathbf{J}_1 \\ \sqrt{w_2}\mathbf{J}_2 \\ \vdots \\ \sqrt{w_{n_t}}\mathbf{J}_{n_t} \\ \sqrt{w_r}\mathbf{I} \end{bmatrix}, \quad \dot{\mathbf{x}}_w = \begin{bmatrix} \sqrt{w_1}\dot{\mathbf{x}}_{1,d} \\ \sqrt{w_2}\dot{\mathbf{x}}_{2,d} \\ \vdots \\ \sqrt{w_{n_t}}\dot{\mathbf{x}}_{n_t,d} \\ \mathbf{0} \end{bmatrix}$$ 加权方法的优缺点：

• 优点：连续性好，计算简单，所有任务都有贡献
• 缺点：不能严格保证优先级，权重调整需要经验

11.3.3 层次化二次规划

层次化二次规划（HQP）结合了严格和加权方法的优点，允许在每层内部使用加权优化，层间保持严格优先级：

第k层： $$\begin{aligned} \min_{\ddot{\mathbf{q}}} &\quad |\mathbf{A}_k\ddot{\mathbf{q}} - \mathbf{b}_k|^2 \\ \text{s.t.} &\quad \mathbf{A}_i\ddot{\mathbf{q}} = \mathbf{b}_i^*, \quad i < k \\ &\quad \mathbf{G}\ddot{\mathbf{q}} \leq \mathbf{h} \end{aligned}$$ 其中：

• $\mathbf{b}_i^*$ 是第i层的最优解（固定为等式约束）
• $\mathbf{G}\ddot{\mathbf{q}} \leq \mathbf{h}$ 表示不等式约束（如关节限位、力矩限制）
• $\mathbf{A}_k$、$\mathbf{b}_k$ 可以包含多个相同优先级的任务

HQP的求解算法：

1. 级联求解：

for k = 1 to n_levels:
    解决第k层QP
    将最优解作为下一层的等式约束

2. 零空间投影法： - 将问题投影到上层的零空间 - 减少变量维度，提高求解效率

3. 松弛方法： - 引入松弛变量处理不可行性 - 以最小代价违反低优先级约束

HQP与其他方法的关系：

• 当每层只有一个任务时，退化为严格优先级
• 当只有一层时，等价于加权方法
• 提供了灵活的权衡机制

11.4 全身控制优化框架

11.4.1 基于QP的全身控制器

现代WBC通常表述为QP问题： $$\begin{aligned} \min_{\ddot{\mathbf{q}}, \mathbf{F}_c, \boldsymbol{\tau}} &\quad \sum_{i} w_i|\mathbf{J}_i\ddot{\mathbf{q}} + \dot{\mathbf{J}}_i\dot{\mathbf{q}} - \ddot{\mathbf{x}}_{i,d}|^2 + w_{\tau}|\boldsymbol{\tau}|^2 \\ \text{s.t.} &\quad \mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T\mathbf{F}_c \\ &\quad \mathbf{J}_c\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c\dot{\mathbf{q}} = \mathbf{0} \\ &\quad \mathbf{F}_c \in \mathcal{K} \\ &\quad \boldsymbol{\tau}_{\min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{\max} \\ &\quad \mathbf{q}_{\min} \leq \mathbf{q} + \Delta t\dot{\mathbf{q}} + \frac{\Delta t^2}{2}\ddot{\mathbf{q}} \leq \mathbf{q}_{\max} \end{aligned}$$ 其中：

• 第一个约束是动力学方程
• 第二个约束维持接触
• 第三个约束确保接触力在摩擦锥内
• 后两个约束是执行器和关节限制

11.4.2 控制分配问题

对于过驱动系统，控制分配决定如何分配执行器输出： $$\begin{aligned} \min_{\boldsymbol{\tau}} &\quad |\mathbf{W}\boldsymbol{\tau}|^2 \\ \text{s.t.} &\quad \mathbf{B}\boldsymbol{\tau} = \mathbf{F}_d \\ &\quad \boldsymbol{\tau}_{\min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{\max} \end{aligned}$$ 其中 $\mathbf{B}$ 是控制效率矩阵，$\mathbf{W}$ 是权重矩阵。

11.4.3 鲁棒性考虑

实际系统需要处理模型不确定性和外部扰动。鲁棒WBC方法包括：

1. 参数自适应：在线估计质量、摩擦等参数
2. 扰动观测器：估计并补偿未建模动力学
3. 滑模控制：对匹配不确定性的鲁棒性
4. 约束收紧：预留安全裕度

扰动观测器设计： $$\begin{aligned} \hat{\mathbf{d}} &= \mathbf{L}(\mathbf{s} - \hat{\mathbf{s}}) \\ \dot{\hat{\mathbf{s}}} &= -\mathbf{L}\hat{\mathbf{s}} + \mathbf{L}(\mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} - \boldsymbol{\tau}) \end{aligned}$$ 其中 $\mathbf{s} = \mathbf{M}\dot{\mathbf{q}}$ 是动量，$\mathbf{L}$ 是观测器增益。

11.5 案例研究：HRP-2人形机器人的平衡控制

11.5.1 系统概述

HRP-2是日本产业技术综合研究所（AIST）和川田工业共同开发的人形机器人，具有30个自由度（不含手指）。其全身控制系统是WBC理论的经典应用案例。

系统规格：

• 身高：154 cm
• 重量：58 kg
• 自由度：30 DOF（腿部各6，臂部各6，腰部2，颈部2，手部各2）
• 控制频率：200 Hz（稳定器）+ 5 Hz（规划器）

        HRP-2 关节配置
            头部(2)
              |
         上躯干(2)
         /         \
    左臂(6)      右臂(6)
         \         /
          骨盆(0)
         /      \
    左腿(6)    右腿(6)

11.5.2 平衡控制架构

HRP-2的平衡控制采用多层架构：

第1层：接触稳定（最高优先级） 维持零力矩点（ZMP）在支撑多边形内： $$\mathbf{p}_{zmp} = \frac{\sum_i m_i(\mathbf{r}_i \times (\ddot{\mathbf{r}}_i + \mathbf{g}))_z + \sum_j \mathbf{I}_j\boldsymbol{\omega}_j \times \boldsymbol{\alpha}_j}{\sum_i m_i(\ddot{\mathbf{r}}_i + \mathbf{g})_z}$$ 控制律采用预览控制： $$u(k) = -G_x\sum_{j=1}^{N_L} p(j)\mathbf{e}_{zmp}(k+j) - G_i\sum_{i=0}^{k}\mathbf{e}_{zmp}(i) - G_x\mathbf{x}(k)$$ 第2层：质心控制 跟踪期望的质心轨迹： $$\ddot{\mathbf{r}}_{com}^d = \ddot{\mathbf{r}}_{com}^{ref} + \mathbf{K}_p(\mathbf{r}_{com}^{ref} - \mathbf{r}_{com}) + \mathbf{K}_d(\dot{\mathbf{r}}_{com}^{ref} - \dot{\mathbf{r}}_{com})$$ 质心雅可比： $$\mathbf{J}_{com} = \frac{1}{m_{total}}\sum_i m_i\mathbf{J}_i$$ 第3层：末端执行器任务 控制手部位置用于操作任务： $$\ddot{\mathbf{x}}_{hand}^d = \ddot{\mathbf{x}}_{hand}^{ref} + \mathbf{K}_p^{hand}\mathbf{e}_p + \mathbf{K}_d^{hand}\mathbf{e}_v$$ 第4层：姿态调整 维持上身直立姿态： $$\boldsymbol{\tau}_{posture} = \mathbf{K}_p^{joint}(\mathbf{q}^{ref} - \mathbf{q}) + \mathbf{K}_d^{joint}(\dot{\mathbf{q}}^{ref} - \dot{\mathbf{q}})$$

11.5.3 动态平衡实验

实验1：推力干扰恢复

施加横向推力（50N，0.5秒）时的响应：

     推力响应时间线
时间(s)  0    0.5   1.0   1.5   2.0
COM_y   |----⟋----╲_____|
ZMP_y   |---⟋⟍\_____|
踝扭矩  |__⟋███⟍\___|

控制器通过以下策略恢复平衡：

1. 踝策略：快速调整踝关节力矩
2. 髋策略：调整上身姿态产生反向动量
3. 步行策略：必要时迈步调整支撑面

实验2：斜坡行走

在15°斜坡上行走的关键挑战：

• 重力投影改变
• 接触力分布不均
• 摩擦约束收紧

适应策略： $$\mathbf{F}_{c,modified} = \mathbf{R}_{slope}\mathbf{F}_{c,nominal}$$ $$\mu_{effective} = \mu_{nominal}\cos(\theta) - \sin(\theta)$$

11.5.4 操作任务中的全身协调

双臂搬运任务：

任务分解：

1. 接触维持（优先级1）：保持双脚接触地面
2. 平衡控制（优先级2）：COM在支撑面内
3. 双臂协调（优先级3）：维持物体姿态
4. 关节限位（优先级4）：避免关节超限

协调约束： $$\mathbf{J}_{left}\dot{\mathbf{q}} = -\mathbf{J}_{right}\dot{\mathbf{q}} = \dot{\mathbf{x}}_{object}$$ 内力优化： $$\begin{aligned} \min_{\mathbf{f}_{internal}} &\quad |\mathbf{f}_{internal}|^2 \\ \text{s.t.} &\quad \mathbf{f}_{left} + \mathbf{f}_{right} = \mathbf{f}_{object} \\ &\quad |\mathbf{f}_{internal}| \leq f_{max} \end{aligned}$$

11.5.5 性能评估

计算性能：

• QP求解时间：2-3 ms（QPOASES求解器）
• 总控制周期：5 ms
• CPU使用率：约60%（Intel Core i7）

控制性能指标：

• ZMP跟踪误差：< 2 cm（静态），< 5 cm（动态）
• COM位置误差：< 1 cm
• 末端执行器精度：< 5 mm
• 最大恢复推力：80 N（横向），100 N（前后）

鲁棒性测试结果： | 扰动类型 | 最大可恢复量 | 恢复时间 |

11.6 本章小结

全身控制是现代机器人实现复杂任务的关键技术。本章核心要点：

理论基础：

1. 任务空间控制：通过雅可比映射实现任务空间到关节空间的转换
2. 操作空间动力学：$\boldsymbol{\Lambda}\ddot{\mathbf{x}} + \boldsymbol{\mu} + \mathbf{p} = \mathbf{F}$
3. 零空间利用：$\dot{\mathbf{q}} = \mathbf{J}^{\dagger}\dot{\mathbf{x}} + \mathbf{N}\dot{\mathbf{q}}_0$

控制框架：

1. 混合运动/力控制：通过选择矩阵统一处理
2. 阻抗控制：指定期望的动力学行为
3. 优先级管理：严格优先级vs加权优先级vs HQP

优化方法：

1. QP表述：将WBC问题转化为二次规划
2. 约束处理：动力学、接触、执行器限制
3. 实时求解：利用问题结构加速计算

关键公式汇总：

• 任务雅可比：$\dot{\mathbf{x}} = \mathbf{J}\dot{\mathbf{q}}$
• 动力学方程：$\mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \boldsymbol{\tau} + \mathbf{J}_c^T\mathbf{F}_c$
• 优先级投影：$\mathbf{N}_k = \mathbf{I} - \mathbf{J}_k^{\dagger}\mathbf{J}_k$
• 操作空间惯性：$\boldsymbol{\Lambda} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$

11.7 练习题

基础题

题目 11.1：任务空间映射 一个平面2R机械臂，链接长度为 $l_1 = l_2 = 0.5$ m，关节角度为 $\mathbf{q} = [30°, 60°]^T$。 (a) 计算末端执行器位置 (b) 推导并计算任务雅可比矩阵 (c) 若期望末端速度为 $\dot{\mathbf{x}} = [0.1, 0]^T$ m/s，计算所需的关节速度

提示：使用前向运动学和雅可比定义

题目 11.2：零空间运动 考虑一个3自由度平面机械臂执行2维末端定位任务。 (a) 解释为什么存在冗余自由度 (b) 若雅可比矩阵为 $\mathbf{J} = \begin{bmatrix}1 & 0.5 & 0.2 \\ 0 & 0.866 & 0.6\end{bmatrix}$，计算零空间投影矩阵 (c) 设计一个零空间运动来最小化关节速度范数

提示：使用 $\mathbf{N} = \mathbf{I} - \mathbf{J}^{\dagger}\mathbf{J}$

题目 11.3：优先级任务 两个任务：任务1控制末端位置（2维），任务2保持关节1在期望角度。机器人有3个关节。 (a) 写出严格优先级的解 (b) 如果任务1的雅可比为 $\mathbf{J}_1 = \begin{bmatrix}1 & 1 & 0 \\ 0 & 1 & 1\end{bmatrix}$，任务2的雅可比为 $\mathbf{J}_2 = [1, 0, 0]$，计算综合速度 (c) 讨论任务冲突的情况

提示：使用递归零空间投影

题目 11.4：QP全身控制 设计一个简化的QP问题，控制漂浮基座机器人的质心和末端执行器： (a) 写出目标函数（最小化任务误差和力矩） (b) 列出必要的约束条件 (c) 解释如何处理接触力约束

提示：考虑动力学约束和摩擦锥

挑战题

题目 11.5：操作空间惯性矩阵性质 证明操作空间惯性矩阵 $\boldsymbol{\Lambda} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$ 的以下性质： (a) $\boldsymbol{\Lambda}$ 是对称正定的 (b) 推导 $\boldsymbol{\Lambda}$ 相对于时间的导数 (c) 证明能量守恒：$\frac{1}{2}\dot{\mathbf{q}}^T\mathbf{M}\dot{\mathbf{q}} = \frac{1}{2}\dot{\mathbf{x}}^T\boldsymbol{\Lambda}\dot{\mathbf{x}}$（当无冗余时）

提示：利用矩阵性质和链式法则

题目 11.6：接触一致的零空间运动 漂浮基座机器人在双脚接触地面时，设计保持接触的零空间运动： (a) 推导接触一致的零空间投影 (b) 证明该投影保持接触约束 (c) 讨论与普通零空间的区别

提示：考虑增广系统 $[\mathbf{J}_{\text{task}}; \mathbf{J}_{\text{contact}}]$

题目 11.7：实时WBC优化 分析WBC的计算复杂度并提出加速策略： (a) 分析标准QP求解的复杂度 (b) 提出三种加速方法 (c) 讨论精度与速度的权衡

提示：考虑问题结构、近似方法和并行化

题目 11.8：学习增强的WBC 设计一个结合机器学习的WBC系统： (a) 识别WBC中可以用学习改进的部分 (b) 提出一个具体的学习框架 (c) 讨论安全性保证

提示：考虑模型误差、任务预测和优化加速

11.8 常见陷阱与错误（Gotchas）

调试技巧

1. 数值问题 - 症状：QP求解器返回"数值不稳定"或解振荡 - 原因：条件数差的矩阵、尺度不匹配 - 解决：

检查清单：
✓ 归一化任务权重（同数量级）
✓ 正则化项（最小1e-6）
✓ 检查雅可比秩（SVD分解）
✓ 避免接近奇异配置

2. 任务冲突 - 症状：机器人行为异常、任务无法完成 - 原因：任务在几何上不兼容 - 检测：

if (||J1*N2*J2_pinv*x2_dot|| > threshold) {
  // 任务2与任务1冲突
}

• 解决：调整优先级或使用软约束

3. 接触不稳定 - 症状：接触力振荡、意外滑动 - 原因： - 接触雅可比计算错误 - 摩擦锥近似太粗糙 - 接触刚度设置不当 - 调试步骤：
4. 可视化接触力和摩擦锥
5. 降低控制频率测试
6. 增加接触阻尼
7. 检查力传感器噪声

常见错误

1. 伪逆使用错误

# 错误：使用标准伪逆处理冗余
J_inv = np.linalg.pinv(J)  # 忽略了惯性加权

# 正确：使用动力学一致伪逆
J_inv = M_inv @ J.T @ np.linalg.inv(J @ M_inv @ J.T)

2. 零空间投影顺序

# 错误：先投影到任务1零空间，再到任务2零空间
N1 = I - J1_pinv @ J1
N2 = I - J2_pinv @ J2
q_dot = J1_pinv @ x1_dot + N1 @ N2 @ q0_dot  # 错误！

# 正确：递归构建零空间
N1 = I - J1_pinv @ J1
N1_2 = N1 @ (I - (J2 @ N1)_pinv @ (J2 @ N1))
q_dot = J1_pinv @ x1_dot + (J2 @ N1)_pinv @ (x2_dot - J2 @ J1_pinv @ x1_dot) + N1_2 @ q0_dot

3. 忽略速度级运动学

# 错误：直接使用位置误差
x_error = x_desired - x_current
F = Lambda @ K_p @ x_error  # 忽略了Coriolis项

# 正确：包含完整动力学
x_ddot = x_ddot_ref + K_p @ x_error + K_d @ x_dot_error
F = Lambda @ x_ddot + mu + p  # 包含所有项

性能优化建议

1. 分层计算： - 高频（1kHz）：关节伺服、平衡控制 - 中频（100Hz）：全身QP求解 - 低频（10Hz）：轨迹规划

2. 稀疏性利用： - 使用稀疏QP求解器（OSQP、qpOASES） - 预计算不变矩阵 - 分块矩阵运算

3. 并行化机会： - 任务雅可比并行计算 - 多接触点力优化分解 - GPU加速矩阵运算

实战经验

初始化策略：

• 从重力补偿配置开始
• 渐进式激活任务（避免跳变）
• 使用S曲线轨迹平滑过渡

故障恢复：

• 实现安全监督层
• 定义紧急停止条件
• 保持最小可行控制器作为后备

调参指南：

基准值（30自由度人形机器人）：

- COM控制：Kp=100-200, Kd=20-40
- 姿态控制：Kp=50-100, Kd=10-20  
- 末端执行：Kp=200-500, Kd=40-100
- QP正则化：1e-6 到 1e-4
- 摩擦系数：0.3-0.8（依赖表面）