第 80 章：模式切换——轮 / 足 / 混合的 FSM、混杂系统与安全裁决

本章定位：把轮足机器人从“会滚、会走”推进到“知道什么时候滚、什么时候走、什么时候混合”。 前置主线：复合/60 轮式运动学与 Pfaffian 约束，复合/70 轮足混合 MPC，复合/80 轮足强化学习。 读者画像：已经能读懂 OCS2 ModeSchedule，知道足式机器人接触序列，也见过高程图和地形分类。 难度：⭐⭐⭐。 预计时间：1 周，理论 8 小时，工程 8 小时，实验 6 小时。

---

80.0 前置自测

开始本章前，请独立回答下面问题。

#	问题	建议回顾
1	轮子的 Pfaffian 非完整约束为什么写在速度层，而不是位置层？	复合/60
2	OCS2 的 ModeSchedule 表示什么？它与足端接触表如何对应？	足式/110
3	高程图中的 slope、roughness、step height 分别如何计算？	足式/160
4	强化学习策略输出连续动作时，如何加入离散模式选择？	复合/80
5	若模式切换瞬间接触约束突变，MPC 线性化会出现什么症状？	复合/70

自测标准：

• 5/5 正确：可以直接阅读本章。
• 3-4/5 正确：阅读时重点关注公式旁的物理解释。
• 0-2/5 正确：建议先回顾轮式约束、OCS2 模式调度和高程图章节。

---

80.1 本章目标

学完本章，你应能：

1. 用混杂系统语言形式化轮足机器人的模式切换问题。
2. 写出轮、足、混合、过渡四类模式的状态机图。
3. 从高程图和本体感觉中提取模式切换特征。
4. 设计带滞回、驻留时间和安全锁的工程 FSM。
5. 解释 FSM、专家混合策略和强化学习门控之间的关系。
6. 在 OCS2 中把模式切换转成 ModeSchedule 与约束激活逻辑。
7. 编写可测试的 C++ 状态机框架，并给出中文注释。
8. 识别切换冲击、抖动、误判、约束不可行等常见故障。

---

80.2 为什么轮足机器人必须显式处理模式切换 ⭐

纯四足机器人的模式切换已经存在：每条腿在触地和摆动之间切换。

轮足机器人的困难更高一层：同一个末端既可以滚动，也可以当作脚支撑。

滚动模式追求低能耗、高速度和连续地面接触。

足式模式追求越障、爬阶和静态安全。

混合模式追求在不确定地形中保留两者的退路。

如果不显式处理切换，控制器会把这些物理假设混在一起。

表面上看，它只是“轮子转不转”的开关。

本质上看，它是动力学约束、接触力锥、执行器命令类型和安全边界的同步切换。

模式	主要约束	主要输入	优点	代价
轮模式	横向无滑移、纯滚动	轮速 + 腿姿态	快、省电	越障差
足模式	足端零速度、冲击管理	腿关节力矩	越障强	能耗高
混合模式	部分轮滚动、部分脚支撑	轮速 + 接触力	适应坡道和碎石	调参复杂
过渡模式	约束逐渐改变	增益、接触力、轮速插值	降低冲击	需要额外时序

本质洞察：模式切换不是任务层的“偏好选择”，而是控制问题定义本身的改变。 一旦模式改变，系统的可行动作集合也改变。 因此，模式切换必须进入 MPC/WBC/硬件接口，而不能只停留在行为树节点名称上。

回顾足式/110 OCS2 完整栈与双线程 MPC：步态调度通过 ModeSchedule 决定每条腿在什么时刻触地、什么时刻离地，OCS2 根据这个时序激活或关闭对应的接触约束。在那里，模式切换只涉及一个二值选择——每条腿要么触地，要么摆动。现在我们把这个思想扩展到轮足系统，切换的维度大幅增加。

足式步态只切换”触地/离地”。

轮足切换还要改变”触地以后如何运动”：静止支撑、纯滚动、允许小滑移、主动抬腿。每种运动方式对应不同的约束集合、不同的控制器接口和不同的安全边界。

---

80.3 反面案例：一个没有滞回的模式选择器

假设我们写一个最简单的规则：

if step_height > 0.10 m:
    mode = LEG
else:
    mode = WHEEL

这段逻辑在实验室平地上看起来有效。

到了真实地面，它会迅速失败。

深度相机有噪声。

高程图有空洞。

车身俯仰会改变局部坡度估计。

轮子接触软地面时会下陷。

同一块台阶在不同距离处的估计高度会跳变。

当 step_height 在 0.095 m 到 0.105 m 之间抖动时，控制器会在轮模式和足模式之间来回切换。

这种现象称为模式抖动。

模式抖动的后果不是“状态机看起来乱”这么简单。

MPC 的约束集合每次切换都会改变。

WBC 的任务优先级会改变。

轮电机从速度控制切到制动或力矩补偿。

腿端接触目标从“移动接触点”变成“静止支撑点”。

如果 100 ms 内来回切换两次，系统根本没有足够时间完成过渡。

反事实推理：如果我们只调高阈值，而不加滞回和驻留时间，会怎样？ 阈值调高可以减少误入足式模式，但会让机器人更晚识别台阶。 阈值调低可以提前识别危险，但会增加误触发。 这说明单阈值不是稳定切换的根本解决方案。

---

80.4 混杂系统形式化 ⭐⭐

轮足模式切换最自然的数学语言是混杂系统（Hybrid System）。

跨领域类比：混杂系统的概念在计算机科学中随处可见。一个 TCP 连接的状态机（LISTEN -> SYN_SENT -> ESTABLISHED -> FIN_WAIT -> CLOSED）就是混杂系统——连续状态是数据包序列号和窗口大小，离散模式是连接状态，保护条件是收到特定类型的数据包，重置映射是状态变量的初始化。轮足模式切换与 TCP 状态机的核心思想完全一致：系统在不同的离散模式下遵循不同的连续动力学规则，通过事件触发模式迁移。

混杂系统由连续状态、离散模式、流映射、保护条件和重置映射组成。

我们记离散模式集合为：

\[ \mathcal{M}=\{\mathrm{WHEEL},\mathrm{LEG},\mathrm{HYBRID},\mathrm{TRANSITION}\} \]

连续状态记为：

\[ x=\begin{bmatrix} p_b & \theta_b & v_b & \omega_b & q_\ell & \dot q_\ell & \phi_w & \dot\phi_w \end{bmatrix}^\top \]

其中 \(p_b\) 是基座位置。

\(\theta_b\) 是基座姿态。

\(v_b,\omega_b\) 是基座线速度和角速度。

\(q_\ell,\dot q_\ell\) 是腿关节位置和速度。

\(\phi_w,\dot\phi_w\) 是轮角和轮速。

输入记为：

\[ u=\begin{bmatrix} \tau_\ell & \tau_w & v_w^{cmd} & f_c^{ref} \end{bmatrix}^\top \]

不同模式下，输入的语义不同。

轮模式中，轮速命令是主输入。

足模式中，腿部关节力矩和接触力分配是主输入。

混合模式中，两者同时存在。

过渡模式中，输入还包含增益插值和约束松弛参数。

连续流映射写成：

\[ \dot x=f_m(x,u,t),\quad m\in\mathcal{M} \]

保护条件写成：

\[ g_{m\rightarrow n}(x,z,t)\ge 0 \]

