简介：面向无人机自主导航与控制领域的开发者，这份压缩包提供了一个结合ego-planner路径规划算法与PX4飞行控制系统的自定义控制器V1版本实现，适合用于研究自主避障、动态路径规划及飞控二次开发，也可作为相关课程设计的完整参考。包内共74个文件，以Python脚本、C++源程序、ROS消息定义（msg）、YAML参数配置及XML描述等为主，整体仅55KB，是紧凑的源码级资料，目录结构包含ROS工作空间与四旋翼专用消息定义，便于在仿真或实机上编译使用。目前已有739人学习，具备一定参考热度。通过阅读源码，可系统理解ego-planner从状态估计、环境建模、目标设定到路径决策的完整流程，以及PX4模块化架构和MAVLink通信机制；同时还能看到V1版本如何针对ego-planner需求调整PX4默认控制器，优化性能并增强故障检测与复杂环境适应性，便于快速迁移至实际无人机平台。

1. ego-planner+PX4自定义控制器V1版本：这套组合到底在干什么

做过无人机自主导航的人应该都遇到过这个场景：ego-planner在仿真里规划出一条行云流水的轨迹，但落到PX4真机上，飞机要么追不上、要么来回震荡，甚至直接触发失控保护。问题往往出在PX4默认的控制器处理不了ego-planner这种高频更新的、带速度和加速度前馈的轨迹输入。ego-planner+PX4自定义控制器V1版本，就是为这个衔接问题提供一个可落地的答案：把ego-planner输出的多项式轨迹转换成PX4能消化的指令，并写一套自己的外环控制器来接住这条轨迹。本方案适合已经跑通ego-planner规划、想在PX4上实现完整闭环的开发者，也适合正在做自主无人机毕设或工程项目的同学——它解决的不是规划算法本身，而是“规划完之后飞机怎么跟得上”这最后一公里。

2. 先定数据链路：ego-planner轨迹怎么进PX4，选OFFBOARD还是uORB

2.1 接口选型的核心矛盾：PX4默认控制器为什么接不住ego-planner轨迹

PX4默认的位置控制器是为遥控器或航点飞行设计的，它内部有位置环、速度环、姿态环三级串联，期望输入通常是期望位置。而ego-planner输出的是一段带时间轴的多项式轨迹，在每个规划周期内会给出期望位置、速度、加速度，甚至加加速度。如果只把位置塞给PX4，速度环和加速度前馈完全用不上，控制器只能在后面“追位置”，轨迹稍微曲折一点就会甩出误差。更麻烦的是，ego-planner在避障时会频繁重新规划，期望位置会跳变，PX4默认控制器对这种跳变很敏感，轻则轨迹变形，重则姿态振荡。

常见做法是把ego-planner的输出降级成PX4能接收的“位置+速度+加速度”组合指令，再写一个外环控制器做轨迹跟踪。这里有个分工：PX4的内环（姿态环）继续用它自己的姿态控制器，外环换成我们自定义的控制器，这样既保留了PX4内环成熟的控制效果，又能让轨迹跟踪特性符合ego-planner的输入风格。V1版本的核心思路就是只替换外环，不去动内环，改动范围小，出问题也好回退。

2.2 数据链路选型：MAVROS的setpoint_raw还是PX4内部uORB消息

确定分工之后，下一个问题就是数据怎么传。做ego-planner适配PX4有两类常见方案，一种是通过MAVROS把规划结果发给PX4，走加外部的“离板控制器”；另一种是直接在PX4的Pixhawk上跑自定义控制模块，通过uORB消息订阅规划轨迹。两者的区别很实际：MAVROS方案在Linux主机上做，调试方便、ego-planner部署容易，但链路延时会受串口或WiFi质量影响；uORB方案在飞控里做，实时性好，但编译部署麻烦，ego-planner本身也要跨平台移植。

