越野环境自动驾驶状态机：基于TinyFSM的工程实践

引言：为什么越野决策需要状态机？

与结构化道路不同，越野环境没有清晰的车道线、没有固定交通规则、路况瞬息万变。沙地、泥泞、陡坡、涉水、岩石攀爬------这些场景对决策系统的鲁棒性提出了极高要求。

传统的端到端方法在越野场景下面临三大困境：

1. 缺乏训练数据------极端工况的组合爆炸
2. 可解释性差------无法解释为什么陷车
3. 故障容错难------单个模块出错导致全系统崩溃

有限状态机（FSM）的价值 不在于处理所有细节，而在于构建决策的骨架------让系统在正确的时机做正确的事情，将复杂问题分解为可管理的状态集合。

本文将以无人越野车穿越未知地形为背景，使用TinyFSM实现一个生产级状态机，涵盖：

• 越野场景的状态划分与转换逻辑
• TinyFSM的高级特性应用（多状态机协同、条件转换）
• 与感知、规划、控制模块的接口设计

---

一、系统设计：越野状态机的架构哲学

1.1 核心设计原则

原则1：状态即责任

每个状态对应一个明确的系统责任。例如：

• Recovery状态的责任是"脱困"，而不是"向前开"
• ObstacleNegotiation状态的责任是"通过障碍"，而不是"绕行"

原则2：转换即决策

状态转换点集中体现了决策逻辑。所有"if-else"都应映射为状态转换条件。

原则3：事件即刺激

传感器数据、规划结果、安全监控都抽象为事件，统一流入状态机。

1.2 越野场景状态划分

我们设计8个核心状态，覆盖越野任务全生命周期：

状态定义表：

状态ID	名称	责任描述	退出条件
S_OFFLINE	离线	系统未就绪，不响应任何指令	自检通过
S_INIT	初始化	传感器校准、定位初始化	RTK Fix、IMU对准
S_IDLE	待命	等待任务指令，可手动遥控	收到MissionStart
S_NORMAL	正常行驶	平缓地形、目标速度巡航	路面粗糙度>阈值
S_SLOW_GO	缓行	颠簸/松软路面，降低速度	路面恢复平整
S_OBSTACLE	避障绕行	障碍物占据可通行区域	绕行路径生成
S_WATER	涉水模式	防水策略、匀速通过	水深<0.3m
S_ROCK	岩石攀爬	低速高扭矩、精确轮控	脱离岩石区
S_RECOVERY	脱困/倒车	无法前进时自主倒车/寻路	脱困成功/人工接管
S_EMERGENCY	紧急停车	安全边界被突破	安全员确认
S_COMPLETE	任务完成	到达终点，上传日志	新任务下发

---

二、TinyFSM实现：从抽象到代码

2.1 项目结构

offroad_fsm/
├── include/
│   ├── events.hpp        # 事件定义
│   ├── fsm_base.hpp      # 状态机基类
│   ├── mission_fsm.hpp   # 任务状态机
│   ├── safety_fsm.hpp    # 安全监控状态机（独立）
│   └── vehicle.hpp       # 车辆接口抽象
├── src/
│   ├── main.cpp          # 入口与事件循环
│   └── vehicle_sim.cpp   # 模拟车辆实现
└── CMakeLists.txt

2.2 事件定义：系统的"神经信号"

// events.hpp
#pragma once
#include "tinyfsm.hpp"

// 所有事件基类
struct OffroadEvent : tinyfsm::Event { };

// ============ 任务指令事件 ============
struct EvMissionStart : OffroadEvent {
    int mission_id;
    std::vector<Waypoint> waypoints;
    EvMissionStart(int id, const std::vector<Waypoint>& wps) 
        : mission_id(id), waypoints(wps) { }
};

struct EvMissionAbort : OffroadEvent { };

// ============ 感知事件 ============
struct EvTerrainUpdate : OffroadEvent {
    TerrainType type;      // SAND, MUD, ROCK, WATER
    float roughness;       // 0-1, 路面粗糙度
    float slope;           // 坡度(度)
    float water_depth;     // 水深(m)
};