这里 \(z\) 是外部感知特征，例如坡度、粗糙度、台阶高度、语义类别和置信度。

重置映射写成：

\[ x^+=R_{m\rightarrow n}(x^-) \]

在理想滚动到平滑步行的切换中，重置映射接近恒等。

在高速撞上台阶、轮子瞬间停转的情况下，重置映射必须考虑冲击。

---

80.5 四个模式的动力学假设 ⭐⭐

轮模式的核心假设：接触点沿轮平面方向运动，横向速度接近零。

足模式的核心假设：支撑足接触点在世界系近似静止。

混合模式的核心假设：一部分末端滚动，一部分末端支撑，或同一末端允许低速滚动但限制侧滑。

过渡模式的核心假设：约束不突然从一个集合跳到另一个集合，而是在短时间内平滑迁移。

项	轮模式	足模式	混合模式	过渡模式
接触点速度	纵向 \(r\dot\phi\)	近似 0	由局部地形决定	插值
横向速度	约束为 0	约束为 0	强约束或软约束	逐步收紧
法向力	连续	支撑相连续、着地瞬间突变	连续或分段连续	斜坡函数
轮速命令	主控制量	归零或阻尼	主/辅皆可	限斜率
腿部控制	姿态保持	步态/WBC	姿态 + 接触力	增益插值

对一个轮足末端，第 \(i\) 个接触点速度为：

\[ v_{c,i}=J_{c,i}(q)\dot q \]

在轮模式下，地形局部坐标为 \(\{e_\parallel,e_\perp,n\}\)。

纯滚动约束写成：

\[ e_\parallel^\top v_{c,i}-r_i\dot\phi_i=0 \]

横向无滑移约束写成：

\[ e_\perp^\top v_{c,i}=0 \]

法向不穿透约束写成：

\[ n^\top v_{c,i}=0 \]

在足模式下，静止支撑约束写成：

\[ J_{c,i}(q)\dot q=0 \]

在混合模式下，可以把纵向滚动约束改成软约束：

\[ \ell_{\mathrm{roll}}=\frac{1}{2}w_r(e_\parallel^\top v_{c,i}-r_i\dot\phi_i)^2 \]

当碎石、草地或软土使纯滚动假设不可靠时，软约束比硬约束更稳。

---

80.6 从约束集合看模式切换 ⭐⭐⭐

把每个模式的可行速度集合记为：

\[ \mathcal{V}_m(q)=\{\dot q \mid A_m(q)\dot q=b_m(q,u)\} \]

轮模式：

\[ A_{\mathrm{wheel}}\dot q=b_{\mathrm{wheel}}(\dot\phi) \]

足模式：

\[ A_{\mathrm{leg}}\dot q=0 \]

混合模式：

\[ A_{\mathrm{hybrid}}\dot q=b_{\mathrm{hybrid}} \]

若直接从轮模式切到足模式，相当于从一个仿射速度子空间跳到另一个子空间。

若切换时当前速度不在新子空间中，约束会要求瞬时速度变化。

瞬时速度变化意味着冲量。

冲量会在真实硬件上表现为轮胎打滑、减速器冲击或基座俯仰。

因此过渡控制器的第一任务是速度投影。

给定当前速度 \(\dot q^-\)，求最接近新约束集合的速度：

\[ \dot q^+=\arg\min_{\dot q}\|\dot q-\dot q^-\|_W^2 \]

约束为：

\[ A_n(q)\dot q=b_n \]

拉格朗日函数为：

\[ \mathcal L=\frac12(\dot q-\dot q^-)^\top W(\dot q-\dot q^-)+\lambda^\top(A_n\dot q-b_n) \]

对 \(\dot q\) 求导：

\[ W(\dot q-\dot q^-)+A_n^\top\lambda=0 \]

得到：

\[ \dot q=\dot q^- - W^{-1}A_n^\top\lambda \]

代入约束：

\[ A_n\dot q^- - A_nW^{-1}A_n^\top\lambda=b_n \]

所以，在约束矩阵满行秩时：

\[ S=A_nW^{-1}A_n^\top \]

\[ \lambda=S^{-1}(A_n\dot q^- - b_n) \]

最终投影为：

\[ \dot q^+=\dot q^- - W^{-1}A_n^\top S^{-1}(A_n\dot q^- - b_n) \]

这个公式的前提很重要：\(S\) 必须可逆。

真实轮足系统经常破坏这个前提。

例如四个轮足同时接触地面时，左右轮的横向无滑移约束可能在线速度层高度相关。

又例如足端接触点共面、法向估计相近、轮胎侧偏模型近似重复时，\(A_n\) 的行会变得近似线性相关。

此时 \(S\) 是半正定矩阵，直接写 \(S^{-1}\) 会把很小的奇异值放大成很大的修正速度。

工程实现应使用伪逆：

\[ \lambda=S^\dagger(A_n\dot q^- - b_n) \]

\[ \dot q^+=\dot q^- - W^{-1}A_n^\top S^\dagger(A_n\dot q^- - b_n) \]

如果现场噪声较大，常用阻尼形式：

\[ S_\epsilon^{-1}=(S+\epsilon^2 I)^{-1} \]

\[ \dot q^+=\dot q^- - W^{-1}A_n^\top S_\epsilon^{-1}(A_n\dot q^- - b_n) \]

本质洞察：过渡控制器不是“让动作更柔和”的装饰，而是在新旧约束集合之间做可行速度投影。 没有这一步，切换就会把不可行速度强行交给硬件。

阻尼逆牺牲了一点约束精确性，换来有界的速度修正。

这正适合模式切换瞬间的工程需求：宁可让约束误差在几十毫秒内由 WBC 和低层控制慢慢消掉，也不要因为一个秩亏矩阵产生不现实的冲量命令。

---

80.7 工程 FSM 的最小状态集合 ⭐⭐

一个可部署 FSM 至少需要以下状态：

BOOT
STAND
WHEEL
LEG
HYBRID
WHEEL_TO_LEG
LEG_TO_WHEEL
WHEEL_TO_HYBRID
HYBRID_TO_WHEEL
LEG_TO_HYBRID
HYBRID_TO_LEG
FAULT
RECOVERY

BOOT 负责传感器和执行器初始化。

STAND 是安全中间态。

WHEEL 是高速巡航态。

LEG 是越障和爬阶态。

HYBRID 是不确定地形或坡道态。

所有带箭头含义的状态都是过渡态。

FAULT 表示已经触发硬件或估计故障。

RECOVERY 表示从可恢复故障中回到站立。

为什么不只用三个状态？

因为切换本身有时长。

如果不显式建模过渡态，就无法给 MPC 插入过渡约束。

也无法在 ros2_control 层正确切换轮控制器和腿控制器。

---

80.8 状态机图

                           ┌──────────┐
                           │  BOOT    │
                           └────┬─────┘
                                │ init ok
                                ▼
                           ┌──────────┐
                  ┌───────▶│  STAND   │◀────────┐
                  │        └────┬─────┘         │
                  │             │ stable start   │ fault cleared
                  │             ▼                │
              ┌───┴────┐   ┌──────────┐     ┌───┴────┐
              │ FAULT  │◀──│  WHEEL   │     │RECOVERY│
              └────────┘   └────┬─────┘     └────────┘
                    ▲           │ step / high risk
                    │           ▼
                    │     ┌──────────────┐
                    │     │ WHEEL_TO_LEG │
                    │     └──────┬───────┘
                    │            ▼
                    │       ┌──────────┐
                    │       │   LEG    │
                    │       └────┬─────┘
                    │            │ rough / slope
                    │            ▼
                    │     ┌──────────────┐
                    │     │ LEG_TO_      │
                    │     │ HYBRID       │
                    │     └──────┬───────┘
                    │            ▼
                    │       ┌──────────┐
                    └──────▶│ HYBRID   │
                            └──────────┘

