PX4算法解析（一）：L1制导律

耄耋客 · 发表于 2019-6-22 15:58

一、前沿

从今天开始，会陆续为大家解读开源飞控软件px4中使用到的各种算法，今天要讲述的是L1制导律，这是固定翼飞机进行路径跟踪的重要算法之一。这个算法是由MIT的研究人员提出，因为具有比较好的路径跟踪效果而得到广泛应用。在这之前，无人机在进行路径跟踪时一般采用基于侧偏距的PD控制器方法，至于为什么使用PD而不使用PID算法，那是因为位置环响应较慢，所以一般在外环采用PD算法，而在内环采用PID算法。

二、什么是L1制导律

之前我们在无人机研发什么的文章中提到过制导、导航和控制是无人机开发中三个最为重要的部分，而制导就是告诉我们无人机应该往哪飞。L1制导律就是在期望路径上选取一个参考点，根据这个参考点和当前水平速度计算出水平期望加速度的算法。而L1指的就是期望路径上的参考点与无人机当前水平位置的距离。

上图可以看到，无人机在当前时刻的期望运动是一个半径为R，速度为V的圆周运动。所以在航向控制上也应该给一个相应的期望偏航角速率前馈，否则要等到出现航向角偏差时再进行反馈控制，会影响跟踪效果。

具体情况分析：

在实际飞行中，一般要跟踪的航线都是直线或者圆，因为计算机是离散系统，所以如果是不规则曲线也会进行线性化处理。所以，接下来针对直线路径和圆路径两种情况进行分析。

直线路径：

在进行具体分析之前，我们先进行两个小角度假设，和角度很小，其实本质上是因为d相对于L1较小，相对于V较小，这符合实际飞行中的大部分情况:

根据上图以及加速度的公式，我们可以得出以下结论：

这跟传统的基于侧偏距的PD方法是一样的。但这里的PD参数是跟V和L1同时相关的。速度越大，PD的参数也越大。

圆形路径：

对于圆形路径，同样我们先进行两个小角度假设，和角度很小，

期望加速度为：

代入：

得：

这个跟传统的侧偏距PD算法相比多了一项，而一般情况下我们在使用传统算法时也会在前馈项中加入此项，从而达到更好的控制效果。

三、L1制导律的优势

其实，从上面的推导过程可以看出，L1制导律和传统算法具有很大的相似性，但是其中一个很大的却别是L1算法包含了V的变化情况，总的来讲，相比传统基于侧偏距的PD算法，L1制导律具有三个优势：

1、对于各种不同的路径，不需要单独进行设计，一个公式就可以覆盖全部路径，在实际应用中尤其是代码实现时更加方便。

2、对于初始条件偏差较大的情况，比如侧偏距和侧偏速度较大时可以比较平滑地向期望路径去过渡。

3、跟传统基于侧偏距的PD算法相比，L1算法中会根据V的速度不同，计算出的期望加速度也不同，这对于实际飞行时不同的期望飞行速度以及环境风的干扰都有很好的适应能力。

四、PX4参数的调试

在PX4飞控中，L1对应的参数主要有两个：FW_L1_PERIOD和FW_L1_DAMPING。我们在飞行中主要调试FW_L1_PERIOD，当无人机转弯不够快时把此参数调小，直到飞机出现些许震荡后调大此参数2-3左右即可，至于FW_L1_DAMPING一般在0.65-0.85之间，每次调整0.05找到最好的飞行状态即可。

限于作者水平，如有错误，欢迎指正！

（技术交流请勿涉及联系方式版主留）