APM学习（8）：TECS控制（1）

TECS控制是ArduPilot在固定翼高度和速度控制上的核心算法，其核心原理是计算高度和速度的能量平衡，使飞行器的能量平衡保持在期望的数值，从而控制飞机达到期望的高度和速度。这里先介绍TECS控制中的更新函数

1. update_50Hz() 函数

这个函数通过任务管理（AP_Scheduler）中的update_speed_height函数来运行，为Plane的成员函数，在Plane.cpp中对应任务规划的条目如下

cpp 复制代码

    SCHED_TASK(update_speed_height, 50, 200, 12)

表示以50Hz频率运行，最大允许执行时间200微秒，优先级12。

函数Plane::update_speed_height如下

cpp 复制代码

void Plane::update_speed_height(void)
{
    bool should_run_tecs = control_mode->does_auto_throttle();
		...
    if (should_run_tecs) {
	    // Call TECS 50Hz update. Note that we call this regardless of
	    // throttle suppressed, as this needs to be running for
	    // takeoff detection
        TECS_controller.update_50hz();
    }
		...
}

控制模式的does_auto_throttle函数返回是否采用TECS控制，通常自动控制模式（如AUTO，GUIDED等）都采用TECS控制，如果采用TECS控制，则运行TECS控制器的update_50hz函数，通过这个函数更新控制器所需要的高度、速度等信息

cpp 复制代码

void AP_TECS::update_50hz(void)
{
    _ahrs.get_relative_position_D_home(_height);
    _height *= -1.0f;
	...

    // Use inertial nav vertical velocity and height if available
    Vector3f velned;
    if (_ahrs.get_velocity_NED(velned)) {
        // if possible use the EKF vertical velocity
        _climb_rate = -velned.z;
    } else {
		...
    }

    // Update the speed estimate using a 2nd order complementary filter
    _update_speed(DT);
}

在update_50hz函数中通过ahrs获得高度（_height）、爬升速率（_climb_rate）等信息，然后运行_update_speed函数更新速度等其它信息，DT为运行频率的时间间隔

2. _update_speed(...) 函数

函数如下

cpp 复制代码

