从EGO-Planner到集群协同：分布式轨迹优化在无人机编队中的应用

1. 项目概述：从单机到集群的自主飞行进化

如果你玩过无人机，或者关注过机器人领域，大概会知道让一台机器在空中自主规划路径、避开障碍物已经是个不小的挑战。那么，想象一下，让一群无人机像鸟群一样，在复杂、未知的环境中协同飞行，彼此间既要保持队形、完成共同任务，又要实时闪避动态障碍和队友，这背后的技术难度是指数级增长的。ego-planner-swarm这个开源项目，正是为了解决这个“集群自主飞行”的核心难题而生。

简单来说，它是 FAST-Lab（浙江大学流体动力与机电系统国家重点实验室的智能无人系统团队）在原有高性能单机轨迹规划器EGO-Planner基础上，针对多智能体（无人机集群）场景深度优化的版本。其核心目标就一个：让一群无人机在资源受限的机载计算单元上，能够实时地、分布式地规划出安全、平滑、高效的飞行轨迹。这不仅仅是学术上的炫技，在物流配送、协同测绘、编队表演、灾难搜救等实际场景中，都有着迫切的需求。

我自己在部署和测试多机系统时，最头疼的问题就是“耦合”。单机的规划器设计得再精妙，一旦扩展到多机，机与机之间的轨迹就会相互干扰，传统的集中式规划计算量爆炸，而简单的分布式方案又容易陷入“死锁”——就像十字路口没有红绿灯的车流，各自为政，结果谁都动不了。ego-planner-swarm的巧妙之处在于，它引入了一种“增量式”的协同策略。每架无人机都以自我（Ego）为中心进行规划，但同时会通过轻量级的通信，感知邻居飞机的意图，并在自己的优化问题中，将别人的未来轨迹视为“动态障碍物”进行规避。这种“自私”又“礼貌”的机制，是实现高效分布式协同的关键。

接下来，我会深入拆解这个项目的设计思路、核心实现，并分享从源码编译到实际飞行的全流程实操经验与避坑指南。无论你是机器人方向的学生、无人机开发者，还是对集群智能感兴趣的研究者，这篇文章都将为你提供一个从理论到实践的完整路线图。

2. 核心设计思想与架构拆解

要理解ego-planner-swarm，必须先搞懂它的前身EGO-Planner以及它所应对的集群挑战。这不仅仅是代码的堆叠，更是一套完整方法论的应用。

2.1 EGO-Planner 的精髓：后端优化驱动的前端搜索

单机EGO-Planner本身就是一个非常优秀的规划器，它打破了传统“前端路径搜索（如A*）+ 后端轨迹优化”的管道式模式。传统方法中，前端搜索出的路径可能在后端优化时被发现不可行（例如太靠近障碍物导致优化失败），造成规划失败。

EGO-Planner采用了一种“后端优化引导的前端搜索”策略。它核心是一个梯度优化的弹性带（Gradient-based Elastic Optimization）框架：

1. 初始轨迹生成：首先，它可能通过一个简单的全局规划器（如RRT*）或甚至一条直线，生成一条初始的、可能碰撞的轨迹。
2. 碰撞代价与弹性力：将轨迹离散成一系列控制点（B样条曲线的控制点）。当某个控制点侵入障碍物时，会产生一个指向障碍物表面的“排斥梯度”（就像一根弹簧被压缩，产生一个向外推的力）。
3. 联合优化：算法并不直接移动这个碰撞点，而是将所有这些“排斥梯度”作为惩罚项，与轨迹平滑度（加速度、加加速度的积分）、动态可行性（速度、加速度限制）的代价一起，放入一个非线性优化问题中统一求解。通过数值优化方法（如L-BFGS），同时调整所有控制点的位置，使得最终轨迹既远离障碍物，又平滑可行。

这种方法的优势在于，优化过程本身在“拉直”和“推开”轨迹，相当于搜索与优化同步进行，避免了前后端解耦带来的不一致问题，对复杂环境的适应性更强。

2.2 集群带来的核心挑战与分布式策略

将EGO-Planner扩展到集群，面临几个核心挑战：

1. 交互耦合：无人机A的轨迹会影响B的决策，B的新轨迹又会反过来影响A，形成复杂的循环依赖。集中式规划需要知道所有飞机的全局信息，计算和通信开销巨大，且存在单点故障风险。
2. 实时性要求：集群飞行，尤其是避障，需要在毫秒级完成重规划。计算必须高效，且分布在各无人机上。
3. 通信受限：机间通信带宽有限、可能有延迟和丢包。不能依赖高频、大数据的全局同步。

