启元 Q1 的控制中枢：输入源优先级仲裁机制拆解

本文结论：启元 Q1 把多路控制源收敛成一把排他锁------同一时刻只允许一个输入源握有控制权，锁靠优先级抢占、靠 1000 ms 看门狗回收。它不是加权求和，也不是多源叠加。想清楚这一点，才能解释"指令明明发出去了，机器人却不动"这类现象。

|-------|-------------------------------------------------------------------------------------|
| 项 | 值 |
| 素材基线 | github.com/PrimebotTech/primebot-q1-sdk，tag v1.0.0，commit 7310314（2026-10-08） |
| 验证环境 | Ubuntu 22.04 + ROS 2 Humble，容器镜像 pgc/primebot-humble:1.0 |
| 硬件 | 无真机（全部结论止于接口契约与规则语义） |
| 配套代码 | https://atomgit.com/2301_76341691/primebot-input-arbiter-lab |

一、先看两行互相矛盾的注释

官方《接口说明.md》里，source 这个字段出现两次，两次的说法不一样。

在「速度控制」章节，话题载荷的注释写着：

string  source    # 指令下发端口来源 (如: pad, rc, navigation)

在「输入源控制」章节，输入源结构的注释写着：

string name       # 输入源名称（rc/vr/app_proxy/interaction/pnc）

两处加起来是七个名字：pad、rc、navigation、pnc、vr、app_proxy、interaction。但同一节的「内置输入源配置表」只列了五个：rc、pnc、interaction、vr、app_proxy。

pad 和 navigation 不在表里。

同一节还写了一句话：source 必须填入"合法的自定义的已注册标识"。把这三句话放在一起，唯一自洽的读法是------任何输入源都必须先注册，才有参与仲裁的资格 ；话题注释里的 pad、navigation 只是"端口"层面的举例，不是可以直接拿来用的源名。

这不是抠字眼。照抄注释写 source="pad"，实际会发生什么，我在第五节场景 5 跑了一遍：每一帧都被丢弃，GetCurrentInputSource 返回空 name，整个过程没有一行报错、没有一个非零返回码。

这套机制最反直觉的地方就在这里：它不告诉你为什么不动。

二、机制：四段流水线，四种裁定

把文档里的三条准则摊开，一帧速度指令要过四道关：

1. 发布 。外部程序以 50--100 Hz 向 /aima/mc/locomotion/velocity 发 McLocomotionVelocity，QoS 为 Reliable / Volatile / Depth=10（《接口说明.md》2.1.1.3）。
2. 校验 。按 source 字段去注册表查：是否存在、是否 enabled。
3. 裁定。与当前持锁方比优先级，决定这一帧算不算数。
4. 执行。只有持锁方的速度分量生效，其余帧不参与任何形式的合成。

第 3 段是全部复杂度的所在，而它的输出只有四种。

图注：第 1 至 4 段为数据通路，下方四个卡片是第 3 段的全部可能输出。红色虚线是看门狗回路------持锁方静默超过自身 timeout 后，锁被回收并重新挑选候选，没有候选则触发安全停机。ACQUIRED / PREEMPTED / THROTTLED / UNREGISTERED 是本文为便于叙述起的名字，官方文档没有为这些分支定义术语。

关键在"排他"两个字。很多机器人系统会把多路速度指令做加权融合，或者按控制区域（底盘、手臂、头部）分别归属；启元的做法更硬------控制权是一个 标量 ，不分割、不叠加 。McInputSource 这个结构只有三个字段：

string name       # 输入源名称（rc/vr/app_proxy/interaction/pnc）
int32  priority   # 输入源优先级(0-100)
int32  timeout    # 超时时间(ms), 默认 1000 ms

没有权重，没有融合系数，也没有"控制区域"字段。字段表本身就是设计意图的声明：参与仲裁的只有身份、优先级、存活时间。

三、参数：五段区间、五个内置源、五个动作

优先级不是连续刻度，而是五段语义区间。这五段的划分决定了"什么样的控制该压过什么样的控制"。

|-----------|----------|----------------------------|
| 优先级区间 | 控制层级 | 文档给出的典型信源 |
| 100--80 | 系统级控制 | 紧急停止（E-Stop）、安全保护模式、物理阻感回撤 |
| 79--60 | 高级控制 | 工业遥控器（RC）、App 手动遥控、实体摇杆 |
| 59--40 | 中级控制 | 语音控制指令、手势动作交互控制 |
| 39--20 | 低级控制 | SDK 二次开发信号、PNC 仿真/自主算法 |
| 19--0 | 备用控制 | 研发调试模式、低压测试、离线仿真补丁 |