V1版本我建议先用MAVROS方案跑通链路，核心原因是ego-planner的依赖普遍集中在ROS环境，飞控端只负责接收指令。MAVROS需要把ego-planner输出的轨迹点转换成目标格式发出去，常用的是SETPOINT_POSITION_VELOCITY_ACCEL，也就是setpoint_raw的一种。这个命令同时携带位置、速度、加速度和姿态角，PX4拿到之后会进入位置模式的外环控制。发送频率建议和ego-planner的重规划频率对齐，一般是10Hz到50Hz，具体看你的规划器参数。发送是可以丢包的，但注意PX4端的OFFBOARD模式有超时保护：收不到指令会退出离板模式，这个后面踩坑里专门说。

如果用uORB方案，V1版本可以订阅vehicle_local_position_setpoint消息，但PV消息里期望的坐标系是NED（北东地），ego-planner的输出往往是ENU或机体坐标，需要先做坐标变换。这块我建议不要一上来就做，V1先在MAVROS链路里把坐标变换做扎实，V2再往机载端迁移。

2.3 OFFBOARD模式与位置控制模式的选择

PX4里接收外部控制的模式是OFFBOARD，在QGroundControl里也叫“外环访问模式”。进入OFFBOARD之前，要确保飞控有人在开关切到外环模式，并且遥控器的急停通道保持有效。V1版本建议的模式组合是：姿态内环用PX4默认的“姿态控制”，外环由我们的自定义控制器发出“期望姿态”或“期望角速率”指令。如果你发的是带姿势的setpoint，PX4会跳过它的外环位置控制，直接用你给的姿态作为内环输入；如果你发的是位置+速度+加速度，PX4还是用自己的外环。这里的细节很重要：V1版本自定义控制器的输出到底是往哪里打，决定了整个代码结构。

我推荐的做法是：ego-planner的输出先经过我们自定义的外环控制器，产生期望加速度，再把加速度投影成期望姿态（滚转和俯仰），偏航角单独走一个yaw控制器，最后通过setpoint_raw发给PX4。这样PX4完全没有参与轨迹跟踪，只做姿态稳定和油门补偿。优点是ego-planner的轨迹特性完全由我们的控制器决定，缺点是控制器写的不好时很难甩锅给PX4。V1就这么干，先把事情简化。

3. V1控制器核心：位置环、速度环与姿态映射的实现代码

3.1 控制律设计：从加速度到期望姿态的数学推导

自定义控制器的本质是一个“外环轨迹跟踪器”。它的输入是ego-planner给出的期望位置、速度、加速度，以及当前飞机的速度、姿态，输出是期望的滚转角、俯仰角和油门。V1版本采用“位置环 + 速度环 + 加速度前馈”的级联结构，公式如下：

首先算位置误差和速度误差：

e_p = p_des - p_cur e_v = v_des - v_cur

然后期望加速度：

a_des = K_p * e_p + K_v * e_v + a_feedforward

其中a_feedforward直接取ego-planner轨迹里的期望加速度。接下来把期望加速度映射到姿态角。忽略空气阻力的小角度近似下，水平方向加速度由姿态倾角产生：

roll_des = (a_des_y * sin(yaw_ref) - a_des_x * cos(yaw_ref)) / g pitch_des = (a_des_x * sin(yaw_ref) + a_des_y * cos(yaw_ref)) / g

注意这里用的是NED坐标系，a_des_x是北向加速度，a_des_y是东向加速度。滚转和俯仰的符号要特别注意，PX4的NED系下正滚转是右倾，正俯仰是机头下沉，ego-planner内部常用ENU，坐标变换错了整架飞机会往反方向冲。V1建议在代码里用一个单独的函数处理坐标系转换，并且让所有中间量都用结构体打包，几个小时后调试方便很多。

3.2 核心代码：V1控制器的状态机与主循环

下面是我在V1版本里写的外环控制器核心代码，跑在ROS节点里，订阅ego-planner的轨迹点，发布期望姿态给MAVROS。这个节点可以完全脱离PX4跑在仿真里，便于逐步调试。

