状态机概述与实现

基类状态机

状态机基类 StateMachineBase 负责周期运行、状态切换和控制线程调度。

运行示例

run_thread_ = std::thread(&StateMachineBase::RunThread, this);
rclcpp::spin(ri_ptr_->get_node());

Run() 启动独立控制线程后，主线程阻塞在 rclcpp::spin 上处理 ROS2 回调。

定时循环示例

set_timer.time_init(5);   // 5ms 定时周期
while (rclcpp::ok()) {
    if (set_timer.time_interrupt()) {
        ri_ptr_->RefreshRobotData();
        current_controller_->Run();

        if (emergency_stop_requested_.exchange(false)) {
            next_state_name_ = StateName::kJointDamping;
        } else if (current_controller_->LoseControlJudge()) {
            next_state_name_ = StateName::kJointDamping;
        } else {
            next_state_name_ = current_controller_->GetNextStateName();
        }

        if (next_state_name_ != current_state_name_) {
            current_controller_->OnExit();
            current_controller_ = GetStateControllerPtr(next_state_name_);
            current_controller_->OnEnter();
            current_state_name_ = next_state_name_;
        }
    }
}

定时器基于 timerfd + epoll 实现，每 5ms 触发一次中断。每个周期依次执行：刷新传感器数据 → 运行当前状态控制器 → 急停/失控判断 → 状态切换。

状态机抽象

StateBase 是所有状态的抽象类，提供状态进入、退出、运行和切换判断接口。

状态接口

virtual void OnEnter() = 0;           // 进入状态时执行
virtual void OnExit() = 0;            // 退出状态时执行
virtual void Run() = 0;               // 每个控制周期执行
virtual bool LoseControlJudge() = 0;  // 失控判断
virtual StateName GetNextStateName() = 0;  // 获取下一状态

共享数据

每个状态通过 ControllerData 共享以下指针：

struct ControllerData {
    std::shared_ptr<RobotInterface> ri_ptr;          // 机器人接口
    std::shared_ptr<UserCommandInterface> uc_ptr;    // 用户命令接口
    std::shared_ptr<ControlParameters> cp_ptr;       // 控制参数
};

状态机实现

QStateMachine 是 Lite3 的具体状态机实现，包含 4 个控制状态。

启动示例

if (robot_name_ == RobotName::Lite3) {
    ri_ptr_ = std::make_shared<Lite3Interface>("Lite3");
    cp_ptr_ = std::make_shared<ControlParameters>(robot_name_);
    joints_data_shadow_subscriber_ =
        std::make_shared<JointsDataShadowSubscriber>(ri_ptr_->get_node());
}

状态构建

idle_controller_ = std::make_shared<IdleState>(robot_name_, "idle_state", data_ptr);
           // 空闲状态（待机）
standup_controller_ = std::make_shared<StandUpState>(robot_name_, "standup_state", data_ptr);
           // 站立状态
rl_controller_ = std::make_shared<RLControlState>(robot_name_, "rl_control", data_ptr);
           // RL 控制状态
joint_damping_controller_ = std::make_shared<JointDampingState>(robot_name_, "joint_damping", data_ptr);
           // 关节阻尼状态（安全保护）

状态切换

UserCommandInterface 中按键会直接设置目标状态：

usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);     // Y 键
usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);  // A 键
usr_cmd_->target_mode = uint8_t(RobotMotionState::JointDamping);   // 双摇杆按键

空闲状态（IdleState）

IdleState 是机器人的待机状态，负责传感器校验和用户指令等待。

进入行为

void OnEnter() {
    msfb_.UpdateCurrentState(RobotMotionState::WaitingForStand);
    enter_state_time_ = ri_ptr_->GetInterfaceTimeStamp();
}

周期运行

void Run() {
    GetProprioceptiveData();        // 读取关节位置/速度/力矩 + IMU
    joint_normal_flag_ = JointDataNormalCheck();  // 关节数据校验
    DisplayProprioceptiveInfo();    // 每秒打印本体感知信息
    MatXf cmd = MatXf::Zero(12, 5);
    ri_ptr_->SetJointCommand(cmd);  // 发送零力矩指令
}

状态跳转

关节数据异常 → 保持 kIdle，打印警告
safe_control_mode != 0 → 保持 kIdle
target_mode == StandingUp → 切换到 kStandUp

站立状态（StandUpState）

StandUpState 控制机器人从坐姿平滑过渡到站立姿态。

站立轨迹规划

站立过程分为两个阶段，总时长 2 × stand_duration_：

阶段一（0 ~ stand_duration_）： 从当前关节位置通过三次样条插值到预站立目标位置：

planning_joint_pos(i) = GetCubicSplinePos(init_pos, init_vel, goal_pos, 0, t, T);
planning_joint_vel(i) = GetCubicSplineVel(init_pos, init_vel, goal_pos, 0, t, T);

阶段二（stand_duration_ ~ 2×stand_duration_）： 从预站立高度插值到最终站立高度，通过逆运动学计算 HipY 和 Knee 角度：

