做固定翼的朋友应该都有同感给Pixhawk刷好固件、在地上站里画好航线以为接下来就是看飞机自己绕圈子。结果真拉起来才发现事情远没那么简单——航线横滚角乱晃、切弯掉高度、油门一抽一抽地反复加减这些问题我全都踩过。PixhawkPX4固定翼之所以默认采用L1导航算法管横向路径跟踪用TECS总能量控制算法管纵向速度与高度是因为这套组合经过多年验证在固定翼这种不能悬停、升力靠机翼、速度不可独立固定的载体上比简单PID稳定得多也更好理解。这篇文章打算写给两类人一类是刚把Pixhawk装到固定翼上正准备把所有控制参数研究一遍的开发者另一类是已经在调固定翼但被L1和TECS参数搞得一头雾水的朋友。我会从PX4固定翼控制链路讲起把L1的几何原理、TECS的能量思路、PX4源码里的实现逻辑、关键参数怎么调、以及我从SITL仿真到实机飞行过程中踩过的坑全部拆开讲一遍。文章里的代码是“基于PX4实现逻辑的简化解析”方便你拿着思路去源码里对号入座。1. 固定翼控制链路全景L1和TECS各管哪一摊1.1 从导航到舵面的完整控制链PX4固定翼的软件控制链路跟很多人想象的不太一样它不是“一个控制器负责所有事情”而是清晰的分层结构导航层Navigator→ 位置控制层固定翼专用位置控制模块→ 姿态控制层→ 混控器 → 舵机/电机其中最关键、也最容易让新手迷路的就是中间这一层位置控制。固定翼的位置控制又被拆成两个正交的问题横向x-y平面飞机要沿着规划好的航线飞这靠L1控制器输出横向加速度指令再转成滚转指令。纵向高度-速度平面飞机要保持目标高度和空速这靠TECS控制器输出俯仰角指令和油门指令。我刚开始接触PX4固定翼时犯过一个典型错误以为固定翼的纵向控制就是“高度差给一个PID速度差再给一个PID”两个PID各管各的。后来才发现飞机是一个高度耦合的系统油门和升降舵互相影响。如果直接把高度差给PID后再推俯仰遇到爬升和转弯同时发生时速度会掉、高度也托不住严重的甚至往失速方向走。这就是PX4选择TECS而不是简单PID的核心原因。1.2 为什么横向用L1、纵向用TECS而不是一套算法打天下先说横向。固定翼在水平面上不能像多旋翼一样“往左平移”它只能通过倾斜机身改变升力分量画出一个圆弧来改变航向。所以横向控制本质上是“路径跟踪”——飞机沿着一条预定航迹飞偏离了要平滑地拉回来不能来回振荡。L1算法最早由MIT的Sanghyuk Park等人提出核心思路是通过“前视距离look-ahead distance”在地平线上选一个参考点把“跟踪直线/圆弧”转换成“追踪一个动态点”。相比传统PIDL1的优势很明显弯道可以自动调整过弯半径、对侧风干扰不敏感、而且调参维度很少一个前视距离和一个阻尼参数基本就能覆盖大部分场景。再说纵向。飞机在垂直平面内油门和升降舵之间的耦合关系是固定的物理特性你拉杆抬头高度上升但同时阻力增大、速度下降你推油门速度增加但升力也增加高度跟着变。这种耦合用两个独立PID很难解。TECS把飞机看作一个“能量系统”用总能量和能量分配两个概念来解耦油门管总能量升降舵管能量分配。这样无论是爬升、下降、加速还是减速油门和升降舵各司其职不会再互相抢。2. L1导航控制算法几何直觉与PX4代码落地2.1 L1的控制原理把“跟踪航线”变成“追踪一个移动参考点”L1算法的好处是它有一个非常直观的几何解释。假设飞机当前位置是P期望航迹是一条直线或者一段圆弧在航迹前方找一个基准点R使PR之间的距离正好等于L1。这个L1就是前视距离通常和地速成正比。接下来看两个向量的夹角一个是飞机的地速向量不光是空速因为地速包含风的影响这对抗风很重要另一个是从P指向R的向量。这两个向量之间的夹角记为eta。当飞机正好在航迹上、且航向与航迹方向一致时eta等于0不需要任何横向修正。一旦飞机偏出航线或者航向不对eta就不为零。L1算法给出的横向加速度指令是a_lat 2 * V^2 * sin(eta) / L1其中V是地速L1是前视距离a_lat是需要的横向加速度。这个公式的物理意义很明确V越大要产生同样的航向变化率就需要更大的横向加速度sin(eta)越大说明偏离越严重修正越用力L1越大前视越远修正越平缓。那L1距离具体怎么取在PX4里它跟地速成正比相当于“飞多少秒的距离”。设L1对应的时间是T固定翼上一般取15到25秒之间那么L1 V * T / π再乘一个阻尼系数相关的因子。为什么是π因为这样算出来在理想情况下飞机对航迹的收敛过程具有近似临界阻尼的特性。用生活化类比来说L1前视距离就像你开车时看多远来决定打方向盘视线只看车头方向一定乱甩看得太远过弯就容易切太多。L1的作用就是帮你找到那个“看得正合适”的距离。2.2 PX4里L1的代码实现拆解下面这段是简化后的实现逻辑思路跟PX4固定翼位置控制模块是一致的只是我去掉了航段切换、圆弧生成、保护限幅等外围细节把核心骨架露出来// 基于PX4 fw_pos_control_l1逻辑的简化解析 void L1Control::update(vehicle_state state, path_info path) { // 1. 计算L1前视距离与地速成正比 // 典型实现L1 地速 * period / (pi * damping) float l1_distance state.ground_speed * _period / (M_PI_F * _damping); l1_distance constrain(l1_distance, 20.0f, 200.0f); // 2. 在当前期望航迹上找参考点R使PR的距离约等于L1 // 对于直线航段直接从当前航段插值对于圆弧航段沿弧线找 Vector2f ref_point find_ref_point(state.pos, path, l1_distance); // 3. 计算地速向量与“飞机-参考点”向量的夹角eta Vector2f course_vec(cosf(state.ground_course), sinf(state.ground_course)); Vector2f ref_vec ref_point - state.pos; float eta atan2f(cross(course_vec, ref_vec), dot(course_vec, ref_vec)); // 4. 核心公式横向加速度指令 float lat_acc 2.0f * state.ground_speed * state.ground_speed * sinf(eta) / l1_distance; // 5. 限幅并转换为滚转指令由内环姿态控制器执行 lat_acc constrain(lat_acc, -_max_lat_acc, _max_lat_acc); _roll_cmd atan2f(lat_acc, CONSTANTS_ONE_G); }注意几个细节第一PX4用的是地速不是空速。为什么因为地速直接反映了飞机相对地面的运动趋势而航线跟踪最终要的是“飞机在地面上的轨迹”。地速本身已经包含了风的影响所以L1算法天然对风不那么敏感。第二代码里的find_ref_point在真实PX4里是有讲究的。对于当前航段两个航点之间它要找的是“飞机当前位置沿航迹方向投影过去、距离正好L1的点”。实际上PX4做了一点简化它会把L1分成横向分量和纵向分量必要时还会考虑航段切换时的提前量。这些细节在源码里对应FW_L1_EXTRA_WP之类的参数。第三滚转指令不能无限大。飞机结构有极限坡度PX4里对横向加速度做了限幅实际还会叠加一个“最大滚转角”保护。滚转角指令到了姿态控制内环以后还会有滚转速率限制。2.3 L1关键参数说明与调参策略PX4里L1相关的核心参数不多主要就这几个参数名作用推荐初始值调参影响FW_L1_DAMPING阻尼系数0.75越小响应越灵敏过小会S形振荡越大越平缓FW_L1_PERIOD前视距离时间常数25秒越大前视越远转弯越钝越小切弯越狠FW_L1_EXTRA_WP航段切换时的额外前视1影响转弯时切航点的时机FW_L1_HTE是否用空速替代地速5极端风况下用空速更稳定如果你刚开始调L1我的建议是先只用FW_L1_DAMPING和FW_L1_PERIOD这两个参数。实际调参时我习惯按下面这个顺序走先看直线跟踪让飞机沿一条很长的直线航段飞观察横向跟踪误差曲线。如果飞机在航迹左右来回画S形说明前视太短或者阻尼太小增大FW_L1_PERIOD或者FW_L1_DAMPING。再看90度和180度转弯看飞机是在航点之前提前切弯还是过了航点之后才慢慢追过去。切得太早减小FW_L1_EXTRA_WP切得太晚增大FW_L1_EXTRA_WP。最后看大风下的表现如果侧风很大飞机压不住航迹检查一下用的是地速还是空速FW_L1_HTE以及地速能不能从GPS正常解算出来。前期在仿真里把L1调好比实机一遍遍飞要省太多时间和电池。这一点后文会专门讲。3. TECS总能量控制算法让油门管总能量、升降舵管能量分配3.1 为什么固定翼不像多旋翼一样直接给油门和俯仰指令很多人第一次接触TECS会有个疑问为什么PX4固定翼不用“高度PID速度PID”这样直观的方案我在第一章已经提过关键在耦合。这里展开讲讲。固定翼在垂直平面内受四个力的影响推力油门产生、阻力、升力机翼产生、重力。升降舵改变的是飞机的迎角和俯仰姿态这一举动会同时影响升力和阻力。举个例子你想维持高度但发现高度偏低于是拉杆抬头。迎角增加升力确实增大但阻力也跟着增大阻力增大导致速度下降。速度下降又导致升力下降——结果就是你拉了一杆高度短暂上升之后反而可能往下掉。这就是典型的“用升降舵管高度”的副作用。反过来如果你只靠油门管速度加油门速度上升但升力也随速度上升而增加飞机会爬升爬升本身又会消耗能量速度又掉下来。把这两个回路叠加起来就是油门和升降舵在两个PID之间互相“打架”表现出来就是油门忽大忽小、俯仰一直在修正。TECS的思路是换个坐标系不直接管“高度”和“速度”而是管“总能量”和“能量分配”。飞机在垂直平面内的机械能分两部分动能和速度平方成正比和势能和高度成正比。油门主要改变总能量因为推力做功会同时影响动能和势能升降舵主要改变能量分配因为抬头会牺牲一部分速度来换取高度低头则反过来。这样就把耦合的系统变成了两个独立的控制通道。3.2 TECS的数学模型与PX4代码实现TECS里的公式核心是定义“比能量”用单位重力的能量这样好跟高度直接比较比总能EE h V^2 / (2g)比能量分配DD h - V^2 / (2g)E代表飞机作为整体的能量水平由油门控制D代表能量在势能高度和动能速度之间的分配由升降舵控制。在实际控制里TECS计算两组误差一组是期望比总能和当前比总能之差对应油门一组是期望能量分配和当前能量分配之差对应俯仰。再经过PI控制器、前馈补偿、限幅之后输出油门指令和俯仰角指令。下面是简化后的实现逻辑// 基于PX4 src/lib/tecs/tecs.cpp逻辑的简化解析 void TECS::update(float h_est, float h_sp, float v_est, float v_sp) { const float g 9.81f; // 1. 比总能误差 // 注意这里用空速v_est不是地速 float sep (v_est * v_est - v_sp * v_sp) / (2.0f * g) (h_est - h_sp); // sep 0 表示总能量低了需要加大油门 // 2. 能量分配误差 float sed (h_est - h_sp) - (v_est * v_est - v_sp * v_sp) / (2.0f * g); // sed 0 表示势能相对不足需要抬头 // 3. 通过PI控制器得到油门和俯仰输出 // 前馈项按SEP_dot能量变化率估算可以补偿大爬升/下降 _throttle_sp _throttle_ff _sep_pi.pid(sep); _pitch_sp _pitch_ff _sed_pi.pid(sed); }真实PX4实现比我这里写的复杂得多比如它对误差做了归一化、加入了爬升率/加速度前馈SEP_dot、S_dot、有限幅、有积分抗饱和、还有速度权重在能量分配里的融合。但核心骨架就是上面这段总能量管油门能量分配管俯仰。这里有个细节必须强调TECS用的是空速不是地速。因为动能要的是飞机相对空气的速度而不是相对地面。这也就是为什么固定翼必须装空速计而且空速计校准质量直接决定TECS能不能正常工作。你可以没有空速计飞手控但开了自动油门之后空速计不准TECS会把油门和俯仰算到天上去。3.3 TECS参数表与常见误区PX4里TECS相关参数很多但真正需要手动动的其实就那么几个参数名作用推荐初始值调参影响FW_T_SPDWEIGHT速度与高度之间的权重2.0越大越重视速度保持越小越重视高度保持FW_T_HGT_OMEGA高度回路自然频率1.0越大高度响应越快过大容易振荡FW_T_SPD_OMEGA速度回路自然频率2.0越大速度响应越快过大油门动作剧烈FW_T_CLMB_MAX最大爬升率10 m/s限制爬升阶段速度保护失速FW_T_SINK_MAX最大下沉率3 m/s限制下降阶段速度避免俯冲FW_T_THR_DAMP油门阻尼0.2抑制油门震荡FW_T_INTEG_GAIN积分增益0.1消除稳态误差过大会超调新手调TECS时最常见的三个误区误区一把TECS当“高度PID”来理解。如果你发现高度控制不好第一反应是调FW_T_HGT_OMEGA没错但要注意它和FW_T_SPDWEIGHT是联合作用的。SPDWEIGHT越大飞机越倾向于用速度变化来吸收能量扰动高度响应就显得“钝”。误区二忽略姿态内环。TECS输出的是俯仰角指令底层姿态控制器负责把俯仰角飞出来。如果姿态控制器的俯仰通道没调好TECS再准也没用。调TECS之前必须确保手动定高模式下姿态跟踪干净利落。误区三在没校准空速的情况下调TECS。空速不准能量计算全错。我见过有人花了很长时间调TECS调到最后发现是动压传感器标定偏了换成校准好的空速后再看默认参数都很好用。4. 仿真验证与实机调参从SITL到真实飞行的完整流程4.1 SITL仿真环境与传感器仿真配置在实机飞行之前我强烈建议先在SITL里把算法跑通。PX4的SITL软件在环仿真不只是“飞机在一个三维空间里飞”它对传感器也做了仿真IMU的加速度/角速度、气压计的海拔换算、GPS的位置和速度、空速管的动压、以及磁力计数据都有对应的仿真模型。这些传感器仿真数据会被原封不动地送进PX4的估计器和控制链路所以你在地面站里看到的控制行为和实机非常接近。启动固定翼SITL的方式很简单# 编译并启动固定翼SITL仿真模型可选Gazebo或JSBSim make px4_sitl gazebo-plane也可以指定JSBSim模型PX4对固定翼的JSBSim支持很不错make px4_sitl jsbsim启动之后在QGroundControl里连接规划一条包含直线和转弯的航线然后切换自动模式观察飞行日志。重点观察横向跟踪误差、高度跟踪、空速保持、油门变化率。SITL里训练手感有一个额外好处你可以放心大胆地把参数改成很夸张的值看飞机怎么失控、怎么振荡然后再总结规律。这些经验在实机上你是舍不得试的一次炸机可能就是几百块。4.2 如何读日志判断控制质量判断L1和TECS好不好不能只看“飞机最后有没有飞到航线附近”要看动态过程。推荐用PX4自带的Flight Review飞行日志分析网页工具也可以把ulog导入到PlotJuggler里自己画曲线。我重点关注四条曲线横向跟踪误差期望航迹与飞机实际位置的垂直距离。理想情况是平滑收敛到接近0不能出现等幅振荡。高度误差和空速误差TECS的两个核心控制量。看误差是快速收敛还是有周期性波动。油门指令与实际油门如果油门指令是一条剧烈锯齿波说明TECS在“抖”需要查看速度权重或阻尼参数。俯仰角指令与实际俯仰角如果指令跟实际总是差半拍说明姿态内环响应太慢要先调内环。我的判断标准很简单高度误差在爬升/下降阶段允许大一点但平飞阶段应该在正负1米到2米以内空速误差在正负1m/s左右油门曲线在平飞时应该接近一条平直线而不是高频摆动。4.3 一套实用的调参顺序我自己的固定翼调参顺序踩过很多坑之后固定成下面这套先调姿态内环。手动模式下打满副翼、升降舵观察滚转和俯仰响应。用手动定高高度保持模式飞确认俯仰通道干净。这一步没做好后面L1和TECS怎么调都别扭。再调TECS纵向。在自动模式下设定固定高度和空速给一个高度阶跃比如切一个100米高的航线观察高度和空速如何收敛。高度超调大就适当减小FW_T_HGT_OMEGA速度掉太多就增大FW_T_SPDWEIGHT。再调L1横向。先用直线航线看横向跟踪误差。然后画一个带90度弯的小型航线看在转弯处会不会切弯过度。最后做综合测试。加上迎风、侧风条件在SITL里把风速设置到5到8米每秒观察航线偏移量。如果偏移很大检查L1和TECS的配合而不是单独调某一个。这套顺序的核心思想是先让内环可靠再调外环先保证纵向稳定再修横向精度。反过来调你永远不知道问题是出在内环还是外环。5. 实机飞行中常见问题与排查笔记5.1 航线切弯转向掉高度的处理现象飞机在自动模式下飞航线一到转弯点就掉高度有时候能掉一二十米过了转弯之后又慢慢爬回来。原因分析转弯时飞机倾斜升力垂直分量减小如果不额外增加总能量高度就会掉。TECS理论上能预测这一点但它需要看到“能量变化趋势”才能提前补偿。如果你发现掉高度比较明显大概率是两个原因一个是FW_T_SPDWEIGHT设置得太大TECS优先保住空速宁可掉高度也不牺牲速度另一个是姿态内环滚转速率太慢导致进入转弯时建立坡度的时间太长飞机长时间处于“既不保持高度也不加速转弯”的悬停状态。处理步骤先在日志里对比空速和高度曲线确认到底是“速度没掉但高度掉了”还是“速度和高度一起掉”。前者调TECS后者查姿态内环。如果确实是速度优先导致的可以微调减小FW_T_SPDWEIGHT让TECS在转弯时允许一点速度损失来换高度。同时在SITL里把同样的航线跑一遍观察是否复现。5.2 速度保持不住、油门振荡的排查现象自动油门模式下空速一直在目标值附近来回摆油门指令像锯齿一样来回抽飞一会儿整个飞机的俯仰也在跟着抖。原因分析第一嫌疑是空速计校准或者安装位置问题空速值本身有振荡。第二嫌疑是TECS的速度回路自然频率FW_T_SPD_OMEGA设得偏高导致油门对速度误差过度响应。第三是FW_T_THR_DAMP不足油门阻尼小控制量来回冲。处理步骤首先在地面先看空速在地面静止时是否跳动然后用PX4的空速校准流程重新校准。接着把FW_T_SPD_OMEGA从默认值往下调到1.5左右FW_T_THR_DAMP往上加到0.3左右在SITL里验证油门曲线是否变平滑。如果油门平滑了但速度保持变差再适当回调FW_T_SPD_OMEGA。这里最关键的一点是每次只能改一个参数改完在仿真里测试再改下一个。5.3 迎风/侧风下的航线偏移纠正现象航线规划的是直线飞机在无风时飞得很好但遇到侧风时整个航线被“吹”到下风侧横向跟踪误差稳定在一个非零值。原因分析L1用的是地速理论上能抵消恒定风的影响。但PX4的地速来自GPS如果GPS更新频率或者速度解算有延迟或者FW_L1_PERIOD设置过大导致前视太远飞机就可能长期处于“被风吹着走但修正滞后”的状态。处理步骤在SITL里把风速设置成固定侧风观察横向跟踪误差。如果误差是稳定的偏置考虑减小FW_L1_PERIOD让前视距离更近一点修正更主动。如果误差是振荡的考虑增大FW_L1_DAMPING让修正更平滑。另外检查GPS速度信号是否有明显滞后必要时在EKF参数里调整GPS速度测量噪声不过这步比较进阶新手优先调L1参数就好。如果你在实机上遇到类似问题记住一个原则优先保证飞行安全。L1和TECS的参数都可以慢慢调但如果在自动模式里飞机姿态开始出现不可控趋势第一时间切回手动不要恋战。我在实机上吃过亏当时总想着“再观察几秒看它自己能不能修正”结果差点让飞机飘到危险区域。关于L1和TECS的实战经验最后再分享一点体会这两个算法单独理解都不难难的是把它们放在“内环姿态外环位置导航逻辑”的整体链条里理解。我见过很多人把自己埋在参数堆里今天调FW_L1_PERIOD明天调FW_T_SPDWEIGHT结果发现越调越乱。先花一个晚上把PX4控制链路的数据流图画出来想清楚每个环节的输入输出再动手调参数效率会高很多。固定翼的乐趣在于它能给你完全不同于多旋翼的飞行手感而掌握L1和TECS就是打开这扇门的第一把钥匙。