第6章：多接触规划

多接触规划是机器人控制领域的核心挑战之一，特别是对于人形机器人、四足机器人和攀爬机器人等需要与环境进行复杂物理交互的系统。本章将深入探讨如何规划和协调多个接触点的建立、维持和切换，以实现复杂的运动任务。我们将学习接触图的构建、混合动力学系统的处理，以及如何在存在不确定性的情况下保证规划的鲁棒性。

6.1 多接触运动生成

多接触运动生成的核心在于同时考虑运动学、动力学和接触约束，生成物理可行且任务最优的运动轨迹。与单一接触点的简单情况不同，多接触规划必须处理离散的接触模式选择和连续的运动轨迹优化，这种混合特性使得问题极具挑战性。

多接触运动生成的应用场景广泛：人形机器人在崎岖地形上行走时需要手脚并用以保持平衡；四足机器人攀爬陡坡时需要精确控制每个脚掌的接触力分配；操作机器人在装配任务中需要同时用双手协调操作并保持与工件的稳定接触。这些场景都要求系统能够智能地选择接触点、规划接触序列，并生成满足物理约束的运动轨迹。

6.1.1 接触图与运动原语

接触图（Contact Graph）是描述机器人与环境接触模式及其转换关系的有向图结构。它不仅编码了可能的接触配置，还定义了配置间的可行转换路径：

     站立(双脚)
      /     \
    单脚站立  移动右脚
      |        |
    迈步     双脚新位置

定义 6.1（接触模式）: 接触模式 $\sigma \in \Sigma$ 定义为活跃接触点的集合，其中 $\Sigma$ 为所有可能接触模式的集合。每个接触模式对应一组特定的运动学和动力学约束。

接触图的数学表示： 接触图可形式化为 $\mathcal{G} = (\mathcal{V}, \mathcal{E}, \mathcal{W})$，其中：

• $\mathcal{V} = \{\sigma_1, \sigma_2, ..., \sigma_n\}$ 为节点集合，每个节点代表一个接触模式
• $\mathcal{E} \subseteq \mathcal{V} \times \mathcal{V}$ 为边集合，表示可行的模式转换
• $\mathcal{W}: \mathcal{E} \rightarrow \mathbb{R}^+$ 为边权重函数，量化转换代价

转换代价通常考虑多个因素： $$w_{ij} = \alpha_1 \cdot T_{switch}(i,j) + \alpha_2 \cdot E_{impact}(i,j) + \alpha_3 \cdot R_{stability}(i,j)$$ 其中 $T_{switch}$ 为切换时间，$E_{impact}$ 为碰撞能量损失，$R_{stability}$ 为稳定性风险。

对于每个接触模式 $\sigma$，系统动力学可表示为： $$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) + \mathbf{g}(\mathbf{q}) = \mathbf{S}^T\boldsymbol{\tau} + \sum_{i \in \sigma} \mathbf{J}_i^T(\mathbf{q}) \mathbf{f}_i$$ 其中：

• $\mathbf{M}(\mathbf{q})$ 为质量矩阵
• $\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})$ 为科氏力和离心力项
• $\mathbf{g}(\mathbf{q})$ 为重力项
• $\mathbf{S}$ 为驱动选择矩阵
• $\mathbf{J}_i$ 为第 $i$ 个接触点的雅可比矩阵
• $\mathbf{f}_i$ 为第 $i$ 个接触点的接触力

运动原语（Motion Primitives） 是连接不同接触模式的参数化运动模板。它们作为构建复杂运动的基本单元，能够大幅简化规划问题的复杂度。常见的运动原语包括：

1. 接触建立原语（Contact Making）：从非接触到接触的过渡 - 接近阶段：末端执行器向目标表面运动 - 接触检测：通过力传感器或触觉反馈确认接触 - 力建立：逐渐增加接触力至期望值 - 数学描述：$\mathbf{f}_c(t) = \rho(t) \cdot \mathbf{f}_{desired}$，其中 $\rho(t) \in [0,1]$ 为渐进函数

2. 接触释放原语（Contact Breaking）：从接触到非接触的过渡 - 力卸载：逐渐减小接触力 - 分离运动：确保干净的分离轨迹 - 稳定性维持：调整其他接触点补偿失去的支撑 - 约束条件：$\sum_{i \in \sigma \setminus \{k\}} \mathbf{f}_i = \sum_{i \in \sigma} \mathbf{f}_i$（力平衡转移）

3. 接触滑动原语（Contact Sliding）：保持接触的同时改变接触位置 - 切向力控制：$|\mathbf{f}_t| < \mu \cdot f_n$（避免滑脱） - 速度约束：$|\mathbf{v}_{slide}| \leq v_{max}$ - 表面跟踪：使用法向力控制维持接触 - 应用场景：机器人手指在物体表面的探索运动

