接触规划是机器人实现复杂运动的关键技术，它决定了机器人何时、何处以及如何与环境建立接触。从人形机器人的步态生成到攀岩机器人的路径规划，接触规划都扮演着核心角色。本章深入探讨接触规划的基本理论与方法，重点关注接触序列的离散决策、连续运动的可行性约束，以及混合整数规划在统一框架下的应用。我们将看到，接触规划的本质是一个混合离散-连续优化问题，需要在组合爆炸的搜索空间中寻找最优解。
机器人的运动可以视为不同接触模式之间的切换。定义接触模式 $\sigma \in {0,1}^{n_c}$，其中 $n_c$ 是潜在接触点数量，$\sigma_i = 1$ 表示第 $i$ 个接触点激活。
接触模式的数学表示：
完整的接触状态不仅包括二进制的接触标志，还应包含接触的几何和力学信息： \(\mathcal{S} = \{\sigma, \mathbf{P}_c, \mathbf{N}_c, \boldsymbol{\mu}_c\}\)
其中：
对于一个四足机器人，典型的接触模式包括：
状态转移的可行性条件：
从模式 $\sigma^{(k)}$ 到 $\sigma^{(k+1)}$ 的转移必须满足：
单步变化约束：最多改变一个接触状态 \(\|\sigma^{(k+1)} - \sigma^{(k)}\|_0 \leq 1\)
稳定性保持：转移前后都满足平衡条件 \(\text{IsStable}(\sigma^{(k)}) \land \text{IsStable}(\sigma^{(k+1)})\)
运动学连续性：新接触点必须可达 \(\mathbf{p}_i^{new} \in \mathcal{W}_{reachable}(\mathbf{q}^{(k)})\)
状态机示例（完整的四足trot步态）：
[四足支撑] --准备相--> [四足支撑(预载)]
^ |
| v
落地缓冲 推进相
| |
| v
[双对角支撑] <--摆动相-- [双对角支撑]
| ^
| |
切换对角 -------------------|
步态的形式化定义：
步态可以表示为接触模式的周期序列： \(\mathcal{G} = \{\sigma^{(0)}, \sigma^{(1)}, ..., \sigma^{(P-1)}\}\)
其中P是周期。步态参数包括：
常见步态的参数化：
接触规划可以形式化为图搜索问题。定义接触图 $G = (V, E)$，其中：
图的构建策略：
构建接触图时需要考虑物理可行性和计算效率的平衡。完全图包含 $2^{n_c}$ 个节点（所有可能的接触组合），但大部分在物理上不可行或不稳定。
有效的图构建方法：
成本函数设计： 边的成本 $c(e)$ 应反映：
| 接触切换代价：$c_{switch} = \sum_i | \sigma_i^{new} - \sigma_i^{old} | $ |
总成本： \(c(e) = w_{kin}c_{kin} + w_{dyn}c_{dyn} + w_{stab}c_{stab} + w_{switch}c_{switch}\)
搜索算法选择：
不同的搜索算法适用于不同场景：
Dijkstra算法：保证找到最短路径，但需要探索所有节点，计算量大。适用于离线预计算。
A*算法：使用启发式函数加速搜索： \(f(n) = g(n) + h(n)\) 其中 $g(n)$ 是从起点到节点 $n$ 的实际成本，$h(n)$ 是到目标的估计成本。
RRT-Connect：适用于高维空间，通过随机采样避免显式构建完整图：
算法：RRT-Connect for Contact Planning
1. 初始化两棵树 T_start, T_goal
2. while (未连接):
3. q_rand ← 随机接触模式
4. q_near ← T_start中最近节点
5. if (可行转换(q_near → q_rand)):
6. T_start.add(q_rand)
7. 尝试连接到T_goal
给定接触序列 $\Sigma = {\sigma_0, \sigma_1, …, \sigma_N}$，需要确定：
时间优化问题： \(\min_{\mathbf{t}, \mathbf{p}_c} J = \sum_{k=0}^{N-1} \int_{t_k}^{t_{k+1}} L(\mathbf{x}(t), \mathbf{u}(t)) dt\)
约束条件：
由于问题的复杂性，通常采用分层策略：
高层：接触序列搜索
高层规划器工作在离散的接触模式空间，使用简化的动力学模型（如倒立摆、质心动力学）快速评估可行性。关键是设计合适的状态抽象，既保留必要的物理约束，又避免过度的计算复杂度。
中层：接触时序和位置优化
中层优化器负责将离散的接触序列细化为连续的时空轨迹。这一层需要考虑更详细的动力学约束，如角动量守恒、接触力限制等。典型的优化问题形式：
\[\min_{\mathbf{t}, \mathbf{p}_c} \sum_{k} \left( \Delta t_k \cdot E_k + \alpha \|\mathbf{p}_c^{(k)} - \mathbf{p}_c^{nominal}\|^2 \right)\]subject to:
低层：全身运动生成
低层控制器将任务空间的轨迹映射到关节空间，处理冗余自由度的分配和次要任务（如关节限位回避、奇异性处理）。通常使用加权伪逆或二次规划方法：
\[\dot{\mathbf{q}} = \mathbf{J}^{\dagger}(\dot{\mathbf{x}}_{des} + \mathbf{K}_p \mathbf{e}) + (\mathbf{I} - \mathbf{J}^{\dagger}\mathbf{J})\dot{\mathbf{q}}_{null}\]层间反馈机制：
实际系统中，各层之间需要双向信息流：
这种反馈机制确保了系统的鲁棒性，能够处理模型误差和环境变化。
接触点必须在机器人的可达工作空间内：
单链可达性： 对于串联链，末端执行器位置 $\mathbf{p}_{ee}$ 由正运动学给出： \(\mathbf{p}_{ee} = FK(\mathbf{q})\)
可达工作空间： \(\mathcal{W} = \{\mathbf{p} \in \mathbb{R}^3 : \exists \mathbf{q} \in \mathcal{Q}, FK(\mathbf{q}) = \mathbf{p}\}\)
其中 $\mathcal{Q}$ 是关节限位定义的配置空间。
多链协调约束： 当多个肢体同时接触时，需要满足闭链约束： \(\mathbf{g}(\mathbf{q}) = \mathbf{0}\)
例如，双足支撑时两脚相对位置受骨盆运动学限制。
静态稳定性是接触规划的基础约束，确保机器人在准静态条件下维持平衡。虽然动态运动越来越重要，但静态稳定性仍是安全保障的底线。
支撑多边形判据： 质心投影必须位于支撑多边形内： \(\mathbf{p}_{com}^{xy} \in \text{ConvexHull}(\{\mathbf{p}_i^{xy} : \sigma_i = 1\})\)
这可以表示为线性不等式： \(\mathbf{A} \mathbf{p}_{com}^{xy} \leq \mathbf{b}\)
其中 $\mathbf{A}$ 和 $\mathbf{b}$ 定义支撑多边形的半平面表示。
计算支撑多边形的算法：
1. 收集所有活跃接触点 P = {p_i | σ_i = 1}
2. 投影到水平面 P_xy = {(x_i, y_i)}
3. 计算凸包 ConvexHull(P_xy)
4. 提取半平面表示 (A, b)
稳定性裕度量化：
定义稳定性裕度 $d_{margin}$ 为质心投影到支撑多边形边界的最小距离： \(d_{margin} = \min_{i} \frac{\mathbf{b}_i - \mathbf{a}_i^T \mathbf{p}_{com}^{xy}}{\|\mathbf{a}_i\|}\)
归一化稳定性指标： \(\eta_{stab} = \frac{d_{margin}}{r_{polygon}}\) 其中 $r_{polygon}$ 是支撑多边形的内接圆半径。$\eta_{stab} \in [0,1]$，越大越稳定。
力可行性判据： 存在接触力 $\mathbf{f}_c$ 满足：
其中 $\mathbf{G}$ 是接触力到质心的映射矩阵： \(\mathbf{G} = \begin{bmatrix} \mathbf{I} & \mathbf{I} & ... & \mathbf{I} \\ \mathbf{r}_1 \times & \mathbf{r}_2 \times & ... & \mathbf{r}_n \times \end{bmatrix}\)
力分配优化：
当系统过约束时（接触点多于必要），存在无穷多组力分配。选择最优分配的准则：
最小范数：\(\min \sum_i \|\mathbf{f}_i\|^2\) 减少能量消耗，降低接触磨损
最大裕度：\(\max \min_i \left(\mu f_{n,i} - \|\mathbf{f}_{t,i}\|\right)\) 最大化到摩擦锥边界的距离
均匀分布：\(\min \text{var}(\{f_{n,i}\})\) 避免单点过载
统一的二次规划形式： \(\min_{\mathbf{f}} \mathbf{f}^T\mathbf{Q}\mathbf{f} + \mathbf{c}^T\mathbf{f}\) \(\text{s.t. } \mathbf{G}\mathbf{f} = -\mathbf{g}_0, \quad \mathbf{C}\mathbf{f} \leq \mathbf{0}\)
零力矩点（ZMP）约束： ZMP必须严格位于支撑多边形内部： \(\mathbf{p}_{zmp} = \mathbf{p}_{com} - \frac{z_{com}}{g} \ddot{\mathbf{p}}_{com}\) \(\mathbf{p}_{zmp} \in \text{Interior}(\text{SupportPolygon}) - \epsilon\)
其中 $\epsilon$ 是安全裕度。
捕获点（Capture Point）约束： 对于动态行走，使用捕获点确保可恢复性： \(\mathbf{p}_{cp} = \mathbf{p}_{com} + \frac{\dot{\mathbf{p}}_{com}}{\omega}\)
其中 $\omega = \sqrt{g/z_{com}}$ 是自然频率。
角动量约束： 质心角动量变化率受接触力矩限制： \(\dot{\mathbf{L}}_{com} = \sum_{i:\sigma_i=1} \mathbf{r}_i \times \mathbf{f}_i\) \(\|\dot{\mathbf{L}}_{com}\| \leq L_{max}\)
接触力裕度： 确保接触力远离摩擦锥边界： \(\|\mathbf{f}_{t,i}\| \leq \alpha \mu f_{n,i}, \quad \alpha \in (0,1)\)
典型取 $\alpha = 0.7 \sim 0.9$。
支撑多边形收缩： 考虑不确定性，使用收缩的支撑域： \(\mathcal{S}_{robust} = \mathcal{S} \ominus \mathcal{B}_r\)
其中 $\ominus$ 表示Minkowski差，$\mathcal{B}_r$ 是半径为 $r$ 的球。
接触规划可以统一表述为混合整数线性规划（MILP）问题：
决策变量：
目标函数： \(\min J = \sum_{k=0}^{N-1} \left( \|\mathbf{x}_k - \mathbf{x}_{goal}\|_Q + \|\mathbf{u}_k\|_R + \sum_i c_{switch}\|\sigma_{i,k+1} - \sigma_{i,k}\| \right)\)
约束条件：
动力学约束（线性化）： \(\mathbf{x}_{k+1} = \mathbf{A}_k\mathbf{x}_k + \mathbf{B}_k\mathbf{u}_k + \mathbf{E}_k\mathbf{f}_k\)
Big-M接触逻辑： \(-M(1-\sigma_{i,k}) \leq \mathbf{f}_{i,k} \leq M\sigma_{i,k}\) \(-M(1-\sigma_{i,k}) \leq \delta_{i,k} \leq M(1-\sigma_{i,k})\)
其中M是足够大的常数。
Big-M方法用于编码逻辑约束”如果接触则有力，否则无力”：
接触激活逻辑： \(\begin{align} f_{n,i} &\leq M \sigma_i \\ f_{n,i} &\geq m \sigma_i \\ \|\mathbf{f}_{t,i}\| &\leq M \sigma_i \end{align}\)
其中 $m > 0$ 是最小法向力，避免奇异接触。
位置一致性： \(\begin{align} \mathbf{p}_{c,i}^{(k+1)} - \mathbf{p}_{c,i}^{(k)} &\leq M(1 - \sigma_i^{(k)}) \\ \mathbf{p}_{c,i}^{(k+1)} - \mathbf{p}_{c,i}^{(k)} &\geq -M(1 - \sigma_i^{(k)}) \end{align}\)
M值的选择：
经验公式： \(M = 10 \times \max(\|\mathbf{f}_{expected}\|, \|\mathbf{p}_{workspace}\|)\)
McCormick松弛： 对于双线性项 $z = \sigma \cdot f$（二进制×连续），使用： \(\begin{align} z &\geq f^{min} \sigma \\ z &\leq f^{max} \sigma \\ z &\geq f - f^{max}(1-\sigma) \\ z &\leq f - f^{min}(1-\sigma) \end{align}\)
分支定界加速：
时间离散化是将连续时间问题转化为有限维优化问题的关键步骤。不同的离散化策略在精度、计算效率和约束表达能力之间有不同的权衡。
固定时间步： 将时间均匀离散化为N步： \(t_k = k \Delta t, \quad k = 0,1,...,N\)
优点：
缺点：
实践建议：$\Delta t = \min(0.1 T_{swing}, 0.05 T_{stance})$，确保捕获接触切换动态。
可变时间步： 引入时间间隔变量 $\Delta t_k$： \(t_{k+1} = t_k + \Delta t_k\) \(\Delta t_{min} \leq \Delta t_k \leq \Delta t_{max}\)
这增加了优化的自由度但引入非线性。处理方法：
分段线性化：将 $\Delta t_k$ 离散为几个候选值 \(\Delta t_k \in \{\Delta t^{(1)}, \Delta t^{(2)}, ..., \Delta t^{(m)}\}\) 使用二进制变量选择。
for iter in 1:max_iterations
solve_with_current_grid()
identify_critical_times()
refine_grid_locally()
相位固定方法： 预定义接触相位数量和顺序，仅优化持续时间： \(T_{phase} = \sum_{i \in phase} \Delta t_i\) \(T_{min}^{phase} \leq T_{phase} \leq T_{max}^{phase}\)
这种方法特别适用于周期性步态，如：
多分辨率策略：
结合不同时间尺度的优势：
粗尺度（100ms）：接触模式决策
中尺度（10ms）：质心轨迹规划
细尺度（1ms）：接触力详细分配
实现框架： \(\mathbf{x}(t) = \sum_{i} \mathbf{x}_i^{coarse} \phi_i^{coarse}(t) + \sum_{j} \mathbf{x}_j^{fine} \phi_j^{fine}(t)\)
其中 $\phi$ 是基函数（如B样条），不同分辨率的基函数有不同支撑域。
考虑一个四肢攀岩机器人在垂直岩壁上的运动规划。机器人需要：
环境模型：
变量定义：
目标函数： \(\min J = \sum_k \left( w_1 \|\mathbf{x}_{com,k} - \mathbf{x}_{goal}\| + w_2 \sum_l \|\mathbf{f}_{l,k}\|^2 + w_3 \sum_{l,h} (1-r_h)\sigma_{l,h,k} \right)\)
包含：路径长度、能量消耗、抓握可靠性。
约束条件：
单点抓握： \(\sum_h \sigma_{l,h,k} \leq 1, \quad \forall l,k\)
至少三点支撑： \(\sum_{l,h} \sigma_{l,h,k} \geq 3, \quad \forall k\)
运动学可达性： \(\|\mathbf{x}_{com,k} - \mathbf{p}_h\| \leq L_{max} + M(1-\sigma_{l,h,k})\)
平衡约束： \(\sum_l \mathbf{f}_{l,k} + m\mathbf{g} = \mathbf{0}\) \(\sum_l (\mathbf{p}_{l,k} - \mathbf{x}_{com,k}) \times \mathbf{f}_{l,k} = \mathbf{0}\)
分层求解：
计算结果：
在真实岩壁环境测试：
本章深入探讨了接触规划的核心概念和方法：
关键要点：
核心公式汇总：
与后续章节的联系：
习题5.1 考虑一个简化的二维双足机器人，有两个点足。机器人需要从位置A走到位置B（相距2米）。假设最大步长为0.5米，画出所有可能的接触序列图，并标注最少步数路径。
提示：将问题简化为离散格子，考虑左右脚交替的约束。
习题5.2 给定支撑多边形顶点为 $p_1=(0,0)$, $p_2=(1,0)$, $p_3=(1,1)$, $p_4=(0,1)$，质心位置 $p_{com}=(0.5, 0.5, 1)$，重力加速度 $g=10 m/s^2$。计算质心允许的最大水平加速度 $\ddot{p}_{com}^{max}$，使得ZMP保持在支撑多边形内。
提示：ZMP公式为 $p_{zmp} = p_{com} - \frac{z_{com}}{g}\ddot{p}_{com}$，考虑最坏情况。
习题5.3 写出Big-M方法的约束，编码以下逻辑：”如果接触点i激活($\sigma_i = 1$)，则接触力法向分量在[10N, 100N]之间；否则为0”。假设合理的M值。
提示：需要同时约束上下界，注意符号。
习题5.4 设计一个启发式函数 $h(n)$ 用于A*搜索四足机器人的接触序列。考虑：(a) 到目标的欧氏距离，(b) 地形高度变化，(c) 步态周期性。证明你的启发式函数是可采纳的（admissible）。
提示：可采纳性要求 $h(n) \leq h^(n)$，其中 $h^$ 是真实代价。
习题5.5 推导四足机器人trot步态（对角腿同步）的支撑多边形面积变化。设机身长宽为 $l \times w$，分析面积最小的危险时刻，提出改进策略。
提示：考虑对角支撑和四足支撑两种情况的切换。
习题5.6 考虑接触规划的计算复杂度。证明在一般情况下，判断是否存在可行的接触序列是NP-hard的。然后提出一个多项式时间的近似算法。
提示：可以规约到已知的NP-complete问题，如旅行商问题。
习题5.7（开放题）讨论机器学习方法（如强化学习）与传统接触规划方法的优劣。在什么场景下应该选择哪种方法？如何结合两者的优势？
提示：考虑泛化能力、安全保证、计算效率、数据需求等方面。
问题：Big-M值选择不当导致数值不稳定
错误：M = 1e10 // 太大，导致病态矩阵
正确：M = 10 * max_expected_value
解决：使用物理意义的界限，避免任意大数。
问题：过度约束导致无可行解
常见错误组合：
- 严格的ZMP约束 + 大步长要求
- 高速运动 + 静态稳定性
调试技巧：
问题：接触点数量增长，搜索空间指数爆炸
n_contacts = 10, n_limbs = 4
可能组合数 = C(10,4) × 4! = 5040
缓解策略：
问题：点接触假设忽略了接触面积效应
实际：面接触，压力分布不均
模型：点接触，集中力
后果：边缘倾翻时预测错误
改进：
问题：固定时间步vs可变时间步的权衡
固定Δt：
优点：线性约束
缺点：可能错过关键时刻
可变Δt：
优点：自适应精度
缺点：非线性，求解困难
实用建议：
问题：贪心策略陷入局部最优
场景：狭窄通道
贪心：选择最近接触点 → 卡死
最优：暂时远离后绕行
解决方法：
问题：离线规划快but在线执行慢
离线：预计算所有可能序列
问题：存储爆炸，查询时间长
假象：看起来"实时"实际有延迟
真正的实时策略：
问题：确定性规划在真实世界失败
计划：精确的接触位置
现实：定位误差±5cm
结果：接触失败，机器人跌倒
鲁棒化方法：