EGO-Planner与D435i实现无人机自主导航：从仿真到真机全流程指南

最近总有人私信我说在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版本是NoeticGazebo用的是自带版本。依赖库主要是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 Noeticnlopt这个包有时候装不上。解决办法是直接源码编译nlopt然后把Fast-planner里的find_package(nlopt REQUIRED)改成手动指定路径。另外整个工程对C14标准有依赖编不过的时候检查一下CMakeLists里的设置但这个在Noetic底下问题不大。2.2 跑通仿真的完整步骤编完源码之后仿真分两步先启动仿真器和规划器再启动Rviz可视化。终端1source ~/catkin_ws/devel/setup.bash roslaunch ego_planner basic_obstacle.launch这一步会启动Gazebo仿真环境、PX4的SITL仿真模式、以及ego-planner的规划模块。启动完成之后你在终端里应该能看到“UGV Planner”相关的输出日志。终端2roslaunch ego_planner rviz.launch然后在Rviz里使用2D Nav Goal工具在地图上点一个目标点就可以看到EGO-Planner规划的轨迹了。如果仿真环境下感应到地面上的障碍物Cube你会看到轨迹会选择绕行而非穿过。这一步跑通的意义很大真机上所有的ROS话题结构、轨迹下发方式、飞控控制接口在仿真是和真机基本一致的。你后续真机调试的所有问题90%都能在仿真阶段先暴露出来。2.3 仿真阶段的三个关键话题接口在仿真环境下你如果开了rostopic list会看到几个核心话题真机阶段也会用到话题名类型作用/planning/pos_cmdquadrotor_msgs/PositionCommand规划器输出的位置、速度、加速度期望值/odomnav_msgs/Odometry当前位置和速度由仿真PX4发布/map_generator/global_cloudsensor_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代i516GB内存256GB NVMe SSD视觉传感器Intel RealSense D435i定位补充室内飞的话我加装了OptiTrack动捕系统的机载Marker贴点NUC装在飞机中心板上注意要用减震板减震球固定否则电机高频振动会导致IMU数据质量下降VIO容易飘。D435i的安装位置我建议放在机头正前方朝下倾斜15°~30°这样前方和下方都能看到障碍物避障视野更好。3.2 Pixhawk与NUC的通信链路NUC和Pixhawk之间用USB线直连。Pixhawk通过USB模拟出一个串口设备/dev/ttyACM0NUC上跑mavros节点负责把PX4的飞控消息姿态、位置、速度、状态机和EGO-Planner的控制指令相互转换。这里强烈建议你改一改PX4的参数MAV_1_CONFIG设置为TELEM2口具体看飞控接口MAV_1_MODE设置为Onboard在QGroundControl里把EKF2_AID_MASK设置为视觉定位或光流定位可用模式如果你的定位方案是VIO那么还需要把视觉里程计的位置数据通过mavros/vision_pose/pose话题发到PX4PX4的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~10HzNUC的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_marginVIO位置漂移严重D435i标定精度不够VNS配置中相机和IMU外参不对重新跑kalibr标定检查配置文件中的body_T_cam_0Offboard模式下飞机不动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镜头清洁这三件事一定要列进检查单里。螺旋桨有一点裂纹在高转速下都会引出不正常震动震动直接传导到IMUVIO就会发飘整条链路就全完了。还有一个小技巧D435i的镜头前面我建议贴一张防刮膜。这玩意儿看着不起眼但在低速穿越树枝和墙角的场合它可能是保住整架飞机价值的唯一防线。如果大家后续也跑通了这套系统欢迎回来交流下你用的真机参数配置。每个人手上的硬件不一样标定和整定的结果也会有差异多对比几组数据会让你对这套系统更加心里有数。