4. 接触转换原语（Contact Switching）：同时建立新接触和释放旧接触 - 重叠阶段：新旧接触共存 - 负载转移：平滑的力重分配 - 时序协调：确保稳定性不被破坏 - 优化目标：$\min \int_0^T |\dot{\mathbf{f}}|^2 dt$（最小化力变化率）

运动原语的参数化表示： 每个运动原语 $\pi$ 可以参数化为： $$\pi = (\mathbf{p}_{start}, \mathbf{p}_{end}, T_{duration}, \boldsymbol{\theta})$$ 其中：

• $\mathbf{p}_{start}, \mathbf{p}_{end}$ 为起始和目标配置
• $T_{duration}$ 为执行时间
• $\boldsymbol{\theta}$ 为原语特定参数（如接触点位置、力分布等）

原语组合与序列化： 复杂运动通过原语序列构建： $$\Pi = \{\pi_1, \pi_2, ..., \pi_n\}$$ 序列的可行性需满足：

• 连续性约束：$\mathbf{p}_{end}^{(i)} = \mathbf{p}_{start}^{(i+1)}$
• 动力学一致性：状态转换满足系统动力学
• 稳定性保证：每个原语执行期间保持平衡

6.1.2 时空优化框架

多接触运动生成可以表述为一个混合整数非线性规划（MINLP）问题。这个框架需要同时优化离散决策（接触模式选择）和连续变量（轨迹和时间），是计算上极具挑战性的问题类别： $$\begin{align} \min_{\mathbf{x}, \mathbf{u}, \sigma, T} \quad & \int_0^T L(\mathbf{x}(t), \mathbf{u}(t)) dt + \Phi(\mathbf{x}(T)) \\ \text{s.t.} \quad & \dot{\mathbf{x}}(t) = f_{\sigma(t)}(\mathbf{x}(t), \mathbf{u}(t)) \\ & g_{\sigma(t)}(\mathbf{x}(t), \mathbf{u}(t)) \leq 0 \\ & h_{trans}(\mathbf{x}(t^-_k), \mathbf{x}(t^+_k), \sigma_k, \sigma_{k+1}) = 0 \\ & \mathbf{x}(0) = \mathbf{x}_0, \quad \mathbf{x}(T) \in \mathcal{X}_f \end{align}$$ 其中：

• $L$ 为阶段成本函数（通常包括能量消耗、运动平滑性、任务进度等）
• $\Phi$ 为终端成本（确保达到目标状态）
• $f_{\sigma}$ 为模式 $\sigma$ 下的动力学（包含接触约束的影响）
• $g_{\sigma}$ 为模式相关的不等式约束（如接触力约束、关节限位、避障等）
• $h_{trans}$ 为模式切换时的转换约束（如碰撞定律、接触一致性）
• $t_k$ 为第 $k$ 次模式切换时刻

成本函数的设计： 实践中，成本函数通常采用多目标加权形式： $$L(\mathbf{x}, \mathbf{u}) = w_1 L_{energy}(\mathbf{u}) + w_2 L_{smooth}(\mathbf{x}, \mathbf{u}) + w_3 L_{task}(\mathbf{x}) + w_4 L_{stability}(\mathbf{x})$$ 其中：

• $L_{energy} = \boldsymbol{\tau}^T \mathbf{R} \boldsymbol{\tau}$：能量消耗
• $L_{smooth} = |\ddot{\mathbf{q}}|^2 + |\dot{\boldsymbol{\tau}}|^2$：运动平滑性
• $L_{task} = |\mathbf{x} - \mathbf{x}_{ref}|^2_{\mathbf{Q}}$：任务跟踪误差
• $L_{stability} = \exp(-d_{margin})$：稳定性裕度惩罚

时间优化：接触切换时刻 $\{t_k\}$ 的优化是关键挑战。采用时间缩放技术，引入归一化时间 $s \in [0,1]$： $$t = \sum_{k=1}^{N_p} \Delta t_k \cdot \mathbb{1}_{s \in [(k-1)/N_p, k/N_p]}$$ 这允许将可变时长问题转换为固定时域优化，其中 $\Delta t_k$ 成为优化变量。

分层求解策略： 由于MINLP问题的复杂性，通常采用分层策略：

1. 高层：接触序列规划 - 使用图搜索或采样方法确定接触模式序列 - 简化动力学模型（如质心动力学） - 输出：$\{\sigma_1, \sigma_2, ..., \sigma_n\}$

2. 中层：时间分配 - 给定接触序列，优化各阶段时长 - 考虑动力学可行性和稳定性约束 - 输出：$\{t_1, t_2, ..., t_n\}$

3. 底层：轨迹优化 - 固定接触序列和时间，优化连续轨迹 - 完整动力学模型 - 输出：$\{\mathbf{q}(t), \dot{\mathbf{q}}(t), \boldsymbol{\tau}(t)\}$