内置的五个源落在其中三个区间里，超时全部是 1000 ms：

|---------------|-----------|----------|----------------|
| 源标识 | 预设优先级 | 超时脱锁 | 物理信源 |
| rc | 80 | 1000 ms | 工业级实体遥控器 |
| pnc | 80 | 1000 ms | 导航规划算法 |
| interaction | 80 | 1000 ms | 语音、视觉手势等交互指令源 |
| vr | 70 | 1000 ms | VR 远程操控终端 |
| app_proxy | 60 | 1000 ms | 移动端 App 代理转发信源 |

图注 ：横轴为 0--100 的 priority 值，五段区间对应五类控制来源。黑色标签是五个内置源的实际落点：rc、pnc、interaction 三条线重叠在 80，vr 在 70，app_proxy 在 60。紫色虚线框是根据「39--20 典型信源示例」推断的二开源建议落点，本文实测取 30。

这三个表都出自《接口说明.md》2.1.1.4。如果你读过更早的 AIMDK 文档，需要留意启元这一版把 interaction 与 pnc 都提到了 80，与早期取值不同------照抄旧文档的数值，会得到一套完全不同的抢占关系。

注册表的增删改查由两个服务承担。请求结构很朴素：

CommonRequest   request
McInputAction   action          # ADD / MODIFY / DELETE / DISABLE / ENABLE
McInputSource   input_source
---
CommonTaskResponse response

action 的取值是定死的整数枚举，不是字符串：

|-----------------------|-------|----------------------|
| 动作 | 值 | 语义 |
| INPUTACTION_ADD | 1001 | 添加新输入源，成功后默认 enabled |
| INPUTACTION_MODIFY | 1002 | 修改优先级或超时 |
| INPUTACTION_DELETE | 1003 | 删除 |
| INPUTACTION_ENABLE | 2001 | 使能 |
| INPUTACTION_DISABLE | 2002 | 下使能 |

数值不是连续的 1--5，而是 1001--1003 与 2001--2002 两组。这种编法在嵌入式接口里常见：千位分组表示"操作类别"，个位表示具体动作。它带来的实际便利是日志里一眼能看出这是配置类还是状态类操作。

文档对持久化有一句明确交代：上述修改只保存在 运行内存 ，不回刷 YAML 配置， 重启 后恢复为配置预设。这一条决定了所有动态注册都是"进程生命周期内有效"。

四、状态：只有两个，七种迁移

把三条准则画成状态机，会得到一个比预想更简单的图。

图注：系统只有「无主」与「持锁」两个持久状态，四种裁定里有三种是「持锁」上的自环。红色虚线是唯一的回退路径：静默超时且无候选时脱锁并触发安全停机事件。图右侧注明------安全停机不是第三个状态，事件之后系统仍回到「无主」。

这个图想说的是：锁只在"谁持有"这一件事上变化，从来不会出现两个源各控制一半的情况。抢占不是叠加，顺延不是合并，拦截就是彻底丢弃。

其中「安全停机不是独立状态」这一判断，不是从文字推的，是从接口可观测行为反推的：脱锁后 GetCurrentInputSource 返回的是 name=""，也就是"无人持锁"，而不是某个异常态。第六节步骤 7 的实测印证了这一点。

五、验证一：把三条准则写成代码

文档给的三条准则是自然语言，边界条件全靠读的人自己补。为了把"我理解的规则"和"规则本身"分开，我把它们逐条翻译成了一份可执行实现，放在仓库的 arbiter/core.py。判定核心只有二十来行：

def submit(self, name, t_ms):
    self._advance(t_ms)                       # 先推进虚拟时钟，处理已到期的看门狗

    src = self._sources.get(name)
    if src is None:
        return self._log(t_ms, name, "UNREGISTERED")   # 未注册，没有优先级可比
    if not src.enabled:
        return self._log(t_ms, name, "DISABLED")

    self._last_seen[name] = t_ms              # 任何一次合法发帧都刷新存活时间

    if self._owner is None:                   # 空位：直接获锁
        self._owner, self._owner_seen_ms = name, t_ms
        return self._log(t_ms, name, "ACQUIRED")
    if self._owner == name:                   # 自己持锁：刷新
        self._owner_seen_ms = t_ms
        return self._log(t_ms, name, "REFRESHED")

    owner = self._sources[self._owner]
    if src.priority > owner.priority:         # 严格更高：抢占
        self._owner, self._owner_seen_ms = name, t_ms
        return self._log(t_ms, name, "PREEMPTED")
    if src.priority == owner.priority:        # 相等：先入为主
        return self._log(t_ms, name, "EQUAL_HELD")
    return self._log(t_ms, name, "THROTTLED") # 更低：静默拦截

