4. 全身控制问题建模

全身控制（Whole-Body Control, WBC）的核心挑战在于：人形机器人拥有数十个自由度，同时需要协调运动、力交互与多类约束。若直接在高维关节空间中求解，问题将变得病态且难以实时处理。本章从数学建模的角度出发，系统梳理全身动力学模型的基本形式、任务空间与操作空间的映射关系，以及接触、关节限位、力矩限位等约束的规范化表达。这些内容构成了后续全身控制器设计、优化求解与仿真验证的理论基石。

4.1 全身动力学模型：从浮动基座到关节空间

4.1.1 核心原理：浮动基座动力学方程

人形机器人不同于固定基座机械臂，其基座（骨盆/躯干）在空间中自由浮动，没有物理接地。因此，全身动力学模型必须采用浮动基座（Floating Base）描述。广义坐标定义为：

q = [q_base; q_joint] ∈ R^(6+n)
q_base = [x, y, z, φ, θ, ψ]  （基座位置与欧拉角/四元数）
q_joint ∈ R^n              （n 个驱动关节角度）

全身动力学方程的标准形式为：

M(q)q̈ + C(q, q̇)q̇ + G(q) = Sᵀτ + J_cᵀF_c

其中：M(q) 为广义惯性矩阵（正定对称），C(q,q̇)q̇ 为科氏力与离心力项，G(q) 为重力项，S = [0₆ₓₙ; Iₙₓₙ] 为选择矩阵（基座不受直接驱动），τ 为关节力矩，J_c 为接触约束雅可比矩阵，F_c 为接触外力（含地面反作用力）。

4.1.2 判断方法：模型参数获取与验证

在实际工程中，建立准确的全身动力学模型需要以下步骤：

1. 运动学参数标定：通过CAD模型或激光扫描获取各连杆的长度、质心位置、惯性张量。
2. 动力学参数辨识：设计激励轨迹，利用最小二乘法或最大似然估计辨识摩擦系数、惯性参数。
3. 模型验证：对比仿真预测力矩与实际测量力矩，计算均方根误差（RMSE），通常要求关节力矩预测误差小于5%额定力矩。
4. 实时性评估：检查模型计算耗时，确保在控制周期（通常1kHz）内完成正逆动力学计算。

4.1.3 案例拆解：双足站立时的动力学简化

以双足平地站立为例，接触约束为两足底与地面的固定接触。此时，接触雅可比矩阵 J_c 将地面反作用力映射到广义坐标。若假设两足完全静止，则接触点加速度为零，可推导出：

J_c q̈ + J̇_c q̇ = 0  （接触点加速度约束）

将动力学方程代入并消去 q̈，可得到地面反作用力的解析表达式。实际工程中，我们常使用质心动力学（Centroidal Dynamics）简化：

L̇ = Σ (p_i - p_com) × F_i + τ_ext
ṗ_com = L / m

其中 L 为质心角动量，p_i 为接触点位置，F_i 为接触力。这种简化将全身动力学降维到质心层面，大幅降低计算复杂度，适用于步态规划与平衡控制。

4.2 任务空间与操作空间：从关节坐标到任务坐标

4.2.1 核心原理：雅可比映射与操作空间惯性

任务空间（Task Space）描述机器人需要控制的关键变量，如质心位置、足端位置、手端姿态等。设任务变量为 x ∈ Rᵐ，其与关节坐标的关系为：

x = f(q)          （正向运动学）
ẋ = J(q)q̇        （微分映射）
ẍ = J(q)q̈ + J̇(q)q̇  （加速度映射）

其中 J(q) = ∂f/∂q ∈ Rᵐˣⁿ 为任务雅可比矩阵。操作空间（Operational Space）特指末端执行器（如手、脚）的位姿空间。操作空间动力学方程为：

Λ(q)ẍ + μ(q,q̇) + ρ(q) = F_task

其中 Λ(q) = (J M⁻¹ Jᵀ)⁻¹ 为操作空间惯性矩阵，μ 为操作空间科氏力，ρ 为操作空间重力，F_task 为施加在任务空间上的广义力。

4.2.2 判断方法：任务优先级与零空间投影

当任务数量超过机器人自由度时（m > n），任务之间可能冲突。此时需要引入任务优先级（Task Priority）框架：