凸化技术： 为提高求解效率，常用凸化技术包括：

• 摩擦锥的多面体近似
• 动力学的线性化
• 信赖域方法的迭代凸化

6.1.3 接触力分配

在给定接触模式下，接触力分配是实现期望运动的关键。这个问题需要在满足物理约束的前提下，找到最优的力分配策略。接触力分配问题可表述为二次规划（QP）： $$\begin{align} \min_{\mathbf{f}, \boldsymbol{\tau}} \quad & |\mathbf{f}|^2_{\mathbf{W}_f} + |\boldsymbol{\tau}|^2_{\mathbf{W}_\tau} \\ \text{s.t.} \quad & \mathbf{A}_{eq}\begin{bmatrix}\mathbf{f} \\ \boldsymbol{\tau}\end{bmatrix} = \mathbf{b}_{eq} \quad \text{(动力学约束)} \\ & \mathbf{f}_i \in \mathcal{FC}_i \quad \forall i \in \sigma \quad \text{(摩擦锥约束)} \\ & \mathbf{f}_{n,i} \geq 0 \quad \text{(单向约束)} \\ & \boldsymbol{\tau}_{min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{max} \quad \text{(力矩限制)} \end{align}$$ 动力学约束的构建： 等式约束矩阵 $\mathbf{A}_{eq}$ 和向量 $\mathbf{b}_{eq}$ 来自于质心动力学和角动量方程： $$\begin{bmatrix} m\mathbf{I}_3 & \mathbf{0} \\ [\mathbf{r}_1]_\times & [\mathbf{r}_2]_\times & \cdots \end{bmatrix} \begin{bmatrix} \mathbf{f}_1 \\ \mathbf{f}_2 \\ \vdots \end{bmatrix} = \begin{bmatrix} m(\ddot{\mathbf{r}}_{CoM} - \mathbf{g}) \\ \dot{\mathbf{L}}_{desired} \end{bmatrix}$$ 其中 $[\mathbf{r}_i]_\times$ 是位置向量 $\mathbf{r}_i = \mathbf{p}_i - \mathbf{r}_{CoM}$ 的反对称矩阵。

权重矩阵的选择： 权重矩阵 $\mathbf{W}_f$ 和 $\mathbf{W}_\tau$ 反映了不同优化目标的相对重要性：

• 最小化接触力：$\mathbf{W}_f = \mathbf{I}$（减少接触面磨损）
• 均匀力分配：$\mathbf{W}_f = diag(1/f_{max,i}^2)$（避免单点过载）
• 最小化关节力矩：$\mathbf{W}_\tau = diag(1/\tau_{max,i}^2)$（降低能耗）

摩擦锥线性化：为了保持问题的凸性，通常将摩擦锥约束线性化为多面体： $$\mathcal{FC}_i^{lin} = \{\mathbf{f}_i | \mathbf{V}_i \mathbf{f}_i \leq \mathbf{0}\}$$ 其中 $\mathbf{V}_i$ 定义了摩擦锥的线性化facets。对于 $n$ 面体近似： $$\mathbf{V}_i = \begin{bmatrix} \cos(\theta_1) & \sin(\theta_1) & -\mu \\ \cos(\theta_2) & \sin(\theta_2) & -\mu \\ \vdots & \vdots & \vdots \\ \cos(\theta_n) & \sin(\theta_n) & -\mu \end{bmatrix}$$ 其中 $\theta_j = 2\pi j/n$ 为圆周均匀采样角度。

接触力分配的实时性考虑： 为保证实时性能（通常要求 < 1ms 求解时间），采用以下优化策略：

1. 热启动（Warm Start）：使用前一时刻的解作为初始猜测 $$\mathbf{f}_{init}^{(k+1)} = \mathbf{f}_{solution}^{(k)}$$

2. 主动集方法（Active Set Method）：跟踪活跃约束集合，减少计算量

3. 分解技术：将大规模问题分解为多个小规模子问题 - 按接触点分组 - 利用稀疏结构 - 并行求解

接触力的可行域分析： 给定接触配置，可行接触力集合形成一个凸多面体： $$\mathcal{F}_{feasible} = \{\mathbf{f} | \mathbf{A}_{eq}\mathbf{f} = \mathbf{b}_{eq}, \mathbf{A}_{ineq}\mathbf{f} \leq \mathbf{b}_{ineq}\}$$ 可行域的体积反映了系统的操作能力： $$V(\mathcal{F}_{feasible}) = \int_{\mathcal{F}} d\mathbf{f}$$ 体积越大，系统抵抗扰动的能力越强。

6.2 接触切换与模式调度

接触切换与模式调度是多接触规划的核心难题。系统需要在保证稳定性和连续性的前提下，实现不同接触模式间的平滑过渡。这涉及到混合系统理论、优化调度算法，以及实际执行中的鲁棒控制策略。

6.2.1 混合动力学系统

多接触系统本质上是一个混合动力学系统，包含连续状态演化和离散模式切换。理解这种混合特性对于设计有效的控制策略至关重要：

定义 6.2（混合系统）: 多接触混合系统 $\mathcal{H}$ 定义为元组： $$\mathcal{H} = (\Sigma, \mathcal{X}, \mathcal{U}, f, \mathcal{I}, \mathcal{G}, \mathcal{R})$$ 其中：

• $\Sigma = \{\sigma_1, \sigma_2, ..., \sigma_m\}$：离散模式集合（接触配置）
• $\mathcal{X} \subseteq \mathbb{R}^n$：连续状态空间（位置、速度、力等）
• $\mathcal{U} \subseteq \mathbb{R}^p$：控制输入空间
• $f: \Sigma \times \mathcal{X} \times \mathcal{U} \rightarrow T\mathcal{X}$：向量场（模式相关的动力学）
• $\mathcal{I}: \Sigma \rightarrow 2^{\mathcal{X}}$：不变集（模式的有效状态域）
• $\mathcal{G}: \Sigma \times \Sigma \rightarrow 2^{\mathcal{X}}$：切换保护条件
• $\mathcal{R}: \Sigma \times \Sigma \times \mathcal{X} \rightarrow \mathcal{X}$：重置映射（状态跳变）

混合执行轨迹： 系统的混合执行定义为序列： $$\chi = (\tau, \sigma, \mathbf{x})$$ 其中：

• $\tau = \{[t_0, t_1], [t_1, t_2], ...\}$ 为时间区间序列
• $\sigma = \{\sigma_0, \sigma_1, ...\}$ 为模式序列
• $\mathbf{x} = \{\mathbf{x}_0(\cdot), \mathbf{x}_1(\cdot), ...\}$ 为状态轨迹序列

在每个区间 $[t_i, t_{i+1}]$ 内，系统遵循模式 $\sigma_i$ 的连续动力学： $$\dot{\mathbf{x}}(t) = f(\sigma_i, \mathbf{x}(t), \mathbf{u}(t)), \quad t \in [t_i, t_{i+1})$$ 切换条件：从模式 $\sigma_i$ 到 $\sigma_j$ 的切换发生当且仅当：

1. 保护条件满足：$\mathbf{x}(t_i^-) \in \mathcal{G}(\sigma_i, \sigma_j)$
2. 重置映射执行：$\mathbf{x}(t_i^+) = \mathcal{R}(\sigma_i, \sigma_j, \mathbf{x}(t_i^-))$
3. 不变集满足：$\mathbf{x}(t_i^+) \in \mathcal{I}(\sigma_j)$

接触模式的特殊性： 对于接触系统，重置映射通常涉及碰撞动力学： $$\mathbf{M}(\mathbf{v}^+ - \mathbf{v}^-) = \mathbf{J}_c^T \boldsymbol{\lambda}$$ 其中 $\boldsymbol{\lambda}$ 为碰撞冲量，需满足：

• 恢复系数约束：$v_n^+ = -e \cdot v_n^-$，$e \in [0,1]$
• 摩擦约束：$|\boldsymbol{\lambda}_t| \leq \mu |\boldsymbol{\lambda}_n|$

Zeno现象的避免： Zeno现象是指在有限时间内发生无限次切换，这在实际系统中必须避免： $$\exists \epsilon > 0: t_{i+1} - t_i \geq \epsilon \quad \forall i$$ 通过引入最小驻留时间约束或设计适当的保护集可以避免Zeno行为。

6.2.2 切换时机优化

最优切换时机的确定是一个复杂的决策问题，需要平衡多个相互冲突的目标。切换过早可能导致不必要的能量损失，切换过晚则可能错过最佳时机甚至导致失稳。

稳定性裕度评估： 稳定性是决定切换时机的首要考虑因素。常用的稳定性指标包括：

1. 零矩点（ZMP）稳定性裕度： $$m_{ZMP}(t) = \min_{edge} d(\mathbf{p}_{ZMP}(t), edge)$$ 其中 $\mathbf{p}_{ZMP}$ 为零矩点位置，$edge$ 为支撑多边形边界。

2. 捕获点（Capture Point）稳定性： $$m_{CP}(t) = |\mathbf{p}_{CP}(t) - \mathbf{p}_{support}|$$ 捕获点 $\mathbf{p}_{CP} = \mathbf{r}_{CoM} + \dot{\mathbf{r}}_{CoM}/\omega$ 反映了动态平衡能力，其中 $\omega = \sqrt{g/z_{CoM}}$。

3. 可行性裕度（Viability Margin）： $$m_{viability}(t) = \max_{\mathbf{u} \in \mathcal{U}} V(\mathbf{x}(t), \mathbf{u})$$ 其中 $V$ 为可行性核函数，表示系统维持稳定的能力。

切换代价函数： 综合考虑多个因素的切换代价： $$J_{switch}(t_k) = \alpha \cdot E_{impact}(t_k) + \beta \cdot \Delta_{stability}(t_k) + \gamma \cdot T_{exec}(t_k) + \delta \cdot R_{failure}(t_k)$$ 其中：

• $E_{impact} = \frac{1}{2}(\mathbf{v}^+ - \mathbf{v}^-)^T\mathbf{M}(\mathbf{v}^+ - \mathbf{v}^-)$：接触碰撞能量损失
• $\Delta_{stability} = m_{stability}(t_k^-) - m_{stability}(t_k^+)$：稳定性变化
• $T_{exec} = t_k - t_{current}$：执行时间
• $R_{failure} = P(failure|t_k) \cdot C_{failure}$：失败风险成本

时间窗口优化： 切换时间必须在可行窗口内： $$t_k \in [t_k^{min}, t_k^{max}]$$ 窗口边界由多个约束决定：

• 动力学约束：$t_k^{min} = t_{current} + T_{preparation}$
• 稳定性约束：$t_k^{max} = \arg\max_t\{t | m_{stability}(t) \geq m_{threshold}\}$
• 任务约束：$t_k \leq t_{deadline}$

双层优化框架： 切换时机优化可以表述为双层优化问题：

上层：切换时刻优化 $$\min_{t_1, ..., t_n} J_{total} = \sum_{k=1}^n J_{switch}(t_k) + J_{trajectory}(\mathbf{x}(\cdot))$$ 下层：给定切换时刻的轨迹优化 $$\min_{\mathbf{u}(\cdot)} \int_0^T L(\mathbf{x}(t), \mathbf{u}(t))dt \quad \text{s.t. 动力学和约束}$$ 在线决策策略： 实时系统采用基于事件的切换策略：

if (m_stability < m_critical) then
    立即切换到安全模式
else if (J_switch(t_now) < J_switch(t_now + dt)) then
    执行切换
else
    继续当前模式
end if

6.2.3 平滑过渡策略

接触切换时的不连续性是控制实现的主要挑战。常用的平滑策略包括：

1. 虚拟弹簧阻尼模型： 在接触建立阶段，使用虚拟弹簧阻尼器： $$\mathbf{f}_c = \mathbf{K}_p \Delta \mathbf{x} + \mathbf{K}_d \Delta \dot{\mathbf{x}}$$

2. 力控制过渡： 采用S型曲线进行力过渡： $$\mathbf{f}(t) = \mathbf{f}_{init} + (\mathbf{f}_{final} - \mathbf{f}_{init}) \cdot s(t)$$ 其中 $s(t)$ 为平滑过渡函数： $$s(t) = \begin{cases} 0 & t < t_0 \\ \frac{1}{2}[1 + \sin(\pi \frac{t-t_0}{\Delta t} - \frac{\pi}{2})] & t_0 \leq t \leq t_0 + \Delta t \\ 1 & t > t_0 + \Delta t \end{cases}$$

3. 阻抗控制框架： $$\mathbf{M}_d(\ddot{\mathbf{x}} - \ddot{\mathbf{x}}_d) + \mathbf{D}_d(\dot{\mathbf{x}} - \dot{\mathbf{x}}_d) + \mathbf{K}_d(\mathbf{x} - \mathbf{x}_d) = \mathbf{f}_{ext}$$

6.3 鲁棒性与不确定性处理

实际系统中的多接触规划必须处理各种不确定性来源，包括模型误差、感知噪声、环境变化和执行偏差。

6.3.1 接触不确定性建模

不确定性来源分类：

1. 几何不确定性：接触表面位置和法向量的误差 $$\mathbf{p}_{contact} = \mathbf{p}_{nominal} + \Delta \mathbf{p}, \quad |\Delta \mathbf{p}| \leq \epsilon_p$$

2. 摩擦系数不确定性： $$\mu \in [\mu_{min}, \mu_{max}]$$

3. 动力学参数不确定性： $$\mathbf{M}(\mathbf{q}) = \mathbf{M}_0(\mathbf{q}) + \Delta \mathbf{M}(\mathbf{q})$$ 随机接触模型：将接触力建模为随机过程： $$\mathbf{f}_c(t) = \overline{\mathbf{f}}_c(t) + \mathbf{w}_f(t)$$ 其中 $\mathbf{w}_f(t) \sim \mathcal{N}(0, \mathbf{\Sigma}_f)$ 为高斯白噪声。

接触检测不确定性：使用概率模型描述接触状态： $$P(\text{contact} | \mathbf{z}) = \frac{1}{1 + e^{-\alpha(d - d_{threshold})}}$$ 其中 $d$ 为测量距离，$\alpha$ 为敏感度参数。

6.3.2 鲁棒优化方法

1. 最坏情况优化： $$\begin{align} \min_{\mathbf{u}} \max_{\mathbf{w} \in \mathcal{W}} \quad & J(\mathbf{x}, \mathbf{u}, \mathbf{w}) \\ \text{s.t.} \quad & \mathbf{g}(\mathbf{x}, \mathbf{u}, \mathbf{w}) \leq 0 \quad \forall \mathbf{w} \in \mathcal{W} \end{align}$$ 其中 $\mathcal{W}$ 为不确定性集合。

2. 机会约束规划： $$\begin{align} \min_{\mathbf{u}} \quad & \mathbb{E}[J(\mathbf{x}, \mathbf{u}, \mathbf{w})] \\ \text{s.t.} \quad & P(\mathbf{g}(\mathbf{x}, \mathbf{u}, \mathbf{w}) \leq 0) \geq 1 - \epsilon \end{align}$$

3. 管道优化（Tube-based）方法： 构建状态管道确保鲁棒性： $$\mathcal{T} = \{\mathbf{x}(t) : |\mathbf{x}(t) - \mathbf{x}_{nominal}(t)| \leq r(t)\}$$ 管道半径 $r(t)$ 通过Lyapunov分析确定： $$\dot{V}(\mathbf{e}) \leq -\alpha V(\mathbf{e}) + \beta |\mathbf{w}|^2$$

4. 鲁棒摩擦锥约束： 考虑摩擦系数不确定性的保守摩擦锥： $$\mathcal{FC}_{robust} = \{\mathbf{f} : |\mathbf{f}_t| \leq \mu_{min} f_n - \delta_\mu\}$$ 其中 $\delta_\mu$ 为安全裕度。

6.3.3 自适应重规划

在线参数估计：使用扩展卡尔曼滤波器（EKF）估计接触参数： $$\begin{align} \hat{\mathbf{x}}_{k+1}^- &= f(\hat{\mathbf{x}}_k, \mathbf{u}_k) \\ \mathbf{P}_{k+1}^- &= \mathbf{F}_k \mathbf{P}_k \mathbf{F}_k^T + \mathbf{Q} \\ \mathbf{K}_{k+1} &= \mathbf{P}_{k+1}^- \mathbf{H}_{k+1}^T (\mathbf{H}_{k+1} \mathbf{P}_{k+1}^- \mathbf{H}_{k+1}^T + \mathbf{R})^{-1} \\ \hat{\mathbf{x}}_{k+1} &= \hat{\mathbf{x}}_{k+1}^- + \mathbf{K}_{k+1}(\mathbf{z}_{k+1} - h(\hat{\mathbf{x}}_{k+1}^-)) \\ \mathbf{P}_{k+1} &= (\mathbf{I} - \mathbf{K}_{k+1}\mathbf{H}_{k+1})\mathbf{P}_{k+1}^- \end{align}$$ 触发式重规划：定义重规划触发条件： $$|\mathbf{x}_{actual} - \mathbf{x}_{planned}| > \theta_{replan} \quad \text{或} \quad V_{Lyapunov}(\mathbf{e}) > V_{threshold}$$ 分层重规划架构：

高层规划器（1-10 Hz）
    ↓ 接触序列
中层规划器（10-100 Hz）
    ↓ 轨迹参考
低层控制器（100-1000 Hz）
    ↓ 关节力矩
机器人

滚动时域重规划： 在每个控制周期，求解有限时域优化问题： $$\begin{align} \min_{\mathbf{u}_{k:k+N}} \quad & \sum_{i=k}^{k+N-1} L(\mathbf{x}_i, \mathbf{u}_i) + V_f(\mathbf{x}_{k+N}) \\ \text{s.t.} \quad & \mathbf{x}_{i+1} = f(\mathbf{x}_i, \mathbf{u}_i) \\ & \mathbf{g}(\mathbf{x}_i, \mathbf{u}_i) \leq 0 \\ & \mathbf{x}_k = \mathbf{x}_{current} \end{align}$$

案例研究：NASA Valkyrie的多接触操作

NASA Valkyrie（R5）是为火星探索任务设计的人形机器人，其多接触控制系统展示了本章所述理论的实际应用。

系统概述

Valkyrie具有44个自由度，身高1.8米，重125公斤。其多接触能力包括：

• 双足行走与平衡
• 手脚并用的爬梯动作
• 利用手臂辅助的稳定操作
• 在不平整地形上的多点接触移动

多接触规划实现

接触点选择： Valkyrie使用分层方法选择接触点：

1. 可达性分析：计算各潜在接触点的可达性得分
2. 稳定性评估：评估不同接触配置的稳定裕度
3. 任务相关性：考虑任务目标对接触点的要求

动作生成流程：

任务规范 → 接触序列规划 → 轨迹优化 → 全身控制
     ↑                               ↓
     └──────── 执行监控 ←────────────┘

关键技术特点：

1. 混合位置/力控制：手臂采用阻抗控制，腿部采用位置控制
2. 接触力优化：通过QP实时分配接触力
3. 柔顺控制：Series Elastic Actuators提供天然柔顺性

爬梯任务分析

爬梯任务展示了复杂的多接触切换序列：

接触模式序列：

1. 初始站立（双脚接触）
2. 抓握梯子（双脚+双手）
3. 抬起右脚（左脚+双手）
4. 右脚踏上梯阶（双脚+双手，新配置）
5. 重复直至完成

优化目标： $$J = w_1 \cdot T_{total} + w_2 \cdot \sum E_{joint} + w_3 \cdot \sum |\mathbf{f}_{contact}|^2 + w_4 \cdot risk_{fall}$$ 鲁棒性措施：

• 视觉反馈用于梯阶位置修正
• 力传感器监测接触建立
• 平衡控制器处理外部扰动

本章小结

本章深入探讨了多接触规划的核心概念和实现方法。主要知识点包括：

关键概念

1. 多接触运动生成： - 接触图描述了接触模式之间的转换关系 - 时空优化同时考虑轨迹和切换时机 - 接触力分配通过QP实现实时求解

2. 混合动力学系统： - 系统包含连续动力学和离散模式切换 - 切换条件由保护集和不变集定义 - 平滑过渡策略减少切换不连续性

3. 鲁棒性保证： - 不确定性建模考虑几何、摩擦和动力学误差 - 鲁棒优化方法包括最坏情况、机会约束和管道方法 - 自适应重规划提供在线调整能力

核心公式

多接触动力学方程： $$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) + \mathbf{g}(\mathbf{q}) = \mathbf{S}^T\boldsymbol{\tau} + \sum_{i \in \sigma} \mathbf{J}_i^T(\mathbf{q}) \mathbf{f}_i$$ 接触力优化问题： $$\min_{\mathbf{f}, \boldsymbol{\tau}} |\mathbf{f}|^2_{\mathbf{W}_f} + |\boldsymbol{\tau}|^2_{\mathbf{W}_\tau} \quad \text{s.t.} \quad \text{动力学、摩擦锥、力矩约束}$$ 鲁棒性约束： $$P(\mathbf{g}(\mathbf{x}, \mathbf{u}, \mathbf{w}) \leq 0) \geq 1 - \epsilon$$