用整数毫秒做虚拟时钟，不依赖真实时间，所以六个场景在任何机器上跑出来的结果逐字节一致。运行方式：

python3 sim/replay.py        # 全部六个场景
python3 sim/replay.py s1 s3  # 指定场景

六个场景的结果：

|------------|------------------------------------------|-----------------------------------------------------------------------------|
| 场景 | 构造 | 结果 |
| 1 抢占 | app_proxy(60) 已持锁，400 ms 时 rc(80) 介入 | 400 ms 首帧即换锁；app_proxy 在 400--600 ms 的 11 帧全部 THROTTLED，累计 27 帧里 16 帧有效 |
| 2 等优先级 | rc(80) 与 interaction(80) 同时发流 2 s | interaction 的 101 帧全部 EQUAL_HELD；rc 停发后 2001 ms 才顺延给它 |
| 3 多级顺延 | rc、vr、app_proxy 同时发流，依次停发 | 2001 ms 顺延给 vr，4001 ms 顺延给 app_proxy，6001 ms 无候选安全停机；共脱锁 3 次 |
| 4 发布频率 | 自定义源 30，分别以 1 Hz 与 50 Hz 发 3 s | 1 Hz 恰好踩线、0 脱锁；叠 12 ms 抖动即 3 次脱锁 + 3 次安全停机；50 Hz 下 151 帧全部有效 |
| 5 未注册源 | source="pad" 发流 | 11 帧全部 UNREGISTERED，GetCurrentInputSource 始终返回 name="" |
| 6 RAM-only | ADD 自定义源后重建实例 | 源数量 5 → 6 → 5，my_app 不复存在；ADD 与内置重名 rc 直接被拒 |

场景 4 值得单独说。文档说"每个信源设 1000 ms Watchdog"，但没说发流周期该是多少。把发布周期设成正好 1000 ms，虚拟时钟下是踩线通过的------因为判定用的是"严格大于"。可真实系统里 12 ms 的调度抖动就足以让它翻车：

t(ms)  事件                      裁决            持锁方
       0  发布 my_app               ACQUIRED      my_app
    1001  my_app 看门狗到期            UNLOCK:watchdog(无)
    1001  无就绪候补                   SAFE_STOP     (无)
    1012  发布 my_app               ACQUIRED      my_app
    2013  my_app 看门狗到期            UNLOCK:watchdog(无)
    2013  无就绪候补                   SAFE_STOP     (无)
    2024  发布 my_app               ACQUIRED      my_app

每 1 秒一次的"脱锁 → 安全停机 → 重新获锁"，在真机上就是每秒一次的停车动作。这不是理论风险，是把 timeout 当成"发送间隔上限"来用必然踩到的坑。发流周期必须显著小于 timeout，工程上取 20 ms（50 Hz）比较稳妥。

六、验证二：用官方消息跑一次真实服务回环

上面的复现器验证的是"我理解的规则自洽"。但规则写在文档里、接口编译在 aimdk_msgs 里，这两者是不是对得上，得用真类型跑一次。

节点直接依赖官方消息包，没有自定义任何类型：

from aimdk_msgs.msg import CommonState, McLocomotionVelocity
from aimdk_msgs.srv import GetCurrentInputSource, SetMcInputSource

self.create_subscription(McLocomotionVelocity, "/aima/mc/locomotion/velocity",
                         self.on_velocity, VELOCITY_QOS)   # Reliable/Volatile/Depth=10
self.create_service(SetMcInputSource, "/aimdk_5Fmsgs/srv/SetMcInputSource",
                    self.on_set_input_source)
self.create_service(GetCurrentInputSource, "/aimdk_5Fmsgs/srv/GetCurrentInputSource",
                    self.on_get_input_source)

注册一个自定义源，用的是官方推荐的命令行形式：

ros2 service call /aimdk_5Fmsgs/srv/SetMcInputSource aimdk_msgs/srv/SetMcInputSource \
"{action: {value: 1001}, input_source: {name: 'my_app', priority: 30, timeout: 1000}}"

返回体是统一的 CommonTaskResponse：header.code=0、task_id=2、state.value=1（SUCCESS）。task_id 是自增的，说明服务端把每次调用都编了号------排查时序问题时这个号比时间戳好用。

十一个场景的实测摘要（完整日志见仓库 scripts/ros2_demo.sh 的运行输出）：

