APM的L1制导算法改装思路

首先请先阅读参考的官方说明网页及其中所引用的文献，APM的L1制导，文献即那篇A New Nonlinear Guidance Logic for Trajactory Tracking,（如果不是很想看英语，在网上甚至可以找到好心人的中文翻译版）

可以看到在APM的官方说明中，其对L1算法进行了更改，那么为什么这么改呢？原因如下

本来a_s_cmd = 2 * V_K_ABS^2 / L1 * sin(η)    （文献p2）

由文献p5页，在跟直线的时候，如果不管内环动力学，状态量取侧偏距d，ddot，则其为阻尼比0.707的一个二阶系统

这样就有可能出现不爽的情况，我们跟踪航线的时候总要超调一下，而且如果内环姿态动力学不是重阻尼的，则整个航迹跟踪系统的阻尼还要减小，为此，将制导算法改装一下：

K = 4 * xi^2; % xi为阻尼比

a_s_cmd = K * V_K_ABS^2 / L1 * sin(η)， a_s_cmd为侧向加速度指令

当xi = 0.707时此式与原始式一样，

进一步，如果想将时间常数T作为算法输入， 由Omega = 2 * pi / T

则L1 = T * xi * V_K_ABS / pi

这样一改动，跟航点就可以选择自己喜爱的阻尼了！ 可以做到无超调

但是要注意，如果做了这种更改，那么飞盘旋的时候给的向心加速度就不对了，将阻尼比不设为0.707，将可能无法绕给定半径盘旋，一种方法是盘旋的时候把阻尼比改回0.707，APM采用了另一种方法，其在RTL, LOITER,GUIDED这三种情况下不使用L1制导，而是采用了PD控制器