第5章：接触规划基础

接触规划是机器人实现复杂运动的关键技术，它决定了机器人何时、何处以及如何与环境建立接触。从人形机器人的步态生成到攀岩机器人的路径规划，接触规划都扮演着核心角色。本章深入探讨接触规划的基本理论与方法，重点关注接触序列的离散决策、连续运动的可行性约束，以及混合整数规划在统一框架下的应用。我们将看到，接触规划的本质是一个混合离散-连续优化问题，需要在组合爆炸的搜索空间中寻找最优解。

5.1 接触序列规划

5.1.1 接触模式与状态机

机器人的运动可以视为不同接触模式之间的切换。定义接触模式 $\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 \in \{0,1\}^{n_c}$：接触激活向量
• $\mathbf{P}_c = \{\mathbf{p}_i \in \mathbb{R}^3 : \sigma_i = 1\}$：接触点位置
• $\mathbf{N}_c = \{\mathbf{n}_i \in S^2 : \sigma_i = 1\}$：接触法向量
• $\boldsymbol{\mu}_c = \{\mu_i \in \mathbb{R}^+ : \sigma_i = 1\}$：摩擦系数

对于一个四足机器人，典型的接触模式包括：

• 四足支撑：$\sigma = [1,1,1,1]$ - 静态稳定，最大支撑域
• 三足支撑：$\sigma = [1,1,1,0]$ 等四种组合 - 静态稳定，允许单腿运动
• 两足支撑：$\sigma = [1,0,1,0]$ 等六种组合 - 条件稳定，需要特定质心位置
• 单足支撑：$\sigma = [1,0,0,0]$ 等四种组合 - 动态平衡，瞬时状态
• 飞行相：$\sigma = [0,0,0,0]$ - 弹道运动，完全由初始条件决定

状态转移的可行性条件：

从模式 $\sigma^{(k)}$ 到 $\sigma^{(k+1)}$ 的转移必须满足：

1. 单步变化约束：最多改变一个接触状态 $$|\sigma^{(k+1)} - \sigma^{(k)}|_0 \leq 1$$

2. 稳定性保持：转移前后都满足平衡条件 $$\text{IsStable}(\sigma^{(k)}) \land \text{IsStable}(\sigma^{(k+1)})$$

3. 运动学连续性：新接触点必须可达 $$\mathbf{p}_i^{new} \in \mathcal{W}_{reachable}(\mathbf{q}^{(k)})$$

状态机示例（完整的四足trot步态）：

    [四足支撑] --准备相--> [四足支撑(预载)]
         ^                      |
         |                      v
    落地缓冲                推进相
         |                      |
         |                      v
    [双对角支撑] <--摆动相-- [双对角支撑]
         |                      ^

         |                      ^
         |                      |
    切换对角 -------------------|

步态的形式化定义：

步态可以表示为接触模式的周期序列： $$\mathcal{G} = \{\sigma^{(0)}, \sigma^{(1)}, ..., \sigma^{(P-1)}\}$$ 其中P是周期。步态参数包括：

• 占空比（Duty Factor）：$DF_i = T_{stance,i} / T_{cycle}$
• 相位偏移：$\phi_{ij} = (t_i - t_j) / T_{cycle}$
• 步频：$f = 1 / T_{cycle}$

常见步态的参数化：

• Walk: $DF > 0.5$, 相位均匀分布
• Trot: $DF \approx 0.5$, 对角腿同相
• Pace: $DF \approx 0.5$, 同侧腿同相
• Gallop: $DF < 0.5$, 前后腿组同相

5.1.2 接触序列的图搜索

接触规划可以形式化为图搜索问题。定义接触图 $G = (V, E)$，其中：

• 节点 $v \in V$ 表示接触模式
• 边 $e \in E$ 表示模式转换

图的构建策略：

构建接触图时需要考虑物理可行性和计算效率的平衡。完全图包含 $2^{n_c}$ 个节点（所有可能的接触组合），但大部分在物理上不可行或不稳定。

有效的图构建方法：

1. 邻域扩展：从当前模式出发，只考虑单个接触的增加/删除
2. 稳定模式筛选：预先排除明显不稳定的模式（如单点支撑的人形机器人）
3. 运动学可达性剪枝：基于机器人运动学模型排除不可达的接触组合

成本函数设计： 边的成本 $c(e)$ 应反映：

