简介ego-plannerPX4自定义控制器V1版本面向无人机自主导航与运动控制方向的开发者将自我中心路径规划算法与开源PX4飞控系统深度结合提供一套可运行、可扩展的自定义控制器实现方案。资源共74个文件、55KB采用ROS工作空间结构包含quadrotor_msgs消息定义、Python控制脚本、yaml参数配置与C源码msg与py文件占比高便于快速理解规划与控制的数据链路。目前已有739人学习浏览适合具备基础ROS与PX4知识、希望上手ego-planner二次开发或改造PX4控制逻辑的进阶用户。阅读cxr_ego_ctrl-master源码可掌握自定义控制器与ego-planner的接口设计、消息通信方式及参数调优思路为实际无人机开发提供参考。1. ego-planner 是什么为什么你绕不开 PX4 自定义控制器做无人机自主飞行的工程师大概率会遇到这个组合ego-planner 负责把一条从 A 到 B 的可行轨迹规划出来PX4 负责让飞机真正沿着轨迹飞。问题在于ego-planner 输出的不是一串航点而是一段带期望速度、期望加速度的 B 样条轨迹而 PX4 默认的位置控制器并不吃这套格式它按航点级联 PID 工作。于是你必须写一个“自定义控制器”作为两者的翻译层和内核层。V1 版本的难点不在控制率本身而在三个地方轨迹消息怎么进飞控、期望值的时间戳怎么对齐、以及各环限幅在哪里触发。这篇内容适合正在跑 px4 仿真验证、以及想把航点式飞行升级成轨迹飞行的从业者我把实际调通的路径和参数边界一次性讲清楚。2. 先从 ego-planner 的输出端下手拿到手的是 B 样条不是坐标点2.1 三个输出字段位置、速度、加速度一个都不能丢EGO-planner 属于局部规划器它的核心产物是一条带时间参数的均匀 B 样条uniform B-spline。这条样条不是一串离散 waypoint它是由若干控制点control points张成的连续曲线任何时刻 t 的期望状态——位置、速度、加速度——都能由控制点和时间戳精确算出来。很多人在第一步就误解了以为拿到 path 就行直接把控制点当作航点喂给 PX4 的 offboard 模式结果飞出来一顿一挫轨迹全程在抖。原因很简单PX4 的位置环按离散航点切换时速度期望是突变的而 ego-planner 里速度、加速度是连续可导的。你丢掉加速度前馈就丢掉了曲线在弯曲段的主要驱动力。正确做法是把这三组字段一并取出来位置期望用于修正偏差速度期望是主控制量加速度期望作为前馈项抵消轨迹曲率带来的跟踪滞后。V1 控制器里加速度前馈往往不是“锦上添花”而是曲线跟踪性能的关键。2.2 采样轨迹时前瞻时间怎么选把 B 样条离散成 PX4 能接受的 setpoint核心是确定“当前该发哪个点”。直接发当前时刻的 t 对应的期望值会让控制器永远在追滞后目标——因为飞控从收到指令到真正产生推力中间有通信延迟、状态估计延迟、电机响应延迟加起来往往超过 30 毫秒。我一般会加一个前瞻时间lookahead也就是取 t lookahead 时刻的期望值作为当前目标。这个参数在 V1 版本里可以直接硬编码之后再去调。前视时间太小轨迹转弯处会内切前视时间太大飞机会抄近路偏离走廊。取值经验是室内小飞机轴距 250mm 左右从 0.3 秒起步室外大飞机轴距 550mm 以上从 0.6 秒起步。采样频率也有讲究。PX4 的 offboard 指令通常要求不低于 10Hz但实际接 ego-planner 时我会按 50Hz 到 100Hz 发布。低于 30Hz位置环会看到明显的阶梯状输入轨迹跟踪的平滑性肉眼可见地变差。以下是采样逻辑的关键片段# 从 ego-planner 的 B-spline 中取前瞻点并打包成 setpoint def sample_trajectory(spline, current_time, lookahead0.5): t_query min(current_time lookahead, spline.end_time) pos, vel, acc spline.evaluate(t_query) # 三阶状态一次取出 return pos, vel, acc这里lookahead的单位是秒取值在 0.3 到 0.8 之间。evaluate是 B 样条求值函数底层用 de Boor 递推实现。重点在于不要把lookahead设成固定帧数而要设成固定时间——因为 ego-planner 的轨迹时间戳不是均匀分布在航点上的用帧数算会引入额外相位误差。采样频率由外部定时器控制建议用独立的 100Hz 定时中断而不是在规划回调里顺带发布。2.3 轨迹终点不收敛的源头在这个设计里ego-planner 默认在目标点附近会生成一段减速轨迹也就是末段期望速度逐渐趋近于零。但如果你直接把完整轨迹发给 PX4飞机会在终点附近反复横跳看起来像校正不过来的振荡。根源在两点。第一PX4 的位置环和速度环带宽有限对末段的微小偏差做积分校正时会产生一个“过冲—回调”的极限环第二ego-planner 的轨迹在末段时间戳会压缩得很短前瞻点会提前滑到终点之后相当于给控制器的目标变成了终点本身偏差信号被瞬间放大。V1 阶段我建议的做法是加一个“近终点切换阈值”当轨迹剩余距离小于 0.4 米时不再跟踪样条而是直接把控制器切到位置保持模式用 PX4 自带的位置控制器锁住当前点。这个阈值在不同机架上有差异但逻辑是通用的——样条负责把你送进一个球进球之后交给悬停控制。3. 自定义控制器 V1 的三种落地方式改模块、加模块、外部接管3.1 方式 A直接改 mc_pos_control出 V1 最快PX4 多旋翼的默认位置控制链路是 mc_pos_control位置环 速度环→ mc_rate_control姿态环 角速度环。如果想快速做 V1 版本最常见做法是直接在mc_pos_control里把期望速度来源从“航点位置误差”改成“ego-planner 给的期望速度”并且把加速度前馈加进去。这种做法的优点是改动面小px4 开发环境搭建时不需要重建太多依赖编译时间短。缺点也明显你是在修改一个闭环内部逻辑PX4 原有的位置环积分项、限幅逻辑还在生效容易造成控制量双重累积。实际改法是在PositionControl类里加一个输入订阅 ego-planner 的轨迹然后把_pos_sp和_vel_sp直接赋值为输入值不再走位置误差 P 环。速度环的 PID 仍然保留因为速度环需要对抗风阻和模型误差纯前馈无法实现精确跟踪。代码大致的骨架如下// mc_pos_control 内部新增回调订阅外部轨迹期望 void PositionControl::setExternalTrajectory(const vehicle_trajectory_waypoint_s traj) { // 直接用外部轨迹替代位置环输出只保留速度环和加速度前馈 _pos_sp(0) traj.position[0]; _pos_sp(1) traj.position[1]; _pos_sp(2) traj.position[2]; _vel_sp(0) traj.velocity[0]; _vel_sp(1) traj.velocity[1]; _vel_sp(2) traj.velocity[2]; _acc_sp(0) traj.acceleration[0]; _acc_sp(1) traj.acceleration[1]; _acc_sp(2) traj.acceleration[2]; }注意_pos_sp仍然被位置环 P 项使用所以这里不能直接把位置环去掉否则会失去位置保持能力。正确做法是把位置环的 P 增益调低让它在轨迹跟踪时只起修正作用而不是主导作用。V1 阶段一个重要参数就是MC_POS_P从默认的 3.5 调低到 2.0 左右能明显减轻转弯时的超调。3.2 方式 B新建 px4_module保留默认内层控制另一种更干净的落地路径是新建一个独立模块在飞控内部订阅 ego-planner 的轨迹消息输出到内置的速度控制器。它不碰 mc_pos_control 的任何逻辑只把期望值“注入”到控制链路里。新模块的写法有固定套路在src/modules/下建目录写好模块主逻辑再在CMakeLists.txt里把模块加进去。PX4 里新建模块不是改个文件就能跑起来的光一个px4_module模板就有任务调度、参数声明、uORB 订阅发布几个部分要写踩坑概率比方式 A 高但胜在隔离性好——你写的控制逻辑出问题最多是把指令发错不会污染默认制导逻辑。我一般会在这个模块里直接订阅vehicle_local_position_setpoint和trajectory_setpoint两个 uORB topic前者用来做模式判断后者用来接收 ego-planner 的轨迹。模块内部把速度期望通过vehicle_rates_setpoint发布出去这一步要特别小心坐标系转换PX4 内部用的都是 NED 系而 ego-planner 的输出通常也是 NED但如果中间经过 ROS就可能变成 ENU这个转换错一处飞机会直接侧飞出去。3.3 方式 C机载电脑跑控制器PX4 只留角速度环如果飞机上带了性能足够的机载电脑且你后续打算上 MPC 或几何控制这类新型控制律那 V1 版本最省事的路径是在机载电脑上做全套位置环和速度环PX4 只负责姿态环和角速度环也就是把 px4 降级成“执行器”。这种方式的实现不用动 PX4 源码PX4 固件里跑的还是默认代码只通过 offboard 模式接收高层的角速度期望。常见的做法是机载电脑一条链路订阅 ego-planner 轨迹另一条链路订阅 PX4 的状态估计两边的数据在自己的控制代码里融合然后把计算出来的 body rate 发给 PX4。由于角速度环带宽远高于位置环对 50Hz 指令里的微小抖动不敏感这种方法对通信延迟的容忍度最高。代价是开发量全堆在机载电脑侧。你需要自己维护状态估计的预处理、坐标变换、限幅逻辑、故障切换逻辑。真机上如果机载电脑死机PX4 不会自动接管所以 V1 阶段要额外写一个看门狗连续 300ms 没收到新指令就切换到 PX4 自稳模式。我建议第一次做整体方案时先按方式 B 跑通流程再在 V1.2 阶段切到方式 C 提高上限。4. 数据通路决定成败uORB、MAVLink、还是 FastDDS4.1 三条通道的选型对比ego-planner 和 PX4 之间的通信方式直接决定了 V1 控制器的延迟、复杂度和可调试性。我整理了三条常用路径的对比通道延迟部署复杂度适用场景uORB 内部直连 1ms低改固件方式 A / 方式 B状态全在飞控内MAVLink / MAVROS10-50ms中机载电脑 ROS快速原型方式 C 前期验证FastDDS / Micro XRCE-DDS1-10ms高额外通信栈方式 C 正式版PX4 1.14 配套uORB 直连只适用于控制器逻辑跑在 PX4 内部的情况如果你在机载电脑上跑 ego-planner那必经之路是后两者。MAVLink 方案开发最快不需要改固件QGC 也能直接看 topicFastDDS 方案延迟更低、消息吞吐更大但 px4 从放弃到精通这条路上栽在 DDS 配置上的人不在少数。V1 阶段我不建议一上来就上 FastDDS先把功能闭环跑通更重要。4.2 桥接层代码把 ego-planner 的轨迹翻译成飞控指令以 MAVROS 为例机载电脑侧的桥接层是这么做的先订阅 ego-planner 的规划轨迹再转成mavros/setpoint_raw/local发布出去。关键在于setpoint_raw里type_mask的配置它决定 PX4 是否使用速度、加速度字段。很多人写桥接时漏设加速度字段对应的 mask 位导致 PX4 忽略加速度前馈。// 桥接节点核心逻辑订阅轨迹发布 setpoint_raw auto sub_traj nh.subscribequadrotor_msgs::Trajectory( /ego_planner/trajectory, 10, traj_cb); void traj_cb(const quadrotor_msgs::Trajectory::ConstPtr msg) { // 取当前时间对应的轨迹点 double t (ros::Time::now() - msg-start_time).toSec() LOOKAHEAD_TIME; geometry_msgs::PoseStamped pose; geometry_msgs::TwistStamped twist; // 从 B 样条求值并填充到 setpoint_raw setpoint_raw.twist.twist.linear.x vel[0]; // 速度主控 setpoint_raw.accel.accel.linear.x acc[0]; // 加速度前馈 setpoint_raw.type_mask 0b100; // 忽略位置使用速度加速度 pub_raw.publish(setpoint_raw); }LOOKAHEAD_TIME是前面说的前瞻参数这里取 0.5 是一个兼顾跟踪误差与稳定性的保守值。type_mask的二进制位你可以查 MAVLink 官方定义但记住一个便捷判断方法不想要的字段对应 bit 置 1想要的字段置 0。上面 0b100 表示忽略位置、使用速度和加速度。若想用位置修正mask 就得改成其他组合但 V1 里我建议优先用速度 加速度模式因为这是 ego-planner 输出信息最完整的模式。4.3 时间戳对齐轨迹延迟几十毫秒才是最大问题数据通路里最容易忽略的是时间基准不统一。ego-planner 在机载电脑上运行它记录的轨迹时间戳用的是 ROS 时钟PX4 侧状态估计用的是飞控内部的时间。两套时钟如果不做同步控制器拿到的“当前时刻的期望状态”实际上是几十毫秒之前的这在高速转弯时会造成持续偏航。我在 V1 阶段用的土办法是桥接节点收到轨迹消息后用ros::Time::now()和消息里的起始时间做差而不去依赖 PX4 的时间戳。因为 MAVLink 转发本身延迟较大飞控内部时间与主机时间具备可比性稳妥做法是在机载电脑上统一用调度周期标记轨迹时刻。更深层的做法是启用 PX4 的 time sync over MAVLink 功能它会自动补偿两边的时钟偏移但配置难度高建议 V1 版本不碰。还有一个常被忽略的点轨迹里带的加速度期望是 NED 系下的PX4 内部控的是机体系角速度和姿态。桥接层可以直接把 NED 系数据发给飞控PX4 会自动完成系间转换但前提是你的坐标系声明要准确——搞反 ENU 和 NED飞机偏航 180 度是必然结果。排查这类问题看日志最直观期望速度方向与实际速度方向差 180 度那就是坐标系反了。5. PX4 自定义控制器 V1 避坑我实际踩过的 5 个坑5.1 uORB 订阅不到 topic数据全为零现象新写的模块跑起来了日志里显示没有收到任何 trajectory 数据QGC 上也看不到对应 topic。原因多半是消息类型没在模块的 CMakeLists 里声明依赖。PX4 的 uORB 编译是按消息类型生成的你订阅trajectory_setpoint_s但模块构建时没把对应头文件拉进来运行时就静默失败不会报错。解决在模块的CMakeLists.txt里确保添加DEPENDS声明把用到的消息类型名列进去重新编译后再次运行用listener trajectory_setpoint命令验证订阅通道是否激活。这一步 90% 的“没数据”问题都能解决。5.2 加速度期望给得很大但机体纹丝不动现象ego-planner 在急转弯段规划出 5 m/s² 以上的期望加速度飞机却明显跟不上甚至出现短暂停滞。原因PX4 内部有默认加速度限幅参数而且限幅发生在内环。如果你的自定义控制器走的是位置速度环加速度前馈会先被MC_ACC_LIM这类参数截断。直接把期望加速度写进 setpoint会被视为“超限指令”而不是“前馈项”。解决在 V1 阶段把 ego-planner 输出的加速度期望先人工限幅到 2.5 m/s² 以内同时将 PX4 的MC_ACC_LIM参数调大。更可靠的路径是检查你的 setpoint 究竟写入了哪个 topic——如果走的是trajectory_setpoint它本身不带限幅但进入控制器后会受内部参数约束走setpoint_raw则受 MAVLink 自身字段限制。无论如何真机首飞前一定要手动打一条大加速度测试轨迹确认油门有响应。5.3 终点附近绕圈轨迹结束不了现象规划轨迹的终点到了飞机不悬停以终点为圆心小幅绕圈转速越来越快。原因前瞻点在轨迹末端被强制钳到终点但轨迹时间戳已经结束会反复输出同一点叠加位置环 P 和速度环 I 的累积就形成了极限环。解决我在 2.3 里提的近终点切换逻辑这里用得更直接当剩余轨迹距离小于 0.5 米时模块主动让出控制权切换成 PX4 的定点模式。注意切换要无扰即速度期望先归零再切否则会产生一次跳跃。这个坑在纯 SITL 仿真里不容易暴露因为仿真模型没有真实风扰和电机延迟真机测试时是必现的。5.4 改了参数不生效或者重启后被复位现象QGC 里把控制参数调好解锁试飞发现行为没变化甚至参数表里显示的还是旧值。原因参数写入 Flash 后需要重新读取而且部分参数属于飞控启动期加载改动后不重启不生效另一些参数比如部分模块私有参数只在模块启动时读取一次热修改无效。解决改参数后先点 QGC 的“写入并重启”再从日志确认参数版本。如果改的是自定义模块里的参数要确认它是不是param系统管理的参数——用param set改私有参数时模块必须重启所以 V1 阶段我习惯在模块的run函数入口统一param_get避免运行时读旧值。5.5 真机一解锁 EKF 就不收敛日志里全是振动现象V1 在仿真里飞得不错上真机解锁后 PX4 直接报 EKF 错误状态估计发散。原因真机的机械振动和 IMU 饱和是仿真模型没有的。ego-planner 轨迹里高频加速度期望会通过控制器转换成高频油门变化IMU 采样到的振动淹没真实加速度EKF 的加速度观测直接失真。解决这个坑没有算法层面的后悔药只能在机械层面解决先裸飞悬停确认 EKF 方差正常再跑慢速轨迹逐步提高速度上限观察estimator_status里的方差变化。另外务必确认加速度计低通滤波参数没有被人为调太高——滤波越高EKF 看到的高频振动越少但相位延迟也越大轨迹跟踪会变软。V1 阶段用默认滤波即可不要调参。6. V1 版本的验证闭环用一条 B 样条把整条链路打穿V1 阶段别急着上真机。先用 SITL 仿真把整条链路的通畅性验证完再考虑真机。最小验证方法是启动 px4 仿真机载电脑上运行一个假轨迹发生器发布一段已知的 B 样条——比如直线加速再刹停一段“S”形转弯——然后让自定义控制器跟踪它。# 终端 1启动 PX4 的 SITL 仿真gazebo Classic / 新版均可 make px4_sitl gazebo-classic # 终端 2启动 MAVROS 桥接 roslaunch mavros px4.launch fcu_url:udp://:14540127.0.0.1:14557 # 终端 3跑轨迹发生器直接发布 setpoint rosrun my_controller fake_trajectory_node.pyfake_trajectory_node.py不需要真的调用 ego-planner只需要按固定时间戳输出一组已知位置、速度、加速度目的是验证自定义控制器的跟踪能力。发布频率设为 50Hz加速度峰值设为 2 m/s²先保守后放开。跑完看日志三个指标必须同时合格一是期望速度和实际速度的误差曲线稳定在 ±0.3 m/s 以内二是期望加速度与实际加速度的相位差不能出现明显滞后三是电机指令不能周期性顶到上下限。前两个指标在 QGC 的日志分析页面里都能看第三个指标要到 “Actuator Output” 图里查。如果速度跟踪收敛但位置偏移持续增大说明前瞻时间调小了如果曲线高频抖动说明采样频率或滤波有问题。V1 全流程走完后我再强调一遍我的习惯每次改参数只动一个变量并且把飞行日志存档标注参数版本。PX4 从放弃到精通不是靠一次大改而是靠一版一版的参数迭代。ego-planner 加自定义控制器的组合V1 的目标不是飞得多好而是建立一个可复现、可回归的测试基线仿真里同一段轨迹参数改完前后对比误差曲线才是判断改进是否有效的唯一依据。希望这些经验能帮你少走几个坑让你的 ego-planner 和 PX4 自定义控制器早日跑通你的第一条完整轨迹。本文还有配套的精品资源点击获取 
长河君 · 2026/10/31 1:12:52 · 阅读 1.2k ego-planner与PX4自定义控制器：从B样条到轨迹跟踪的落地实践
简介ego-plannerPX4自定义控制器V1版本面向无人机自主导航与运动控制方向的开发者将自我中心路径规划算法与开源PX4飞控系统深度结合提供一套可运行、可扩展的自定义控制器实现方案。资源共74个文件、55KB采用ROS工作空间结构包含quadrotor_msgs消息定义、Python控制脚本、yaml参数配置与C源码msg与py文件占比高便于快速理解规划与控制的数据链路。目前已有739人学习浏览适合具备基础ROS与PX4知识、希望上手ego-planner二次开发或改造PX4控制逻辑的进阶用户。阅读cxr_ego_ctrl-master源码可掌握自定义控制器与ego-planner的接口设计、消息通信方式及参数调优思路为实际无人机开发提供参考。1. ego-planner 是什么为什么你绕不开 PX4 自定义控制器做无人机自主飞行的工程师大概率会遇到这个组合ego-planner 负责把一条从 A 到 B 的可行轨迹规划出来PX4 负责让飞机真正沿着轨迹飞。问题在于ego-planner 输出的不是一串航点而是一段带期望速度、期望加速度的 B 样条轨迹而 PX4 默认的位置控制器并不吃这套格式它按航点级联 PID 工作。于是你必须写一个“自定义控制器”作为两者的翻译层和内核层。V1 版本的难点不在控制率本身而在三个地方轨迹消息怎么进飞控、期望值的时间戳怎么对齐、以及各环限幅在哪里触发。这篇内容适合正在跑 px4 仿真验证、以及想把航点式飞行升级成轨迹飞行的从业者我把实际调通的路径和参数边界一次性讲清楚。2. 先从 ego-planner 的输出端下手拿到手的是 B 样条不是坐标点2.1 三个输出字段位置、速度、加速度一个都不能丢EGO-planner 属于局部规划器它的核心产物是一条带时间参数的均匀 B 样条uniform B-spline。这条样条不是一串离散 waypoint它是由若干控制点control points张成的连续曲线任何时刻 t 的期望状态——位置、速度、加速度——都能由控制点和时间戳精确算出来。很多人在第一步就误解了以为拿到 path 就行直接把控制点当作航点喂给 PX4 的 offboard 模式结果飞出来一顿一挫轨迹全程在抖。原因很简单PX4 的位置环按离散航点切换时速度期望是突变的而 ego-planner 里速度、加速度是连续可导的。你丢掉加速度前馈就丢掉了曲线在弯曲段的主要驱动力。正确做法是把这三组字段一并取出来位置期望用于修正偏差速度期望是主控制量加速度期望作为前馈项抵消轨迹曲率带来的跟踪滞后。V1 控制器里加速度前馈往往不是“锦上添花”而是曲线跟踪性能的关键。2.2 采样轨迹时前瞻时间怎么选把 B 样条离散成 PX4 能接受的 setpoint核心是确定“当前该发哪个点”。直接发当前时刻的 t 对应的期望值会让控制器永远在追滞后目标——因为飞控从收到指令到真正产生推力中间有通信延迟、状态估计延迟、电机响应延迟加起来往往超过 30 毫秒。我一般会加一个前瞻时间lookahead也就是取 t lookahead 时刻的期望值作为当前目标。这个参数在 V1 版本里可以直接硬编码之后再去调。前视时间太小轨迹转弯处会内切前视时间太大飞机会抄近路偏离走廊。取值经验是室内小飞机轴距 250mm 左右从 0.3 秒起步室外大飞机轴距 550mm 以上从 0.6 秒起步。采样频率也有讲究。PX4 的 offboard 指令通常要求不低于 10Hz但实际接 ego-planner 时我会按 50Hz 到 100Hz 发布。低于 30Hz位置环会看到明显的阶梯状输入轨迹跟踪的平滑性肉眼可见地变差。以下是采样逻辑的关键片段# 从 ego-planner 的 B-spline 中取前瞻点并打包成 setpoint def sample_trajectory(spline, current_time, lookahead0.5): t_query min(current_time lookahead, spline.end_time) pos, vel, acc spline.evaluate(t_query) # 三阶状态一次取出 return pos, vel, acc这里lookahead的单位是秒取值在 0.3 到 0.8 之间。evaluate是 B 样条求值函数底层用 de Boor 递推实现。重点在于不要把lookahead设成固定帧数而要设成固定时间——因为 ego-planner 的轨迹时间戳不是均匀分布在航点上的用帧数算会引入额外相位误差。采样频率由外部定时器控制建议用独立的 100Hz 定时中断而不是在规划回调里顺带发布。2.3 轨迹终点不收敛的源头在这个设计里ego-planner 默认在目标点附近会生成一段减速轨迹也就是末段期望速度逐渐趋近于零。但如果你直接把完整轨迹发给 PX4飞机会在终点附近反复横跳看起来像校正不过来的振荡。根源在两点。第一PX4 的位置环和速度环带宽有限对末段的微小偏差做积分校正时会产生一个“过冲—回调”的极限环第二ego-planner 的轨迹在末段时间戳会压缩得很短前瞻点会提前滑到终点之后相当于给控制器的目标变成了终点本身偏差信号被瞬间放大。V1 阶段我建议的做法是加一个“近终点切换阈值”当轨迹剩余距离小于 0.4 米时不再跟踪样条而是直接把控制器切到位置保持模式用 PX4 自带的位置控制器锁住当前点。这个阈值在不同机架上有差异但逻辑是通用的——样条负责把你送进一个球进球之后交给悬停控制。3. 自定义控制器 V1 的三种落地方式改模块、加模块、外部接管3.1 方式 A直接改 mc_pos_control出 V1 最快PX4 多旋翼的默认位置控制链路是 mc_pos_control位置环 速度环→ mc_rate_control姿态环 角速度环。如果想快速做 V1 版本最常见做法是直接在mc_pos_control里把期望速度来源从“航点位置误差”改成“ego-planner 给的期望速度”并且把加速度前馈加进去。这种做法的优点是改动面小px4 开发环境搭建时不需要重建太多依赖编译时间短。缺点也明显你是在修改一个闭环内部逻辑PX4 原有的位置环积分项、限幅逻辑还在生效容易造成控制量双重累积。实际改法是在PositionControl类里加一个输入订阅 ego-planner 的轨迹然后把_pos_sp和_vel_sp直接赋值为输入值不再走位置误差 P 环。速度环的 PID 仍然保留因为速度环需要对抗风阻和模型误差纯前馈无法实现精确跟踪。代码大致的骨架如下// mc_pos_control 内部新增回调订阅外部轨迹期望 void PositionControl::setExternalTrajectory(const vehicle_trajectory_waypoint_s traj) { // 直接用外部轨迹替代位置环输出只保留速度环和加速度前馈 _pos_sp(0) traj.position[0]; _pos_sp(1) traj.position[1]; _pos_sp(2) traj.position[2]; _vel_sp(0) traj.velocity[0]; _vel_sp(1) traj.velocity[1]; _vel_sp(2) traj.velocity[2]; _acc_sp(0) traj.acceleration[0]; _acc_sp(1) traj.acceleration[1]; _acc_sp(2) traj.acceleration[2]; }注意_pos_sp仍然被位置环 P 项使用所以这里不能直接把位置环去掉否则会失去位置保持能力。正确做法是把位置环的 P 增益调低让它在轨迹跟踪时只起修正作用而不是主导作用。V1 阶段一个重要参数就是MC_POS_P从默认的 3.5 调低到 2.0 左右能明显减轻转弯时的超调。3.2 方式 B新建 px4_module保留默认内层控制另一种更干净的落地路径是新建一个独立模块在飞控内部订阅 ego-planner 的轨迹消息输出到内置的速度控制器。它不碰 mc_pos_control 的任何逻辑只把期望值“注入”到控制链路里。新模块的写法有固定套路在src/modules/下建目录写好模块主逻辑再在CMakeLists.txt里把模块加进去。PX4 里新建模块不是改个文件就能跑起来的光一个px4_module模板就有任务调度、参数声明、uORB 订阅发布几个部分要写踩坑概率比方式 A 高但胜在隔离性好——你写的控制逻辑出问题最多是把指令发错不会污染默认制导逻辑。我一般会在这个模块里直接订阅vehicle_local_position_setpoint和trajectory_setpoint两个 uORB topic前者用来做模式判断后者用来接收 ego-planner 的轨迹。模块内部把速度期望通过vehicle_rates_setpoint发布出去这一步要特别小心坐标系转换PX4 内部用的都是 NED 系而 ego-planner 的输出通常也是 NED但如果中间经过 ROS就可能变成 ENU这个转换错一处飞机会直接侧飞出去。3.3 方式 C机载电脑跑控制器PX4 只留角速度环如果飞机上带了性能足够的机载电脑且你后续打算上 MPC 或几何控制这类新型控制律那 V1 版本最省事的路径是在机载电脑上做全套位置环和速度环PX4 只负责姿态环和角速度环也就是把 px4 降级成“执行器”。这种方式的实现不用动 PX4 源码PX4 固件里跑的还是默认代码只通过 offboard 模式接收高层的角速度期望。常见的做法是机载电脑一条链路订阅 ego-planner 轨迹另一条链路订阅 PX4 的状态估计两边的数据在自己的控制代码里融合然后把计算出来的 body rate 发给 PX4。由于角速度环带宽远高于位置环对 50Hz 指令里的微小抖动不敏感这种方法对通信延迟的容忍度最高。代价是开发量全堆在机载电脑侧。你需要自己维护状态估计的预处理、坐标变换、限幅逻辑、故障切换逻辑。真机上如果机载电脑死机PX4 不会自动接管所以 V1 阶段要额外写一个看门狗连续 300ms 没收到新指令就切换到 PX4 自稳模式。我建议第一次做整体方案时先按方式 B 跑通流程再在 V1.2 阶段切到方式 C 提高上限。4. 数据通路决定成败uORB、MAVLink、还是 FastDDS4.1 三条通道的选型对比ego-planner 和 PX4 之间的通信方式直接决定了 V1 控制器的延迟、复杂度和可调试性。我整理了三条常用路径的对比通道延迟部署复杂度适用场景uORB 内部直连 1ms低改固件方式 A / 方式 B状态全在飞控内MAVLink / MAVROS10-50ms中机载电脑 ROS快速原型方式 C 前期验证FastDDS / Micro XRCE-DDS1-10ms高额外通信栈方式 C 正式版PX4 1.14 配套uORB 直连只适用于控制器逻辑跑在 PX4 内部的情况如果你在机载电脑上跑 ego-planner那必经之路是后两者。MAVLink 方案开发最快不需要改固件QGC 也能直接看 topicFastDDS 方案延迟更低、消息吞吐更大但 px4 从放弃到精通这条路上栽在 DDS 配置上的人不在少数。V1 阶段我不建议一上来就上 FastDDS先把功能闭环跑通更重要。4.2 桥接层代码把 ego-planner 的轨迹翻译成飞控指令以 MAVROS 为例机载电脑侧的桥接层是这么做的先订阅 ego-planner 的规划轨迹再转成mavros/setpoint_raw/local发布出去。关键在于setpoint_raw里type_mask的配置它决定 PX4 是否使用速度、加速度字段。很多人写桥接时漏设加速度字段对应的 mask 位导致 PX4 忽略加速度前馈。// 桥接节点核心逻辑订阅轨迹发布 setpoint_raw auto sub_traj nh.subscribequadrotor_msgs::Trajectory( /ego_planner/trajectory, 10, traj_cb); void traj_cb(const quadrotor_msgs::Trajectory::ConstPtr msg) { // 取当前时间对应的轨迹点 double t (ros::Time::now() - msg-start_time).toSec() LOOKAHEAD_TIME; geometry_msgs::PoseStamped pose; geometry_msgs::TwistStamped twist; // 从 B 样条求值并填充到 setpoint_raw setpoint_raw.twist.twist.linear.x vel[0]; // 速度主控 setpoint_raw.accel.accel.linear.x acc[0]; // 加速度前馈 setpoint_raw.type_mask 0b100; // 忽略位置使用速度加速度 pub_raw.publish(setpoint_raw); }LOOKAHEAD_TIME是前面说的前瞻参数这里取 0.5 是一个兼顾跟踪误差与稳定性的保守值。type_mask的二进制位你可以查 MAVLink 官方定义但记住一个便捷判断方法不想要的字段对应 bit 置 1想要的字段置 0。上面 0b100 表示忽略位置、使用速度和加速度。若想用位置修正mask 就得改成其他组合但 V1 里我建议优先用速度 加速度模式因为这是 ego-planner 输出信息最完整的模式。4.3 时间戳对齐轨迹延迟几十毫秒才是最大问题数据通路里最容易忽略的是时间基准不统一。ego-planner 在机载电脑上运行它记录的轨迹时间戳用的是 ROS 时钟PX4 侧状态估计用的是飞控内部的时间。两套时钟如果不做同步控制器拿到的“当前时刻的期望状态”实际上是几十毫秒之前的这在高速转弯时会造成持续偏航。我在 V1 阶段用的土办法是桥接节点收到轨迹消息后用ros::Time::now()和消息里的起始时间做差而不去依赖 PX4 的时间戳。因为 MAVLink 转发本身延迟较大飞控内部时间与主机时间具备可比性稳妥做法是在机载电脑上统一用调度周期标记轨迹时刻。更深层的做法是启用 PX4 的 time sync over MAVLink 功能它会自动补偿两边的时钟偏移但配置难度高建议 V1 版本不碰。还有一个常被忽略的点轨迹里带的加速度期望是 NED 系下的PX4 内部控的是机体系角速度和姿态。桥接层可以直接把 NED 系数据发给飞控PX4 会自动完成系间转换但前提是你的坐标系声明要准确——搞反 ENU 和 NED飞机偏航 180 度是必然结果。排查这类问题看日志最直观期望速度方向与实际速度方向差 180 度那就是坐标系反了。5. PX4 自定义控制器 V1 避坑我实际踩过的 5 个坑5.1 uORB 订阅不到 topic数据全为零现象新写的模块跑起来了日志里显示没有收到任何 trajectory 数据QGC 上也看不到对应 topic。原因多半是消息类型没在模块的 CMakeLists 里声明依赖。PX4 的 uORB 编译是按消息类型生成的你订阅trajectory_setpoint_s但模块构建时没把对应头文件拉进来运行时就静默失败不会报错。解决在模块的CMakeLists.txt里确保添加DEPENDS声明把用到的消息类型名列进去重新编译后再次运行用listener trajectory_setpoint命令验证订阅通道是否激活。这一步 90% 的“没数据”问题都能解决。5.2 加速度期望给得很大但机体纹丝不动现象ego-planner 在急转弯段规划出 5 m/s² 以上的期望加速度飞机却明显跟不上甚至出现短暂停滞。原因PX4 内部有默认加速度限幅参数而且限幅发生在内环。如果你的自定义控制器走的是位置速度环加速度前馈会先被MC_ACC_LIM这类参数截断。直接把期望加速度写进 setpoint会被视为“超限指令”而不是“前馈项”。解决在 V1 阶段把 ego-planner 输出的加速度期望先人工限幅到 2.5 m/s² 以内同时将 PX4 的MC_ACC_LIM参数调大。更可靠的路径是检查你的 setpoint 究竟写入了哪个 topic——如果走的是trajectory_setpoint它本身不带限幅但进入控制器后会受内部参数约束走setpoint_raw则受 MAVLink 自身字段限制。无论如何真机首飞前一定要手动打一条大加速度测试轨迹确认油门有响应。5.3 终点附近绕圈轨迹结束不了现象规划轨迹的终点到了飞机不悬停以终点为圆心小幅绕圈转速越来越快。原因前瞻点在轨迹末端被强制钳到终点但轨迹时间戳已经结束会反复输出同一点叠加位置环 P 和速度环 I 的累积就形成了极限环。解决我在 2.3 里提的近终点切换逻辑这里用得更直接当剩余轨迹距离小于 0.5 米时模块主动让出控制权切换成 PX4 的定点模式。注意切换要无扰即速度期望先归零再切否则会产生一次跳跃。这个坑在纯 SITL 仿真里不容易暴露因为仿真模型没有真实风扰和电机延迟真机测试时是必现的。5.4 改了参数不生效或者重启后被复位现象QGC 里把控制参数调好解锁试飞发现行为没变化甚至参数表里显示的还是旧值。原因参数写入 Flash 后需要重新读取而且部分参数属于飞控启动期加载改动后不重启不生效另一些参数比如部分模块私有参数只在模块启动时读取一次热修改无效。解决改参数后先点 QGC 的“写入并重启”再从日志确认参数版本。如果改的是自定义模块里的参数要确认它是不是param系统管理的参数——用param set改私有参数时模块必须重启所以 V1 阶段我习惯在模块的run函数入口统一param_get避免运行时读旧值。5.5 真机一解锁 EKF 就不收敛日志里全是振动现象V1 在仿真里飞得不错上真机解锁后 PX4 直接报 EKF 错误状态估计发散。原因真机的机械振动和 IMU 饱和是仿真模型没有的。ego-planner 轨迹里高频加速度期望会通过控制器转换成高频油门变化IMU 采样到的振动淹没真实加速度EKF 的加速度观测直接失真。解决这个坑没有算法层面的后悔药只能在机械层面解决先裸飞悬停确认 EKF 方差正常再跑慢速轨迹逐步提高速度上限观察estimator_status里的方差变化。另外务必确认加速度计低通滤波参数没有被人为调太高——滤波越高EKF 看到的高频振动越少但相位延迟也越大轨迹跟踪会变软。V1 阶段用默认滤波即可不要调参。6. V1 版本的验证闭环用一条 B 样条把整条链路打穿V1 阶段别急着上真机。先用 SITL 仿真把整条链路的通畅性验证完再考虑真机。最小验证方法是启动 px4 仿真机载电脑上运行一个假轨迹发生器发布一段已知的 B 样条——比如直线加速再刹停一段“S”形转弯——然后让自定义控制器跟踪它。# 终端 1启动 PX4 的 SITL 仿真gazebo Classic / 新版均可 make px4_sitl gazebo-classic # 终端 2启动 MAVROS 桥接 roslaunch mavros px4.launch fcu_url:udp://:14540127.0.0.1:14557 # 终端 3跑轨迹发生器直接发布 setpoint rosrun my_controller fake_trajectory_node.pyfake_trajectory_node.py不需要真的调用 ego-planner只需要按固定时间戳输出一组已知位置、速度、加速度目的是验证自定义控制器的跟踪能力。发布频率设为 50Hz加速度峰值设为 2 m/s²先保守后放开。跑完看日志三个指标必须同时合格一是期望速度和实际速度的误差曲线稳定在 ±0.3 m/s 以内二是期望加速度与实际加速度的相位差不能出现明显滞后三是电机指令不能周期性顶到上下限。前两个指标在 QGC 的日志分析页面里都能看第三个指标要到 “Actuator Output” 图里查。如果速度跟踪收敛但位置偏移持续增大说明前瞻时间调小了如果曲线高频抖动说明采样频率或滤波有问题。V1 全流程走完后我再强调一遍我的习惯每次改参数只动一个变量并且把飞行日志存档标注参数版本。PX4 从放弃到精通不是靠一次大改而是靠一版一版的参数迭代。ego-planner 加自定义控制器的组合V1 的目标不是飞得多好而是建立一个可复现、可回归的测试基线仿真里同一段轨迹参数改完前后对比误差曲线才是判断改进是否有效的唯一依据。希望这些经验能帮你少走几个坑让你的 ego-planner 和 PX4 自定义控制器早日跑通你的第一条完整轨迹。本文还有配套的精品资源点击获取