第4章：接触力学基础

接触是机器人与外部世界交互的根本方式。无论是人形机器人的行走、机械臂的抓取，还是四足机器人的奔跑，都离不开对接触力的精确建模与控制。本章深入探讨接触力学的核心理论，从库仑摩擦模型到互补性问题求解，再到接触稳定性分析，为后续的接触规划和全身控制奠定坚实的理论基础。我们将特别关注如何在计算效率和物理真实性之间取得平衡，这是实时机器人控制的关键挑战。

4.1 库仑摩擦锥与接触约束

4.1.1 点接触模型

在机器人学中，我们通常将接触简化为点接触模型。这种简化虽然忽略了接触面积和压力分布，但在大多数控制应用中提供了良好的精度-复杂度平衡。考虑机器人某个链接与环境表面的接触点，定义接触坐标系，其中z轴沿接触法向量指向机器人，x-y平面为切平面。

接触力 $\mathbf{f} = [f_x, f_y, f_z]^T$ 可分解为：

  • 法向力：$f_n = f_z$（垂直于接触面）
  • 切向力：$\mathbf{f}_t = [f_x, f_y]^T$（平行于接触面）

单边接触约束（Signorini条件）确保接触力只能是压力： $$f_n \geq 0$$ 这个简单的不等式约束是接触问题非凸性的根源，也是使接触动力学问题在数学上具有挑战性的核心原因。当 $f_n = 0$ 时，接触断开；当 $f_n > 0$ 时，接触激活。这种离散的开关行为导致了系统的混合动力学特性。

接触运动学约束： 当接触激活时，接触点的法向速度必须为零（假设刚性接触）： $$v_n = 0 \quad \text{if} \quad f_n > 0$$ 这与单边约束一起构成了完整的法向接触条件。

4.1.2 库仑摩擦锥

库仑摩擦定律是1785年由Charles-Augustin de Coulomb提出的经验定律，尽管其简单性掩盖了摩擦的微观复杂性，但它在工程应用中极其有效。该定律描述了切向力与法向力的关系，将可行接触力限制在一个锥形区域内。

静摩擦情况： 当接触点无相对滑动时，切向力必须满足： $$|\mathbf{f}_t| \leq \mu_s f_n$$ 其中 $\mu_s$ 是静摩擦系数，通常在0.3-1.5范围内（取决于材料对）。这定义了一个二阶锥（Second-Order Cone）： $$\mathcal{K} = \{\mathbf{f} \in \mathbb{R}^3 : \sqrt{f_x^2 + f_y^2} \leq \mu_s f_z, f_z \geq 0\}$$ 几何上，这是一个顶点在原点、半锥角为 $\theta = \arctan(\mu_s)$ 的圆锥：

      z (法向)
        ^
        |
        | f_n
    ____|____
   /    |    \
  /     |     \    摩擦锥
 /      |θ     \   tan(θ) = μ_s
/       |       \
--------|---------> y
        |       /
        |      /
        |     /
        |    /
        x (切向)

动摩擦情况： 当发生滑动时（$|\mathbf{v}_t| > 0$），切向力具有确定的大小和方向： $$\mathbf{f}_t = -\mu_d f_n \frac{\mathbf{v}_t}{|\mathbf{v}_t|}$$ 其中 $\mu_d$ 是动摩擦系数（通常 $\mu_d < \mu_s$），$\mathbf{v}_t$ 是接触点的相对切向速度。这个关系表明：

  • 摩擦力大小：$|\mathbf{f}_t| = \mu_d f_n$（与法向力成正比）
  • 摩擦力方向：与滑动速度相反（耗散性）

Stribeck效应： 实际摩擦在静-动转换时表现出复杂行为： $$\mu(|\mathbf{v}_t|) = \mu_d + (\mu_s - \mu_d) \exp(-|\mathbf{v}_t|/v_{stribeck})$$ 其中 $v_{stribeck}$ 是Stribeck速度，典型值约0.001-0.01 m/s。这种速度依赖性在精密控制中很重要，但在大多数机器人应用中可以忽略。

4.1.3 摩擦锥的线性化

二阶锥约束虽然精确，但在优化问题中处理较为复杂，需要专门的锥规划求解器。实践中，特别是在实时控制中，常用多面体锥近似将问题转化为线性规划或二次规划。

