无人机路径规划2.0：Mid360 + Fast-LIO 感知建图 + Ego-Planner 运动规划全流程整理

无人机路径规划2.0Mid360 Fast-LIO 感知建图 Ego-Planner 运动规划全流程整理摘要本文是我在开发学习过程中的一份完整实战总结覆盖 Mid360 雷达数据读取、Fast-LIO 话题验证、Ego-Planner 源码下载与编译、OpenCV 常见报错处理以及将/Odometry与/cloud_registered接入 Ego-Planner 的完整流程。文中的egoplenner统一按标准名称Ego-Planner书写方便后续检索与复现。一、项目概览本流程的目标是把Livox Mid-360、Fast-LIO和Ego-Planner串起来形成一套从感知建图到运动规划的完整链路。环境信息如下电脑平台Xavier-NX系统版本Ubuntu 18.04ROS 版本ROS Melodic激光雷达Livox Mid-360最终效果可以理解为左侧Mid360 Fast-LIO完成感知建图右侧Ego-Planner在已有环境信息基础上完成运动规划二、整体流程整套链路可以拆成 4 步先读取 Mid360 雷达数据确认imu和lidar相关话题正常发布。再跑Fast-LIO确认能够输出/Odometry和/cloud_registered。下载并编译Ego-Planner对应工程解决依赖和 OpenCV 问题。把Fast-LIO输出的话题接入Ego-Planner完成规划测试。三、读取 Mid360 数据并确认话题第一步的重点不是建图而是确认雷达驱动和基础数据链路没有问题。这一阶段的关键检查点是查看imu数据话题查看lidar数据话题可以先启动你本机对应的Livox驱动然后执行如下命令确认话题类型rostopictype/livox/imu rostopictype/livox/lidar如果话题存在且类型正常说明雷达数据已经进入 ROS 侧后面就可以继续接Fast-LIO。这里的判断标准很简单/livox/imu正常发布/livox/lidar正常发布终端无持续性报错四、运行 Fast-LIO 并验证核心输出第二步开始进入感知建图部分。这里的思路很直接先启动Fast-LIO然后确认它的关键输出话题确实存在。1. 修改 launch先不要打开 RViz因为这一阶段的重点是先验证话题输出不是直接做可视化所以建议暂时不打开rviz。launch修改示例如下launch!-- Launch file for Livox MID360 LiDAR --argnamervizdefaulttrue/rosparamcommandloadfile$(find fast_lio)/config/mid360.yaml/paramnamefeature_extract_enabletypeboolvalue0/paramnamepoint_filter_numtypeintvalue2/paramnamemax_iterationtypeintvalue3/paramnamefilter_size_surftypedoublevalue0.5/paramnamefilter_size_maptypedoublevalue0.5/paramnamecube_side_lengthtypedoublevalue1000/paramnameruntime_pos_log_enabletypeboolvalue0/nodepkgfast_liotypefastlio_mappingnamelaserMappingoutputscreen/!-- group if$(arg rviz) node launch-prefixnice pkgrviz typerviz namerviz args-d $(find fast_lio)/rviz_cfg/loam_livox.rviz / /group --/launch这里的核心意思只有两点Fast-LIO的配置文件使用mid360.yamlrviz可视化节点先不启动优先验证数据链路2. 启动 Fast-LIOroslaunch fast_lio mapping_mid360.launch3. 查看当前话题rostopic list这里重点关注的话题包括/Odometry/cloud_effected/cloud_registered/cloud_registered_body/livox/imu/livox/lidar4. 检查/Odometry与/cloud_registeredrostopicecho/Odometry rostopicecho/cloud_registered只要这两个话题能稳定输出就说明后续给Ego-Planner提供输入的基础数据已经具备了。五、下载 Ego-Planner 源码并编译这一部分进入Ego-Planner的源码下载与编译阶段。源码入口如下Github: ZJU-FAST-Lab/Fast-Drone-250也就是常见的这个工程https://github.com/ZJU-FAST-Lab/Fast-Drone-2501. 为什么建议把依赖先装全这里有一点非常重要这个工程会带上实际飞行相关的Mavros包也会带上一些视觉相关依赖如果依赖缺失catkin_make阶段很容易报错所以一个比较稳妥的方式是先进入项目说明文档把环境配置章节涉及的依赖尽量补齐再开始编译2. 编译流程gitclone https://github.com/ZJU-FAST-Lab/Fast-Drone-250cdFast-Drone-250 catkin_makesourcedevel/setup.bash结合实际配置过程环境部分的重点可以归纳为ROS Melodic基础环境Livox相关依赖realsense相关依赖mavros若干编译依赖与功能包如果你是按“缺什么补什么”的策略来配环境也完全可行。哪一步报错就针对那一步补依赖。六、OpenCV 与 cv_bridge 常见报错处理这一部分是实操里最容易卡住、也最值得单独记录的经验之一。1. 报错 1cv_bridgeConfig.cmake:113常见的典型报错如下CMake Error at /opt/ros/melodic/share/cv_bridge/cmake/cv_bridgeConfig.cmake:113这里的处理思路是统一OpenCV的查找方式。也就是把工程里类似下面这些写法find_package(OpenCV 4 REQUIRED) find_package(OpenCV 3 REQUIRED)统一替换为find_package(OpenCV REQUIRED)2. 报错 2OpenCV 安装路径不在标准位置排查命令如下locateopencv2/core/core.hpp返回路径可能类似于/home/nvidia/opencv/modules/core/include/opencv2/core/core.hpp /usr/include/opencv4/opencv2/core/core.hpp /usr/local/opencv346/include/opencv2/core/core.hpp如果你的 OpenCV 确实装在非标准路径例如/usr/local/opencv346/include那么可以把它加入环境变量exportCPATH$CPATH:/usr/local/opencv346/include也可以直接写入~/.bashrcexportCPATH$CPATH:/usr/local/opencv346/include然后重新加载source~/.bashrc处理完上述问题后再重新执行catkin_make只要编译顺利通过就说明Ego-Planner的工程环境已经基本跑通。七、只启动 Ego-Planner 做仿真如果你暂时只想验证Ego-Planner本身能否启动而不急着对接真实雷达数据那么可以先直接跑仿真。对应启动命令如下sourcedevel/setup.bash roslaunch ego_planner single_run_in_sim.launch这一步的意义是先确认Ego-Planner自身没有安装问题先把基础仿真跑通后续再切到真实话题输入八、把 Fast-LIO 输出接入 Ego-Planner当Mid360 Fast-LIO已经能稳定输出/Odometry和/cloud_registered后就可以把这两个话题接到Ego-Planner。这里的关键操作是修改single_run_in_exp.launch把odom_topic和cloud_topic改成Fast-LIO的输出话题。1. 关键 launch 配置launch!-- number of moving objects --argnameobj_numvalue10/argnamedrone_idvalue0/argnamemap_size_xvalue100/argnamemap_size_yvalue50/argnamemap_size_zvalue3.0/argnameodom_topicvalue/Odometry/!-- main algorithm params --includefile$(find ego_planner)/launch/advanced_param_exp.xmlargnamedrone_idvalue$(arg drone_id)/argnamemap_size_xvalue$(arg map_size_x)/argnamemap_size_yvalue$(arg map_size_y)/argnamemap_size_zvalue$(arg map_size_z)/argnameodometry_topicvalue$(arg odom_topic)/argnameobj_num_setvalue$(arg obj_num)/!-- dont set cloud_topic if you already set these ones --argnamecamera_pose_topicvaluemouse1/argnamedepth_topicvalue/camera/depth/image_rect_raw/!-- lio point cloud measurement, such as from LIDAR --argnamecloud_topicvalue/cloud_registered/!-- intrinsic paras of the depth camera --argnamecxvalue323.3316345214844/argnamecyvalue234.95498657226562/argnamefxvalue384.39654541015625/argnamefyvalue384.39654541015625/!-- maximum velocity and acceleration the drone will reach --argnamemax_velvalue0.5/argnamemax_accvalue6.0/!-- always set 1.5 times greater than sensing horizon --argnameplanning_horizonvalue6/argnameuse_distinctive_trajsvaluefalse/!-- 1: use 2D Nav Goal to select goal --!-- 2: use global waypoints below --argnameflight_typevalue1/!-- global waypoints --argnamepoint_numvalue1/argnamepoint0_xvalue15/argnamepoint0_yvalue0/argnamepoint0_zvalue1//include!-- trajectory server --nodepkgego_plannernamedrone_$(arg drone_id)_servertypetraj_serveroutputscreenremapfromplanning/bsplinetodrone_$(arg drone_id)_planning/bspline/paramnametraj_server/time_forwardvalue1.0typedouble//node/launch这段配置里真正最关键的只有两个地方argnameodom_topicvalue/Odometry/argnamecloud_topicvalue/cloud_registered/只要这两个话题名与你当前Fast-LIO输出一致Ego-Planner就能直接消费前端感知结果。2. 启动命令roslaunch ego_planner single_run_in_exp.launch roslaunch ego_planner rviz.launch到这里整条链路就完成了Livox Mid-360负责提供原始点云与 IMUFast-LIO负责状态估计与点云配准Ego-Planner负责轨迹生成与避障规划九、复现时最值得注意的 5 个点为了避免后面重复踩坑这里把最关键的经验再浓缩一下不要一上来就盯着 RViz先确认 ROS 话题是否正常输出。Fast-LIO阶段最关键的是/Odometry和/cloud_registered。Ego-Planner编译前尽量先补齐mavros、视觉、传感器相关依赖。OpenCV 版本问题非常常见优先统一find_package(OpenCV REQUIRED)。真正把Fast-LIO对接到Ego-Planner时本质上就是改两个话题名。十、参考资料后续继续深入时我建议优先参考以下几个方向基于Fast-LIO2跑Ego-Planner的经验整理Livox Mid360 Fast-LIO Ego-Planner的实战部署记录基于Livox Mid40跑FAST-LIO2的调试笔记ZJU-FAST-Lab/Fast-Drone-250官方工程如果你准备长期做这条链路建议优先把Fast-Drone-250仓库说明、Fast-LIO配置文件、以及你当前雷达驱动版本一并对照检查。十一、总结把这套流程压缩成一句话其实就是先让Mid360正常出数再让Fast-LIO稳定输出/Odometry和/cloud_registered最后把这两个话题接进Ego-Planner。其中最容易卡住的地方并不是算法本身而是依赖没有装全OpenCV版本或路径不一致话题名没有和Fast-LIO对齐只要把这三个问题处理干净Mid360 Fast-LIO Ego-Planner这一套链路基本就能跑起来。如果你后面准备继续往实机飞行走可以在这篇文章的基础上继续补两部分内容机体系与传感器外参标定mavros与飞控侧的控制链路对接