实践要点

• 接触序列规划应优先考虑稳定性和可行性
• 实时性要求需要高效的优化求解器
• 传感器反馈对处理不确定性至关重要
• 分层架构平衡了规划质量和计算效率

练习题

基础题

练习 6.1：给定一个四足机器人在平面上的三种接触模式：四脚站立、三脚站立（抬起右前脚）、两脚对角站立，画出接触模式转换图，并分析每种转换的可行性条件。

Hint：考虑质心投影必须在支撑多边形内的静态稳定性条件。

四脚站立 ←→ 三脚站立(RF抬起)
    ↓              ↓
两脚对角(LF-RH) ← 三脚站立(LH抬起)

练习 6.2：推导平面三连杆机器人末端与地面接触时的动力学方程。设关节角为 $\mathbf{q} = [q_1, q_2, q_3]^T$，接触点在末端，接触力为 $\mathbf{f}_c = [f_x, f_y]^T$。

Hint：使用拉格朗日方程，并将接触力通过雅可比矩阵映射到关节空间。

练习 6.3：设计一个QP问题来分配三个接触点的接触力，使得产生期望的质心加速度 $\ddot{\mathbf{r}}_{CoM}^{des} = [0, 0, 1]^T$ m/s²，同时最小化接触力。每个接触点的摩擦系数为 $\mu = 0.5$。