反向边：
WHEEL <- LEG        : LEG_TO_WHEEL
WHEEL <- HYBRID     : HYBRID_TO_WHEEL
LEG   <- HYBRID     : HYBRID_TO_LEG
WHEEL -> HYBRID     : WHEEL_TO_HYBRID

这张图不是为了好看。

它表达四个工程约束。

第一，BOOT 只能进入 STAND，不能根据地形直接跳到 WHEEL、LEG 或 HYBRID。

第二，所有高能动作都可以退回 STAND 或 FAULT。

第三，WHEEL 到 LEG 不直接跳，而是经过 WHEEL_TO_LEG。

第四，HYBRID 是一个可持续控制模式，不只是过渡态。

BOOT -> STAND -> 目标模式 是安全链。

原因是 BOOT 阶段只证明通信、传感器和电机使能成功，并没有证明机器人已经稳定承重。

STAND 阶段要完成姿态收敛、接触力建立、零速命令确认和安全裁剪激活。

只有这些条件成立以后，地形分类结果才有资格驱动 WHEEL、LEG 或 HYBRID。

这条链路避免了一个常见错误：系统刚启动就因为前方地形看起来平坦而进入轮模式，但此时轮速命令、腿部支撑和状态估计还没有完全对齐。

LEG_TO_HYBRID 与 HYBRID_TO_LEG 也属于正式过渡态。

它们用于处理从纯足支撑转为轮足混合支撑，或从混合滚动回到纯足支撑的接触语义变化。

---

80.9 地形特征：从高程图到切换依据 ⭐⭐

常用输入不是单个高度，而是一组局部统计量。

设机器人前方规划窗口为 \(\Omega\)。

高程图为 \(h(x,y)\)。

局部坡度可由最小二乘平面拟合得到。

拟合平面：

\[ h(x,y)=a x+b y+c \]

坡度角为：

\[ \alpha=\arctan\sqrt{a^2+b^2} \]

粗糙度定义为残差均方根：

\[ \rho=\sqrt{\frac{1}{|\Omega|}\sum_{(x,y)\in\Omega}(h(x,y)-a x-b y-c)^2} \]

台阶高度可以用局部高度分位数差估计：

\[ s=h_{95\%}-h_{5\%} \]

地形置信度记为 \(c_{\mathrm{terrain}}\)。

它可以来自点云密度、深度有效像素比例、时间一致性或语义分类置信度。

切换决策不应只看平均坡度。

一个 12 度的平整坡道适合混合或轮模式。

一个 6 度但布满碎石的区域可能更适合足模式或混合模式。

一个高程图缺失严重的区域不应贸然高速滚动。

特征	低值含义	高值含义	对模式的影响
\(\alpha\)	平地	坡道	高值倾向混合或足式
\(\rho\)	平整	粗糙	高值降低轮模式置信
\(s\)	无台阶	有台阶	高值倾向足式
\(c_{\mathrm{terrain}}\)	感知不可靠	感知可靠	低值限速或混合
\(\mu_{\mathrm{est}}\)	摩擦小	摩擦大	低值限制轮加速度

---

80.10 滞回阈值 ⭐⭐

单阈值会抖动。

滞回使用进入阈值和退出阈值。

例如台阶高度：

\[ s_{\mathrm{enter\_leg}}=0.10\text{ m} \]

\[ s_{\mathrm{exit\_leg}}=0.06\text{ m} \]

当当前不是足模式时，只有 \(s>0.10\) m 才进入足模式。

当当前已经是足模式时，只有 \(s<0.06\) m 才允许离开足模式。

中间区间 \([0.06,0.10]\) 保持原模式。

驻留时间进一步限制：

\[ t_{\mathrm{dwell}}\ge t_{\min} \]

工程上常用：

切换	最小驻留时间	原因
WHEEL → LEG	0.5-1.0 s	需要降速和调整腿姿态
LEG → WHEEL	1.0-2.0 s	需要确认平地连续存在
WHEEL → HYBRID	0.3-0.8 s	坡道和碎石变化较快
HYBRID → WHEEL	1.0 s	避免出碎石后立即加速
任何模式 → FAULT	0 s	安全优先

陷阱框：滞回不是延迟

滞回的目标不是让系统”反应慢”。 滞回的目标是让离散决策对小噪声不敏感。 真正危险的事件，例如跌倒、过流、通信丢失，应绕过普通滞回，直接进入故障态。

跨领域类比：滞回在电子工程中被广泛使用。施密特触发器（Schmitt Trigger）就是电路层面的滞回——输入电压从低到高时，输出在 \(V_H\) 翻转；从高到低时，输出在 \(V_L < V_H\) 翻转。如果没有滞回，当输入电压在阈值附近波动时，输出会剧烈抖动，后级电路无法正常工作。轮足模式切换的滞回与施密特触发器的原理完全一致——地形特征对应输入电压，模式选择对应输出电平，阈值差对应滞回带宽。

---

80.11 模式评分函数

FSM 可以用规则写死，也可以先计算每个模式的评分。

评分函数形式：

\[ S_m=w_\alpha^m\alpha+w_\rho^m\rho+w_s^m s+w_\mu^m(1-\mu_{\mathrm{est}})+w_c^m(1-c_{\mathrm{terrain}}) \]

选择分数最小的模式：

\[ m^\star=\arg\min_m S_m \]

这种方法的优点是可解释。

每个权重都可以写进参数文件。

也可以在日志中解释“为什么切换”。

缺点是权重耦合。

坡度和粗糙度的单位不同。

视觉置信度和摩擦估计的可靠性不同。

所以评分函数必须先做归一化。

归一化示例：

\[ \bar\alpha=\mathrm{clip}(\alpha/\alpha_{\max},0,1) \]

\[ \bar\rho=\mathrm{clip}(\rho/\rho_{\max},0,1) \]

\[ \bar s=\mathrm{clip}(s/s_{\max},0,1) \]

---

80.12 安全裁决层

评分函数给出偏好。

安全裁决层给出禁用集合。

例如：

• 轮电机温度过高：禁用长时间轮模式。
• 轮速编码器丢包：禁用高速轮模式。
• 高程图置信度低：禁止超过安全速度。
• IMU 俯仰角过大：禁止继续加速。
• 电池电压过低：降低最大腿部冲击动作。

形式化写成：

\[ \mathcal{M}_{\mathrm{allowed}}=\mathcal{M}\setminus\mathcal{M}_{\mathrm{blocked}} \]

最终选择：

\[ m^\star=\arg\min_{m\in\mathcal{M}_{\mathrm{allowed}}}S_m \]

如果允许集合为空，进入 FAULT 或 STAND。

本质洞察：学习策略可以建议，安全裁决必须有最终否决权。 这不是保守主义，而是硬件部署的基本结构。

---

80.13 FSM 与强化学习门控的关系 ⭐⭐⭐

强化学习可把模式选择看成离散动作。

策略网络输出：

