Pixhawk学习9——固定翼位置控制（L1控制+TECS总能量控制）

本篇博客本来没有在之前的计划中。但由于最近项目上遇到了固定翼轨迹控制的一些问题，所以就结合pix的程序学习总结了一下。

不同于旋翼，固定翼要实现位置变化必须得有一个轨迹变化过程，因为它不像旋翼那样可以直接调整力的矢量，也不能旋翼那样悬停和直上直下。
Pixhawk中的固定翼位置控制分为两部分：横航向的L1控制和纵向的TECS控制（总能量）。作为普通固定翼的执行机构，分为横滚（副翼）、偏航（方向舵）、俯仰（升降舵）、速度（油门），而这四个量的控制目标值都是由这两个位置控制模块得到的。
代码中，src/modules/fw_pos_control_l1里只进行了各目标姿态角的提取，实际的位置控制代码分别在以下两个文件中：
L1：src/lib/ecl/l1/ecl_l1_pos_controller.cpp
TECS：src/lib/external_lgpl/tecs/tecs.cpp

（本部分的pixhawk代码比较简单，所以就不详解，只是列出来子函数名称和解释。主要是算法推到过程写的比较细一点。）

1.横航向L1控制（得到目标横滚角和目标航向角）

1.1 L1控制公式推导
横航向控制主要是保证让飞机飞到目标点，普遍使用的就是L1控制方法。

如上图，飞机当前点为O，期望航线上的P点为要飞达的点，O与P的连线为L1。对L1做中位线并与飞机速度矢量Vn的垂线交于一点C，飞机与C的距离为R。那么，要想飞向P点，需要绕C点做圆周运动，则此时飞机需要有右滚横滚角以提供向心角速度：

将R用线段L1和 n（yita）（速度矢量和L1矢量的夹角）表示如下：

则侧向加速度为：

上述过程即为L1控制的基本概念。
由此，得到横滚角和偏航角的目标指令为：

1.2 Pixhawk中L1控制流程
在Pixhawk代码中的流程如下：
#ECL_L1_Pos_Controller::navigate_waypoints
根据当前飞机所处的位置，计算nav_bearing，即为导航航向角。
还根据L1控制的逻辑，计算了RollT。

#ECL_L1_Pos_Controller::navigate_loiter
Pix作者根据文献Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013.中的内容对算法进行了改进。具体改进办法我还没有时间看，先留在这里，不过不影响整个程序的逻辑。也是为了计算nav_bearing。
另外，该函数中根据不同的飞机模式（是跟踪点还是盘旋飞行），侧向加速度的计算是不同的。例如盘旋飞行时，侧向加速度是由飞机与盘旋点的距离、盘旋半径、侧向速度的等变量通过简单的PID计算得到的。

#ECL_L1_Pos_Controller::navigate_heading
由target_bearing – current_bearing, 得到目标的航向角值，即YawT。

2.TECS控制（得到目标俯仰角和目标油门）

首先，说一下为什么有总能量控制。固定翼飞机的升降舵和油门杆对飞机的响应具有耦合特性。即升降舵摆动和油门变化都能够影响俯仰角和空速，且在飞机低动压飞行状态下飞行速度和航迹耦合更加剧烈。一般的做法是在高度保持模态下进行速度控制或者在速度保持模态下进行高度控制，但由于缺少升降舵和油门的协同控制，解耦控制效果并不好。因此引入总能量控制。
总能量控制是从能量的角度来进行控制。飞机的能量分为动能（kinetic energy）和势能（potential energy），而这些能量都是由油门增大或者减小产生的，而动能和势能的转换是由升降舵控制飞机的航迹来实现的。从这个角度来讲，总能量/能量差是与油门/升降舵是解耦的。

2.1 总能量控制公式推导
飞机在纵向平面的动力学方程为（mathtype的符号太多，智能截图了）：


可以看出，总能量的变化主要由飞机发动机拉力的变化来控制。
因此，推力的增量正比于总能量的变化率的增量。可以采用以下控制律：

定义势能和动能差的变化率如下：

因此对于升降舵控制，采用以下控制律：

防止系统中不期望的零点引起的控制响应的超调，将比例部分直接采用变化率进行代替。

2.2 Pixhawk中TECS控制流程
在Pixhawk代码中的流程如下：
#TECS::update_state
TECS::_update_speed
TECS::_update_speed_demand
TECS::_update_height_demand
这几个函数更新了当前的空速和高度，以及需要的空速和高度。

#TECS::_update_energies
更新了动能和势能。包括当前状态和需求状态的。

#TECS::_update_throttle
更新油门值。
#TECS::_update_pitch
更新俯仰角指令。