struct EvObstacleDetected : OffroadEvent {
    std::vector<Point> footprint;  // 障碍物轮廓
    float distance;                // 距离(m)
    bool is_surpassable;           // 是否可跨越
};

// ============ 规划/控制事件 ============
struct EvPathGenerated : OffroadEvent {
    std::vector<PathPoint> path;   // 绕行/脱困路径
    bool success;
};

struct EvVehicleStatus : OffroadEvent {
    float speed;           // 当前车速(m/s)
    float wheel_slip;      // 滑转率
    bool is_stuck;         // 陷车标志
};

// ============ 安全事件 ============
struct EvSafetyTimeout : OffroadEvent {
    std::string module;    // 超时模块
};

struct EvEStop : OffroadEvent { };  // 急停按钮/远程指令

2.3 状态机基类：统一的反应接口

// fsm_base.hpp
#pragma once
#include "tinyfsm.hpp"
#include "events.hpp"
#include "vehicle.hpp"

class OffroadFSM : public tinyfsm::Fsm<OffroadFSM> {
protected:
    // 所有状态机共享的车辆接口
    static std::shared_ptr<VehicleInterface> vehicle;
    
    // 当前目标速度(所有状态可通过此字段与下层控制通信)
    static float target_speed;
    static float target_steering;

public:
    // 基类初始化(在整个程序启动时调用一次)
    static void setVehicle(std::shared_ptr<VehicleInterface> v) {
        vehicle = v;
    }

    // 默认事件处理：忽略所有未重载的事件
    void react(tinyfsm::Event const &) { }
    
    // 必须由具体状态实现的事件接口
    virtual void react(EvMissionStart const &) = 0;
    virtual void react(EvMissionAbort const &) = 0;
    virtual void react(EvTerrainUpdate const &) = 0;
    virtual void react(EvObstacleDetected const &) = 0;
    virtual void react(EvPathGenerated const &) = 0;
    virtual void react(EvVehicleStatus const &) = 0;
    virtual void react(EvSafetyTimeout const &) = 0;
    virtual void react(EvEStop const &) = 0;
    
    // 进入/退出动作
    virtual void entry() { }
    virtual void exit()  { }
    
    // 获取当前状态名称(用于日志/监控)
    virtual std::string getStateName() const = 0;
    
    // 状态机复位
    static void reset() {
        tinyfsm::StateList<
            S_Offline, S_Initialize, S_Idle,
            S_Normal, S_SlowGo, S_Obstacle,
            S_Water, S_Rock, S_Recovery,
            S_Emergency, S_Complete
        >::reset();
        target_speed = 0.0f;
        target_steering = 0.0f;
    }
};

// 静态成员定义
std::shared_ptr<VehicleInterface> OffroadFSM::vehicle = nullptr;
float OffroadFSM::target_speed = 0.0f;
float OffroadFSM::target_steering = 0.0f;

2.4 核心状态实现：以NORMAL与RECOVERY为例

状态1：正常行驶（S_Normal）

// mission_fsm.hpp (部分)
class S_Normal : public OffroadFSM {
private:
    float cruise_speed = 5.0f;  // 18km/h越野巡航
    
public:
    void entry() override {
        vehicle->enableAutonomousMode();
        vehicle->setLightSignal(LightSignal::CRUISE);
        target_speed = cruise_speed;
        Logger::info(">> 进入状态: 正常行驶 @ %.1f m/s", cruise_speed);
    }
    
    void exit() override {
        vehicle->setLightSignal(LightSignal::NONE);
        Logger::info("<< 退出状态: 正常行驶");
    }
    
    void react(EvTerrainUpdate const & e) override {
        // 越野粗糙路面 -> 降速缓行
        if (e.roughness > 0.6) {
            Logger::warn("路面粗糙度%.2f > 0.6, 切换至缓行模式", e.roughness);
            transit<S_SlowGo>();
        }
        // 涉水区域
        else if (e.water_depth > 0.3) {
            Logger::warn("水深%.2fm > 0.3, 切换至涉水模式", e.water_depth);
            transit<S_Water>();
        }
        // 保持正常行驶
    }
    
