当前位置：首页 > news >正文

深入ego_planner状态机：从FSM回调函数看无人机如何应对突发障碍与目标点变化

news 2026/4/21 18:41:15

深入解析ego_planner状态机：无人机动态避障与轨迹重规划的核心逻辑

当无人机在复杂环境中执行任务时，如何实时应对突发障碍和目标点变化是运动规划算法的核心挑战。ego_planner通过精心设计的状态机机制，实现了从初始规划到动态调整的全流程自动化决策。本文将深入剖析ego_replan_fsm.cpp中的五个关键状态（INIT、WAIT_TARGET、GEN_NEW_TRAJ、EXEC_TRAJ、REPLAN_TRAJ）及其转换逻辑，揭示无人机在三维空间中的智能决策过程。

1. 状态机架构与核心组件

ego_planner的状态机本质上是一个事件驱动的决策系统，其运行依赖于三个关键组件：

传感器数据流：通过odometryCallback获取无人机实时位姿，depthPoseCallback处理深度相机数据
规划决策层：planner_manager负责轨迹生成与优化，包含A*搜索和B样条优化模块
执行控制层：traj_server将规划结果转化为控制指令，完成闭环执行

状态转换的核心判断条件如下表所示：

状态变量	触发条件	关联回调函数
`have_odom`	接收到有效里程计数据	`odometryCallback`
`trigger`	接收到新航点指令	`waypointCallback`
`occ`	检测到障碍物碰撞风险	`CheckCollisionCallback`

// 典型状态判断逻辑示例 if (exec_state_ == INIT && have_odom_ && trigger_) { exec_state_ = WAIT_TARGET; publishSwarmTrajs(); }

2. 状态转换的完整生命周期

2.1 初始化与目标等待（INIT → WAIT_TARGET）

系统启动后进入INIT状态，此时需要满足两个基本条件才会转换：

获取到稳定的里程计数据（have_odom == true）
接收到任务触发信号（trigger == true）

注意：在实际部署中，建议添加传感器健康状态检查，避免无效状态转换。

2.2 全局轨迹生成（GEN_NEW_TRAJ）

当系统获得新目标点时，会触发全局规划流程：

调用planFromGlobalTraj()生成初始B样条轨迹
使用mini-snap方法进行轨迹优化
通过Bspline_pub发布可行轨迹

def planFromGlobalTraj(): if not callReboundReplan(): return False adjustTimeAllocation() # 关键时间参数调整 publishTrajectory()

典型问题处理方案：

规划失败：保持GEN_NEW_TRAJ状态继续尝试
优化超时：降低轨迹平滑度要求或缩短规划视界

2.3 执行与动态调整（EXEC_TRAJ ↔ REPLAN_TRAJ）

轨迹执行期间的系统行为遵循以下决策树：

if 到达终点阈值范围内: 切换至WAIT_TARGET elif 轨迹执行超时: 终止当前任务 elif 检测到障碍物: if planFromCurrentTraj()成功: 更新轨迹继续执行 else: 进入紧急悬停模式

关键参数配置建议：

重规划阈值：通常设为2-3倍机体半径
时间容限：保留10%-15%的时间余量应对突发状况

3. 碰撞检测与应急处理机制

CheckCollisionCallback实现了多层次的安全防护：

前瞻性检测：在轨迹执行前预判碰撞风险
动态避障：发现障碍物时自动生成绕飞路径
目标点调整：当目标点被占据时自动寻找最近安全点

障碍物处理流程：

通过grid_map获取障碍物三维信息
使用膨胀模型计算安全距离
在球坐标系下离散采样候选路径

// 障碍物规避示例代码 for (double r = 0.5; r <= 3.0; r += 0.5) { for (double theta = 0; theta < 2*M_PI; theta += M_PI/6) { Eigen::Vector3d new_goal = goal + r*Eigen::Vector3d(cos(theta), sin(theta), 0); if (!checkCollision(new_goal)) { return new_goal; } } }