ROS Control 深入：控制循环、硬件接口与真实电机对接

ROS
5.7k 词

上一篇（PID 工程实践）解决”控制器怎么写”，这一篇解决”控制器怎么跑在真实硬件上”。ros_control 的文档很多，但大部分停留在”照着模板抄 RobotHW”。这里从控制循环的实时性讲起，一路讲到 CAN 总线电机对接的实战细节。

一、先理解控制循环：50Hz 背后的实时性陷阱

ros_control 的核心是一个固定频率的控制循环，典型实现：

1
2
3
4
5
6
7
8
9
10
ros::Rate rate(50);  // 50Hz
while (ros::ok()) {
    ros::Time now = ros::Time::now();
    ros::Duration dt = now - prev;
    robot_hw.read();          // 1. 读硬件状态
    cm.update(now, dt);       // 2. 更新所有控制器（PID计算）
    robot_hw.write();         // 3. 写控制指令
    prev = now;
    rate.sleep();
}

这个循环看起来简单，但真实工程里有三个大坑：

坑1：ros::Rate::sleep() 不保证周期精确

ros::Rate 基于 ros::Time（系统时钟），当系统负载高时，回调被推迟，实际控制周期会抖动。对 50Hz 速度环，周期抖动 5ms 就足以让 PID 微分项产生明显噪声。

解法：控制循环用 hardware_interface::RobotHW 的 getTime()/getPeriod()（内部用 ros::Time 但可替换为硬件时钟），更激进的做法是硬实时——用 RT 内核 + clock_nanosleep 或直接在 MCU 上跑控制环，ROS 只做通信。

坑2：read/write 阻塞会吃掉控制周期

read() 里如果用了阻塞式串口读取（等数据到达才返回），一旦硬件响应慢，整个循环被拖住，控制器频率掉到 10Hz 都没人知道。

解法：read/write 必须非阻塞或超时保护；硬件通信放独立线程，控制循环只读写共享缓冲区（注意加锁）。

坑3：时间戳对不齐

cm.update(now, dt) 的 now 必须与 read() 拿到的数据时刻一致。如果 read() 的数据是 20ms 前采的，而 now 是当前时刻，微分项会算出虚假的”大变化”。

二、RobotHW 的完整生命周期

一个生产级 RobotHW 不只是 read/write，还包括初始化、诊断、切换：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
class MyRobotHW : public hardware_interface::RobotHW {
public:
    bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override {
        // 1. 读取 URDF/参数：关节名、CAN ID、减速比
        // 2. 初始化硬件通信（打开 CAN 设备 / 串口）
        // 3. 注册 Handle 到各 Interface
        // 4. 初始化诊断 updater（上报电机温度、电压）
        return true;
    }

    void read(const ros::Time& time, const ros::Duration& period) override {
        // 非阻塞读取硬件 → 更新 pos_, vel_, eff_
    }

    void write(const ros::Time& time, const ros::Duration& period) override {
        // 将 cmd_ 打包发送给硬件
    }

    bool prepareSwitch(const std::vector<hardware_interface::ControllerInfo>& start,
                       const std::vector<hardware_interface::ControllerInfo>& stop) override {
        // 控制器切换前：例如停止当前控制器时先让电机回零力矩
        return true;
    }

    void doSwitch(const std::vector<hardware_interface::ControllerInfo>& start,
                  const std::vector<hardware_interface::ControllerInfo>& stop) override {
        // 真正执行切换：更新内部状态标记
    }
};

注意 ROS1 里 read()/write() 在 controller_manager 内部被调用，ROS2 中则是 read() 由实时控制循环（如 ros2_control 的 ControllerManager 组件）驱动，且 ROS2 的 hardware_interface 接口签名统一为带 time 和 period 参数。

为什么需要 prepareSwitch / doSwitch 两段式？

因为切换控制器是有状态转移的操作：先”准备”（所有控制器确认可以切换），再”执行”（统一切换）。如果直接切换，可能出现：轨迹控制器还没释放关节，位置控制器已经接管，两个控制器同时写一个 joint——指令冲突，电机抖动。两段式保证切换的原子性。

三、三种硬件接口：Effort / Velocity / Position 怎么选

接口 指令含义 适用场景 RoboMaster 对应
EffortJointInterface 力矩/电流指令 需要精确力控、阻抗控制 电流环已由电调完成，一般不直接用
VelocityJointInterface 速度指令 速度伺服、移动底盘 M3508/C620 电调速度环
PositionJointInterface 位置指令 位置伺服、关节角控制 M2006 位置环

选型核心：ROS 层做哪一级闭环，取决于底层硬件已经完成了哪一级。

RoboMaster 的 C620 电调内置电流环（FOC）+ 速度环，你发给它的目标是”转速”——所以上层 ROS 应该用 VelocityJointInterface，再往上自己加位置环 PID（位置误差 → 目标速度）。如果电调只做电流环（如自研电调），ROS 层就得自己写速度环 PID（上一篇文章的内容），用 EffortJointInterface 发电流目标。

新手最常见的设计错误：买了带速度环的电调，还在 ROS 层写了个速度环 PID 发给电调”速度目标”——两层速度环串联，带宽互相打架，调参调到头秃。

四、真实电机对接：CAN 总线实战