多面体近似原理： 将圆形摩擦锥用 $m$ 个半平面的交集近似，每个半平面对应一个摩擦锥的生成方向。将摩擦锥离散化为 $m$ 个均匀分布的方向： $$\mathbf{d}_i = \frac{1}{\sqrt{1 + \mu^2}}[\mu\cos(2\pi i/m), \mu\sin(2\pi i/m), 1]^T, \quad i = 1, ..., m$$ 这些向量是摩擦锥边缘的单位方向。接触力可表示为这些方向的非负线性组合： $$\mathbf{f} = \sum_{i=1}^m \lambda_i \mathbf{d}_i, \quad \lambda_i \geq 0$$ 近似精度分析：

  • $m = 4$（矩形锥）：最大相对误差约29%，但计算最快
  • $m = 8$（八边形锥）：最大相对误差约8%，良好的精度-速度平衡
  • $m = 16$：最大相对误差约2%，接近精确

近似误差定义为： $$\epsilon = \max_{\mathbf{f} \in \mathcal{K}_{poly}} \frac{d(\mathbf{f}, \mathcal{K})}{|\mathbf{f}|}$$ 其中 $\mathcal{K}_{poly}$ 是多面体锥，$d(\cdot, \cdot)$ 是点到集合的距离。

计算优势： 线性化后，接触力优化问题变为： $$\min_{\boldsymbol{\lambda}} f(\boldsymbol{\lambda}) \quad \text{s.t.} \quad \boldsymbol{\lambda} \geq \mathbf{0}$$ 这是标准的非负最小二乘问题，可用高效的QP求解器（如qpOASES、OSQP）在微秒级时间内求解。

4.1.4 接触力的可行域

当机器人与环境有多个接触点时（如四足机器人的四只脚、人形机器人的双脚、或多指抓取），需要考虑所有接触力的联合可行域。这个高维空间的结构对理解系统的力能力至关重要。

多接触力的笛卡尔积： 考虑 $n$ 个接触点，每个接触点 $i$ 有其摩擦锥 $\mathcal{K}_i$。整体接触力的可行域是各接触锥的笛卡尔积： $$\mathcal{F} = \mathcal{K}_1 \times \mathcal{K}_2 \times ... \times \mathcal{K}_n \subset \mathbb{R}^{3n}$$ 这意味着每个接触力独立地受其摩擦锥约束。

可行域的几何性质： 这个可行域具有以下重要性质：

  1. 非凸性：由于单边约束 $f_{ni} \geq 0$，当某个接触断开时（$f_{ni} = 0$），可行域发生不连续变化。这导致优化问题可能有多个局部最优。

  2. 多面体结构：在摩擦锥线性化后，$\mathcal{F}$ 成为多面体锥： $$\mathcal{F}_{poly} = \{\mathbf{f} : \mathbf{A}\mathbf{f} \leq \mathbf{0}, \mathbf{f} \geq \mathbf{0}\}$$ 其中 $\mathbf{A}$ 编码了所有摩擦锥约束。

  3. 维度爆炸：随接触点数量增长，可行域维度呈线性增长（$3n$ 维），但约束数量可能呈超线性增长（每个锥 $m$ 个约束）。

  4. 稀疏结构：约束矩阵 $\mathbf{A}$ 具有块对角结构，每个块对应一个接触点，这可被高效求解器利用。

扳手空间的可行域： 通过抓取矩阵 $\mathbf{G} \in \mathbb{R}^{6 \times 3n}$ 将接触力映射到机器人质心的扳手（力和力矩）： $$\mathbf{w} = \mathbf{G}\mathbf{f} = \sum_{i=1}^n \mathbf{G}_i \mathbf{f}_i$$ 扳手可行域： $$\mathcal{W} = \mathbf{G}(\mathcal{F}) = \{\mathbf{w} : \exists \mathbf{f} \in \mathcal{F}, \mathbf{w} = \mathbf{G}\mathbf{f}\}$$ 这是 $\mathcal{F}$ 在 $\mathbf{G}$ 下的像，通常是6维空间中的非凸集。

计算复杂度考虑：

  • 精确表示：需要枚举所有接触模式（$2^n$ 种）
  • 内近似：使用凸包络，保守但计算简单
  • 外近似：使用约束松弛，可能不可行但提供界限

4.2 互补性问题与接触求解

4.2.1 接触的互补性条件

接触的本质特征——非贯穿、单边性、和摩擦——可用互补性条件精确而优雅地描述。互补性理论源于数学规划，完美捕捉了接触的"非此即彼"特性：要么有间隙无力，要么有力无间隙。

法向互补性（Signorini条件，1933）： 对于每个潜在接触点，法向力 $f_n$ 和法向间隙 $\delta_n$ 满足： $$0 \leq f_n \perp \delta_n \geq 0$$ 其中 $\perp$ 表示互补性，即 $f_n \cdot \delta_n = 0$。这个简洁的数学表达包含三个约束：

  1. $f_n \geq 0$（单边性）
  2. $\delta_n \geq 0$（非贯穿）
  3. $f_n \cdot \delta_n = 0$（互补性）

