轮足机械臂机器人的全身控制是实现复杂动态行为的核心技术。不同于传统的固定基座机械臂，移动机器人需要在执行任务的同时保持动态平衡，这要求我们在统一的优化框架下协调所有关节、轮子和接触力。本章将深入探讨从经典的ZMP理论到现代的QP优化方法，帮助读者掌握设计和实现鲁棒全身控制器的关键技术。我们将特别关注如何处理欠驱动系统、接触约束和实时计算要求之间的权衡。
零力矩点是判断机器人动态稳定性的经典准则，最早由Vukobratović在1972年提出，至今仍是双足和多足机器人控制的基石。ZMP定义为地面上一点，在该点处地面反作用力产生的净力矩在水平方向上为零。这个概念的物理意义在于：如果我们能将所有分布的地面反力等效为作用在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$ 是重力加速度。
值得注意的是，ZMP公式中包含了惯性项（加速度项），这使其不同于静态的压力中心(CoP)。在静态或准静态运动中，ZMP退化为CoP，但在动态运动中，两者可能显著不同。这种差异正是机器人能够执行动态运动（如跑步、跳跃）的理论基础。
ZMP的数学性质：
支撑多边形(Support Polygon)是机器人稳定性分析的几何基础，由所有接触点的凸包(Convex Hull)定义。对于轮足机器人，这个概念需要特别注意，因为轮子的接触可能是点接触（理想情况）或小面积接触（实际情况）。
稳定性判据的核心原则：ZMP必须位于支撑多边形内部才能保证不倾覆。这可以从力矩平衡的角度理解：当ZMP位于支撑边界外时，意味着需要”拉力”来维持平衡，而地面只能提供推力。
支撑多边形示意图（俯视图）
FL ●━━━━━━━━━━━● FR
┃ ┃
┃ ZMP ┃ <- 稳定区域
┃ × ┃
┃ ┃
RL ●━━━━━━━━━━━● RR
FL: 前左轮 FR: 前右轮
RL: 后左轮 RR: 后右轮
不同运动状态下的支撑多边形变化：
静止：完整矩形
单轮抬起：三角形
对角支撑：线段（临界稳定）
稳定裕度(Stability Margin)定量描述了系统距离失稳的”距离”：
\[\text{margin} = \min_{e \in \text{edges}} d(p_{zmp}, e)\]稳定裕度的工程考量：
对于轮足机器人的特殊考虑：
质心(Center of Mass, CoM)与ZMP之间的动力学关系是理解和控制机器人平衡的关键。Kajita等人提出的线性倒立摆模型(Linear Inverted Pendulum Model, LIPM)提供了一个优雅的简化框架，将复杂的多体动力学简化为可解析求解的形式。
LIPM的核心假设：
在这些假设下，水平面内的动力学方程简化为：
\[\ddot{x}_{com} = \omega^2(x_{com} - x_{zmp})\] \[\ddot{y}_{com} = \omega^2(y_{com} - y_{zmp})\]其中 $\omega = \sqrt{g/z_{com}}$ 是系统的自然频率。这个频率决定了系统的时间尺度：
解析解与轨迹规划：
给定初始状态 $(x_0, \dot{x}0)$ 和常值 $x{zmp}$，LIPM的解为：
\[x_{com}(t) = x_{zmp} + (x_0 - x_{zmp})\cosh(\omega t) + \frac{\dot{x}_0}{\omega}\sinh(\omega t)\]这个解析解使得我们可以：
LIPM的扩展模型：
3D-LIPM：考虑质心高度变化 \(\ddot{x}_{com} = \frac{g}{z_{com}}(x_{com} - x_{zmp}) + \frac{2\dot{z}_{com}}{z_{com}}\dot{x}_{com}\)
LIPM with Flywheel：加入角动量 \(\ddot{x}_{com} = \omega^2(x_{com} - x_{zmp}) - \frac{\dot{L}_y}{mz_{com}}\)
传统ZMP理论虽然强大，但存在明显局限，特别是应用于轮足机器人时：
基本假设的限制：
理论局限性：
对于轮足机器人的特殊挑战：
1. 虚拟ZMP (Virtual ZMP, VZMP)： 轮子的滚动约束使得接触力分布与传统足式机器人不同。VZMP概念将轮地接触力等效映射：
\[p_{vzmp} = \frac{\sum_i (p_i \times F_i) + \tau_{wheel,i}}{F_{total}}\]其中 $\tau_{wheel,i}$ 是轮子驱动力矩的贡献。
2. 摩擦锥约束下的可行ZMP区域： 有限摩擦系数 $\mu$ 限制了可实现的ZMP范围：
\[\mathcal{Z}_{feasible} = \{p_{zmp} | \exists F_i \in \mathcal{F}_{\mu}, \sum_i (p_i - p_{zmp}) \times F_i = 0\}\]| 其中 $\mathcal{F}_{\mu} = {F | F_{tangential} | \leq \mu F_{normal}}$ 是摩擦锥。 |
3. 斜坡地形的广义ZMP： 对于倾斜角度为 $\theta$ 的斜坡：
\[x_{gzmp} = x_{zmp}\cos\theta - \frac{h}{\cos\theta}(a_x\cos\theta + (a_z + g)\sin\theta)/(a_z + g)\]4. 柔性地面的等效ZMP： 考虑地面刚度 $k_{ground}$ 和阻尼 $c_{ground}$：
\[F_{ground} = k_{ground}\delta + c_{ground}\dot{\delta}\]ZMP位置需要迭代求解以满足力平衡。
现代机器人控制已经从传统的分层控制架构转向统一的优化框架。二次规划(Quadratic Programming, QP)因其凸性、计算效率和直观的物理意义，成为全身控制的标准方法。本节将详细介绍如何构建和求解适用于轮足机械臂的QP控制器。
轮足机械臂系统是一个高度复杂的多体系统，通常包含30-50个自由度。其完整动力学方程可通过拉格朗日方法或牛顿-欧拉递归算法导出：
\[M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = S^T\tau + J_c^T F_c\]各项的物理意义和计算方法：
质量矩阵 $M(q) \in \mathbb{R}^{n \times n}$：
科氏力和离心力 $C(q,\dot{q})\dot{q} \in \mathbb{R}^n$：
重力项 $G(q) \in \mathbb{R}^n$：
选择矩阵 $S \in \mathbb{R}^{m \times n}$： 区分驱动(actuated)和欠驱动(underactuated)自由度： \(S = \begin{bmatrix} 0_{m \times 6} & I_{m \times m} \end{bmatrix}\) 前6个自由度通常对应浮动基座（3个平移+3个旋转），后m个对应关节。
接触雅可比 $J_c \in \mathbb{R}^{3k \times n}$： 将广义速度映射到k个接触点的笛卡尔速度： \(v_{c,i} = J_{c,i}(q)\dot{q}\)
对于轮足机器人，需要特别处理：
动力学方程的数值特性：
全身控制的核心在于同时满足多个可能冲突的任务目标。任务空间控制允许我们直接在操作空间（如笛卡尔空间）指定期望行为，而不是在复杂的关节空间。
任务定义与分类：
任务可以表示为一般形式： \(\ddot{x}_{task} = J_{task}\ddot{q} + \dot{J}_{task}\dot{q}\)
常见任务类型包括：
1. 质心(CoM)任务 - 维持平衡的核心： \(\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. 姿态任务 - 身体方向控制：
使用SO(3)上的对数映射处理旋转： \(\alpha^{des} = K_p^{ori} \log(R^{ref} R^T) + K_d^{ori}(\omega^{ref} - \omega)\)
其中 $\log: SO(3) \rightarrow \mathfrak{so}(3)$ 是矩阵对数，提取旋转向量。
实际实现中的四元数形式： \(e_{ori} = q_{ref} \otimes q_{current}^{-1}\) \(\alpha^{des} = K_p^{ori} \cdot 2 \cdot \text{sign}(e_{ori,w}) \cdot [e_{ori,x}, e_{ori,y}, e_{ori,z}]^T + K_d^{ori}(\omega^{ref} - \omega)\)
4. 动量任务 - 动态运动控制：
线动量控制： \(\dot{p}^{des} = m(\ddot{x}_{com}^{ref} + K_p^{lin}(x_{com}^{ref} - x_{com}) + K_d^{lin}(\dot{x}_{com}^{ref} - \dot{x}_{com}))\)
角动量控制： \(\dot{L}^{des} = K_p^{ang}(L^{ref} - L) + \dot{L}^{ref}\)
5. 关节空间任务 - 配置优化：
姿态优化（远离奇异性）： \(\ddot{q}_{posture}^{des} = K_p^{joint}(q_{nominal} - q) + K_d^{joint}(0 - \dot{q})\)
其中 $q_{nominal}$ 是期望的默认姿态，通常选择为：
任务优先级策略：
实际系统中任务常有明确优先级：
加权优先级（Weighted Priority）： 使用权重矩阵 $W_i$ 柔性处理冲突： \(\min \sum_i ||W_i(A_i\ddot{q} - b_i)||^2\)
标准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$ 个任务的加速度误差。
处理任务冲突的层级方法：
# 伪代码：层级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
为满足1kHz控制频率要求：
轮足机器人的接触可分为：
每种接触的力空间维度不同：
库仑摩擦锥的非线性约束：
\[\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\]给定期望的净力矩 $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}\]轮子的滚动约束要求切向力与驱动力矩一致：
\[f_{wheel,tangent} = \tau_{wheel} / r_{wheel}\]转向轮增加额外约束：
\[f_{lateral} \cos(\delta) - f_{longitudinal} \sin(\delta) = 0\]其中 $\delta$ 是转向角。
处理接触建立/断开的平滑过渡：
接触力profile：
Force
▲
│ ___稳定接触___
│ / \
│ / \
│ /建立 断开\
│ / \
──┴─────────────────────────► Time
t0 t1 t2 t3
使用sigmoid函数平滑过渡：
\[\gamma(t) = \frac{1}{1 + e^{-k(t - t_{switch})}}\]与静态稳定不同，动态行走允许ZMP暂时离开支撑多边形，利用角动量维持平衡：
\[\dot{L} = \sum_i (p_i - p_{com}) \times F_i + \tau_{external}\]捕获点(Capture Point)定义：
\[\xi = x_{com} + \frac{\dot{x}_{com}}{\omega}\]稳定条件：捕获点必须在可达支撑区域内。
轮足机器人的独特优势在于可以根据地形和任务需求切换运动模式：
模式切换状态机：
┌─────────┐
│纯轮模式 │◄────────┐
└────┬────┘ │
│ │
v > v_max 稳定性恢复
│ │
▼ │
┌─────────┐ │
│混合模式 │─────────┘
└────┬────┘
│
地形复杂度 > θ
│
▼
┌─────────┐
│纯足模式 │
└─────────┘
相位协调：使用中央模式发生器(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}}\]典型值：
全身角动量的主动调节用于动态平衡：
\[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\]约束于可行的接触力和关节限制。
受扰动后的平衡恢复策略层级：
决策阈值基于线性倒立摆的可行域：
\[|\xi - p_{support}| > d_{critical} \Rightarrow \text{需要步进}\]Atlas是波士顿动力开发的人形机器人，展示了卓越的动态运动能力，包括跑步、跳跃和后空翻。其全身控制系统是业界标杆。
Atlas采用三层控制架构：
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. 混合动力学建模
结合刚体动力学和简化模型：
3. 鲁棒接触检测
使用多传感器融合检测接触：
接触置信度估计：
\[p_{contact} = \sigma(w_f f_{measured} + w_a a_{residual} + w_v v_{predicted})\]Atlas在各种任务中的表现：
| 指标 | 数值 | 备注 |
|---|---|---|
| 最大步行速度 | 2.5 m/s | 平坦地形 |
| 跳跃高度 | 0.5 m | 垂直跳跃 |
| 平衡恢复 | 30 cm | 最大推力恢复距离 |
| 控制延迟 | < 5 ms | 传感器到执行器 |
| 能耗 | 500 W | 步行时平均功率 |
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定义了质心角动量与平均角速度的关系：
\[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求解。
本章系统介绍了轮足机械臂机器人的全身控制与平衡技术。核心要点包括：
| 概念 | 公式 | 用途 |
|---|---|---|
| ZMP位置 | $x_{zmp} = \frac{\sum m_i(x_i\ddot{z}_i - z_i\ddot{x}_i - gx_i)}{\sum m_i(\ddot{z}_i + g)}$ | 稳定性判断 |
| LIPM动力学 | $\ddot{x}{com} = \omega^2(x{com} - x_{zmp})$ | 轨迹规划 |
| 全身动力学 | $M\ddot{q} + h = S^T\tau + J_c^TF_c$ | QP约束 |
| 摩擦锥 | $||f_t|| \leq \mu f_n$ | 接触约束 |
| 捕获点 | $\xi = x_{com} + \dot{x}_{com}/\omega$ | 步进决策 |
题目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的挑战？