人形机器人足部受力MPC控制方法介绍

人形机器人足部受力 MPC 控制方法介绍

1. 控制目标

人形机器人在行走、站立或扰动恢复过程中，需通过调节足-地接触力（Ground Reaction Forces, GRFs）维持动态平衡。MPC（Model Predictive Control）通过在线优化未来一段时间内的控制输入，使系统状态跟踪参考轨迹，同时满足动力学约束与接触稳定性条件。

核心目标包括：

• 跟踪期望质心（CoM）轨迹；
• 调节足底接触力分布，防止滑移或翻转；
• 满足摩擦锥约束与零力矩点（ZMP）稳定性条件；
• 保证关节力矩/力在执行器能力范围内。

2. 系统建模

（1）简化动力学模型

常用线性倒立摆（LIP）或全阶刚体动力学模型。以质心动力学为例：

m · p̈ = f + m · g

其中：

• m 为机器人总质量；
• p ∈ ℝ³ 为质心位置；
• f ∈ ℝ³ 为足底合力（即地面反作用力 GRF）；
• g = [0, 0, −g]ᵀ 为重力加速度向量（g ≈ 9.81 m/s²）。

（2）接触模型

假设单足或双足支撑，每只脚有 nₚ 个虚拟接触点（如四角点模型），则总接触力向量为：

F = [f₁ᵀ, τ₁ᵀ, f₂ᵀ, τ₂ᵀ, …]ᵀ ∈ ℝ⁶ⁿᶠ

其中 fᵢ 为第 i 个接触点的力，τᵢ 为其力矩（通常 τᵢ 可忽略或由 fᵢ 分布决定）。

3. 约束条件（MPC 中的关键不等式）

（1）摩擦锥约束（Coulomb 摩擦）

对每个接触点 j，其切向力不能超过法向力的 μ 倍（μ 为摩擦系数）：

√(fⱼₓ² + fⱼᵧ²) ≤ μ · fⱼ_z, fⱼ_z ≥ 0

该非线性约束常被线性化为多面体近似（如八边形锥）以便 MPC 求解。

（2）ZMP 稳定性约束

ZMP 必须位于支撑多边形内部。ZMP 坐标 (x_zmp, y_zmp) 由下式计算：

x_zmp = x_com − (z_com / g) · ẍ_com
y_zmp = y_com − (z_com / g) · ÿ_com

要求：
(x_zmp, y_zmp) ∈ SupportPolygon

该条件可转化为对质心加速度 ẍ, ÿ 的线性不等式约束。

（3）执行器力矩限制

通过逆动力学将接触力 F 映射到关节力矩 τ：

τ = J_cᵀ · F + τ_bias

其中 J_c 为接触雅可比矩阵。需满足：

τₘᵢₙ ≤ τ ≤ τₘₐₓ

4. MPC 优化问题构建

在每个控制周期，求解如下有限时域最优控制问题：

最小化：
J = Σₖ₌₀ᴺ⁻¹ [ ‖pₖ − p_ref,k‖²_Q + ‖fₖ‖²_R ] + ‖p_N − p_ref,N‖²_P

满足：

• 动力学模型：pₖ₊₁ = A · pₖ + B · fₖ（离散化后）
• 摩擦锥约束（线性化）
• ZMP 在支撑多边形内
• 接触开关逻辑（根据步态相位激活/禁用某足力）
• fₖ_z ≥ 0（单向接触，不能“粘地”）

其中：

• N 为预测时域长度；
• Q, R, P 为权重矩阵；
• 决策变量为未来 N 步的接触力序列 {f₀, f₁, …, f_{N−1}}。

---

5. 实时实现要点

• 线性化处理：将非线性动力学在当前状态附近线性化，形成 LTV（线性时变）模型；
• 热启动（Warm Start）：利用上一周期解作为初值，加速 QP 求解；
• 接触模式调度：根据步态相位（如单支撑/双支撑）预设哪些足可施加力；
• 鲁棒性增强：引入扰动估计或 tube-MPC 应对外部干扰。

---

6. 优势与挑战

优势：

• 显式处理多约束（力、摩擦、ZMP）；
• 支持多接触协调（如双手+双脚）；
• 可融合高层任务（如避障、推拉操作）。

挑战：

• 计算复杂度高，需高效求解器（如 OSQP、qpOASES）；
• 模型失配可能导致不稳定；
• 足底力传感器噪声影响性能（常需滤波或观测器融合）。