第9章：全身控制与平衡

开篇

轮足机械臂机器人的全身控制是实现复杂动态行为的核心技术。不同于传统的固定基座机械臂，移动机器人需要在执行任务的同时保持动态平衡，这要求我们在统一的优化框架下协调所有关节、轮子和接触力。本章将深入探讨从经典的ZMP理论到现代的QP优化方法，帮助读者掌握设计和实现鲁棒全身控制器的关键技术。我们将特别关注如何处理欠驱动系统、接触约束和实时计算要求之间的权衡。

9.1 ZMP与稳定性准则

9.1.1 零力矩点(ZMP)理论基础

零力矩点是判断机器人动态稳定性的经典准则。ZMP定义为地面上一点，在该点处地面反作用力产生的净力矩在水平方向上为零。对于平坦地面，ZMP的位置可以通过以下公式计算：

$$ x_{zmp} = \frac{\sum_i m_i(x_i\ddot{z}_i - z_i\ddot{x}_i - g x_i)}{\sum_i m_i(\ddot{z}_i + g)} $$

$$ y_{zmp} = \frac{\sum_i m_i(y_i\ddot{z}_i - z_i\ddot{y}_i - g y_i)}{\sum_i m_i(\ddot{z}_i + g)} $$

其中 $m_i$ 是第 $i$ 个质点的质量，$(x_i, y_i, z_i)$ 是其位置，$g$ 是重力加速度。

9.1.2 支撑多边形与稳定裕度

支撑多边形由所有接触点的凸包定义。稳定性判据要求ZMP必须位于支撑多边形内部：

      支撑多边形示意图（俯视图）

    FL ●━━━━━━━━━━━● FR
       ┃            ┃
       ┃     ZMP    ┃
       ┃      ×     ┃
       ┃            ┃
    RL ●━━━━━━━━━━━● RR

    FL: 前左轮  FR: 前右轮
    RL: 后左轮  RR: 后右轮

稳定裕度定义为ZMP到支撑多边形边界的最小距离：

$$ \text{margin} = \min_{e \in \text{edges}} d(p_{zmp}, e) $$

9.1.3 CoM-ZMP动力学关系

质心(CoM)与ZMP之间的动力学关系可以简化为线性倒立摆模型(LIPM)：

$$ \ddot{x}_{com} = \omega^2(x_{com} - x_{zmp}) $$

其中 $\omega = \sqrt{g/z_{com}}$ 是自然频率，$z_{com}$ 是质心高度。这个简化模型在轨迹规划中极其有用。

9.1.4 ZMP的局限性与扩展

传统ZMP理论假设：

1. 地面平坦且水平
2. 无滑动（足够的摩擦力）
3. 点接触或平面接触

对于轮足机器人，我们需要扩展ZMP概念：

虚拟ZMP (VZMP)：考虑轮子的滚动约束，将轮地接触力等效为支撑多边形内的虚拟点力。

摩擦修正ZMP：考虑有限摩擦系数 $\mu$ 时的稳定区域：

$$ |F_{tangential}| \leq \mu F_{normal} $$

9.2 QP优化的全身控制

9.2.1 全身动力学方程

轮足机械臂系统的完整动力学方程：

$$ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = S^T\tau + J_c^T F_c $$

其中：

• $M(q)$：质量矩阵
• $C(q,\dot{q})$：科氏力和离心力
• $G(q)$：重力项
• $S$：选择矩阵（区分驱动和欠驱动自由度）
• $\tau$：关节力矩
• $J_c$：接触雅可比
• $F_c$：接触力

9.2.2 任务空间控制目标

全身控制器通常需要同时追踪多个任务：

1. 质心任务： $$ \ddot{x}_{com}^{des} = K_p^{com}(x_{com}^{ref} - x_{com}) + K_d^{com}(\dot{x}_{com}^{ref} - \dot{x}_{com}) + \ddot{x}_{com}^{ref} $$

