机器人实时控制与嵌入式

三种实时性：
  硬实时（Hard）：超时即系统失败（安全停机、力控）
  软实时（Firm）：偶尔超时可接受，质量下降（视觉推理）
  非实时（Best-effort）：超时无影响（日志、可视化）

控制回路的时限来源：
  控制频率 ≥ 10 × 闭环带宽。闭环带宽 50 Hz 则至少 500 Hz，典型 1 kHz
  抖动预算通常取周期的 5%~10%，1 ms 周期约 50~100 µs

机器人的实时性分层（频率递增、范围递减）：
  任务规划层（1~10 Hz）：非实时；运动规划层（10~100 Hz）：软实时
  全身控制/轨迹插值（100~500 Hz）：软到硬实时
  关节伺服环（1~4 kHz）与电流环（10~20 kHz）：硬实时，在驱动器内

周期选择的依据：闭环带宽（周期 ≤ 1/(10×带宽)）、执行器带宽、
  总线能力（EtherCAT 通常 1 ms 或 500 µs）、算力预算
  经验：机械臂 1 kHz，移动机器人 100~500 Hz，足式 1~2 kHz

抖动预算分解（1 ms 周期、100 µs 预算为例）：
  内核调度抖动 < 20 µs；中断延迟 < 10 µs；
  总线通信（EtherCAT）< 50 µs；计算时间波动 < 20 µs；合计 < 100 µs

超时的后果按控制类型不同：
  位置控制：单次超时是一次位置误差，通常可容忍
  力控/阻抗：表现为力冲击或振荡，可能损坏工件
  安全回路：可能意味着保护失效，绝不可容忍

PREEMPT_RT 的核心改动：
  1. 把大部分内核自旋锁替换为可睡眠的互斥锁，长临界区不再阻塞实时线程
  2. 中断线程化：中断处理移到内核线程，可被调度与设优先级
  3. 优先级继承：解决优先级反转；高精度定时器与更细调度粒度

安装（Ubuntu）：sudo apt install linux-image-rt-amd64 linux-headers-rt-amd64
主线 6.12 起 PREEMPT_RT 已合并，也可配置 PREEMPT_RT=y 自行编译

uname -a                                  # 确认是 -rt 内核
zcat /proc/config.gz | grep PREEMPT       # 应为 PREEMPT_RT=y
cat /sys/kernel/realtime                  # 1 表示 RT 内核已启用

sudo cpupower frequency-set -g performance        # 关闭 CPU 节能
sudo sysctl -w kernel.sched_rt_runtime_us=-1      # 禁用实时节流（关键）
# 中断亲和性：把非关键中断移出实时核
# 内核命令行加 isolcpus=3 nohz_full=3 rcu_nocbs=3 隔离实时核

# cyclictest：测量从定时器到期到线程被调度的延迟
# -p 80 优先级 80，-t 4 四个线程，-m 锁定内存，-i 1000 周期 1000µs
sudo cyclictest -p 80 -t 4 -m -i 1000 -d 0 -h 400 -q

# 输出解读（关注 Max 与分布）：
# T: 0 ( 1234) P:80 I:1000 C: 60000 Min:  2 Act:  3 Avg:  4 Max: 27
#                                                    ↑ 最大延迟 27µs
# 目标：Max < 50µs（1 kHz 控制）

cyclictest 结果解读与对策：
  Max < 50 µs   ：良好，可支撑 1 kHz 控制
  Max 50~200 µs ：临界，需排查（中断、SMI、电源管理）
  Max > 200 µs  ：不达标，不可用于硬实时控制
  常见超标原因：BIOS 的 SMI（更新 BIOS）、CPU 频率切换
    （设 performance 并禁深度 C-state）、USB/网络中断在实时核
    （调中断亲和性）、内存分配（mlockall）

端到端延迟的分解测量：
  应用层：时间戳打点测控制回调执行时间与抖动
  内核层：cyclictest 测调度延迟，ftrace 测中断与调度事件
  总线层：EtherCAT 的 DC 偏差、CAN 的报文时延
  执行器层：驱动器报告的实际响应延迟
工具链：cyclictest、ftrace/trace-cmd、ros2_tracing + LTTng、Wireshark

EtherCAT 的核心机制：
  1. 主站发送一个以太网帧遍历所有从站，每个从站 on-the-fly 读写数据，
     帧绕一圈返回，一次通信完成全部从站的数据交换
  2. 分布式时钟（DC）：主站时钟为参考，从站时钟同步（偏移与漂移补偿），
     所有从站动作同一时刻触发，同步精度 < 1 µs
  3. PDO（周期性过程数据，实时）与 SDO/CoE（非周期配置）

主站实现：IgH（内核模块，性能好）、SOEM（用户态，简单）、
          Acontis / TwinCAT（商业，功能全）

周期与抖动：典型周期 1 ms（500/250 µs 也常见）；
  从站处理约 1~2 µs/个，20 个从站约 20~40 µs；
  DC 同步精度 < 1 µs；主站抖动 PREEMPT_RT 下 < 50 µs

