第12章：任务空间控制与优化

在前面的章节中，我们探讨了机器人的运动规划和模型预测控制。然而，实际的机器人任务通常定义在操作空间（末端执行器空间）而非关节空间。本章将深入研究如何在任务空间中直接设计控制器，以及如何通过优化方法协调多个竞争性任务。我们将特别关注全身控制（Whole-Body Control, WBC）框架，它能够同时处理运动控制、力控制和约束满足问题。通过本章的学习，读者将掌握现代机器人系统中广泛应用的任务空间控制理论，并理解如何在实际系统中实现高性能的全身控制器。

12.1 操作空间控制

12.1.1 操作空间动力学

操作空间控制的核心思想是直接在任务相关的坐标系中设计控制律，而不是在关节空间中。考虑一个n自由度的机器人，其关节空间动力学方程为：

$$\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}_{ext}$$ 其中，$\mathbf{M}(\mathbf{q}) \in \mathbb{R}^{n \times n}$是质量矩阵，$\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})$包含科氏力和离心力项，$\mathbf{g}(\mathbf{q})$是重力项，$\boldsymbol{\tau}$是关节力矩，$\mathbf{f}_{ext}$是外部接触力。

设任务空间坐标为$\mathbf{x} \in \mathbb{R}^m$（通常$m < n$），其与关节空间的关系为： $$\mathbf{x} = \mathbf{f}(\mathbf{q})$$ 对时间求导得到速度和加速度关系： $$\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}$$ $$\ddot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\ddot{\mathbf{q}} + \dot{\mathbf{J}}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}}$$ 其中$\mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}}{\partial \mathbf{q}}$是任务雅可比矩阵。

通过投影关节空间动力学到任务空间，我们得到操作空间动力学方程： $$\mathbf{\Lambda}(\mathbf{x})\ddot{\mathbf{x}} + \mathbf{\mu}(\mathbf{x}, \dot{\mathbf{x}}) + \mathbf{p}(\mathbf{x}) = \mathbf{F} + \mathbf{F}_{ext}$$ 其中：

$\mathbf{\Lambda}(\mathbf{x}) = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$：操作空间惯性矩阵
$\mathbf{\mu}(\mathbf{x}, \dot{\mathbf{x}}) = \mathbf{\Lambda}\mathbf{J}\mathbf{M}^{-1}(\mathbf{C}\dot{\mathbf{q}} - \mathbf{M}\dot{\mathbf{J}}\dot{\mathbf{q}})$：操作空间科氏力/离心力
$\mathbf{p}(\mathbf{x}) = \mathbf{\Lambda}\mathbf{J}\mathbf{M}^{-1}\mathbf{g}$：操作空间重力
$\mathbf{F}$：操作空间控制力

12.1.2 操作空间控制律设计

基于操作空间动力学，我们可以设计解耦的控制律。一个典型的操作空间控制器具有如下形式： $$\mathbf{F} = \mathbf{\Lambda}(\mathbf{x})\mathbf{a}_d + \mathbf{\mu}(\mathbf{x}, \dot{\mathbf{x}}) + \mathbf{p}(\mathbf{x})$$ 其中$\mathbf{a}_d$是期望的任务空间加速度，可以通过PD控制律计算： $$\mathbf{a}_d = \ddot{\mathbf{x}}_d + \mathbf{K}_p(\mathbf{x}_d - \mathbf{x}) + \mathbf{K}_d(\dot{\mathbf{x}}_d - \dot{\mathbf{x}})$$ 相应的关节力矩为： $$\boldsymbol{\tau} = \mathbf{J}^T\mathbf{F} + \mathbf{N}^T\boldsymbol{\tau}_0$$ 其中$\mathbf{N} = \mathbf{I} - \mathbf{J}^T\mathbf{J}^{#T}$是零空间投影矩阵，$\mathbf{J}^{#} = \mathbf{M}^{-1}\mathbf{J}^T\mathbf{\Lambda}$是动力学一致的伪逆，$\boldsymbol{\tau}_0$是零空间控制力矩。

        任务空间                     关节空间
    ┌─────────────┐              ┌─────────────┐
    │  x_d, ẋ_d   │              │      q      │
    └──────┬──────┘              └──────┬──────┘
           │                             │
           ▼                             ▼
    ┌─────────────┐              ┌─────────────┐
    │  PD控制器   │              │   正运动学   │
    └──────┬──────┘              └──────┬──────┘
           │                             │
           ▼                             ▼
    ┌─────────────┐              ┌─────────────┐
    │     a_d     │              │   x, ẋ      │
    └──────┬──────┘              └──────┬──────┘
           │                             │
           ▼                             │
    ┌─────────────┐                     │
    │  动力学补偿  │◄────────────────────┘
    └──────┬──────┘
           │
           ▼
    ┌─────────────┐
    │      F      │
    └──────┬──────┘
           │
           ▼
    ┌─────────────┐
    │   τ = J^TF  │
    └─────────────┘

12.1.3 力/位置混合控制

在许多实际应用中，我们需要同时控制位置和力。例如，在装配任务中，某些方向需要位置控制以保证精度，而其他方向需要力控制以避免过大的接触力。混合控制框架通过选择矩阵实现这种分离： $$\mathbf{F} = \mathbf{S}\mathbf{F}_{motion} + (\mathbf{I} - \mathbf{S})\mathbf{F}_{force}$$ 其中$\mathbf{S} \in \mathbb{R}^{m \times m}$是对角选择矩阵，对角元素为0或1：

$s_{ii} = 1$：第i个方向执行位置控制
$s_{ii} = 0$：第i个方向执行力控制

位置控制分量： $$\mathbf{F}_{motion} = \mathbf{\Lambda}(\ddot{\mathbf{x}}_d + \mathbf{K}_p\mathbf{e}_p + \mathbf{K}_d\mathbf{e}_v) + \mathbf{\mu} + \mathbf{p}$$ 力控制分量： $$\mathbf{F}_{force} = \mathbf{F}_d + \mathbf{K}_f(\mathbf{F}_d - \mathbf{F}_{measured})$$

12.1.4 阻抗控制与导纳控制

阻抗控制是一种统一处理位置和力控制的方法，它通过调节机器人的动态行为来实现期望的接触交互。阻抗控制的目标是使末端执行器表现出期望的质量-弹簧-阻尼系统行为： $$\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}_{ext}$$ 其中$\mathbf{M}_d$、$\mathbf{D}_d$、$\mathbf{K}_d$分别是期望的惯性、阻尼和刚度矩阵。