    void react(EvObstacleDetected const & e) override {
        if (e.distance < 10.0 && !e.is_surpassable) {
            // 10米内不可跨越障碍 -> 避障模式
            Logger::warn("障碍物距离%.1fm, 无法跨越, 切换至避障", e.distance);
            transit<S_Obstacle>();
        }
        // 可跨越障碍(草丛、小石块)继续行驶
    }
    
    void react(EvVehicleStatus const & e) override {
        if (e.is_stuck) {
            Logger::error("检测到陷车, 切换至脱困模式");
            transit<S_Recovery>();
        }
        // 更新目标速度(无级变速)
        target_speed = cruise_speed * (1.0 - e.wheel_slip * 0.5);
    }
    
    void react(EvMissionAbort const &) override {
        transit<S_Idle>();
    }
    
    void react(EvEStop const &) override {
        transit<S_Emergency>();
    }
    
    std::string getStateName() const override { return "NORMAL"; }
};

状态2：脱困/倒车（S_Recovery）

class S_Recovery : public OffroadFSM {
private:
    int retry_count = 0;
    static constexpr int MAX_RETRY = 3;
    
public:
    void entry() override {
        retry_count = 0;
        vehicle->enableAutonomousMode();
        vehicle->setLightSignal(LightSignal::RECOVERY);
        target_speed = -1.5f;  // 倒车1.5m/s
        Logger::warn(">> 进入状态: 脱困模式(尝试%d)", retry_count + 1);
        
        // 触发路径规划: 寻找倒车/绕行路径
        vehicle->requestRecoveryPath();
    }
    
    void react(EvVehicleStatus const & e) override {
        // 如果倒车后仍陷车
        if (e.is_stuck && vehicle->getDistanceMoved() < 0.5) {
            retry_count++;
            if (retry_count >= MAX_RETRY) {
                Logger::error("脱困失败%d次, 请求人工接管", MAX_RETRY);
                transit<S_Emergency>();
            } else {
                Logger::warn("脱困尝试%d/%d失败, 调整方向重试", 
                           retry_count, MAX_RETRY);
                // 尝试不同转向角度
                target_steering = (retry_count % 2 == 0) ? 0.5f : -0.5f;
            }
        }
    }
    
    void react(EvPathGenerated const & e) override {
        if (e.success && !e.path.empty()) {
            Logger::info("脱困路径生成成功, 长度: %zu", e.path.size());
            vehicle->followPath(e.path);
            transit<S_Normal>();  // 脱困成功, 恢复正常行驶
        }
    }
    
    void react(EvObstacleDetected const &) override {
        // 脱困过程中忽略新障碍物
        Logger::debug("脱困中, 忽略障碍物");
    }
    
    void react(EvTerrainUpdate const &) override {
        // 脱困中不响应地形变化
    }
    
    void react(EvMissionAbort const &) override {
        transit<S_Idle>();
    }
    
    void react(EvEStop const &) override {
        transit<S_Emergency>();
    }
    
    std::string getStateName() const override { return "RECOVERY"; }
};

2.5 独立安全监控状态机

越野场景对安全监控 要求极高。我们实现一个独立的状态机，与主任务状态机并行运行：

// safety_fsm.hpp
class SafetyFSM : public tinyfsm::Fsm<SafetyFSM> {
private:
    static std::chrono::steady_clock::time_point last_heartbeat;
    static constexpr int HEARTBEAT_TIMEOUT_MS = 500;  // 500ms无心跳则急停
    
public:
    void react(EvVehicleStatus const &) {
        last_heartbeat = std::chrono::steady_clock::now();
    }
    
    void react(tinyfsm::Event const &) { }
    
    void entry() override { }
    
    void exit() override { }
    
    // 周期性监控任务(在主循环调用)
    static void checkTimeout() {
        auto now = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
            now - last_heartbeat).count();
        
