PX4 Development: TECS (Fixed Wing Control)

PX4的总能量控制系统(TECS)用于同时控制固定翼飞行器的空速和高度，通过油门和俯仰角的调节实现能量平衡。TECS的性能受俯仰控制环的影响，调试前需确保姿态控制器正常工作。文档中详细介绍了TECS的基本原理、控制回路、实现函数及其在PX4中的应用场景和运行流程。

type
status
date
slug
summary
tags
category
icon
password
更新时间
更新内容
修改备注
2023.05.11
文档创建
ㅤ
ㅤ
ㅤ
ㅤ

文档说明

  1. PX4 VTOL功能参考手册《pixhawkVTOL手册初版.docx》
      2. TECS参数调整参考手册 3.6.5章节 TECS (总功率控制系统) 对速度 & 高度 – 调整指南
  1. PX4 VTOL所需的控制参数参考表格《VTOL试飞步骤及参数设置.xlsx》
  1. PX4 VTOL所需的控制参数设置方法参考表格《四旋翼复合翼VTOL飞机——quadplane通用设置流程.xlsx》

总能量控制系统(TECS)

通过总能量控制系统(TECS)，PX4可以同时对固定翼飞行器的空速和高度进行控制。 这其中的代码通过一个用库（这个库是用于固定翼位置控制模块）作为接口。
notion image
从上面的图表可以看出，TECS 接受空速和高度需求量，然后输出油门和俯仰角控制量。 These two outputs are sent to the fixed-wing attitude controller which implements the attitude control solution. However, the throttle setpoint is passed through if it is finite and if no engine failure was detected. It's therefore important to understand that the performance of TECS is directly affected by the performance of the pitch control loop. A poor tracking of airspeed and altitude is often caused by a poor tracking of the aircraft pitch angle.
在试图调试总能量控制系统 TECS 前，请一定要调试好姿态控制器。 增加飞行器的俯仰角度不仅会导致高度上升还会导致空速下降。
同时控制飞机的空速和高度不是一件简单的事。 增加飞行器的俯仰角会导致高度上升，同时也会导致空速下降。 推力（通过油门控制）增加整个飞机的总能量。 因此，俯仰角和油门两个输入量都会对空速和高度产生影响，从而使控制问题变得难了。
TECS 提供了一种解决方案，即根据能量而不是初始设定值来反映问题。 一架飞行器的总能量是飞行器动能和势能之和。 推力（通过油门控制）可以增加飞机的总能量。 一个给定的总能量状态可以通过势能和动能的任意组合来实现。 换句话说，飞行器在高海拔以低空速飞行和在低海拔以高空速飞行时的总能量是等价的。 我们称这种情况叫做比能量平衡，它是根据当前高度和真实空速设定值计算的。 可以通过控制俯仰角来控制飞行器的比能量平衡。 俯仰角增加将动能转变为势能，俯仰角减少则情况相反。 这样，通过将初始空速和海拔设定值转化为能量大小（空速和海拔存在耦合，而能量大小可以独立控制），就可以把控制问题解耦。 我们利用油门调节飞行器的特定总能量，利用俯仰角来维持势能（高度）和动能（真空速）的特定平衡点。

总能量控制回路

notion image

比能量控制回路

notion image

TECS基本原理

用推力控制总能量变化率，用升降舵构成的俯仰姿态回路控制总能量的分配率。在源码中，主要是通过协调油门和俯仰角要求来控制飞机的高度和速度。简单的来说：在飞行过程中，如果升降舵和油门杆不能有效协同作用,它们对其各自主控制量的控制将受到影响。如在升降舵保持不变的同时，由自动油门改变飞机速度状态，将必然引起飞机飞行航迹的变化而在保持油门不变的同时，通过升降舵改变飞机飞行航迹，将导致飞机飞行速度产生偏差。

源码的基本框图

notion image

TECS实现函数

旧版本PX4，在ecl/tecs文件夹下，主要的实现函数如下：
新版本PX4，TECS更新函数位于src\\modules\\fw_pos_control\\FixedwingPositionControl.cpp文件中
函数定义如下：
Parameters:
control_interval – Time since the last position control update [s]
alt_sp – Altitude setpoint, AMSL [m]
airspeed_sp – Calibrated airspeed setpoint [m/s]
pitch_min_rad – Nominal pitch angle command minimum [rad]
pitch_max_rad – Nominal pitch angle command maximum [rad]
throttle_min – Minimum throttle command [0,1]
throttle_max – Maximum throttle command [0,1]
desired_max_sink_rate – The desired max sink rate commandable when altitude errors are large [m/s]
desired_max_climb_rate – The desired max climb rate commandable when altitude errors are large [m/s]
disable_underspeed_detection – True if underspeed detection should be disabled
hgt_rate_sp – Height rate setpoint [m/s]

TECS控制函数使用场景