Hint：使用质心动力学方程和线性化摩擦锥约束。

挑战题

练习 6.4：考虑一个人形机器人从站立到爬行的接触模式转换。设计一个优化问题来生成过渡轨迹，考虑：(a) 最小化切换时间，(b) 保持动态平衡，(c) 避免关节极限。

Hint：使用多阶段优化，每个阶段对应一个接触模式。

练习 6.5：推导考虑接触不确定性的鲁棒MPC公式。假设接触位置有高斯噪声 $\mathbf{p}_c = \hat{\mathbf{p}}_c + \mathbf{w}$，其中 $\mathbf{w} \sim \mathcal{N}(0, \Sigma)$。要求以95%的概率保证稳定性。

Hint：将稳定性约束转换为机会约束，使用分布的性质。

练习 6.6：分析多接触系统的可控性。给定一个漂浮基座机器人有 $n$ 个潜在接触点，每个接触点可以施加3维力，推导系统可控的必要条件。

Hint：考虑接触力对质心和角动量的作用。

练习 6.7：设计一个实时接触序列重规划算法。当检测到预期接触点不可达时（例如地形变化），如何快速生成替代方案？

Hint：使用启发式搜索和预计算的运动原语库。

Algorithm: 实时接触序列重规划
Input: 当前状态 x_current, 失败接触点 c_failed, 时间预算 t_max
Output: 新接触序列 σ_new