        if (elapsed > HEARTBEAT_TIMEOUT_MS) {
            Logger::fatal("控制模块心跳超时! %.1fms", (double)elapsed);
            send_event(EvSafetyTimeout{"control_heartbeat"});
            send_event(EvEStop());  // 触发急停
        }
    }
};

// 静态成员初始化
std::chrono::steady_clock::time_point SafetyFSM::last_heartbeat = 
    std::chrono::steady_clock::now();

关键设计 ：安全监控与主决策解耦，即使主状态机崩溃，SafetyFSM仍能独立触发急停。

---

三、状态机集成：多FSM协同工作

3.1 状态机列表与事件路由

// main.cpp
#include "tinyfsm.hpp"
#include "fsm_base.hpp"
#include "mission_fsm.hpp"
#include "safety_fsm.hpp"

// 定义状态机列表
using FsmHandle = tinyfsm::FsmList<
    OffroadFSM,      // 主任务状态机
    SafetyFSM        // 安全监控状态机
>;

// 全局事件分发函数
template<typename E>
void send_event(E const & event) {
    FsmHandle::template dispatch<E>(event);
}

// 初始化所有状态机
void initialize_fsms(std::shared_ptr<VehicleInterface> vehicle) {
    // 设置车辆接口(所有状态机共享)
    OffroadFSM::setVehicle(vehicle);
    
    // 设置初始状态
    FSM_INITIAL_STATE(OffroadFSM, S_Offline);
    FSM_INITIAL_STATE(SafetyFSM, SafetyFSM);  // SafetyFSM无状态
    
    // 启动所有状态机
    FsmHandle::start();
    
    Logger::info("所有状态机初始化完成");
}

3.2 主事件循环

int main(int argc, char** argv) {
    // 1. 初始化日志、车辆接口、传感器等
    Logger::init(LogLevel::DEBUG);
    auto vehicle = std::make_shared<VehicleSimulator>();
    vehicle->connect();
    
    // 2. 初始化状态机
    initialize_fsms(vehicle);
    
    // 3. 主循环(100Hz)
    const int CYCLE_MS = 10;
    while (vehicle->isOperational()) {
        auto cycle_start = std::chrono::steady_clock::now();
        
        // 3.1 获取传感器数据
        auto terrain = vehicle->getTerrainInfo();
        auto obstacle = vehicle->getNearestObstacle();
        auto status = vehicle->getStatus();
        
        // 3.2 转换为事件并分发
        send_event(EvTerrainUpdate{
            terrain.type, 
            terrain.roughness,
            terrain.slope,
            terrain.water_depth
        });
        
        if (obstacle.distance < 30.0) {
            send_event(EvObstacleDetected{
                obstacle.footprint,
                obstacle.distance,
                obstacle.is_surpassable
            });
        }
        
        send_event(EvVehicleStatus{
            status.speed,
            status.wheel_slip,
            status.is_stuck
        });
        
        // 3.3 安全监控超时检查
        SafetyFSM::checkTimeout();
        
        // 3.4 将决策结果(target_speed/target_steering)发送至控制模块
        vehicle->setControlTarget(
            OffroadFSM::target_speed, 
            OffroadFSM::target_steering
        );
        
        // 3.5 维持循环频率
        auto elapsed = std::chrono::steady_clock::now() - cycle_start;
        std::this_thread::sleep_for(
            std::chrono::milliseconds(CYCLE_MS) - elapsed);
    }
    
    return 0;
}

---

四、进阶技巧：让状态机更"聪明"

4.1 条件转换：带guard的transit

// 从SlowGo -> Normal: 只有当路面连续5秒保持良好时才升级
class S_SlowGo : public OffroadFSM {
private:
    int smooth_frames = 0;
    
public:
    void react(EvTerrainUpdate const & e) override {
        if (e.roughness < 0.3) {
            smooth_frames++;
            if (smooth_frames > 50) {  // 50帧 ≈ 5秒 @100Hz
                auto guard = [this] { 
                    return vehicle->isStable() && smooth_frames > 50; 
                };
                transit<S_Normal>(guard);
            }
        } else {
            smooth_frames = 0;  // 粗糙路面, 重置计数器
        }
    }
};

