在前面的章节中，我们探讨了机器人的运动规划和模型预测控制。然而，实际的机器人任务通常定义在操作空间（末端执行器空间）而非关节空间。本章将深入研究如何在任务空间中直接设计控制器，以及如何通过优化方法协调多个竞争性任务。我们将特别关注全身控制（Whole-Body Control, WBC）框架，它能够同时处理运动控制、力控制和约束满足问题。通过本章的学习，读者将掌握现代机器人系统中广泛应用的任务空间控制理论，并理解如何在实际系统中实现高性能的全身控制器。
操作空间控制的核心思想是直接在任务相关的坐标系中设计控制律，而不是在关节空间中。考虑一个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{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 │
└─────────────┘
在许多实际应用中，我们需要同时控制位置和力。例如，在装配任务中，某些方向需要位置控制以保证精度，而其他方向需要力控制以避免过大的接触力。混合控制框架通过选择矩阵实现这种分离：
\[\mathbf{F} = \mathbf{S}\mathbf{F}_{motion} + (\mathbf{I} - \mathbf{S})\mathbf{F}_{force}\]其中$\mathbf{S} \in \mathbb{R}^{m \times m}$是对角选择矩阵，对角元素为0或1：
位置控制分量： \(\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})\)
阻抗控制是一种统一处理位置和力控制的方法，它通过调节机器人的动态行为来实现期望的接触交互。阻抗控制的目标是使末端执行器表现出期望的质量-弹簧-阻尼系统行为：
\[\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}$是修正后的位置误差。
全身控制（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}\]其中：
在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}\)
对于实时控制，QP求解器的选择至关重要。常用的求解器包括：
1. 稠密QP求解器：
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$是加速度的海塞矩阵。
┌─────────────────────────┐
│ 传感器数据获取 │
│ (q, q̇, IMU, F/T) │
└───────────┬─────────────┘
▼
┌─────────────────────────┐
│ 状态估计 │
│ (浮动基座状态) │
└───────────┬─────────────┘
▼
┌─────────────────────────┐
│ 任务定义 │
│ (位置、力、平衡等) │
└───────────┬─────────────┘
▼
┌─────────────────────────┐
│ 构建QP问题 │
│ (目标函数+约束) │
└───────────┬─────────────┘
▼
┌─────────────────────────┐
│ 求解QP │
│ (获得q̈*, τ*, f*) │
└───────────┬─────────────┘
▼
┌─────────────────────────┐
│ 力矩映射 │
│ (τ_cmd = S^Tτ*) │
└───────────┬─────────────┘
▼
┌─────────────────────────┐
│ 执行器控制 │
└─────────────────────────┘
在全身控制中，正确建模接触约束是保证控制器性能的关键。接触约束包括运动学约束和力学约束两个方面。
运动学接触约束：
对于刚性接触，接触点的加速度必须为零（无滑动、无分离）：
\[\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$是摩擦系数。
将圆锥约束线性化为棱锥： \(\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₂
动力学一致性确保计算的加速度、力和力矩满足机器人的动力学方程。这需要在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$表示驱动关节。
质心（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}$定义支撑多边形的半空间表示。
处理接触模式切换是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}\]为提高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}\)
Baxter是Rethink Robotics开发的双臂协作机器人，具有独特的设计特点和控制挑战：
硬件配置：
控制挑战：
系统建模：
将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}\]自碰撞检测：
使用胶囊体（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}}\)
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})]\)
任务场景：双臂搬运
实验设置：
性能指标：
位置误差 (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 │
└─────────────────┴──────────┘
鲁棒性测试：
代码架构：
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()
本章深入探讨了任务空间控制与优化的核心理论和实践方法。我们从操作空间控制的基本原理出发，逐步构建了完整的全身控制框架，并通过Baxter双臂机器人的案例展示了这些理论在实际系统中的应用。
操作空间控制：
二次规划基全身控制（QP-WBC）：
接触一致性与动力学可行性：
任务空间到关节空间映射： \(\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\)
练习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$。
练习12.2 动力学一致伪逆的性质
证明动力学一致的伪逆$\mathbf{J}^{#} = \mathbf{M}^{-1}\mathbf{J}^T\mathbf{\Lambda}$满足以下性质：
提示：利用$\mathbf{\Lambda} = (\mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T)^{-1}$的定义。
练习12.3 摩擦锥线性化误差分析
考虑将圆锥摩擦约束$|\mathbf{f}_t| \leq \mu f_n$线性化为4面棱锥。计算线性化引入的最大相对误差。
提示：考虑圆锥和棱锥在切平面上的投影差异。
练习12.4 ZMP稳定性条件
推导平面接触情况下ZMP必须位于支撑多边形内的条件，并说明为什么这保证了动态平衡。
提示：考虑绕支撑边缘的力矩平衡。
练习12.5 层次化QP的数值稳定性
设计一个数值稳定的层次化QP求解算法，处理任务之间可能的数值冲突。考虑以下情况：
提示：考虑使用正则化和松弛变量。
练习12.6 多接触场景的力分配优化
设计一个算法，在四足机器人的四个脚掌接触情况下，优化分配接触力以：
提示：将问题分解为解析部分和优化部分。
练习12.7 接触切换的平滑过渡
设计一个控制策略，使机器人从双脚支撑平滑过渡到单脚支撑，同时保持：
提示：使用时变权重和预测控制。
练习12.8 WBC框架的形式化验证
证明在给定的假设条件下，QP-based WBC框架能够保证：
提示：构造合适的Lyapunov函数并利用QP的最优性条件。
陷阱1：雅可比矩阵奇异
陷阱2：数值缩放问题
陷阱3：忽略非线性动力学项
陷阱4：接触模型不准确
陷阱5：实时性能不足
陷阱6：约束冲突
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)