// trajectory_tracker_v1.cpp // 订阅：/trajectory_planner/traj_point（ego-planner输出） // 发布：/mavros/setpoint_raw/attitude（期望姿态） // 坐标系：输入为ENU，输出转NED #include <ros/ros.h> #include <mavros_msgs/AttitudeTarget.h> #include <mavros_msgs/PositionTarget.h> #include <nav_msgs/Odometry.h> #include <Eigen/Dense> // 控制器参数，用dynamic_reconfigure或roslaunch传参 struct CtrlParams { double K_p_pos = 2.0; // 位置增益 double K_v_vel = 0.8; // 速度增益 double K_p_att = 2.5; // 姿态内环增益（传给PX4的比例） double max_tilt = 0.4; // 最大倾斜角，单位rad，约23度 double max_acc = 3.0; // 最大期望加速度，单位m/s^2 double yaw_gain = 1.2; // 偏航角增益 } params;

这个控制器节点的结构是：主循环以50Hz频率运行，读取最新的期望轨迹点、当前飞机状态，计算误差，再生成姿态指令。状态控制的核心是提供一条从ego-planner到PX4的独立通道：如果ego-planner在规划点之间插值，我们收到的是一个随时间变化的连续序列；如果ego-planner是一段一段发来，V1控制器就要维护一个轨迹点缓存，在两个点之间做线性插值，避免速度突变。

// 主控制循环，50Hz void controllerLoop() { ros::Rate rate(50); while (ros::ok()) { // 1. 获取当前状态（来自PX4的odometry） nav_msgs::Odometry odom; if (!getCurrentOdom(odom)) { // 拿不到状态就跳过 rate.sleep(); continue; } // 2. 计算期望状态（从轨迹点缓存里插值得到） Eigen::Vector3d pos_des, vel_des, acc_des; bool valid = getReferenceAtTime(pos_des, vel_des, acc_des); if (!valid) { rate.sleep(); continue; } // 轨迹结束或尚未开始 // 3. 计算位置速度误差，得到期望加速度（含前馈） Eigen::Vector3d pos_cur(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z); Eigen::Vector3d vel_cur(odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.linear.z); Eigen::Vector3d e_p = pos_des - pos_cur; Eigen::Vector3d e_v = vel_des - vel_cur; Eigen::Vector3d a_des = params.K_p_pos * e_p + params.K_v_vel * e_v + acc_des; // 4. 限幅 if (a_des.norm() > params.max_acc) { a_des = a_des.normalized() * params.max_acc; } // 5. ENU -> NED // 注意ENU的Z轴朝上，NED的Z轴朝下，这里做坐标翻转 double a_x_ned = a_des.y(); // ENU的X向东，NED的X向北 double a_y_ned = a_des.x(); // ENU的Y向北，NED的Y向东 double a_z_ned = -a_des.z(); // 高度轴反向 // 6. 加速度转期望姿态（小角度近似） double yaw_ref = currentYaw(); double roll_des, pitch_des; roll_des = (a_y_ned * sin(yaw_ref) - a_x_ned * cos(yaw_ref)) / 9.81; pitch_des = (a_x_ned * sin(yaw_ref) + a_y_ned * cos(yaw_ref)) / 9.81; // 限幅到最大倾斜角 roll_des = clamp(roll_des, -params.max_tilt, params.max_tilt); pitch_des = clamp(pitch_des, -params.max_tilt, params.max_tilt); // 7. 发布ATTITUDE_TARGET mavros_msgs::AttitudeTarget att; att.header.stamp = ros::Time::now(); att.type_mask = 0b11; // 忽略角速率，只用人目标姿态 att.orientation = quaternionFromEuler(roll_des, pitch_des, yaw_ref); att.thrust = computeThrust(a_z_ned); // 需要单独做油门补偿 att_pub.publish(att); rate.sleep(); } }

