robot_control_tutorial

第6章:多接触规划

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

6.1 多接触运动生成

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

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

6.1.1 接触图与运动原语

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

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

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

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

转换代价通常考虑多个因素: \(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\)

其中:

运动原语(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})\)

其中:

原语组合与序列化: 复杂运动通过原语序列构建: \(\Pi = \{\pi_1, \pi_2, ..., \pi_n\}\)

序列的可行性需满足:

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(\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})\)

其中:

时间优化:接触切换时刻 ${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$ 反映了不同优化目标的相对重要性:

摩擦锥线性化:为了保持问题的凸性,通常将摩擦锥约束线性化为多面体: \(\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})\)

其中:

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

其中:

在每个区间 $[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}$ 为碰撞冲量,需满足:

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$ 为支撑多边形边界。

  1. 捕获点(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}}$。

  1. 可行性裕度(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)\)

其中:

时间窗口优化: 切换时间必须在可行窗口内: \(t_k \in [t_k^{min}, t_k^{max}]\)

窗口边界由多个约束决定:

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

上层:切换时刻优化 \(\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抬起) ``` 可行性条件: 1. 四脚→三脚:质心投影在剩余三个支撑点形成的三角形内 2. 三脚→两脚对角:质心在对角线上,需要动态平衡 3. 直接四脚→两脚对角:需要快速切换或使用角动量 静态稳定性判据:$\mathbf{p}_{CoM} \in ConvexHull(\{\mathbf{p}_i\}_{i \in \sigma})$

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

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

参考答案 动力学方程: $$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}^T(\mathbf{q})\mathbf{f}_c$$ 其中雅可比矩阵: $$\mathbf{J}(\mathbf{q}) = \begin{bmatrix} -l_1s_1 - l_2s_{12} - l_3s_{123} & -l_2s_{12} - l_3s_{123} & -l_3s_{123} \\ l_1c_1 + l_2c_{12} + l_3c_{123} & l_2c_{12} + l_3c_{123} & l_3c_{123} \end{bmatrix}$$ 记号:$s_i = \sin(q_i)$, $c_i = \cos(q_i)$, $s_{ij} = \sin(q_i + q_j)$等。 质量矩阵包含惯性项,科氏矩阵包含离心项,重力向量依赖配置。

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

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

参考答案 QP问题表述: $$\begin{align} \min_{\mathbf{f}_1, \mathbf{f}_2, \mathbf{f}_3} \quad & \sum_{i=1}^3 \|\mathbf{f}_i\|^2 \\ \text{s.t.} \quad & \sum_{i=1}^3 \mathbf{f}_i = m(\ddot{\mathbf{r}}_{CoM}^{des} - \mathbf{g}) \\ & \sum_{i=1}^3 (\mathbf{p}_i - \mathbf{r}_{CoM}) \times \mathbf{f}_i = \mathbf{0} \\ & |\mathbf{f}_{i,x}| \leq \mu \mathbf{f}_{i,z}, \quad |\mathbf{f}_{i,y}| \leq \mu \mathbf{f}_{i,z} \\ & \mathbf{f}_{i,z} \geq 0, \quad i = 1,2,3 \end{align}$$ 第一个约束为牛顿方程,第二个为力矩平衡(零角动量变化),后两个为摩擦锥约束。

挑战题

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

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

参考答案 多阶段优化问题: $$\begin{align} \min \quad & \sum_{k=1}^{N_p} T_k + \alpha \sum_{k,t} \|\boldsymbol{\tau}_{k,t}\|^2 \\ \text{s.t.} \quad & \text{阶段 } k \in \{1,...,N_p\}: \\ & \mathbf{x}_{k,t+1} = f_{\sigma_k}(\mathbf{x}_{k,t}, \mathbf{u}_{k,t}) \cdot \Delta t_k \\ & \mathbf{L}_{CoM} = \sum_i (\mathbf{p}_i - \mathbf{r}_{CoM}) \times \mathbf{f}_i \in \mathcal{L}_{stable} \\ & \mathbf{q}_{min} \leq \mathbf{q}_{k,t} \leq \mathbf{q}_{max} \\ & \text{边界条件}: \\ & \mathbf{x}_{k,T_k} = \mathbf{x}_{k+1,0} \quad \text{(连续性)} \\ & h_{impact}(\mathbf{x}_{k,T_k}^-, \mathbf{x}_{k+1,0}^+) = 0 \end{align}$$ 关键阶段:站立→单膝跪地→双膝跪地→四点支撑

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

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

参考答案 鲁棒MPC公式: $$\begin{align} \min_{\mathbf{u}_{0:N-1}} \quad & \mathbb{E}[\sum_{k=0}^{N-1} L(\mathbf{x}_k, \mathbf{u}_k) + V_f(\mathbf{x}_N)] \\ \text{s.t.} \quad & \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k, \mathbf{w}_k) \\ & P(\mathbf{p}_{ZMP} \in \mathcal{P}_{support}(\mathbf{w})) \geq 0.95 \\ & \mathbf{u}_k \in \mathcal{U} \end{align}$$ 稳定性机会约束转换: 对于高斯噪声,$\mathbf{p}_{ZMP} \sim \mathcal{N}(\mu_{ZMP}, \Sigma_{ZMP})$ 保守线性化: $$\mu_{ZMP} - 1.96\sqrt{diag(\Sigma_{ZMP})} \in \mathcal{P}_{support}^{shrink}$$ 其中 $\mathcal{P}_{support}^{shrink}$ 是收缩后的支撑多边形。

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

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

参考答案 可控性必要条件: 1. **质心可控性**:至少需要一个接触点产生3维力 $$rank\begin{bmatrix}\mathbf{I}_3 & \mathbf{I}_3 & \cdots & \mathbf{I}_3\end{bmatrix} = 3$$ 2. **角动量可控性**:需要至少两个非共线接触点 $$rank\begin{bmatrix}[\mathbf{p}_1]_\times & [\mathbf{p}_2]_\times & \cdots & [\mathbf{p}_n]_\times\end{bmatrix} = 3$$ 其中 $[\mathbf{p}]_\times$ 为反对称矩阵。 3. **完全可控性条件**: $$rank\begin{bmatrix} \mathbf{I}_3 & \mathbf{I}_3 & \cdots \\ [\mathbf{p}_1]_\times & [\mathbf{p}_2]_\times & \cdots \end{bmatrix} = 6$$ 4. **内力调节**:对于 $n > 2$ 个接触点,存在 $3(n-2)$ 维内力空间。 实践意义:双足站立提供完全可控性,单足需要角动量调节。

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

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

参考答案 实时重规划算法: ```python 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(力矩控制) ``` **优势分析**: - RL:处理离散决策、长期规划、复杂地形适应 - MPC:短期最优、约束满足、安全保证 - WBC:实时响应、力控制、稳定性 **接口设计**: 1. RL → MPC: - 目标接触位置和时机 - 代价函数权重调整 - 约束松弛参数 2. MPC → RL: - 可行性反馈 - 实际执行代价 - 状态预测误差 **训练策略**: - 仿真预训练 + 真实环境微调 - 使用MPC作为专家演示 - 安全约束作为奖励塑形 **安全保证**: - MPC层始终验证RL决策 - 紧急情况下的后备策略 - 渐进式信任度调整 研究方向:可微分MPC用于端到端训练、元学习快速适应新地形。

常见陷阱与错误

1. 接触建模错误

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

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

调试技巧

2. 数值优化问题

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

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

调试技巧

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在支撑多边形内
必需:考虑动态效应和扰动

鲁棒性检查清单

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. 经验积累:建立失败案例库,持续改进