\[ \pi(a_m\mid o_t) \]

其中 \(a_m\) 是模式动作。

观测 \(o_t\) 包括本体感觉、高程图特征、历史速度和目标速度。

选择模式：

\[ m_t=\arg\max_m \pi(a_m\mid o_t) \]

这看起来与 FSM 不同。

但从系统角度看，它仍然是“观测到离散模式”的映射。

FSM 的规则由工程师写。

学习门控的规则由数据和奖励塑造。

两者都需要安全边界。

两者都需要驻留时间。

两者都需要处理不确定感知。

维度	显式 FSM	学习门控	混合方案
可解释性	高	低	中
调参来源	阈值和规则	数据和奖励	规则 + 数据
安全证明	相对容易	困难	安全层可验证
泛化	依赖规则覆盖	依赖训练分布	更稳健
工程部署	可控	需大量验证	推荐

推荐结构：

感知与状态估计
      │
      ▼
候选模式评分或策略网络
      │
      ▼
安全裁决层
      │
      ▼
带滞回 FSM
      │
      ▼
OCS2 ModeSchedule / WBC 任务 / 硬件控制器

---

80.14 专家混合模式选择

专家混合模型把不同模式看成不同专家。

滚动专家负责平地高速。

步行专家负责台阶和不连续地形。

混合专家负责坡道、碎石和低置信区域。

门控网络输出权重：

\[ \gamma_m(o_t)\ge 0,\quad \sum_m\gamma_m(o_t)=1 \]

动作可以是加权混合：

\[ u_t=\sum_m\gamma_m u_t^m \]

但注意：离散接触约束通常不能简单加权。

轮模式的硬约束和足模式的硬约束不一定有共同可行解。

因此在真实控制器中，更推荐让门控选择“主模式”，再由过渡控制器平滑。

连续加权更适合用于高层偏好或代价权重。

例如：

\[ Q_{\mathrm{ee}}=\gamma_{\mathrm{leg}}Q_{\mathrm{ee}}^{leg}+\gamma_{\mathrm{wheel}}Q_{\mathrm{ee}}^{wheel} \]

---

80.15 过渡控制器的三件事 ⭐⭐⭐

过渡控制器必须同时做三件事：

1. 速度匹配。
2. 接触力平滑。
3. 控制接口切换。

速度匹配已在前面用投影公式解释。

接触力平滑通常使用斜坡函数：

\[ \lambda(t)=\sigma(t)\lambda_{\mathrm{new}}+(1-\sigma(t))\lambda_{\mathrm{old}} \]

其中：

\[ \sigma(t)=3s^2-2s^3,\quad s=\mathrm{clip}\left(\frac{t-t_0}{T_{\mathrm{tr}}},0,1\right) \]

这个三次函数在起点和终点导数为零。

因此比线性插值更不容易激发机械振动。

控制接口切换包括：

• 轮速度控制器限斜率。
• 腿部力矩控制器保持输出连续。
• 关节 PD 增益从滚动姿态增益切到支撑增益。
• WBC 约束从滚动约束切到静止支撑约束。
• 日志记录切换原因和传感器特征。

---

80.16 OCS2 中的模式调度 ⭐⭐⭐

在 OCS2 里，离散模式通常由整数编码。

轮足系统可定义：

enum class WheelLeggedMode : int {
  WHEEL = 0,
  LEG = 1,
  HYBRID = 2,
  WHEEL_TO_LEG = 3,
  LEG_TO_WHEEL = 4,
  WHEEL_TO_HYBRID = 5,
  HYBRID_TO_WHEEL = 6,
  LEG_TO_HYBRID = 7,
  HYBRID_TO_LEG = 8
};

模式时间表包含：

eventTimes: [t0, t1, t2, ...]
modeSequence: [WHEEL, WHEEL_TO_LEG, LEG, ...]

回顾足式/110 OCS2 完整栈与双线程 MPC：OCS2 的 ModeSchedule 最初是为四足步态设计的——modeSequence 中的每个整数编码了四条腿的接触状态组合（如 15 表示全部触地，14 表示右后腿摆动）。现在我们把同样的机制复用到轮足场景，但语义完全不同：整数编码的不再是"哪条腿触地"，而是"整机处于什么运动模态"。这种复用之所以可行，是因为 OCS2 的约束激活逻辑本身不关心整数的语义——它只需要知道"当前时刻处于哪个模式"，然后根据用户定义的 isActive() 函数激活对应的约束。

约束类通过 isActive(time) 查询当前模式。

轮约束只在轮模式或混合模式激活。

足端零速度约束只在足模式或某些混合子模式激活。

过渡模式激活软化约束。

示例接口：

class WheelRollingConstraint final : public ocs2::StateInputConstraint {
 public:
  WheelRollingConstraint(int wheelIndex,
                         std::shared_ptr<ModeProvider> modeProvider)
      : wheelIndex_(wheelIndex), modeProvider_(std::move(modeProvider)) {}

  bool isActive(ocs2::scalar_t time) const override {
    const auto mode = modeProvider_->modeAt(time);
    // 中文注释：轮模式和混合模式下，轮子的纯滚动约束参与优化。
    return mode == WheelLeggedMode::WHEEL ||
           mode == WheelLeggedMode::HYBRID;
  }

  size_t getNumConstraints(ocs2::scalar_t) const override {
    // 中文注释：纵向纯滚动、横向无滑移、法向不穿透。
    return 3;
  }

 private:
  int wheelIndex_;
  std::shared_ptr<ModeProvider> modeProvider_;
};

---

80.17 C++ 状态机框架

下面给出一个可测试的工程骨架。

它不依赖 ROS。

它只处理状态转移。

ROS 节点可以把它包起来。

#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <optional>
#include <string>

enum class Mode {
  Boot,
  Stand,
  Wheel,
  Leg,
  Hybrid,
  WheelToLeg,
  LegToWheel,
  WheelToHybrid,
  HybridToWheel,
  LegToHybrid,
  HybridToLeg,
  Fault,
  Recovery
};

struct TerrainFeatures {
  double slope_rad = 0.0;
  double roughness_m = 0.0;
  double step_m = 0.0;
  double confidence = 1.0;
  double friction = 0.8;
};

struct RobotHealth {
  bool imu_ok = true;
  bool wheel_encoder_ok = true;
  bool motor_ok = true;
  bool estimator_ok = true;
  double pitch_rad = 0.0;
  double wheel_motor_temp_c = 40.0;
};

struct ModeDecision {
  Mode next = Mode::Stand;
  std::string reason;
  bool emergency = false;
};

class WheelLeggedFsm {
 public:
  ModeDecision update(double now_s,
                      const TerrainFeatures& terrain,
                      const RobotHealth& health) {
    if (!health.imu_ok || !health.motor_ok || !health.estimator_ok) {
      mode_ = Mode::Fault;
      last_switch_s_ = now_s;
      return {mode_, "关键状态估计或电机状态异常", true};
    }

    if (std::abs(health.pitch_rad) > params_.max_pitch_rad) {
      mode_ = Mode::Fault;
      last_switch_s_ = now_s;
      return {mode_, "基座俯仰角超过安全范围", true};
    }

    if (mode_ == Mode::Boot) {
      mode_ = Mode::Stand;
      target_after_transition_.reset();
      last_switch_s_ = now_s;
      return {mode_, "初始化通过，先进入安全站立态", false};
    }

    if (target_after_transition_.has_value()) {
      return {mode_, "过渡仍在进行，保持当前过渡模式", false};
    }

    const Mode desired = chooseDesiredMode(terrain, health);
    if (desired == mode_) {
      return {mode_, "当前模式仍适合地形", false};
    }

    if (!dwellSatisfied(now_s, mode_, desired)) {
      return {mode_, "方向相关驻留时间未满足，保持当前模式", false};
    }

    const Mode transition = transitionMode(mode_, desired);
    mode_ = transition;
    target_after_transition_ = desired;
    last_switch_s_ = now_s;
    return {mode_, "进入过渡模式", false};
  }

