ROS 2 控制系统实战：ros2_control 从硬件接口到控制器

做移动机器人时，很多人第一眼会盯着 Nav2：地图、定位、规划、避障、RViz 里点一个目标点，机器人开始走。真正上硬件后才发现，问题通常不在“路径算不算得出来”，而在更底层：

Nav2 算出了 /cmd_vel
底盘有没有收到？
轮子命令单位对不对？
编码器反馈有没有回到 /odom？
TF 树是不是连续？
控制循环有没有按频率跑？
急停、限速、失联超时有没有兜底？

这就是 ros2_control 要解决的边界。它不是导航算法，也不是电机驱动本身，而是 ROS 2 里连接“上层软件意图”和“真实硬件执行”的控制框架。

一、2026 年该选哪个 ROS 2 版本

先把版本说清楚。2026 年做新项目，不建议从过时教程里的 Foxy / Galactic / Iron 起步。官方 ROS 2 distribution 页面显示：Lyrical Luth 已在 2026-05-22 发布，EOL 到 2031-05；Jazzy Jalisco 是 2024-05 发布的长期支持版本，EOL 到 2029-05；Humble Hawksbill 仍支持到 2027-05；Kilted Kaiju 支持到 2026-12。 [¹]

工程建议：

场景	推荐版本	原因
新项目、能接受较新生态	Lyrical	生命周期最长，面向 2026 之后
生产项目、Ubuntu 24.04、生态稳定	Jazzy	LTS，支持到 2029，资料和包更稳
老项目维护、Ubuntu 22.04	Humble	仍在支持期，工业项目常见
过渡验证	Kilted	标准版本，生命周期短，不适合作长期产品基线

下面的架构讲法适用于 ROS 2 近年的主线，但参数默认值、消息类型和插件名称会随发行版变化。写配置时一定看你实际发行版对应的文档，不要混用 Humble 教程和 Lyrical 包。

二、ros2_control 解决的不是“控制算法”，而是“控制边界”

ros2_control 的核心价值是把四件事拆开：

1. 机器人描述：URDF / Xacro 里声明有哪些关节、执行器、传感器；
2. 硬件访问：硬件插件负责和电机板、CAN、串口、EtherCAT、MCU 通信；
3. 控制器：速度控制、轨迹控制、差速底盘控制、关节状态发布；
4. 运行管理：加载、配置、激活、切换控制器，管理实时循环和接口占用。

可以把它理解成一条清晰的数据链路：

官方 Getting Started 文档里，Controller Manager 负责连接控制器和硬件抽象侧；Resource Manager 负责加载硬件组件、管理生命周期，并让控制器访问 state / command interfaces。控制循环里，硬件侧的 read() 和 write() 分别处理硬件状态读取和命令下发。 [²]

这意味着：你的电机驱动不应该散落在导航节点里，也不应该让 Nav2 直接知道串口协议。 Nav2 只需要发机器人速度目标；控制器负责把速度目标变成轮子命令；硬件插件负责把轮子命令写到真实设备。

三、三个最重要的对象

1. Hardware Component

硬件组件是你和真实机器人打交道的地方。官方文档把硬件分成三类：

类型	典型硬件	读写能力
system	差速底盘、机械臂、整车底盘、多个执行器共享一个通信通道	通常既读又写
actuator	单个电机、阀、舵机	通常既读又写
sensor	编码器、力矩传感器、IMU、距离传感器	通常只读

移动底盘通常建成 system：一个硬件插件里同时处理左右轮速度命令、编码器反馈、电机状态、急停状态。

伪代码长这样：

class BaseHardware : public hardware_interface::SystemInterface {
public:
  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;

  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override {
    // 从 MCU / CAN / EtherCAT 读取编码器、速度、电流、错误码
    left_position_ = driver_.left_encoder_position();
    right_position_ = driver_.right_encoder_position();
    return hardware_interface::return_type::OK;
  }

  hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override {
    // 把控制器写入的 command interface 下发到硬件
    driver_.set_wheel_velocity(left_velocity_cmd_, right_velocity_cmd_);
    return hardware_interface::return_type::OK;
  }
};

