L1导航算法是非常经典的非线性无人机路径跟随算法，最早由MIT于2004年提出，论文为《A New Nonlinear Guidance Logic for Trajectory Tracking》，其导航算法中是先选点，生成一段为L1的路径，最近学习了一下算法和代码，做一下总结，图源论文。
L1路径跟随算法的基本思想就是在期望轨迹上选择一个参考点，并且用这个产生一个横向的加速度，加速度表示为，
a
s
c
m
d
=
2
V
2
L
1
sin
η
{{a}_{{{s}_{cmd}}}}=2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \eta
ascmd=2L1V2sinη
固定翼无人机在横向加速度作用下飞圆弧接近期望轨迹，公式很容易推导，可以看出，加速度与当前空速、空速与L1期望点夹角，以及无人机与L1期望点之间的距离有关。空速可以观测，故横向加速度的求解主要是要确定L1长度，求解
η
\eta
η角。
对于直线期望路径，如下图所示，所求夹角可以表示为，
sin
η
=
η
1
+
η
2
η
1
≈
d
L
1
η
2
≈
d
˙
V
a
s
c
m
d
=
2
V
2
L
1
sin
η
≈
2
V
L
1
(
d
˙
+
V
L
1
d
)
\sin \eta ={{\eta }_{1}}+{{\eta }_{2}} \\ {{\eta }_{1}}\approx \frac{d}{{{L}_{1}}} \\ {{\eta }_{2}}\approx \frac{{\dot{d}}}{V} \\ {{a}_{{{s}_{cmd}}}}=2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \eta \approx 2\frac{V}{{{L}_{1}}}\left( \dot{d}+\frac{V}{{{L}_{1}}}d \right)
sinη=η1+η2η1≈L1dη2≈Vd˙ascmd=2L1V2sinη≈2L1V(d˙+L1Vd)
sin
η
3
≈
L
1
2
R
c
≡
cos
η
3
≈
1
−
(
L
1
2
R
)
2
η
1
≈
d
L
1
cos
η
3
η
2
≈
d
˙
V
\sin {{\eta }_{3}}\approx \frac{{{L}_{1}}}{2R} \\ c\equiv \cos {{\eta }_{3}}\approx \sqrt{1-{{\left( \frac{{{L}_{1}}}{2R} \right)}^{2}}} \\ {{\eta }_{1}}\approx \frac{d}{{{L}_{1}}}\cos {{\eta }_{3}} \\ {{\eta }_{2}}\approx \frac{{\dot{d}}}{V}
sinη3≈2RL1c≡cosη3≈1−(2RL1)2
η1≈L1dcosη3η2≈Vd˙
圆弧期望加速度求解主要是几何上的一些推导，具体可以看论文中的推导，主要涉及三个角度空速与该点切线的夹角
η
2
\eta_{2}
η2，该点到L1连线与L1对应弦的夹角
η
1
\eta_{1}
η1，
2
η
3
2\eta_{3}
2η3相当于是L1所对应于的圆心角。
a
s
c
m
d
=
2
V
2
L
1
sin
η
=
2
V
2
L
1
sin
(
η
1
+
η
2
+
η
3
)
=
2
V
2
L
1
sin
(
η
1
+
η
2
+
η
3
)
≈
2
V
2
L
1
(
η
1
cos
η
3
+
η
2
cos
η
3
+
sin
η
3
)
≈
2
V
2
L
1
2
c
2
d
+
2
V
L
1
c
d
˙
+
V
2
R
{{a}_{{{s}_{cmd}}}}=2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \eta =2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \left( {{\eta }_{1}}+{{\eta }_{2}}+{{\eta }_{3}} \right) \\ =2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \left( {{\eta }_{1}}+{{\eta }_{2}}+{{\eta }_{3}} \right) \\ \approx 2\frac{{{V}^{2}}}{{{L}_{1}}}\left( {{\eta }_{1}}\cos {{\eta }_{3}}+{{\eta }_{2}}\cos {{\eta }_{3}}+\sin {{\eta }_{3}} \right) \\ \approx 2\frac{{{V}^{2}}}{{{L}_{1}}^{2}}{{c}^{2}}d+2\frac{V}{{{L}_{1}}}c\dot{d}+\frac{{{V}^{2}}}{R}
ascmd=2L1V2sinη=2L1V2sin(η1+η2+η3)=2L1V2sin(η1+η2+η3)≈2L1V2(η1cosη3+η2cosη3+sinη3)≈2L12V2c2d+2L1Vcd˙+RV2
（1）L1轨迹跟踪是非线性的导航算法，在实际的使用中，主要就是要解决L1的取值问题和
sin
η
\sin \eta
sinη的求解，因为是基于角度正弦的，所以相比于基于偏距的线性算法，在初始偏差比较大的时候，不会出现过激调节，可以比较平滑的向期望路径过渡，而在进入期望路径后，其控制效果较偏距控制更好，尤其是在圆形轨迹跟踪。
（2）L1轨迹跟踪相当于在跟踪一个L1点，这个点的轨迹就是期望路径，点按一定频率刷新，按一定频率计算角度，刷新侧向加速度输出。
（3）对
sin
η
\sin \eta
sinη进行小角度线性化，可以得到在期望路径附近的加速度线性化的形式，以跟踪直线为例，实际上就是一个二阶系统，
a
s
c
m
d
≈
d
¨
{{a}_{{{s}_{cmd}}}}\approx \ddot{d}
ascmd≈d¨，
d
¨
+
2
ζ
ω
n
d
˙
+
ω
n
2
d
=
0
ζ
=
0.707
ω
n
=
2
V
L
1
\ddot{d}+2\zeta {{\omega }_{n}}\dot{d}+{{\omega }_{n}}^{2}d=0 \\ \zeta =0.707 \\ {{\omega }_{n}}=\frac{\sqrt{2}V}{{{L}_{1}}}
d¨+2ζωnd˙+ωn2d=0ζ=0.707ωn=L12
V
这个二阶系统固有频率和速度以及L1长度有关，阻尼实际上是与那个增益就是那个2倍有关，在论文中，L1取了固定值150m，而V也只测试了25m/s的速度。对于圆的跟踪和直线差不多，只是固有频率也与盘旋半径有关。
L1 Control for Straight and Curved Path Following
（1）改进了L1航点更新，在原论文中L1是定值，改进后L1长度是动态计算的，可以指定一个恒定的跟踪回路周期，并使导航回路增益可以自动调整以适应速度的变化，并增加了两个参数。在ArduPilot中给出的L1更新公式为：L1_dist = 1/π * damping * period * speed ，这样固有频率就变成了1/π/period。
（2）改进了阻尼，在原论文中，近似的二阶系统阻尼是固定的，在ArduPilot中通过修改了加速度的增益，latAccDem = 4 * damping² * speed² * sin(Nu) / L1_dist ，阻尼为1/damping。
关于ArduPilot源码的解读参考了以下两篇博客：
https://blog.csdn.net/u012814946/article/details/79817719
https://blog.csdn.net/u012814946/article/details/81451769