2. 末端执行器任务： $$ \ddot{x}_{ee}^{des} = K_p^{ee}(x_{ee}^{ref} - x_{ee}) + K_d^{ee}(\dot{x}_{ee}^{ref} - \dot{x}_{ee}) + \ddot{x}_{ee}^{ref} $$

3. 姿态任务： $$ \alpha^{des} = K_p^{ori} \log(R^{ref} R^T) + K_d^{ori}(\omega^{ref} - \omega) $$

9.2.3 QP问题构建

标准QP形式：

$$ \begin{aligned} \min_{\ddot{q}, F_c, \tau} \quad & \sum_i w_i ||A_i\ddot{q} + b_i||^2 + w_\tau ||\tau||^2 + w_F ||F_c||^2 \\ \text{s.t.} \quad & M\ddot{q} + h = S^T\tau + J_c^T F_c \\ & \underline{F}_c \leq F_c \leq \bar{F}_c \\ & \underline{\tau} \leq \tau \leq \bar{\tau} \\ & \underline{\ddot{q}} \leq \ddot{q} \leq \bar{\ddot{q}} \end{aligned} $$

其中 $A_i\ddot{q} + b_i = 0$ 表示第 $i$ 个任务的加速度误差。

9.2.4 层级任务优化

处理任务冲突的层级方法：

# 伪代码：层级QP求解
def hierarchical_qp(tasks, constraints):
    solution = None
    accumulated_constraints = constraints

    for priority_level in tasks:
        # 构建当前层级的QP
        qp = build_qp(priority_level, accumulated_constraints)
        solution = solve_qp(qp)

        # 将解作为等式约束加入下一层级
        accumulated_constraints.add(
            task_jacobian @ solution == task_acceleration
        )

    return solution

9.2.5 实时求解策略

为满足1kHz控制频率要求：

1. 热启动：使用上一时刻的解初始化
2. 早停：限制迭代次数
3. 稀疏性利用：利用问题结构的稀疏性
4. 近似海塞矩阵：使用对角近似减少计算

9.3 接触力分配与摩擦锥约束

9.3.1 接触模型

轮足机器人的接触可分为：

1. 点接触：简化的轮地接触
2. 线接触：轮子侧滑时
3. 面接触：足式模式下的平面接触

每种接触的力空间维度不同：

• 点接触：3D力
• 线接触：4D（3D力 + 1D力矩）
• 面接触：6D（3D力 + 3D力矩）

9.3.2 摩擦锥线性化

库仑摩擦锥的非线性约束：

$$ \sqrt{f_x^2 + f_y^2} \leq \mu f_z $$

线性化为金字塔约束（4面体）：

$$ \begin{aligned} |f_x| &\leq \frac{\mu}{\sqrt{2}} f_z \\ |f_y| &\leq \frac{\mu}{\sqrt{2}} f_z \end{aligned} $$

或使用8面体获得更好的近似：

$$ |f_x| + |f_y| \leq \mu f_z $$

9.3.3 接触力优化分配

给定期望的净力矩 $w^{des} = [f^{des}, \tau^{des}]^T$，分配到各接触点：

$$ \min_{F_c} \sum_i ||F_{c,i}||^2_Q + \lambda ||A_c F_c - w^{des}||^2 $$

其中 $A_c$ 是接触力到净力矩的映射矩阵：

$$ A_c = \begin{bmatrix} I_3 & I_3 & \cdots & I_3 \\ [p_1]_\times & [p_2]_\times & \cdots & [p_n]_\times \end{bmatrix} $$

9.3.4 轮子特殊约束

轮子的滚动约束要求切向力与驱动力矩一致：

$$ f_{wheel,tangent} = \tau_{wheel} / r_{wheel} $$

转向轮增加额外约束：

$$ f_{lateral} \cos(\delta) - f_{longitudinal} \sin(\delta) = 0 $$

其中 $\delta$ 是转向角。