PX4为固定翼或VTOL飞行器提供了以下几种控制模式，以下控制函数内部调用TECS函数输出期望的俯仰角以及油门量：
  • control_auto_fixed_bank_alt_hold
  • control_auto_descend
  • control_auto_position
  • control_auto_velocity
  • control_auto_loiter
  • control_auto_takeoff
  • control_auto_landing_straight
  • control_auto_landing_circular
  • control_manual_altitude
  • control_manual_position

TECS控制函数运行流程

PX4固定翼参数定义参考src\\modules\\fw_pos_control\\FixedwingPositionControl.hpp头文件中的DEFINE_PARAMETERS函数
tecs_update_pitch_throttle()函数的运行流程：
  1. 计算距离上次运行tecs（_last_tecs_update）的时间dt，dt最小0.01，更新_last_tecs_update
  1. 更新属性_is_tecs_runing=true，只在滞空时候运行tecs模块，当飞行器为VTOL时，不在旋翼模式/变形模式下运行，（空速太小）通过更新_tecs_is_running这个函数内变量
  1. 如果是VTOL模式的变形模式：
      2. 是：设置属性（_was_in_transition）为true，设置变形后的速度属性（_airspeed_after_transition）为控制的空速
      否：
      如果_was_in_transition为true，
      是：说明已经从VTOL模式改变，期望速度(_airspeed_after_transition)增加（2m/s），设置airspeed_sp，如果速度大于airspeed_sp时候，认为变形完成重置_was_in_transition和_airspeed_after_transition
  1. 如果tecs_is_running为false，设置属性_reinitialize_tecs为true，表示下次运行_tecs需要初始化(initialize())。 根据_reinitialize_tecs的值决定是否初始化_tecs
  1. 设置降落模式下取消失速保护，调用set_detect_underspeed_enabled()函数，如果飞行器处于已经降落状态，则用把当前的空速和高度等数值初始化_tecs
  1. 更新飞行器的TECS状态估计，调用calculateTrimThrottle()函数，计算更新后的平衡油门量
      • 获取标准大气压下，使飞行器以平衡空速进行水平飞行状态所需的油门量(throttle_trim)
      • 计算从空速到油门的映射(slope_below_trim&slope_above_trim)，假设对于不同的梯度均有一个线性关系（throttle/airspeed）；高于或低于平衡量的阈值通过调节FW_THR_ASPD_MIN参数和FW_THR_ASPD_MAX参数决定
      • 如果FW_THR_ASPD_MIN参数大于0，且设定的空速小于FW_AIRSPD_TRIM参数，更新平衡油门数值(throttle_trim)
      • 如果FW_THR_ASPD_MAX参数大于0，且设定的空速大于FW_AIRSPD_TRIM参数，更新平衡油门数值(throttle_trim)
      • 更新飞行器重量比例参数，取决于weight_base参数和weight_gross参数
      • 计算EAS（当量空速）到TAS（真空速）之间的比值，更新油门与大气密度缩放比率(air_density_throttle_scale)
      • 返回飞行器重量和空气密度补偿后的平衡油门量
  1. 更新TECS数据，调用update()函数并传入参数：
      • 根据输入参数更新参考模型参数(_reference_param)和控制模型参数(_control_param)
      • 更新时间间隔(dt)在DT_MIN和DT_MAX之间时：
        • 更新滤波后的空速(_airspeed_filter)，获取滤波后的空速状态(getStates())
        • 更新空速参考模型(_altitude_reference_model)，获取高度参考值(getAltitudeReference())和高度变化率参考值(getHeightRateSetpointDirect())
        • （不推荐使用）若真实空速未知，则根据期望的速度计算期望的真空速(_update_speed_setpoint())
        • 根据之前更新的高度、高度变换率、真空速、真空速速率更新控制输入模型(_control)
        • 设置TECS模式（失速模式ECL_TECS_MODE_UNDERSPEED或正常模式ECL_TECS_MODE_NORMAL）
        • 输出调试信息(_debug_status)
  1. 发布TECS状态数据，调用tecs_status_publish()函数，传入设定的高度，空速，空速速率估计，调整后的油门量
      • 设定TECS状态（正常模式TECS_MODE_NORMAL或失速模式TECS_MODE_UNDERSPEED）
      • 使用libevent事件库发布TECS状态信息(tecs_status)

Reference

Link: PX4 Docs
Reference:
  1. PX4总能量控制（TECS）实现
      2. (279条消息) 总能量控制（TECS）及其在px4中的应用（学习笔记）_tecs控制_hahaha_buaa的博客-CSDN博客
  1. PX4固定翼位置控制实现
      2. (280条消息) px4源码学习五--固定翼位置控制模块_px4 landing源码_znr1995的博客-CSDN博客
  1. 飞行原理——几种空速的测量
      2. 飞行原理—几种空速的测量TAS EAS CAS IAS - 知乎 (zhihu.com)
  1. PX4源码解读之fw_pos_control_l1
      2. (280条消息) PX4源码解读之fw_pos_control_l1_小方爱自律的博客-CSDN博客
Loading...