  void finishTransition(double now_s) {
    if (target_after_transition_.has_value()) {
      mode_ = *target_after_transition_;
      target_after_transition_.reset();
      last_switch_s_ = now_s;
    }
  }

  Mode mode() const { return mode_; }

 private:
  struct DwellTimeConfig {
    double stand_to_active_s = 0.8;
    double wheel_to_leg_s = 0.8;
    double leg_to_wheel_s = 1.5;
    double wheel_to_hybrid_s = 0.6;
    double hybrid_to_wheel_s = 1.2;
    double leg_to_hybrid_s = 0.5;
    double hybrid_to_leg_s = 0.8;
  };

  struct Params {
    static constexpr double kDegToRad = 3.14159265358979323846 / 180.0;

    double enter_leg_step_m = 0.10;
    double exit_leg_step_m = 0.06;
    double enter_hybrid_slope_rad = 15.0 * kDegToRad;
    double exit_hybrid_slope_rad = 8.0 * kDegToRad;
    double enter_hybrid_roughness_m = 0.035;
    double exit_hybrid_roughness_m = 0.020;
    DwellTimeConfig dwell;
    double max_pitch_rad = 28.0 * kDegToRad;
    double min_confidence_for_fast_wheel = 0.65;
  };

  bool dwellSatisfied(double now_s, Mode current, Mode desired) const {
    return now_s - last_switch_s_ >= dwellTime(current, desired);
  }

  double dwellTime(Mode current, Mode desired) const {
    if (current == Mode::Stand &&
        (desired == Mode::Wheel || desired == Mode::Leg ||
         desired == Mode::Hybrid)) {
      return params_.dwell.stand_to_active_s;
    }
    if (current == Mode::Wheel && desired == Mode::Leg) {
      return params_.dwell.wheel_to_leg_s;
    }
    if (current == Mode::Leg && desired == Mode::Wheel) {
      return params_.dwell.leg_to_wheel_s;
    }
    if (current == Mode::Wheel && desired == Mode::Hybrid) {
      return params_.dwell.wheel_to_hybrid_s;
    }
    if (current == Mode::Hybrid && desired == Mode::Wheel) {
      return params_.dwell.hybrid_to_wheel_s;
    }
    if (current == Mode::Leg && desired == Mode::Hybrid) {
      return params_.dwell.leg_to_hybrid_s;
    }
    if (current == Mode::Hybrid && desired == Mode::Leg) {
      return params_.dwell.hybrid_to_leg_s;
    }
    return params_.dwell.stand_to_active_s;
  }

  Mode chooseDesiredMode(const TerrainFeatures& terrain,
                         const RobotHealth& health) const {
    if (!health.wheel_encoder_ok) {
      return Mode::Leg;
    }
    if (terrain.step_m > params_.enter_leg_step_m) {
      return Mode::Leg;
    }
    if (mode_ == Mode::Leg && terrain.step_m > params_.exit_leg_step_m) {
      return Mode::Leg;
    }
    if (terrain.confidence < params_.min_confidence_for_fast_wheel) {
      return Mode::Hybrid;
    }
    if (terrain.slope_rad > params_.enter_hybrid_slope_rad ||
        terrain.roughness_m > params_.enter_hybrid_roughness_m) {
      return Mode::Hybrid;
    }
    if (mode_ == Mode::Hybrid &&
        (terrain.slope_rad > params_.exit_hybrid_slope_rad ||
         terrain.roughness_m > params_.exit_hybrid_roughness_m)) {
      return Mode::Hybrid;
    }
    return Mode::Wheel;
  }

  Mode transitionMode(Mode current, Mode desired) const {
    if (current == Mode::Wheel && desired == Mode::Leg) {
      return Mode::WheelToLeg;
    }
    if (current == Mode::Leg && desired == Mode::Wheel) {
      return Mode::LegToWheel;
    }
    if (current == Mode::Wheel && desired == Mode::Hybrid) {
      return Mode::WheelToHybrid;
    }
    if (current == Mode::Hybrid && desired == Mode::Wheel) {
      return Mode::HybridToWheel;
    }
    if (current == Mode::Leg && desired == Mode::Hybrid) {
      return Mode::LegToHybrid;
    }
    if (current == Mode::Hybrid && desired == Mode::Leg) {
      return Mode::HybridToLeg;
    }
    return desired;
  }

  Params params_;
  Mode mode_ = Mode::Boot;
  double last_switch_s_ = 0.0;
  std::optional<Mode> target_after_transition_;
};

这个框架可以单元测试。

测试用例应覆盖：

• BOOT 初始化后只能进入 STAND，不能直接进入地形期望模式。
• 台阶高度从 0.05 m 增加到 0.12 m。
• 台阶高度在阈值附近抖动。
• 感知置信度突然下降。
• 轮编码器故障。
• 驻留时间未满足。
• LEG 与 HYBRID 双向切换时进入对应过渡态。
• 过渡完成后进入目标模式。

---

80.18 参数文件设计

工程参数不要散落在代码中。

建议单独使用 YAML。

mode_switch:
  dwell_time:
    stand_to_active: 0.8
    wheel_to_leg: 0.8
    leg_to_wheel: 1.5
    wheel_to_hybrid: 0.6
    hybrid_to_wheel: 1.2
    leg_to_hybrid: 0.5
    hybrid_to_leg: 0.8

  terrain_threshold:
    enter_leg_step_m: 0.10
    exit_leg_step_m: 0.06
    enter_hybrid_slope_deg: 15.0
    exit_hybrid_slope_deg: 8.0
    enter_hybrid_roughness_m: 0.035
    min_confidence_for_fast_wheel: 0.65

  safety:
    max_pitch_deg: 28.0
    max_roll_deg: 25.0
    wheel_motor_temp_limit_c: 85.0
    max_mode_switch_per_min: 20

  transition:
    duration_s: 0.5
    wheel_speed_slew_rad_s2: 40.0
    contact_force_slew_n_s: 1200.0
    pd_gain_blend: cubic

参数命名要表达物理意义。

enter_leg_step_m 比 threshold1 好。

wheel_speed_slew_rad_s2 比 rate_limit 好。

单位必须写进键名或注释。

---

80.19 切换日志

每次模式切换都应记录：

字段	含义
time	切换时刻
from_mode	原模式
to_mode	目标模式
transition_mode	是否经过过渡
reason	触发原因
slope	坡度
roughness	粗糙度
step_height	台阶高度
confidence	地形置信度
base_velocity	切换前基座速度
pitch_roll	切换前姿态
health_flags	硬件健康状态

如果没有这些字段，调试会非常困难。

切换失败时，工程师需要知道是地形误判、硬件故障、驻留时间问题，还是过渡控制器不可行。

---

80.20 模式切换与 WBC 任务优先级

