PX4飞控L1制导律：从航点追踪到航向保持的实战解析
1. L1制导律的前世今生：从导弹到无人机
我第一次接触L1制导律是在调试一架固定翼无人机时。当时飞机在转弯时总是"画龙"，就像新手司机在高速公路上不断修正方向一样。后来在PX4飞控参数列表里发现了一个叫FW_L1_PERIOD的神秘参数，这才打开了L1制导律的大门。
L1制导律最初是为导弹设计的，它的核心思想非常简单：让飞行器始终朝着未来某个时间点的预测位置飞行。这个"未来时间点"就是L1距离，相当于汽车驾驶中的"视线引导"——老司机不会盯着车头看，而是看着远方道路来打方向盘。
在PX4飞控中，L1算法主要用在两个场景：
- navigate_waypoints：让飞机沿着预设的航点A→B→C飞行
- navigate_heading：保持固定航向飞行（比如测绘任务中的直线飞行）
实测下来，L1比传统的PID控制更适合固定翼，因为它考虑了飞机的动力学特性。就像骑自行车时，你不会等到偏离路线才转向，而是提前预判转弯幅度。
2. 航点追踪的魔法：navigate_waypoints详解
2.1 航点间的三种位置关系
假设现在要让飞机从航点A飞向航点B，L1算法会根据飞机当前位置动态计算参考点。这里有个关键概念：L1距离（由FW_L1_PERIOD参数决定，默认20秒），可以理解为"预瞄距离"。
飞机相对于航段AB的位置有三种情况：
正常追踪状态：飞机位于航段AB的起始区域
// PX4源码片段（简化版） if (distance_plane_to_AB < L1_distance) { reference_point = 沿AB方向距离飞机L1_length的点; }此时参考点(reference_point)会放在航段AB的延长线上，距离飞机正好一个L1距离。就像开车时看着前方100米的路面调整方向。
航点切换状态：飞机接近航点B
else if (distance_to_B < L1_distance) { reference_point = B; }当距离下一个航点不足L1距离时，直接以航点B作为参考。相当于快到路口时，司机开始盯着路口中心点准备转弯。
错过航点状态：飞机飞过了航段AB
else { reference_point = B; // 紧急修正 _circle_mode = true; // 进入盘旋模式 }如果因为风扰等原因错过航段，飞控会命令飞机绕圈返回。这个逻辑我在野外测试时深有体会——有次强侧风导致飞机偏离航线，就看到它自动画了个优美的弧线重新接上航线。
2.2 横向加速度计算
确定了参考点后，算法会计算需要的横向加速度：
a_{lat} = 2 \cdot V^2 \cdot \frac{\sin(\eta)}{L1}其中：
- V是空速（注意不是地速！）
- η是飞机当前航向与参考点连线的夹角
- L1是前文说的预瞄距离
这个公式的物理意义很直观：偏差角越大、速度越快，需要的转弯力度就越大。PX4会将这个加速度转换为滚转角指令：
roll_setpoint = atan2(a_lat, 9.81) * (180/M_PI); // 转换为角度这里有个实战经验：FW_L1_PERIOD参数需要根据飞机性能调整。我的轻型测绘机用15秒，而重型货运机需要25秒。值太小会导致飞机频繁摇摆，太大则反应迟钝。
3. 航向保持的秘诀：navigate_heading解析
当执行测绘等需要保持固定航向的任务时，navigate_heading函数就开始发挥作用了。虽然原理类似，但实现更简单：
// 伪代码表示逻辑 Vector2f reference_point = 当前位置 + L1_distance * 目标航向向量;这里L1算法会虚拟一个永远在前方的航点。我常用这个模式做电力巡线，即使有侧风干扰，飞机也能像被磁铁吸引一样牢牢咬住航线。
有个容易忽略的细节：当航向改变时（比如从90°转到180°），PX4会平滑过渡：
// 在commander模块中 heading_setpoint = wrap_pi(heading_setpoint + delta_angle * 0.05); // 每次只转5%的差值这种渐进式调整避免了急转弯。有次我直接修改目标航向90度，结果飞机像醉汉一样晃晃悠悠转了半分钟，后来才发现是这个过渡逻辑在起作用。
4. 调参实战：让L1算法发挥最佳性能
4.1 关键参数表
| 参数名 | 默认值 | 作用 | 调整建议 |
|---|---|---|---|
| FW_L1_PERIOD | 20秒 | L1预瞄时间 | 轻型机15-18秒，重型机22-25秒 |
| FW_L1_DAMPING | 0.75 | 阻尼系数 | 风大时增至0.8-0.9 |
| FW_R_LL | 0.9 | 滚转时间常数 | 通常不需修改 |
| FW_L1_SKIP_PERIOD | 10秒 | 航点跳过超时 | 复杂地形可缩短 |
4.2 调试技巧
画圆测试：设置4个构成正方形的航点，观察转弯轨迹。理想的轨迹应该是平滑的弧线，如果出现棱角说明L1_PERIOD需要增大。
风扰测试：在侧风条件下观察航向保持效果。如果飞机像螃蟹一样斜着飞，需要适当增加L1_DAMPING。
空速校准：L1算法依赖真实空速。有次我的空速管堵塞导致算法误判，飞机不断"画8字"。现在每次起飞前都会用手持风速仪校准。
日志分析：重点看
vehicle_attitude_setpoint消息中的roll_setpoint变化曲线。健康的L1控制应该是平滑的正弦波，如果出现锯齿状说明需要调整参数。
记得有次帮客户调试农药喷洒机，发现转弯时药液洒布不均匀。最后把L1_PERIOD从20调到16秒，同时将roll_rate_limit放宽到60度/秒，问题完美解决。这种细节只有实际踩过坑才能体会。
5. 进阶应用：特殊场景处理
5.1 山地飞行
在起伏地形中，单纯的L1控制可能导致飞机撞山。这时需要配合FW_ALT_MODE参数：
- 设置为0（默认）：保持相对高度
- 设置为1：保持绝对海拔高度
我在阿尔卑斯山做冰川监测时，就遇到过飞机在下降段突然拉起的惊险情况。后来发现是因为默认模式会维持与地面的相对高度，当飞越悬崖时突然"看到"地面高度骤降，导致紧急爬升。改用绝对高度模式后问题消失。
5.2 编队飞行
用L1实现多机编队是个有趣的应用。僚机可以把长机的轨迹点作为虚拟航点：
// 伪代码 if (is_wingman) { waypoint_A = leader_position - offset; waypoint_B = leader_position + leader_velocity * L1_period - offset; }实测时需要特别注意：
- 每架飞机的
FW_L1_PERIOD必须完全一致 - 建议关闭GPS更新延迟补偿（
EKF2_GPS_DELAY=0） - 通信延迟超过200ms时建议改用其他编队算法
5.3 应急处理
当GPS信号丢失时，L1算法会自动退化到航向保持模式。但这里有个隐患：如果丢失前飞机正在转弯，初始的heading_setpoint可能是斜向的。我的应急方案是：
param set EKF2_MAG_CHECK 0 # 禁用磁罗盘检查 param set COM_POS_FS_DELAY 5 # 延长位置失效判定时间这些经验都是用炸机换来的。最惨的一次是在峡谷中GPS信号被遮挡，飞机进入应急模式后保持错误航向直撞山壁。现在我的所有机型都加装了激光雷达避障。