PX4固定翼控制器详解(四)——TECS高度、速度控制器（续）

回顾复习

上一期关于TECS控制器的讲解，我们得到了基础框架下的TECS控制器如下图所示。

总能量、比能量相关量计算方式如下：
$$\{\begin{array}{l}\dot{E}=\frac{\dot{V}}{g}+\gamma \\ \dot{B}=\gamma -\frac{\dot{V}}{g}\\ {\dot{E}}_e=(\frac{{\dot{V}}_{sp}}{g}+{\gamma }_{sp})-(\frac{\dot{V}}{g}+\gamma )\\ {\dot{B}}_e=({\gamma }_{sp}-\frac{{\dot{V}}_{sp}}{g})-(\gamma -\frac{\dot{V}}{g})\end{array}$$
$$\{\begin{array}{l}\gamma =arcsin\frac{-v_d}{\sqrt{v_e^2+v_n^2}}\\ {\gamma }_{sp}=arcsin\frac{{v_{climb}}_{sp}}{\sqrt{v_e^2+v_n^2}}=arcsin\frac{K_h(h_{sp}-h)}{\sqrt{v_e^2+v_n^2}}\\ {\dot{V}}_{sp}=K_a(V_{sp}-V)\end{array}$$

最终的油门和俯仰角输出为：
$$\{\begin{array}{l}{T}_{sp}=\frac{K_{ei}}{s}\dot{E_e}+\frac{K_{ep}}{s}\dot{E}\\ {\theta }_{sp}=\frac{K_{bi}}{s}\dot{B_e}+\frac{K_{bp}}{s}\dot{B}\end{array}$$

这里注意，由比能量数据给出的控制量为俯仰角${\theta }_{sp}$，而非${\delta }_{sp}$。这样的处理方式更为合理，得到俯仰角期望值${\theta }_{sp}$可以输入到姿态控制器中。原论文中在后续章节也采用了类似的处理方式。

改进完善

到这里TECS的基本框架已经有了，我们给出高度控制模式下，完整的操控流程框图。

改进1——滚转输入补偿

考虑到当存在期望滚转角输入后，姿态控制器会根据期望滚转角控制副翼，使得飞行器发生滚转。当存在滚转角时，飞行器阻力增大，总能量会降低，会产生掉高、减速的现象。

这里可以引入部分前馈控制量，根据期望的滚转角，为输出的$T_{sp}$、${\theta }_{sp}$提供前馈补偿。具体的控制原理图如下图所示。

改进2——误差PI控制

上一期文章中我们提到，原始论文对总能量误差${\dot{E}}_e$，比能量误差${\dot{B}}_e$采用的是积分控制。在PX4和APM中，$K_{ep}\dot{E}$和$K_{bp}\dot{B}$被称为前馈项，对于总能量误差${\dot{E}}_e$，比能量误差${\dot{B}}_e$的控制可以改进为PI控制器。

此处使用PI控制器而非PID控制器原因：在PX4、APM控制器中，经过总能量误差${\dot{E}}_e$，比能量误差${\dot{B}}_e$的控制为PI控制，但是其将P值称为阻尼项。而且在笔者测试过程中，如果对${\dot{E}}_e$、${\dot{B}}_e$加D控制，会造成油门和升降舵的剧烈震动。因此，这里仅推荐大家使用PI控制器即可如下图所示。

改进3——高度、速度分配控制

在APM飞控中，有一项参数为TECS_SPDWEIGHT。该参数用于调整俯仰控制对速度与高度误差的权重。将其设置为0.0时，俯仰控制将控制高度而忽略速度误差。这通常会提高高度精度，但空速误差较大。将其设置为2.0将使俯仰控制回路控制速度而忽略高度误差。这通常会减少空速误差，但会增大高度误差。默认值1.0允许俯仰控制同时控制高度和速度。

这项改进，在1983年Lambregts给出的论文[4]中有提及（原论文中图3）。改进后的结构如下图所示。

总得来说，如果对高度控制精度要求更高可以忽略速度误差，可以设置$K=0$；如果对速度控制精度要求更高可以忽略高度误差，可以设置$K=2$。

总结

综上所述，TECS总能量控制至此全部介绍完，下一期文章将介绍L1侧向控制器。

L1侧向控制器用于控制飞行器的前向飞行，在定高模式的基础上，引入L1侧向控制器，最终实现飞行器的自动水平匀速直线飞行。

参考文献及链接

[1]张庆振.QFT/TECS在飞机自动着陆控制中的应用研究[D].西北工业大学,2004.

[2]张庆振,安锦文.一种基于飞机总能量控制飞行速度/航迹的解耦控制系统设计新方法[J].航空学报,2004,(04):389-392.

[3]张庆振,安锦文,刘小刚.基于飞机总能量控制系统(TECS)的飞行航迹/速度解耦控制方法研究[J].西北工业大学学报,2004,(03):384-387.

[4]LAMBREGTS, A. Vertical flight path and speed control autopilot design using total energy principles. In: Guidance and Control Conference. 1983. p. 2239.

[5]VOTH, Christopher; LY, Uy-Loi. Design of a total energy control autopilot using constrained parameter optimization. Journal of Guidance, Control, and Dynamics, 1991, 14.5: 927-935.

[6]WARREN, Anthony. Application of total energy control for high-performance aircraft vertical transitions. Journal of guidance, control, and dynamics, 1991, 14.2: 447-452.

[7]https://docs.px4.io/main/zh/flight_stack/controller_diagrams.html

后续会陆续带来规划、控制相关的文章、仿真分享。

END

迅翼SwiftWing致力于固定翼技术共享，汇聚固定翼领域技术极客，推动固定翼技术持续创新！