实现阻抗控制的控制律为： $$\mathbf{F} = \mathbf{\Lambda}\mathbf{M}_d^{-1}[\mathbf{M}_d\ddot{\mathbf{x}}_d + \mathbf{D}_d(\dot{\mathbf{x}}_d - \dot{\mathbf{x}}) + \mathbf{K}_d(\mathbf{x}_d - \mathbf{x}) - \mathbf{F}_{ext}] + \mathbf{\mu} + \mathbf{p}$$ 导纳控制则是阻抗控制的对偶形式，它根据测量的力来调整位置： $$\mathbf{M}_d\ddot{\mathbf{e}} + \mathbf{D}_d\dot{\mathbf{e}} + \mathbf{K}_d\mathbf{e} = \mathbf{F}_{ext} - \mathbf{F}_d$$ 其中$\mathbf{e} = \mathbf{x}_m - \mathbf{x}$是修正后的位置误差。

12.2 二次规划基全身控制（QP-Based WBC）

12.2.1 WBC问题表述

全身控制（Whole-Body Control）是一种系统性的控制框架，能够同时处理多个任务、约束和接触。与传统的操作空间控制不同，WBC通过优化方法来协调可能相互冲突的目标。标准的WBC问题可以表述为以下二次规划（QP）问题： $$\begin{aligned} \min_{\ddot{\mathbf{q}}, \boldsymbol{\tau}, \mathbf{f}} \quad & \sum_{i=1}^{N_t} w_i |\mathbf{A}_i\ddot{\mathbf{q}} + \mathbf{b}_i|^2 + w_\tau |\boldsymbol{\tau}|^2 \\ \text{s.t.} \quad & \mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \sum_{j=1}^{N_c}\mathbf{J}_j^T\mathbf{f}_j \\ & \mathbf{J}_j\ddot{\mathbf{q}} + \dot{\mathbf{J}}_j\dot{\mathbf{q}} = \mathbf{0}, \quad j \in \mathcal{C} \\ & \mathbf{f}_j \in \mathcal{F}_j, \quad j = 1, \ldots, N_c \\ & \mathbf{q}_{min} \leq \mathbf{q} + \Delta t\dot{\mathbf{q}} + \frac{\Delta t^2}{2}\ddot{\mathbf{q}} \leq \mathbf{q}_{max} \\ & \dot{\mathbf{q}}_{min} \leq \dot{\mathbf{q}} + \Delta t\ddot{\mathbf{q}} \leq \dot{\mathbf{q}}_{max} \\ & \boldsymbol{\tau}_{min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{max} \end{aligned}$$ 其中：

$N_t$是任务数量，$N_c$是接触点数量
$w_i$是第i个任务的权重
$\mathbf{A}_i$和$\mathbf{b}_i$定义第i个任务的期望加速度
$\mathbf{S}$是选择矩阵（对于浮动基座系统）
$\mathcal{C}$是活跃接触集合
$\mathcal{F}_j$是第j个接触点的摩擦锥

12.2.2 任务定义与优先级

在WBC框架中，任务通常定义为期望的加速度约束。常见的任务类型包括：

1. 笛卡尔空间任务： $$\mathbf{A}_i = \mathbf{J}_i, \quad \mathbf{b}_i = -\dot{\mathbf{J}}_i\dot{\mathbf{q}} + \ddot{\mathbf{x}}_i^{des}$$ 其中$\ddot{\mathbf{x}}_i^{des} = \ddot{\mathbf{x}}_i^{ref} + \mathbf{K}_{p,i}(\mathbf{x}_i^{ref} - \mathbf{x}_i) + \mathbf{K}_{d,i}(\dot{\mathbf{x}}_i^{ref} - \dot{\mathbf{x}}_i)$

2. 关节空间任务： $$\mathbf{A}_i = \mathbf{I}, \quad \mathbf{b}_i = \ddot{\mathbf{q}}_i^{des}$$

3. 质心（CoM）任务： $$\mathbf{A}_i = \mathbf{J}_{CoM}, \quad \mathbf{b}_i = -\dot{\mathbf{J}}_{CoM}\dot{\mathbf{q}} + \ddot{\mathbf{c}}^{des}$$

4. 角动量任务： $$\mathbf{A}_i = \mathbf{A}_G, \quad \mathbf{b}_i = -\dot{\mathbf{A}}_G\dot{\mathbf{q}} + \dot{\mathbf{L}}^{des}$$ 任务优先级可以通过两种方式实现：

软优先级：通过调整权重$w_i$来实现相对优先级

高优先级任务: w_high = 1000
中优先级任务: w_mid = 100  
低优先级任务: w_low = 1

硬优先级：使用层次化QP（Hierarchical QP） $$\begin{aligned} \text{Level 1:} \quad & \min_{\mathbf{x}} |\mathbf{A}_1\mathbf{x} - \mathbf{b}_1|^2 \\ & \text{s.t. } \text{dynamics and constraints} \\ \text{Level 2:} \quad & \min_{\mathbf{x}} |\mathbf{A}_2\mathbf{x} - \mathbf{b}_2|^2 \\ & \text{s.t. } \mathbf{A}_1\mathbf{x} = \mathbf{A}_1\mathbf{x}_1^* \\ & \quad \text{dynamics and constraints} \end{aligned}$$

12.2.3 QP求解器选择与实现

对于实时控制，QP求解器的选择至关重要。常用的求解器包括：

1. 稠密QP求解器： - qpOASES：适用于中小规模问题（<1000变量） - OSQP：基于ADMM，适用于稀疏问题 - ProxQP：基于原始-对偶方法，数值稳定性好

2. 求解器性能对比：

问题规模: 30个变量, 50个约束
┌────────────┬──────────┬──────────┬──────────┐
│  求解器     │ 求解时间 │ 迭代次数 │ 精度     │
├────────────┼──────────┼──────────┼──────────┤
│ qpOASES    │  0.8 ms  │    15    │  1e-8    │
│ OSQP       │  0.5 ms  │    25    │  1e-6    │
│ ProxQP     │  0.6 ms  │    10    │  1e-7    │
└────────────┴──────────┴──────────┴──────────┘

3. 问题规模化与稀疏性利用：

对于大规模机器人系统，利用问题的稀疏结构可以显著提升性能： $$\mathbf{H} = \begin{bmatrix} \mathbf{H}_{qq} & \mathbf{0} & \mathbf{J}_1^T & \cdots & \mathbf{J}_{N_c}^T \\ \mathbf{0} & \mathbf{R}_\tau & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{J}_1 & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \mathbf{J}_{N_c} & \mathbf{0} & \mathbf{0} & \cdots & \mathbf{0} \end{bmatrix}$$ 其中$\mathbf{H}_{qq} = \sum_{i=1}^{N_t} w_i\mathbf{A}_i^T\mathbf{A}_i$是加速度的海塞矩阵。

12.2.4 WBC实现流程

┌─────────────────────────┐
│    传感器数据获取        │
│  (q, q̇, IMU, F/T)      │
└───────────┬─────────────┘
            ▼
┌─────────────────────────┐
│    状态估计             │
│  (浮动基座状态)         │
└───────────┬─────────────┘
            ▼
┌─────────────────────────┐
│    任务定义             │
│  (位置、力、平衡等)     │
└───────────┬─────────────┘
            ▼
┌─────────────────────────┐
│    构建QP问题           │
│  (目标函数+约束)        │
└───────────┬─────────────┘
            ▼
┌─────────────────────────┐
│    求解QP              │
│  (获得q̈*, τ*, f*)     │
└───────────┬─────────────┘
            ▼
┌─────────────────────────┐
│    力矩映射            │
│  (τ_cmd = S^Tτ*)      │
└───────────┬─────────────┘
            ▼
┌─────────────────────────┐
│    执行器控制          │
└─────────────────────────┘

12.3 接触一致性与动力学可行性

12.3.1 接触约束建模

在全身控制中，正确建模接触约束是保证控制器性能的关键。接触约束包括运动学约束和力学约束两个方面。

运动学接触约束：

对于刚性接触，接触点的加速度必须为零（无滑动、无分离）： $$\mathbf{J}_c\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c\dot{\mathbf{q}} = \mathbf{0}$$ 对于软接触或柔顺接触，可以放松这个约束： $$\mathbf{J}_c\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c\dot{\mathbf{q}} = \mathbf{a}_c^{des}$$ 其中$\mathbf{a}_c^{des}$可以包含期望的接触运动（如调整接触位置）。

力学接触约束：

接触力必须满足物理约束：

单向约束（接触力只能推不能拉）： $$f_{n,j} \geq 0, \quad \forall j \in \mathcal{C}$$
摩擦锥约束： $$|\mathbf{f}_{t,j}| \leq \mu_j f_{n,j}$$ 其中$\mathbf{f}_{t,j}$是切向力，$f_{n,j}$是法向力，$\mu_j$是摩擦系数。
线性化摩擦锥（用于QP求解）：

将圆锥约束线性化为棱锥： $$\mathbf{f}_j = \sum_{k=1}^{N_f} \lambda_{jk}\mathbf{v}_{jk}, \quad \lambda_{jk} \geq 0$$ 其中$\mathbf{v}_{jk}$是棱锥的生成向量：

      法向 n
        ↑
        │     
    v₃  │  v₂
      ╲ │ ╱
       ╲│╱
    ────┼────► 切向 t₁
       ╱│╲
      ╱ │ ╲
    v₄  │  v₁
        │
        ▼ 切向 t₂

12.3.2 动力学一致性条件

动力学一致性确保计算的加速度、力和力矩满足机器人的动力学方程。这需要在QP中加入等式约束： $$\mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \sum_{j \in \mathcal{C}}\mathbf{J}_j^T\mathbf{f}_j$$ 对于欠驱动系统（如人形机器人），动力学方程可以分解为： $$\begin{bmatrix} \mathbf{M}_{ff} & \mathbf{M}_{fa} \\ \mathbf{M}_{af} & \mathbf{M}_{aa} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}}_f \\ \ddot{\mathbf{q}}_a \end{bmatrix} + \begin{bmatrix} \mathbf{h}_f \\ \mathbf{h}_a \end{bmatrix} = \begin{bmatrix} \mathbf{0} \\ \boldsymbol{\tau}_a \end{bmatrix} + \sum_j \begin{bmatrix} \mathbf{J}_{f,j}^T \\ \mathbf{J}_{a,j}^T \end{bmatrix} \mathbf{f}_j$$ 其中下标$f$表示浮动基座（6自由度），$a$表示驱动关节。