9.3.5 接触状态切换

处理接触建立/断开的平滑过渡：

接触力profile：

Force
  ▲
  │     ___稳定接触___
  │    /               \
  │   /                 \
  │  /建立              断开\
  │ /                       \
──┴─────────────────────────► Time
  t0  t1              t2  t3

使用sigmoid函数平滑过渡：

$$ \gamma(t) = \frac{1}{1 + e^{-k(t - t_{switch})}} $$

9.4 动态平衡与步态转换

9.4.1 动态行走的稳定性

与静态稳定不同，动态行走允许ZMP暂时离开支撑多边形，利用角动量维持平衡：

$$ \dot{L} = \sum_i (p_i - p_{com}) \times F_i + \tau_{external} $$

捕获点(Capture Point)定义：

$$ \xi = x_{com} + \frac{\dot{x}_{com}}{\omega} $$

稳定条件：捕获点必须在可达支撑区域内。

9.4.2 轮足模式切换策略

轮足机器人的独特优势在于可以根据地形和任务需求切换运动模式：

1. 纯轮模式：高速、高效、平坦地形
2. 轮足混合：中速、复杂地形
3. 纯足模式：低速、极端地形（楼梯、碎石）

模式切换状态机：

    ┌─────────┐
    │纯轮模式 │◄────────┐
    └────┬────┘         │
         │              │
    v > v_max      稳定性恢复
         │              │
         ▼              │
    ┌─────────┐         │
    │混合模式 │─────────┘
    └────┬────┘    
         │
    地形复杂度 > θ
         │
         ▼
    ┌─────────┐
    │纯足模式 │
    └─────────┘

9.4.3 步态生成与优化

相位协调：使用中央模式发生器(CPG)或优化方法生成步态：

$$ \phi_i(t) = \omega t + \phi_{i,0} + \sum_j w_{ij} \sin(\phi_j - \phi_i) $$

占空比优化：根据速度和稳定性要求调整支撑相/摆动相比例：

$$ \text{duty factor} = \frac{T_{stance}}{T_{stance} + T_{swing}} $$

典型值：

• Walk: 0.75
• Trot: 0.5
• Gallop: 0.4

9.4.4 角动量规划

全身角动量的主动调节用于动态平衡：

$$ L = \sum_i I_i \omega_i + \sum_i m_i (p_i - p_{com}) \times v_i $$

通过优化问题规划角动量轨迹：

$$ \min \int_0^T ||\dot{L}(t) - \dot{L}^{ref}(t)||^2 dt $$

约束于可行的接触力和关节限制。

9.4.5 推力恢复策略

受扰动后的平衡恢复策略层级：

1. 踝策略：小扰动，调整CoP位置
2. 髋策略：中等扰动，调整角动量
3. 步进策略：大扰动，改变支撑位置

决策阈值基于线性倒立摆的可行域：

$$ |\xi - p_{support}| > d_{critical} \Rightarrow \text{需要步进} $$

案例研究：波士顿动力Atlas的动态平衡

背景

Atlas是波士顿动力开发的人形机器人，展示了卓越的动态运动能力，包括跑步、跳跃和后空翻。其全身控制系统是业界标杆。

控制架构

Atlas采用三层控制架构：

1. 高层规划器（10-50Hz） - 轨迹优化 - 步态规划 - 任务分解

2. 中层控制器（200-500Hz） - 全身QP控制 - 平衡控制 - 接触力分配

3. 底层控制器（1-5kHz） - 关节伺服 - 力/力矩控制 - 传感器融合

关键技术创新

1. 预测控制与MPC

Atlas使用模型预测控制处理动态运动：

$$ \min_{u_{0:N-1}} \sum_{k=0}^{N-1} ||x_k - x_k^{ref}||_Q + ||u_k||_R $$

预测时域通常为1-2秒，允许机器人"预见"未来并提前调整。

2. 混合动力学建模

