最近总有人私信我，说在B站刷到浙大Fast-Lab那套无人机自主导航视频，被EGO-Planner在杂乱走廊里穿来穿去的效果震撼到了，想在自己飞机上复现一套。但真上手之后，不少人卡在第一步——视频里看起来就是“一个四旋翼带着Intel RealSense D435i飞”，可真要把EGO-Planner跑起来，涉及仿真环境、机载电脑、飞控通信、相机标定、VIO定位、参数调优一整条链路，任何一个环节脱节都会让飞机在地上趴着不动或者起飞即炸。
这篇文章我就按自己的复现过程，从B站那个视频出发，聊清楚EGO-Planner和RealSense D435i这套方案到底是怎么协同工作的，以及从仿真到真机之间到底差了多少个“细节”。整个项目以Fast-Lab开源的ego-planner代码为主，视觉感知部分用D435i的深度图做避障，定位部分用VIO或外部动捕，飞控用PX4，机载计算单元装Linux + ROS。
先说结论：这套东西能跑通，但不轻松。如果你想当飞手那样拿着遥控器飞穿树林，那别指望EGO-Planner帮你做一切；它是一个真正的全局规划器，处理的是“从A点到B点，在三维空间中找到一条安全可行且平滑的轨迹”。你真正要做的，是把它和底层飞控、深度感知、定位系统拼成一个完整闭环。
整篇内容我会按复现顺序来写：方案选型和技术原理、仿真环境搭建、真机硬件配置、D435i标定与VIO调试、真机飞行参数整定、以及最后踩坑汇总。中间会穿插大量实机测试中的经验和参数参考，希望能让你少走几周弯路。
1. 项目整体设计与技术方案拆解
1.1 这套系统到底是干什么的
EGO-Planner，全称是Edge-aware Gradient-based Online Planner，是浙大Fast-Lab在2019年前后开源的一个无人机局部路径规划算法，后来被收录进Fast-Planner家族。和传统规划算法最大的区别在于：它把“碰撞避免”和“轨迹优化”同时放进一个基于梯度的优化框架里，不需要提前构建完整的高精度地图，也不依赖昂贵的激光雷达，只用一颗普通的深度相机就能在线规划出安全、平滑、动力学可行的飞行轨迹。
放在实际场景里就是：无人机收到一个目标点（比如“飞到前方8米，高度1.2米处”），EGO-Planner会先基于当前传感器观测生成一条碰撞代价最低的B样条轨迹，然后保证轨迹满足无人机速度、加速度限制，最后把规划好的位置序列或速度序列发给底层飞控，由飞控执行。
RealSense D435i在这里承担的是“眼睛”角色，输出彩色图、双目深度图、以及IMU数据。深度图用于EGO-Planner的障碍物感知，IMU数据用于视觉惯性里程计（VIO）估算无人机自身位置和姿态。
整体系统可以简化成这么一条链路：D435i采集深度图 → 深度数据转成局部点云或占据栅格 → EGO-Planner接收并生成轨迹 → 轨迹通过MAVROS发送给PX4 → PX4执行飞行。定位信息可以由VIO提供，也可以由室内动捕提供，二选一即可。
1.2 为什么选EGO-Planner而不是其他的规划器
市面上能和EGO-Planner对标的方案不少，比如基于RRT的全局规划、基于A的栅格规划、基于模型预测控制（MPC）的规划等。可对于无人机这种动力学限制极强的平台，光“找到路”是不够的，关键是轨迹还要能飞出来。
举个我自己的例子：最开始我用的是A* + 多项式轨迹拟合，离线算好路径再飞。结果路径在栅格地图上看着没问题，真正让飞机飞的时候，转折点太多，机体会剧烈晃动，姿态直接失控。EGO-Planner把轨迹表示成B样条，目标函数里同时考虑碰撞代价、平滑性、动力学可行性和终点误差，优化出来的轨迹天然就是平滑且连续的，不抖不动，非常适合四旋翼这类平台。
另外，EGO-Planner是“局部规划器”，它不要求你预先建立完整地图。每次规划只依赖当前传感器视野内的障碍物信息，轨迹每100ms~200ms重规划一次。这意味着只要相机不被完全遮盖，无人机就能在未知环境里持续飞行，这也是Fast-Lab视频里飞机能在陌生房间、走廊里飞得那么顺的原因。
1.3 D435i在方案里的真实角色定位
很多人误以为D435i是拿来“建地图”的，其实在这个项目里，D435i更像是“实时避障雷达眼睛”。
在仿真环境里，障碍物信息来自Gazebo模型物体的ground truth，但在真机里，EGO-Planner必须知道“哪里有障碍物”。Fast-Lab的实机实现中，D435i的深度图会被转换成局部点云，然后投影到ego-planner自带的占据栅格地图（Occupancy Grid）里，规划器只在这个局部栅格里做碰撞检查。
D435i的优势很明显：
- 深度范围覆盖0.2m~10m，室内环境完全够用
- 体积小、重量轻，整机不到80g，不会对无人机载荷造成太大压力
- 自带IMU，同一个传感器能同时服务VIO定位和深度避障
- 在Intel NUC这类低功耗平台上，单目深度配准的CPU占用率只有15%~25%
需要注意的点是：D435i的双目深度对纯色白墙、玻璃、反光地面效果比较差，在强光逆光环境下深度会有大量空洞。实际操作中我建议要么调高深度置信度阈值，要么把飞行速度降下来，给规划器足够的响应时间。
2. 仿真先行：Ubuntu下跑通EGO-Planner全流程
2.1 环境要求与依赖安装
Fast-Lab开源的ego-planner仓库（GitHub上的Fast-Lab-FAST-Lab/Fast-planner）主要基于ROS1和Ubuntu 18.04/20.04开发。我自己用的是一台装了Ubuntu 20.04的笔记本，ROS版本是Noetic，Gazebo用的是自带版本。
依赖库主要是Eigen3、OpenCV、PCL、nlopt，以及Fast-Lab自研的uav_simulator仿真包。安装命令我建议按官方README来，核心步骤其实就三条：
sudo apt install ros-noetic-nlopt libeigen3-dev libopencv-dev libpcl-dev mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src git clone https://github.com/HKUST-Aerial-Robotics/Fast-Planner.git cd ~/catkin_ws && catkin_make这里有个非常容易踩的坑：如果你用Ubuntu 20.04 + ROS Noetic，nlopt这个包有时候装不上。解决办法是直接源码编译nlopt，然后把Fast-planner里的find_package(nlopt REQUIRED)改成手动指定路径。另外，整个工程对C++14标准有依赖，编不过的时候检查一下CMakeLists里的设置，但这个在Noetic底下问题不大。
2.2 跑通仿真的完整步骤
编完源码之后，仿真分两步：先启动仿真器和规划器，再启动Rviz可视化。
终端1：
source ~/catkin_ws/devel/setup.bash roslaunch ego_planner basic_obstacle.launch这一步会启动Gazebo仿真环境、PX4的SITL仿真模式、以及ego-planner的规划模块。启动完成之后，你在终端里应该能看到“UGV Planner”相关的输出日志。
终端2：
roslaunch ego_planner rviz.launch然后在Rviz里使用2D Nav Goal工具，在地图上点一个目标点，就可以看到EGO-Planner规划的轨迹了。如果仿真环境下感应到地面上的障碍物Cube，你会看到轨迹会选择绕行而非穿过。
这一步跑通的意义很大：真机上所有的ROS话题结构、轨迹下发方式、飞控控制接口，在仿真是和真机基本一致的。你后续真机调试的所有问题，90%都能在仿真阶段先暴露出来。
2.3 仿真阶段的三个关键话题接口
在仿真环境下，你如果开了rostopic list，会看到几个核心话题，真机阶段也会用到：
| 话题名 | 类型 | 作用 |
|---|---|---|
| /planning/pos_cmd | quadrotor_msgs/PositionCommand | 规划器输出的位置、速度、加速度期望值 |
| /odom | nav_msgs/Odometry | 当前位置和速度，由仿真PX4发布 |
| /map_generator/global_cloud | sensor_msgs/PointCloud2 | 仿真环境中的障碍物点云 |
在仿真里，规划器是直接从/map_generator/global_cloud接收障碍物信息的，但真机上这个信息要换成D435i的深度点云。所以仿真阶段建议做一个测试：手动把global_cloud的话题停掉，换成realsense点云输入，看规划器是否会正常更新局部占据栅格。这个我在真机前做了测试，发现点的坐标系和话题频率对规划器影响巨大，建议你们也提前测。
3. 真机硬件选型与整机组装
3.1 无人机平台配置参考
我对真机平台的建议是：不要用微型穿越机，也不要一上来就搞大轴距载重机。最适合这套方案的是轴距450mm~550mm之间的四旋翼，负载能力在1kg左右，续航能到15分钟以上。我自己的配置可以参考一下：
- 机架：T-Motor F450改进款，碳纤维机臂
- 飞控：Pixhawk 6C，固件PX4 v1.13.3
- 动力：T-Motor MN3110 700KV + 1045桨
- 电池：6S 5200mAh
- 机载电脑：Intel NUC 10代i5（16GB内存，256GB NVMe SSD）
- 视觉传感器：Intel RealSense D435i
- 定位补充：室内飞的话我加装了OptiTrack动捕系统的机载Marker贴点
NUC装在飞机中心板上，注意要用减震板+减震球固定，否则电机高频振动会导致IMU数据质量下降，VIO容易飘。D435i的安装位置我建议放在机头正前方朝下倾斜15°~30°，这样前方和下方都能看到障碍物，避障视野更好。
3.2 Pixhawk与NUC的通信链路
NUC和Pixhawk之间用USB线直连。Pixhawk通过USB模拟出一个串口设备（/dev/ttyACM0），NUC上跑mavros节点，负责把PX4的飞控消息（姿态、位置、速度、状态机）和EGO-Planner的控制指令相互转换。
这里强烈建议你改一改PX4的参数：
- MAV_1_CONFIG：设置为TELEM2口（具体看飞控接口）
- MAV_1_MODE：设置为Onboard
- 在QGroundControl里把EKF2_AID_MASK设置为视觉定位或光流定位可用模式
如果你的定位方案是VIO，那么还需要把视觉里程计的位置数据通过mavros/vision_pose/pose话题发到PX4，PX4的EKF会融合视觉位姿。刚开始调的时候，建议先切到手动模式或定高模式，确认mavros状态正常再切Offboard。
3.3 机载电脑的软件部署
NUC上我装的是Ubuntu 20.04 + ROS Noetic，和仿真环境完全一致。软件栈包含四部分：
- ego-planner主程序（编译好的catkin工作空间）
- realsense-ros驱动（从Intel官方源安装）
- mavros通信节点
- vins-fusion或Fast-Lab自带的VIO节点，用于融合D435i的IMU和图像输出位置姿态
部署的时候有一点必须提醒：整个系统是三线程高频运行的，D435i出图30fps、VIO融合30fps、EGO-Planner规划5~10Hz，NUC的CPU会一直处于中高负载。建议给NUC装一个主动散热风扇，否则板子过热降频后，深度图和VIO帧率骤降，规划器会开始频繁报motion plan fail。
4. RealSense D435i标定：复现路上最大的坑
4.1 为什么要标定，标的是什么
说实话，这颗D435i是我复现过程中花时间最多的地方。B站视频里相机好像插上就能用，但你真的拿到手会发现，直接用出厂深度图给VIO用，位置飘得非常快，十分钟能飘出去好几米，根本飞不了。
D435i的标定分两块：
- 相机内参标定：包括彩色相机内参（焦距、主点、畸变系数）、深度相机内参、以及彩深相机之间的外参
- 相机与IMU的外参标定：也就是D435i内部那颗BMI055 IMU相对RGB相机、深度相机坐标系的旋转和平移量
之所以必须标，是因为RealSense的出厂参数是基于单个相机模组的标定结果，但每台设备的组装公差不同，尤其IMU在电路板上的贴合位置和角度存在工厂装配误差，导致出厂外参和实际不一致。直接拿来给VIO做融合，误差会很快累积。
4.2 D435i标定实操流程
我自己用的标定方案是Intel官方工具Dynamic Calibration，再配合kalibr做相机到IMU的外参标定。
第一步：安装Dynamic Calibration工具，这个工具在Windows和Linux下都有。把D435i固定住，打开工具，跟着提示旋转设备，软件会自动生成一个校准JSON文件。把这个文件用rs-enumerate-devices --json写入设备即可。
这里有个细节：标定IMU之前，先把相机放在桌面上静置几分钟，让IMU温度达到稳定。BMI055这种MEMS陀螺仪对温度漂移很敏感，你刚从室外拿进来就标定，结果大概率不准。
第二步：相机到IMU外参标定，我用的是kalibr。操作步骤大致是：
- 准备一个Aprilgrid标定板，打印A3大小，尽量平铺
- 录制一个5分钟左右的rosbag，手持相机在标定板前缓慢旋转、平移，动作要小要慢
- 用kalibr_calibrate_imu_camera标定外参和时间偏移
命令参考：
kalibr_calibrate_imu_camera \ --target april_6x6_50x50cm.yaml \ --cam camchain.yaml \ --imu imu.yaml \ --bag dataset.bag \ --time-calibration标完之后，你会得到imu到cam的旋转和平移矩阵。在使用vins-fusion时，这四个参数要手动填进配置文件的body_T_cam_0矩阵里。
4.3 标定验证：看VIO飘不飘
标定完不能直接上真机，先做一次验证。步骤很简单：把相机固定好，蒙上镜头，命令行里跑vins-fusion，观察轨迹是否在几分钟内保持静止。
如果标定精确，你会看到位姿在10cm以内微小晃动；如果外参标定错了，位姿会朝固定方向缓慢漂移，或者高度方向出现周期性震荡。
我第二次复现时，因为IMU外参标定的旋转矩阵有一个元素符号填反了，导致VIO输出在x方向持续漂移，无人机一推油门就往右猛偏，差点撞墙。所以这一步真的值得花上两整天去做，别省。
5. 从仿真到真机：飞行前的联调与参数整定
5.1 PLAN-A方案和PLAN-B方案怎么选
Fast-Lab在真机里提供了两种控制方案，我在复现时都试过，差别非常大，很多人没搞懂就乱接，结果飞机根本不动。
PLAN-A：板载自治方案
PLAN-A是NUC完全自主的流程：EGO-Planner在机载电脑上接收目标点，规划出轨迹，然后把位置控制指令直接通过mavros发给PX4。这个方案适合完全自主飞行，不依赖外部电脑。
PLAN-A的启动流程是把ego-planner里的实机launch文件和vins-fusion、mavros、realsense全部跑在NUC上，一键起飞后自主导航。优点是自治能力强，缺点是所有问题都在飞机上，排障难度高。
PLAN-B：地面站协同方案
PLAN-B是地面站电脑和机载NUC协同工作的方案。地面站负责运行EGO-Planner的规划模块，NUC只负责收集传感器数据和执行飞控指令，二者通过网络连接共享ROS topic。
PLAN-B的调试难度低很多，因为你可以在地面站上用Rviz直接看到规划器视角里的点云、轨迹、目标点，出问题也方便分析。但代价是需要高稳定性的局域网，一旦断连飞机就会失去规划能力，只能切回手动。
我自己是先用PLAN-B完成了整个飞行验证，确认稳定之后才切到PLAN-A的。强烈建议新手也这样干。
5.2 真机飞行参数整定经验
EGO-Planner主要参数都在ego_planner_node的yaml文件里，我贴几个我调试后比较稳的值供参考：
max_vel: 1.0 # 最大速度 m/s，室内新手别超过1.2 max_acc: 3.0 # 最大加速度 m/s^2 swarm_enable: false use_optimal_time: true optimization: lambda_smooth: 1000.0 lambda_obs: 150.0 lambda_fitness: 1.5 lambda_time: 5.0 obstacle: safety_margin: 0.35 # 避障安全距离，单位m box_side: 8.0 # 局部地图半边长这里重点说下避障安全距离。safety_margin这个参数决定了规划轨迹和障碍物之间保持的最小距离。数值越大越安全，但也会让轨迹绕的幅度更大，在窄走廊里甚至可能找不到可行路径。我实测在0.3m~0.5m之间比较合适，0.35m是稳妥值。
另外，max_vel千万别一手推太高。D435i在30fps下，深度空洞和噪点会让规划器误判一些障碍物，速度高了之后留给重规划的时间就短，容易出现“看到障碍物但来不及绕”的情况。我飞了十几架次之后，觉得在室内环境1.2m/s是安全上限。
5.3 Offboard模式切换与安全保护
真机起飞前，一定要设置好PX4的失控保护。我的做法是：
- 在QGroundControl里把遥控器通道RC_MAP_MODE设置为飞行模式切换通道
- 设置一个两段开关：一段是Position控制，一段是Offboard
- 在ego-planner的launch里加一个/traj_server节点，持续向PX4发送位置指令，防止PX4因为没有Offboard指令源而自动退出
起飞流程我是这么走的：遥控器解锁 → 切Position模式 → 推油门到1米悬停 → 确认VIO姿态稳定 → 发送目标点 → 切Offboard → EGO-Planner接管。一旦发现轨迹不对头，立刻切回Position模式。别直接拔电池，那是最无奈的办法。
6. 常见问题与排查技巧实录
复现过程我踩的坑比想象中多得多，这里把那几个最具代表性的整理成一张速查表，方便大家遇到问题直接对号入座：
| 现象 | 可能原因 | 排查方法 |
|---|---|---|
| EGO-Planner启动后无轨迹输出 | 目标点没有正确收到，或者没有通过Rviz 2D Nav Goal发布 | 用rostopic echo /move_base_simple/goal检查是否有消息 |
| 规划轨迹反复抖动 | 深度点云噪声太大，或者局部地图的esdf更新太慢 | 打开Rviz观察点云，适当增大safety_margin |
| VIO位置漂移严重 | D435i标定精度不够，VNS配置中相机和IMU外参不对 | 重新跑kalibr标定，检查配置文件中的body_T_cam_0 |
| Offboard模式下飞机不动 | mavros和PX4的Offboard模式没有正确建立 | 确认QGC里Offboard状态，检查mavros输出的控制指令话题 |
| PX4报EKF拒收视觉位姿 | vision_pose的坐标系定义和飞控期望不一致 | 检查位置数据点是ENU还是NED，统一XYZ轴方向 |
| 深度相机画面上有大量黑色空洞 | 反光、纯色墙面、超出深度范围 | 调整D435i的depth confidence，或者开启HDR模式 |
| 飞机起飞后往一个方向猛偏 | VIO的IMU外参有误，或飞控的机架方向配置不对 | 先看px4的EKF yaw是否稳定，再检查IMU外参标定 |
这里单拎出来讲一个非常坑的地方：PX4和VIO的坐标系。Fast-Lab的demo默认世界坐标系是ENU（东北天），PX4内部惯导用的也是ENU，但ROS里不少相机驱动和VIO输出的是相机坐标系或ROS标准坐标系。如果你不做坐标系转换，直接把vins-fusion输出的位姿发给PX4的vision_pose/pose，飞控EKF会认为你给的姿态和RTL的位置是错的，会拒绝融合，甚至在空中生成一个奇怪的目标位置，飞机会朝反方向猛冲。
我最后一次炸机就是这么来的。还好当时飞得低，只在草地上滚了一圈，螺旋桨断了一根。后期我把vins-fusion配置里的output坐标系改成了ENU，同时在mavros里加了frame_id转换，这才彻底解决。
7. 最后再分享几点自己测下来的感受
这套项目从B站视频到真机飞起来，整个周期我用了将近一个月。中间有至少五天时间都在和标定、坐标系、飞控参数打交道，真正写代码的时间反而很少。说白了，这个项目的难点根本不在算法本身，而在于你能否把算法正确嵌进一套真实的无人机系统里。
我觉得对打算复现的朋友来说，最经济的路径是这样的：先花三天跑通仿真，把EGO-Planner的输入输出机制彻底吃透；再花一周做真机平台组装和D435i标定；然后先不装桨，在地面做完整的联合调试，确认所有ROS节点话题都在正常运转；最后才找一片空旷的、没有信号的草坪做低空试飞。
有一个测试方法我觉得值得推荐：第一次真机飞行之前，你可以用一根细绳子把无人机拴在地面锚点上，然后手动加重力悬停，让EGO-Planner在受限状态下执行规划。这样就算是飞控或规划器出现错误，飞机最多被绳子拽住，不会直接飞走。
另外，每次试飞前的电池电压、螺旋桨动平衡、D435i镜头清洁这三件事一定要列进检查单里。螺旋桨有一点裂纹在高转速下都会引出不正常震动，震动直接传导到IMU，VIO就会发飘，整条链路就全完了。
还有一个小技巧：D435i的镜头前面我建议贴一张防刮膜。这玩意儿看着不起眼，但在低速穿越树枝和墙角的场合，它可能是保住整架飞机价值的唯一防线。
如果大家后续也跑通了这套系统，欢迎回来交流下你用的真机参数配置。每个人手上的硬件不一样，标定和整定的结果也会有差异，多对比几组数据，会让你对这套系统更加心里有数。
