全身控制（Whole-Body Control, WBC）是现代机器人控制的核心技术，它将复杂的多自由度机器人系统视为一个整体，通过统一的优化框架同时处理运动生成、力控制和约束满足。与传统的分层控制不同，WBC能够充分利用机器人的所有自由度，实现更加自然、高效的运动。本章将深入探讨WBC的理论基础，包括任务空间控制、优先级管理和优化框架，并通过HRP-2人形机器人的案例展示这些理论的实际应用。
传统的机器人控制通常在关节空间进行，但实际任务往往定义在笛卡尔空间或其他任务相关的空间中。任务空间控制的核心思想是直接在任务空间中指定期望行为，然后映射到关节空间执行。这种方法的优势在于：任务目标直观明确、便于人机交互、自然处理约束。
任务空间的选择取决于具体应用。常见的任务空间包括：
考虑一个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}}$ 项表示由于雅可比矩阵时变引起的加速度，在高速运动时不可忽略。
雅可比矩阵的几何意义深刻：
为了在任务空间中进行力控制，我们需要推导操作空间动力学。这种表述由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}}\]其中：
通过运动学约束 $\ddot{\mathbf{x}} = \mathbf{J}\ddot{\mathbf{q}} + \dot{\mathbf{J}}\dot{\mathbf{q}}$，并利用动力学方程求解 $\ddot{\mathbf{q}}$，可以推导出操作空间动力学。
推导过程的关键步骤：
其中：
操作空间惯性矩阵 $\boldsymbol{\Lambda}$ 具有重要的物理意义：
关节力矩与任务空间力的映射关系： \(\boldsymbol{\tau} = \mathbf{J}^T\mathbf{F} + \mathbf{N}^T\boldsymbol{\tau}_0\)
其中第二项是零空间力矩，不产生任务空间力但影响内部配置。
当 $m < n$ 时，机器人具有运动学冗余，存在无穷多组关节配置可以实现相同的任务空间位置。零空间投影是处理冗余的关键工具。冗余自由度为机器人提供了额外的灵活性：避障能力、关节限位回避、优化次要目标、提高容错性。
冗余度的定量描述：
雅可比矩阵的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}$，它考虑了惯性分布，产生的运动在动力学意义下是最优的。
不同伪逆的选择准则：
零空间投影矩阵为：
\[\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$ 是任意的关节速度，可用于优化次要目标。
零空间运动的典型应用：
零空间的动态特性也很重要。零空间动力学可表示为： \(\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 -------- 零空间
(冗余自由度)
实际任务通常需要同时控制位置和力。例如，擦拭表面需要在法向维持接触力，在切向跟踪轨迹。统一框架通过选择矩阵实现这种混合控制。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}}\]其中：
实际应用中的关键问题：
典型应用场景：
插孔任务： 研磨任务： 装配任务：
Z(力) Z(力) Z(力)
| | |
v v v
[peg] [tool] [part]
| ~~~~ ====
[hole] surface fixture
X,Y(位置) X,Y(位置) X,Y,Rz(位置)
阻抗控制提供了另一种统一运动和力控制的方法，通过指定期望的动力学行为。与混合控制不同，阻抗控制不需要显式地分离运动和力控制方向，而是通过调节机器人的“柔顺性”来适应环境。
阻抗控制的核心思想是将机器人表现为一个虚拟的机械系统（质量-弹簧-阻尼系统）：
\[\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{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)$是根据任务阶段或环境感知调整的自适应参数。
浮动基座机器人（如人形机器人）的全身控制必须考虑接触约束。与固定基座机器人不同，浮动基座机器人的基座本身是可动的，必须通过接触力来维持平衡和产生运动。
接触约束的数学表述包括运动学和动力学两个方面。接触点的加速度约束为：
\[\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}$ 是接触空间的有效惯性。
接触约束的分类：
接触力必须满足物理约束（如摩擦锥）：
\[\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框架中求解。
机器人系统通常需要同时满足多个任务目标，这些任务可能相互冲突。优先级任务管理提供了系统化的方法来处理这些冲突，确保关键任务（如安全约束）始终得到满足，同时尽可能实现次要目标。
优先级管理的基本原则：
当多个任务存在冲突时，优先级管理确保高优先级任务得到满足。严格优先级通过零空间投影实现，保证低优先级任务不会干扰高优先级任务：
第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)$ 的意义：
一般地，第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}$
这个递归公式确保：
严格优先级的优缺点：
优先级层次结构：
┌─────────────────┐
│ 任务1（最高） │ ← 平衡控制 │ 安全关键
├─────────────────┤ │
│ 任务2 │ ← 末端执行器位置 │ 主要任务
├─────────────────┤ │
│ 任务3 │ ← 姿态调整 │ 辅助任务
├─────────────────┤ │
│ 任务4（最低） │ ← 关节限位回避 │ 优化目标
└─────────────────┘
零空间维度变化：
层次 任务维度 剩余自由度
0 - n
1 m₁ n-m₁
2 m₂ n-m₁-m₂
3 m₃ n-m₁-m₂-m₃
...
加权方法通过优化问题软性处理优先级，避免了严格优先级的不连续性问题：
\[\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\]其中：
权重选择策略：
这导致闭式解：
\[\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}\)
加权方法的优缺点：
层次化二次规划（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}\)
其中：
HQP的求解算法：
for k = 1 to n_levels:
解决第k层QP
将最优解作为下一层的等式约束
HQP与其他方法的关系：
现代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}\]其中：
对于过驱动系统，控制分配决定如何分配执行器输出：
\[\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}$ 是权重矩阵。
实际系统需要处理模型不确定性和外部扰动。鲁棒WBC方法包括：
扰动观测器设计： \(\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}$ 是观测器增益。
HRP-2是日本产业技术综合研究所（AIST）和川田工业共同开发的人形机器人，具有30个自由度（不含手指）。其全身控制系统是WBC理论的经典应用案例。
系统规格：
HRP-2 关节配置
头部(2)
|
上躯干(2)
/ \
左臂(6) 右臂(6)
\ /
骨盆(0)
/ \
左腿(6) 右腿(6)
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}})\]实验1：推力干扰恢复
施加横向推力（50N，0.5秒）时的响应：
推力响应时间线
时间(s) 0 0.5 1.0 1.5 2.0
COM_y |----⟋----╲_____|
ZMP_y |---⟋⟍\_____|
踝扭矩 |__⟋███⟍\___|
控制器通过以下策略恢复平衡：
实验2：斜坡行走
在15°斜坡上行走的关键挑战：
适应策略： \(\mathbf{F}_{c,modified} = \mathbf{R}_{slope}\mathbf{F}_{c,nominal}\) \(\mu_{effective} = \mu_{nominal}\cos(\theta) - \sin(\theta)\)
双臂搬运任务：
任务分解：
协调约束： \(\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}\)
计算性能：
控制性能指标：
鲁棒性测试结果： | 扰动类型 | 最大可恢复量 | 恢复时间 | |———|————|———| | 横向推力 | 80 N×0.5s | 2.5 s | | 前向推力 | 100 N×0.5s | 2.0 s | | 负载突变 | 5 kg | 1.5 s | | 地面倾斜 | 15° | N/A |
全身控制是现代机器人实现复杂任务的关键技术。本章核心要点：
理论基础：
控制框架：
优化方法：
关键公式汇总：
题目 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) 讨论安全性保证
提示：考虑模型误差、任务预测和优化加速
1. 数值问题
检查清单：
✓ 归一化任务权重（同数量级）
✓ 正则化项（最小1e-6）
✓ 检查雅可比秩（SVD分解）
✓ 避免接近奇异配置
2. 任务冲突
if (||J1*N2*J2_pinv*x2_dot|| > threshold) {
// 任务2与任务1冲突
}
3. 接触不稳定
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 # 包含所有项
初始化策略：
故障恢复：
调参指南：
基准值（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（依赖表面）