float hipy_pos = GetHipYPosByHeight(plan_height);
float knee_pos = GetKneePosByHeight(plan_height);

状态跳转

safe_control_mode != 0 → kJointDamping
run_time > 2 × stand_duration_ 且 target_mode == RLControlMode → kRLControl
否则保持 kStandUp

RL 控制状态（RLControlState）

RLControlState 运行强化学习策略，通过独立线程执行 ONNX 推理。

策略初始化

auto model_path = fs::canonical(base / ".." / ".." / "policy" / "policy.onnx");
lite3_policy_ = std::make_shared<Lite3PolicyRunner>("lite3_policy", model_path.string());
policy_ptr_ = lite3_policy_;

推理线程

void PolicyRunner() {
    while (start_flag_) {
        if (state_run_cnt_ % policy_ptr_->decimation_ == 0) {
            auto ra = policy_ptr_->getRobotAction(rbs_, *(uc_ptr_->GetUserCommand()));
            ri_ptr_->SetJointCommand(ra.ConvertToMat());
        }
        std::this_thread::sleep_for(std::chrono::microseconds(100));
    }
}

推理线程以 decimation_ 分频运行，每次读取 RobotBasicState（RPY、角速度、加速度、关节状态）和用户指令，输出 12 关节命令。

退出清理

void OnExit() {
    start_flag_ = false;
    run_policy_thread_.join();  // 等待推理线程退出
    state_run_cnt_ = -1;
}

阻尼状态（JointDampingState）

JointDampingState 是安全保护状态，对所有关节施加纯阻尼力矩使机器人安全落地。

阻尼指令

JointDampingState(...) {
    VecXf kd_ = one_leg_kd.replicate(4, 1);
    joint_cmd_ = MatXf::Zero(12, 5);
    joint_cmd_.col(2) = kd_;   // 仅设置 kd，kp=0 即纯阻尼
}

状态跳转

StateName GetNextStateName() {
    if (run_time_ - time_record_ < 3.) return StateName::kJointDamping;
    return StateName::kIdle;
}

阻尼持续 3 秒后自动回到 kIdle。转出时重置 target_mode 为 WaitingForStand。

安全控制逻辑

SafeController 在独立线程中以 1ms 周期运行，监控机器人健康状态。

异常检测项

检测项	方法	触发条件	`safe_control_mode`
驱动器报错	`IsDriverStatusNormal()`	`status_word != 1`	2
关节数据异常	`IsJointDataNormal()`	NaN 值	3
电机过温	`IsMotorTempertureNormal()`	温度 > 100°C	2
IMU 数据异常	`IsImuDataNormal()`	NaN / 超时 0.2s	2
电池电量低	`IsBatteryNormal()`	电量 < 10%	—
手柄心跳超时	`IsUserCommandNormal()`	超时 1000ms	—

当 safe_control_mode != 0 时，各状态的 GetNextStateName() 会返回 kJointDamping，状态机自动进入安全阻尼。

错误状态码

union RobotErrorState {
    struct {
        uint32_t imu_num_error: 1;          // IMU 数值异常
        uint32_t imu_update_overtime: 1;    // IMU 更新超时
        uint32_t command_heartbeat_overtime: 1;  // 手柄心跳超时
        uint32_t driver_error: 1;           // 驱动器报错
        uint32_t motor_heat_warn: 1;        // 电机过温
        uint32_t joint_num_error: 1;        // 关节数值异常
        uint32_t joint_update_error: 1;     // 关节更新超时
        uint32_t battery_low_warn: 1;       // 电池低电量
        uint32_t battery_heat_warn: 1;      // 电池过温
        uint32_t cpu_heat_warn: 1;          // CPU 过温
        uint32_t cpu_freq_warn: 1;          // CPU 降频
    };
    uint32_t error_code;
};

跨进程急停

急停通过三层链路实现：

UDP 命令层：lite3_sdk_service 收到 estop 命令，调用 /EMERGENCY_STOP 服务
硬件层：lite3_transfer 向机器人固件发送急停命令码 0x21010C0E
状态机层：通过 /EMERGENCY_STOP_SIGNAL 跨进程通知 Lite3_sdk_deploy 切换到 JointDamping

急停信号发布

// lite3_transfer 中发布急停信号
estop_signal_pub_ = create_publisher<drdds::msg::StdMsgInt32>("/EMERGENCY_STOP_SIGNAL", 1);
drdds::msg::StdMsgInt32 sig;
sig.value = 1;
estop_signal_pub_->publish(sig);

急停信号订阅

// Lite3_sdk_deploy 中订阅急停信号
estop_signal_sub_ = ri_ptr_->get_node()->create_subscription<drdds::msg::StdMsgInt32>(
    "/EMERGENCY_STOP_SIGNAL", 1,
    [this](const drdds::msg::StdMsgInt32::SharedPtr msg) {
        emergency_stop_requested_.store(true);
    });