机器人流式关节控制：接口契约、发送实现与故障排查

机器人控制器的传统指令形态是点位控制（Point-to-Point）：下发一个目标位置，等待到位事件，再发下一条；轨迹规划与插值都在控制器内部完成。当轨迹由外部产生——力控修正量、遥操作指令、学习策略的输出——控制器需要另一种接收方式：以固定周期持续接收关节目标并逐点跟踪，这就是流式关节控制（joint streaming，不少 SDK 称 joint sequence）。本文以一款工业机器人控制 SDK 的流式接口为例，整理这类接口的公共契约、发送侧的正确实现与典型故障的排查方法。适合需要在机器人控制器上做二次开发的 C++ 工程师阅读。流式接口在控制软件分层中的位置，见本站《工业控制器到 ros2_control 的分层映射与流式模式》。

一、点位控制与流式控制

两种指令形态的对比：

	点位控制	流式控制
指令单位	一条指令一个目标点	一条指令一个目标点，持续下发
完成语义	每条指令有明确的到位事件	无逐条到位事件，轨迹「随发随走」
轨迹来源	控制器内部规划	业务进程外部产生
典型场景	示教再现、单点搬运	力控、遥操作、外部规划器输出

流式模式的契约核心是三个量的匹配关系：相邻目标点的差值、发送周期、期望执行时间。轨迹的平滑性、速度和加速度由这三个量共同决定——控制器对流式目标仍会做点间平滑与插补（lookahead 类机制），但连续性保证主要落在发送侧：相邻目标差值与期望执行时间不匹配时，控制器会拒绝执行并报错。示例控制器要求发送周期不小于 0.01 s，期望执行时间参数通常就填 0.01，与发送周期保持一致。

二、流式模式开关

多数控制器的流式功能有一个显式开关，语义值得注意：

• 打开开关：控制器进入流式接收模式，此后只处理流式指令；点动（jog）、示教程序等其他运动功能同时被禁止。业务代码必须先打开开关才能通过流式接口控制机械臂。
• 关闭开关：若正在执行流式运动，关闭开关会暂停机械臂运动。

此外，实际运行前通常还要在示教器上完成运行模式选择与上伺服操作，仿真与真机模式下的操作路径一般不同，具体步骤以所用控制器的手册为准。

三、三组接口

流式控制的接口一般分三组，分别承担持续下发、单次控制与状态查询：

职责	典型形态	要点
轨迹下发	sendJointSequence(positions, vel, acc, dt, ...)	每周期调用一次，核心参数是目标位置与期望执行时间
服务控制	暂停 / 恢复 / 停止	单次命令，与下发通道独立
状态查询	关节位置 / 速度 / 加速度	用于轨迹对齐与诊断，返回值通常是「成功标志 + 数据」结构

状态查询接口的返回值先查成功标志再读数据：查询失败时数据字段可能为空，不检查标志直接取值是常见错误。另外要留意参数的生效范围——示例接口的速度、加速度与前馈力矩参数暂未实现，传零向量即可，实际使用前查阅手册确认哪些参数生效。

四、发送侧实现

发送代码放在独立线程，用绝对时间锚点维持周期：

// 以 10ms 周期从轨迹缓冲持续下发关节目标（示意）
auto anchor = std::chrono::steady_clock::now();
std::size_t i = 0;
while (running) {
    int ret = sendJointSequence(trajectory[i], /*dt=*/0.01);
    if (ret != 0) {
        // 记录错误码与序号，连续失败达到阈值时停发并上报
    }
    anchor += std::chrono::milliseconds(10);
    std::this_thread::sleep_until(anchor);   // 绝对时间锚点
    i = (i + 1) % trajectory.size();
}

四个要点：

• 用 sleep_until 而非 sleep_for：绝对锚点自动吸收每轮的处理与通信耗时，周期不随时间漂移；相对睡眠会把每轮误差累积成长期偏差。
• dt 参数与实际发送周期一致：两套周期不一致是流式接口最高频的错误来源之一。
• 所有向量长度等于机器人自由度：下发接口的每个参数向量都必须与自由度严格相等，长度不匹配直接报错。
• 不要在主线程里挤时间片：发送线程被业务逻辑阻塞时，控制器收不到预期周期的数据，轻则轨迹抖动，重则触发数据超时保护。

五、暂停与恢复

两个容易踩的坑：

1. 暂停只作用于控制器。调用暂停接口后机械臂停住，但业务侧的发送线程并不知道——它还在继续往控制器堆目标。恢复瞬间，控制器面对的是一段「未来」的轨迹。暂停时应同时停发或协调两侧时序。
2. 重新打开流式开关可能被拒绝。暂停期间机械臂位置与流式目标产生偏差，直接恢复会触发跟随误差保护（目标位置与当前位置偏差过大）。

被拒绝后的标准恢复流程是四步：关闭流式开关 → 停止发送线程 → 用点动（jog）回到零位或安全位 → 重新打开开关并恢复发送。跳过回零步骤直接重开，大概率再次被拒。

六、故障模式速查

流式接口的错误码各厂商不同，但故障类别高度一致：

故障类别	典型原因	处置
自由度不匹配	参数向量长度与机器人自由度不等	校验各向量长度（六轴机器人通常为 6）
离散点间隔过大	相邻目标差值超出速度约束	先点动回零位再发送；检查轨迹数据是否有跳变点
周期参数不匹配	dt 参数与实际发送周期不一致	两者对齐（通常均为 0.01 s）
关节超限	目标位置超出软限位	检查轨迹数据的关节值范围
状态机不兼容	流式开关未打开，或伺服状态不对	停止发送，重新上伺服后再开流式开关
状态查询失败	控制器未启动、通信中断或 SDK 与固件版本不匹配	检查连接与版本配套关系

排查的第一手信息永远是错误码与日志：先定位故障类别，再对照上表处置，比逐项猜配置快得多。

七、与 ros2_control 流式模式的对照

这套契约不是某一家的私有设计。ros2_control 侧，JointTrajectoryController 订阅 /joint_trajectory 话题就是流式接口，MoveIt 或应用节点是流的生成者；工业界的对应物包括 UR 的 servoJ、KUKA RSI/EGM 的周期性修正量通道，典型周期 4–20 ms。不同生态的表述不同，内核一致：对外暴露的只是控制器参考输入的边界，外部流不可信，限幅复核与断流看门狗必须留在控制器侧。分层的精确对应关系见开头链接的分层映射一文。

结语

流式关节控制的使用可以归纳为三条：契约上，发送周期稳定、相邻差值受控、dt 与实际周期一致，三个量不匹配就是故障的直接来源；实现上，发送线程用绝对时间锚点维持节拍，错误码连续失败要停发上报；操作上，暂停后恢复走「关开关、停发送、回零位、再重开」四步，不要硬闯跟随误差保护。照这三条写发送侧代码，绝大多数运行期问题可以在上线前避免。