Ego-Planner与PX4自定义控制器：从轨迹到航线的桥接

简介：面向无人机自主导航与飞控研究的集成代码包，围绕自我中心路径规划算法与 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 输出的轨迹为什么不能直接喂给 PX4

ego-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 控制器从接口上拆成了三个独立模块，没有做成一个大而全的节点。这样拆的理由不是代码洁癖，而是排查效率。下面这张表是每个模块的输入输出和职责。

三个模块各自独立，好处非常直接。如果接收模块挂了，发布模块还能源源不断发悬停点，飞控保持在 offboard 模式不掉线；如果重采样模块的时间戳算歪了，你能在日志里快速定位到时序问题，而不是怀疑坐标写错；如果发布模块里坐标变换有误，直接看它输出的 setpoint 就能发现。我一般调试时先订阅发布模块的输出话题，和期望轨迹放到同一张图里对比，是坐标系问题还是频率问题一眼就能分开。

V1 版本的实现里，三个模块在同一个进程内，用回调函数串联，没有引入复杂的消息队列。这是刻意控制的复杂度，因为第一版的目标是先跑通链路，再谈性能。

2.3 轨迹丢失时的降级策略：offboard 不中断的底线

ego-planner 是局部规划器，它会因为障碍物变化、新目标到来随时切换轨迹，甚至在某段时间内完全不发布新轨迹。V1 控制器里我默认实现了一套三段降级策略，这是整个控制器在逻辑层面最重要的设计。

正常跟踪状态下，控制器按重采样结果发布 setpoint，位置、速度、加速度三个量都带上。当当前时间超过轨迹最后一点时，进入越界状态，把期望位置锁在终点，速度置零，不再向后插值。当没有有效轨迹时，进入悬停状态，持续发布悬停点，保持 offboard 不掉线。

很多 demo 代码在轨迹播完后直接停发 setpoint，PX4 两秒后自动踢回 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::vector<double>& 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-classic

gazebo-classic对应的是 Gazebo 11 这套老架构，不是新版 Gazebo（gz 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就是宿主机的 loopback，PX4 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.launch

controller_v1.launch 里 MAVROS 的fcu_url填 4.2 节记录的 SITL UDP 端口，常见配置是udp://127.0.0.1:14540@14557这一组。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 物理内存的机器很容易 OOM，Ubuntu 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 米高度

这一步可以把坐标变换、时间重采样、降级策略三个模块分开验证。我之前遇到过坐标变换和重采样同时有问题的情况，一起排查时很难定位，把变量减少到单个之后五分钟就找到了根因。验证时主要看这张表里的指标：

固定轨迹验证通过之后，再开 ego-planner。此时如果出现问题，可以确定是规划器输入的问题，而不是控制器的问题。飞行结束后在 QGC 里保存一份 ulog 日志，打开分析工具看 setpoint 与实际位置的误差曲线，不需要看多高深的指标，就看稳态误差是否在 0.3 米内，以及有没有周期性跳变。周期性跳变大概率是时间戳抖动造成的，优先查 MAVROS 的时钟源和重采样里的时间计算。

我自己的习惯是，任何改版后的控制器都强制先跑一遍固定轨迹测试，再接入 ego-planner。Rviz 里那条绿色的轨迹再好看，和真实航线之间始终隔着坐标变换、时间同步和控制响应这三道坎，只有数据会说真话。希望帮到你。

本文还有配套的精品资源，点击获取