代码中的关键参数需要做说明。K_p_pos = 2.0表示无人机偏离期望位置1米时会产生2 m/s²的加速度指令，对应大约0.2g，这在室内飞行是合理的。K_v_vel = 0.8是速度环阻尼，决定了跟轨迹时超调的程度，调太大会振荡，调太小会追不上轨迹。max_tilt = 0.4 rad限制最大倾斜角约23度，对应最大水平加速度约3.9 m/s²，这个值要根据机架动力做调整。需要特别关注的是坐标系转换：ego-planner使用ENU坐标系，而PX4的setpoint_raw使用NED坐标系，两者之间不仅需要翻转Z轴，X和Y的对应关系也要交换。

3.3 油门补偿与偏航控制：V1容易被忽略的两个坑

期望姿态有了，但垂直方向不能只靠加速度误差里的Z轴分量。PX4在离板模式下需要外部指定推力值。V1版本的油门算法有两种选择：简单粗暴地固定油门，等于开环定高，适合仿真验证但真机危险；稍好一点的做法是用一个高度环的PID把a_z_ned映射成油门指令，再叠加一个针对悬停油门的偏移量。

thrust = thrust_hover + (K_p_alt * e_z + K_v_alt * e_vz - a_z_ned) / max_thrust

这个式子里的thrust_hover是悬停油门，需要用实测值标定。仿真里PX4默认模型大约在0.5左右，真机要根据电池电压和重量做校准。V1版本建议把推力值和姿态角限制在一条日志里同时输出，起飞前先看这两个参数是否在合理区间。偏航角控制则要单独给yaw_ref做平滑处理，ego-planner的偏航角可能在轨迹点之间跳变，直接塞给PX4会导致原地打转。我一般在发布前对偏航角做一个低通滤波，但滤波截止频率不能太低，否则曲线转弯时机身指向跟不上。

4. 在PX4 SITL里跑通ego-planner闭环：环境、参数与验证流程

4.1 PX4开发环境搭建：Ubuntu 24上的编译与SITL启动

V1版本最痛苦的往往是PX4开发环境搭建，尤其是现在很多人用Ubuntu 24，之前一堆脚本跑不通。我整理一下当前比较顺的流程。PX4官方现在推荐用px4-dev的bash脚本搭环境，但Ubuntu 24上要特别注意python版本和pip来源。不要直接装Ubuntu仓库里的ros相关包，而是用户目录下的~/px4_ros环境，这样编译升级互相独立。

常见做法是拉PX4源码到~/PX4-Autopilot，然后跑./Tools/setup/ubuntu.sh。安装完成后不需要重新编译整个固件，直接启动SITL即可：

cd ~/PX4-Autopilot export PX4_SIM_SPEED_FACTOR=1 make px4_sitl gazebo-classic

如果第一次跑，make会先编译PX4固件，这个时间取决于机器性能，通常15到30分钟。遇到编译报错时先看是不是依赖问题，尤其注意Ubuntu 24默认的gcc版本，如果不匹配可以安装gcc-12或者使用Docker镜像来绕开。

4.2 ego-planner与PX4的联合仿真启动流程

ego-planner侧需要跑在ROS里，我一般用ROS 1的Noetic或ROS 2的Foxy，取决于ego-planner的版本。联合仿真启动脚本的关键是让ego-planner在Gazebo里获取地图数据、定位和传感器信息，再将其输出发布成MAVROS能接收的轨迹话题。

下面是一个启动脚本的框架，用于把ego-planner、MAVROS和PX4 SITL串起来：

#!/bin/bash # 终端1：启动PX4 SITL cd ~/PX4-Autopilot make px4_sitl gazebo-classic # 终端2：启动MAVROS roslaunch mavros px4.launch fcu_url:="udp://:14550@127.0.0.1:14557" # 终端3：启动ego-planner roslaunch ego_planner run_in_sim.launch # 终端4：启动自定义控制器 rosrun trajectory_tracker_v1 tracker_node

