开头写什么我直接从B站视频切入用一个真实的场景钩子。在B站看到浙大Fast-Lab的无人机穿林视频时我相信不少人第一反应是这是特效吧。一架四旋翼在杂乱树枝和柱子之间高速穿行轨迹像老司机开赛车一样流畅几乎没有冗余的刹车和转向。评论区最高赞的问题永远是这代码开源了吗答案是开源了而且核心就是EGO-Planner加Intel RealSense D435i这套组合。我自己是做机器人运动规划的去年花了大概三周从看视频、下代码、跑仿真一路折腾到真机悬停和慢速绕障中间踩了非常多的坑。这篇博文就是完全围绕如何复现这套项目来写的先讲清楚EGO-Planner和D435i各自扮演什么角色再讲硬件选型、软件环境、D435i标定到底标什么然后给出从仿真到真机的完整操作路线最后把我实际遇到的坑和排查方法全部列出来。适合手里有或者准备入手一台四旋翼、想搞无人机自主导航的学生、工程师和极客哪怕你是第一次接触PLANNER按这个流程走也大概率能跑起来。1. 先把穿树飞行这件事拆开看1.1 EGO-Planner在这个项目里到底干了什么很多人把EGO-Planner理解成避障算法这个说法不算错但不完整。它本质是一个实时的局部轨迹规划器输入的是环境感知信息点云或地图输出的是从当前状态到目标点的速度、加速度、姿态指令。在Fast-Lab的框架里它是整条自主飞行链路里做决策的那个模块。EGO-Planner的全称是Efficient Geometry-based local planner。它最核心的贡献是让无人机能够在没有预先建好的全局地图下仅依赖局部传感器数据不断优化出一条平滑、安全、动力学可行的轨迹。和传统方法不一样的地方在于它不需要把空间全部离散成网格然后做昂贵的ESDF欧式符号距离场计算而是通过碰撞信息直接在原始点云附近构造梯度把轨迹优化变成一个效率很高的优化问题。你可以粗暴地把它理解成不是画一张精确的地图再找路而是边走边用周围哪块空间是障碍物来引导轨迹修正所以对算力要求低、反应非常快。在视频里那些行云流水的穿枝动作实际就是EGO-Planner每几十毫秒收到一次新的点云然后基于当前无人机速度、目标点和障碍分布重新规划出未来一小段时间的轨迹。它强大的地方在于对动态环境也有一定鲁棒性突然闯入的障碍能在下一帧重新规划时被绕开。1.2 RealSense D435i负责把世界变成无人机懂的语言EGO-Planner再聪明也得有人告诉它左边三米有柱子、右前方两米是树枝。这个感知环节在Fast-Lab很多项目里就是由Intel RealSense D435i完成的。D435i是一款主动式红外立体深度相机内置两颗红外传感器和一个红外点阵投射器能够输出RGB彩色图和深度图。在无人机上我们更关心深度图——每个像素保存的是该位置到相机的距离。深度图再结合相机内参就可以生成一帧一帧的3D点云。EGO-Planner吃的就是这个点云。选D435i而不是激光雷达或者普通双目相机主要有几个原因。第一是重量和体积D435i大概只有几十克一个手掌大小塞进机载电脑旁边很轻松第二是深度范围标称0.2米到10米左右对于几米每秒的室内或园区飞行完全够用第三是它自带一颗IMU惯性测量单元后续要跑VINS-Fusion这种视觉惯性里程计做定位时非常方便第四是ROS支持太成熟了官方驱动realsense-ros直接发布点云、深度、IMU话题省掉大量开发时间。当然D435i不是万能的。它是主动红外方案在强阳光下会有一点衰减对玻璃、黑色吸光物体、水面的深度估计也容易出错。这些坑后面在第五章专门说。1.3 一条完整的复现链路由哪些模块组成我把这套系统拆成六块也是后续复现的顺序定位模块无人机世界里没有GPS或者只有很弱的GPS时需要知道我在哪。常用方案是VINS-Fusion融合D435i的图像和IMU数据输出相机位姿有动捕系统的地方也可以用Motive这类光学动捕。感知模块D435i的深度信息转成局部点云加上一些预处理比如降低分辨率、裁剪太远太近的点、降噪。局部地图模块EGO-Planner会把点云转换成代价环境实际代码里就是构建一个局部的占用地图或者点云体素块。轨迹规划模块EGO-Planner根据目标点、当前状态和地图信息解算出轨迹输出位置、速度、加速度序列。控制模块把规划出的轨迹转换成飞行控制器能接收的指令。Fast-Lab社区常配合px4ctrl这个控制包或者直接通过MAVROS发期望位置指令给PX4。飞控与执行机构PX4飞控在OFFBOARD模式下接收外部指令控制电机转速最终让机体按轨迹飞。这里我特别想强调定位模块。很多新手上来就把D435i点云丢给EGO-Planner以为就能飞了结果无人机一离开机架就飘。原因很简单EGO-Planner只解决往哪走和怎么走我现在在哪必须由别的模块给。在GPS信号好的室外你可以用PX4自带的位置估计但在室内或者楼间GPS不稳定的场景一定要先解决视觉定位否则规划器和控制器都在对着一个错误的当前位置做计算越算越飞偏。2. 硬件准备视频里那台无人机到底怎么配的2.1 为什么是D435i而不是T265、激光雷达或纯双目我在初期选传感器时对比过好几种方案。Intel RealSense T265是一颗双目鱼眼相机加IMU它的主要能力是输出视觉惯性里程计VIO直接给你定位结果但它不输出深度点云。也就是说如果你用T265还得额外再带一个深度相机才能喂EGO-Planner这样一来重量、功耗、装机的复杂度全上去了。D435i一个人干两份活测量深度给规划器同时成像给VINS-Fusion做定位这才是它在这个项目里最舒服的位置。激光雷达比如Livox MID-360这类固态雷达在感知距离和抗光照上确实比D435i强但价格基本上是D435i的十几倍而且驱动、标定、点云处理的门槛更高。对于复现Fast-Lab项目这个目标来说D435i是最平衡的选择成本低、资料多、官方支持好、社区踩坑记录也多出了任何问题基本都能搜到答案。纯被动双目比如D400系列去掉主动红外投射器也可以但室内白墙、弱纹理环境深度会大面积缺失对规划器非常不友好。D435i有主动红外投射器在室内生成可见纹理能显著提升深度图完整性。2.2 整机怎么搭机架、飞控、机载电脑和电源的搭配传感器定了之后整机配置是我踩坑最多的一个环节。因为我早期只顾着省钱拿了一台450mm轴距的老机架加一块Pixhawk 4飞控配了一块树莓派4B当机载电脑结果发现根本飞不了——不是算法问题是整机重量逼近起飞限重留出来的机动裕量太小EGO-Planner在后端优化时算出的许多轨迹对这台胖机根本不可行。如果你要复现这套项目我建议机架选轴距350-450mm的X型四旋翼电机建议配至少500g单轴推力的组合电池用4S规格机载电脑选择NUC、Jetson Orin Nano这类性能远强于树莓派的x86或ARM电脑。因为EGO-PlannerLivox?不是——至少EGO-Planner加VINS-Fusion加D435i点云处理树莓派4B算起来很吃力点云帧率会掉到5Hz以下规划器反应明显迟钝。供电要分两路飞控、电机、电调用动力电池直供或带BEC的电源模块机载电脑、D435i、数传模块建议通过独立的5V降压模块或专门的稳压模块供电避免电机启动瞬间电压跌落导致相机掉线、电脑重启。我遇到过最尴尬的情况就是推油门起飞瞬间D435i深度流突然中断排查半天发现是USB供电被拉崩了。2.3 相机安装角度和减震的小细节D435i的安装角度直接影响感知效果。视频里Fast-Lab的无人机相机通常是前向稍微下倾15到20度这个角度既能看到前方障碍又不会在平飞时看到大片地面导致规划器误以为前方空间很多。如果你装得完全水平在稍微低头加速时近处地面会大量进入视野点云里冒出很多地面点EGO-Planner会把它们当成障碍导致无人机飞飞停停。另一个细节是减震。D435i同时给VINS-Fusion提供图像和IMU数据机身高频振动会严重影响VINS的位姿估计。强烈建议在相机和机架之间加一层减震泡棉或专门的相机减震球把高频振动滤掉。不过要注意不能太软否则相机会在飞行中摆动反而制造新的运动模糊。3. 软件环境与D435i标定的正确打开方式3.1 系统的版本焦虑怎么选EGO-Planner官方仓库是基于ROS开发的最常见的是Ubuntu 18.04 ROS Melodic也有不少人在20.04和Noetic上跑通过。如果你是第一次接触这个项目我建议直接用官方README里推荐的组合不要一上来就追新系统因为PX4固件、MAVROS、realsense-ros、VINS-Fusion在18.04Melodic下的兼容性最稳B站和博客上的教程也基本是这个组合出问题时能搜到现成答案。安装顺序大概是装Ubuntu系统、装ROS、拉取EGO-Planner仓库并编译、安装realsense-ros、安装MAVROS、选择PX4 SITL或真机飞控固件、配置VINS-Fusion。每一步都有官方文档重点在于每个包的依赖要装干净尤其是rosdep这一步。我遇到至少三次编译报错都是因为缺了某个系统依赖最后用rosdep install一条命令解决的。3.2 D435i标定到底标的是什么最新网络热词里realsense d435i标定非常火我估计很多人复制这个项目时都在标定这里卡住了。D435i的标定其实分成三件不同的事不要混为一谈。第一件是深度相机本身的厂内标定也就是让深度图在不同温度、不同距离下尽量准确。如果你买的D435i是全新的出厂标定是够用的不需要用户自己做。只有当相机受过磕碰、外壳变形、深度图明显出现系统性误差时才需要用Intel官方的Dynamic Calibrator重新标定。这个工具需要在黑色标定板的帮助下完成操作并不复杂但很多人其实根本不需要走到这一步。第二件是IMU的标定。D435i内置一颗IMU如果你要用它跑VINS-FusionIMU的噪声和随机游走参数最好通过imu_utils这种工具实测出来不要直接用默认值。实测方法很简单将相机静置一到两小时记录IMU数据然后离线计算得到加速度计和陀螺仪的噪声密度和随机游走参数把这些参数填进VINS配置文件里。这一步对定位精度的影响非常大特别是静止或低速状态下的漂移。第三件是相机到机体坐标系的外参标定。EGO-Planner规划时点云需要从相机坐标系转换到机体坐标系或世界坐标系。如果你用VINS-Fusion输出相机在世界系中的位姿相机和机体的固定外参也参与了这个变换。比较粗糙但可用的做法是量好相机的安装位置和欧拉角填进配置里追求精度可以用Kalibr做相机-IMU外参标定或者用手的眼标定方法。我的建议是第一件通常不做第二件要认真做第三件先用测量法等能稳定悬停后再考虑上标定板精标。别一上来就卡在标定上很多项目根本不需要那么高的精度就能飞起来。3.3 EGO-Planner的核心参数给它一个飞机的性格EGO-Planner编译通过、能跑起仿真后真正决定飞行手感的是配置文件。我建议用官方仓库自带的参数作为起点只调整几个关键量无人机尺寸和膨胀半径常见参数如collision_radius、model_size这个参数直接决定规划器离障碍物多远算碰壁。设得越小轨迹越激进越容易贴障碍飞行设得越大越安全但可能出现明明能过去却绕路的情况。最大速度和最大加速度max_vel、max_acc视频里飞得快不是因为电机猛而是规划器允许它飞得快。第一次真机首飞建议把最大速度压到2到3米每秒最大加速度压到3到5米每秒平方等验证整个链路稳定后再逐步放开。速度极限设得过高而且机体推力不够时规划轨迹会经常优化失败。规划频率和地图尺寸规划器每次收到新点云都会重新规划一次轨迹频率太低则反应迟钝频率太高CPU吃紧。地图尺寸则决定了无人机能看到多远的未来。D435i的有效深度约10米地图尺寸设6到8米是比较合理的选择太远的大部分点云都是无效噪声。这些参数并没有一个万能最优值它高度依赖机身动态特性和飞行场景。我的建议是在仿真和手动飞行测试中每调整一个参数就记录飞行表现不要一次改多个否则出了奇怪现象根本定位不到原因。4. 从仿真到真机的完整复现流水线4.1 先让规划器在仿真里跑出第一批轨迹我强烈建议任何人不要跳过仿真直接上真机。EGO-Planner官方仓库通常会带一个仿真器或者提供Gazebo模型你将四旋翼模型加载到Gazebo里启动EGO-Planner节点再打开Rviz就能看到飞机模型在三维环境中飞行。仿真阶段的核心任务是验证三件事第一点云输入链路是否正常D435i仿真模型是否向规划器发布了有效点云第二EGO-Planner能否根据点云和随机目标点生成平滑轨迹Rviz里会显示起点到终点的规划曲线绿黄渐变代表速度第三如果仿真里没有定位模块可以直接用仿真模型的真值位姿代替这样规划器会运行得非常理想。这个阶段特别适合调上面说的那批参数。我第一次跑仿真时发现飞机老是突然蹿高又压下后来定位到是点云里有大片地面引发的膨胀区域把安全距离调小并在点云预处理里裁剪掉靠近地面的部分后飞行就平稳了。4.2 定位解决不了什么规划器都白搭仿真的理想条件掩盖了真机最麻烦的问题定位。如果在室内没有任何GPS无人机必须靠自己感知知道自己在哪。这里我推荐VINS-Fusion配合D435i。VINS-Fusion是Fast-Lab团队常用的视觉惯性定位方案核心是读取相机图像和IMU数据经过紧耦合优化后输出相机在世界坐标系中的位姿。你需要在VINS配置里正确填入D435i的相机内参、畸变系数、IMU到相机的外参和IMU噪声参数这些参数来源就是第三章说的IMU标定和外参标定。把VINS-Fusion跑起来用D435i对着一个有纹理的房间慢慢移动在Rviz里看是否输出稳定的轨迹相机移动时轨迹是否平滑跟随静止时漂移是否可控。如果静止时轨迹还在飘多半是IMU外参或噪声参数没填对。VINS输出的是视觉坐标系不是机体坐标系后续控制模块做坐标转换时要把这一层关系理清楚我习惯用一个简单的TF广播把VINS坐标系接到机体系上。4.3 真机首飞前的最后一百米真机飞行是整个复现过程中最刺激也最容易出问题的一段。我的首飞流程是这样安排的先不装螺旋桨用代码通过MAVROS发送小的位置指令确认飞控能收到来自机载电脑的OFFBOARD指令并观察电机是否按预期响应。装桨后到室外宽敞无人场地先用遥控器手动起飞到1米高度。然后切到OFFBOARD模式让EGO-Planner接管发送一个只有零点几米的微动目标点观察无人机是否自己横移到目标位置。逐步增加目标点距离和速度每一步都做好随时切回手动模式的准备。全程另一位同事跟着无人机STOP键或者遥控器的紧急关闭开关要一直在手上。EGO-Planner和飞控之间的控制指令流一般是这样的规划器输出带时间戳的位置、速度、加速度轨迹控制模块按当前时间采样轨迹得到期望状态然后通过MAVROS发送给飞控。你可以选择发位置设定点也可以发速度设定点PX4的OFFBOARD模式都支持。注意OFFBOARD模式下飞控要求外部指令必须持续不断到达一般要保持在5Hz以上否则会超时自动退出OFFBOARD回到自稳模式。突然退出OFFBOARD本身不算危险但如果切回的是手动模式而遥控器又不在手边那就会很麻烦。所以我的习惯是机载电脑没准备好绝不切OFFBOARD遥控器拇指始终待命。5. 实践中的踩坑记录与排查方法5.1 D435i在使用中最常见的几种翻车第一个坑是暖机漂移。D435i刚开机时芯片温度没稳定深度图会有明显偏差特别是在开机前两分钟内。我一开始不知道这件事总是一上电就起飞深度数据忽远忽近导致EGO-Planner生成的轨迹来回抖动。后来养成习惯至少开机静止五到十分钟等深度预热深度图稳定了再进入飞行流程。第二个坑是USB接口和带宽。D435i需要USB 3.0且实际使用中占用带宽比较大。如果你把它接到机载电脑的前置USB口或者经过延长线经常会出现深度流时断时续。建议直连主板原生的USB 3.0接口避免使用Hub。机载电脑如果同时接遥控接收机、数传、相机USB电源管理很容易出现问题Linux下可以用lsusb -t检查设备连接状态。第三个坑是点云噪点。D435i在室外阳光下、玻璃幕墙前、纯白色吸光物体上深度容易出现孔洞和飞点。EGO-Planner收到飞点后会把远处一个孤立点当成障碍规划出一条奇怪绕路。解决办法是在点云预处理阶段加上距离裁剪和时间滤波比如只保留0.5米到8米范围内的点并做体素降采样再送入规划器。5.2 EGO-Planner规划器的几类典型故障我在跑EGO-Planner时遇到过几个很有代表性的故障。一是规划结果不断震荡无人机在目标点附近来回窜。这通常是点云更新频率和规划频率不匹配或者点云噪声太大导致每次规划的起始段不同。我把点云降采样后震荡明显缓解。二是轨迹规划失败甚至不输出。EGO-Planner在环境过于复杂且膨胀半径过大时可能找不到可达的轨迹。这时候检查日志里是否提示约束不满足如果是把安全距离和速度上限适当放松一点。另外无人机初始状态必须传给规划器一个合理的当前速度如果你冷启动时填的初始速度不对规划器可能在本地最小解里出不来。三是规划器和定位之间出现延迟。VINS输出位姿、EGO-Planner收到位姿后重新规划整个链路总延迟超过一两百毫秒时无人机飞行会有打太极一样的迟滞感。排查方法是用rostopic hz逐个检查相机、VINS、EGO-Planner输出频率找出哪个环节掉链子再优化对应节点的处理逻辑。5.3 真机安全底线这句话必须放在最后所有自主无人机的复现项目到最后拼的不是算法有多花哨而是安全底线够不够扎实。我第一次上真机前特意把首飞地点选在开阔操场周围一百米内没有人车地面平整。飞行高度保持在2米以下通过B站视频里学的先低速贴地策略把所有速度参数压到非常保守。还有一件事我非常后悔没早点做给机载电脑配一个一键急停脚本。后来我通过MAVROS绑定了一个独立按键按下后立即发送紧急停桨指令效果比慌乱中找遥控器开关靠谱得多。这套脚本不复杂但关键时刻能救无人机也更让我理解了为什么Fast-Lab的很多试飞视频里地面站总有一个一直放在急停键上的手指。写在最后的个人体会这套项目我从B站视频看到真机起飞最大的收获不是跑通了EGO-Planner而是真正理解了自主无人机其实是感知-定位-规划-控制四个模块的接力赛任何一个环节薄弱都会拖垮整条链路。D435i标定、VINS外参、EGO-Planner参数这些东西单拎出来每一个都是看似可有可无的小事但串起来之后任何一个小误差都会被放大成航向偏移、高度漂移甚至炸机。最后再分享一个我在实际使用中觉得很有用的小技巧把EGO-Planner所有参数调参前先在Rviz里开着点云和规划的轨迹人拿着无人机小范围移动模拟几次穿过障碍的过程观察规划轨迹是否及时避开再决定要不要上真机。这相当于一次廉价的人手搬运仿真能过滤掉大部分愚蠢错误比直接飞省下大量修机时间。如果你现在正卡在标定或者第一个真机起飞的恐惧里记住所有人都是这么过来的稳住心态按链路一步步排查很快就能看到属于你自己的那台无人机在树下画出流畅弧线了。