12.3.3 质心动力学与ZMP约束

质心（CoM）动力学：

质心的运动由牛顿-欧拉方程决定： $$m\ddot{\mathbf{c}} = m\mathbf{g} + \sum_{j \in \mathcal{C}}\mathbf{f}_j$$

$$\dot{\mathbf{L}}_G = \sum_{j \in \mathcal{C}}(\mathbf{p}_j - \mathbf{c}) \times \mathbf{f}_j + \boldsymbol{\tau}_{ext}$$ 其中$\mathbf{L}_G$是关于质心的角动量，$\mathbf{p}_j$是接触点位置。

ZMP（零力矩点）约束：

对于平面接触，ZMP必须位于支撑多边形内部以保证动态平衡： $$\mathbf{p}_{ZMP} = \frac{\sum_j \mathbf{p}_j f_{n,j}}{\sum_j f_{n,j}}$$ ZMP约束可以表示为线性不等式： $$\mathbf{A}_{ZMP}\mathbf{p}_{ZMP} \leq \mathbf{b}_{ZMP}$$ 其中$\mathbf{A}_{ZMP}$和$\mathbf{b}_{ZMP}$定义支撑多边形的半空间表示。

12.3.4 接触切换与模式调度

处理接触模式切换是WBC的一个重要挑战。常见的策略包括：

1. 接触检测与分类：

根据力传感器和运动学信息判断接触状态：

if (f_measured > f_threshold && |v_contact| < v_threshold):
    contact_state = STABLE_CONTACT
elif (f_measured > f_threshold && |v_contact| > v_threshold):
    contact_state = SLIDING_CONTACT
else:
    contact_state = NO_CONTACT

2. 平滑接触建立：