ego-planner-swarm的解决方案是“基于一致性的分布式模型预测控制（Consensus-based Distributed Model Predictive Control, DMPC）”思想的一种具体实现。其核心架构可以分解为以下几个层级：

• 感知层：每架无人机通过自身的传感器（如激光雷达、深度相机）实时构建局部环境地图（例如ESDF-欧几里得符号距离场）。关键点：它只需要构建自己规划范围内的局部地图，无需全局一致地图，这大大降低了计算和通信负担。
• 通信层：无人机之间通过轻量级通信（如Wi-Fi自组网、UWB）广播两种核心信息：
  • 未来轨迹意图：将自己当前规划好的、未来一小段时间（例如未来1-2秒）的轨迹控制点（B样条控制点）广播给邻居。
  • 关键状态信息：如当前位置、速度、目标点。通信频率不需要与规划频率完全一致，可以略低。
• 规划层（核心）：每架无人机独立运行一个改进的EGO-Planner实例。但它的优化代价函数中，新增了一项至关重要的交互代价：

  交互代价函数：将接收到的邻居无人机未来轨迹，也视为一种特殊的“动态障碍物”。在自己的ESDF地图中，为这些轨迹生成一个临时的、管状的“危险区域”。当自身规划轨迹的控制点进入这个区域时，就会产生强大的排斥梯度，迫使自己的轨迹优化过程主动避开邻居的“领空”。

这种机制的美妙之处在于去中心化和增量一致性。每架飞机只关心自己和邻居的意图，无需全局协调器。通过不断广播新意图、接收邻居意图并重新优化，整个集群的轨迹会在数次迭代后趋于一种“协商一致”的、无碰撞的状态。这类似于人类在拥挤街道上的行走，通过观察对方的身形和移动趋势，实时调整自己的步伐和方向。

2.3 与经典集群算法的对比

为了更清晰理解其设计优势，我们将其与两种经典方法对比：

可以看出，ego-planner-swarm在轨迹质量、可扩展性和实时性之间取得了很好的平衡，特别适合需要生成高质量、可执行轨迹的中小型无人机集群应用。

3. 环境搭建与源码深度解析

理论理解了，我们就要动手把它跑起来。这里面的坑，我一个个带你过。

3.1 系统环境与依赖部署

项目基于 ROS (Robot Operating System) 和 Gazebo 仿真环境。强烈建议使用 Ubuntu 20.04 + ROS Noetic这一组合，这是最稳定、社区支持最全的版本。

# 1. 安装 ROS Noetic (如果已安装请跳过) sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt update sudo apt install ros-noetic-desktop-full # 2. 初始化ROS环境 echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc source ~/.bashrc # 3. 创建工作空间并克隆代码 mkdir -p ~/swarm_ws/src cd ~/swarm_ws/src git clone https://github.com/ZJU-FAST-Lab/ego-planner-swarm.git # 克隆依赖的子模块，这一步非常关键！ cd ego-planner-swarm git submodule update --init --recursive

关键依赖解析与安装：

• C++编译环境：需要支持C++14以上的编译器（GCC 7+）。Ubuntu 20.04默认的GCC 9没问题。
• 非线性优化库：项目依赖NLopt或Ceres Solver进行轨迹优化。通常使用Ceres，因为它速度更快、更稳定。

  sudo apt-get install libceres-dev

• 几何计算库：依赖PCL(点云库)、Eigen3。ROS桌面版通常已包含。
• 仿真可视化：依赖Gazebo和RViz。ROS桌面版已包含。

注意：子模块更新是关键！项目引用了FAST-Lab的其他核心库，如plan_manage、bspline、bspline_opt等。如果不执行git submodule update --init --recursive，编译一定会失败。这是新手最容易踩的坑。

3.2 核心源码目录结构解析

克隆下来的代码结构清晰，反映了其模块化设计思想：

ego-planner-swarm/ ├── README.md ├── launch/ # ROS启动文件 │ ├── swarm.launch # 集群仿真总启动文件 │ └── ... (其他场景启动文件) ├── src/ │ ├── plan_manage/ # **规划管理器核心** │ │ ├── src/planner_manager.cpp # 规划状态机、主循环 │ │ └── ... │ ├── bspline/ # B样条轨迹表示与求值 │ ├── bspline_opt/ # **轨迹优化器核心** │ │ ├── src/ego_replan_fsm.cpp # 单机重规划状态机 │ │ ├── include/bspline_optimizer.h │ │ └── src/bspline_optimizer.cpp # **优化问题构建与求解** │ ├── uav_simulator/ # Gazebo无人机模型、控制器与传感器仿真 │ │ ├── map_generator/ # 随机地图生成器 │ │ └── so3_control/ # 姿态控制器 │ └── utils/ # 工具函数（如点云转换、参数读取） ├── rviz_config/ # RViz可视化配置文件 └── ...