# 用 IgH 主站查看与调试
ethercat master                          # 主站状态
ethercat slaves                          # 从站列表与状态
ethercat pdos                            # 过程数据映射
ethercat dc                              # 分布式时钟状态与偏差
ethercat graph                           # 拓扑图

CANopen 的核心概念：
  COB-ID：决定报文的优先级与用途
  PDO（过程数据对象）：周期性实时数据，映射到驱动器的对象字典
  SDO（服务数据对象）：非周期配置，读写对象字典
  NMT（网络管理）：节点状态机；心跳（Heartbeat）：周期性宣告存活

CiA402 驱动器状态机（驱动器子协议）：
  状态：Not Ready → Switch On Disabled → Ready to Switch On
        → Switched On → Operation Enabled → Quick Stop → Fault
  控制字与状态字驱动状态迁移
  模式：位置模式（PP/CSP）、速度模式（PV/CSV）、力矩模式（PT/CST）

CAN 的实时性限制与对策：
  1 Mbps、报文 ≤ 8 字节，一帧约 50~130 µs；负载上升则延迟抖动增大
  经验：总线负载 < 50% 才能保证实时性
  对策：用 SYNC 报文统一触发 PDO；减少周期 PDO 数量；用 CAN-FD 提带宽

EtherCAT 与 CANopen 的取舍：
  EtherCAT：周期短（250 µs~1 ms）、同步精度高、带宽大、成本高
  CANopen ：周期长（1~10 ms）、成本低、抗干扰好、带宽小
  高精度多轴协调用 EtherCAT，低成本单轴或移动底盘用 CANopen

ros2_control 的三层（见机械臂一篇）：
  硬件接口（Hardware Interface）：read() 与 write()，实时
  控制器管理器（Controller Manager）：固定周期调用 update()
  控制器（Controller）：joint_trajectory_controller 等

实时约束：
  controller_manager 的 update() 在实时线程里运行
  read() 与 write() 必须是实时安全的：
    无动态内存分配、无阻塞锁、无系统调用（或极少）
  控制器之间的切换、参数的读写发生在非实时线程

// 硬件接口的实时实现要点
class MyRobotHardware : public hardware_interface::SystemInterface {
 public:
  hardware_interface::CallbackReturn on_init(...) override {
    joints_.resize(info_.joints.size());   // 非实时：分配内存、打开设备
    return hardware_interface::CallbackReturn::SUCCESS;
  }
  hardware_interface::return_type read(const rclcpp::Time &,
                                       const rclcpp::Duration &) override {
    for (auto & j : joints_) {             // 实时：只做总线读取，无分配无锁
      j.pos = bus_.readPosition(j.id);
      j.vel = bus_.readVelocity(j.id);
    }
    return hardware_interface::return_type::OK;
  }
  hardware_interface::return_type write(const rclcpp::Time &,
                                        const rclcpp::Duration &) override {
    for (auto & j : joints_) {             // 实时：只做总线写入
      bus_.writeCommand(j.id, j.cmd_pos, j.cmd_vel, j.cmd_eff);
    }
    return hardware_interface::return_type::OK;
  }
 private:
  std::vector<Joint> joints_;              // on_init 里分配好
  BusInterface bus_;                       // 预初始化的总线对象
};

controller_manager:
  ros__parameters:
    update_rate: 1000          # 1 kHz 控制周期
    # 关键：把 update 线程绑到隔离核，并设为实时优先级
    # 通常由 launch 里的 taskset / chrt 完成

# 把 controller_manager 绑定到隔离核并设实时优先级
taskset -c 3 chrt -f 80 ros2 run controller_manager ros2_control_node \
  --ros-args --params-file /etc/robot/controllers.yaml

实时线程的「七宗罪」与对策：
  1. 动态内存分配：可能触发缺页与锁，抖动可达毫秒
     → 内存在初始化阶段分配，实时路径只用预分配缓冲
  2. 互斥锁：可能被低优先级线程持有，造成优先级反转
     → 无锁数据结构，或优先级继承互斥锁（PREEMPT_RT 支持）
  3. 系统调用：多数系统调用不确定（I/O、文件、网络）
     → 实时路径避免系统调用，数据搬运交给其他线程
  4. 日志：spdlog 等会加锁与分配 → 只写无锁环形缓冲，后台落盘
  5. 缺页中断：首次访问未映射内存会触发 → mlockall 锁定内存
  6. 分支预测失败与缓存未命中 → 热路径代码紧凑，避免大跳转
  7. 浮点异常与除零：罕见但代价大 → 输入校验，避免除零

// 实时路径的无锁日志：只写环形缓冲
struct RtLogEntry { uint64_t cycle; uint32_t jitter_us; };
static constexpr size_t kRingSize = 4096;   // 编译期固定，无分配
RtLogEntry ring_[kRingSize];
std::atomic<size_t> head_{0};