使用时间函数平滑地建立接触约束： $$w_{contact}(t) = \begin{cases} 0 & t < t_0 \\ \frac{1}{2}(1 - \cos(\pi\frac{t-t_0}{T})) & t_0 \leq t \leq t_0 + T \\ 1 & t > t_0 + T \end{cases}$$

3. 接触力分配：

当存在多个接触点时，需要优化力分配： $$\begin{aligned} \min_{\mathbf{f}} \quad & \sum_j |\mathbf{f}_j - \mathbf{f}_{j,nom}|^2 \\ \text{s.t.} \quad & \sum_j \mathbf{f}_j = \mathbf{F}_{total} \\ & \sum_j (\mathbf{p}_j - \mathbf{c}) \times \mathbf{f}_j = \boldsymbol{\tau}_{total} \\ & \mathbf{f}_j \in \mathcal{F}_j \end{aligned}$$

12.3.5 鲁棒性增强策略

为提高WBC的鲁棒性，可以采用以下策略：

1. 约束软化：

引入松弛变量处理不可行情况： $$\begin{aligned} \min \quad & |\mathbf{e}_{task}|^2 + w_{slack}|\boldsymbol{\epsilon}|^2 \\ \text{s.t.} \quad & \text{constraints} + \boldsymbol{\epsilon} \end{aligned}$$

2. 接触力正则化：

添加接触力的正则项以避免数值问题： $$J_{reg} = \alpha_f\sum_j|\mathbf{f}_j|^2 + \alpha_{\Delta f}\sum_j|\mathbf{f}_j - \mathbf{f}_{j,prev}|^2$$

3. 加速度限制：

限制加速度变化率以提高平滑性： $$|\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{prev}| \leq \Delta\ddot{q}_{max}$$

12.4 案例研究：双臂协作机器人Baxter

12.4.1 系统概述与挑战

Baxter是Rethink Robotics开发的双臂协作机器人，具有独特的设计特点和控制挑战：

硬件配置：

双7自由度手臂（共14个驱动关节）
串联弹性驱动器（Series Elastic Actuators, SEA）
集成力/力矩传感
末端执行器：平行夹爪或真空吸盘
工作空间：每臂约1.2米范围

控制挑战：

双臂协调：两臂协同操作时的运动同步和力分配
柔顺性管理：SEA带来的固有柔顺性需要特殊处理
安全约束：人机协作环境下的安全性要求
计算效率：实时控制下的计算资源限制

12.4.2 双臂协作的WBC实现

系统建模：

将Baxter建模为具有14个驱动自由度的固定基座机器人： $$\mathbf{q} = [\mathbf{q}_L^T, \mathbf{q}_R^T]^T \in \mathbb{R}^{14}$$ 其中$\mathbf{q}_L, \mathbf{q}_R \in \mathbb{R}^7$分别为左右臂的关节角度。

动力学方程考虑SEA的弹性： $$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{K}_s(\boldsymbol{\theta}_m - \mathbf{q}) + \mathbf{J}^T\mathbf{f}_{ext}$$ 其中$\mathbf{K}_s$是弹簧刚度矩阵，$\boldsymbol{\theta}_m$是电机角度。

双臂协作任务定义：

相对位姿任务：

定义两个末端执行器之间的相对位姿： $$\mathbf{x}_{rel} = \mathbf{T}_L^{-1} \mathbf{T}_R$$ 相应的任务雅可比： $$\mathbf{J}_{rel} = [-\mathbf{J}_L, \mathbf{J}_R]$$

载荷分配任务：

对于共同抓持的物体，力分配满足： $$\mathbf{f}_L + \mathbf{f}_R = \mathbf{f}_{obj}$$ $$\boldsymbol{\tau}_L + \boldsymbol{\tau}_R + (\mathbf{p}_L - \mathbf{p}_{obj}) \times \mathbf{f}_L + (\mathbf{p}_R - \mathbf{p}_{obj}) \times \mathbf{f}_R = \boldsymbol{\tau}_{obj}$$

内力优化：

最小化内力以减少能耗： $$\mathbf{f}_{int} = \frac{1}{2}(\mathbf{f}_L - \mathbf{f}_R)$$ 目标函数包含内力惩罚项： $$J_{int} = w_{int}|\mathbf{f}_{int}|^2$$ QP问题构建：