4.2 转换动作：在切换时执行额外逻辑

void react(EvWaterCrossed const &) override {
    auto action = [this] {
        // 涉水结束后, 自动执行刹车干燥程序
        vehicle->applyBrakes(0.2f, 3.0f);  // 轻刹3秒
        vehicle->setWiperSpeed(2);          // 开启雨刮
    };
    
    transit<S_Normal>(action);
}

4.3 状态内定时器

class S_Obstacle : public OffroadFSM {
private:
    std::chrono::steady_clock::time_point wait_start;
    
public:
    void entry() override {
        wait_start = std::chrono::steady_clock::now();
        vehicle->stop();  // 先停车观察
    }
    
    void react(EvTimer const & e) override {  // 自定义定时器事件
        auto now = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(
            now - wait_start).count();
        
        if (elapsed > 3) {
            // 停车3秒后, 请求绕行路径
            vehicle->requestBypassPath();
        }
    }
};

---

五、调试与监控：状态机的"黑匣子"

5.1 状态转换日志

// 在transit时自动记录
void transit(StateType & next_state) {
    Logger::debug("STATE_TRANSITION: %s -> %s", 
                  this->getStateName().c_str(), 
                  next_state.getStateName().c_str());
    tinyfsm::Fsm<OffroadFSM>::transit(next_state);
}

运行示例：

[2026-02-13 10:23:45.123] DEBUG: STATE_TRANSITION: NORMAL -> SLOW_GO
[2026-02-13 10:23:45.124] INFO: >> 进入状态: 缓行模式 @ 2.0 m/s
[2026-02-13 10:23:47.345] WARN: 路面粗糙度0.65 > 0.6, 切换至缓行模式
[2026-02-13 10:23:52.001] WARN: 障碍物距离8.3m, 无法跨越, 切换至避障
[2026-02-13 10:23:52.002] DEBUG: STATE_TRANSITION: SLOW_GO -> OBSTACLE

5.2 Web监控界面

// 提供状态机当前状态的REST API
void setup_web_server() {
    httplib::Server svr;
    
    svr.Get("/api/fsm/state", [](const auto& req, auto& res) {
        json state = {
            {"mission", OffroadFSM::getCurrentState()->getStateName()},
            {"safety", "ACTIVE"},  // SafetyFSM始终active
            {"target_speed", OffroadFSM::target_speed},
            {"target_steering", OffroadFSM::target_steering},
            {"timestamp", get_current_timestamp()}
        };
        res.set_content(state.dump(), "application/json");
    });
    
    svr.listen("0.0.0.0", 8080);
}

5.3 状态覆盖率测试

// 自动化测试：确保所有状态可达
void test_state_coverage() {
    std::vector<std::string> covered;
    
    auto test_transition = [&](OffroadFSM* from, OffroadFSM* to, auto event) {
        FSM_INITIAL_STATE(OffroadFSM, from);
        OffroadFSM::start();
        send_event(event);
        assert(OffroadFSM::getCurrentState()->getStateName() == 
               to->getStateName());
        covered.push_back(from->getStateName() + "->" + to->getStateName());
    };
    
    test_transition(new S_Idle, new S_Normal, EvMissionStart());
    test_transition(new S_Normal, new S_Recovery, EvVehicleStatus().set_stuck());
    // ... 测试所有转换路径
    
    Logger::info("状态覆盖测试完成: %zu条路径", covered.size());
}

六、典型场景下的状态迁移时序图

6.1正常行驶→陷车→脱困→正常

6.2涉水场景完整流程

---

附录：资源与致谢

• TinyFSM GitHub: https://github.com/digint/tinyfsm
• 越野数据集: RUGD, RELLIS-3D
• 感谢DARPA地下挑战赛团队公开的决策架构文档

---