1. 主任务：如保持质心平衡，具有最高优先级。
2. 次任务：如足端轨迹跟踪，在主任务零空间中执行。
3. 零空间投影：N = I - J⁺J，其中 J⁺ 为雅可比的伪逆。

实际控制律可写为：

q̈ = J₁⁺(ẍ₁_des - J̇₁q̇) + N₁ J₂⁺(ẍ₂_des - J̇₂q̇) + ...

其中 N₁ = I - J₁⁺J₁ 将次任务投影到主任务的零空间中，保证次任务不影响主任务。

4.2.3 案例拆解：全身行走控制中的任务分层

以 Atlas 机器人行走控制为例，任务空间通常按以下优先级排列：

在实现中，每个任务层通过零空间投影逐级叠加。例如，手臂运动（优先级5）的加速度指令需投影到前四个任务的零空间中，确保不影响平衡、足端轨迹和躯干姿态。

4.3 约束的数学表达：接触、关节限位与力矩限位

4.3.1 核心原理：三类约束的规范化形式

全身控制问题本质上是一个带约束的最优控制问题。约束的数学表达直接影响求解器的收敛性与控制效果。

（1）接触约束——描述机器人与环境的交互关系：

单边约束：φ(q) ≥ 0  （足底与地面距离非负）
摩擦锥：√(f_x² + f_y²) ≤ μ f_z, f_z ≥ 0
接触点速度：J_c q̇ = 0  （固定接触）
互补条件：φ(q) · f_z = 0  （接触力与距离互补）

摩擦锥通常线性化为多边形（如四棱锥或八棱锥），以便嵌入QP（二次规划）求解器。

（2）关节限位——保护机械结构：

位置限位：q_min ≤ q ≤ q_max
速度限位：|q̇| ≤ q̇_max
加速度限位：|q̈| ≤ q̈_max

在优化中，位置限位可通过障碍函数或投影法处理；速度与加速度限位则直接作为线性不等式约束。

（3）力矩限位——电机物理限制：

τ_min ≤ τ ≤ τ_max
|τ̇| ≤ τ̇_max  （力矩变化率限制）

力矩限位通常作为控制输出的饱和约束，在QP中表示为箱式约束（Box Constraint）。

4.3.2 判断方法：约束建模的QP标准形式

全身控制问题通常转化为以下二次规划（QP）形式：

min  ½ ‖A x - b‖² + ½ xᵀW x
s.t.  C x ≤ d
      E x = f
      x_min ≤ x ≤ x_max

其中决策变量 x = [q̈; τ; F_c] 包含关节加速度、关节力矩和接触力。约束矩阵 C、E 由上述三类约束的线性化形式构成。求解QP时需注意：

1. 约束一致性检查：确保约束集合非空，避免不可行问题。
2. 软约束与硬约束：接触约束通常设为硬约束，关节限位可设为软约束（通过松弛变量）以避免不可行。
3. 实时性：QP规模通常为 n=30-50 个变量，约束数 100-200 条，需在1ms内求解完毕。

4.3.3 案例拆解：单腿支撑期的约束处理

以单腿站立为例，支撑脚与地面接触，摆动脚悬空。约束建模如下：

接触约束（支撑脚）：
  φ(q) = 0 （足底与地面接触）
  J_c q̇ = 0 （接触点无滑动）
  f_z ≥ 0, |f_x| ≤ μ f_z, |f_y| ≤ μ f_z

关节限位：
  q_min + ε ≤ q ≤ q_max - ε （ε为安全裕度）
  |q̇| ≤ q̇_max

力矩限位：
  τ_min ≤ τ ≤ τ_max

实际调试中，我们发现摩擦系数 μ 的取值对控制稳定性影响显著。μ 过小导致接触力频繁超出摩擦锥，控制器判定打滑；μ 过大则允许过大的水平力，可能导致足底滑动。建议根据地面材质实测摩擦系数，并留取10%-20%的安全裕度。

4.3.4 总结复盘：约束建模的工程要点

全身控制问题建模的核心在于：用准确的动力学描述系统行为，用合理的任务空间映射表达控制目标，用规范的约束形式保证安全与可行性。三者缺一不可。在实际工程中，建议采用分层控制架构：上层用质心动力学做平衡规划，中层用全身QP求解关节指令，底层用关节PD控制器跟踪。这种架构既能保证实时性，又能兼顾控制精度与安全性。