inline void rtLog(uint64_t cycle, uint32_t jitter_us) {
  const size_t idx = head_.fetch_add(1, std::memory_order_relaxed) % kRingSize;
  ring_[idx] = {cycle, jitter_us};          // 无锁、无分配
}
// 后台非实时线程定期读取 ring_ 并落盘

// 锁定内存：启动时调用一次，避免运行中缺页
#include <sys/mman.h>
if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0) {
  // 记录告警：无法锁定内存，实时性可能受影响
}

安全功能的层次（详见部署与安全一篇）：
  STO：切断电机力矩，无制动，靠机械制动停车
  SS1：先受控减速，再切断力矩
  SS2：受控减速并保持力矩（位置保持）
  SLS：限制速度；SLP：限制位置
实现载体：安全 PLC 或安全驱动器（硬件级，符合 ISO 13849 /
  IEC 61508）、双通道急停回路、安全总线（FSoE、CIP Safety）

看门狗（Watchdog）的层次：
  1. 硬件看门狗：独立定时器，未按时喂狗则复位 CPU，防软件完全卡死
  2. 应用看门狗：控制线程周期更新计数器，监控线程检查，
     防「控制线程卡死但其他线程还在跑」
  3. 通信看门狗：主站检测从站心跳，从站检测主站心跳

关键：看门狗超时必须小于「危险发生所需时间」
  例：若 100 ms 的失控会造成伤害，看门狗超时应 < 100 ms

// 应用看门狗：控制线程喂狗，监控线程检查
std::atomic<uint64_t> control_heartbeat{0};

// 实时控制线程（每周期）
void controlLoop() {
  // ... 控制计算 ...
  control_heartbeat.fetch_add(1, std::memory_order_relaxed);
}

// 非实时监控线程
void watchdogLoop() {
  uint64_t last = control_heartbeat.load();
  while (rclcpp::ok()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    uint64_t now = control_heartbeat.load();
    if (now == last) {                       // 计数未变，控制线程卡死
      triggerSafeStop();
      break;
    }
    last = now;
  }
}

心跳的设计要点：周期 10~100 ms（控制周期的 10~100 倍）；
  超时为 3 个心跳周期未收到即判定失效；内容含序号与时间戳；
  通道独立于数据通道（或复用但可区分）

机器人系统的三层心跳：软件层（ROS 2 lifecycle bond）、
  通信层（DDS 的 liveliness）、硬件层（主站/安全 PLC 与驱动器）

故障恢复的分级策略：
  瞬时故障（丢一帧）→ 重试或忽略，记录告警
  短暂故障（中断 < 1s）→ 减速保持，等待恢复
  持续故障（> 1s）→ 受控停机（SS1），进入安全状态
  严重故障（安全相关）→ 立即 STO，切断力矩
关键原则：任何降级都不能「带着不确定性继续跑」

平台类型的取舍：
  1. 工控机 + RT 内核（x86）：算力强、生态好、需调优，功耗高
     → 机械臂控制器、机器人主控
  2. ARM 嵌入式（树莓派、Jetson、RK3588）：功耗低、体积小，
     实时调优难、算力有限 → 移动机器人、轻量控制
  3. MCU（STM32、TI C2000）：确定性极好（RTOS/裸机）、低功耗、便宜，
     算力弱、开发难 → 关节控制器、驱动器、安全回路
  4. FPGA / SoC-FPGA：纳秒级确定性、并行，开发难、成本高
     → 高速电流环、多轴同步、专用加速

典型架构：MCU/FPGA 做硬实时底层，工控机做规划与控制，以太网连接

算力预算的估算：
  控制周期的计算时间应 ≤ 周期的 30%~50%
  留余量给：抖动、最坏情况分支、未来功能增加
  例：1 ms 周期，控制计算应 < 300~500 µs
  实测方法：用高精度计时器记录每次 update() 的耗时分布

上机调试的顺序：
  1. 内核与系统层：cyclictest 达标（Max < 50µs）
  2. 应用层：控制回调的执行时间与抖动在预算内
  3. 总线层：EtherCAT DC 偏差 < 1µs，CAN 负载 < 50%
  4. 闭环层：跟踪误差在预期范围，无振荡
  5. 压力测试：高负载下延迟仍达标

延迟回归测试（CI 化）：固定场景（同样轨迹与负载）→ 记录关键指标
  → 与基线对比超阈值告警 → 每次内核/驱动/软件改动后重跑

sudo cyclictest -p 80 -t 4 -m -i 1000 -D 60 -q | tail -1   # 内核调度延迟
ethercat dc | grep -i "deviation"                          # EtherCAT 时钟偏差
candump -T 1000 can0 | wc -l                               # 总线负载

关键回归指标与阈值（1 kHz 控制）：
  cyclictest Max        ：< 50 µs
  控制回调执行时间 Max  ：< 300 µs
  控制回调抖动 Max      ：< 100 µs
  EtherCAT DC 偏差      ：< 1 µs
  CAN 总线负载          ：< 50%
  跟踪误差 RMS          ：< 关节限值的 1%