WBC 的任务栈随模式改变。

轮模式：

1. 动力学一致性。
2. 轮接触法向不穿透。
3. 横向无滑移。
4. 基座姿态。
5. 轮速跟踪。
6. 腿部姿态正则。

足模式：

1. 动力学一致性。
2. 支撑足零速度。
3. 摩擦锥。
4. 基座姿态。
5. 摆腿轨迹。
6. 关节姿态正则。

混合模式：

1. 动力学一致性。
2. 接触一致性。
3. 摩擦锥。
4. 横向滑移抑制。
5. 基座姿态。
6. 轮速或足端轨迹。

过渡模式：

1. 动力学一致性。
2. 速度投影约束。
3. 接触力限斜率。
4. 基座姿态。
5. 轮速限斜率。
6. 关节姿态插值。

陷阱框：不要只切换上层模式名称

如果上层进入足模式，但 WBC 仍在使用轮模式的横向滚动约束，控制器会互相打架。 如果 WBC 切到足模式，但硬件仍给轮电机速度命令，轮端会在支撑点处打滑。 模式切换必须同时更新 OCP、WBC 和硬件接口。

---

80.21 切换与硬件控制器

轮足系统常见硬件控制组合：

子系统	轮模式	足模式	过渡
轮电机	速度控制	阻尼或制动	限斜率到目标
髋关节	姿态跟踪	力矩/WBC	增益插值
膝关节	高度调节	足端轨迹	增益插值
IMU	状态估计	状态估计	提高故障检测
足端力	辅助监测	主接触估计	冲击检测

ros2_control 中，可以用控制器切换实现命令接口转换。

但控制器切换本身不是零时间。

因此推荐在单个硬件接口内部暴露统一命令结构：

struct WheelLegCommand {
  double leg_tau[12];
  double wheel_velocity[4];
  double wheel_tau_ff[4];
  double kp[12];
  double kd[12];
  int mode_id;
};

实时控制循环只发送一个结构。

模式逻辑在非实时线程更新目标。

实时线程只读取已经准备好的原子快照。

---

80.22 模式切换与状态估计

轮模式下，轮速里程计非常有用。

足模式下，足端静止接触约束非常有用。

混合模式下，两者都可能部分失效。

若状态估计器不知道当前模式，它会使用错误观测模型。

例如在轮模式下把接触点当成静止足端，会把真实滚动速度解释成滑移。

在足模式下把轮速积分当成里程计，会把轮子微小空转解释成底盘位移。

因此模式必须传给状态估计器。

观测模型可写成：

\[ y=h_m(x)+v \]

轮模式观测：

\[ y_{\mathrm{wheel}}=r\dot\phi-v_\parallel+\epsilon \]

足模式观测：

\[ y_{\mathrm{foot}}=J_c(q)\dot q+\epsilon \]

混合模式观测：

\[ y_{\mathrm{hybrid}}= \begin{bmatrix} w_r(r\dot\phi-v_\parallel)\\ w_fJ_c(q)\dot q \end{bmatrix} +\epsilon \]

权重 \(w_r,w_f\) 可以随地形置信度变化。

---

80.23 切换约束进入 MPC

一个完整的 MPC 目标可写为：

\[ \min_{x(\cdot),u(\cdot),m(\cdot)} \int_0^T \ell_{m(t)}(x,u,t)\,dt+\Phi(x(T)) \]

约束：

\[ \dot x=f_{m(t)}(x,u,t) \]

\[ g_{m(t)}(x,u,t)=0 \]

\[ h_{m(t)}(x,u,t)\ge 0 \]

如果同时优化离散模式 \(m(t)\)，问题变成混合整数最优控制。

实时轮足控制通常不直接这样做。

工程上更常见的是：

1. 高层 FSM 或策略给出未来 1-3 秒模式序列。
2. OCS2 根据这个序列求连续最优控制。
3. MRT/WBC 以高频跟踪输出。
4. 下一次 MPC 周期重新滚动。

这种做法把离散搜索和连续优化分离。

优点是实时。

缺点是模式序列可能不是全局最优。

---

80.24 为什么不用混合整数 MPC 直接求模式

混合整数 MPC 可以写得很漂亮。

但轮足系统维度高、约束多、频率要求高。

离散变量数量随时域长度增长。

若每个阶段有 3 个模式，预测步数 \(N=20\)，候选模式序列有：

\[ 3^{20}\approx 3.4\times 10^9 \]

当然求解器不会暴力枚举。

但组合复杂度仍然巨大。

100 Hz 控制频率下，几十毫秒内要求解全阶混合整数问题并不现实。

因此现代工程常用：

• 规则或学习策略决定模式。
• OCS2/SQP 解决连续轨迹。
• 安全层覆盖危险决策。

反事实推理：如果坚持用混合整数 MPC，可能得到更优模式序列，但求解超时会让机器人错过真实地形变化。 对真实机器人而言，晚来的最优解不如准时的可行解。

---

80.25 故障排查：模式抖动

现象：

• 模式在 WHEEL 和 HYBRID 之间频繁跳。
• 日志中每秒出现多次切换。
• 基座速度出现周期性掉速。
• MPC 求解时间变长。

常见原因：

• 单阈值无滞回。
• 高程图噪声未滤波。
• 驻留时间过短。
• 地形窗口太小。
• 语义分类置信度被当成硬标签。

处理步骤：

1. 绘制 step_height、roughness、slope 随时间曲线。
2. 标出每次切换时刻。
3. 检查特征是否在阈值附近来回穿越。
4. 增加进入/退出阈值差。
5. 增加最小驻留时间。
6. 对高程特征做时间中值滤波。
7. 将低置信度地形切到 HYBRID，而不是 WHEEL 和 LEG 之间摇摆。

---

80.26 故障排查：切换瞬间冲击

现象：

• 轮到足切换时机身点头。
• 轮胎发出摩擦声。
• 足端力峰值明显。
• 关节力矩出现尖峰。

常见原因：

• 轮速未降到安全范围。
• 新接触约束与当前速度不一致。
• 接触力从 0 突然跳到支撑力。
• PD 增益突变。
• 过渡时间太短。

处理步骤：

1. 检查切换前基座速度。
2. 检查轮速限斜率是否生效。
3. 检查接触力曲线是否连续。
4. 检查 WBC 约束激活是否与硬件命令同步。
5. 将过渡曲线从线性改成三次光滑函数。
6. 在 MPC 中加入过渡模式，而不是只在 WBC 里切。
7. 必要时降低切换前速度上限。

---

80.27 故障排查：模式选择过于保守

现象：

• 平地上仍长时间使用足模式。
• 速度上不去。
• 能耗明显高。
• 操作日志显示置信度经常低。

常见原因：

• 地形置信度阈值过高。
• 退出足模式阈值过低。
• 高程图窗口覆盖了机器人身后障碍。
• 语义分类把阴影当成障碍。
• 安全层错误禁用了轮模式。

处理步骤：

1. 分别记录规则层和安全层的输出。
2. 检查 blocked_modes 的原因。
3. 将高程图窗口限定在前进方向。
4. 对平地数据集统计特征分布。
5. 用分位数而不是经验猜测设阈值。
6. 设计测试场景逐项打开安全规则。

---

80.28 故障排查：模式选择过于激进

现象：

• 机器人在碎石或小台阶上仍高速滚动。
• 轮胎频繁打滑。
• 里程计误差增大。
• IMU 横滚角峰值变大。