核心文件深度解读：

1. src/bspline_opt/src/bspline_optimizer.cpp：这是项目的“心脏”。其中BsplineOptimizer::optimize()函数实现了轨迹优化的主循环。你需要重点关注代价函数的构建：

   • calcSmoothnessCost()：计算轨迹平滑度代价（加速度、加加速度的积分）。
   • calcDistanceCost()：计算轨迹与静态障碍物的距离代价，利用局部ESDF地图。
   • calcFeasibilityCost()：计算动态可行性代价（速度、加速度限幅）。
   • 关键新增：在集群版本中，会有一个calcSwarmCost()或类似的函数（可能集成在distance cost中），用于计算与邻居轨迹的距离代价。它会遍历所有接收到的邻居轨迹控制点，计算它们与自身控制点的距离，并在距离过近时施加一个强大的惩罚梯度。

2. src/plan_manage/src/planner_manager.cpp：这是规划流程的“大脑”。它管理着规划状态（如INIT，PLANNING，EXECUTING），触发重规划（例如当检测到新障碍物或收到新目标时），并调用优化器。在集群中，它还负责处理接收到的邻居轨迹消息，并将其传递给优化器。

3. src/bspline_opt/src/ego_replan_fsm.cpp：单机规划状态机。定义了无人机在“等待目标”、“规划中”、“跟踪轨迹”等状态间的转换逻辑。理解这个状态机对调试非常有帮助。

3.3 编译与首次仿真运行

cd ~/swarm_ws catkin_make -j4 # 使用4个线程编译，数字可根据你的CPU核心数调整

编译成功后，让我们启动一个最简单的集群仿真：

source ~/swarm_ws/devel/setup.bash roslaunch ego_planner swarm.launch

这个swarm.launch文件会启动以下节点：

1. Gazebo：加载一个包含多架无人机模型的空世界或简单障碍世界。
2. 地图服务器：可能加载一个预设的栅格地图，或启动一个随机地图生成器。
3. 多个ego_planner_node实例：每个实例对应一架无人机，订阅该机的传感器话题（如/uavX/cloud_registered， 仿真点云），发布控制指令话题（如/uavX/command）。
4. RViz：可视化每架无人机的规划轨迹（不同颜色）、ESDF切片、目标点等。

启动后，你会在RViz中看到多架无人机模型。此时它们还不会动，因为还没有给它们发送目标点。

4. 集群飞行实操与参数调优指南

让集群飞起来，并飞得漂亮，需要理解其工作流程并掌握关键参数的调节。

4.1 完整工作流程与指令发送

一个典型的集群飞行任务流程如下：

1. 启动仿真环境：如上所述，通过roslaunch启动所有节点。
2. 设置目标点：项目通常提供两种方式：
   • RViz工具设置：在RViz中，使用Publish Point工具，点击3D空间中的某一点。但需要确认对应的ROS话题（如/goal）是否被正确映射到某架无人机。更常见的是使用项目提供的专门目标设置工具或脚本。
   • ROS Topic发布：通过命令行或编写脚本，向每架无人机的目标话题发布几何消息。例如：

     # 向无人机1发布目标点 (x=5, y=5, z=1.5) rostopic pub /uav1/planning/goal geometry_msgs/PoseStamped “{header: {stamp: now, frame_id: ‘world’}, pose: {position: {x: 5.0, y: 5.0, z: 1.5}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}”

3. 自主规划与飞行：每架无人机收到目标后，其ego_replan_fsm状态机进入PLANNING状态，调用规划管理器。规划器执行以下步骤：
   • 前端初始路径：可能使用一个简单的A或RRT在全局粗糙地图上找一条无碰撞路径，作为B样条优化的初始猜测。
   • 优化求解：构建包含平滑度、静态障碍距离、动态可行性、集群交互代价的优化问题，调用Ceres求解器进行迭代优化。
   • 轨迹发布与执行：优化成功后，将得到的B样条轨迹参数发送给底层的姿态控制器（so3_control），控制器生成电机转速指令，驱动Gazebo中的无人机模型飞行。
   • 实时重规划：在飞行过程中，规划器以固定频率（如10-20Hz）检查当前轨迹是否安全（与最新局部地图碰撞？）。如果发现危险，立即触发局部重规划，生成一条绕开新障碍物的新轨迹。