关键是：控制器并不直接调用你的驱动类。它只读写接口值。read() 把硬件状态刷新到 state interfaces；控制器在 update() 里算 command；write() 再把 command 写到硬件。

2. Controller Manager

controller_manager 是控制系统的调度器。它负责：

• 从 URDF 解析 <ros2_control>；
• 用 pluginlib 加载硬件插件；
• 加载控制器插件；
• 管理控制器 lifecycle；
• 在控制循环中协调 read()、controller update()、write()；
• 暴露 ros2 control CLI 和 service 接口。

常用调试命令：

ros2 control list_hardware_components
ros2 control list_hardware_interfaces
ros2 control list_controllers
ros2 control load_controller joint_state_broadcaster
ros2 control set_controller_state diff_drive_controller active

第一次上硬件时，不要先跑 Nav2。先保证这些命令能看到硬件接口和控制器状态。

3. Controllers

控制器是真正把“目标”转成“命令接口”的模块。常用控制器：

控制器	用途	输入	输出
joint_state_broadcaster	发布关节状态	state interfaces	/joint_states
diff_drive_controller	差速底盘	body twist	左右轮 velocity command
joint_trajectory_controller	机械臂/多关节轨迹	JointTrajectory	position / velocity / effort command
forward command controllers	直接转发命令	topic / reference	对应 command interface

diff_drive_controller 是移动机器人最常见的控制器。官方文档说明它接收机器人本体速度命令，将其转换为差速轮命令，并根据硬件反馈计算里程计；它还支持实时安全实现、里程计发布、速度/加速度/jerk 限制、命令超时自动停止和 chainable controller。 [³]

这条链路通常是：

四、最小配置长什么样

URDF 里声明硬件和接口：

<ros2_control name="base_hardware" type="system">
  <hardware>
    <plugin>my_robot_control/BaseHardware</plugin>
    <param name="serial_port">/dev/ttyUSB0</param>
    <param name="baud_rate">115200</param>
  </hardware>

  <joint name="left_wheel_joint">
    <command_interface name="velocity"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>

  <joint name="right_wheel_joint">
    <command_interface name="velocity"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

YAML 里加载 broadcaster 和差速控制器：

controller_manager:
  ros__parameters:
    update_rate: 50

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]
    wheel_separation: 0.32
    wheel_radius: 0.05
    odom_frame_id: odom
    base_frame_id: base_link
    enable_odom_tf: true
    publish_limited_velocity: true

这只是骨架。真实机器人一定要标定：

• wheel_radius：实际半径不是轮子标称值；
• wheel_separation：左右轮接地点距离，不是机械外宽；
• 左右轮方向：一个轮子反了会导致原地转圈；
• 编码器单位：tick、rad、rev、m/s 不要混；
• timeout：上层失联时必须停；
• emergency stop：硬件急停不应该依赖 ROS topic。

五、Nav2 和 ros2_control 的边界

Nav2 的输出不是“电机命令”，而是机器人层面的速度或路径跟随目标。典型边界：

层	负责什么	不该负责什么
Nav2 Planner	全局路径	电机协议
Nav2 Controller Server	局部跟踪、避障、输出速度	编码器读取
Velocity Smoother	平滑速度、限加速度、超时归零	轮子里程计
ros2_control Controller	把机器人速度转成关节命令	地图规划
Hardware Component	读写硬件	路径选择
MCU / Driver	电流环、速度环、保护	ROS 行为树

官方 Nav2 velocity smoother 文档说明，它用于平滑 Nav2 发送给机器人控制器的速度，降低加速度/jerk 对电机和硬件控制器的冲击，并可在高于 controller server 的频率插值发布速度命令。 [⁴]

所以工程上推荐：

Nav2 controller_server
  -> nav2_velocity_smoother
  -> diff_drive_controller
  -> hardware_interface
  -> motor firmware

不要让 Nav2 直接发串口；也不要让 MCU 做全局避障。边界清楚后，问题才可定位。

六、真实机器人调试顺序

我建议按这个顺序上电调试：

Step 1：只启动 robot_state_publisher 和 ros2_control

目标：URDF、硬件插件、控制器能加载。