常见原因：

• 只看坡度，不看粗糙度。
• 没有使用摩擦估计。
• 轮速上限没有随置信度降低。
• 训练环境缺少湿滑和软地面。
• 高层速度命令没有被模式层限幅。

处理步骤：

1. 添加滑移指标 \(\kappa\) 日志。
2. 统计轮速里程计和视觉/激光里程计差异。
3. 低置信区域自动限速。
4. 粗糙度高时优先进入 HYBRID。
5. 高层命令通过模式相关速度包络裁剪。

---

80.29 速度包络

不同模式应使用不同最大速度。

示例：

模式	最大线速度	最大角速度	最大加速度
WHEEL	2.5 m/s	1.5 rad/s	2.0 m/s²
HYBRID	1.0 m/s	1.0 rad/s	0.8 m/s²
LEG	0.5 m/s	0.6 rad/s	0.4 m/s²
TRANSITION	0.3 m/s	0.4 rad/s	0.3 m/s²

速度包络不是简单限幅。

它应随地形置信度连续变化：

\[ v_{\max}=v_{\min}+c_{\mathrm{terrain}}(v_{\max}^{nom}-v_{\min}) \]

当感知可靠时，允许接近标称速度。

当感知不可靠时，回到保守速度。

---

80.30 模式切换测试矩阵

测试不应只看“能否走过去”。

要覆盖触发、驻留、过渡和故障。

场景	期望模式	关键观测
平整走廊	WHEEL	无多余切换
低坡道	WHEEL 或 HYBRID	不打滑
高坡道	HYBRID	速度降低
10 cm 台阶	LEG	切换提前
碎石路	HYBRID	里程计误差受控
深度相机遮挡	HYBRID 或限速	不高速前冲
轮编码器丢失	LEG 或 FAULT	轮模式禁用
IMU 异常	FAULT	立即停机
台阶误检	保持原模式	滞回生效
阈值附近噪声	保持原模式	驻留生效

---

80.31 练习

练习 A：高程图 FSM

输入：局部高程图、目标速度、机器人当前模式。

输出：下一模式和切换原因。

要求：

1. 计算坡度、粗糙度、台阶高度、置信度。
2. 实现进入和退出阈值。
3. 实现最小驻留时间。
4. 记录每次切换原因。
5. 用三个地形序列测试：平地、坡道、台阶。

练习 B：过渡控制器

设计 WHEEL 到 LEG 的 0.5 s 过渡。

要求：

1. 轮速从当前值限斜率降到 0。
2. 接触力参考用三次曲线上升。
3. 腿部 PD 增益用三次曲线插值。
4. 新接触约束只在过渡后半段变硬。
5. 画出轮速、接触力、增益随时间曲线。

练习 C：学习门控与安全裁决

训练一个小型分类器或策略网络输出模式偏好。

要求：

1. 输入至少包括坡度、粗糙度、台阶高度、置信度、速度。
2. 输出三种模式概率。
3. 在输出后加入安全裁决层。
4. 构造一个轮编码器故障样本，验证安全层能禁用轮模式。
5. 对比纯策略和策略加安全层的失败次数。

练习 D：OCS2 模式表

把下面模式序列写成 eventTimes 和 modeSequence：

0.0-1.0 s  WHEEL
1.0-1.5 s  WHEEL_TO_LEG
1.5-3.0 s  LEG
3.0-3.6 s  LEG_TO_WHEEL
3.6-5.0 s  WHEEL

要求：

1. 给出整数编码。
2. 写出每段激活的约束。
3. 写出每段对应的 WBC 任务优先级。

---

80.32 累积项目：轮足模式切换小闭环

项目目标：在仿真中实现一个可解释、可记录、可回放的轮足模式切换系统。

平台可选：

• Go2-W 简化模型。
• B2-W 简化模型。
• Upkie 平衡轮腿模型。
• 自建二维轮腿模型。

模块分解：

1. 地形特征提取。
2. 模式评分。
3. 安全裁决。
4. FSM。
5. 过渡控制器。
6. 模式相关速度包络。
7. 日志与可视化。

交付要求：

1. 一段平地到台阶再回平地的仿真。
2. 一段坡道到碎石的仿真。
3. 一次传感器置信度下降实验。
4. 一份切换日志。
5. 一张模式时间线图。
6. 一张接触力时间曲线。
7. 一张轮速时间曲线。
8. 一页故障分析。

评分建议：

• 正确性：模式是否按地形合理切换。
• 平滑性：切换时速度和力是否连续。
• 稳定性：是否避免抖动。
• 可解释性：日志是否能解释每次切换。
• 安全性：故障是否进入安全状态。

---

80.33 本章速查

1. 模式切换改变的是控制问题定义，不只是行为标签。
2. 轮模式的核心是纯滚动和横向无滑移。
3. 足模式的核心是支撑接触点零速度。
4. 混合模式适合坡道、碎石和不确定地形。
5. 过渡模式用于平滑约束、力和控制接口。
6. 混杂系统由模式、流映射、保护条件和重置映射组成。
7. 单阈值会导致模式抖动。
8. 滞回需要进入阈值和退出阈值。
9. 驻留时间用于限制频繁切换。
10. 安全故障不应受普通驻留时间限制。
11. 高程图特征至少包括坡度、粗糙度和台阶高度。
12. 置信度低时应限速或进入保守模式。
13. 轮足模式必须传给状态估计器。
14. 错误观测模型会把滚动解释成滑移。
15. WBC 任务栈随模式变化。
16. 硬件控制器命令语义随模式变化。
17. OCS2 用模式表激活不同约束。
18. 直接优化离散模式会变成混合整数问题。
19. 工程上通常先决策模式，再优化连续控制。
20. 学习门控可以建议模式，安全层必须能否决。
21. 专家混合适合调权重，不适合直接混合硬约束。
22. 速度投影用于消除切换时的新约束违背。
23. 接触力插值应使用端点导数为零的曲线。
24. 轮速限斜率可以减少打滑和冲击。
25. 切换日志必须包含触发原因。
26. 模式切换测试要覆盖噪声、故障和误检。
27. 速度包络应随模式和感知置信度变化。
28. 过于保守会浪费轮足机器人的能效优势。
29. 过于激进会导致打滑和姿态峰值。
30. 真机部署前必须先做阈值附近噪声回放。

---

80.34 延伸阅读

材料	难度	阅读重点
Hybrid Systems: Modeling, Analysis, and Control	⭐⭐⭐	混杂自动机、保护条件、重置映射与稳定性分析
Underactuated Robotics 中的 hybrid systems 章节	⭐⭐	用低维动力学理解接触切换、碰撞和步态事件
OCS2 ModeSchedule 与 legged_robot 示例	⭐⭐⭐	离散模式如何激活不同动力学、约束和代价项
足式/120_步态管理与接触序列	⭐⭐	将足端接触表推广到轮 / 足 / 混合模式表
轮足机器人真机调试日志或公开演示复现	⭐⭐	从模式时间线、速度包络和安全裁决观察切换质量

---

80.35 调参清单