4. 集群交互：在整个过程中，每架无人机会以一定频率（如10Hz）通过ROS的swarm_bridge节点（或直接通过话题）广播自己的未来轨迹片段。其他无人机订阅这些消息，并将其融入自己的优化代价中，实现分布式避碰。

4.2 关键参数解析与调优心得

项目的参数通常存储在config目录下的YAML文件中，如planning.yaml或swarm.yaml。调参是让集群表现良好的关键。

1. 轨迹优化相关参数 (bspline_optimizer):

• lambda_smooth,lambda_dist,lambda_feas,lambda_swarm: 这些是代价项的权重系数，决定了优化器对平滑性、避障、动态可行性、集群避碰的重视程度。
  • 调优心得：lambda_dist（避障权重）和lambda_swarm（集群避碰权重）是最关键的。在密集障碍物环境中，需要调高lambda_dist。在集群密集编队时，需要大幅调高lambda_swarm，否则无人机容易“擦肩而过”。一个经验是，lambda_swarm通常需要比lambda_dist大一个数量级，因为无人机之间的安全距离要求更严格。
• control_point_distance: B样条控制点间距。间距越小，轨迹描述越精细，避障能力越强，但优化变量增多，计算量变大。间距越大，计算越快，但轨迹可能不够灵活，在狭窄空间容易规划失败。通常设置在0.3m到0.8m之间，根据无人机速度和环境复杂度调整。
• max_vel,max_acc: 最大速度和加速度。务必根据你仿真或真机模型的动力学能力设置。设得太高，优化出的轨迹控制器可能无法跟踪，导致失控；设得太低，则效率低下。

2. 规划器管理相关参数 (planner_manager):

• planning_horizon: 规划视野长度（秒）。优化器只优化未来这段时间的轨迹。太短则“目光短浅”，容易陷入局部陷阱；太长则计算量大，且环境不确定性增加。一般设为2-3秒。
• replan_threshold: 触发重规划的距离阈值（米）。当当前轨迹与最新地图中障碍物的最小距离小于此值时，立即重规划。这个值需要大于无人机的半径，并留有一定安全余量。通常设为无人机半径的1.5-2倍。
• swarm_clearance:集群安全距离。这是最重要的集群参数之一。它定义了无人机之间必须保持的最小距离。优化器中的lambda_swarm代价项会强烈惩罚距离小于此值的轨迹。这个值必须大于两架无人机的物理半径之和。

3. 集群通信相关参数:

• swarm_communicate_range: 通信范围（米）。只有在此范围内的邻居才会交换轨迹信息。这模拟了真实通信限制，也减少了不必要的计算。
• trajectory_broadcast_freq: 轨迹广播频率（Hz）。频率越高，集群协同越及时，但通信负载越大。一般10-20Hz足够。

实操心得：调参是一个“观察-调整”的循环。在Gazebo中设置一个简单场景（如两架无人机对飞），打开RViz的轨迹可视化，观察它们是如何相互避让的。如果发生碰撞或轨迹过于保守，就调整lambda_swarm和swarm_clearance。如果单机频繁撞墙，就检查lambda_dist和局部地图的精度（map_resolution）。永远不要一次性修改大量参数，每次只调整1-2个，观察效果。

5. 从仿真到真机：部署挑战与问题排查

让算法在仿真中运行稳定只是第一步，部署到真实无人机上是终极考验，也是问题高发区。

5.1 真机部署的核心差异与适配

1. 感知接口替换：仿真中使用的是Gazebo发布的完美点云。真机需要接入实际的激光雷达（如Livox）或深度相机（如Intel Realsense）的ROS驱动节点，输出sensor_msgs/PointCloud2话题。你需要确保点云的坐标系（frame_id）与无人机机体坐标系正确对齐，并通过tf树正确转换到世界坐标系。
2. 控制接口替换：仿真中使用的是so3_control节点，它输出的是期望的推力与姿态，由Gazebo物理引擎直接作用。真机则需要连接飞控（如PX4, ArduPilot）。通常需要一个“桥接”节点，将规划器输出的轨迹（可能是位置、速度、加速度序列）或直接是期望姿态，转换成飞控能理解的指令（如MAVROS的setpoint_position或attitudetarget）。这需要深入了解你所使用飞控的接口协议。
3. 状态估计：仿真中无人机位姿是直接获取的“真值”。真机依赖状态估计器（如VIO视觉惯性里程计、激光SLAM）提供实时的、带噪声的位置和速度估计。规划器需要订阅这个估计话题（如/vins_estimator/odometry）。状态估计的延迟和漂移是影响规划性能的最大因素之一。规划器的max_vel等参数必须考虑状态估计的精度。
4. 计算资源：笔记本上仿真很轻松，但机载计算机（如Jetson NX, Intel NUC）算力有限。你需要：
   • 编译时开启编译器优化（catkin_make -DCMAKE_BUILD_TYPE=Release）。
   • 考虑降低局部地图的分辨率或大小。
   • 降低规划频率（如从20Hz降到10Hz）。
   • 使用性能分析工具（如perf,ros2 topic hz）监控节点CPU占用。