1. 运动学可达性：$c_{kin} = |\mathbf{p}_{goal} - \mathbf{p}_{reach}|^2$
2. 动力学代价：$c_{dyn} = \int_0^T |\mathbf{u}(t)|^2 dt$
3. 稳定性裕度：$c_{stab} = -\min(\text{stability_margin})$
4. 接触切换代价：$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

5.1.3 接触序列的时间参数化

给定接触序列 $\Sigma = \{\sigma_0, \sigma_1, ..., \sigma_N\}$，需要确定：

1. 切换时刻：$\mathbf{t} = [t_1, t_2, ..., t_N]$
2. 接触位置：$\mathbf{p}_c^{(k)} = [p_1^{(k)}, p_2^{(k)}, ..., p_{n_c}^{(k)}]$

时间优化问题： $$\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$$ 约束条件：

• 最小/最大相位持续时间：$t_{min} \leq t_{k+1} - t_k \leq t_{max}$
• 运动学限制：$\mathbf{p}_c^{(k)} \in \mathcal{W}_{reachable}$
• 动力学可行性：$\mathbf{M}\ddot{\mathbf{q}} + \mathbf{h} = \mathbf{S}^T\boldsymbol{\tau} + \mathbf{J}_c^T\mathbf{f}_c$

5.1.4 分层规划策略

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

高层：接触序列搜索

• 输入：初始/目标配置，环境地图
• 输出：接触模式序列 $\Sigma$
• 方法：A、RRT、动态规划

高层规划器工作在离散的接触模式空间，使用简化的动力学模型（如倒立摆、质心动力学）快速评估可行性。关键是设计合适的状态抽象，既保留必要的物理约束，又避免过度的计算复杂度。

中层：接触时序和位置优化

• 输入：接触序列 $\Sigma$
• 输出：切换时刻 $\mathbf{t}$，接触位置 $\mathbf{p}_c$
• 方法：非线性规划、轨迹优化

中层优化器负责将离散的接触序列细化为连续的时空轨迹。这一层需要考虑更详细的动力学约束，如角动量守恒、接触力限制等。典型的优化问题形式： $$\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:

• 动力学可行性约束
• 接触一致性约束
• 时间窗口约束

低层：全身运动生成

• 输入：参数化的接触计划
• 输出：关节轨迹 $\mathbf{q}(t)$
• 方法：逆运动学、动力学优化

低层控制器将任务空间的轨迹映射到关节空间，处理冗余自由度的分配和次要任务（如关节限位回避、奇异性处理）。通常使用加权伪逆或二次规划方法： $$\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}$$ 层间反馈机制：

实际系统中，各层之间需要双向信息流：

• 自上而下：高层提供引导，中层细化，低层执行
• 自下而上：低层反馈不可行性，触发中层重新优化或高层重新规划

这种反馈机制确保了系统的鲁棒性，能够处理模型误差和环境变化。

5.2 可行性与稳定性约束

5.2.1 运动学可行性

接触点必须在机器人的可达工作空间内：

单链可达性： 对于串联链，末端执行器位置 $\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}$$ 例如，双足支撑时两脚相对位置受骨盆运动学限制。

5.2.2 静态稳定性

静态稳定性是接触规划的基础约束，确保机器人在准静态条件下维持平衡。虽然动态运动越来越重要，但静态稳定性仍是安全保障的底线。

支撑多边形判据： 质心投影必须位于支撑多边形内： $$\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$ 满足：

1. 平衡方程：$\mathbf{G}\mathbf{f}_c + \mathbf{g}_0 = \mathbf{0}$
2. 摩擦锥约束：$\mathbf{f}_{c,i} \in \mathcal{K}_i, \forall i : \sigma_i = 1$

其中 $\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}$$

力分配优化：

当系统过约束时（接触点多于必要），存在无穷多组力分配。选择最优分配的准则：

1. 最小范数：$$\min \sum_i |\mathbf{f}_i|^2$$ 减少能量消耗，降低接触磨损

2. 最大裕度：$$\max \min_i \left(\mu f_{n,i} - |\mathbf{f}_{t,i}|\right)$$ 最大化到摩擦锥边界的距离

3. 均匀分布：$$\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}$$

5.2.3 动态稳定性

零力矩点（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}$$

5.2.4 鲁棒性裕度

接触力裕度： 确保接触力远离摩擦锥边界： $$|\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$ 的球。

5.3 混合整数规划方法

5.3.1 问题的MILP表述

接触规划可以统一表述为混合整数线性规划（MILP）问题：

决策变量：

