1. ego-planner仿真的核心逻辑与环境规划1.1 仿真这套东西到底在解决什么问题ego-planner这个名字全称是Edge-based Gradient Optimization Planner说人话就是“基于边界梯度的优化规划器”。它是香港科技大学空中机器人实验室开源的一套无人机路径规划方案核心思路是把路径生成问题拆成“找一条可行路径”和“把路径压得更短更平滑”两步走。和传统基于采样或搜索的规划器不同ego-planner不依赖预先构建好的代价地图它直接在传感器点云数据上做优化所以对未知环境的适应能力很强。说到仿真很多人容易把RViz和Gazebo混在一起用其实这两个东西的角色完全不一样。RViz是ROS里的数据可视化工具它本身不负责物理仿真也不产生传感器数据它只负责把你已经有的数据——比如规划出来的路径、当前位姿、点云、地图——画出来给你看。Gazebo则是一个带物理引擎的三维仿真环境里面可以放一个带重力、带碰撞、带传感器噪声的无人机模型让算法在一个“假但真实”的世界里跑起来。所以一套完整的ego-planner仿真体系逻辑上是这样分工的Gazebo负责提供“这个世界长什么样、无人机飞起来什么手感”ego-planner算法负责“根据传感器输入算出接下来怎么飞”RViz负责“把你算出来的东西和整个决策过程可视化出来”。三者的关系有点像你做菜时候的“厨房、菜谱和成品摆盘”——Gazebo是厨房有真实的灶台和食材ego-planner是菜谱逻辑决定先放什么后放什么RViz是摆盘让你看清楚做出来的菜长什么样。1.2 版本选型ROS1还是ROS2Ubuntu哪一版这是新手问得最多的问题。ego-planner官方仓库目前最成熟、最稳定的支持环境是ROS1 Noetic Ubuntu 20.04。虽然也有ROS2的移植版本但官方文档、教程视频、社区里踩坑记录基本都集中在Noetic上而且很多依赖库比如plan_env、path_searching这类底层模块在ROS2环境下需要自己改编译配置工作量不小。如果你是第一次跑ego-planner仿真我的建议非常直接老老实实用Ubuntu 20.04 ROS Noetic。不要上来就挑战Ubuntu 22.04 ROS2 Humble的组合除非你已经有大量ROS2开发经验否则光是把依赖编译过、把TF树对齐、把RVIZ插件加载起来就可能耗掉你好几天。我见过太多人在ROS2上折腾ego-planner最后发现某几个包在ROS2下根本发布不出正确的话题又灰溜溜退回Noetic的案例。环境组合推荐程度说明Ubuntu 20.04 ROS Noetic首选ego-planner官方支持依赖完整社区案例多Ubuntu 18.04 ROS Melodic可用但过时部分依赖版本较老需要手动修补Ubuntu 22.04 ROS2 Humble进阶尝试需要自己处理大量依赖适配不推荐新手另外一个很多人忽略的点是磁盘空间和内存。ego-planner编译安装加上Gazebo模型库轻轻松松十几G。内存建议至少8G16G会更舒服因为编译的时候多个进程同时跑内存不够会直接OOM杀死编译进程报错还很迷惑有时候看半天以为是代码问题。1.3 源码编译与工程配置的几个关键细节ego-planner的编译过程本身不算复杂clone官方仓库然后catkin_make就好。但有几个细节你如果没注意后面一定会回来补课。第一个是依赖安装。官方仓库给的依赖清单是ros-noetic-nlopt、ros-noetic-catkin、python3-catkin-tools这些但你还需要确认系统里有armadillo、eigen等线性代数库。sudo apt install libarmadillo-dev和libeigen3-dev这两条命令装完之后再去catkin_make能少报很多错。第二个是工作空间的编译顺序。ego-planner的代码分成几个包有底层的plan_env、path_searching有上层的ego_planner还有仿真用的grid_path_searcher、rviz的插件包。catkin_make会自动处理依赖顺序但如果你用了catkin build最好加上--jobs参数控制并发度不然四个包同时编译内存一吃紧就把某个包编译进程杀掉了。第三个是编译前的环境变量。source /opt/ros/noetic/setup.bash这一步必须放到~/.bashrc里然后每次打开终端会自动加载。很多人编译时找不到ros包就是因为在当前终端里没source或者source的顺序不对。建议把ROS的环境变量放在所有其他source语句之后避免被覆盖。2. RViz可视化让规划过程真正“看得见”2.1 RViz显示组件配置的核心思路ego-planner跑起来之后你第一个要面对的就是RViz里的一堆显示选项。默认的ego_planner.rviz文件已经在仓库里配好了大部分显示项但很多人打开之后发现界面是黑的或者地图不显示就开始怀疑算法是不是出了问题。其实大多数情况下不是算法的问题而是RViz的显示配置和你当前的话题不匹配。RViz里的显示项本质上就是“订阅某个话题把它画出来”。你需要在左侧Display面板逐一检查每个显示项的Topic名称是不是正确。最常见的几个话题是/map_generator/global_cloud用于显示全局点云地图、/odom显示里程计位置、/planning/pos_cmd显示轨迹指令、/planning/trajectory显示当前规划的轨迹。如果这些话题没有数据RViz里面自然是空的。我自己常用的做法是先打开一个终端运行rostopic list把所有话题列出来再对照检查RViz里的Topic设置。这比在RViz界面里瞎点快得多。检查顺序是先确认话题存在、再确认话题有数据rostopic echo看一眼、最后确认RViz的Fixed Frame设置正确。Fixed Frame这一项默认是world如果和你的TF树对不上就算有数据也显示不出来。2.2 规划轨迹的可视化判读技巧当ego-planner正常规划之后RViz里会同时出现好几条不同颜色的线。很多人看不懂这些线分别代表什么这里我讲清楚一下。红色/粉色那条是全局参考路径就是前端路径搜索得到的初始解它满足避障但可能不够平滑。蓝色/绿色那条是经过后端优化后的最终轨迹也就是真正会发给飞控去执行的路径。如果优化效果好的话蓝色线应该和红色线大体走向一致但弯折更少、曲率更平滑。还有一个青蓝色的锥形或线框表示当前无人机的膨胀模型或者检测范围。判断规划质量时我习惯关注三点一是蓝色轨迹是否穿过了点云或地图中的障碍物二是轨迹的起点是否和无人机当前实际位置重合如果起点飘了说明状态估计或坐标系有问题三是轨迹终点是否落在你设定的目标点附近。这里有个很实用的调试技巧当你发现规划出来的轨迹剧烈左右摆动或者呈现锯齿状通常是膨胀半径设置太小、单步规划时间太短或者优化权重参数不合理导致的。可以先把轨迹显示里的Buffer Length调大把历史轨迹保留下来多观察几轮变化趋势再回去调整参数而不是看到一次异常就去改代码。2.3 RViz打不开、闪退、卡死的排查路径RViz打不开这个问题在ego-planner相关的报错帖里出现频率极高。但我可以负责任地说十次里面八次不是RViz本身的锅而是它的运行环境出了问题。最常见的一种情况是OpenGL渲染问题。如果你是在虚拟机上跑或者当前的显卡驱动不支持RViz要求的OpenGL版本界面就会卡在启动画面然后闪退。解决办法是在启动前加上export LIBGL_ALWAYS_SOFTWARE1强制软件渲染。这条命令能解决很多虚拟机里的渲染问题代价是渲染性能下降但作为可视化调试工具完全够用。第二种情况是分布式部署问题。如果你把RViz跑在远程机器上通过ssh转发显示过来需要确认ssh的时候加了-X参数而且远程机器有足够的X11权限。错误提示通常在终端里是QXcbConnection错误这时候可以去检查/etc/ssh/sshd_config里的X11Forwarding设置。第三种情况比较隐蔽是显示项里订阅了一个不存在的话题而且数据频率很高导致RViz的渲染线程阻塞。出现这种问题启动RViz之后CPU占用率极高、界面卡成PPT建议先把Display面板里的显示项一个个关掉找出是哪一个数据源把渲染拖垮了。3. 从RViz到Gazebo把“会画线”变成“会飞过去”3.1 为什么RViz跑通了还不够一定要过一遍Gazebo很多初学者在RViz里看到蓝色轨迹穿过了点云觉得自己已经把ego-planner吃透了。但这里有一个很关键的盲区RViz里只有“规划”没有“控制”和“动力学”。你在RViz里看到无人机从A点平滑飞到B点那是ego-planner规划出来的名义轨迹不代表无人机真的能跟着这条轨迹飞出来。Gazebo补上的正是这一环。它有物理引擎会给无人机模型加重力、空气阻力电机会有响应延迟传感器会有噪声飞控跑的是和真机上几乎一样的控制循环。所有在真机上可能炸机的问题——轨迹曲率过大导致侧翻、终点速度没收敛导致冲出目标、传感器噪声导致状态估计漂移——都能在Gazebo里提前暴露出来。我个人的体会是RViz仿真验证的是“算法想的对不对”Gazebo仿真验证的是“物理上能不能实现”。两者是递进关系。跳过Gazebo直接上真机等于你写了一道数学题没验算就去考试能用但是风险很大。3.2 Gazebo环境搭建与模型导入ego-planner的Gazebo仿真核心是跑一个带各种障碍物的三维世界让无人机在这个世界里自主飞行。仓库里提供了几个现成world文件比如basic.world和test.world在launch文件里通过world_name参数加载。默认情况下Gazebo会自动从在线模型库下载模型但国内网络环境经常下载失败导致启动后世界一片空白。解决办法是手动把用到的模型文件下到本地~/.gazebo/models目录或者在launch文件里指定GAZEBO_MODEL_PATH环境变量指向你下载的模型库目录。我建议的做法是先用带有--verbose参数的roslaunch启动一次Gazebo看终端里卡在哪一个模型的下载请求上然后单独去模型库里把这个模型拉下来放到本地对应目录。模型问题解决之后还有坐标系对齐的问题。Gazebo里的无人机模型有自己的sensor坐标系、机身坐标系真机的TF树在Gazebo里也完全复现。ego-planner要求点云数据在world坐标系下如果你发现RViz里地图有偏移或者旋转优先检查Gazebo模型里各类传感器的XYZ坐标准不准以及插件发布TF时的父坐标系和子坐标系设置。3.3 RViz与Gazebo联动的配置细节当Gazebo和ego-planner一起启动后整个系统里其实跑了两条链路。一条是Gazebo内部无人机模型→传感器→点云→发布到ROS话题另一条是ego-planner订阅点云→全局规划→局部优化→发布控制指令→Gazebo接收指令→作用到模型上。两条链路能否闭环取决于所有话题名称、坐标系、时间戳是否对齐。实际操作中我遇到过最隐蔽的问题是时间戳不同步。Gazebo仿真时所有话题的时间戳都要和仿真时钟/clock保持一致如果你没有在launch文件里设置use_sim_time为trueROS的话题会去用系统真实时间导致TF变换和时间戳错位有时表现为无人机在原地打转有时表现为规划的轨迹有滞后。检查方法很简单在启动RViz之前先把use_sim_time确认好然后在终端里rostopic echo /odom看一眼时间戳是否和当前仿真时间一致。还有一个是坐标系名称的硬编码问题。ego-planner代码里很多地方写死了world、odom、base_link这几个坐标系名称如果你的Gazebo模型里用的是别的名字比如base_footprint或者body那TF树就会断裂算法会因为找不到变换而报错。修改模型URDF文件里的坐标系名称是最干净的方案别想着去改ego-planner源码里所有硬编码字符串工作量太大了而且容易引入新bug。4. 常见问题排查与避坑实录4.1 Gazebo界面闪烁、卡死的原因排序“为什么gazebo界面一直在闪”这类问题底下回答五花八门但真正原因其实就那么几种。排第一的是显卡渲染问题和前面RViz打不开的原理类似虚拟机和双显卡笔记本尤其容易出现。解决办法是先试export LIBGL_ALWAYS_SOFTWARE1启动一次这能定位问题是否在GPU驱动层面。排第二的是模型加载失败导致的渲染异常。如果一个模型文件损坏或者缺少贴图Gazebo的渲染线程会在每次试图绘制这个模型时出错表现出来就是界面闪一下就恢复。排查方法是把终端输出打开找到和model、texture、mesh相关的报错信息一个模型一个模型地移除测试。排第三的是物理引擎时钟和渲染时钟跑飞。Gazebo的实时因子real_time_factor如果低于0.5界面刷新和物理仿真就会错开看起来画面一顿一顿像在闪烁。你可以通过roslaunch命令里的参数调整最大步长和迭代次数比如把max_step_size调大、把iters调小让物理计算更快跟上渲染节奏。4.2 规划器输出异常和发散问题规划算法跑着跑着突然输出NaN或者轨迹像抽风一样往天上窜这个情况在Gazebo里比在纯RViz仿真里更容易出现。原因很简单Gazebo里的点云数据带噪声会随机产生一些孤立点这些点如果恰好落在规划器检测范围边缘优化算法就容易计算出异常梯度。我的处理思路是把问题分层。先判断是输入异常还是算法异常把点云话题录成rosbag回放几次看是不是每次都在同一个位置出问题。如果是固定位置出现大概率是世界模型里有尖锐凸起算法在凸起附近无法找到光滑的路径解决办法是增大膨胀半径或者平滑滤波器尺寸。如果每次出现位置不固定则更可能是传感器噪声和优化器噪声叠加导致的数值不稳定这时候可以降低规划频率、增加时间分辨率给优化器更多迭代次数。提示如果你在Gazebo仿真里跑ego-planner强烈建议把传感器点云的降采样参数打开。默认pcl的VoxelGrid滤波如果没启用点云数量动辄几十万不仅拖慢规划速度还容易把优化器带入病态区域。4.3 坐标系与TF树问题速查坐标系问题是ego-planner仿真中绕不开的坎。下面这张表是我自己整理的最常见情况基本覆盖了大部分TF相关的报错现象可能原因排查与解决RViz地图乱转或贴地Fixed Frame和TF父级不匹配在RViz里把Fixed Frame改成world轨迹起点和无人机对不上odom消息的frame_id错误检查Gazebo插件里odometry发布器的frame_id报错“Frame [xxx] does not exist”TF树断裂缺少某个frame用tf2_echo world base_link逐级检查点云和地图错位传感器坐标系到机体坐标系的变换错误检查URDF中激光雷达/深度相机的xyz、rpy参数TF问题最有效的定位工具是rviz里自带的TF显示插件把TF树打开后你能直观看到每个坐标系之间的连接关系。断了哪一截终端对应会有红色警告。新手最容易犯的错是在URDF里给sensor坐标系直接用了一个和链接名称不一样的父坐标系名字导致TF树少了一环而这一环又恰好是ego-planner代码里写死要取的那个。4.4 仿真性能瓶颈与降载技巧ego-planner的Gazebo仿真对CPU的吃紧程度是出了名的。我自己测试下来一个中等大小带几千个障碍点的地图Gazebo加ego-planner加RViz三件套i7处理器跑起来CPU占用能到150%以上。如果把场景模型做大、点云加密直接逼近满负载。省CPU的思路有三个方向。第一降低点云分辨率VoxelGrid的leaf size从0.1改到0.2点云数量减少到原来的八分之一规划速度提升肉眼可见。第二调低Gazebo物理引擎频率把max_step_size从0.001改到0.002也就是把物理更新频率从1000Hz降到500Hz对无人机这种低速机器来说影响很小但CPU省很多。第三如果只是调规划参数不需要看整体环境物理仿真可以直接把Gazebo的GUI关掉用headless模式跑能省掉全部渲染开销CPU占用直接下降三到四成。5. 仿真向真机迁移前的最后检查清单5.1 参数一致性检查从仿真转到真机最大的坑不是算法本身而是那些在仿真里被默认值掩盖了的参数。Gazebo里的无人机模型参数——质量、惯量矩阵、电机推力系数、力矩系数——是理想化的设定真机上这些参数会和模型有偏差影响不会突然致命但会让控制效果一步步劣化。迁移前建议做的一份检查清单包括确认规划器里的最大速度、最大加速度是否与飞控限制一致确认膨胀半径要比真机的物理宽度再大个10%到20%留出安全余量确认轨迹时间分辨率与飞控控制频率匹配避免轨迹更新太慢导致控制滞后。这些如果放在仿真阶段就记录下来真机上调试会省非常多的时间。5.2 仿真中难以覆盖的极端情况我在仿真里踩过最深的一个坑是ego-planner在理想传感器模型下表现得完美无缺但切到真机后因为视觉里程计的漂移局部地图频繁抖动优化器根本收敛不动。仿真里没有视觉里程计漂移这个模型你没法通过场景设计逼出这个问题它只会在真机上突然出现。所以我的建议是把Gazebo仿真当作验证算法逻辑正确性的工具而不是验证一切可能性的替身。传感器噪声、时间同步误差、通信延迟、状态估计漂移这些Gazebo只能模拟其中一部分。做真机前最好在Gazebo里给传感器加入更大的高斯噪声、在话题链路里插入人为延迟用自己的方法把不确定性放大几倍再观察算法表现这会大大提高你第一次真机飞行的成功率。注意如果你没有条件做真机验证至少要在Gazebo里跑足够多组随机地图和随机起点终点的测试。ego-planner这类优化类规划器最好在各类极端场景下都稳定运行一遍才能放心。只跑一次成功就以为自己搞定了这和没跑过几乎没什么区别。6. 基于个人经验的三条务实建议最后分享几个我在ego-planner仿真上最希望有人早点告诉我的事情。第一条把日志当成第一调试工具。ego-planner运行时会输出很多INFO日志比如前端搜索耗时、后端优化迭代次数、最终轨迹长度。很多人遇到问题第一时间改代码然后重新编译重新跑效率很低。我通常的做法是先看日志如果优化耗时一直在波动说明环境和规划条件不稳定先解决环境问题再聊算法问题。第二条把rviz配置保存下来。调好的显示项布局、话题配置、视角位置通过RViz左上角的File→Save Config保存成rviz文件之后每次启动用rviz -d xxx.rviz加载。很多人在反复配置中浪费了大量时间这个问题完全可以通过一条启动命令解决。第三条善用rostopic和rqt_graph。运行中如果发现表现不对先rostopic list看有哪些话题、rqt_graph看节点订阅关系确认每个环节都有数据流动再推理问题出在内核的哪个阶段。这种“自底向上”的排查方式比直接在代码里加打印要高效得多尤其适合ego-planner这种多模块组合的工程。ego-planner是一个非常好的规划算法学习载体它把路径搜索、梯度优化、环境感知、状态估计融合在一个完整的仿真闭环里。你可以通过调参直观感受规划效果变化也能通过修改优化权重理解每个模块对整体表现的贡献。仿真不会教你所有答案但它给了你一个低成本试错的空间在真机之前把所有低级错误都犯一遍这就是仿真最大的意义。