以 RoboMaster M3508 + C620 为例，RobotHW 的 write() 本质是把速度指令打包成 CAN 帧：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
// C620 电调 CAN 发送格式（0x200 + 电机ID组）
// 数据: [ID1_高8位, ID1_低8位, ID2_高8位, ID2_低8位, ...]
// 每个速度值范围 -16384 ~ +16384（对应 -10000 ~ +10000 rpm）

void write(const ros::Time&, const ros::Duration&) override {
    uint8_t data[8] = {0};
    for (int i = 0; i < 4; ++i) {
        int16_t vel = static_cast<int16_t>(
            std::clamp(cmd_[i] / MAX_RPM * 16384.0, -16384.0, 16384.0));
        data[i*2]   = vel >> 8;
        data[i*2+1] = vel & 0xFF;
    }
    can_send(0x200 + group_id_, data, 8);
}

读状态（反馈转速）用 0x201 组帧，解析格式：[ID1_高, ID1_低, 角度_高, 角度_低, ID1转速_高, ID1转速_低, ...]。

几个实战细节：

  1. CAN 波特率：C620 默认 1Mbps，必须和电调拨码一致，否则全车静默
  2. ID 分配：电调 ID 通过拨码开关设置，代码里要能从参数服务器读 ID 而不是硬编码
  3. 数据溢出：-16384~+16384 对应 int16，符号位处理要小心
  4. 超时保护：CAN 总线断线时电调会锁死输出（C620 默认 1s 无指令自动停止），但 ROS 侧也要监控反馈超时，触发急停
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void read(const ros::Time&, const ros::Duration&) override {
    // 非阻塞读 CAN 帧
    while (can_receive(&frame) == OK) {
        int idx = (frame.id - 0x201) * 4 + group_offset;
        vel_[idx] = (static_cast<int16_t>((frame.data[4] << 8) | frame.data[5]))
                    / 16384.0 * MAX_RPM;
        last_rx_[idx] = ros::Time::now();
    }
    // 超时检查
    for (int i = 0; i < n_joints_; ++i) {
        if ((ros::Time::now() - last_rx_[i]).toSec() > 0.5) {
            ROS_ERROR_THROTTLE(1.0, "Joint %d feedback timeout!", i);
            estop_ = true;  // 触发急停
        }
    }
}

五、Transmission 的数学本质

Transmission 文档里只说了”SimpleTransmission 带减速比”，但它的本质是执行器空间与关节空间的双向变换：

$$\tau_{joint} = \frac{1}{n}\tau_{actuator}, \quad \dot{q}{actuator} = n \cdot \dot{q}{joint}$$

其中 n 是机械减速比。对应到代码：

1
2
3
4
// 速度映射：关节速度 → 电机转速
double actuator_vel = reduction_ * joint_vel;
// 力矩映射：电机力矩 → 关节力矩
double joint_effort = actuator_effort / reduction_;

多级减速（谐波/行星 + 带轮）：SimpleTransmission 只支持单级，复杂传动链（如 RoboMaster 云台 yaw 轴：电机 1:359 谐波 + 同步带）需要自定义 Transmission 或直接在 RobotHW 里做变换。后者在多数 RoboMaster 工程中更常见——直接读电机反馈除以总减速比得到关节角，指令方向乘以总减速比。

六、控制器切换实战：spawner 背后的机制

spawner 命令的本质是调用 controller_manager 的 service：

1
2
load   →  /controller_manager/load_controller   (加载 .so 插件)
start  →  /controller_manager/switch_controller (启动)

启动顺序很重要：先 load 所有控制器 → 再 switch 启动。如果只 load 不 start，控制器被加载但不运行，可以用于”预加载后按需启动”。这在多模式机器人（底盘速度模式 / 云台位置模式切换）中很常用：

1
2
3
4
5
6
7
8
9
# 预加载全部控制器
rosrun controller_manager spawner --stopped \
    chassis_vel_controller gimbal_pos_controller

# 启动底盘模式
rosservice call /controller_manager/switch_controller \
    "{start_controllers: ['chassis_vel_controller'], \
      stop_controllers: ['gimbal_pos_controller'], \
      strictness: 1}"

七、Joint Limits：最后的安全网

URDF/参数服务器配置的 joint_limits 不只是”文档”，它在 ros_control 内部作用于控制器输出——软限位（soft limits）会在接近极限时让控制器”温柔地”减速，而不是硬切：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
joint_limits:
  yaw_joint:
    has_position_limits: true
    min_position: -3.14
    max_position: 3.14
    has_velocity_limits: true
    max_velocity: 6.28
    has_effort_limits: true
    max_effort: 5.0
    # 软限位：极限前 0.5 rad 开始减速
    soft_lower_limit: -2.64
    soft_upper_limit: 2.64
    k_position: 10.0   # 软限位弹簧刚度
    k_velocity: 5.0    # 软限位阻尼

对于 RoboMaster 云台，yaw 轴是无限旋转的（滑环），绝对不能配位置硬限位，否则陀螺仪坐标系翻转时控制器会往极限方向猛打。这种”限位配置本身会引发事故”的案例，比限位不够更常见。

总结

  1. 控制循环是实时系统：周期抖动、阻塞 IO、时间戳对齐，三个坑都比算法本身更致命
  2. 接口选型看底层：电调有速度环就用 Velocity 接口，别叠两层速度环
  3. 两段式切换：prepareSwitch/doSwitch 保证控制器交接的原子性
  4. CAN 对接注意边界：波特率、ID、溢出、超时急停
  5. 限位是双刃剑：软限位 + 旋转轴特殊处理