检查：

ros2 control list_hardware_components
ros2 control list_hardware_interfaces
ros2 control list_controllers

如果 hardware interface 不存在，优先查：

• <plugin> 名称是否和 pluginlib_export_plugin_description_file 匹配；
• URDF 里的 joint 名是否和 robot_description 里的 joint 完全一致；
• YAML controller type 是否和安装包版本一致；
• 包是否 source 了正确 workspace。

Step 2：只跑 joint_state_broadcaster

目标：编码器状态能进入 ROS。

检查：

ros2 topic echo /joint_states
ros2 topic hz /joint_states

如果 /joint_states 不动，不要跑 Nav2。先看硬件 read() 有没有读到真实编码器。

Step 3：只跑 diff_drive_controller

目标：手动发速度，底盘能走，/odom 能回。

ros2 topic pub /diff_drive_controller/cmd_vel geometry_msgs/msg/TwistStamped \
"{header: {frame_id: base_link}, twist: {linear: {x: 0.1}, angular: {z: 0.0}}}"

注意不同发行版和配置里，cmd_vel 可能是 Twist 或 TwistStamped，Nav2 Kilted 以后越来越多默认走 stamped 速度消息。实际以你的控制器文档和参数为准。

检查：

ros2 topic echo /diff_drive_controller/odom
ros2 run tf2_tools view_frames
ros2 topic echo /diff_drive_controller/cmd_vel_out

Step 4：接入 Nav2，但先低速

目标：Nav2 能发目标，底层能跟踪，不出现漂移/振荡/恢复循环。

先把最大速度和角速度压低：

max_vel_x: 0.15
max_vel_theta: 0.6
max_accel_x: 0.3
max_accel_theta: 1.0

确认稳定后再提升参数。不要一开始按仿真速度跑真实机器人。

七、最常见的 10 个坑

症状	常见原因	排查方向
list_controllers 只有 broadcaster	controller YAML 没加载或 type 写错	看 controller_manager 日志
控制器 active 但轮子不动	command interface 没写到硬件	打印 write() 中命令值
机器人只原地转	左右轮方向/关节名反了	单独下发左右轮速度
里程计方向反	编码器符号或 TF 坐标反	检查 REP-105 坐标约定
Nav2 一发目标就恢复	/odom、TF、costmap 时间不连续	看 /tf, /odom, /clock
速度很抖	controller rate、smoother、MCU 速度环冲突	分层限速，检查 loop rate
停不下来	command timeout 没配置	检查 diff_drive timeout 和 MCU watchdog
走直线偏一边	轮径/轮距未标定	调 wheel multipliers
仿真正常、实物失败	Gazebo 插件绕过了真实硬件延迟	在硬件层加日志和限幅
控制器切换失败	接口被其它 controller 占用	list_hardware_interfaces 看 claimed 状态

八、上线清单

一台能跑 Nav2 的真实机器人，底层控制至少要满足：

• ros2 control list_controllers 能明确看到 active/inactive；
• /joint_states 频率稳定；
• /odom 与轮子运动一致；
• TF 树满足 map -> odom -> base_link -> sensors；
• 手动 /cmd_vel 测试通过；
• 上层失联超时能停；
• MCU 自身有 watchdog 和急停；
• 最大速度、加速度、jerk 限制分层配置；
• 控制器日志能区分硬件断连、编码器异常、命令超时；
• rosbag 记录 /cmd_vel、/odom、/tf、/joint_states、诊断 topic。

结论：ros2_control 的价值不是让机器人“更聪明”，而是让机器人“可控、可测、可替换”。先把底层控制链路做硬，再谈导航算法。否则 Nav2、SLAM、行为树调得再久，最后也会被一个轮径参数或编码器符号拖死。

参考资料

[¹] ROS 2 Distributions: https://docs.ros.org/en/kilted/Releases.html
[²] ros2_control Getting Started: https://control.ros.org/rolling/doc/getting_started/getting_started.html
[³] diff_drive_controller: https://control.ros.org/rolling/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html
[⁴] Nav2 Velocity Smoother: https://docs.nav2.org/configuration/packages/configuring-velocity-smoother.html