• 二进制变量：$\sigma_{i,k} \in \{0,1\}$ - 第k时刻接触i的状态
• 连续变量：$\mathbf{x}_k \in \mathbb{R}^n$ - 机器人状态
• 接触力：$\mathbf{f}_{i,k} \in \mathbb{R}^3$ - 接触力

目标函数： $$\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)$$ 约束条件：

1. 动力学约束（线性化）： $$\mathbf{x}_{k+1} = \mathbf{A}_k\mathbf{x}_k + \mathbf{B}_k\mathbf{u}_k + \mathbf{E}_k\mathbf{f}_k$$

2. 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是足够大的常数。

3. 摩擦锥线性化： $$\mathbf{C}_i\mathbf{f}_{i,k} \leq \mathbf{0}, \quad \forall i,k : \sigma_{i,k} = 1$$

5.3.2 Big-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}|)$$

5.3.3 凸松弛与分支定界

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}$$ 分支定界加速：

1. 热启动：使用启发式解初始化
2. 分支优先级：优先分支时间早的决策变量
3. 剪枝规则：基于物理不可行性提前剪枝

5.3.4 时间离散化策略

时间离散化是将连续时间问题转化为有限维优化问题的关键步骤。不同的离散化策略在精度、计算效率和约束表达能力之间有不同的权衡。

固定时间步： 将时间均匀离散化为N步： $$t_k = k \Delta t, \quad k = 0,1,...,N$$ 优点：

• 线性约束，保持MILP结构
• 实现简单，数值稳定
• 易于并行化

缺点：

• 时间分辨率固定，可能错过关键事件
• 需要预先确定时域长度
• 对快速动态响应分辨率不足

实践建议：$\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}$$ 这增加了优化的自由度但引入非线性。处理方法：

1. 分段线性化：将 $\Delta t_k$ 离散为几个候选值 $$\Delta t_k \in \{\Delta t^{(1)}, \Delta t^{(2)}, ..., \Delta t^{(m)}\}$$ 使用二进制变量选择。

2. 迭代细化：先用粗网格求解，识别关键时刻，局部细化

for iter in 1:max_iterations
   solve_with_current_grid()
   identify_critical_times()
   refine_grid_locally()

3. 自适应网格：基于梯度或约束违反程度动态调整 $$\Delta t_{k+1} = \Delta t_k \cdot \begin{cases} 0.5 & \text{if } |\nabla J| > \epsilon \\ 2.0 & \text{if } |\nabla J| < \epsilon/10 \\ 1.0 & \text{otherwise} \end{cases}$$ 相位固定方法： 预定义接触相位数量和顺序，仅优化持续时间： $$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样条），不同分辨率的基函数有不同支撑域。

5.4 案例研究：攀岩机器人的接触点选择

5.4.1 问题描述

考虑一个四肢攀岩机器人在垂直岩壁上的运动规划。机器人需要：

1. 从起始位置攀爬到目标位置
2. 选择合适的抓握点序列
3. 保证全程的静态稳定性
4. 最小化能量消耗

环境模型：

• 岩壁上有 $n_{holds} = 50$ 个可能的抓握点
• 每个抓握点有位置 $\mathbf{p}_i$ 和可靠性 $r_i \in [0,1]$
• 摩擦系数 $\mu = 0.8$

5.4.2 MILP建模

变量定义：

• $\sigma_{l,h,k} \in \{0,1\}$：时刻k肢体l是否抓握点h
• $\mathbf{x}_{com,k} \in \mathbb{R}^3$：质心位置
• $\mathbf{f}_{l,k} \in \mathbb{R}^3$：肢体l的接触力

目标函数： $$\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)$$ 包含：路径长度、能量消耗、抓握可靠性。

约束条件：

1. 单点抓握： $$\sum_h \sigma_{l,h,k} \leq 1, \quad \forall l,k$$

2. 至少三点支撑： $$\sum_{l,h} \sigma_{l,h,k} \geq 3, \quad \forall k$$

3. 运动学可达性： $$|\mathbf{x}_{com,k} - \mathbf{p}_h| \leq L_{max} + M(1-\sigma_{l,h,k})$$

4. 平衡约束： $$\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.4.3 求解策略

分层求解：

1. 阶段1：图搜索找到抓握点序列
2. 阶段2：固定序列，优化时序
3. 阶段3：生成详细运动轨迹

计算结果：