1. 初始化优先队列 Q ← {x_current}
2. 预计算可达接触集 C_reach ← ReachableContacts(x_current)
3. While time < t_max and Q not empty:
   a. x ← Q.pop()  # 最低代价状态
   b. For each c in C_reach \ {c_failed}:
      i.   如果 IsStable(x, c):

           - 计算过渡代价 J_trans
           - 如果找到目标，返回路径
      ii.  生成后继状态 x_next
      iii. Q.push(x_next, priority=f(x_next))

4. 返回最佳部分解

代价函数：f(x) = g(x) + h(x)

- g(x): 已消耗代价（时间、能量）
- h(x): 启发式（到目标距离）

加速技巧：

- 预计算运动原语转换表
- 使用简化动力学模型
- 并行评估多个候选

练习 6.8（开放性思考）：讨论如何将深度强化学习应用于多接触规划。设计一个结合模型预测控制和学习的混合架构，分析各自的优势和接口设计。

Hint：考虑分层决策、安全保证和样本效率。

高层：RL策略网络（接触序列决策）
  ↓ 接触目标序列
中层：MPC（轨迹优化）
  ↓ 参考轨迹
低层：WBC（力矩控制）

常见陷阱与错误

1. 接触建模错误

陷阱：假设接触是刚性的，忽略柔性和碰撞动力学

