运动控制算法库RTMotion拆解

项目概述

RTmotion 是由 Intel 开发的一个 C++ 运动控制库，实现了 PLCopen 标准的功能块（Function Block）接口规范。它运行在 实时 Linux（PREEMPT_RT 或 Xenomai）上，通过 EtherCAT（CiA402 协议） 控制伺服电机。核心依赖包括 Ruckig 在线轨迹规划库。

项目结构

RTmotion/
├── CMakeLists.txt              # 顶层构建配置
├── README.md                   # 项目说明
├── Doxyfile                    # 文档生成配置
├── library/                    # 核心库
│   ├── algo/                   # 轨迹规划算法层
│   │   ├── common/             # 公共算法工具 (S曲线基类、数学工具)
│   │   ├── private/            # 私有规划器 (Poly5、Line插补)
│   │   └── public/             # 公开规划器 (Online/Offline SCurve、Ruckig)
│   └── fb/                     # 功能块层
│       ├── common/             # 公共基础 (Axis、MotionKernel、ExecutionNode、Servo等)
│       ├── private/            # 内部FB
│       └── public/             # 公开FB (MC_Power、MC_MoveRelative等)
├── demo/                       # 示例程序
│   ├── multi-axis.cpp          # 基础多轴演示
│   ├── multi-axis-monitor.cpp  # 带性能监控的演示
│   ├── multi-axis-tcc-measure.cpp  # TCC 性能测量
│   └── multi-axis-tcc-ssram.cpp    # TCC SSRAM 优化
├── test/                       # 单元测试
└── util/                       # 工具 (matplotlib-cpp 绘图、PMU 读取)

整体分层架构

核心类职责

Axis --- 轴对象（核心编排器）

每个物理轴对应一个 Axis 实例，它是整个控制循环的入口和编排中心。

职责	说明
轴状态机	管理 PLCopen 标准 7 状态：Disabled → Standstill → DiscreteMotion / ContinuousMotion / Homing / SynchronizedMotion / Stopping / ErrorStop
伺服控制模式	位置模式 (CSP)、速度模式 (CSV)、力矩模式 (CST)、回零模式 (HM)，支持模式间平滑切换
安全限位	软件位置限位 (pos_positive_limit_ / pos_negative_limit_)、速度限位、加速度限位、方向限位
坐标转换	toUserUnit() / toEncoderUnit() --- 用户单位与编码器脉冲之间的双向转换，基于 encoder_count_per_unit_
溢出处理	fixOverFlow() 处理 32 位编码器计数器翻转
Override 调速	setAxisOverrideFactors() 实时调整速度/加速度/加加速度倍率

MotionKernel --- 运动内核

管理 FB 的执行队列，是运动指令调度和叠加运动的核心。

核心数据结构：

fb_queue_:  deque<ExecutionNode*>   // 待执行的 FB 队列
fb_hold_:   ExecutionNode*          // 已完成但保留的 FB（用于叠加运动衔接）
sup_move_node_:  ExecutionNode*     // 叠加运动节点
underlying_move_node_: ExecutionNode*  // 被叠加的基础运动节点

核心机制：

• Buffer Mode 支持 ：
  • mcAborting：新 FB 到达时立即中断当前运动
  • mcBuffered：新 FB 在当前运动完成后衔接执行
• Hold 机制 ：完成后的 FB 保留在 fb_hold_，用于：
  • 速度模式（mcMoveVelocityMode）的持续运行
  • 叠加运动的基准轨迹
• 叠加运动（MoveSuperimposed） ：允许在基础运动上叠加额外的增量位移，由 sup_move_node_ 管理
• Override 动态调速 ：检测 override_factors_.override_flag，触发在线重规划

ExecutionNode --- 执行节点

封装单个运动任务，是 FB 指令与轨迹规划器之间的桥梁。

生命周期：

reset() → onActive() → onExecution() → onDone()
              ↓              ↓
         切换轴状态      每周期调用 Planner 计算 pos/vel/acc

核心流程 (onExecution)：

void ExecutionNode::onExecution(mcLREAL master_ref_pos, mcLREAL master_ref_vel) {
    // 1. 如果需要重规划（首次或 Override 触发）
    if (need_plan_) {
        planner_.setCondition(start_pos_, end_pos_, start_vel_, end_vel_,
                              start_acc_, end_acc_, duration_,
                              velocity_, acceleration_, jerk_, planner_type_);
        planner_.onReplan();  // 执行轨迹规划
    }

    // 2. 根据当前时间计算轨迹点
    planner_.onExecution(node_active_time_, &pos_cmd_, &vel_cmd_, &acc_cmd_);

    // 3. 超限检查
    // 4. 同步状态到 FB
}

节点池管理：

Axis 预分配了 NODE_BUFFER_MAX_SIZE（默认10）个 ExecutionNode 组成的对象池 node_buffer_[]，通过 taken_ 标志位管理分配与回收，避免实时线程中动态内存分配。

Servo --- 伺服抽象

虚基类，定义了与实际伺服驱动器的标准接口。在真实部署中，需要派生该类实现 EtherCAT PDO 读写。

class Servo {
    // 指令下发（→ EtherCAT PDO 写）
    virtual MC_SERVO_ERROR_CODE setPos(mcDINT pos);    // 目标位置 (0x607A)
    virtual MC_SERVO_ERROR_CODE setVel(mcDINT vel);    // 目标速度 (0x60FF)
    virtual MC_SERVO_ERROR_CODE setTorque(mcLREAL t);  // 目标力矩 (0x6071)
    virtual MC_SERVO_ERROR_CODE setPower(mcBOOL, mcBOOL& isDone);
    virtual MC_SERVO_ERROR_CODE setMode(mcSINT mode);  // 控制模式 (0x6060)
    virtual MC_SERVO_ERROR_CODE setHomeEnable(mcBOOL); // 回零使能

    // 反馈读取（← EtherCAT PDO 读）
    virtual mcDINT pos();       // 实际位置 (0x6064)
    virtual mcDINT vel();       // 实际速度 (0x606C)
    virtual mcLREAL torque();   // 实际力矩 (0x6077)
    virtual mcSINT mode();      // 当前模式 (0x6061)
    virtual mcBOOL getHomeState();

    virtual void runCycle(mcLREAL freq);  // 周期更新
};

在 demo 中使用的 Servo 基类是一个虚拟伺服 （mVirtualServo = mcTRUE），runCycle() 中将目标位置直接赋给当前位置，用于纯算法验证。

轴状态机