void AP_TECS::_update_speed(float DT)
{
    // calculate a low pass filtered _vel_dot
    if (_flags.reset) {
        _vdot_filter.reset();
        _vel_dot_lpf = _vel_dot;
    } else {
        // Get DCM
        const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
        // Calculate speed rate of change
        float temp = rotMat.c.x * GRAVITY_MSS + AP::ins().get_accel().x;
        // take 5 point moving average
        _vel_dot = _vdot_filter.apply(temp);
        const float alpha = DT / (DT + timeConstant());
        _vel_dot_lpf = _vel_dot_lpf * (1.0f - alpha) + _vel_dot * alpha;
    }
    ...

前面一个if表示重置后滤波器的初始化，正常情况下（else）函数首先计算飞行器的线性加速度（x轴），通过ins获得的加速度减去重力加速度的分量，然后通过一个平滑滤波器和一个一阶低通滤波器获得平滑后的加速度数值

cpp 复制代码

	...
    bool use_airspeed = _use_synthetic_airspeed_once || _use_synthetic_airspeed.get() || _ahrs.using_airspeed_sensor();

    // Convert equivalent airspeeds to true airspeeds and harmonise limits
    float EAS2TAS = _ahrs.get_EAS2TAS();
    _TAS_dem = _EAS_dem * EAS2TAS;
    if (_flags.reset || !use_airspeed) {
        _TASmax = aparm.airspeed_max * EAS2TAS;
    } else if (_thr_clip_status == clipStatus::MAX) {
        // wind down airspeed upper limit  to prevent a situation where the aircraft can't climb
        // at the maximum speed
        const float velRateMin = 0.5f * _STEdot_min / MAX(_TAS_state, aparm.airspeed_min * EAS2TAS);
        _TASmax += _DT * velRateMin;
        _TASmax = MAX(_TASmax, aparm.airspeed_cruise * EAS2TAS);
    } else {
        // wind airspeed upper limit back to parameter defined value
        const float velRateMax = 0.5f * _STEdot_max / MAX(_TAS_state, aparm.airspeed_min * EAS2TAS);
        _TASmax += _DT * velRateMax;
    }
    _TASmax   = MIN(_TASmax, aparm.airspeed_max * EAS2TAS);
    ...

然后计算_TASmax和_TASmin，分别为控制空速的最大值和最小值，用于后面控制设定的最大和最小空速值。这里的控制空速都是指真实空速（True Air Speed），所以要通过一个因子对当量空速（空速计测得的速度）进行转换。_TASmax的最大上限为全局参数中的最大空速（aparm.airspeed_max）乘以转换因子，如果油门饱和时，最大控制空速不断下降，最低到标称巡航速度，不然的话最大控制空速不断上升，但是不超过参数所设置的最大速度

cpp 复制代码

	...
    _TASmin   = aparm.airspeed_min * EAS2TAS;

    if (_landing.is_on_final() && is_positive(aparm.airspeed_stall)) {
        _TASmin = aparm.airspeed_stall * EAS2TAS;
    }

    if (aparm.stall_prevention) {
        // when stall prevention is active we raise the minimum
        // airspeed based on aerodynamic load factor
        if (is_positive(aparm.airspeed_stall)) {
            _TASmin = MAX(_TASmin, aparm.airspeed_stall*EAS2TAS*sqrtf(_load_factor));
        } else {
            _TASmin *= sqrtf(_load_factor);
        }
    }

    if (_TASmax < _TASmin) {
        _TASmax = _TASmin;
    }
    ...

最小控制空速_TASmin则通过参数中的最小速度计算。如果参数中的airspeed_stall以及stall_prevention有设置，则进行调整。

cpp 复制代码

	...
    // Equivalent airspeed
    float _EAS;
    if (!use_airspeed || !_ahrs.airspeed_estimate(_EAS)) {
		...
    }

    // limit the airspeed to a minimum of 3 m/s
    const float min_airspeed = 3.0;

    // Reset states of time since last update is too large
    if (_flags.reset) {
        _TAS_state = (_EAS * EAS2TAS);
        _TAS_state = MAX(_TAS_state, min_airspeed);
        _integDTAS_state = 0.0f;
        return;
    }
	...

这里获取真实空速（_TAS_state），通过ahrs估计的_EAS转换。

cpp 复制代码

	...
    // Implement a second order complementary filter to obtain a
    // smoothed airspeed estimate
    // airspeed estimate is held in _TAS_state
    float aspdErr = (_EAS * EAS2TAS) - _TAS_state;
    float integDTAS_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
    // Prevent state from winding up
    if (_TAS_state < 3.1f) {
        integDTAS_input = MAX(integDTAS_input, 0.0f);
    }
    _integDTAS_state = _integDTAS_state + integDTAS_input * DT;
    float TAS_input = _integDTAS_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
    _TAS_state = _TAS_state + TAS_input * DT;
    _TAS_state = MAX(_TAS_state, min_airspeed);
}

这里最后对_TAS_state进行滤波，采用二阶滤波，具体原理可以查阅相关资料，这里不作详细解释。

3. 总结

1 任务规划器规划运行（50Hz）

Plane::update_speed_height()

在其中调用TECS控制器的更新函数

AP_TECS::update_50hz()

2 update_50hz函数中更新高度和爬升速率

_height

_climb_rate

然后运行TECS控制的速度更新函数

AP_TECS::_update_speed(...)

3 在_update_speed函数中计算获得如下信息

通过加速度计算空速变化率（经过滑动滤波和一阶低通滤波）

_vel_dot_lpf

计算最大和最小控制空速（即空速控制的最大最小值）

_TASmax

_TASmin

计算当前真实空速（通过二阶互补滤波）

_TAS_state