结合刚体动力学和简化模型：

• 全身：30+ DoF刚体模型（精确但计算密集）
• 规划：3D SLIP模型（简单但捕获主要动态）

3. 鲁棒接触检测

使用多传感器融合检测接触：

• 力传感器：直接测量
• IMU + 运动学：间接推断
• 视觉：预测接触

接触置信度估计：

$$ p_{contact} = \sigma(w_f f_{measured} + w_a a_{residual} + w_v v_{predicted}) $$

性能指标

Atlas在各种任务中的表现：

| 指标 | 数值 | 备注 |

经验教训

1. 计算资源权衡：复杂模型 vs 实时性能
2. 传感器冗余：单一传感器故障不应导致失败
3. 平滑过渡：模式切换必须连续
4. 学习组件：某些参数通过强化学习优化

高级话题：Centroidal动力学与角动量规划

Centroidal动力学框架

Centroidal动力学将机器人简化为质心处的集总动量：

$$ \begin{bmatrix} m\ddot{c} \\ \dot{L} \end{bmatrix} = \begin{bmatrix} \sum_i f_i \\ \sum_i (p_i - c) \times f_i + \tau_i \end{bmatrix} + \begin{bmatrix} mg \\ 0 \end{bmatrix} $$

其中 $c$ 是质心位置，$L$ 是关于质心的角动量。

质心复合刚体惯量(CCRBI)

CCRBI定义了质心角动量与平均角速度的关系：

$$ L = I_G(\q) \omega_{avg} $$

$I_G$ 是配置相关的3×3矩阵，可离线计算并存储。

动量空间的凸优化

在动量空间中，动力学约束变为线性：

$$ \begin{aligned} \dot{h} &= Ag + u \\ y &= Cx \end{aligned} $$

其中 $h = [mc\dot{}, L]^T$ 是广义动量，$u$ 是控制输入。

可达动量空间

给定接触配置，可达动量变化率形成凸多面体：

$$ \mathcal{U} = \{u | u = A_c f_c, f_c \in \mathcal{F}_c\} $$

这允许快速验证动作可行性。

角动量的最优分配

将期望角动量分配到各肢体：

$$ \min_{\dot{q}} ||\dot{q}||^2 \quad \text{s.t.} \quad L^{des} = \sum_i L_i(\q, \dot{q}) $$

使用伪逆或QP求解。

实际应用考虑

1. 计算效率：Centroidal模型比全身模型快100倍
2. 规划精度：足够用于轨迹规划，需要全身控制器跟踪
3. 在线适应：可实时重规划以应对扰动

本章小结

本章系统介绍了轮足机械臂机器人的全身控制与平衡技术。核心要点包括：

关键概念

1. ZMP理论：零力矩点提供了判断动态稳定性的直观准则，但需要根据轮足系统特点进行扩展
2. QP优化框架：将多任务控制问题转化为二次规划，实现实时全身协调
3. 接触力分配：考虑摩擦锥约束的力优化分配是保证运动可行性的关键
4. 动态平衡：利用角动量和捕获点概念扩展静态稳定性到动态场景
5. Centroidal动力学：提供计算高效的规划框架，将复杂动力学简化为质心动量演化

关键公式回顾

| 概念 | 公式 | 用途 |

设计要点

• 选择合适的模型复杂度：规划用简化模型，控制用详细模型
• 实时性优先：宁可次优解也要保证控制频率
• 传感器融合：多源信息提高鲁棒性
• 平滑过渡：模式切换和接触状态变化需要连续性

练习题

基础题

题目1：给定一个四轮机器人，轮距为1m×0.6m（长×宽），质心高度0.5m，计算在质心横向加速度为2m/s²时的ZMP位置。

题目2：一个全身控制QP问题有3个任务：质心追踪（权重10）、手臂末端追踪（权重5）、姿态保持（权重1）。如果质心任务和手臂任务发生冲突，QP求解器会如何权衡？

