分层机器人：层与层之间，
到底是什么关系？
一台真正部署运行的机器人，身上同时跑着十几套算法：最高层切换任务，上层在不同决策阶段间选择， 中层算轨迹位姿，底层驱动电机，还有一圈永远在盯着的安全规约。它们运行在差好几个数量级的频率上。 自然的问题是：这些层之间到底是什么关系？上层凭什么、在什么时候、以什么方式影响下层？
具体一点：当我们说「上层给下层一个参考」，这个参考到底是个什么东西——它的数学是什么、代码是什么？ 上层会不会突然把下层「kill 掉」、切换到另一层？我怎么知道此刻该跑上层还是下层，难道只是「轮到它了」？ 控制权又是在什么时候、依据什么，从一套控制器交到另一套的？这篇就把这些问题逐一拆开， 再讲清楚交接怎么做才不出岔子，最后用腿足机器人、飞控、航天器三个真实控制栈看它们怎么落地。 预设读者：会基本控制、微积分、线性代数。
层之间不是轮流、不是谁 kill 谁——它们同时在跑。下层一直在算「发给电机的命令」， 上层一直在算「给下层的目标（参考）」，两者干的不是同一件事，所以不存在「现在跑哪个」。上层影响下层有四种方式 （设参考 / 设约束 / 选模式 / 抢占），其中只有最后一种「抢占」才是真正的夺权。控制权何时交接由四类触发决定 （时间 / 事件 / 值 / 安全）——每一类下面都单独讲、配代码与经典工作。
§1并发，不是轮流
先纠正一个最常见的误解。分层不像接力赛——不是上层跑一段、交棒、停下，再换下层跑。 机器人的各层是同时、持续地运行，每层跑在自己的频率上。最底层的电机电流环从不停，它一直在以上万赫兹算「该给电机多少电流」； 最高层的任务规划也从不停，它每秒重新盘算一次大局。它们干的根本不是同一件事，所以「现在该跑上层还是下层」是个伪问题——两个都在跑。
下层一直在算「发给执行器的命令」$u$（电机力矩/电流）。上层一直在算「给下层的目标」，也就是参考 $r$。 上层的输出正是下层的输入目标。所以上层不会「kill」下层——它只是不断地改下层要追的目标。 频率自上而下递增：上层慢（想得多、看得远），下层快（反应快、贴着电机）。
最朴素的例子是级联控制：一个伺服里套着位置环、速度环、电流环，三个环同时在跑。外层位置环不去碰电机，它只算一个 「期望速度」交给速度环；速度环再算一个「期望力矩」交给电流环；只有最内的电流环真正驱动 PWM。外环慢、内环快：
$$ v_{\text{ref}} = K_p^{\text{pos}}(x_d - x), \qquad \tau_{\text{ref}} = K_p^{\text{vel}}(v_{\text{ref}} - v), \qquad f_{\text{pos}} < f_{\text{vel}} < f_{\text{cur}} $$推广到整台机器人：第 $k$ 层从上方收到参考 $r_k$，以频率 $f_k$ 运行，把它细化成给下层的参考 $r_{k+1}$；最底层第 $n$ 层产出执行器命令 $u$。向上则汇报状态与事件。
$$ r_{k+1} = \pi_k(x,\, r_k), \qquad u = \pi_n(x,\, r_n), \qquad f_1 < f_2 < \cdots < f_n $$全篇的地图。每个方框是一个一直在跑的循环，频率自上而下递增。橙箭头是参考下传（上层给下层设目标）， 青箭头是事件/状态上传（下层把结果与异常报上去）。第一部讲这两条箭头到底是什么；第二部讲交接的时机；第三部看经典架构；第四部讲交接的工程；第五部看真实系统。
为什么内环要比外环快？工程铁律：内环带宽约为外环的 5–10 倍。够快时外环可以把内环近似当成「瞬间完成」， 外环设计大大简化，而且扰动会在传到上层之前就被快内环吃掉。下面的演示就是这一点。
§2什么是"参考"
既然上层影响下层的主要方式是「给一个参考」，那就得把参考讲清楚。一句话：参考 $r$ 是相邻两层之间的接口变量，是下层的「期望值 / 目标」。 上层的输出就是下层的参考；下层的全部职责，就是让现实去逼近这个参考。
关键在于：参考的「类型」随层而变，越往下越具体、维度越低、越贴近物理。所谓「分解」，就是把抽象参考一层层翻译成具体参考：
按「参考是个什么形状」，可分成五类——前两类最常见：
| 参考的类型 | 是什么 | 下层拿它干嘛 | 例子 |
|---|---|---|---|
| 设定点 setpoint | 一个要保持的目标值（常数） | 把误差 $r-y$ 调到 0 | 期望速度 $v_{\text{ref}}$ |
| 轨迹 trajectory | 时间索引的参考 $r(t)$，一条要跟的曲线 | 跟踪整条曲线 | 期望末端轨迹 $x_{\text{des}}(t)$ |
| 目标 / 代价 objective | 一个要最小化的代价，不是单点 | 自己优化出动作 | MPC 把"期望力"编码成跟踪项 |
| 约束集 constraint | 一个"不许出去"的可行域 | 在域内自由，只是不越界 | 安全包络、keep-out |
| 模式 / 符号 mode | 一个离散选择 | 切到被指定的行为 | "现在跑抓取技能" |
参考的数学：它就是下层自己那个控制问题里的「期望值」。下层要么用一条反馈律把误差消掉，要么把参考当成优化目标去最小化：
$$ u = \kappa(x,\, r)\quad(\text{e.g. } u = K(r - y)), \qquad u = \operatorname*{arg\,min}_{u}\ \lVert y(x,u) - r\rVert^2 + \lambda\lVert u\rVert^2 $$参考的代码：下面这段把「上层何时动作」也一并讲清楚了——上层在它自己的时钟上周期性地刷新参考， 下层则远快于它、持续地用当前保持着的那个参考（零阶保持 ZOH）。所谓「上层做了一次动作」，具体就是「刷新了一次参考」。
# 上层(慢) 产出参考 r；下层(快) 用 r。两者并发，r 在更新之间保持不变(ZOH)
r = upper.initial_reference() # 当前生效的参考
while True: # 下层主循环：每 dt 一次，远快于上层
y = sense()
u = K @ (r - y) # 参考进入误差： desired - actual
actuate(u) # 只有下层真正驱动执行器
if clock.tick(period=T_upper): # "轮到上层了"——它的时钟到点
r = upper.update(state) # 刷新参考 = 上层的一次"动作"
§3上层决定下层的四种方式
「设参考」是最主要、但不是唯一的耦合方式。把上层影响下层的全部手段摊开，一共四种。前三种里上层都没有「杀掉」下层—— 下层始终在算执行器命令，上层只是改它的目标 / 边界 / 模式；只有第四种「抢占」才是真正把控制权从一套控制器抢到另一套。
| 耦合方式 | 上层给下层的是 | 下层还自由吗 | 是"夺权"吗 | 例子 |
|---|---|---|---|---|
| 设参考 reference | 一个目标值 / 轨迹 | 否，必须追踪它 | 否（下层一直在跑） | 级联、MPC→WBC、制导→控制 |
| 设约束 constraint | 一个可行域 / 边界 | 是，域内自由 | 否 | 安全包络、参考调速器、CBF |
| 选模式 mode | 一个离散选择 / 激活哪个技能 | 否，按所选模式 | 否（命令下层切，非夺权） | 3T executive、行为树、自驾仪选模式 |
| 抢占 preemption | 直接接管执行器 | — | 是（唯一真正夺权） | subsumption 抑制、Simplex 安全切换 |
正常情况下不会。设参考 / 设约束 / 选模式，下层都一直活着、一直在算执行器命令，上层只是改它追的东西。 真正的「夺权」只发生在抢占这一种，而且它通常是同一层级上两套控制器之间的切换（比如「性能控制器」和「安全控制器」都在控制层，安全的把性能的换下来）， 或由优先级最高的安全层发起——不是「上层 kill 下层」，而是「更该信的那套把不该信的那套顶替掉」。
知道了上层通过哪四种方式影响下层（§3），还剩一个问题：每一次「上层动作」或「控制权交接」，到底在什么时候、依据什么发生？答案有四类触发。 你之前的猜测「是不是只是轮到它了」——对时间触发确实是，但另外三类不是。前三类（时间 / 事件 / 安全）直觉上好懂，唯独值触发最微妙、也最常用在机器人的运动/行为仲裁上，所以单独展开。先用一张图对比最常见的两类：
§4时间触发：到点就轮到它
判据：纯时钟驱动，第 $i$ 个任务每 $\Delta t_i = 1/f_i$ 秒跑一次，与状态无关。这就是你猜的「轮到它了」——但请注意它只适用于这类周期刷新，不是所有交接都这样。 确定性来自时间表，不来自数据。一个直接推论是零阶保持（ZOH）：在两次刷新之间命令保持不变，$u(t)=u(t_k),\ t\in[t_k,t_{k+1})$——这是每个数字控制回路的根本假设。
经典工作与工程：
- 循环执行体（cyclic executive，Baker & Shaw 1988）：一张静态调度表把「大周期」切成若干「小周期」，每个时隙固定跑哪些任务。运行时没有调度器、不抢占，只按帧计数查表——极其确定、易认证，但僵硬难改。
- 时间触发架构（TTA，Kopetz）：所有计算/通信/采样都由一个全局同步时钟驱动，节点按预分配的 TDMA 时隙发送。完全先验确定，因此能过 DO-178C / ISO 26262 认证。部署：FlexRay（汽车底盘/动力）、TTEthernet（NASA Orion 飞船航电）、TTP（A380 子系统）。
- 速率单调调度（RMS，Liu & Layland 1973）：周期越短优先级越高；可调度的充分利用率界 $U=\sum_i C_i/T_i \le n(2^{1/n}-1) \to \ln 2 \approx 0.693$。
- ros2_control：controller-manager 跑一个固定速率（常见 1 kHz）的
read → update → write回路——活生生的时间触发控制。
# 固定速率回路（ros2_control 风格）：决策是"到点了"，与状态无关
T = 1.0 / 1000.0 # 1 kHz 周期
next_t = now()
while running:
x = read_state()
u = controller.update(x, T) # 每周期更新一次
write_command(u)
next_t += T
sleep_until(next_t) # 用绝对截止时刻，避免漂移
§5事件触发：条件成真才切
判据：当一个状态谓词 $g(x)$ 变真时才动；事件之间时钟无关紧要。$g$ 可以是「到达子目标 $\lVert x-x_{\text{goal}}\rVert<\epsilon$」「检测到接触 $\hat f>F_{\text{th}}$」「误差超阈」「电量低」等。 FSM 的转移、行为树的 condition 节点（每 tick 重判），都是这类。
控制理论里有一个专门的分支把它形式化：事件触发控制（event-triggered control，Tabuada 2007）。把控制 $u=K\hat x$ 锁在上次采样的状态 $\hat x$ 上，定义测量误差 $e=\hat x - x$， 只在误差涨到与状态成比例的阈值时才重新采样、重算控制：
$$ \lVert e\rVert \ge \sigma\,\lVert x\rVert, \qquad \sigma \in (0,1) $$每次触发把 $e$ 清零。这保证 Lyapunov 函数持续下降（系统稳定），同时把「周期执行」换成「按需执行」，省下大量算力与通信——网络化控制、多智能体一致性里特别有用。 它还有个预测变种自触发控制（self-triggered，Anta & Tabuada 2010）：每次更新时预先算出下一次更新时刻 $t_{k+1}=t_k+\tau(x(t_k))$，连持续监测都省了。
# Tabuada 事件触发：只在 ‖e‖ ≥ σ‖x‖ 时才重算控制，其余时刻 ZOH 保持
sigma = 0.2 # 设计常数 ∈ (0,1)
x_hat = read_state() # 上次采样的状态
u = K @ x_hat # 保持的控制
while running:
x = read_state()
e = x_hat - x # 自上次更新以来的测量误差
if norm(e) >= sigma * norm(x): # 触发条件被破坏
x_hat = x # 重新采样 → e 清零
u = K @ x_hat # 重算控制
write_command(u) # 否则：保持上一个 u（ZOH）
其余事件源还有：硬件中断（编码器边沿）、看门狗超时 / deadline 错过（当成故障事件，见 §16）、以及 ROS action 的 goal/feedback/preemption——都是异步的事件触发。
§6值触发：给候选打分，选最优的那个（cost）
这是最微妙、也是机器人在运动/行为层最常用的触发。它不靠离散事件，而是每个周期给一组候选动作/轨迹打一个标量代价（或效用），取最优的那个：
$$ a^{\star} = \operatorname*{arg\,min}_{a\in A}\ \sum_i w_i\, c_i(a) \qquad\Big(\equiv \operatorname*{arg\,max}_{a\in A}\ \textstyle\sum_i w_i v_i(a),\ \ v_i=-c_i\Big) $$（右边是等价的效用 / 投票形式，令 $v_i=-c_i$ 即可。）
妙处在于：没有 if 语句。被选中的动作随代价地形变化而自动翻转——障碍一出现，绕行轨迹的代价跌破直行轨迹，机器人就自己绕开了，看起来像「触发了避障」，其实只是 argmin 的结果变了。
经典例子：ROS 的 DWA 局部规划器
这正是很多人记忆里那套「每一步选 cost 最低的行动」的设计。Dynamic Window Approach（Fox, Burgard, Thrun 1997；ROS dwa_local_planner）在动态窗口
（一个周期内由加减速可达的 $(v,\omega)$ 集合）里采样，对每个样本前向仿真一小段轨迹，按三项加权代价打分，取最低：
$d_{\text{path}}$ 是轨迹末端到全局路径的距离，$d_{\text{goal}}$ 是到局部目标的距离，$c_{\text{obs}}$ 是沿途查 costmap 的最大障碍代价（撞了即 $\infty$，直接淘汰）。
ROS 的经典配置权重是 path_distance_bias=32.0、goal_distance_bias=24.0、occdist_scale=0.01（脚注：包里 .cfg 的裸默认是 0.6/0.8/0.01，而教程/TurtleBot 配置用 32/24/0.01——后者是大家实际在用的）。
其中 $c_{\text{obs}}$ 来自 costmap 的膨胀层，从致命障碍向外指数衰减 $\text{cost}(d)=252\cdot\exp(-k\,(d-r_{\text{inscribed}}))$（系数 $252=253-1$）。
# DWA：采样动态窗口里的 (v, ω)，前向仿真打分，取 cost 最低（ROS 经典 32/24/0.01）
def dwa_pick(state, candidates, costmap):
B_PATH, B_GOAL, B_OCC = 32.0, 24.0, 0.01
best, best_cost = None, float("inf")
for v, w in candidates: # 动态窗口内的速度对
traj = rollout(state, v, w) # 前向仿真一小段轨迹
cost = (B_PATH * dist_to_path(traj, state.path)
+ B_GOAL * dist_to_goal(traj, state.goal)
+ B_OCC * max_obstacle_cost(traj, costmap)) # 撞了即 inf 淘汰
if cost < best_cost: best_cost, best = cost, (v, w)
return best # argmin：选 cost 最低
goal_bias 机器人变得「目标饥渴」贴着障碍走；调大 occdist_scale 它变胆小绕更远；点「移动障碍」看最优弧自动改变——这就是值触发：调权重＝调性格，没有一行 if。
值触发的一大家族
把候选、代价、权重换成不同的东西，就得到一整族工程方法——它们本质都是「打分取最优」：
| 方法 | 候选是什么 | 代价 / 选择 | 部署 |
|---|---|---|---|
| DWB(Nav2) | 动态窗口 $(v,\omega)$ | 可插拔 critic 的加权和（PathAlign/GoalAlign/Obstacle…）取 min | ROS2 Nav2 |
| TEB(Rösmann) | 整条带（位姿+时间间隔） | 多目标加权最小二乘(时间+离障+运动学)，g2o 优化 | ROS 局部规划 |
| MPC | 未来一段控制序列 | 滚动时域 argmin 阶段代价 s.t. 约束，执行第一步 | AV、腿足、无人机 |
| MPPI(Williams) | $K$ 条带噪控制序列 | 按 $w_k\propto e^{-S(\tau_k)/\lambda}$ 软加权（软 argmin） | AutoRally、Nav2 MPPI |
| Frenet 采样(Werling 2010) | 横/纵多项式候选轨迹 | 代价 = jerk积分 + 时间 + 终端偏差，取 min | 自动驾驶（Apollo EM 同理） |
| DAMN(Rosenblatt) | 离散转向命令 | 行为投票加权和 $V(c)=\sum w_i v_i(c)$ 取 argmax | CMU Navlab |
| 势场 / motor schema | (连续)运动方向 | 代价场负梯度 / 加权向量和（有局部极小） | 机械臂避障、早期移动机器人 |
| 加权 QP(软优先级) | 关节加速度 | $\min\sum w_i\lVert J_i\ddot q+\dot J_i\dot q-\ddot x_i^*\rVert^2$，权重决定冲突时谁赢 | 人形/四足 WBC |
| 抓取选择 | 候选抓取位姿 | 抓取质量(force-closure ε-度量 / Dex-Net / GraspNet 打分) 取 argmax | 分拣/拣选产线 |
| 落脚点选择 | 候选落脚点 | 地形代价图(坡度/粗糙/可达)取 min | ANYmal / 四足 |
| 效用 AI(IAUS) | 候选行为 | 各行为按效用曲线打分取 argmax | 商业游戏 AI |
值触发是「软」的：用权重把多个目标揉进一个标量代价，谁权重大谁在冲突里赢，低优先目标会优雅退化而非被清零。 与之相对的是 §10 / 控制层的硬优先级（零空间投影、严格分层 QP）：高优先任务一分不让，低优先只在其零空间里做。 两者都在「协调多个目标」，但一个用打分排序、一个用线性代数投影。
再往外，RL 的贪心策略 $a^\star=\operatorname*{arg\,max}_a Q(s,a)$、anytime 规划（A*/D* Lite 最小化 $g+h$、代价变了就重规划）、交叉熵法 CEM（采样-保留低代价精英-重拟合分布）都属于同一家族：定义标量代价、重打分、取最优。 值触发是机器人在运动/行为层仲裁竞争选项最通用的方式。
§7安全触发：快越界才接管
判据：当状态即将离开安全集时才触发——不是时钟、不是一般 guard，而是「即将违约」这个特殊谓词。它通常发起一次抢占（§3）。几种形态：
- Simplex（Sha 2001）：当一步前瞻的状态要离开可恢复集时切到安全控制器，判据 $x^{+}=f(x,u_{\text{complex}})\notin\mathcal R$。
- 控制屏障函数 CBF（Ames 等 2019）：QP 滤波器只在屏障约束 $L_f h + L_g h\,u + \alpha(h)\ge 0$ 将被违反、即 $h(x)\to 0$ 时才介入。
- HJ 可达性（Bansal/Tomlin）：在后向可达不安全集的边界 $V(x)=0$ 处触发「最不保守」的安全控制。
- 运行时保障 RTA：一个可信监视器盯着不可信（常是学出来的）控制器，即将违约就 monitor-and-switch（NASA/AFRL，ASTM F3269 标准）。
# 安全触发：一步前瞻，若预测状态要出可恢复集就接管（Simplex / RTA 形态）
def safety_trigger(x, u_complex, dt):
x_next = predict(x, u_complex, dt) # 一步前瞻
return x_next not in R # R = 可恢复 / 安全集
# CBF 变体：当屏障约束将被违反（h(x) → 0）时介入
# HJ 变体：当 V(x) ≤ 0（到达后向可达边界）时介入
这一类「该切」之后，前向夺权与反向交还的完整工程在 §15（Simplex）讲。下面先把所有切换「切得干净」的工程补齐。
§8Subsumption：抢占式耦合的极致
Brooks 1986（AI Memo 864）的反应式分层，是 §3 里「抢占」这种耦合的纯粹形态。 它按能力切层（0 避碰、1 漫游、2 探索……），每层是一张增广有限状态机（AFSM）网络，把传感直接连到动作。 协调靠两种节点接在导线上：抑制器（suppressor）在一段时间内用高层信号替换低层模块的输入线； 压制器（inhibitor）在一段时间内掐断低层模块的输出线。所以「explore」要拐弯时，把信号灌到「wander」的输入线上盖过它； 而最底层的「avoid」始终在跑、随时能压制一切——高层 subsume（接管）低层，低层永不下线，作安全兜底。这里没有中央世界模型，「世界就是它自己最好的模型」。
§9三层架构 3T：「选模式」耦合的范本
Gat 1998（On Three-Layer Architectures）按内部状态的性质切三层： Controller（无状态反应技能，传感器速率，每次迭代必须有界时间，且要「能认错地失败」cognizant failure）、 Deliberator（规划/搜索，时间不设上限）、以及夹在中间、最容易被忽视的 Sequencer / executive。 executive 正是 §3 里「选模式」耦合的执行者：它做条件式排序（RAPs / ESL / PRS 这类语言）， 按当前情境激活 / 停用一组 controller 里的技能、给它们填参数、监视成功/失败并处理意外，且每个决策都必须在有界时间内给出。 它对 deliberator 的使用是异步的：「在调用规划器到它出结果这段时间里，可以发生好几次技能切换」——慢规划永远不坐进快回路，executive 拿到结果再择机并入。 「为什么恰好三层」：状态只有无 / 关于过去 / 关于未来三种。
§10RCS / 4D-RCS：「设参考」逐层分解的极致
Albus 在 NIST 做了几十年的 RCS，及其无人车版 4D-RCS（NISTIR 6910）， 是 §2「参考逐层分解」这件事做得最完整的范例——而且真在美军 Demo III 自主车队和 NIST 工厂里跑过。 整摞由结构相同的控制节点堆成，每个节点都含同样的四件套：SP（感知处理：把约 10 个子事件归并成 1 个往上送）、 WM（世界模型：预测、回答查询）、VJ（价值判断：算代价/收益）、 BG（行为生成：把上层给的一条任务命令分解成约 10 条子命令发给下层，每隔自己周期的约 1/10 重规划一次）。 于是命令向下走是「参考的分解」，事件向上走是「聚合」，相邻层的时空尺度天然差约 10 倍——这正是 §2 那张抽象阶梯的工业化版本。
| 4D-RCS 层级（单车） | 规划视野 | 重规划周期 | 地图范围/分辨率 |
|---|---|---|---|
| Servo 伺服 | 50 ms | 5 ms | — |
| Primitive 基元 | 500 ms | 50 ms | — |
| Subsystem 子系统（含避障） | 5 s | 500 ms | ~500 m / 4 m |
| Vehicle 整车战术 | 1 min | 5 s | — |
| Section / Platoon 多车 | 10 min – 小时 | 1 – 5 min | 2–10 km / 30 m |
到这里，「层之间是什么关系、何时依据什么交接」已经清楚了。但决定了「该切」之后，切的那一瞬间还得做干净——否则命令会跳、模式会抖、安全切换会反复横跳。这是第四部。
§11无扰切换 bumpless：切控制器时怎么不跳
这是「交接」里最经典的工程细节。控制器 A 正在控、控制器 B 待命，某一刻要从 A 切到 B。 问题：B 的内部状态（尤其是积分器）是陈旧的——它没在控，积分项停在某个老值。切过去的瞬间，B 的输出 $u_B = K_p e + I_B$ 和 A 刚才的输出 $u_A$ 对不上， 执行器命令瞬间跳一下。这一跳轻则颠簸，重则触发限位、打坏齿轮、激起振荡。
机器人的执行器是有惯量、有限幅的物理设备，一个阶跃式的命令跳变会激起高频暂态、可能瞬间饱和。 切换在控制系统里无处不在（手动↔自动、自驾仪换模式、性能↔安全、摆动↔触地），每一次切换都是一个潜在的冲击源，所以「无扰」是安全攸关的基本功。
标准解法：让待命的控制器一直「跟踪」实际施加的命令，这样切换时它的输出早已对齐。 做法是在待命控制器的积分器上加一条回算（back-calculation）项——也就是 anti-windup 里那条「跟踪」反馈：
$$ u_c = K_p\,e + I, \qquad \dot I = K_i\,e + K_t\,\big(u_{\text{applied}} - u_c\big) $$当这个控制器没在控时，$u_{\text{applied}}$ 是另一个控制器实际施加的命令；$K_t(u_{\text{applied}}-u_c)$ 这一项把它的积分器拽到「使自己输出 = 实际命令」的值上。 切换那一刻，$u_c \approx u_{\text{applied}}$，命令连续。工业上还有两个等价做法：手动→自动切换时直接反算积分项 $I \leftarrow u^- - K_p e$；以及用 增量式（velocity-form）PID——它输出 $\Delta u$ 而非 $u$，天生没有「绝对积分状态」要对齐，白送无扰切换。
# 带回算跟踪的 PI：待命时跟踪实际施加的 u，切换处输出连续（无扰）
def pi_update(c, e, u_applied, dt, active):
Kp, Ki, Kt = c["Kp"], c["Ki"], c["Kt"]
u_self = Kp * e + c["I"] # 本控制器"想"输出的
if active:
c["I"] += Ki * e * dt # 在控：正常积分
return u_self
else:
# 待命：回算项把积分器拽向"使自己输出=实际命令"，对齐到切换那一刻
c["I"] += (Ki * e + Kt * (u_applied - u_self)) * dt
return u_self # ≈ u_applied，切过去不跳
§12防抖：迟滞带 + 驻留时间
第二个交接难点：抖动（chattering）。如果切换判据是「信号过阈值就切」，而信号正好在阈值附近徘徊（带噪声），系统就会在两个模式间疯狂来回切。两个标准武器：
迟滞带（hysteresis）——升和降用不同阈值：高于 $\theta + b/2$ 才切上去，低于 $\theta - b/2$ 才切下来，中间「保持」。就是施密特触发器/恒温器的原理：
$$ \text{mode} = \begin{cases} \text{ON}, & s > \theta + b/2 \\ \text{OFF}, & s < \theta - b/2 \\ \text{hold}, & \text{otherwise} \end{cases} $$驻留时间（dwell time）——切换后强制至少停留 $\tau_d$ 才允许再切。它还有更深的理由：在切换系统理论里，即使每个模式单独都稳定，切太快也可能整体发散。 Hespanha–Morse 的平均驻留时间定理给出稳定的充分条件——只要切换次数 $N_\sigma(t)$ 不太密：
$$ N_\sigma(t) \le N_0 + \frac{t}{\tau_a}, \qquad \tau_a > \frac{\ln \mu}{\lambda_0} $$其中 $\mu$ 是切换时 Lyapunov 函数的跳变因子、$\lambda_0$ 是每个模式的衰减率。直觉：每次切换都「亏」一点能量，你得给系统足够时间把它赚回来；切得太密，亏空来不及补，就垮了。
# 迟滞带 + 最小驻留时间：消除阈值附近的抖动
def hysteresis_dwell(state, s, theta, band, dwell, since_switch):
if since_switch < dwell: # 驻留期内：锁定，不准切
return state, since_switch
if not state and s > theta + band/2: # 升：高阈值
return True, 0.0
if state and s < theta - band/2: # 降：低阈值（迟滞）
return False, 0.0
return state, since_switch # 否则保持
§13增益调度：把「切」做成「连续地变」
有时候根本不想「切」，而想连续地从一套控制器过渡到另一套。增益调度（gain scheduling）就是这招，也是飞控的看家本领： 在若干工作点（不同空速、马赫数、关节角、负载）各设计一套线性控制器，再按一个慢变的调度变量 $\sigma$ 在它们之间插值：
$$ K(\sigma) = \sum_i w_i(\sigma)\,K_i, \qquad K = (1-\beta)\,K_i + \beta\,K_{i+1} $$（右式是在相邻两个工作点之间线性插值。）关键工程注意点：(1) 「冻结时间」假设——调度变量必须比闭环动态慢得多，控制器才能近似认为自己工作在一个准静态的工作点上； (2) 隐藏耦合（hidden coupling）——调度变量随状态变化会引入设计时没考虑的项，是经典坑；(3) 调度变量要选慢变、可测、能代表工况的量。 飞机从起飞到巡航，动压 $\bar q = \tfrac12 \rho V^2$ 变好几个量级，全靠增益调度把一套套线性律平滑接起来。
§14混杂自动机 / 监督器：把所有切换收进一个框架
前面这些切换可以统一成一个混杂自动机（hybrid automaton）：系统由若干模式组成，每个模式有自己的连续动态； 模式之间有卫式条件（guard）触发跳转，跳转时有复位（reset）规定连续状态怎么重新初始化。坐在这些连续控制器之上的，是一个监督器 / 模式管理器，它只管「现在该哪个模式当家」。
guard 就是 §4–§7 的「何时交接」的判据；reset 就是 §11 的无扰切换（复位积分器让命令连续）；选哪个 mode 就是 §3 的「选模式」耦合。 而给 guard 配上迟滞带和驻留时间（§12），是为了防止 Zeno 现象——无穷多次切换挤在有限时间里，把系统锁死。一句话：监督器 = 「何时切（§4–7）+ 切哪个（§3）+ 切得干净（§11/§12）」。
操作类机器人里还有一个漂亮的「选择式」切换：混合力/位控制（Raibert & Craig 1981）。用一个对角的选择矩阵 $S$，逐任务空间方向决定这一维控位置还是控力——插孔时 $xy$ 控位置、$z$ 控力：
$$ \tau = J^\top \big[\, S\,f_{\text{pos}} + (I - S)\,f_{\text{force}} \,\big] $$§15Simplex 安全切换：什么时候夺权，什么时候敢交还
最关键的一种抢占：安全。你想用一个高性能但没法验证的复杂控制器（可能是学出来的、可能有 bug），又必须保证不出事。 Lui Sha 的 Simplex 架构（IEEE Software 2001）给出三件套：高性能复杂控制器、简单且形式验证过的安全控制器、以及一个决策模块。
决策模块的核心是一个可恢复集 $\mathcal R$——从这些状态出发，安全控制器仍能把系统永远保持在约束内。它常取安全控制器稳定域的一个 Lyapunov 子水平集：
$$ \mathcal{R} = \{\,x : x^\top P x \le c\,\} $$前向切换（复杂→安全）用 §7 的安全触发：一步前瞻预测复杂控制器这一步会把状态带到 $x^{+}$，若 $x^{+}$ 要离开 $\mathcal R$ 就夺权刹车。 真正微妙、也最少被讲的是反向切换（安全→复杂）：什么时候敢把控制权还回去？不能一回到 $\mathcal R$ 就还——那会在边界上反复横跳（正是 §12 的抖动）。 得等状态深入到一个更靠里的内区 $\mathcal R_{\text{in}}$、并保持一段驻留时间，才交还：
$$ u = \begin{cases} u_{\text{complex}}, & x^{+}(u_{\text{complex}}) \in \mathcal{R} \\ u_{\text{safe}}, & \text{otherwise} \end{cases} \qquad \text{hand back iff}\ \ x \in \mathcal{R}_{\text{in}} \subset \mathcal{R}\ \text{and dwell} > \tau_d $$# Simplex 决策模块：前向夺权 + 带迟滞/驻留的反向交还
def simplex_decision(x, u_complex, model, dt, S):
x_next = model.predict(x, u_complex, dt) # 一步前瞻
if S["mode"] == "complex":
if not in_recoverable(x_next, margin=0.0): # 要出可恢复集
S["mode"], S["dwell"] = "safe", 0.0 # 前向：夺权
else: # safe
if in_recoverable(x, margin=BIG): # 深入内区 R_in（更大裕度）
S["dwell"] += dt
if S["dwell"] > TAU_D: # 且保持足够久
S["mode"] = "complex" # 反向：才敢交还
else:
S["dwell"] = 0.0
return u_complex if S["mode"] == "complex" else safety_controller(x)
§16失效切换：看门狗 → 安全态、降级链
最后一种交接是出错时的。Simplex 假设状态可测、模型可信；但传感器会坏、回路会错过 deadline、算力会爆。工程上的标准做法是一条降级链（graceful degradation）：
- 看门狗（watchdog）：一个独立计时器，控制回路每个周期「喂」它一次；一旦回路卡死、错过 deadline，看门狗超时触发，立刻切到安全态（保持位姿、刹车、急停）。这是「实时回路」与「安全」之间的硬连接，也是 §5 事件触发的一种。
- 分级控制律降级：空客电传飞控就是教科书例子——传感器/计算逐步失效时，控制律从 Normal Law（带包络保护）降到 Alternate、Direct、最后 Mechanical Backup，每一级都比上一级简单、保护更少、但更可信。
- 安全模式（safe mode）：航天器的终极兜底——一旦姿态/健康异常，自主切到「太阳指向安全模式」，只保证活着（对日充电、稳住姿态、等地面指令），放弃任务。它就是 Simplex 安全控制器在系统级的化身。
§17腿足机器人：gait FSM → 凸 MPC → WBC
把前面所有概念拼起来，看一个真实、开源、跑得很好的机器人控制栈：MIT Cheetah 的腿足控制 （Di Carlo 2018 凸 MPC + Kim 2019 WBIC）。 腿足是个欠驱动的混杂系统：只能通过触地的脚对世界施力，而哪些脚触地是离散变化的。先按本文的词汇点明它的关系：
| 层间接口 | 参考是什么（§2） | 耦合方式（§3） | 触发（§4–7） |
|---|---|---|---|
| 步态调度器 → MPC | 接触调度 $c_i(k)$ + 期望体速 | 设参考 + 选模式(stance/swing) | 时间 + 接触事件 |
| MPC → WBC | 期望地面力 $f^*$ + 期望体姿 | 设参考（目标/代价，值触发） | 时间（~30–50 Hz 刷新） |
| WBC → 关节 | 期望力矩 $\tau_{\text{ff}}$ + $q_{\text{des}}$ | 设参考 | 时间（~1 kHz） |
为什么这样分层：MPC 解一个带摩擦锥约束的凸 QP，几毫秒一次，只能跑到几十 Hz、且用粗糙的单刚体模型——它的价值是预view（未来 0.5 秒力该怎么分配）； WBC 没有预view、但在完整全身模型上 1 kHz 跟踪——它的价值是realize（把那些力在真实动力学上实现、跟踪摆动脚）。二者按频率拆开、各取所长。MPC 本身就是一个值触发器（§6），大致长这样（接触调度通过「摆动脚出力=0」这个约束进入优化）：
$$ \min_{x,\,f}\ \sum_{k=0}^{N-1} \lVert x_{k+1}-x_{k+1}^{\text{ref}}\rVert_{Q}^{2} + \lVert f_k\rVert_{R}^{2} \quad\text{s.t.}\ \ x_{k+1}=A_k x_k + B_k f_k,\ \ |f_{i,xy}|\le \mu f_{i,z},\ \ f_i = 0\ \text{if } c_i=0 $$步态调度器给每条腿一个相位 $\varphi_i$，决定它 stance（触地）还是 swing（摆动）——这是一个纯事件触发的接触切换：
$$ \varphi_i(t) = \big(t/T + \theta_i\big) \bmod 1, \qquad c_i = \mathbb{1}\big[\varphi_i < d\big] $$不同步态只是不同的相位偏移 $\theta_i$ 和占空比 $d$。摆动脚落哪用 Raibert 落脚点启发式（落点 = 髋投影 + 半个支撑期的位移 + 速度反馈修正）：
$$ p_{\text{foot}} = p_{\text{hip}} + \frac{T_{\text{stance}}}{2}\,v + k\,(v - v_{\text{des}}) $$纯时间表假设地面正好在预测位置——它从来不在。脚提前触地（上坡、碎石），还当它在摆动就往地里砸；脚延迟触地（下坡、缺口），机器人以为这条腿在支撑、开始加载，结果腿下是空的。 所以时间触发必须叠加事件触发：从动力学反推地面力 $\hat f$ 过阈值、或接触传感器、或概率接触估计。提前触地就把腿立刻锁为 stance（相位前跳），延迟就让它保持「探地」的 guarded 摆动直到真的碰到。
# 步态调度器：相位(时间触发) + 接触检测(事件触发) 共同决定接触状态
def gait_scheduler(legs, t, T, duty, offset, grf_est, dt):
for i in legs:
phase = (t / T + offset[i]) % 1.0
scheduled = "STANCE" if phase < duty[i] else "SWING"
detected = grf_est[i] > F_THRESH # 事件：从动力学反推接触
if legs[i] == "SWING" and (scheduled == "STANCE" or
(detected and phase > EARLY_OK)):
legs[i] = "STANCE" # 提前/正常触地 -> 锁支撑
elif legs[i] == "STANCE" and scheduled == "SWING":
legs[i] = "SWING" # 抬腿
elif legs[i] == "STANCE" and not detected:
pass # 延迟触地：保持探地，别发力
return [1 if legs[i] == "STANCE" else 0 for i in legs] # 接触表喂给 MPC
swing→stance 那一刻怎么不冲击（§11 的精神用在这里）？落脚点选得让脚落地时相对速度≈0；MPC 里这条腿的力约束从「=0」放开成摩擦锥，于是力从 0 渐增而非跳变； WBC 把摆动跟踪任务的优先级降下来、把力任务提上去。靠「零速触地 + 零起步力 + 滚动重解」把暂态压住。
§18飞控 GNC：分层切换的工业标杆
航空飞控把本文几乎每个概念都用上了。它的层叫 制导-导航-控制（Guidance-Navigation-Control）： 制导（外环，慢，1–10 Hz：由任务目标生成期望轨迹/姿态，这是给下层的参考）→ 导航（状态估计）→ 控制（内环，快，50–1000 Hz：跟踪制导给的参考）。和级联控制同一个套路——快内环 + 慢外环，且 $\omega_{\text{in}} \gtrsim 5\,\omega_{\text{out}}$。
- 自驾仪模式切换（高度保持、航向保持、进近、复飞）是 §3 的「选模式」+ §5 的事件触发（飞行员/FMS 触发）；切模式时用同步（synchronization）把积分器初始化好让舵面不跳，正是 §11：$I(t_0) = (u^- - K_p e)/K_i$。
- 增益调度跨整个飞行包线（按动压/马赫/高度），正是 §13。
- 包络保护是 §15 的系统级 Simplex——飞行员/自驾仪即将超限时抢占。空客 Normal→Alternate→Direct 律降级则是 §16。
- 一个血的教训是模式混淆：飞行员不知道自驾仪此刻在哪个模式，是已知的人因事故根源——所以才有醒目的飞行模式通报（FMA）。交接不仅要对机器无扰，还要对人「可见」。
§19航天器自主：Remote Agent (DS1) 的三层与故障恢复
1999 年 NASA 在 Deep Space 1 上飞的 Remote Agent，是史上第一次让 AI 分层自主真的在轨控制一艘飞船 （Muscettola/Nayak/Pell/Williams, AIJ 1998）。它是一个三层架构，把「故障恢复」做成了核心：
| 层 | 角色 | 它给下层的参考 / 它的触发 |
|---|---|---|
| PS 规划/调度器 | 从目标生成一段时间内有弹性的计划 | 参考 = 计划（一串带时窗的目标） |
| EXEC 智能执行器（ESL） | 执行计划、分解任务、管资源、监视、重试 | 选模式 + 事件触发（监视到失败） |
| MIR（Livingstone）模式识别与恢复 | 从传感器推断部件健康/模式，建议恢复 | 基于模型诊断 → 触发重构/重规划 |
故障恢复怎么交接：MIR 用一个部件模型做基于模型的诊断——发现异常就推断「哪个部件处于什么模式」，并建议重构；EXEC 据此切到恢复程序，或在无法本地处理时请求 PS 重新规划。 这是「事件触发的模式切换」在自主系统级的体现：诊断 → 重构 → 必要时重规划。最终兜底是飞船的太阳指向安全模式（§16）。 Remote Agent 在轨还暴露了一个著名的并发死锁 bug（两个线程的竞态，地面诊断后上传补丁修复）——一个关于「分层执行器里并发有多难」的真实教训（细节据飞行后复盘，引用前建议核对原始报告）。
§20总结，与还没解决的
层与层不是轮流、不互相 kill，而是并发：下层一直在算执行器命令，上层一直在给下层设参考（一个随层变得越来越具体的目标值）。 上层影响下层有四种方式——设参考 / 设约束 / 选模式 / 抢占，只有抢占是真夺权；每一次交接由四类触发决定——时间（到点了）、 事件（条件成真）、值（打分取最优，cost 最低的赢）、安全（即将越界）。决定了「该切」之后，再用无扰切换、迟滞+驻留、增益调度、可恢复集+反向交还把那一瞬间做干净。 腿足、飞控、航天器三个真实栈，讲的都是同一件事。
- 层界怎么画：RCS/3T 给了按时间尺度/状态切的判据，但一个新机器人该切几层、界画在哪，仍靠工程经验。
- 跨层端到端的保证：单点有无扰切换、单模式有平均驻留时间定理，但「穿过五层、从传感到电机」的端到端时序与稳定性，缺统一形式化。
- 学习式控制器的可验证交接：把复杂控制器换成学出来的策略很有前景，但它进不了 Simplex 的「可恢复集证明」——这正是它暂时只能当复杂控制器、底下必须垫经典安全控制器的原因。
- 语义级安全约束：CBF/包络保护守得住几何与运动学约束，但「先救人再灭火」这类语义规约怎么进安全层，还没有干净答案。
§A名词表
| 术语 | 解释 |
|---|---|
| 参考 (reference) | 相邻两层的接口变量，下层的期望值；类型随层变（符号→位姿→关节→电流）。 |
| 四种耦合 | 上层影响下层的方式：设参考 / 设约束 / 选模式 / 抢占（只有抢占是夺权）。 |
| 四种触发 | 何时交接：时间(周期) / 事件(卫式条件) / 值(打分 argmin) / 安全(出可恢复集)。 |
| 时间触发 | 纯时钟驱动；cyclic executive / TTA / RMS / ros2_control 的固定速率回路。 |
| 事件触发 | 谓词变真才动；Tabuada 事件触发控制 ‖e‖≥σ‖x‖；FSM/BT guard。 |
| 值触发 (cost) | 每周期给候选打分取最优 argmin Σwᵢcᵢ；DWA/MPPI/MPC/Frenet/加权QP/抓取打分…。 |
| 安全触发 | 即将越界才接管；Simplex(x⁺∉R)/CBF(h→0)/HJ 可达/RTA。 |
| 零阶保持 (ZOH) | 参考在上层两次刷新之间保持不变，下层持续用它。 |
| 级联 / 嵌套控制 (cascade) | 外环算参考、内环跟参考；内环比外环快约 5–10 倍。 |
| DWA / DWB | 对动态窗口里的 (v,ω) 轨迹按加权 cost 取最低；ROS 经典 32/24/0.01。 |
| MPPI | 采样 K 条控制序列按 exp(−cost/λ) 软加权（软 argmin）。 |
| Subsumption / 3T / RCS | 抢占耦合 / 选模式(executive) / 设参考逐层分解。 |
| 无扰切换 (bumpless) | 切控制器时用回算让待命控制器跟踪实际命令，命令连续不跳。 |
| 迟滞 / 驻留时间 | 升降用不同阈值、切后强制最小停留；防抖动；平均驻留时间定理保证稳定。 |
| 增益调度 | 在多个工作点设计线性控制器并按慢变调度变量插值（连续切换）。 |
| Simplex / 可恢复集 | 复杂+安全控制器+决策模块；近边界前向夺权、深入内区+驻留才反向交还。 |
| 看门狗 / 安全模式 | 回路错过 deadline 即超时切安全态；航天器终极兜底只保证活着。 |
| 凸 MPC + WBC | 腿足栈：慢 MPC 预view 出力 + 快 WBC 全身实现。 |
| GNC / Remote Agent | 制导-导航-控制；DS1 飞船三层自主 PS/EXEC/MIR。 |