物理含义的完整解释：

  • 若 $\delta_n > 0$（分离），则必有 $f_n = 0$（无接触力）
  • 若 $f_n > 0$（有接触力），则必有 $\delta_n = 0$（无间隙）
  • 边界情况：$f_n = 0$ 且 $\delta_n = 0$（刚好接触，临界状态）

切向互补性（摩擦条件）： 摩擦的互补性更为复杂，涉及力-速度关系。定义滑动速度 $\mathbf{v}_t$ 和最大静摩擦力 $f_{max} = \mu f_n$： $$ \begin{cases} |\mathbf{v}_t| = 0 \Rightarrow |\mathbf{f}_t| \leq f_{max} & \text{（粘滞/静摩擦）} \\ |\mathbf{v}_t| > 0 \Rightarrow \mathbf{f}_t = -f_{max} \frac{\mathbf{v}_t}{|\mathbf{v}_t|} & \text{（滑动/动摩擦）} \end{cases} $$ 这可以用最大耗散原理统一表述： $$\mathbf{f}_t \in \arg\min_{|\mathbf{f}'_t| \leq \mu f_n} \mathbf{v}_t^T \mathbf{f}'_t$$ 速度级互补性： 在速度级别（常用于仿真），互补性条件变为： $$0 \leq f_n \perp v_n + \epsilon \delta_n \geq 0$$ 其中 $\epsilon > 0$ 是稳定化参数（Baumgarte稳定化），防止约束漂移。

能量一致性： 互补性条件确保接触不产生能量： $$\mathbf{f}^T \mathbf{v} = f_n v_n + \mathbf{f}_t^T \mathbf{v}_t \leq 0$$ 等号仅在完全非弹性碰撞时成立。

4.2.2 线性互补性问题（LCP）

线性互补性问题（Linear Complementarity Problem）提供了求解接触动力学的数学框架。通过时间离散化和适当的假设，复杂的接触动力学可以转化为标准的LCP形式。

时间离散化： 考虑离散时间步长 $h$，从时刻 $t$ 到 $t+h$。使用半隐式Euler积分：

动力学方程（Newton-Euler方程的离散形式）： $$\mathbf{M}\mathbf{v}^{+} = \mathbf{M}\mathbf{v}^{-} + h\mathbf{f}_{ext} + \mathbf{J}^T\boldsymbol{\lambda}$$ 其中：

  • $\mathbf{M} \in \mathbb{R}^{n \times n}$：广义质量矩阵（包含惯量）
  • $\mathbf{v}^{-}, \mathbf{v}^{+} \in \mathbb{R}^n$：时间步开始和结束时的广义速度
  • $\mathbf{f}_{ext} \in \mathbb{R}^n$：外力（重力、关节力矩等）
  • $\mathbf{J} \in \mathbb{R}^{3m \times n}$：接触雅可比（$m$ 个接触点）
  • $\boldsymbol{\lambda} \in \mathbb{R}^{3m}$：接触冲量（$\boldsymbol{\lambda} = h\mathbf{f}_c$）

接触速度约束： 接触点的速度由广义速度通过雅可比映射得到： $$\mathbf{v}_c = \mathbf{J}\mathbf{v}^{+}$$ LCP推导： 将动力学方程解出 $\mathbf{v}^{+}$： $$\mathbf{v}^{+} = \mathbf{v}^{-} + h\mathbf{M}^{-1}\mathbf{f}_{ext} + \mathbf{M}^{-1}\mathbf{J}^T\boldsymbol{\lambda}$$ 代入接触速度约束： $$\mathbf{v}_c = \mathbf{J}(\mathbf{v}^{-} + h\mathbf{M}^{-1}\mathbf{f}_{ext}) + \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T\boldsymbol{\lambda}$$ 这给出标准LCP形式： $$\mathbf{v}_c = \mathbf{A}\boldsymbol{\lambda} + \mathbf{b}$$ $$0 \leq \boldsymbol{\lambda} \perp \mathbf{v}_c \geq 0$$ 其中：

  • $\mathbf{A} = \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T \in \mathbb{R}^{3m \times 3m}$：Delassus矩阵（有效质量矩阵）
  • $\mathbf{b} = \mathbf{J}(\mathbf{v}^{-} + h\mathbf{M}^{-1}\mathbf{f}_{ext})$：自由速度（无接触时的速度）

Delassus矩阵的性质：

  1. 对称性：若 $\mathbf{M}$ 对称，则 $\mathbf{A}$ 对称
  2. 半正定性：$\mathbf{A} \succeq 0$（由于 $\mathbf{M} \succ 0$）
  3. 稀疏性：继承自 $\mathbf{J}$ 的稀疏结构
  4. 物理意义：$A_{ij}$ 表示接触点 $j$ 的单位冲量对接触点 $i$ 速度的影响

存在性与唯一性：

  • 存在性：对于半正定 $\mathbf{A}$，LCP总有解
  • 唯一性：若 $\mathbf{A}$ 正定，解唯一；否则可能有多解（如静不定问题）

4.2.3 LCP求解算法

求解LCP是接触仿真的计算瓶颈。不同算法在精度、速度和鲁棒性之间有不同权衡。

Lemke算法（主元法）： Lemke算法是求解LCP的经典直接法，基于线性规划的单纯形法思想。

算法特点：

  • 理论上能找到精确解（若存在）
  • 时间复杂度 $O(n^3)$，其中 $n$ 是变量数
  • 对于协正矩阵（copositive）保证收敛
  • 对于某些病态矩阵可能循环或失败

实现要点：

  1. 引入人工变量将LCP转化为线性规划
  2. 通过主元操作维护基本可行解
  3. 当人工变量全部离开基时得到解

投影Gauss-Seidel（PGS）： PGS是接触仿真中最流行的迭代算法，因其简单性和鲁棒性。

迭代公式（分块形式）： $$\lambda_i^{k+1} = \text{proj}_{[0,\infty)}\left(\lambda_i^k - \omega \frac{1}{A_{ii}}\left(\sum_{ji} A_{ij}\lambda_j^k + b_i\right)\right)$$ 其中 $\omega \in (0, 2)$ 是超松弛因子。

收敛性分析：

  • 对于对称正定 $\mathbf{A}$：总是收敛
  • 收敛速度：$|\boldsymbol{\lambda}^{k+1} - \boldsymbol{\lambda}^*| \leq \rho^k |\boldsymbol{\lambda}^0 - \boldsymbol{\lambda}^*|$
  • 谱半径：$\rho < 1$，取决于 $\mathbf{A}$ 的条件数
  • 最优松弛因子：$\omega_{opt} = \frac{2}{1 + \sqrt{1 - \rho_{GS}^2}}$

实践优化：

  1. 预条件：使用对角缩放改善条件数
  2. 热启动：用上一时间步的解作为初值
  3. 自适应终止：监控残差 $|\mathbf{A}\boldsymbol{\lambda} + \mathbf{b} - \mathbf{v}_c|$

加速方法：

  1. 共轭梯度（CG）变体： - 将LCP重构为二次规划 - 使用修改的CG处理约束 - 收敛速度 $O(\sqrt{\kappa})$，其中 $\kappa$ 是条件数

  2. 多重网格方法： - 构造问题的层次表示 - 粗网格修正加速收敛 - 对大规模问题特别有效

  3. GPU并行化： - Jacobi迭代天然并行 - 彩色Gauss-Seidel保持部分串行特性 - 可达100倍加速（对大规模问题）

算法选择指南：

  • 小规模（<100接触）：Lemke算法
  • 中规模（100-1000）：PGS with预条件
  • 大规模（>1000）：多重网格或GPU方法
  • 实时要求：定迭代次数的PGS

4.2.4 非线性互补性问题（NCP）

当考虑库仑摩擦时，问题变为NCP。摩擦锥约束是非线性的：

最大耗散原理： 接触力应最小化功率耗散： $$\boldsymbol{\lambda} = \arg\min_{\boldsymbol{\lambda}' \in \mathcal{K}} \mathbf{v}_c^T \boldsymbol{\lambda}'$$ 锥互补性问题（CCP）： $$\mathcal{K} \ni \boldsymbol{\lambda} \perp \mathbf{v}_c \in \mathcal{K}^*$$ 其中 $\mathcal{K}^*$ 是对偶锥。

求解策略：

  1. 固定点迭代：交替更新法向力和切向力
  2. 锥投影方法：直接投影到摩擦锥
  3. 内点法：处理锥约束的障碍函数方法

4.2.5 时间积分方案

隐式Euler方法： 稳定但数值阻尼大： $$\mathbf{q}^{t+h} = \mathbf{q}^t + h\mathbf{v}^{t+h}$$ $$\mathbf{M}\mathbf{v}^{t+h} = \mathbf{M}\mathbf{v}^t + h(\mathbf{f}_{ext} + \mathbf{f}_c^{t+h})$$ 半隐式方法： 平衡稳定性和精度： $$\mathbf{v}^{t+h} = \mathbf{v}^t + h\mathbf{M}^{-1}(\mathbf{f}_{ext} + \mathbf{f}_c^{t+h})$$ $$\mathbf{q}^{t+h} = \mathbf{q}^t + h\mathbf{v}^{t+h}$$ 变分积分器： 保持能量和动量守恒： $$\delta \int_0^T L(\mathbf{q}, \dot{\mathbf{q}}) dt = 0$$

4.3 接触稳定性分析

4.3.1 力封闭与形封闭

力封闭（Force Closure）： 接触配置能够抵抗任意外力/力矩： $$\exists \boldsymbol{\lambda} \in \mathcal{K} : \mathbf{G}\boldsymbol{\lambda} = -\mathbf{w}_{ext}$$ 其中 $\mathbf{G}$ 是抓取矩阵，$\mathbf{w}_{ext}$ 是外部扳手。

检验条件： $$\text{rank}(\mathbf{G}) = 6 \text{ 且 } \mathbf{0} \in \text{int}(\text{ConvexHull}(\mathbf{G}\mathcal{K}))$$ 形封闭（Form Closure）： 更强的条件，纯几何约束阻止所有运动： $$\mathbf{G}\boldsymbol{\lambda} = \mathbf{0}, \boldsymbol{\lambda} \in \mathcal{K} \Rightarrow \boldsymbol{\lambda} = \mathbf{0}$$ 对于点接触，至少需要7个接触点才能实现形封闭。

4.3.2 接触刚度建模

实际接触并非刚性，存在局部变形。Hertz接触模型：

法向刚度： $$f_n = k_n \delta_n^{3/2}$$ 其中 $k_n$ 取决于材料属性和接触几何。

切向刚度（预滑动）： $$\mathbf{f}_t = \mathbf{K}_t \boldsymbol{\delta}_t$$ 刚度矩阵： $$\mathbf{K}_c = \begin{bmatrix} k_t & 0 & 0 \\ 0 & k_t & 0 \\ 0 & 0 & k_n \end{bmatrix}$$ 典型值：$k_n \sim 10^6$ N/m，$k_t \sim 0.5k_n$

4.3.3 接触阻尼

Hunt-Crossley模型包含非线性阻尼： $$f_n = k_n \delta_n^{3/2}(1 + \frac{3(1-e^2)}{4}\dot{\delta}_n)$$ 其中 $e$ 是恢复系数。

线性化阻尼模型： $$\mathbf{f}_c = -\mathbf{K}_c\boldsymbol{\delta} - \mathbf{D}_c\dot{\boldsymbol{\delta}}$$ 阻尼比：$\zeta = \frac{d}{2\sqrt{km}}$，典型取 $\zeta \in [0.1, 0.3]$。

4.3.4 Lyapunov稳定性分析

考虑接触系统的稳定性，定义Lyapunov函数：

机械能函数： $$V = \frac{1}{2}\mathbf{v}^T\mathbf{M}\mathbf{v} + U(\mathbf{q})$$ 其中 $U(\mathbf{q})$ 是势能（包括重力势和弹性势）。

耗散不等式： $$\dot{V} \leq -\alpha V - \beta |\mathbf{v}|^2$$ 其中 $\alpha, \beta > 0$ 由摩擦和阻尼决定。

接触Lyapunov函数： 包含接触惩罚项： $$V_c = V + \frac{1}{2}\sum_i k_i \delta_i^2$$ 稳定性条件：

  1. $V_c$ 正定
  2. $\dot{V}_c \leq 0$（耗散性）
  3. 不变集仅包含平衡点

4.3.5 鲁棒性分析

参数不确定性： 摩擦系数、接触位置、表面法向的不确定性： $$\mu \in [\mu_{min}, \mu_{max}]$$ $$\mathbf{p}_c = \mathbf{p}_{nom} + \Delta\mathbf{p}, |\Delta\mathbf{p}| \leq \epsilon$$ 鲁棒力封闭： 在所有不确定性下保持力封闭： $$\forall \mu \in [\mu_{min}, \mu_{max}] : \mathbf{0} \in \text{int}(\mathcal{W})$$ 稳定裕度： 定义最小抗扰动能力： $$\sigma = \min_{|\mathbf{w}|=1} \max_{\boldsymbol{\lambda} \in \mathcal{K}} \mathbf{w}^T\mathbf{G}\boldsymbol{\lambda}$$ $\sigma > 0$ 表示鲁棒力封闭。

4.4 案例研究：人形机器人脚掌接触建模

4.4.1 多点接触模型

人形机器人脚掌通常建模为矩形，四个角点接触：

    脚掌俯视图

    p1 -------- p2
    |           |
    |     C     |  C: 压力中心

    |     C     |  C: 压力中心
    |           |

    p3 -------- p4

    x: 前进方向
    y: 侧向

每个接触点 $i$ 的力：$\mathbf{f}_i = [f_{xi}, f_{yi}, f_{zi}]^T$

总接触扳手： $$\mathbf{w} = \sum_{i=1}^4 \begin{bmatrix} \mathbf{f}_i \\ \mathbf{p}_i \times \mathbf{f}_i \end{bmatrix}$$

4.4.2 压力中心（CoP）与零力矩点（ZMP）

压力中心： 所有接触力的等效作用点： $$\mathbf{p}_{CoP} = \frac{\sum_i f_{zi} \mathbf{p}_i}{\sum_i f_{zi}}$$ CoP必须在支撑多边形内： $$\mathbf{p}_{CoP} \in \text{ConvexHull}(\{\mathbf{p}_1, \mathbf{p}_2, \mathbf{p}_3, \mathbf{p}_4\})$$ 零力矩点： 地面反力的等效作用点，水平力矩为零： $$\mathbf{p}_{ZMP} = \frac{\mathbf{n} \times (\mathbf{M}_O + \mathbf{p}_O \times \mathbf{F})}{\mathbf{n} \cdot \mathbf{F}}$$ 其中 $\mathbf{n}$ 是地面法向量。

稳定性条件：$\mathbf{p}_{ZMP} \in$ 支撑多边形

4.4.3 接触力分配

给定期望的总扳手 $\mathbf{w}_{des}$，求解各接触点的力：

优化问题： $$ \begin{align} \min_{\mathbf{f}_1, ..., \mathbf{f}_4} & \quad \sum_{i=1}^4 |\mathbf{f}_i|^2 \\ \text{s.t.} & \quad \sum_{i=1}^4 \mathbf{G}_i \mathbf{f}_i = \mathbf{w}_{des} \\ & \quad \mathbf{f}_i \in \mathcal{K}_i, \quad i = 1, ..., 4 \end{align} $$ 其中 $\mathbf{G}_i$ 是第 $i$ 个接触点的抓取矩阵。

解析解（无摩擦情况）： 使用伪逆： $$\mathbf{f} = \mathbf{G}^+ \mathbf{w}_{des} + (\mathbf{I} - \mathbf{G}^+\mathbf{G})\mathbf{f}_0$$ 其中 $\mathbf{f}_0$ 是内力（不产生净扳手）。

4.4.4 接触切换策略

行走时的接触模式切换：

双支撑相：

  • 两脚都接触地面
  • 8个接触点（每脚4个）
  • 冗余约束，需要力分配

单支撑相：

  • 仅一脚接触
  • 4个接触点
  • 刚好约束（6自由度vs 12维接触力）

切换条件：

  1. 起脚条件：摆动脚法向力降至阈值 $$\sum_{i \in swing} f_{zi} < \epsilon_{\text{lift}}$$

  2. 落脚条件：摆动脚检测到接触 $$\delta_n < \epsilon_{\text{touch}} \text{ 且 } \dot{\delta}_n < 0$$ 平滑过渡： 使用时变权重： $$\mathbf{f} = \alpha(t)\mathbf{f}_{\text{double}} + (1-\alpha(t))\mathbf{f}_{\text{single}}$$ 其中 $\alpha(t)$ 是平滑过渡函数。

4.4.5 实时计算考虑

简化模型：

  • 将每脚简化为单点（脚踝处）
  • 使用线性化摩擦锥（4或8面体）
  • 忽略脚内力矩

快速求解器：

  • 利用问题结构的专用QP求解器
  • 预计算的查找表
  • 解析近似解

传感器融合：

  • 力传感器：直接测量接触力
  • IMU：估计基座方向
  • 编码器：运动学约束

典型控制频率：1kHz（全身控制），100Hz（规划层）

本章小结

本章系统地介绍了接触力学的核心概念和计算方法：

关键概念：

  1. 库仑摩擦锥：$|\mathbf{f}_t| \leq \mu f_n$ 定义了接触力的可行域
  2. 互补性条件：$0 \leq f_n \perp \delta_n \geq 0$ 精确描述了接触的开关特性
  3. LCP/NCP：将接触动力学转化为数学规划问题
  4. 力封闭：评估接触配置的稳定性
  5. ZMP/CoP：人形机器人平衡控制的关键指标

核心公式：

  • Delassus矩阵：$\mathbf{A} = \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^T$
  • LCP标准形式：$\mathbf{v}_c = \mathbf{A}\boldsymbol{\lambda} + \mathbf{b}$，$0 \leq \boldsymbol{\lambda} \perp \mathbf{v}_c \geq 0$
  • 摩擦锥线性化：$\mathbf{f} = \sum_{i=1}^m \lambda_i \mathbf{d}_i$，$\lambda_i \geq 0$
  • 压力中心：$\mathbf{p}_{CoP} = \frac{\sum_i f_{zi} \mathbf{p}_i}{\sum_i f_{zi}}$

计算方法：

  • Lemke算法：精确但计算量大
  • PGS迭代：简单高效，适合实时控制
  • 锥投影：处理摩擦约束

接触力学是机器人控制的基石，本章的理论将在后续的接触规划、MPC和全身控制中反复应用。

练习题

基础题

习题4.1 考虑一个质量为 $m = 10$ kg的箱子放在水平面上，静摩擦系数 $\mu_s = 0.5$。如果施加水平力 $F = 40$ N，箱子是否会滑动？计算接触力。

提示：比较施加力与最大静摩擦力。

答案

最大静摩擦力：$f_{max} = \mu_s \cdot mg = 0.5 \times 10 \times 9.8 = 49$ N

由于 $F = 40$ N $< f_{max} = 49$ N，箱子不会滑动。

接触力：

  • 法向力：$f_n = mg = 98$ N
  • 切向力：$f_t = F = 40$ N（静摩擦力平衡外力）

习题4.2 给定4个接触点的摩擦锥，摩擦系数 $\mu = 0.7$。使用4面体近似，写出每个基向量 $\mathbf{d}_i$。

提示：考虑东南西北四个方向。

答案

归一化因子：$k = 1/\sqrt{1 + \mu^2} = 1/\sqrt{1.49} \approx 0.819$

四个基向量：

  • $\mathbf{d}_1 = k[0.7, 0, 1]^T$ （+x方向）
  • $\mathbf{d}_2 = k[0, 0.7, 1]^T$ （+y方向）
  • $\mathbf{d}_3 = k[-0.7, 0, 1]^T$ （-x方向）
  • $\mathbf{d}_4 = k[0, -0.7, 1]^T$ （-y方向）

习题4.3 一个机器人手指与物体接触，接触点位于 $\mathbf{p} = [1, 0, 0]^T$ m，接触力 $\mathbf{f} = [0, 0, 10]^T$ N。计算该接触力产生的扳手。

提示：扳手包含力和力矩。

答案

扳手 $\mathbf{w} = \begin{bmatrix} \mathbf{f} \\ \mathbf{p} \times \mathbf{f} \end{bmatrix}$

力矩：$\mathbf{m} = \mathbf{p} \times \mathbf{f} = [1, 0, 0]^T \times [0, 0, 10]^T = [0, -10, 0]^T$ N·m

扳手：$\mathbf{w} = [0, 0, 10, 0, -10, 0]^T$

挑战题

习题4.4 证明对于平面中的刚体，3个不共线的点接触（无摩擦）可以实现力封闭。需要多少个点才能实现形封闭？

提示：考虑平面刚体的自由度和约束数量。

答案

平面刚体有3个自由度（x, y, θ）。

力封闭： 3个不共线点提供3个独立的法向力，其列空间张成整个 $\mathbb{R}^3$（力和力矩空间）。因此可以平衡任意外部扳手，实现力封闭。

形封闭： 形封闭要求纯几何约束完全限制运动。对于无摩擦点接触，每个接触提供1个约束（法向）。需要至少4个点接触才能实现形封闭（过约束系统）。

证明：3个点最多限制2个平动自由度和1个转动自由度，但存在瞬时运动中心使得物体可以绕该点转动而不违反约束。第4个点可以消除这种可能性。

习题4.5 推导PGS算法的收敛条件。证明当Delassus矩阵 $\mathbf{A}$ 是对称正定时，算法必然收敛。

提示：将PGS视为不动点迭代，分析谱半径。

答案

PGS迭代：$\boldsymbol{\lambda}^{k+1} = \text{proj}_{\mathbb{R}^+}(\boldsymbol{\lambda}^k - \omega \mathbf{D}^{-1}(\mathbf{A}\boldsymbol{\lambda}^k + \mathbf{b}))$

其中 $\mathbf{D} = \text{diag}(\mathbf{A})$。

将其写为不动点迭代：$\boldsymbol{\lambda}^{k+1} = T(\boldsymbol{\lambda}^k)$

对于对称正定 $\mathbf{A}$：

  1. 分解 $\mathbf{A} = \mathbf{L} + \mathbf{D} + \mathbf{L}^T$
  2. 迭代矩阵：$\mathbf{G} = (\mathbf{D} + \omega \mathbf{L})^{-1}((1-\omega)\mathbf{D} - \omega\mathbf{L}^T)$
  3. 谱半径：$\rho(\mathbf{G}) < 1$ 当 $\omega \in (0, 2)$

因此算法收敛。最优松弛因子 $\omega_{opt} \approx 1.2-1.5$。

习题4.6 设计一个算法，给定人形机器人质心位置和期望的地面反力，计算每个脚掌四个角点的接触力分配。考虑摩擦锥约束和ZMP稳定性约束。

提示：构造二次规划问题。

答案

决策变量：8个接触力 $\mathbf{f}_i \in \mathbb{R}^3$，$i = 1, ..., 8$

目标函数（最小化接触力）： $$\min \sum_{i=1}^8 |\mathbf{f}_i|^2$$ 约束：

  1. 力平衡：$\sum_{i=1}^8 \mathbf{f}_i = \mathbf{F}_{des}$
  2. 力矩平衡：$\sum_{i=1}^8 \mathbf{p}_i \times \mathbf{f}_i = \mathbf{M}_{des}$
  3. 摩擦锥：$\sqrt{f_{xi}^2 + f_{yi}^2} \leq \mu f_{zi}$，$f_{zi} \geq 0$
  4. ZMP约束：$\mathbf{p}_{ZMP} \in$ 支撑多边形

线性化摩擦锥后成为QP问题，可用商业求解器（如qpOASES）实时求解。

习题4.7（开放题）讨论软接触模型（如Hertz模型）与刚性接触模型在机器人控制中的优缺点。什么情况下必须使用软接触模型？

提示：考虑计算效率、物理真实性、数值稳定性。

答案

刚性接触模型：

  • 优点：计算高效，适合实时控制；数学框架成熟（LCP/NCP）
  • 缺点：可能导致数值不稳定；忽略接触变形；碰撞时能量不守恒

软接触模型：

  • 优点：物理真实；数值稳定；自然处理碰撞
  • 缺点：引入高频动态；需要小时间步长；计算量大

必须使用软接触的情况：

  1. 精密操作（如装配）
  2. 软体机器人
  3. 人机交互（安全性）
  4. 振动分析
  5. 碰撞仿真

实践中常采用混合策略：规划层用刚性模型，控制层用软接触模型。

常见陷阱与错误

1. 摩擦锥线性化误差

陷阱：过度简化的摩擦锥（如4面体）可能导致不可行解。

示例：

# 错误：4面体可能过于保守
friction_cone_4 = [[1,0,μ], [0,1,μ], [-1,0,μ], [0,-1,μ]]

# 正确：使用8面体或更多
friction_cone_8 = []
for i in range(8):
    angle = 2*π*i/8
    friction_cone_8.append([μ*cos(angle), μ*sin(angle), 1])

解决：根据精度需求选择合适的离散化程度。

2. 互补性条件的数值容差

陷阱：严格的互补性 $f_n \cdot \delta_n = 0$ 在数值上难以满足。

正确做法： $$f_n \cdot \delta_n \leq \epsilon_{comp}$$

其中 $\epsilon_{comp} \sim 10^{-6}$ 是容差。

3. LCP求解器不收敛

常见原因：

  • Delassus矩阵病态
  • 初始猜测太差
  • 时间步长太大

调试技巧：

  1. 检查矩阵条件数
  2. 使用上一时刻解作为初始值
  3. 减小时间步长
  4. 添加正则化项：$\mathbf{A}_{reg} = \mathbf{A} + \epsilon \mathbf{I}$

4. 接触检测遗漏

陷阱：仅在离散时刻检测接触可能遗漏快速碰撞。

解决：连续碰撞检测（CCD）或自适应时间步长。

5. 接触力分配的冗余性

问题：多接触时存在无穷多解。

错误做法：

# 可能产生巨大内力
f = pinv(G) @ w_des  # 伪逆解

正确做法： 添加正则化或物理意义的目标函数（如最小化接触力）。

6. 忽略接触切换的瞬态

陷阱：接触模式突变导致控制不连续。

解决：

  • 使用平滑过渡函数
  • 预测接触切换
  • 鲁棒控制设计

7. 实时性与精度的权衡

误区：追求高精度物理仿真而牺牲实时性。

实践经验：

  • 1kHz控制：简化模型 + 快速近似解
  • 100Hz规划：完整模型 + 精确解
  • 离线优化：高保真模型

记住：在机器人控制中，一个及时的近似解往往胜过延迟的精确解。