需要注意：启动顺序有讲究，先把PX4 SITL跑起来，等Gazebo加载完模型后，再启动MAVROS。MAVROS建立连接后，ego-planner才能从odometry话题拿到状态。如果ego-planner启动过早，它会因为等不到观测数据而进入“无解等待”状态。建议在ego-planner的launch文件里加一个启动延时，例如sleep(5)，等PX4和MAVROS稳定后再发起规划。

4.3 仿真里必须调的5个PX4参数

V1版本在SITL里跑得一塌糊涂的常见原因，不是代码逻辑错误，而是PX4参数没调对。以下是仿真离线模式最关键的几个：

注意，如果你通过setpoint_raw/attitude发的是期望姿态，PX4的MC_PITCH_P和MC_ROLL_P决定了姿态响应的快慢。V1控制器里已经做了外环的P增益，内环如果太激进会导致整体闭环出现相位滞后和振荡。这个“外环给得稳、内环别太猛”的原则在V1调试中很重要。

5. 联调避坑：ego-planner+PX4 V1版最容易翻车的5个高频问题

5.1 OFFBOARD模式莫名退出：飞机自动切回自稳

多人第一次跑都遇到过：飞机在仿真里飞得好好的，突然一下失去外部输入，开始悬停或按遥控器模式飞行。原因是PX4的OFFBOARD模式有1秒的超时机制，如果MAVROS没能持续发送指令，飞控就认为外部控制失效，主动退出。ego-planner在重规划期间会有短暂的轨迹输出空隙，如果这期间控制器节点没发布新指令，超时就会触发。解决方法是控制器节点里写一个发布守卫：即使没有新的轨迹点，也要把上一个有效指令以50Hz持续发布；同时在ego-planner端，把轨迹的发布频率与PX4的接收超时匹配，确保最差情况下也不会暂停太久。另一个坑是遥控器模式切换开关，仿真里虽然没有遥控器，但PX4 SITL默认的遥控器输入可能为0，某些固件版本会在这种状态下禁止OFFBOARD使能，需要在启动前检查rc_sim的配置。

5.2 姿态指令死区：飞机悬停但有微小漂移

现象是轨迹跟踪的时候飞机整体飞得可以，但悬停时慢飘，位置环输出很小，姿态指令处于“发抖”状态。原因有两处：一是期望加速度计算里的位置误差在厘米级时，乘以K_p_pos=2.0得到的加速度指令很小，在姿态映射时接近死区，PX4内环不响应；二是ego-planner在悬停时持续输出微小抖动的位置点，控制器跟着抖。解决思路是给期望加速度加一个“死区抑制”：当位置误差和速度误差都小于阈值时，直接把期望加速度置零，并输出上一个稳定姿态；另一个做法是在悬停阶段降低位置环增益，比如把K_p_pos从2.0降到1.0。注意死区不是越大越好，太大会让飞机跟不上小幅度走位，V1版本可以先从5cm开始试。

5.3 期望轨迹往下“钻地”：坐标系翻转的经典翻车

ego-planner在Gazebo里输出的是ENU坐标（Z轴向上），PX4内部用的是NED（Z轴向下）。新版本固件里MAVROS会对mavros/setpoint_raw自动做一次NED转换，但如果你用local_position话题做反馈，就需要注意一次转换带来的问题。常见翻车表现是飞机在平面飞得很正常，但高度方向完全相反——期望高度往下冲。解决方法是打印一条包含ENU原始输入和NED期望输出的日志，起飞前悬停检查高度是否正确；如果发现取反，就在控制器里做一次方向切换。我在代码里已经写了坐标系转换，实际联调时还会在ego-planner和MAVROS之间再确认一次。

5.4 轨迹跟随的“甩尾效应”：急转弯时机体跟不上