双臂协作的QP问题： $$\begin{aligned} \min_{\ddot{\mathbf{q}}, \boldsymbol{\tau}} \quad & w_1|\mathbf{J}_{rel}\ddot{\mathbf{q}} + \dot{\mathbf{J}}_{rel}\dot{\mathbf{q}} - \ddot{\mathbf{x}}_{rel}^{des}|^2 \\ & + w_2|\mathbf{J}_{obj}\ddot{\mathbf{q}} + \dot{\mathbf{J}}_{obj}\dot{\mathbf{q}} - \ddot{\mathbf{x}}_{obj}^{des}|^2 \\ & + w_3|\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{posture}|^2 \\ & + w_4|\boldsymbol{\tau}|^2 \\ \text{s.t.} \quad & \mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \boldsymbol{\tau} + \mathbf{J}^T\mathbf{f}_{ext} \\ & \mathbf{q}_{min} \leq \mathbf{q} + \Delta t\dot{\mathbf{q}} + \frac{\Delta t^2}{2}\ddot{\mathbf{q}} \leq \mathbf{q}_{max} \\ & \dot{\mathbf{q}}_{min} \leq \dot{\mathbf{q}} + \Delta t\ddot{\mathbf{q}} \leq \dot{\mathbf{q}}_{max} \\ & \boldsymbol{\tau}_{min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{max} \\ & \text{collision avoidance constraints} \end{aligned}$$

12.4.3 碰撞避免策略

自碰撞检测：

使用胶囊体（capsule）近似机器人连杆：

     左臂                    右臂
   ┌──────┐                ┌──────┐
   │  L1  │                │  R1  │
   └───┬──┘                └──┬───┘
       │                       │
   ┌───▼──┐                ┌──▼───┐
   │  L2  │                │  R2  │
   └───┬──┘                └──┬───┘
       │      最小距离          │
   ┌───▼──┐  <---------->  ┌──▼───┐
   │  L3  │   d_min > d_safe │  R3  │
   └──────┘                └──────┘

距离约束表示为： $$d_{ij} = |\mathbf{p}_i - \mathbf{p}_j| - r_i - r_j \geq d_{safe}$$ 其中$\mathbf{p}_i, \mathbf{p}_j$是胶囊体中心，$r_i, r_j$是半径。

线性化为速度约束： $$\mathbf{n}_{ij}^T(\mathbf{v}_i - \mathbf{v}_j) \geq -k_{avoid}(d_{ij} - d_{safe})$$ 其中$\mathbf{n}_{ij}$是从$i$指向$j$的单位向量。

环境碰撞避免：

使用虚拟力场方法： $$\mathbf{F}_{rep} = \begin{cases} k_{rep}\left(\frac{1}{d} - \frac{1}{d_{inf}}\right)\frac{1}{d^2}\mathbf{n} & d < d_{inf} \\ \mathbf{0} & d \geq d_{inf} \end{cases}$$ 将排斥力转换为加速度约束： $$\mathbf{J}_{obs}\ddot{\mathbf{q}} \geq -\mathbf{\Lambda}^{-1}\mathbf{F}_{rep} - \dot{\mathbf{J}}_{obs}\dot{\mathbf{q}}$$

12.4.4 SEA补偿与力控制

SEA动力学补偿：

串联弹性驱动器引入额外的动力学： $$\mathbf{I}_m\ddot{\boldsymbol{\theta}}_m + \mathbf{B}_m\dot{\boldsymbol{\theta}}_m = \boldsymbol{\tau}_m - \mathbf{K}_s(\boldsymbol{\theta}_m - \mathbf{q})$$ 其中$\mathbf{I}_m$是电机惯量，$\mathbf{B}_m$是电机阻尼。

力矩估计：

基于弹簧变形估计关节力矩： $$\hat{\boldsymbol{\tau}} = \mathbf{K}_s(\boldsymbol{\theta}_m - \mathbf{q})$$ 使用卡尔曼滤波器融合编码器和力矩测量： $$\begin{aligned} \mathbf{x}_k &= \mathbf{A}\mathbf{x}_{k-1} + \mathbf{B}\mathbf{u}_{k-1} + \mathbf{w}_{k-1} \\ \mathbf{z}_k &= \mathbf{H}\mathbf{x}_k + \mathbf{v}_k \end{aligned}$$ 其中状态$\mathbf{x} = [\mathbf{q}^T, \dot{\mathbf{q}}^T, \boldsymbol{\tau}^T]^T$。

阻抗控制实现：

考虑SEA柔顺性的阻抗控制器： $$\boldsymbol{\tau}_m = \mathbf{K}_s(\boldsymbol{\theta}_m^{des} - \mathbf{q}) + \mathbf{D}_s(\dot{\boldsymbol{\theta}}_m^{des} - \dot{\mathbf{q}})$$ 其中： $$\boldsymbol{\theta}_m^{des} = \mathbf{q} + \mathbf{K}_s^{-1}[\mathbf{J}^T(\mathbf{K}_p\mathbf{e}_p + \mathbf{K}_d\mathbf{e}_v) + \mathbf{g}(\mathbf{q})]$$

12.4.5 实验结果与性能分析

任务场景：双臂搬运

实验设置：

物体质量：2.5 kg
搬运距离：0.8 m
控制频率：100 Hz
QP求解时间：< 5 ms

性能指标：

轨迹跟踪误差：

位置误差 (RMS): 8.3 mm
姿态误差 (RMS): 2.1°
相对位姿误差: 3.5 mm

力控制性能：

内力范围: 5-15 N
载荷分配误差: < 10%
接触力稳定时间: 0.8 s

计算性能：

┌─────────────────┬──────────┐
│ 计算模块         │ 时间(ms) │
├─────────────────┼──────────┤
│ 动力学计算       │   1.2    │
│ 雅可比计算       │   0.8    │
│ QP构建          │   1.5    │
│ QP求解          │   3.2    │
│ 后处理          │   0.3    │
├─────────────────┼──────────┤
│ 总计            │   7.0    │
└─────────────────┴──────────┘

鲁棒性测试：

外部扰动响应： - 施加20N脉冲力 - 恢复时间：1.2s - 最大位置偏差：45mm
负载变化适应： - 负载突变：+1kg - 力分配调整时间：0.5s - 稳态误差：<5%

12.4.6 实现要点与优化

代码架构：

WBC控制器
├── 状态估计器
│   ├── 关节状态滤波
│   ├── 力矩估计
│   └── 外力估计
├── 任务管理器
│   ├── 任务优先级调度
│   ├── 任务激活/去激活
│   └── 任务参数更新
├── QP求解器
│   ├── 问题构建
│   ├── 稀疏矩阵处理
│   └── 热启动策略
└── 安全监控
    ├── 碰撞检测
    ├── 力矩限制
    └── 紧急停止

优化技巧：

QP热启动：使用前一时刻的解作为初始猜测： $$\mathbf{x}_0^{(k)} = \mathbf{x}^{(k-1)} + \Delta t\dot{\mathbf{x}}^{(k-1)}$$
稀疏矩阵利用：利用块对角结构加速计算： $$\mathbf{M} = \begin{bmatrix} \mathbf{M}_L & \mathbf{0} \\ \mathbf{0} & \mathbf{M}_R \end{bmatrix} + \mathbf{M}_{coupling}$$
任务缓存：预计算不变的任务矩阵：

if (!task_changed):
    use_cached_jacobian()
else:
    recompute_jacobian()
    update_cache()

12.5 本章小结

本章深入探讨了任务空间控制与优化的核心理论和实践方法。我们从操作空间控制的基本原理出发，逐步构建了完整的全身控制框架，并通过Baxter双臂机器人的案例展示了这些理论在实际系统中的应用。

关键概念回顾

操作空间控制：

操作空间动力学方程：$\mathbf{\Lambda}(\mathbf{x})\ddot{\mathbf{x}} + \mathbf{\mu}(\mathbf{x}, \dot{\mathbf{x}}) + \mathbf{p}(\mathbf{x}) = \mathbf{F} + \mathbf{F}_{ext}$
动力学一致的伪逆：$\mathbf{J}^{#} = \mathbf{M}^{-1}\mathbf{J}^T\mathbf{\Lambda}$
零空间投影：$\mathbf{N} = \mathbf{I} - \mathbf{J}^T\mathbf{J}^{#T}$
阻抗控制律：$\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}_{ext}$

二次规划基全身控制（QP-WBC）：

多任务优化框架：同时处理多个可能冲突的任务目标
动力学一致性约束：$\mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \sum_j\mathbf{J}_j^T\mathbf{f}_j$
接触约束：运动学约束$\mathbf{J}_c\ddot{\mathbf{q}} + \dot{\mathbf{J}}_c\dot{\mathbf{q}} = \mathbf{0}$和摩擦锥约束$\mathbf{f}_j \in \mathcal{F}_j$
软/硬优先级管理：通过权重或层次化QP实现任务优先级

接触一致性与动力学可行性：

接触模式管理：检测、分类和切换接触状态
ZMP约束：$\mathbf{p}_{ZMP} = \frac{\sum_j \mathbf{p}_j f_{n,j}}{\sum_j f_{n,j}}$必须在支撑多边形内
接触力分配优化：在多接触情况下优化力分配
鲁棒性增强：约束软化、力正则化、加速度限制

核心公式总结

任务空间到关节空间映射： $$\boldsymbol{\tau} = \mathbf{J}^T\mathbf{F} + \mathbf{N}^T\boldsymbol{\tau}_0$$
标准QP-WBC问题： $$\min_{\ddot{\mathbf{q}}, \boldsymbol{\tau}, \mathbf{f}} \sum_{i=1}^{N_t} w_i |\mathbf{A}_i\ddot{\mathbf{q}} + \mathbf{b}_i|^2 + w_\tau |\boldsymbol{\tau}|^2$$
质心动力学： $$m\ddot{\mathbf{c}} = m\mathbf{g} + \sum_{j \in \mathcal{C}}\mathbf{f}_j$$
线性化摩擦锥： $$\mathbf{f}_j = \sum_{k=1}^{N_f} \lambda_{jk}\mathbf{v}_{jk}, \quad \lambda_{jk} \geq 0$$

实践要点

实时性考虑： - 选择合适的QP求解器（qpOASES、OSQP、ProxQP） - 利用问题稀疏性和结构 - 使用热启动策略 - 预计算和缓存不变量
稳定性保证： - 确保动力学一致性 - 满足接触约束 - 处理数值问题（正则化、约束软化） - 平滑接触切换
性能优化： - 任务优先级合理设置 - 权重参数调优 - 计算负载均衡 - 并行化关键计算

与其他章节的联系

第11章（全身控制基础）：本章是第11章理论的深化和实践应用
第9-10章（MPC）：QP-WBC可视为单步MPC，共享优化框架
第4-6章（接触规划）：接触约束建模直接应用了接触力学理论
第13章（实时实现）：本章的算法需要第13章的加速技术才能达到实时性能

12.6 练习题

基础题

练习12.1 导出操作空间惯性矩阵

证明操作空间惯性矩阵$\mathbf{\Lambda} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$确实表示任务空间的有效惯性。

提示：从关节空间动力学方程出发，使用$\ddot{\mathbf{q}} = \mathbf{J}^{#}(\ddot{\mathbf{x}} - \dot{\mathbf{J}}\dot{\mathbf{q}}) + \mathbf{N}\ddot{\mathbf{q}}_0$。

答案

从关节空间动力学方程开始： $$\mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \boldsymbol{\tau}$$ 使用关系式$\ddot{\mathbf{x}} = \mathbf{J}\ddot{\mathbf{q}} + \dot{\mathbf{J}}\dot{\mathbf{q}}$，解出： $$\ddot{\mathbf{q}} = \mathbf{J}^{#}(\ddot{\mathbf{x}} - \dot{\mathbf{J}}\dot{\mathbf{q}}) + (\mathbf{I} - \mathbf{J}^{#}\mathbf{J})\ddot{\mathbf{q}}_0$$ 其中$\mathbf{J}^{#}$是某种伪逆。代入动力学方程并左乘$\mathbf{J}\mathbf{M}^{-1}$： $$\mathbf{J}\ddot{\mathbf{q}} + \mathbf{J}\mathbf{M}^{-1}\mathbf{h} = \mathbf{J}\mathbf{M}^{-1}\boldsymbol{\tau}$$ 由于$\mathbf{J}\ddot{\mathbf{q}} = \ddot{\mathbf{x}} - \dot{\mathbf{J}}\dot{\mathbf{q}}$，且$\boldsymbol{\tau} = \mathbf{J}^T\mathbf{F} + \text{null space terms}$： $$(\ddot{\mathbf{x}} - \dot{\mathbf{J}}\dot{\mathbf{q}}) + \mathbf{J}\mathbf{M}^{-1}\mathbf{h} = \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T\mathbf{F}$$ 重新整理得到： $$(\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}\ddot{\mathbf{x}} + \text{velocity/gravity terms} = \mathbf{F}$$ 因此$\mathbf{\Lambda} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$确实是操作空间的有效惯性矩阵。

练习12.2 动力学一致伪逆的性质

证明动力学一致的伪逆$\mathbf{J}^{#} = \mathbf{M}^{-1}\mathbf{J}^T\mathbf{\Lambda}$满足以下性质：

$\mathbf{J}\mathbf{J}^{#} = \mathbf{I}_m$（左逆性质）
$\mathbf{N} = \mathbf{I} - \mathbf{J}^{#}\mathbf{J}$是幂等的（$\mathbf{N}^2 = \mathbf{N}$）
$\mathbf{J}\mathbf{N} = \mathbf{0}$

提示：利用$\mathbf{\Lambda} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$的定义。

答案

左逆性质： $$\mathbf{J}\mathbf{J}^{#} = \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T\mathbf{\Lambda} = \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T(\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1} = \mathbf{I}_m$$
幂等性： $$\mathbf{N}^2 = (\mathbf{I} - \mathbf{J}^{#}\mathbf{J})(\mathbf{I} - \mathbf{J}^{#}\mathbf{J})$$ $$= \mathbf{I} - \mathbf{J}^{#}\mathbf{J} - \mathbf{J}^{#}\mathbf{J} + \mathbf{J}^{#}\mathbf{J}\mathbf{J}^{#}\mathbf{J}$$ $$= \mathbf{I} - 2\mathbf{J}^{#}\mathbf{J} + \mathbf{J}^{#}(\mathbf{J}\mathbf{J}^{#})\mathbf{J}$$ $$= \mathbf{I} - 2\mathbf{J}^{#}\mathbf{J} + \mathbf{J}^{#}\mathbf{J} = \mathbf{N}$$
零空间性质： $$\mathbf{J}\mathbf{N} = \mathbf{J}(\mathbf{I} - \mathbf{J}^{#}\mathbf{J}) = \mathbf{J} - \mathbf{J}\mathbf{J}^{#}\mathbf{J} = \mathbf{J} - \mathbf{J} = \mathbf{0}$$

练习12.3 摩擦锥线性化误差分析

考虑将圆锥摩擦约束$|\mathbf{f}_t| \leq \mu f_n$线性化为4面棱锥。计算线性化引入的最大相对误差。

提示：考虑圆锥和棱锥在切平面上的投影差异。

答案

圆锥约束：$f_t^2 = f_{t1}^2 + f_{t2}^2 \leq (\mu f_n)^2$

4面棱锥约束：$|f_{t1}| \leq \mu f_n / \sqrt{2}$且$|f_{t2}| \leq \mu f_n / \sqrt{2}$

最大误差出现在45°方向：

圆锥允许：$f_t = \mu f_n$
棱锥允许：$f_t = \mu f_n / \sqrt{2}$（在对角线方向）

相对误差： $$\varepsilon = \frac{\mu f_n - \mu f_n/\sqrt{2}}{\mu f_n} = 1 - \frac{1}{\sqrt{2}} \approx 29.3\%$$ 使用8面棱锥可将误差降至约7.6%。

练习12.4 ZMP稳定性条件

推导平面接触情况下ZMP必须位于支撑多边形内的条件，并说明为什么这保证了动态平衡。

提示：考虑绕支撑边缘的力矩平衡。

答案

ZMP是地面反作用力的等效作用点，其定义为使得该点处净力矩（除垂直轴外）为零的点。

对于平面接触，考虑绕支撑多边形边缘的力矩： $$\tau_{edge} = (\mathbf{p}_{ZMP} - \mathbf{p}_{edge}) \times \mathbf{F}_{GRF}$$ 如果ZMP在支撑多边形内：

所有边缘的力矩都指向多边形内部
机器人不会绕任何边缘倾倒
接触力分布可行（所有$f_{n,i} \geq 0$）

如果ZMP在支撑多边形外：

存在边缘的力矩指向外部
机器人将绕该边缘倾倒
需要负的法向力（物理不可行）

因此，ZMP在支撑多边形内是动态平衡的必要充分条件。

挑战题

练习12.5 层次化QP的数值稳定性

设计一个数值稳定的层次化QP求解算法，处理任务之间可能的数值冲突。考虑以下情况：

高优先级任务接近奇异
任务之间存在微小但非零的冲突
数值精度限制导致的约束违反

提示：考虑使用正则化和松弛变量。

答案

层次化QP with数值稳定性增强：

Level k问题： $$\begin{aligned} \min_{\mathbf{x}, \boldsymbol{\epsilon}_k} \quad & |\mathbf{A}_k\mathbf{x} - \mathbf{b}_k|^2 + \alpha_k|\mathbf{x} - \mathbf{x}_{ref}|^2 + \beta_k|\boldsymbol{\epsilon}_k|^2 \\ \text{s.t.} \quad & \mathbf{A}_i\mathbf{x} = \mathbf{A}_i\mathbf{x}_i^* + \boldsymbol{\epsilon}_{i,k}, \quad i < k \\ & |\boldsymbol{\epsilon}_{i,k}| \leq \varepsilon_{tol} \\ & \text{dynamics and inequality constraints} \end{aligned}$$ 关键技术：

Tikhonov正则化（$\alpha_k$项）：处理接近奇异的任务
松弛变量（$\boldsymbol{\epsilon}_{i,k}$）：允许微小违反以处理数值冲突
有界松弛：限制违反量保证合理性
参考解（$\mathbf{x}_{ref}$）：提供备选方向

实现细节：

使用SVD检测任务奇异性
自适应调整$\alpha_k$和$\beta_k$
监控约束违反并动态调整$\varepsilon_{tol}$

练习12.6 多接触场景的力分配优化

设计一个算法，在四足机器人的四个脚掌接触情况下，优化分配接触力以：

最小化关节力矩
最大化稳定性裕度
避免接触滑动考虑计算效率，使其能在1ms内完成。

提示：将问题分解为解析部分和优化部分。

答案

两阶段力分配算法：

阶段1：解析计算基础解

利用虚功原理，给定期望的质心力和力矩： $$\begin{bmatrix} \mathbf{F}_{CoM} \\ \boldsymbol{\tau}_{CoM} \end{bmatrix} = \sum_{i=1}^{4} \begin{bmatrix} \mathbf{I}_3 \\ \mathbf{r}_i \times \end{bmatrix} \mathbf{f}_i$$ 使用伪逆获得最小范数解： $$\mathbf{f}_{base} = \mathbf{G}^{\dagger} \begin{bmatrix} \mathbf{F}_{CoM} \\ \boldsymbol{\tau}_{CoM} \end{bmatrix}$$ 阶段2：快速二次优化

在基础解附近优化： $$\begin{aligned} \min_{\Delta\mathbf{f}} \quad & \mathbf{f}^T\mathbf{W}_\tau\mathbf{f} + \alpha\sum_i d_i^2(\mathbf{p}_{ZMP,i}) \\ \text{s.t.} \quad & \mathbf{f}_i = \mathbf{f}_{base,i} + \Delta\mathbf{f}_i \\ & \mathbf{G}\Delta\mathbf{f} = \mathbf{0} \quad \text{(保持总力/力矩)} \\ & \mathbf{f}_i \in \mathcal{F}_i^{inner} \quad \text{(收缩的摩擦锥)} \end{aligned}$$ 其中：

$\mathbf{W}_\tau = \mathbf{J}^T\mathbf{R}\mathbf{J}$近似力矩权重
$d_i(\mathbf{p}_{ZMP,i})$是局部ZMP到支撑边界的距离
$\mathcal{F}_i^{inner} = 0.8\mathcal{F}_i$提供安全裕度

计算优化：

预计算$\mathbf{G}^{\dagger}$和零空间基
使用解析梯度
限制迭代次数（最多10次）
缓存不变矩阵

典型执行时间：0.3-0.8ms

练习12.7 接触切换的平滑过渡

设计一个控制策略，使机器人从双脚支撑平滑过渡到单脚支撑，同时保持：

连续的关节加速度
有界的接触力变化率
ZMP始终在安全区域内

提示：使用时变权重和预测控制。

答案

平滑接触切换策略：

时间参数化（切换周期$T_{switch}$）： $$s(t) = \begin{cases} 0 & t < 0 \\ \frac{1}{2}(1 - \cos(\pi t/T_{switch})) & 0 \leq t \leq T_{switch} \\ 1 & t > T_{switch} \end{cases}$$ 权重调度：

抬起脚的接触约束权重：$w_{lift}(t) = (1-s(t))w_0$
支撑脚的稳定性权重：$w_{sup}(t) = (1+s(t))w_0$
ZMP约束收缩：$\mathcal{S}(t) = (1-0.3s(t))\mathcal{S}_0$

QP公式修改： $$\begin{aligned} \min \quad & \sum_i w_i(t)|\mathbf{e}_i|^2 + w_\Delta|\ddot{\mathbf{q}} - \ddot{\mathbf{q}}_{prev}|^2 \\ & + w_f\sum_j|\mathbf{f}_j - \mathbf{f}_{j,prev}|^2 \\ \text{s.t.} \quad & \text{dynamics} \\ & f_{n,lift}(t) \geq (1-s(t))f_{min} \\ & \mathbf{p}_{ZMP} \in \mathcal{S}(t) \\ & |\dot{\mathbf{f}}| \leq \dot{f}_{max} \end{aligned}$$ 预测补偿：

使用短时预测（0.1-0.2s）调整当前控制： $$\mathbf{u}(t) = \mathbf{u}_{QP}(t) + \mathbf{K}_{pred}(\mathbf{x}_{pred} - \mathbf{x}_{des})$$ 实施要点：

提前规划ZMP轨迹向支撑脚移动
逐渐减小抬起脚的期望法向力
监控实际接触力，动态调整切换速度
使用力传感器反馈检测实际分离时刻

练习12.8 WBC框架的形式化验证

证明在给定的假设条件下，QP-based WBC框架能够保证：

Lyapunov稳定性
约束满足的递归可行性
渐近跟踪性能

提示：构造合适的Lyapunov函数并利用QP的最优性条件。

答案

定理：在以下假设下，QP-WBC保证闭环稳定性：

QP在每个时刻都可行
期望轨迹是可行的
模型误差有界

证明概要：

Lyapunov函数构造： $$V(\mathbf{e}) = \mathbf{e}^T\mathbf{P}\mathbf{e} + \int_0^t \mathbf{f}^T(\tau)\mathbf{R}\mathbf{f}(\tau)d\tau$$ 其中$\mathbf{e} = [\mathbf{e}_p^T, \mathbf{e}_v^T]^T$是跟踪误差。

稳定性分析：

QP的最优性保证： $$\dot{V} = -\mathbf{e}^T\mathbf{Q}\mathbf{e} - \sum_i w_i|\mathbf{e}_{task,i}|^2 \leq 0$$ 由LaSalle不变性原理，系统收敛到最大不变集： $$\mathcal{M} = \{\mathbf{e} : \dot{V} = 0\} = \{\mathbf{e} : \mathbf{e} = \mathbf{0}\}$$ 递归可行性：

定义可行集： $$\mathcal{X}_k = \{\mathbf{x} : \exists \mathbf{u}, \text{s.t. constraints satisfied}\}$$ QP约束设计保证： $$\mathbf{x}_k \in \mathcal{X}_k \Rightarrow \mathbf{x}_{k+1} \in \mathcal{X}_{k+1}$$ 通过终端约束和输入约束的careful设计实现。

跟踪性能：

定义性能指标： $$J_N = \sum_{k=0}^{N-1} |\mathbf{e}_k|_{\mathbf{Q}}^2 + |\mathbf{u}_k|_{\mathbf{R}}^2$$ QP的优化保证： $$J_N^* \leq J_N^{feasible} \leq \gamma J_N^{nominal}$$

其中$\gamma$依赖于模型误差界。

结论：QP-WBC在满足假设条件下提供稳定性、可行性和有界跟踪误差保证。

12.7 常见陷阱与错误

数值问题

陷阱1：雅可比矩阵奇异

症状：QP求解失败或产生极大的加速度
原因：机器人接近奇异配置
解决：
使用阻尼最小二乘：$\mathbf{J}^{\dagger} = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T + \lambda\mathbf{I})^{-1}$
添加正则化项到QP目标函数
检测并避免奇异配置

陷阱2：数值缩放问题

症状：某些约束总是违反，某些任务被忽略
原因：变量和约束的数量级差异过大
解决：
归一化所有物理量（长度、力、力矩）
调整权重使各项贡献相当
使用预处理器改善矩阵条件数

建模错误

陷阱3：忽略非线性动力学项

症状：高速运动时跟踪性能下降
原因：科氏力和离心力被忽略或计算错误
解决：
完整计算$\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}}$
使用递归Newton-Euler算法
验证动力学模型的能量一致性

陷阱4：接触模型不准确

症状：接触时抖动或不稳定
原因：刚性接触假设不成立
解决：
引入接触柔顺性模型
使用力传感器反馈
实施接触力滤波

实现问题

陷阱5：实时性能不足

症状：控制周期超时，性能降级
原因：计算负载过重
解决：
简化任务数量和约束
使用更快的QP求解器
实施分级控制架构
并行化独立计算

陷阱6：约束冲突

症状：QP无解
原因：约束集不一致
解决：
实施约束优先级
使用松弛变量
运行时约束一致性检查
提供备用控制策略

调试技巧

可视化工具： - 实时显示任务空间轨迹 - 监控约束满足情况 - 绘制QP收敛历史
日志和诊断：

LOG: Task errors: [0.012, 0.003, 0.045]
LOG: Constraint violations: none
LOG: QP iterations: 12
LOG: Solve time: 3.2ms
WARNING: Near singularity detected (condition number: 1e5)

单元测试： - 测试每个任务的雅可比计算 - 验证动力学模型 - 检查QP问题构建
渐进集成： - 从简单任务开始 - 逐步添加约束 - 最后引入接触
性能分析： - Profile计算热点 - 监控内存分配 - 跟踪缓存命中率