5.2 常见问题排查实录

以下是我在仿真和真机调试中遇到过的典型问题及解决思路：

问题1：规划器不启动，终端提示“找不到地图”或“等待点云”。

• 可能原因：传感器话题名称不匹配。规划器默认订阅的话题可能是/cloud_registered，而你的传感器节点发布的话题可能是/points或/livox/lidar。
• 排查：使用rostopic list查看当前活跃的话题列表。使用rostopic echo /话题名 | head -n 1查看话题是否有数据。
• 解决：修改规划器节点的启动文件（.launch）或参数文件（.yaml），将其订阅的话题名改为正确的名称。或者使用remap标签在launch文件中重映射话题。

问题2：无人机规划出的轨迹抖动严重，像“喝醉了”一样。

• 可能原因1：优化器权重参数设置不当。lambda_smooth（平滑权重）过低，或lambda_dist（避障权重）过高，导致优化器过于“敏感”地躲避地图中的微小噪声。
• 排查：在RViz中可视化局部ESDF地图，检查地图中是否有大量噪点（悬浮的孤立点）。这通常是由于传感器噪声或地图生成算法参数不当引起的。
• 解决：调高lambda_smooth，或调低lambda_dist。同时，检查并调整感知前端的地图滤波参数，如体素滤波的叶子大小，或统计滤波的邻域点数，以去除噪点。
• 可能原因2：B样条控制点过密，导致轨迹过度拟合环境中的细节。
• 解决：适当增加control_point_distance。

问题3：两架无人机对飞时，没有相互避让，导致碰撞。

• 可能原因1：集群通信未建立。无人机没有收到邻居的轨迹信息。
• 排查：使用rostopic echo /swarm_trajs（或类似的话题）查看是否有数据。检查每架无人机的swarm_communicate_range参数是否设置合理，以及它们的uav_id是否唯一且正确。
• 解决：确保swarm_bridge节点正确运行，且网络配置允许广播/组播通信（在真机中尤其要注意防火墙设置）。
• 可能原因2：集群避碰权重lambda_swarm设置过低。
• 解决：逐步提高lambda_swarm的值，直到观察到无人机在距离较远时就开始有避让趋势。

问题4：真机测试时，无人机到达目标点后不停顿，或绕圈振荡。

• 可能原因：到达判断逻辑问题。规划器可能以轨迹的末端点是否接近目标点来判断，但由于控制误差，无人机实际位置可能始终无法与末端点重合。
• 排查：查看规划器状态机日志，确认EXECUTING到WAIT_GOAL状态转换的条件。
• 解决：修改判断逻辑，通常改为判断无人机当前位置与目标点的距离小于某个阈值（如0.2米），并持续一小段时间（如0.5秒），才认为到达。

问题5：在狭窄通道中，无人机规划失败（长时间处于PLANNING状态）。

• 可能原因：优化器无法在给定的迭代次数内找到一条满足所有约束（特别是距离约束）的可行解。
• 解决：
  1. 增加优化迭代次数：修改优化器参数max_iteration_num，但会增加单次规划耗时。
  2. 提供更好的初始值：检查前端路径搜索（如果启用）是否能在狭窄通道中找到一条路径。可以尝试切换不同的全局规划器，或调整其搜索参数。
  3. 暂时放松约束：在极端环境下，可以尝试略微调大replan_threshold（重规划阈值）或减小swarm_clearance（集群间距），但这会降低安全性，需谨慎。
  4. 这是分布式算法的固有挑战：在极度拥挤的空间，可能需要更高层的任务分配或交通规则（如规定通行顺序），这超出了当前规划器的范畴。

调试集群系统，可视化是王道。充分利用RViz，将每架机的轨迹、ESDF切片、目标点、传感器点云都显示出来。通过观察这些可视化信息，你能直观地理解算法“在想什么”，从而快速定位问题根源。从仿真到真机的每一步，都要做好充分的单元测试和集成测试，耐心记录参数和现象，你就能逐渐驯服这群“空中精灵”。