现象是直线飞行顺畅，但遇到90度转角、S弯时飞机明显滞后，甚至出现机体姿态瞬间超过设定值又拉回的抖动。原因是ego-planner的轨迹上速度和加速度同时变化，位置环带宽不足，外环输出的期望姿态变化率超过了PX4内环能响应的范围。解决建议：V1先给期望姿态率加一个限幅，比如最大角速率max_att_rate = 1.5 rad/s，让整个轨迹跟踪变成有界输出；同时把ego-planner轨迹的时间参数拉长，缓解转角的惩罚权重。另一个重要经验是给期望轨迹做预滤波，用一阶滞后平滑位置和速度，缺点是会引入跟踪误差，但明显改善姿态突变。V1版本一定要保留这个预滤波开关，实际调才知道轨迹跟不跟得上。

5.5 真机测试前的PIX飞控无法复位

仿真跑通了，一上真机PIXHAWK，发现固件经常“没反应”，接上地面站提示传感器校准不通过。这不是代码问题，而是PX4硬件控制器自检机制。V1版本在上真机前要养成一个习惯：在QGC里完成加速度计和罗盘校准，确认电调校准协议（DShot还是PWM），并且在电池电压满电时检查电源模块。如果上电后PX4一直闪烁黄灯，多半是磁罗盘干扰，这时候别急着怀疑控制器程序，断开电池重新上电，排查传感器数据是否正常。飞控的“黑匣子”日志在V1版本里也要开好，它的sd卡日志是后面排查一切玄学问题的基础。

6. 用log回放量化轨迹误差：V1升V2前必做的性能体检

V1版本跑通后，最值得花时间的是做一次“轨迹跟踪误差体检”。不要只靠肉眼观察飞机跟没跟上，方法是在机载端同时记录ego-planner发布的期望位置和我们收到飞控反馈的实际位置，然后把两条曲线画在一起。MAVROS已经在/mavros/local_position/odom里发布了位姿，ego-planner本身的topic也要开bag记录。具体做法是用一个脚本订阅这两个话题，同步时间戳后以10Hz采样，计算每个时刻的三维位置误差。下面给一个简单的Python脚本框架：

import rosbag import numpy as np bag = rosbag.Bag('tracking_test.bag') traj = [] # (time, x, y, z) 期望位置 odom = [] # (time, x, y, z) 实际位置 for topic, msg, t in bag.read_messages(topics=['/trajectory_planner/traj_point', '/mavros/local_position/odom']): if topic == '/trajectory_planner/traj_point': traj.append((t.to_sec(), msg.position.x, msg.position.y, msg.position.z)) else: odom.append((t.to_sec(), msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z)) # 插值对齐 traj.sort(); odom.sort() ... # 计算每个采样点的误差 err = np.linalg.norm(traj_pos - odom_pos, axis=1) print(f"平均误差: {err.mean():.3f} m, 最大误差: {err.max():.3f} m")

注意这里的期望位置topic名称要按你ego-planner实际发布的话题来写，不同版本可能不同。误差指标建议同时给出平均值、最大误差、P95误差。平均误差反映了整体跟踪精度，P95排除了瞬态尖峰的影响，最大误差则表示极端情况有没有触发避碰。V1版本的合理目标是：平均误差小于0.15米，最大误差小于0.4米。如果差得远，先检查位置环增益，再检查前馈项是否真的生效。

除了数值指标，还要看误差曲线的时间形态。误差方向持续偏一个方向，通常是坐标偏移或重心配平问题。误差在过弯时突然增大再恢复，说明阻尼不足，需要提高K_v_vel或对期望加速度做预滤波。误差波形和ego-planner轨迹的急缓段强相关，说明前馈权重不对，a_feedforward被压得太低。

我的习惯是每次都把跟踪误差曲线打印出来，和上一次版本对比，确认改动带来的变化是可解释的。V2版本去优化重规划带来的轨迹切换冲击时，这套误差对标直接告诉你改成了没有。希望这套V1版本的落地流程能帮你少走弯路——ego-planner和PX4之间不差规划算法，差的是一个能稳稳接住轨迹、可复现的控制器。希望帮到你。

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