PX4源码解读之fw_pos_control_l1

固定翼的位置控制是一个很重要问题,它不同于旋翼的控制,需要对速度和高度进行解耦控制,并且其不能像旋翼那样进行悬停,其转弯的时候有一个转弯半径.本博客不会对源码进行详细的解读,主要是分享一些自己读源码时的资料,自己读的过程中也有注释,想要的同学私信我.话不多说,分享资料!

---

位置控制中的两个算法

在固定翼位置控制中,主要涉及到两个控制算法:L1控制和TECS(total energy control system),这两个算法是干嘛的呢?L1算法在源码中主要是为了得到飞机需要偏转的滚转角和偏航角,其原理主要是控制飞机的横向向心加速度.TECS算法是为了飞机的速度和高度解耦,在源码中主要是得到了期望的油门和俯仰角.通过这两个算法,最终得到了期望的油门和三轴角度(俯仰角,滚转角,偏航角),然后内环的姿态控制就交给了姿态控制器了.

---

L1控制算法

L1算法主要来源于论文“A New Nonlinear Guidance Logic for Trajectory Tracking”,此算法在跟随直线时，相当于一个PD控制器，为了紧密跟随曲线路径，在控制中又加入了预期控制。算法的关键是计算两个值：
（1）计算参考点L1-ref （2）计算横向加速度命令，这里把加速度分解，并且用倾斜角（滚转和偏航）指令进行控制。
Paul Riseborough对L1算法进行了一些修改，也就是我们现在PX4中所用的算法。其主要是通过阻尼和周期动态的计算L1的距离，并且，限制了L1距离为计算的距离和WP半径中的小值。下面主要分享几张确定L1_distance的图片:



相关资料可以参考APM官网:Navigation Tuning
L1算法的实现在ecl/l1文件夹下,主要的两个函数是得到滚转角和偏航角:

/**
 *