|--------|-------------------------|------------------------------------------|
| 步骤 | 动作 | GetCurrentInputSource****返回 |
| 1 | 空态 | name='' priority=0 |
| 3 | my_app(30) 以 50 Hz 发流 | my_app / 30 |
| 4 | 内置 rc(80) 介入 | rc / 80（首帧即抢占） |
| 5 | rc 停发后立刻查询 | 仍为 rc / 80（仍在 1000 ms 窗口内） |
| 6 | 静默 1.4 s 后再查 | my_app / 30（看门狗脱锁并顺延） |
| 7 | 全部停发 | name=''（无候选，安全停机） |
| 8 | 用 pad 发流 | name=''（未注册，连空位都占不到） |
| 11 | ADD 重名 rc | header.code=1、state.value=2（FAILURE） |

节点日志里能直接读到时刻：

[INFO] my_app -> ACQUIRED (持锁=my_app)
[INFO] rc -> PREEMPTED (持锁=rc) 抢占 my_app(30)
[WARN] [t=...334810] rc 看门狗脱锁（watchdog）
[WARN] [t=...334810] 顺延 -> my_app（顺延至 my_app(30)，候选 1 个）
[ERROR] [t=...337255] 无就绪候补，安全停机

rc 在 334.810 s 被记录脱锁。按 1000 ms 阈值反推，它的最后一帧落在 333.810 s，两者正好相差 1000 ms------文档里的"超过阈值自动释放"，在这一次运行里是一毫秒不差的。

顺带一个容易踩的工程细节：第一次跑这段时，用 1.0 s 的发流窗口，节点一帧都没收到。把窗口拉长到 3 s 就正常了。新起的发布进程需要时间完成 DDS 发现，前几百毫秒的帧发出去也没有对端。任何"发了几帧但对方没反应"的排查，先确认发现是否完成，再怀疑仲裁逻辑。

七、五个边界

这一节是全文重点。以下五点都不是文档上写的，而是把规则实现出来、跑到边界才浮出来的。

7.1 被抢占的一方没有任何感知通道

McLocomotionVelocity 是单向话题，没有回执。被抢占方的后续帧被静默丢弃，既不返回错误码，也不会进队列等锁。

场景 1 里 app_proxy 一共发了 27 帧，只有前 16 帧算数；剩下 11 帧在 400--600 ms 之间被丢弃，发端毫不知情。

这意味着一个直接后果：SDK 程序必须自己周期性地调 GetCurrentInputSource****做自检，不能假设"我发了就等于我在控制"。如果程序需要知道"我什么时候被人工接管了"，这是唯一可用的信号。

7.2 三个内置源并列 80，语音打不断手柄

rc、pnc、interaction 都是 80。等优先级互斥、先入为主，于是：

• 操作员握着遥控器连续发流时，语音指令（interaction，80）拿不到锁------它不低，但也不高，只能等 rc 停发并超时；
• 反过来，语音先持锁时，遥控器也要等它超时才能接管。

场景 2 把这层关系量化了：interaction 连发 2 秒、共 101 帧，全部 EQUAL_HELD，一帧都没进去。

这个设计不是疏漏。把"人在现场拨摇杆"和"人在对机器人说话"放进同一个优先级区间，等于明确承认两者谁也不比谁更紧急；真正压过它们的只有系统级保护（100--80 上沿）和更低层的手持终端（70、60）。

对二开者的实际含义是：如果你希望"语音随时能打断手柄"，就必须自己定义一个高于 80 的源------但这会挤进系统级区间，可能与安全策略冲突。更稳妥的做法是接受现状，把手柄与语音当成同一档，靠操作流程而不是优先级来协调。

7.3 发布周期必须显著小于 timeout

已在第五节展开。这里补一句量化的判断依据：文档给的发流频率是 50--100 Hz，即 10--20 ms 一帧，距离 1000 ms 阈值有 50 倍余量。低于 10 Hz 就进入了危险区，任何一次 GC、磁盘 IO 或调度抖动都可能触发脱锁。

7.4 未注册的源连"空位"都占不到

这一条比"被拦截"更彻底。场景 5 中，系统处于「无主」状态（没有任何源持锁），pad 依旧拿不到锁------因为它的优先级根本不存在，没法参与比较。

对比一下就很清楚：一个注册过的源，哪怕优先级只有 1，在无人持锁时也能立刻获锁；一个没注册的源，哪怕在发 100 Hz，也永远是 0 帧有效。

而这件事不会报错。没有"source not found"，没有"invalid source"，什么都没有。

所以第一节那两行注释的差异不是文字问题，是行为差异：照抄 pad 或 navigation 写代码，得到的是一个静默失效的程序。

