1. 从状态机到轨迹执行EGO-Planner的完整工作流如果你刚接触EGO-Planner可能会被它代码库里一堆cpp文件搞得晕头转向。什么ego_replan_fsm.cpp、plan_manage.cpp、traj_server.cpp它们之间到底是怎么协作的规划器是怎么从一个想法目标点变成一条让无人机飞起来的平滑轨迹的今天我就带你像拆解一台精密的机械钟表一样把EGO-Planner从“大脑决策”到“手脚执行”的完整流程彻底捋清楚。简单来说EGO-Planner的工作流可以概括为“状态机决策 - 轨迹规划优化 - 轨迹发布执行”三大步。这听起来有点抽象我打个比方状态机就像是无人机的“驾驶模式切换开关”它根据当前情况有没有收到目标、有没有撞墙风险来决定现在该干什么——是等待指令、开始规划、执行轨迹还是紧急刹车。规划器则是“路径设计师”负责在复杂的障碍物地图里算出一条又快又安全还平滑的飞行路线。最后的轨迹服务器traj_server就是那个“指令翻译官”它把规划器输出的、我们人类看不懂的数学曲线B样条翻译成飞控能听懂的、每隔10毫秒一个的具体位置、速度、加速度指令。这个过程是实时、循环进行的。无人机一边飞状态机一边监控规划器一边根据新的传感器信息微调路线traj_server一边把最新的微调指令发给飞控。下面我们就钻进代码里看看这个精密的闭环到底是怎么转起来的。2. 大脑的决策核心状态机FSM深度解析一切故事的开端都在ego_replan_fsm.cpp这个文件里。这里定义了一个有限状态机Finite State Machine, FSM它是整个EGO-Planner的“大脑”和“指挥官”。状态机的工作就是根据一系列输入比如有没有收到目标点、当前是否安全、时间到了没让系统在不同的“工作模式”之间切换。EGO-Planner主要定义了这么几个核心状态INIT初始化系统刚启动时的状态。这时它啥也不干就等着必要的初始化数据到位比如里程计信息odom。一旦收到odom知道自己在哪里了就立刻切换到WAIT_TARGET状态。WAIT_TARGET等待目标在这个状态无人机通常是悬停的。状态机在等待一个“起飞”或“前往”的指令。这个指令可以来自你在RVIZ里手动点击的一个点话题/move_base_simple/goal也可以来自预设的一系列航点文件。一旦收到目标点状态就切换到SEQUENTIAL_START对于多机编队或直接触发规划。SEQUENTIAL_START顺序启动这个状态主要是为多机协同设计的。想象一下多架无人机依次起飞执行任务后一架飞机需要等前一架飞机规划并发布了自己的轨迹后才能开始规划以避免空中相撞。在这个状态里飞机会检查have_recv_pre_agent_这个标志位。只有当它收到了前一架飞机的轨迹信息或者它自己就是编队里的第一架飞机drone_id 0它才会开始进行首次全局规划planFromGlobalTraj。规划成功就进入EXEC_TRAJ失败则可能报错或重试。EXEC_TRAJ执行轨迹这是最主要的“飞行”状态。在这个状态下traj_server正在持续地将规划好的B样条轨迹转化为控制指令驱动无人机飞行。但状态机在这里可不是闲着它同时在干几件重要的事监控进度计算当前时间是否已经超过了这条局部轨迹的预设执行时间t_cur traj_duration_。判断终点如果快到终点了当前位置离局部轨迹终点很近并且是航点模式它会自动规划下一个航点。触发重规划这是关键如果飞行时间超过了重规划阈值replan_thresh_比如0.5秒或者检测到可能发生碰撞这由另一个定时器checkCollisionCallback负责状态机就会立刻从EXEC_TRAJ切换到REPLAN_TRAJ。REPLAN_TRAJ重规划轨迹这是系统的“纠错”状态。当无人机飞行中遇到未预料到的障碍物或者需要动态调整路线时就会进入此状态。它会调用planFromCurrentTraj()函数从当前轨迹的位置和状态出发快速规划一条新的、绕过障碍的轨迹。如果重规划成功状态切回EXEC_TRAJ继续飞行如果失败可能会根据情况切换到EMERGENCY_STOP。EMERGENCY_STOP紧急停止安全兜底状态。当碰撞检查模块checkCollisionCallback发现无人机即将在很短时间内emergency_time_撞上障碍物且重规划失败时就会强制进入这个状态。它会调用callEmergencyStop()生成一条紧急制动轨迹通常是当前位置的一个停留点并立刻通过bspline_pub_发布出去命令无人机紧急悬停。状态机的运转由一个高频率的定时器exec_timer_通常10ms驱动每次回调都执行execFSMCallback()函数根据当前状态执行相应逻辑并判断是否需要切换状态。这个设计让系统对外部事件新目标、碰撞风险的响应非常及时。2.1 状态切换的“心跳”定时器与回调函数状态机不是自己凭空运行的它依赖于两个关键的定时器就像是它的“心跳”和“安全哨兵”。exec_timer_执行定时器这是主心跳间隔很短代码中常设为10ms。每次触发就调用execFSMCallback()函数。你可以在这个函数里看到那个巨大的switch(exec_state_)语句它根据当前状态执行对应的动作并判断是否满足跳转到下一个状态的条件。比如在EXEC_TRAJ状态下它每10毫秒就会检查一次“是不是该重规划了”。safety_timer_安全定时器这是安全哨兵间隔稍长如50ms。它驱动checkCollisionCallback()回调函数。这个函数的工作非常关键它前瞻性地检查当前正在执行的轨迹。具体做法是沿着规划好的轨迹以一定步长向前采样一系列点然后用grid_map_地图查询这些点是否被占据有障碍物。同时在多机模式下它还会检查这些点与其他无人机发布的轨迹是否太近距离小于swarm_clearance_。一旦发现碰撞风险occ 1它不会等待主定时器而是立即尝试重规划planFromCurrentTraj。如果重规划也来不及了就立刻触发EMERGENCY_STOP。这两个定时器一快一慢一个管流程推进一个管安全兜底共同保证了无人机既灵活又可靠。2.2 多机协同的通信纽带轨迹的发布与订阅EGO-Planner的多机协同能力就体现在状态机对几个特殊发布者Publisher和订阅者Subscriber的运用上。在初始化函数init()里你会看到这样的代码if (drone_id 1) { swarm_trajs_sub_ nh.subscribe(...); // 订阅前一架飞机的轨迹 } swarm_trajs_pub_ nh.advertise(...); // 发布本机轨迹给下一架飞机 broadcast_bspline_pub_ nh.advertise(...); // 广播本机轨迹swarm_trajs_pub_/sub_这是顺序通信链。假设有3架飞机ID: 0, 1, 2。飞机1会订阅飞机0的swarm_trajs话题飞机2订阅飞机1的。当飞机0规划好轨迹后在publishSwarmTrajs()函数中它会把自己的轨迹ID和B样条数据打包发布到自己的swarm_trajs_pub_。飞机1收到后会将这条轨迹存入swarm_trajs_buf_缓冲区并在自己的碰撞检测中避开它。同时飞机1规划时也会参考这条轨迹。当飞机1自己规划完成后它会把飞机0的轨迹和自己新规划的轨迹一起再发布出去给飞机2。这样就形成了一个轨迹信息的传递链后机始终知道前机的意图。broadcast_bspline_pub_/sub_这是广播通道。所有飞机都订阅同一个广播话题。任何一架飞机在规划出新轨迹后除了在链式通道中传递也会通过这个广播通道发布出去。这用于一些需要全局知晓的紧急信息或特定协同策略。在SEQUENTIAL_START状态中飞机在开始规划前检查have_recv_pre_agent_标志位就是确保它已经通过swarm_trajs_sub_收到了前序飞机的轨迹信息从而实现了安全的顺序启动。3. 从决策到路径规划管理器与重规划机制状态机做出了“现在需要规划一条新轨迹”的决策比如从WAIT_TARGET进入SEQUENTIAL_START或者从EXEC_TRAJ进入REPLAN_TRAJ。接下来这个决策就被交给planner_manager_规划管理器来执行。规划管理器是EGO-Planner的“算法引擎”它整合了A*搜索、B样条优化等核心模块。3.1 两种规划起点的选择规划管理器根据状态机的指令从不同的起点开始规划这主要对应两个核心函数planFromGlobalTraj()全局规划。通常用在任务开始时从当前里程计位置odom_pos_规划一条通往第一个全局目标点end_pt_的轨迹。它内部可能会先调用一个全局路径搜索比如A*生成一条粗略的几何路径然后再用B样条优化器进行平滑、动力学可行性优化。在SEQUENTIAL_START状态中调用的就是它并且设定了重试次数比如10次以确保初始规划的成功率。planFromCurrentTraj()局部重规划。这是EGO-Planner应对动态环境的核心。当无人机正在执行轨迹EXEC_TRAJ状态时如果状态机因为时间阈值或碰撞检测触发而切换到REPLAN_TRAJ就会调用这个函数。它与全局规划最大的不同在于起点它不是从当前无人机实际的里程计位置开始规划而是从当前正在执行的轨迹上选取一个未来的时间点比如t_cur time_forwardtime_forward常设为0.8秒作为规划起点。这个设计非常巧妙我举个例子你就明白了假设无人机正在以2米/秒的速度飞行从检测到障碍物到计算新轨迹再到飞控响应执行这中间有不可避免的计算和控制延迟可能100-200毫秒。如果你从当前实际位置开始规划新轨迹等新轨迹算好时无人机已经飞到前面去了新规划的起点可能已经离障碍物很近甚至撞上了。而从当前轨迹的未来点开始规划相当于做了一个“预判”让新轨迹的起点和无人机实际到达新轨迹起点的时间相匹配大大提高了重规划的安全性和平滑性。这个未来点的时间前向量time_forward是调参中的一个关键太小了避不开延迟太大了可能不必要地偏离原路径。3.2 规划的核心步骤搜索与优化无论哪种起点规划过程通常包含两个主要阶段由planner_manager协调前端路径搜索通常使用A*算法在path_searching模块中。它的任务是在grid_map提供的占据栅格地图occupancy_buffer_inflate_即膨胀后的地图上找出一条从起点到终点的、无碰撞的离散路径点。这个路径可能很“磕巴”拐直角弯不符合动力学。后端轨迹优化这是EGO-Planner名字里“EGO”Elastic Band Optimization的体现主要在bspline_optimizer中完成。它接收A*给出的路径点作为初始“引导”然后生成一条均匀的B样条曲线并通过一个优化问题来调整这条曲线的控制点。优化目标通常包括平滑性让轨迹的加速度、加加速度Jerk尽量小飞行起来平稳。安全性让轨迹上的点远离障碍物利用地图的距离场信息。动态可行性确保轨迹的速度、加速度不超过无人机物理极限max_vel_,max_acc_。与引导路径的贴合度不能离A*的路径太远。优化器通过调整权重lambda_smooth,lambda_collision,lambda_feasibility等来平衡这几个目标。最终输出一条时间、空间上都参数化的B样条轨迹它由一系列控制点和时间节点向量knots定义。4. 从路径到动作轨迹服务器的执行艺术规划器费了好大劲算出一条优美的B样条曲线但飞控看不懂这个。飞控通常期望接收的是高频率如100Hz的位置、速度、加速度指令PositionCommand。这个“翻译”工作就由traj_server节点来完成。它是连接规划“大脑”和执行“肢体”的关键桥梁。4.1 轨迹的订阅与解析在启动文件single_run_in_exp.launch里你会看到traj_server节点的定义。它订阅的话题是/planning/bspline在单机模式下可能被重映射为/drone_X_planning/bspline。node pkgego_planner nametraj_server typetraj_server outputscreen remap from~planning/bspline todrone_$(arg drone_id)_planning/bspline/ param nametraj_server/time_forward value1.0 typedouble/ /node当traj_server在bsplineCallback回调函数中收到一条新轨迹消息时它会做这几件事从消息中提取控制点pos_pts类型是geometry_msgs::Point[]和时间节点knots。用这些数据实例化一个均匀B样条对象pos_traj。这个对象封装了B样条的数学公式可以根据时间t快速计算出对应点的位置pos、一阶导速度vel和二阶导加速度acc。记录这条轨迹的起始时间start_time_来自消息和轨迹IDtraj_id_并将receive_traj_标志置为true。4.2 高频率指令生成traj_server内部有一个高精度定时器通常10ms周期即100Hz不断执行cmdCallback()函数。这个函数是执行循环的核心检查首先看receive_traj_是否为true。如果没有收到过有效轨迹直接返回。计算当前时间t_cur (当前时间 - 轨迹开始时间).toSec()。这个t_cur就是沿着这条时间参数化轨迹的“进度条”。分段计算指令正常执行段(0.0 t_cur traj_duration_)这是最常见的情况。将t_cur代入B样条对象pos_traj直接计算出当前时刻期望的位置(pos)、速度(vel)、加速度(acc)。轨迹结束段(t_cur traj_duration_)当进度条走完了规划的总时间说明这条轨迹已经执行完毕。此时位置设置为轨迹终点速度加速度设为0偏航角yaw保持最后的值。这会让无人机在目标点稳定悬停。异常情况(t_cur 0.0)理论上不应发生会报错。计算偏航角通过一个独立的calculate_yaw()函数计算。一个简单的策略是让机头指向速度方向即yaw atan2(vel.y(), vel.x())并对变化率进行限幅保证平滑。打包与发布将计算好的pos,vel,acc,yaw等信息填充到PositionCommand类型的消息cmd中并通过pos_cmd_pub发布到/position_cmd话题可能重映射为/mavros/setpoint_position/local给PX4飞控。4.3 时间前向与延迟补偿这里有一个非常重要的细节traj_server有一个参数叫time_forward默认为1.0秒。但请注意这个time_forward和前面规划器重规划时的time_forward作用不同。在traj_server中time_forward主要用于补偿通信和控制延迟。实际操作中从traj_server发布指令到指令通过串口/数传发送给飞控再到飞控处理并驱动电机产生推力存在几十到上百毫秒的延迟。如果我们发送的是“当前时刻”的指令等无人机执行时已经晚了。因此更高级的做法是traj_server在计算t_cur时并不是简单用now - start_time而是用(now time_forward) - start_time。也就是说它发布的是“未来某个时刻”的指令。飞控在收到这个“未来指令”后会利用自己的高倍率控制器如PID来努力在未来那个时间点达到指令要求的状态。这相当于做了一个前馈补偿显著提升了轨迹跟踪的精度。这个time_forward的值需要根据实际系统的延迟进行标定。5. 闭环验证与实战调试心得理解了整个工作流最终还是要落到实际飞行和调试上。EGO-Planner的代码提供了丰富的可视化话题和参数配置这些都是我们调试的利器。5.1 利用RViz进行可视化调试在planning/plan_manage的launch文件中通常已经配置好了RViz。你需要关注以下几个关键可视化信息/planning/visiblibity_region或/planning/view_points显示当前规划器的感知范围或采样点用于检查地图是否正常更新。/planning/bspline或/planning/traj显示规划器最新生成的B样条轨迹通常是绿色或蓝色的曲线。这是你判断规划是否合理最直接的依据。观察曲线是否平滑是否穿过了显示的障碍物膨胀区域。/planning/ref_path显示A*搜索出来的初始路径通常是红色点线。对比它和最终B样条轨迹可以看优化器的工作效果。/obstacle_map和/inflated_map分别显示原始占据地图和膨胀后的地图。确保障碍物被正确感知并且膨胀半径inflation_dist设置合理。膨胀太小容易撞太大会把通道堵死。无人机模型和/position_cmd观察无人机模型是否平滑地跟随显示的轨迹运动。可以在RViz中发布一个Pose类型的/position_cmd模拟traj_server的输出看跟踪是否及时。5.2 关键参数调优指南EGO-Planner的性能很大程度上依赖于参数配置。以下是一些最核心、最需要调整的参数它们通常分布在advanced_param_exp.xml或代码的param服务器加载部分状态机参数 (ego_replan_fsm.cpp):replan_thresh_重规划时间阈值。无人机执行当前轨迹超过这个时间秒后即使没遇到障碍也会触发重规划。调小它会让系统更频繁地重新规划响应更快但计算负荷大调大则更省算力但可能对动态障碍反应迟钝。一般设在0.3-1.0秒之间。no_replan_thresh_停止重规划距离。当无人机离当前局部轨迹终点小于这个距离米时即使超过replan_thresh_也不再重规划准备切换目标或结束。防止在终点附近“抖动”。emergency_time_紧急制动时间。碰撞检查发现障碍物后允许用于反应和重规划的最大时间。这是安全底线通常设得比较保守如0.5-1.0秒。规划器参数 (plan_manage.cpp,bspline_optimizer.cpp):max_vel_,max_acc_最大速度和加速度。必须根据你的无人机实际动力性能来设置设得太高规划出的轨迹飞控跟不上会剧烈震荡设得太低无人机飞得太慢。通常先设一个保守值如max_vel_1.5,max_acc_6.0确保安全再逐步提高。planning_horizon/planning_horizen_time_规划视界。前者是空间长度米后者是时间长度秒。规划器只规划这么远/这么久以内的轨迹。太短无人机“目光短浅”容易陷入局部陷阱或急转弯太长计算量增大且环境变化可能导致规划失效。一般planning_horizen_time_设为2.0-4.0秒。lambda_smooth,lambda_collision,lambda_feasibilityB样条优化的权重。这是调参的“艺术”。lambda_collision安全权重必须足够大确保轨迹远离障碍物。lambda_smooth平滑权重影响飞行舒适度。lambda_feasibility可行性权重确保速度加速度不超限。通常的调整顺序是先保证安全调高lambda_collision再保证能飞调高lambda_feasibility限制动力学最后优化平滑性。地图与碰撞检测参数 (plan_env):resolution地图分辨率米。分辨率越高地图越精细但内存和计算量立方级增长。0.1米是常用值。inflation_dist障碍物膨胀距离。这是最重要的安全参数之一。必须大于无人机半径加上定位和控制误差的余量。例如无人机轴距0.4米定位误差0.05米那么inflation_dist至少设为0.25米以上。swarm_clearance_多机安全间隔。在多机飞行时两架无人机轨迹之间的最小允许距离。要大于单机inflation_dist的两倍并留有余量。轨迹服务器参数 (traj_server.cpp):time_forward如前所述用于延迟补偿。需要通过实际系统测试来标定。一个方法是让无人机做高速往返运动观察指令和实际位置的相位差来估算。调试时我的习惯是一次只调整一个参数并在简单、可重复的场景下比如穿越一个静态门框测试其影响。同时一定要打开rosconsole的DEBUG或INFO级别日志观察规划成功率、优化迭代次数、碰撞检测结果等打印信息它们能提供最直接的反馈。EGO-Planner的代码结构清晰模块化程度高只要顺着“状态机 - 规划 - 执行”这条主线结合可视化和日志你就能很快掌握其精髓并让它在你自己的无人机上稳定运行起来。