第4章 机器人控制系统架构

做机器人系统这么多年，我越来越觉得——控制架构就是机器人的骨架和神经。骨架撑得住，神经传得快，机器人才能干出漂亮活。这一章咱们就聊聊分层控制、实时要求、ROS2怎么用，还有控制循环怎么设计。嗯，都是实战里天天要碰的东西。

4.1 分层控制架构——为什么非分不可？

你想想看，一个机器人要同时做路径规划、避障、关节插补、电机电流环……如果全揉在一个循环里，代码根本没法维护。我个人习惯把控制拆成三层：决策层、协调层、执行层。每一层各司其职，接口清晰。

• 决策层：管“做什么”。比如抓杯子还是去充电，状态机跳转，全局路径。更新频率低，1~10 Hz 就够了。
• 协调层：管“怎么做”。把上层任务拆成运动轨迹、速度曲线，同时做安全校验。我一般跑在 50~200 Hz。
• 执行层：管“做出来”。关节位置环、电流环、编码器读取。硬实时，1~10 kHz 常见。

4.2 实时控制要求——别让机器人“卡顿”

机器人控制里，“实时”不是快，而是确定性。你必须在规定时间内完成计算，晚到就是失控。我见过不少团队在Linux上用普通进程做控制，一遇到网络中断就掉关节——嗯，那场面挺吓人的。

4.3 ROS2基础——为什么从ROS1迁移？

ROS2 不是简单的升级，而是面向产品级实时控制的重构。我个人最喜欢三点：

• DDS 通信：去中心化，节点挂了不影响其他节点。我在现场调试时经常单独重启某个模块，系统照跑。
• QoS 策略：你可以设置“可靠性”“历史记录”“时效性”。比如传感器数据用“尽力传输”，控制指令用“可靠传输”。
• 多节点/多进程：每个控制层可以独立部署，甚至跨机器。我习惯把执行层放在工控机，决策层放在云端。

// ROS2 发布者示例 (C++)
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64.hpp>

class JointPublisher : public rclcpp::Node {
public:
    JointPublisher() : Node("joint_publisher") {
        publisher_ = this->create_publisher<std_msgs::msg::Float64>("joint_cmd", 10);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(10),
            std::bind(&JointPublisher::timer_callback, this));
    }
private:
    void timer_callback() {
        auto msg = std_msgs::msg::Float64();
        msg.data = 0.5;  // 目标位置
        publisher_->publish(msg);
    }
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

4.4 控制循环设计——把理论跑起来

控制循环是机器人最核心的“心跳”。我设计循环时一般遵循这几个原则：

1. 固定周期：用定时器或实时线程，别用 sleep + 轮询。ROS2 的 create_wall_timer 在软实时场景够用。
2. 状态反馈闭环：每个循环必须读传感器、算控制量、发指令。开环控制只适合调试。
3. 看门狗机制：如果连续几个周期没收到上层指令，执行层要自动进入安全停止。我习惯设 3 个周期超时。

while (running) {
    wait_next_cycle();          // 等待下一个1ms tick
    joint_pos = read_encoder(); // 读当前关节位置
    cmd = get_latest_command(); // 从协调层取目标
    effort = pid_controller(cmd, joint_pos); // 计算力矩
    set_motor_current(effort);  // 发给驱动器
    check_watchdog();           // 超时则急停
    publish_state(joint_pos);   // 反馈给上层
}

说白了，控制循环就是“感知-决策-执行”的微缩版。你把它跑稳了，机器人就稳了。