7.5 规范留白：两处没有答案的地方

实现过程中遇到两个文档没有规定、但代码必须做选择的地方：

其一，同优先级候补如何取舍。 文档说脱锁后"按优先级降序自动寻找下一个已就绪的候补信源"。如果两个候选都是 80，选谁？没有规定。我在复现器里按名称字典序兜了一手，但这只是实现选择，不是规则。实车行为需要实测确认。

其二，抢占是否需要中断协商。 文档用"瞬间切换"描述抢占，没有提被抢占方是否有一段减速或停止的过渡。CommonStateReason 里有两个常量值得注意------REASON_NOT_INTERRUPTIBLE（28）与 REASON_INTERRUPT_INVALID（29），它们说明系统里确实存在"不可中断"的概念，但这两个值出现在任务响应里，不是输入源仲裁的报错通道。二者的关系文档没有交代。

这两点我都没有强行补全。写架构解读最容易犯的错，就是把留白填成结论。

八、为什么是排他锁，而不是加权求和

把多个控制源做加权融合，看起来更"平滑"，但在移动机器人上会出三类问题：

1. 物理不可融合。 遥控器给的是 +0.5 m/s 前进，导航给的是 −0.3 m/s 后退，加权之后是 0.1 m/s 前进------没有人在控制它，但它在动。
2. 责任不可追溯。 出事之后，融合输出无法归因到某一个源。排他锁的日志里，"这一刻谁在控制"是唯一的、可查的。
3. 失效不可检测。 融合模型里，某一路源卡死只会让结果偏移一点，不会报错。排他锁里，源卡死会直接表现为脱锁，是一个强信号。

启元选了后者，并且用 1000 ms 看门狗把"源卡死"转成"控制权回收"，再用"无候选则安全停机"把回收失败转成"停车"。这三步串起来是一条完整的失效链路：检测（超时）→ 处置（顺延）→ 兜底（停机）。

代价同样明确：任何一次超过 1 秒的卡顿都会变成一次真实的停车。这也是为什么文档反复强调发流频率------在这套模型里，心跳不是可选项，是控制权本身。

九、还没解决的问题

三个问题在无真机条件下无法收口，如实列出：

第一，物理层行为未知。 抢占瞬间机器人是急停、减速还是无感切换，取决于运控内部实现（未开源），只能在实车上观察。

第二，注册表不可枚举。 公开接口只有 GetCurrentInputSource（查当前持锁方）与 SetMcInputSource（增删改）。McInputSource 结构里也没有 enabled 字段------使能状态只能通过 ENABLE/DISABLE 动作写入，读不回来。想确认"我现在一共注册了哪些源、哪些是使能状态"，公开接口给不出答案。

第三，超时粒度单一。 五个内置源都是 1000 ms，没有为"低频但重要"的指令留出长超时通道。如果一类任务天然只能 0.5 Hz 下发给定控制指令，就必须自己额外发心跳来续锁。

十、如果要动手，先做这四件事

按踩坑概率排序：

1. 先 ADD，再发流。 顺序反了不会报错，只会什么都不发生。
2. 源名用 ASCII ，且避开 rc**/** pnc**/** interaction**/** vr**/** app_proxy**。** 重名会被 ADD 拒绝（实测 header.code=1、state.value=2）；用 pad、navigation 这类注释里的词则会被静默丢弃。
3. 发流频率不低于 10 Hz，建议 50 Hz。 并且把发流放在独立线程或独立进程里，别和控制逻辑抢时间片。
4. 优先级取 20--39。 这是文档给"SDK 二次开发信号"划定的区间。取高了会和语音、遥控器抢锁，取低了在顺延时永远排最后。

另外记住一条：所有 ADD**/** MODIFY****只写 内存 ， 重启 即还原。别指望用接口改出一份持久配置，要持久化就改部署侧的 YAML。

整套机制读下来，它真正解决的问题不是"谁优先"，而是"谁负责"。优先级只是手段，排他锁才是目的：任何时刻，机器人的运动都能指向唯一一个控制源，以及唯一一个可以被追责的输入。

代码与实测脚本都在 AtomGit：https://atomgit.com/2301_76341691/primebot-input-arbiter-lab。纯模拟部分零依赖，不需要 ROS 环境即可复现第五节的六个场景；ROS 2 部分需要 primebot-q1-sdk v1.0.0 编译出的 aimdk_msgs。所有结论仅覆盖接口契约层面，实车行为请以真机为准。

更多接口细节、SDK 下载与开发文档，见启元机器人开发者社区：https://dev.primebot.com/