• 001 检查所有阈值是否带单位。
• 002 检查进入阈值是否大于退出阈值。
• 003 检查驻留时间是否随切换方向不同。
• 004 检查故障切换是否绕过驻留时间。
• 005 检查地形窗口是否位于机器人前方。
• 006 检查高程图空洞是否进入置信度计算。
• 007 检查坡度估计是否受车身姿态补偿。
• 008 检查粗糙度是否排除了离群点。
• 009 检查台阶高度是否使用分位数而非最大值。
• 010 检查低置信地形是否自动限速。
• 011 检查轮速命令是否限斜率。
• 012 检查腿部增益是否平滑插值。
• 013 检查接触力是否平滑插值。
• 014 检查 WBC 约束是否与当前模式一致。
• 015 检查 MPC 模式表是否覆盖整个预测时域。
• 016 检查过渡段是否有足够采样点。
• 017 检查状态估计器是否读取模式。
• 018 检查日志是否记录原模式和目标模式。
• 019 检查日志是否记录地形特征。
• 020 检查日志是否记录硬件健康标志。
• 021 检查安全层是否能禁用轮模式。
• 022 检查安全层是否能进入故障态。
• 023 检查故障态是否停止轮速命令。
• 024 检查恢复态是否要求人工确认或严格条件。
• 025 检查仿真地形是否覆盖阈值附近样本。
• 026 检查真机测试是否从低速开始。
• 027 检查模式切换频率是否有上限监测。
• 028 检查轮胎滑移指标是否记录。
• 029 检查里程计与视觉位姿差异是否记录。
• 030 检查电机温度是否参与安全规则。
• 031 检查电池电压是否影响最大加速度。
• 032 检查雨雪湿滑地形是否有保守策略。
• 033 检查软地面下陷是否影响轮半径估计。
• 034 检查轮半径参数是否可在线标定。
• 035 检查 IMU 延迟是否影响坡度判断。
• 036 检查相机延迟是否补偿到当前位姿。
• 037 检查语义地形标签是否只作为辅助。
• 038 检查策略网络输出是否经过安全裁决。
• 039 检查模式概率是否做温度或置信度处理。
• 040 检查异常输入是否保持安全默认模式。
• 041 检查 FAULT 是否可重复进入。
• 042 检查 RECOVERY 是否清除旧命令。
• 043 检查 STAND 是否能承接所有普通模式。
• 044 检查过渡完成条件是否不仅依赖时间。
• 045 检查过渡完成是否还检查速度误差。
• 046 检查过渡完成是否还检查接触力误差。
• 047 检查轮到足切换前是否降低基座速度。
• 048 检查足到轮切换前是否确认地面连续。
• 049 检查混合到轮切换前是否确认粗糙度下降。
• 050 检查轮到混合是否能快速响应坡道。
• 051 检查状态枚举是否稳定持久化。
• 052 检查模式 ID 是否与 OCS2 配置一致。
• 053 检查模式 ID 是否与日志解析工具一致。
• 054 检查模式 ID 是否与可视化颜色一致。
• 055 检查命令结构是否含模式字段。
• 056 检查硬件层是否拒绝不合法模式命令。
• 057 检查控制器切换是否原子化。
• 058 检查非实时线程是否不阻塞实时线程。
• 059 检查实时线程是否只读模式快照。
• 060 检查参数更新是否使用双缓冲。
• 061 检查切换规则是否支持回放调试。
• 062 检查每次切换是否能解释成某条规则。
• 063 检查学习策略失败时是否有手工模式。
• 064 检查手工模式是否仍受安全层限制。
• 065 检查远程急停是否优先级最高。
• 066 检查通信丢失是否进入安全状态。
• 067 检查模式抖动报警是否存在。
• 068 检查模式抖动报警阈值是否合理。
• 069 检查高层速度命令是否被模式包络裁剪。
• 070 检查包络裁剪是否记录原始命令和裁剪后命令。
• 071 检查足模式下轮电机是否进入阻尼或安全状态。
• 072 检查轮模式下足端力传感是否用于监测冲击。
• 073 检查混合模式下轮速和接触力是否不会互相抵消。
• 074 检查仿真接触参数是否覆盖真实摩擦范围。
• 075 检查测试报告是否包含失败场景。
• 076 检查阈值是否来自数据统计。
• 077 检查安全阈值是否留有硬件裕量。
• 078 检查过渡时间是否随速度自适应。
• 079 检查高速度下是否禁止直接轮到足切换。
• 080 检查姿态角过大时是否优先恢复平衡。
• 081 检查控制输入是否在切换前后连续。
• 082 检查期望加速度是否在切换前后受限。
• 083 检查摩擦锥约束是否按模式更新。
• 084 检查地形法向是否低通滤波。
• 085 检查法向突变是否触发保守模式。
• 086 检查台阶检测是否区分上台阶和下台阶。
• 087 检查下台阶是否降低轮速。
• 088 检查上台阶是否提前准备抬腿。
• 089 检查支撑足选择是否与轮速方向一致。
• 090 检查倒车时地形窗口是否切换方向。
• 091 检查转弯时左右轮地形是否分别评估。
• 092 检查单侧高障碍是否触发混合而非整体足式。
• 093 检查任务目标是否能限制模式选择。
• 094 检查载荷变化是否影响阈值和速度包络。
• 095 检查机械臂运动是否影响基座稳定规则。
• 096 检查轮足加臂时是否把载荷纳入安全层。
• 097 检查切换前后是否更新可视化 marker。
• 098 检查仿真和真机参数是否分文件管理。
• 099 检查参数文件是否版本化。
• 100 检查切换系统是否能在无外部感知时安全降级。

---

80.36 小结

本章把轮足模式切换从“写几个 if”提升为混杂系统设计问题。

核心结论很直接：模式切换改变约束集合，所以必须被 MPC、WBC、状态估计和硬件接口共同理解。

工程上推荐使用“评分或策略建议 + 安全裁决 + 滞回 FSM + 过渡控制器”的四层结构。

下一章将进入轮足 Sim-to-Real 与硬件集成，重点讨论轮胎接触、滑移、执行器带宽和真实硬件接口如何反过来影响模式切换阈值。

章末统一练习与故障排查

⚠️ 易错点一：只看单个指标。 100_模式切换 中的任何结论都应同时检查任务指标、物理约束和软件接口。只看总误差或总奖励，容易把模型错误误判为参数问题。

💡 易错点二：忽略坐标系和时间戳。 复合机器人控制链很长，坐标系、采样频率和延迟一旦没有显式记录，后续所有优化和学习结果都会失去解释力。

🧠 易错点三：把演示成功当成系统可靠。 教学实验应至少包含一次扰动、一次异常输入和一次日志回放分析，才能说明方法的边界。

练习

1. 选择本章一个核心公式，写出每一项的单位、坐标系和数据来源。
2. 选择本章一个代码片段，说明它依赖哪些配置项；如果配置错一个符号，会出现什么日志现象？
3. 设计一个只改变单个因素的实验，用来验证本章最关键的工程判断。

本质洞察：复合机器人文档中的公式、代码和项目不是三块孤立内容。公式定义可行边界，代码实现边界，项目用日志证明边界是否真实存在。

故障排查

症状	优先怀疑	验证动作
仿真正常但部署异常	观测、坐标系或时间戳不一致	用同一段日志离线回放训练端和部署端
指标突然变差	模式切换、限幅或安全壳触发	画出模式、保护标志和控制命令
调参没有效果	根因不是权重而是模型假设错误	回到最小实验，关闭无关模块
结果难以复现	配置没有版本化	保存模型哈希、配置哈希和随机种子