• 规划时间：2.3秒（50个抓握点）
• 找到12步攀爬序列
• 总攀爬时间：45秒
• 能量消耗：1250焦耳

5.4.4 实验验证

在真实岩壁环境测试：

• 成功率：85%（17/20次）
• 失败原因：抓握点可靠性估计误差
• 改进：在线重规划，动态更新可靠性估计

本章小结

本章深入探讨了接触规划的核心概念和方法：

关键要点：

1. 接触规划是混合离散-连续优化问题，需要同时决策接触模式（离散）和运动轨迹（连续）
2. 可行性约束包括运动学可达性、静/动态稳定性、接触力学约束
3. 混合整数规划提供了统一的优化框架，但计算复杂度高
4. 实用系统通常采用分层规划策略，平衡最优性和计算效率

核心公式汇总：

• 支撑多边形约束：$\mathbf{p}_{com}^{xy} \in \text{ConvexHull}(\{\mathbf{p}_i^{xy}\})$
• ZMP约束：$\mathbf{p}_{zmp} = \mathbf{p}_{com} - \frac{z_{com}}{g}\ddot{\mathbf{p}}_{com}$
• Big-M接触逻辑：$-M(1-\sigma) \leq \mathbf{f} \leq M\sigma$
• 摩擦锥约束：$|\mathbf{f}_t| \leq \mu f_n$

与后续章节的联系：

• 第6章将扩展到多接触场景，处理更复杂的接触切换
• 第9-10章的MPC方法将用于实时接触规划
• 第11-12章的全身控制将执行规划的接触序列

练习题

基础题

习题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问题，如旅行商问题。

Algorithm: GreedyContactPlanning

1. current ← start_config
2. sequence ← []
3. while (not reached goal):
   4. candidates ← ReachableContacts(current)
   5. next ← arg min_{c ∈ candidates} Cost(current, c)
   6. sequence.append(next)
   7. current ← next
8. LocalOptimization(sequence, k=3) // 3-opt
9. return sequence

习题5.7（开放题）讨论机器学习方法（如强化学习）与传统接触规划方法的优劣。在什么场景下应该选择哪种方法？如何结合两者的优势？

提示：考虑泛化能力、安全保证、计算效率、数据需求等方面。

常见陷阱与错误（Gotchas）

1. 数值问题

问题：Big-M值选择不当导致数值不稳定

错误：M = 1e10  // 太大，导致病态矩阵
正确：M = 10 * max_expected_value

解决：使用物理意义的界限，避免任意大数。

2. 约束不兼容

问题：过度约束导致无可行解

常见错误组合：

- 严格的ZMP约束 + 大步长要求
- 高速运动 + 静态稳定性

调试技巧：

1. 逐步放松约束，找出冲突源
2. 可视化约束集合的交集
3. 使用软约束with惩罚项

3. 组合爆炸

问题：接触点数量增长，搜索空间指数爆炸

n_contacts = 10, n_limbs = 4
可能组合数 = C(10,4) × 4! = 5040

缓解策略：

• 启发式剪枝
• 分层规划
• 限制搜索深度

4. 接触模型简化

问题：点接触假设忽略了接触面积效应

实际：面接触，压力分布不均
模型：点接触，集中力
后果：边缘倾翻时预测错误

改进：

• 使用多点接触近似
• 加入力矩约束
• 安全系数补偿

5. 时序规划缺陷

问题：固定时间步vs可变时间步的权衡

固定Δt：
  优点：线性约束
  缺点：可能错过关键时刻

可变Δt：
  优点：自适应精度
  缺点：非线性，求解困难

实用建议：

• 关键相位细化时间步
• 使用事件触发的重规划
• 多分辨率时间网格

6. 局部最优陷阱

问题：贪心策略陷入局部最优

场景：狭窄通道
贪心：选择最近接触点 → 卡死
最优：暂时远离后绕行

解决方法：

• 随机扰动+多次尝试
• 维护多个候选序列
• 使用RRT*等渐近最优算法

7. 实时性假象

问题：离线规划快but在线执行慢

离线：预计算所有可能序列
问题：存储爆炸，查询时间长
假象：看起来"实时"实际有延迟

真正的实时策略：

• 滚动时域规划
• 任意时算法
• 反应式控制+规划

8. 忽视不确定性

问题：确定性规划在真实世界失败

计划：精确的接触位置
现实：定位误差±5cm
结果：接触失败，机器人跌倒

鲁棒化方法：

• 接触区域而非点
• 概率约束规划
• 反馈控制补偿