【导航算法】无人机路径跟踪L1导航算法

L1导航算法是非常经典的非线性无人机路径跟随算法，最早由MIT于2004年提出，论文为《A New Nonlinear Guidance Logic for Trajectory Tracking》，其导航算法中是先选点，生成一段为L1的路径，最近学习了一下算法和代码，做一下总结，图源论文。

一、直线路径跟踪

L1路径跟随算法的基本思想就是在期望轨迹上选择一个参考点，并且用这个产生一个横向的加速度，加速度表示为，
$$a_{s_{cmd}}=2\frac{V^2}{L_1}\sin \eta$$
固定翼无人机在横向加速度作用下飞圆弧接近期望轨迹，公式很容易推导，可以看出，加速度与当前空速、空速与L1期望点夹角，以及无人机与L1期望点之间的距离有关。空速可以观测，故横向加速度的求解主要是要确定L1长度，求解$\eta$角。
对于直线期望路径，如下图所示，所求夹角可以表示为，

$$\begin{gathered} \sin \eta ={\eta }_1+{\eta }_2 \\ {\eta }_1\approx \frac{d}{L_1} \\ {\eta }_2\approx \frac{\dot{d}}{V} \\ {a}_{s_{cmd}}=2L^{} \end{gathered}$$