错误：直接切换接触状态
正确：使用柔性接触模型或碰撞积分

调试技巧：

• 监控接触力的连续性
• 检查速度跳变是否合理
• 使用低速测试验证接触模型

2. 数值优化问题

陷阱：优化问题的病态条件数

症状：求解器收敛慢或失败
原因：不同量级的变量混合（位置 m vs 力 N）
解决：适当的变量缩放和预调节

调试技巧：

• 计算Hessian矩阵条件数
• 使用warm start技术
• 逐步增加问题复杂度

3. 摩擦锥线性化误差

陷阱：过度简化的摩擦锥近似

问题：4面体近似可能过于保守
改进：使用8面体或更多面
权衡：计算复杂度 vs 保守性

验证方法：

% 检查实际接触力是否在真实摩擦锥内
f_actual = [fx, fy, fz];
mu_actual = norm([fx, fy]) / fz;
assert(mu_actual <= mu_max * safety_factor);

4. 时序规划失误

陷阱：固定时间间隔的接触切换

错误：每0.5秒切换一次接触
正确：基于状态的切换条件

改进策略：

• 使用事件触发的切换
• 优化切换时机作为决策变量
• 考虑执行延迟和不确定性

5. 稳定性裕度不足

陷阱：仅检查静态稳定性

不足：ZMP在支撑多边形内
必需：考虑动态效应和扰动