题目3：摩擦系数μ=0.6的接触点，最大法向力100N，计算其摩擦锥允许的最大切向力。使用4面体和8面体线性化，分别计算最大值。

题目4：机器人从轮式模式切换到足式模式，切换时间0.5秒，使用sigmoid函数$\gamma(t) = 1/(1+e^{-k(t-t_0)})$平滑过渡。要求在t=0.25s时达到50%切换，计算k值。

挑战题

题目5：设计一个简化的QP控制器，同时优化6个自由度机械臂的末端位置追踪和避奇异性。写出完整的QP问题formulation，包括目标函数和约束。

题目6：轮足机器人在斜坡上（倾角15°）保持静止，四个轮子形成1m×0.6m矩形，质量50kg，质心高度0.4m。计算各轮所需的最小法向力，以及对摩擦系数的要求。

题目7：实现一个基于Centroidal动力学的轨迹优化问题。机器人需要从静止跳跃0.3m高，优化起跳和落地过程的质心轨迹和角动量。列出优化变量、目标函数和约束。

题目8（开放题）：讨论如何将强化学习集成到全身控制框架中。考虑：哪些参数适合学习？如何保证安全性？sim-to-real的挑战？

常见陷阱与错误

1. ZMP理解误区

• 错误：认为ZMP必须始终在支撑多边形中心
• 正确：ZMP可以在多边形内任意位置，甚至短暂超出（动态运动）

2. QP求解失败

• 原因：约束矛盾、数值病态、初始化差
• 解决：层级化约束、正则化、热启动、约束松弛

3. 摩擦锥线性化过度保守

• 问题：4面锥在对角方向损失40%可用摩擦力
• 改进：使用8面或16面锥，或二阶锥规划(SOCP)

4. 接触切换震荡

• 现象：接触建立/断开时控制输出剧烈变化
• 预防：平滑切换函数、提前规划、阻抗控制过渡

5. 实时性能不足

• 症状：控制延迟导致不稳定
• 优化：简化模型、并行计算、查找表、早停策略

6. 传感器噪声放大

• 问题：微分项放大测量噪声
• 对策：低通滤波、状态估计器、鲁棒控制设计

调试技巧

1. 可视化工具：实时显示ZMP、CoM、接触力
2. 数据记录：高频记录所有状态用于事后分析
3. 仿真验证：先在仿真中调试，注意sim-to-real差异
4. 增量测试：从简单任务逐步增加复杂度
5. 故障注入：主动测试传感器故障、通信延迟等

最佳实践检查清单

设计阶段

• [ ] 明确稳定性要求：静态 vs 动态
• [ ] 选择合适的控制架构：集中式 vs 分布式
• [ ] 确定控制频率：规划(10-50Hz)、控制(200-1000Hz)
• [ ] 设计冗余方案：传感器、执行器、算法

实现阶段

• [ ] QP问题良好条件数：检查海塞矩阵
• [ ] 约束优先级明确：硬约束 vs 软约束
• [ ] 数值稳定性：避免除零、奇异性
• [ ] 实时性验证：最坏情况执行时间

测试阶段

• [ ] 静态平衡测试：各种姿态、负载
• [ ] 动态平衡测试：推力恢复、外部扰动
• [ ] 模式切换测试：所有可能的转换
• [ ] 极限工况测试：最大速度、最大负载
• [ ] 长时间运行测试：内存泄漏、数值漂移

优化阶段

• [ ] 性能分析：瓶颈识别、热点优化
• [ ] 参数调优：系统辨识、自动调参
• [ ] 能耗优化：减少不必要的运动
• [ ] 鲁棒性提升：噪声注入、参数摄动

维护阶段

• [ ] 日志系统：关键事件、异常记录
• [ ] 性能监控：实时指标、趋势分析
• [ ] 故障诊断：自检程序、故障定位
• [ ] 更新机制：参数、固件、算法