深入ego_planner状态机：从代码层面理解XTDrone中无人机的重规划决策逻辑
深入解析ego_planner状态机：XTDrone三维运动规划的核心决策逻辑
当无人机在复杂环境中执行任务时，毫秒级的决策延迟可能导致灾难性后果。XTDrone采用的ego_planner框架通过精巧的状态机设计，实现了动态环境下实时轨迹规划与重规划的平衡。本文将深入剖析其五种核心状态（INIT、WAIT_TARGET、GEN_NEW_TRAJ、EXEC_TRAJ、REPLAN_TRAJ）的转换机制，揭示无人机在面临突发障碍时的智能决策逻辑。
1. 状态机架构与基础工作流程
ego_planner的核心决策单元EGOReplanFSM类通过0.01秒高频触发的ExecFSMCallback函数维持状态运转。这种设计类似于人类神经系统的反射弧，能够在极短时间内完成环境感知-决策-执行的闭环。
状态机的五个基本状态构成完整任务周期：
| 状态 | 触发条件 | 典型行为 |
|---|---|---|
| INIT | 系统启动 | 等待传感器数据就绪 |
| WAIT_TARGET | 收到有效里程计和目标点 | 准备生成新轨迹 |
| GEN_NEW_TRAJ | 有新目标点且无现存轨迹 | 调用planFromGlobalTraj |
| EXEC_TRAJ | 轨迹生成成功 | 发布控制指令并监控执行 |
| REPLAN_TRAJ | 检测到碰撞或偏离预定路径 | 调用planFromCurrentTraj |
关键状态转换逻辑通过以下代码片段实现：
void EGOReplanFSM::ExecFSMCallback(const ros::TimerEvent& e) { static int counter = 0; if (counter++ % 100 == 0) cout << "[FSM]: state: " << exec_state_ << endl; switch (exec_state_) { case INIT: { if (!have_odom_) break; if (!trigger_) break; changeFSMState(WAIT_TARGET, "TRIG"); break; } // 其他状态处理... } }2. 全局规划与局部重规划的差异化策略
ego_planner最精妙的设计在于区分了planFromGlobalTraj和planFromCurrentTraj两种规划策略，分别应对不同场景：
全局规划(planFromGlobalTraj)特点：
- 适用于全新任务场景
- 从起点到终点完整规划
- 计算开销较大（约50-100ms）
- 采用A*全局搜索结合B样条优化
局部重规划(planFromCurrentTraj)特点：
- 适用于突发障碍规避
- 从当前状态延续已有轨迹
- 计算速度快（通常<20ms）
- 采用梯度下降法局部优化
bool EGOReplanFSM::planFromCurrentTraj() { // 获取当前运动状态 Eigen::Vector3d start_pos, start_vel, start_acc; local_data_.getTraj().getStates(ros::Time::now(), start_pos, start_vel, start_acc); // 调用优化器进行局部调整 bool success = planner_manager_->reboundReplan(start_pos, start_vel, start_acc); if (success) { local_data_.updateTraj(planner_manager_->getLocalTraj()); } return success; }3. 碰撞检测与动态响应机制
CheckCollisionCallback作为安全守护者，通过多层检测逻辑确保飞行安全：
目标点可行性检查：
- 在目标点周围半径0.5m球体内采样
- 采用极坐标离散搜索（5°间隔）
- 发现可行点立即触发REPLAN_TRAJ
轨迹实时监控：
def check_trajectory_safety(traj): for t in np.arange(0, traj.duration, 0.1): pos = traj.evaluate(t) if grid_map_.getInflateOccupancy(pos): return False return True紧急制动策略：
- 当连续3次重规划失败
- 立即切换为悬停模式
- 通过ROS服务通知地面站
典型碰撞响应流程：
- 检测到轨迹段occ==True
- 尝试planFromCurrentTraj
- 成功：继续执行
- 失败：检查剩余时间
- 充足：再次重规划
- 不足：紧急停止
4. 时空一致性保障技术
为确保状态转换时的运动连续性，ego_planner采用了一系列关键技术：
速度衔接算法：
- 新轨迹起始速度必须匹配当前实际速度
- 采用四阶B样条保证加速度连续
- 最大允许速度偏差阈值：0.3m/s
时间重参数化方法：
void retimeTrajectory(UniformBspline& traj) { double time_inc = 0.1; double total_time = 0.0; for (int i = 0; i < traj.getControlPointNum() - 1; ++i) { Eigen::Vector3d p1 = traj.getControlPoint(i); Eigen::Vector3d p2 = traj.getControlPoint(i+1); double dist = (p2 - p1).norm(); total_time += dist / max_vel_; } traj.setTimeSpan(total_time); }状态转换保护机制：
- WAIT_TARGET→GEN_NEW_TRAJ：必须确保里程计稳定
- EXEC_TRAJ→REPLAN_TRAJ：需满足最小重规划间隔
- REPLAN_TRAJ→EXEC_TRAJ：需通过轨迹安全检查
5. 多传感器融合下的状态决策
ego_planner通过grid_map整合多源传感器数据，为状态机提供环境感知支持：
传感器数据处理流水线：
- depthPoseCallback接收深度图像和相机位姿
- projectDepthImage将2D深度图投影到3D空间
- raycastProcess生成体素化占据地图
- clearAndInflateLocalMap进行障碍物膨胀
关键参数配置：
- 占据地图分辨率：0.1m
- 安全膨胀半径：0.3m
- 地图更新频率：20Hz
- 最大感知范围：10m×10m×4m
void GridMap::updateOccupancyCallback(const ros::TimerEvent& event) { if (!md_.occ_need_update_) return; // 深度图反投影 projectDepthImage(); // 光线投射处理 if (raycastProcess()) { md_.local_updated_ = true; clearAndInflateLocalMap(); } }6. 实战中的状态机调优经验
在实际无人机项目中，状态机参数的微调往往决定系统性能：
关键调优参数表：
| 参数 | 默认值 | 调整范围 | 影响维度 |
|---|---|---|---|
| 重规划检测阈值 | 0.3m | 0.2-0.5m | 安全性与计算负载 |
| 状态检查频率 | 100Hz | 50-200Hz | 响应延迟与CPU占用 |
| 轨迹前瞻时间 | 1.0s | 0.5-2.0s | 避障提前量 |
| 最大重规划尝试次数 | 3 | 2-5 | 系统鲁棒性 |
常见问题排查指南：
- 状态卡在INIT：
- 检查/odom话题是否正常发布
- 确认have_odom_标志位变化
- 频繁REPLAN_TRAJ：
- 调大安全膨胀半径
- 降低最大飞行速度
- 轨迹执行抖动：
- 检查B样条阶数（建议≥4）
- 验证时间分配均匀性
在XTDrone的Gazebo仿真中，可以通过以下命令实时监控状态机：
rostopic echo /iris_0/ego_planner_node/fsm_state