简介面向无人机自主导航与飞控研究的集成代码包围绕自我中心路径规划算法与 PX4 飞控系统的自定义控制器实现适用于多旋翼自主飞行、避障与任务规划方向的开发者。压缩包共 74 个文件其中 26 个 msg 消息文件用于定义机身与规划器之间的通信协议12 个 py 脚本负责规划逻辑与辅助工具5 个 yaml 配置文件设置控制参数另有 C 源码和头文件实现核心控制器XML 文件则用于工程描述整体体积仅 55KB结构紧凑便于快速阅读。该版本已吸引 739 人学习下载。通过源码可清晰理解路径规划与飞控之间的消息传递及控制适配掌握自定义控制器在故障检测、动态避障和状态估计方面的处理思路quadrotor_msgs 消息库与 catkin 工作区为二次开发提供了便利接口适合作为算法验证与功能扩展的起点。1. ego-planner 与 PX4 自定义控制器 V1规划轨迹和实际航线之间的那层“翻译官”第一次把 ego-planner 跑起来的时候Rviz 里那条光滑的绿色轨迹让我以为大功告成。结果切到 offboard 模式无人机先是原地悬停三秒然后朝一个完全错误的方向加速。问题既不在规划器也不在飞控而是两个模块之间缺一个能同时听懂双方协议的自定义控制器。V1 版本干的就是这件事接收 ego-planner 输出的 B 样条轨迹完成坐标变换、时间重采样和安全降级再以 PX4 认识的 setpoint 消息喂给飞控。这份资源适合正在做无人机自主导航、PX4 仿真验证或者课题实验的工程师。它解决的是“规划器输出和飞控期望之间的协议差”包括坐标系对齐、消息频率匹配、轨迹可用性判断这三件事。如果你已经能把 ego-planner 跑起来、能从 QGroundControl 看到 PX4 姿态数据那这篇笔记你正好接得上。2. PX4 自定义控制器 V1 在飞控链路里的位置先看懂两边的接口再写代码整条链路从上层到底层是 ego-planner → V1 控制器 → MAVROS → PX4 → 电机。很多人以为 ego-planner 算出了轨迹PX4 就应该照着飞实际上 PX4 根本不认识 ego-planner 的轨迹消息。V1 控制器存在的意义就是把规划器的输出翻译成 PX4 offboard 模式能消费的设定点。在做任何编码之前先把两边的接口差异搞清楚否则后面每一步调试都是猜。2.1 ego-planner 输出的轨迹为什么不能直接喂给 PX4ego-planner 输出的是一串离散轨迹点每个点包含位置、速度、加速度和时间信息看起来正好就是 PX4 想要的 setpoint。但我第一次直接接线之后发现问题远没有这么简单双方至少有四个地方对不上。第一是坐标系。ego-planner 在 ROS 的 world 系下工作常用配置是 ENU东-北-天而 PX4 的位置控制链路期望的是 NED北-东-地。不转换的话第一帧轨迹点就会让飞机往错误方向加速也就是开篇那个翻车现象。第二是时间语义。ego-planner 一次规划出一串点规划完成之后如果目标没变就不再发新轨迹了但 PX4 的 offboard 模式要求设定点持续更新低于 2Hz 就会超时退出。也就是说中间必须有人持续“搬运”数据。第三是频率。规划器的轨迹采样间隔由内部参数决定可能是 5Hz 的稀疏点也可能是 100Hz 的密集点而 PX4 内部的控制器是按固定周期跑的。第四是轨迹终点的含义。ego-planner 是局部规划器它的轨迹终点只是当前视野内的目标不是全局任务的终点直接播完就停发会让飞控失去指令来源。这四个差异叠加在一起决定了你不能把 ego-planner 的话题直接接到 MAVROS 上。V1 控制器本质上就是为这四个差异做适配的中间层。从放弃到精通之间隔着的往往不是规划算法本身而是这些看起来不起眼的接口细节。2.2 V1 控制器拆成三块接收、重采样、发布这份工程把 V1 控制器从接口上拆成了三个独立模块没有做成一个大而全的节点。这样拆的理由不是代码洁癖而是排查效率。下面这张表是每个模块的输入输出和职责。模块输入核心处理输出轨迹接收ego-planner 轨迹话题校验坐标原点、时间戳、有效标志缓存最近一条有效轨迹时间重采样缓存轨迹 系统时钟按 50Hz 在相邻轨迹点间插值当前时刻的位置、速度、加速度指令发布重采样结果ENU 转 NED、偏航角处理、类型转换MAVROS setpoint 话题三个模块各自独立好处非常直接。如果接收模块挂了发布模块还能源源不断发悬停点飞控保持在 offboard 模式不掉线如果重采样模块的时间戳算歪了你能在日志里快速定位到时序问题而不是怀疑坐标写错如果发布模块里坐标变换有误直接看它输出的 setpoint 就能发现。我一般调试时先订阅发布模块的输出话题和期望轨迹放到同一张图里对比是坐标系问题还是频率问题一眼就能分开。V1 版本的实现里三个模块在同一个进程内用回调函数串联没有引入复杂的消息队列。这是刻意控制的复杂度因为第一版的目标是先跑通链路再谈性能。2.3 轨迹丢失时的降级策略offboard 不中断的底线ego-planner 是局部规划器它会因为障碍物变化、新目标到来随时切换轨迹甚至在某段时间内完全不发布新轨迹。V1 控制器里我默认实现了一套三段降级策略这是整个控制器在逻辑层面最重要的设计。正常跟踪状态下控制器按重采样结果发布 setpoint位置、速度、加速度三个量都带上。当当前时间超过轨迹最后一点时进入越界状态把期望位置锁在终点速度置零不再向后插值。当没有有效轨迹时进入悬停状态持续发布悬停点保持 offboard 不掉线。很多 demo 代码在轨迹播完后直接停发 setpointPX4 两秒后自动踢回 position 模式。如果当时飞机正处于低空避障阶段模式切换瞬间的响应变化是很危险的。V1 控制器把“没有新轨迹”当成一种正常状态而不是异常状态来处理这是它跟很多一次性演示代码最本质的区别。3. V1 控制器落地实现消息定义、坐标变换与重采样代码接口约定清楚了接下来就是具体的实现。这一章我把 V1 控制器里三个最核心的代码片段拆开讲消息格式怎么定、坐标怎么转、重采样怎么写。这三段代码是控制器的主干其余都是围绕它们的外围逻辑。3.1 消息约定用相对时间戳绕开双节点时钟同步先定接口。V1 控制器订阅 ego-planner 的轨迹话题但只取坐标是不够的必须带上时间信息。我自己定的消息格式如下也是这份工程里的默认约定# 控制器订阅的轨迹消息在 V1 控制器里按此约定解析 Header header geometry_msgs/Point[] positions geometry_msgs/Vector3[] velocities geometry_msgs/Vector3[] accelerations duration[] time_from_start uint8 valid这里最关键的字段是time_from_start它表示这一帧轨迹点相对第一个点的时间偏移单位是秒。这个设计是故意的规划器节点和控制器节点如果跑在不同的容器或者进程里绝对时间戳很容易受时钟漂移影响而相对时间完全不依赖两个节点的时钟同步。控制器收到轨迹后用自己当前时间减去接收时刻就能得到相对时间偏移。valid字段也容易被忽略。ego-planner 在收到新目标后可能会先发布一帧过渡轨迹用于衔接当前状态这一帧不能直接用于跟踪否则飞机会在目标切换瞬间猛拉一下。V1 控制器里对 valid 为 0 的帧直接丢弃只用 valid 为 1 的轨迹做重采样。3.2 ENU 到 NED位置、偏航角一个都不能漏坐标变换是 V1 控制器里最容易翻车的地方。Eigen 实现的核心转换只有三行// 位置坐标变换ENU - NED Eigen::Vector3d EnuToNed(const Eigen::Vector3d enu) { Eigen::Vector3d ned; ned.x() enu.y(); // NED north 对应 ENU y 轴 ned.y() enu.x(); // NED east 对应 ENU x 轴 ned.z() -enu.z(); // NED down 对应 ENU z 轴反向 return ned; }这段代码看着简单但我第一次只交换了 x 和 y漏掉 z 的反向飞机在 Gazebo 里直接往地里扎反复了三次才反应过来。z 轴负号是这地方最容易漏的一项没有之一。除了坐标轴方向还有原点偏移。ego-planner 的轨迹起点不一定在飞控当前位置。V1 控制器在收到有效轨迹的第一帧时把轨迹起点与当前飞控位置的差值记录下来作为原点偏移之后每一帧先加偏移再转 NED。如果第一帧没有对齐后面整条路径都会偏一个固定向量而且从 Rviz 里看轨迹是正常的只有对比真实航线才会发现。偏航角同样要处理PX4 的 yaw 在 NED 系中顺时针为正ego-planner 输出的偏航角在 ENU 系中逆时针为正所以// 偏航角ENU 逆时针为正 - NED 顺时针为正 double yaw_ned -yaw_enu; yaw_ned std::atan2(std::sin(yaw_ned), std::cos(yaw_ned));最后一行atan2(sin, cos)的作用是把角度归一化到 [-pi, pi) 区间。不做这一步当角度跨过正负 180 度边界时飞控会误判期望姿态变化了 360 度执行器反向转一大圈才修正回来。3.3 时间重采样50Hz setpoint 怎么从稀疏轨迹里取出来重采样是 V1 控制器里最核心的算法逻辑。ego-planner 的轨迹点可能是 5Hz 的稀疏序列但 PX4 的 offboard 设定点最好以 50Hz 稳定输入。实现上就是在相邻轨迹点之间按时间线性插值// 在稀疏轨迹上按时间插值t 为相对轨迹起点的当前时刻 bool SampleAtTime(const TrajectoryPoint* pts, size_t n, const std::vectordouble ts, double t, TrajectoryPoint* out) { if (t ts[0]) { *out pts[0]; return true; } size_t i 0; while (i 1 n t ts[i 1]) i; if (i 1 n) return false; // 越界交给降级逻辑处理 double alpha (t - ts[i]) / (ts[i 1] - ts[i]); alpha std::clamp(alpha, 0.0, 1.0); // 抗时间抖动 out-position pts[i].position alpha * (pts[i 1].position - pts[i].position); out-velocity pts[i].velocity alpha * (pts[i 1].velocity - pts[i].velocity); out-acceleration pts[i].acceleration alpha * (pts[i 1].acceleration - pts[i].acceleration); return true; }逻辑说明ts是轨迹点的时间戳序列t是控制器当前时刻相对于轨迹接收时刻的偏移。函数先定位当前时刻落在哪两个轨迹点之间然后用alpha做线性插值。返回false表示已经越出轨迹末端此时调用方必须切换到降级策略不能把这个状态当成正常跟踪。参数说明里值得注意的一点是alpha的 clamp。如果时间源有抖动alpha可能略超出 [0,1] 区间直接 clamp 到边界可以避免轨迹出现不自然的跳变。线性插值在这个场景下够不够用取决于 ego-planner 的输出密度。它内部对 B 样条的采样通常很密间隔在 0.05 到 0.1 秒之间线性插值的误差可以忽略。如果你在别的数据源上碰到间隔超过 0.2 秒的稀疏轨迹线性插值会明显削掉拐角那时候就要改成三次样条插值。4. Ubuntu 24.04 下把 PX4 仿真跑起来依赖、SITL 与 ego-planner 联动编译环境和跑通链路是两回事。这一章我把 Ubuntu 24.04 搭建 PX4 仿真环境的完整过程写出来包括依赖安装、SITL 编译以及怎么让 Ubuntu 24.04 和 ROS1 生态的 ego-planner 共存。4.1 PX4 依赖安装Ubuntu 24.04 与旧教程最大的不同在 Ubuntu 24.04 上最容易踩的坑是拿 Ubuntu 22 时代的教程直接抄gcc、Python 版本都变了很多包名也不一样。我实测下来下面这套依赖可以完整支撑 PX4 SITL 加 Gazebo# Ubuntu 24.04 上实测可用的 PX4 依赖 sudo apt update sudo apt install -y \ build-essential git cmake ninja-build \ python3-pip python3-empy python3-toml python3-yaml \ python3-jinja2 python3-packaging python3-kconfiglib \ libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev \ libjsoncpp-dev libtinyxml2-dev protobuf-compiler \ libeigen3-dev libopencv-dev \ gazebo libgazebo11-dev参数说明里有一条经验python3-empy一定装系统包不要用 pip 装。Ubuntu 24.04 的 Python 3.12 环境里pip 安装 empy 的版本兼容性很看运气系统包是经过完整测试的。另外python3-kconfiglib在 24.04 的源里已经有了不需要再单独去 pip install。这里有一个需要提前说明的架构决策Ubuntu 24.04 的官方 apt 源里没有 ROS1 Noetic而 ego-planner 的主流实现依赖 ROS1。我一般不会在宿主机上硬刚 rosdep 依赖而是用 Docker 起一个 Noetic 容器跑规划器和 V1 控制器宿主机只跑 PX4 SITL 和 Gazebo。两边通过 UDP 端口打通这个结构放在 4.3 里讲。4.2 PX4 SITL 编译内存才是最大的坑依赖装齐之后编译 PX4 本身并不复杂cd ~/PX4-Autopilot git submodule update --init --recursive make px4_sitl gazebo-classicgazebo-classic对应的是 Gazebo 11 这套老架构不是新版 Gazebogz sim。以 V1 控制器目前的对接方式gazebo-classic 最稳PX4 对两套仿真器的支持成熟度也不一样。编译过程中最大的坑是内存。gcc-13 并行编译时峰值内存比 Ubuntu 20.04 时代高不少8GB 内存的机器很容易在编译中段被 OOM killer 干掉。终端里看到cc1plus: fatal error: Killed signal terminated program cc1plus这种输出多半不是代码问题是内存不够。解决方法是把 swap 扩到至少 16GB或者编译时限制并行度make px4_sitl gazebo-classic -j4提示不要一上来就-j$(nproc)笔记本散热和内存都扛不住。编译完成后终端会打印 SITL 的 UDP 端口信息拿笔记一下。MAVROS 接 SITL 时需要这个端口号。启动仿真是在另一个终端再次执行make px4_sitl gazebo-classic。首次运行会自动拉起 Gazebo之后如果 Gazebo 被手动关掉再次执行这条命令会重新连接 SITL。等 QGroundControl 自动发现飞机的14550端口、看到陀螺仪和气压计数据都正常之后再继续往下走。4.3 ego-planner 与 V1 控制器的编译和启动宿主机跑 PX4 SITL容器里跑 ego-planner、V1 控制器和 MAVROS。这个结构能绕开 Ubuntu 24.04 没有 ROS1 官方源的问题。容器使用 host 网络模式直接访问宿主机的 UDP 端口# 进入工程根目录构建 Noetic 镜像 docker build -t ego_noetic:local -f Dockerfile.ego . # 启动容器--network host 是关键参数 docker run --rm -it --network host \ -v ~/ego_ws:/workspace \ ego_noetic:local bash--network host让容器内的127.0.0.1就是宿主机的 loopbackPX4 SITL 监听的端口在容器里直接可访问。如果你是在 Windows 或 macOS 上用虚拟机跑 Ubuntu记得把虚拟网络调成桥接模式否则容器里连不上宿主机的 UDP 端口。容器内编译和启动cd /workspace catkin_make source devel/setup.bash # 终端 A启动 V1 控制器与 MAVROS roslaunch px4_controller_v1 controller_v1.launch # 终端 B启动 ego-planner 规划器 roslaunch ego_planner run_in_sim.launchcontroller_v1.launch 里 MAVROS 的fcu_url填 4.2 节记录的 SITL UDP 端口常见配置是udp://127.0.0.1:1454014557这一组。ego-planner 的 launch 文件里建议把轨迹发布频率限制在 10 到 20Hz不要让它全速刷。V1 控制器的仿真标志位在 launch 里设成true此时控制器走/mavros/setpoint_position/local这套消息接口真机版本再切换到 UORB 直连。这个结构的优点是两个环境互不污染V1 控制器迭代只改容器内代码。第一次 docker build 会比较久但比在 Ubuntu 24.04 上硬刚 rosdep 靠谱得多。5. V1 控制器集成避坑五个高频毛病的现象、原因与处理这一章列五条实际踩过的高频问题每条按“现象 → 原因 → 解决”来写方便你直接对照排查。5.1 切 offboard 后飞机朝随机方向加速现象Rviz 里轨迹完全没问题一切 offboard飞机不是朝着轨迹终点飞而是往某个固定方向猛加速。原因坐标变换缺了 z 轴反向或者 ego-planner 的轨迹是局部坐标系发布的而这个局部系的原点没有和飞控当前位置对齐。解决把 V1 控制器打印的前 20 帧 NED 期望和飞控当前位置放到同一张图里对比。如果 x/y 对调、z 同号就是EnuToNed写错了如果整体差一个固定向量就是原点偏移没记录。我现在每次启动后都会人工扫一眼前 20 帧 setpoint 再切 offboard这个习惯救过我至少三次。5.2 Rviz 里的轨迹和实际航线偏一个固定身位现象飞机飞起来了方向正确但整条航线在空间上偏一个固定距离而且偏移量保持不变。原因轨迹点是规划器地图系下的坐标ego-planner 在地图原点变化后V1 控制器还用旧的原点偏移。解决每次收到 ego-planner 的 new_start 信号后重新计算原点偏移而不是只在控制器启动时算一次。V1 控制器里我把原点偏移的更新逻辑放在了轨迹 valid 标志位复位时这样地图重定位不会带到旧偏移。5.3 Ubuntu 24.04 编译 PX4 报成堆 warning 并被中断现象make 跑一阵后冒出大量 warning随后编译中止关键行是cc1plus: fatal error: Killed。原因编译内存不够。gcc-13 并行编译的峰值内存比老版本高8GB 物理内存的机器很容易 OOMUbuntu 24.04 默认 swap 也偏小。解决把 swap 扩到 16GB 再重编或者直接限制并行度make px4_sitl gazebo-classic -j4。不要一上来就用全部核心并行编译。5.4 offboard 进入两秒后被踢回现象offboard 模式能切进去但一两秒后飞控自动退出QGC 里能看到模式变化记录。原因PX4 对 offboard 模式有硬性要求设定点必须持续以 2Hz 以上频率到达。V1 控制器在 ego-planner 没有轨迹输出时也在“等数据”一个 setpoint 都没发。解决把降级策略做成常态。没有有效轨迹就持续发悬停点重采样越界返回 false 后发布模块立刻切到位置保持而不是静默。这是 2.3 节强调过的 offboard 不掉线底线V1 控制器里这个逻辑是默认开启的。5.5 轨迹终点在前方原地抖动飞机画圈现象飞机飞到接近目标点后开始小幅度绕圈轨迹显示终点不断在当前位置附近跳动。原因ego-planner 在某类局部规划参数下会在目标点附近反复重规划每次新轨迹起点都从当前位置开始造成“终点跟着飞机走”的假象。解决在 V1 控制器里加距离判断当前位置与轨迹终点距离小于阈值我常用 0.5 米时直接切换为纯位置保持不再跟踪新到达的轨迹。这不算绕过规划器而是让局部规划器知道任务已经收敛。6. 验证 V1 控制器的土办法先用固定轨迹检验跟踪再让 ego-planner 介入控制器集成完之后第一件事不是开 ego-planner而是先喂一条固定轨迹。我一般手工生成一条 Lissajous 曲线也就是 x-y 平面上的八字直接停掉 ego-planner 的轨迹话题让 V1 控制器按这条固定轨迹跑# 生成用于验证 V1 控制器的固定轨迹 import numpy as np t np.linspace(0, 20, 400) traj_x 2.0 * np.sin(0.5 * t) traj_y 2.0 * np.sin(0.25 * t) traj_z np.full_like(t, 1.2) # 保持 1.2 米高度这一步可以把坐标变换、时间重采样、降级策略三个模块分开验证。我之前遇到过坐标变换和重采样同时有问题的情况一起排查时很难定位把变量减少到单个之后五分钟就找到了根因。验证时主要看这张表里的指标验证项观察指标期望结果失败特征坐标变换setpoint 与实际位置横向偏差小于 0.3 米存在恒定方向偏移时间对齐setpoint 话题发布频率50Hz 附近且连续频率跳变或断流降级策略轨迹播完后的指令行为位置保持指令突然消失偏航跟踪机头指向与轨迹方向偏差小于 5 度反向或者绕圈固定轨迹验证通过之后再开 ego-planner。此时如果出现问题可以确定是规划器输入的问题而不是控制器的问题。飞行结束后在 QGC 里保存一份 ulog 日志打开分析工具看 setpoint 与实际位置的误差曲线不需要看多高深的指标就看稳态误差是否在 0.3 米内以及有没有周期性跳变。周期性跳变大概率是时间戳抖动造成的优先查 MAVROS 的时钟源和重采样里的时间计算。我自己的习惯是任何改版后的控制器都强制先跑一遍固定轨迹测试再接入 ego-planner。Rviz 里那条绿色的轨迹再好看和真实航线之间始终隔着坐标变换、时间同步和控制响应这三道坎只有数据会说真话。希望帮到你。本文还有配套的精品资源点击获取
