3. 导航控制器：解析L1导航算法，理解固定翼如何‘看’向目标点并生成横向加速度指令
各位同学，欢迎来到第三讲。
上一章我们聊了TECS，那是固定翼的“油门和升降舵”的配合艺术。今天咱们来拆解另一个核心模块——导航控制器。说白了，就是飞机怎么“看”向目标点，然后自己决定该往哪边拐弯。
在ArduPilot里，固定翼的横向导航用的是L1导航算法。这个名字听起来挺唬人，其实核心思想很简单：让飞机像被一根虚拟的橡皮筋拉着，始终指向目标航线。我当年第一次看到这个算法时，心里想的是：“这不就是高中物理里的向心力公式吗？”嗯，还真就是那么回事。
3.1 为什么不用纯PID跟踪航线？
你可能会问：“直接用PID控制航向角偏差不行吗？”
行，但效果不好。我早期在调试一个飞翼时试过纯PID跟踪，结果飞机在航线附近来回震荡，像喝醉了酒。原因很简单：PID只盯着当前误差，没有“前瞻性”。飞机快到目标点了才开始猛打舵，必然过冲。
L1算法高明在哪？它引入了一个虚拟目标点，这个点永远在航线前方。飞机不是追着当前点跑，而是追着“未来的自己”该在的位置。你想想看，这就像开车时你盯着远处弯道的切点，而不是眼前的路沿石。
3.2 L1算法的核心逻辑
咱们直接上干货。L1算法的输入只有两个：
- 飞机当前位置与目标航线的侧偏距（cross-track error）
- 飞机当前的地速向量（ground speed vector）
输出也只有一个：横向加速度指令（lateral acceleration command）。
算法流程分三步：
- 在航线上找一个“L1参考点”——这个点位于航线前方，距离飞机约等于L1距离（通常取飞机当前地速的某个倍数，比如2秒的飞行距离）。
- 计算飞机到L1参考点的视线向量——说白了就是“飞机当前指向L1点的方向”。
- 用这个视线向量与飞机速度向量的夹角，生成横向加速度——公式很简单：
a_cmd = 2 * V^2 / L1 * sin(η)，其中η就是那个夹角。
嗯，这里要注意：V是地速，不是空速。为什么？因为横向加速度是相对于地面的，风的影响必须考虑进去。我见过有人直接用空速算，结果大侧风时飞机拐弯半径完全不对。
核心公式：
a_cmd = 2 * V^2 / L1 * sin(η)其中：
V：地速（m/s）L1：前瞻距离（m），通常取V * 2秒η：飞机速度向量与视线向量的夹角（rad）
3.3 参数调优：L1距离怎么设？
这是实际工程中最头疼的问题。L1距离设大了，飞机拐弯很“肉”，响应慢；设小了，飞机像惊弓之鸟，来回摆动。
我个人的习惯是：先按2秒地速设一个基础值。比如巡航速度15m/s，L1就设30米。然后看实际飞行效果：
- 如果飞机在航线附近有轻微震荡，说明L1偏小，适当增大10%~20%。
- 如果飞机过弯后回正很慢，说明L1偏大，减小10%~20%。
另外，ArduPilot里还有一个L1周期参数（L1_PERIOD），它控制着L1距离与速度的比例关系。默认是20秒，我建议不要轻易改。我曾经在一个小翼展的飞翼上试过改成15秒，结果飞机在顺风转弯时直接螺旋了——因为顺风时地速突然增大，L1距离跟着变大，但转弯半径没跟上。
避坑指南：
我曾经在调试一架2米翼展的飞翼时，发现飞机在航线末端总是“甩尾”。查了半天，原来是L1距离设得太小，导致飞机在接近目标点时提前转弯。后来我把L1距离从1.5秒改到2.5秒，问题解决。记住：L1距离不是越小越好，它决定了飞机的“预判能力”。
3.4 代码层面的实现
咱们来看看ArduPilot里L1算法的核心代码（简化版）：
// 计算L1参考点位置
Vector2f L1_point = get_L1_reference_point(segment, aircraft_pos);
// 计算视线向量
Vector2f sight_vector = L1_point - aircraft_pos;
// 计算速度向量与视线向量的夹角
float eta = wrap_PI(atan2(sight_vector) - heading);
// 计算横向加速度
float V = ground_speed;
float L1_distance = V * L1_period; // L1_period 默认20秒
float a_cmd = 2.0f * V * V / L1_distance * sinf(eta);
// 限制最大加速度（防止舵面饱和）
a_cmd = constrain_float(a_cmd, -max_lat_accel, max_lat_accel);你看，核心代码就这么几行。但实际工程中，边界情况处理才是大头：
- 飞机刚起飞时地速很小，L1距离会变得非常短，这时候要加一个最小L1距离限制（我一般设10米）。
- 当飞机正好在航线上时，η=0，横向加速度为0，飞机直飞。但如果此时有侧风，飞机会被吹偏，所以L1算法天然有抗风能力——因为地速向量已经包含了风的影响。
3.5 用SVG画一张L1算法逻辑图
光说可能不够直观，我画了一张图帮你理解整个流程：
这张图里，飞机已经偏离了航线（有侧偏距）。L1参考点选在航线前方，飞机需要调整航向，让速度向量指向L1点。夹角η越大，需要的横向加速度就越大。
3.6 实际飞行中的表现
L1算法在ArduPilot里已经非常成熟了。我测试过从1.5kg的小飞翼到25kg的测绘机，效果都很稳定。但有几个场景需要特别注意：
| 场景 | 现象 | 解决方法 |
|---|---|---|
| 大侧风飞行 | 飞机“漂移”严重，航线跟踪误差大 | 增大L1_PERIOD（如25秒），让飞机更“前瞻” |
| 顺风转弯 | 转弯半径突然变大，容易冲出航线 | 限制最大横向加速度，或减小L1_PERIOD |
| 低速飞行（如着陆） | L1距离过小，飞机抖动 | 设置最小L1距离（如10米） |
警告：
千万不要在飞行中动态调整L1_PERIOD参数！我见过有人想通过GCS实时改参数来优化转弯效果，结果飞机在转弯时参数突变，直接进入震荡模式。正确的做法是：先在地面仿真，或者用SITL（软件在环仿真）验证参数，再上真机。
3.7 小结
L1导航算法的精髓，就是用一个虚拟的前瞻点，让飞机始终“看向”未来。它不需要复杂的路径规划，也不需要精确的模型参数，靠一个简单的几何关系就能实现平滑的航线跟踪。
我个人觉得，L1算法是ArduPilot里最优雅的设计之一。它把复杂的导航问题，简化成了一个高中物理公式。你想想看，一个公式就能让飞机在风中稳稳地飞出一条直线，是不是很神奇？
嗯，今天就到这里。下一章我们聊聊更进阶的内容——如何用L1算法实现“绕圈”和“盘旋”模式。到时候你会发现，同一个算法，换个参数就能玩出不同花样。