室外飞无人机，我最怕的不是炸机，是撞树。尤其多旋翼在树林、果园或者园区绿地里穿行，树冠、细枝、电线杆，随便来一下就是螺旋桨报废、电机堵转，运气差一点整机直接摔。很多朋友第一反应是“避障没做好”，但真正排查下来会发现，撞树往往不是某一个算法不行，而是整条感知-规划-控制链路里有断点。Ego-Planner这类局部规划器的出现，算是把“规划层动态避障”这一环补得比较完整了。
这篇文章不是单纯讲概念，我会以ROS为操作环境，从零把Ego-Planner跑起来，再深入到参数、原理和排查思路，重点围绕“为什么会撞树”和“怎么让轨迹真正绕开树”这两个问题展开。适合正在做无人机自主导航、ROS路径规划，或者被仿真避障折磨得头疼的开发者。下面直接上干货。
1. 为什么Ego-Planner能终结无人机撞树：核心思路拆解
1.1 撞树问题的本质不是“撞”而是“断链”
先说一个我自己的判断：绝大多数无人机撞树，不是撞在“没有路径规划”上，而是撞在“感知和规划没有形成闭环”上。举个例子，你给飞控一个目标点，全局规划器用A*或者RRT在已知地图里找出一条线，这条线在起飞那一刻可能确实是安全的。但树冠会随风摆动，无人机自身定位有漂移，光照变化让深度相机偶尔丢帧，这些动态因素叠加起来，原先那条安全路径很快就变成了一条“通往树干的准确航线”。
所以真正要解决撞树问题，必须让无人机具备“边飞边看、边看边改”的能力。Ego-Planner走的就是这个方向：它不要求你提前建一张全局障碍物地图，而是直接利用传感器实时给出的点云，在飞行过程中不断重规划局部轨迹，把“当前看到的危险”绕开。
1.2 Ego-Planner的思路：不做全局ESDF，也能安全飞行
Ego-Planner来自浙大FAST-Lab的开源项目，全称是Ego-Planner: An ESDF-free Gradient-based Local Planner。它的核心卖点一句话就能说清：不构建ESDF（欧几里得符号距离场），直接用B样条曲线作为轨迹表达，在障碍物点云上做优化，把轨迹“推”到安全区域。
传统局部规划器，比如很多基于优化方法的方案，都会先建一个ESDF地图，告诉优化器“空间中每个点到最近障碍物的距离是多少”，然后计算梯度，把轨迹往距离大的方向推。ESDF的问题在于构建和更新代价很高，尤其是在线更新动态障碍物时，CPU开销非常可观。Ego-Planner绕开了这一步，它只在当前轨迹附近采样障碍物信息，通过膨胀障碍物点来生成梯度，从而迭代修正B样条控制点。
用句大白话说：传统方法像考试前把整本教材都背下来，Ego-Planner像只复习重点题，题目变了也能现场推导。它的优势是计算量小、反应快，非常适合机载电脑算力有限的无人机平台。
2. 环境搭建：ROS与Ego-Planner仿真一步到位
2.1 从零准备ROS环境（含推荐的一键安装方式）
Ego-Planner目前对ROS的支持很成熟，主流选择是Ubuntu 20.04配ROS Noetic，或者Ubuntu 18.04配ROS Melodic。我自己长期用Noetic，稳定性更好，后续依赖也更好装。如果新机器不想折腾，可以试试鱼香ROS的一键安装脚本（有对应开源项目），它能自动识别系统版本并帮你装好ROS本体和常用工具，实测能省下大量配源、解决依赖的时间。
但不要过度依赖脚本，环境变量和基础工具还是值得多花五分钟理一遍。装完ROS后建议逐个确认：
source /opt/ros/noetic/setup.bash echo $ROS_DISTRO roscore能正常启动roscore，说明ROS核心没问题。接下来安装编译依赖，Ego-Planner需要Eigen、Ceres Solver等库。Ceres我建议直接用apt装，版本匹配且省时间：
sudo apt-get install -y libeigen3-dev libsuitesparse-dev libgoogle-glog-dev libgflags-dev sudo apt-get install -y ros-noetic-ceres-solver2.2 源码编译Ego-Planner与Gazebo森林场景复现
依赖准备好后，创建catkin工作空间并拉取源码。这里提醒一句：Ego-Planner仓库里包含uav_simulator等仿真组件，编译顺序用catkin_make一把梭就行，它会自动处理包之间的依赖。
mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src git clone https://github.com/ZJU-FAST-Lab/ego-planner.git cd ~/catkin_ws catkin_make编译过程中最容易翻车的两件事：一是缺ros-noetic-mav-msgs这类消息包，报错一般是“Could not find the required component”；二是Ceres版本不匹配，报错集中在优化求解部分。遇到缺包就apt补齐，遇到Ceres问题就卸载后统一用apt装同一个版本，不要混用源码安装的Ceres。
编译成功后启动仿真：
source devel/setup.bash roslaunch ego_planner run_in_gazebo.launch不出意外会弹出Gazebo和Rviz，无人机在仿真场景里起飞，并按预设航点自动飞行。这个launch默认会跑一条演示路径，当无人机靠近障碍物时，你能在Rviz里看到彩色轨迹发生变化，那其实就是Ego-Planner在实时重规划。
3. 算法原理与关键避障参数，决定你会不会再次撞树
3.1 B样条优化到底怎么把轨迹推离树干
Ego-Planner用均匀B样条来表示轨迹。B样条的好处是：轨迹形状由一组控制点决定，控制点数量不多，优化变量就少；同时B样条天然自带平滑性，不会像折线轨迹那样出现尖锐拐弯。优化器每一次迭代，都在调整控制点的位置，让整条轨迹满足三个目标：安全（离障碍物足够远）、平滑（不来回乱甩）、动力学可行（速度加速度不超过无人机能力）。
安全这一项是重点。Ego-Planner会在当前轨迹附近采样障碍物点，然后把每个障碍物点膨胀成一个小球，计算轨迹穿过这个区域时产生的“碰撞代价”。代价越大，优化器就越会把控制点往远离障碍物的方向推。这个过程中不需要建全局ESDF，所以每一帧点云来了都能立刻参与优化。
明白这一点之后，你就知道为什么“调参”对避障效果影响巨大：膨胀半径设太小，轨迹就贴着树皮走，机架或者桨叶稍微一晃就蹭到树枝；设太大，轨迹会绕大弯，甚至找不到可通行路径直接悬停。
3.2 直接影响撞树的参数和调整建议
Ego-Planner的参数分散在launch目录下的yaml文件里，不同版本命名略有差异，但主要参数基本一致。我常用的几个关键项如下：
| 参数 | 作用 | 撞树相关的调整建议 |
|---|---|---|
| max_vel | 最大飞行速度 | 树林里建议降到2~3m/s，速度快则刹车距离长，来不及绕 |
| max_acc | 最大加速度 | 太大容易急停急转，太小则机动性不足 |
| safety_margin | 轨迹距离障碍物的安全余量 | 树林场景至少0.5m起，根据机架尺寸加大 |
| weight_collision | 碰撞代价权重 | 发现轨迹贴障碍物太近时调大 |
| weight_smoothness | 平滑代价权重 | 发现轨迹抖得太厉害时调大 |
| obstacle_inflation | 障碍物膨胀半径 | 细枝多的地方适当增大，防止“视觉上绕开，实际刮到” |
| planning_horizon | 规划前探距离 | 太短会导致发现障碍太晚，刹车不及 |
调参有一个基本原则：先固定速度和加速度，再调障碍物相关参数，最后微调权重。不要一上来就把碰撞权重拉到极高，那样轨迹会变得很“神经质”，稍微有点点云噪声就开始乱拐。
3.3 动态避障的重规划回路：树在动，轨迹也要动
Ego-Planner本身没有目标追踪或者运动预测模块，它的动态避障能力来自“感知-重规划”的高频闭环。传感器以10Hz~20Hz的频率输出点云，规划器每收到一帧新点云，就在当前轨迹基础上重新优化一次，然后把新轨迹发给控制模块。只要重规划频率足够高，树哪怕在缓慢摆动或者有人从旁边走过，轨迹也能持续被修正。
这个机制决定了它对慢速动态障碍物很有效，对高速突然闯入的障碍物（比如一辆车突然冲过来）则力不从心。因为从感知到重规划再到执行，中间至少有几十毫秒延迟，无人机物理上需要时间改变速度方向。所以做动态避障时，不要指望“绝对不撞”，而是要做到“能避则避，避不开也能减速就地悬停”。
4. 动态避障实操：从静态绕树到运动障碍物躲避
4.1 先跑通静态绕树：验证规划器本身
不要一上来就搞动态场景，先把静态绕树跑顺。在仿真里最简单的方法是：启动run_in_gazebo.launch后，在Rviz里找到“2D Nav Goal”工具，在odom坐标系下点击一个目标点发布目标位姿（不同launch里目标话题名可能不同，用rostopic list看一眼，一般是/goal或/move_base_simple/goal）。无人机收到目标点后会自动规划路径，从起飞点绕过障碍物飞过去。
实际操作时我建议把目标点放在一颗比较粗的树干模型后方，如果轨迹能明显绕出一个弧线而不是直穿模型，说明规划器工作正常。此时可以把点云显示开关打开，确认Rviz里的障碍物点云和Gazebo中的模型位置对应得上。只要出现“点云偏移”“点云稀疏”这类情况，先不要怀疑Ego-Planner，去查传感器外参、时间戳和坐标系。
4.2 制造一个移动的“树”：验证动态重规划
静态测试通过后，进入动态避障实操。仿真里制造移动障碍物的方式很多，最简单的是直接用Gazebo自带的模型移动机制，用一个节点循环设置模型的位置。比如把一颗树干模型当“巡逻的树”，让它沿直线来回移动，然后给无人机一个穿越目标点，迫使它必须避开这颗移动的树。
我平时会写一个简单的Python节点，定时发布Gazebo模型状态：
#!/usr/bin/env python3 import rospy from gazebo_msgs.msg import ModelState from gazebo_msgs.srv import SetModelState def move_tree(): rospy.init_node('move_tree_node') rospy.wait_for_service('/gazebo/set_model_state') set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) rate = rospy.Rate(20) t = 0.0 while not rospy.is_shutdown(): state = ModelState() state.model_name = 'tree_model' state.pose.position.x = 3.0 * (t % 6 - 3) state.pose.position.y = 5.0 state.pose.position.z = 0.0 state.pose.orientation.w = 1.0 set_state(state) t += 0.05 rate.sleep() if __name__ == '__main__': try: move_tree() except rospy.ROSInterruptException: pass运行之后观察Rviz里的轨迹：正常情况应该是无人机一边前进，一边看到前方点云位置在变化，然后持续修正轨迹，绕出平滑弧线。如果出现轨迹来回抖，优先调低速度上限、加大safety_margin。如果轨迹迟迟不更新，大概率是点云话题被launch里写死成静态地图源了，或者重规划频率不够。
4.3 从仿真搬到真机时要改点什么
仿真能跑通，真机是另一回事。Ego-Planner的输入要求其实很清晰：期望话题类型是sensor_msgs/PointCloud2，坐标系一般是odom或者body系，具体看launch里的设置。真机上常见做法是机载电脑（NUC/树莓派/Jetson）跑Ego-Planner，飞控跑PX4或者ArduPilot，里程计用RTK或者视觉定位。点云则来自RealSense、Livox或者机械式激光雷达，通过话题remap接到规划器上。
真机移植最容易被坑的是坐标系。仿真里点云天然在odom系下对齐得很好，真机上深度相机在机体上，点云通常在camera_link或body系里，必须通过tf变换到规划器期望的坐标系。很多次“撞树”排查到最后，发现不是规划器没避障，而是相机外参标定差了5厘米，导致规划器看到的树位置跟真实位置对不上，轨迹总是“擦着树绕”。
另外，真机的点云预处理非常重要。树叶、细枝在深度相机里非常稀疏，很多人习惯用滤波把离群点去掉，结果把树枝全滤没了。我的建议是：树林场景把离群点检测半径调大，保留更多原始点，宁肯让规划器多绕点路，也不要让它在细枝面前“失明”。
5. 撞树问题排查实录：还没绕过去，先看这张速查表
5.1 常见撞树症状与排查方向
真实项目中，撞树很少只有单一原因。以下是我自己试飞和帮朋友排查时归纳出的速查表，基本覆盖了大多数情况：
| 症状 | 可能原因 | 排查和解决办法 |
|---|---|---|
| 轨迹直接穿树而过 | 点云没进规划器 / 点云被过度滤波 | 用rostopic echo检查点云话题是否有数据，频率是否正常；放宽滤波参数 |
| 轨迹绕到树旁边又拐回来 | 碰撞权重太低 / 膨胀半径不够 | 调大weight_collision和obstacle_inflation |
| 无人机急停或悬停不肯走 | 没有找到可行轨迹 / 前探距离太小 | 降低max_vel，增大planning_horizon，或者给更大的绕行空间 |
| 轨迹抖动明显、来回甩 | 平滑权重低 / 感知噪声大 | 调大weight_smoothness，优化点云预处理 |
| 能看到点云但轨迹不更新 | 坐标系tf错误 / 规划频率受限 | 检查tf树，确认点云帧和里程计帧对齐 |
| 仿真里绕得好，真机就撞 | 相机外参偏差 / 真机飞控响应延迟 | 重新标定外参，校准飞控PID，降低巡航速度 |
5.2 三条容易被忽略的真实经验
第一条，树在点云里往往是“空心”的。树叶对红外深度相机的反射率不稳定，经常出现中间空洞、边缘碎点的情况。如果你只依赖原始点云，轨迹可能直接穿过视觉上的“空洞”，然后就撞上了实际存在的树干。针对这一点，可以在感知阶段对树类目标做聚类和几何填充，或者加大障碍物膨胀半径，把“空心树”当成实心树处理。
第二条，Ego-Planner是局部规划器，不是全局路径规划器。它只负责在视野范围内的动态避障，不负责给你找一条从起点到终点的森林穿越路线。如果你给它一个非常远的目标点，中间隔了一整片树林，它很可能因为视野看不到目标点而陷入“找不到可行轨迹”的困境。正确用法是由全局规划器给出粗略航点，Ego-Planner负责两个航点之间的局部绕行。
第三条，时间分配影响刹车距离。B样条轨迹的时间节点如果过密，轨迹看起来绕开了树干，但无人机实际飞行速度和加速度可能不匹配，导致出现“轨迹过了，飞机没过”的延迟感。遇到这种问题，除了降低速度上限，还要检查规划时间范围和B样条控制点数量，留足物理上的刹车距离。
5.3 推荐调参顺序和最小启动配置
如果你拿到一套代码完全不知道从哪里下手，推荐按这个顺序来：先把max_vel降到2m/s附近，max_acc控制在3m/s²以内，safety_margin设为0.5m，obstacle_inflation设为0.3m左右，然后跑一次静态绕树。跑通了再逐步提高速度和机动性，发现轨迹贴障碍物太近就加碰撞权重，发现抖动就加平滑权重。
一个比较保守的树林仿真启动参数可以参考下面这个片段：
max_vel: 2.0 max_acc: 3.0 safety_margin: 0.6 obstacle_inflation: 0.4 planning_horizon: 8.0 weight_collision: 1.0 weight_smoothness: 0.2这套参数不能说最优，但能保证大多数场景下安全绕行，后续再根据实际效果微调。
我个人在实际操作中的体会是：Ego-Planner最大的价值不是让你“永不撞树”，而是把撞树问题从“一脸懵”变成了“可排查、可调参、可复现”。当无人机在仿真里撞树时，你至少有手段去定位是感知漏了、规划没更新，还是参数不合理。做无人机自主飞行的路上，这种能把问题拆开的能力，比任何算法本身都珍贵。最后再分享一个小技巧：每次测试前，先用rosbag录一段点云和轨迹，撞完树回头分析数据，比现场猜原因效率高得多。