鲁棒性检查清单：

• [ ] 静态稳定性裕度 > 阈值
• [ ] 动态稳定性（Capture Point）
• [ ] 扰动恢复能力
• [ ] 传感器噪声鲁棒性

6. 计算复杂度爆炸

陷阱：穷举所有可能的接触序列

问题：组合爆炸 O(n!)
解决：启发式剪枝、分层规划

优化技巧：

• 限制前瞻步数
• 使用运动原语库
• 采用近似动力学模型

7. 忽略硬件限制

陷阱：理论最优但实际不可执行

常见问题：

- 超出关节速度/加速度限制
- 忽略通信延迟
- 未考虑传感器采样率

实践建议：

# 始终检查硬件约束
def validate_trajectory(q_traj, hardware_specs):
    for q, qdot, qddot in q_traj:
        assert all(q >= hardware_specs.q_min)
        assert all(q <= hardware_specs.q_max)
        assert all(abs(qdot) <= hardware_specs.qdot_max)
        assert all(abs(qddot) <= hardware_specs.qddot_max)

8. 模式切换的震荡

陷阱：在两个接触模式间频繁切换

症状：抖动、能量损失、硬件磨损
原因：切换条件设计不当

解决方案：

• 引入滞后区间
• 最小持续时间约束
• 切换代价惩罚项

调试工具推荐

1. 可视化工具： - 接触力矢量显示 - 支撑多边形实时绘制 - 质心轨迹跟踪

2. 日志分析： - 记录所有模式切换事件 - 监控优化求解器状态 - 跟踪约束违反情况

3. 仿真验证： - 使用不同摩擦系数测试 - 添加随机扰动验证鲁棒性 - 慢动作回放分析失败案例

最佳实践总结

1. 渐进式开发：从简单场景开始，逐步增加复杂度
2. 充分测试：边界条件、异常情况、硬件故障
3. 冗余设计：多传感器融合、备用控制策略
4. 实时监控：关键指标实时显示和报警
5. 经验积累：建立失败案例库，持续改进