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文件夹下,主要的两个函数是得到滚转角和偏航角:

/**
 * @brief ECL_L1_Pos_Controller::nav_roll
 * @return
 * 希望滚转的角度,即横向加速度命令=倾斜角命令
 */
float ECL_L1_Pos_Controller::nav_roll()
{
    float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);//横向加速度和重力加速度的比值
    ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
    return ret;
}

/**
 * @brief ECL_L1_Pos_Controller::nav_bearing
 * @return
 * 期望的方位角
 */
float ECL_L1_Pos_Controller::nav_bearing()
{
    return _wrap_pi(_nav_bearing);//将角度限制在-3.14到3.14之间
}

---

TECS算法

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

关于TECS的原理,我看的是一片博士论文:QFT/TECS在飞机自动着陆控制中的应用研究
其实现在ecl/tecs文件夹下,主要的实现函数如下:

void TECS::update_pitch_throttle(const math::Matrix<3, 3> &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
                 float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
                 float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
{

    // Calculate the time since last update (seconds)
    uint64_t now = ecl_absolute_time();
    _dt = max((now - _pitch_update_timestamp), UINT64_C(0)) * 1.0e-6f;

    // Set class variables from inputs 从输入中设置类变量
    _throttle_setpoint_max = throttle_max;
    _throttle_setpoint_min = throttle_min;
    _pitch_setpoint_max = pitch_limit_max;
    _pitch_setpoint_min = pitch_limit_min;
    _climbout_mode_active = climb_out_setpoint;

    // Initialize selected states and variables as required
    _initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas);
    //tas:真空速 eas:等效空速

    // Don't run TECS control agorithms when not in flight
    if (!_in_air) {
        return;
    }

    // Update the true airspeed state estimate 更新真空速状态估计
    _update_speed_states(EAS_setpoint, indicated_airspeed, eas_to_tas);

    // Calculate rate limits for specific total energy  计算特定总能量的速率限制
    _update_STE_rate_lim();

    // Detect an underspeed condition 检测欠速状态
    _detect_underspeed();

    // Detect an uncommanded descent caused by an unachievable airspeed demand
    //检测由无法实现的空速需求导致的非指令下降
    _detect_uncommanded_descent();

    // Calculate the demanded true airspeed
    //计算期望的真空速
    _update_speed_setpoint();

    // Calculate the demanded height
    //计算所需的高度
    _update_height_setpoint(hgt_setpoint, baro_altitude);

    // Calculate the specific energy values required by the control loop
    //计算控制回路所需的特定能量值
    _update_energy_estimates();

    // Calculate the throttle demand
    //计算所需的油门
    _update_throttle_setpoint(throttle_cruise, rotMat);

    // Calculate the pitch demand
    //计算所需的俯仰角
    _update_pitch_setpoint();

    // Update time stamps
    //更新时间戳
    _pitch_update_timestamp = now;

    // Set TECS mode for next frame
    if (_underspeed_detected) {
        _tecs_mode = ECL_TECS_MODE_UNDERSPEED;

    } else if (_uncommanded_descent_recovery) {
        _tecs_mode = ECL_TECS_MODE_BAD_DESCENT;

    } else if (_climbout_mode_active) {
        _tecs_mode = ECL_TECS_MODE_CLIMBOUT;

    } else {
        // This is the default operation mode
        _tecs_mode = ECL_TECS_MODE_NORMAL;
    }

}

---

fw_pos_control程序框图

自己画了一个简单的框图如下:

参考资料:TECS (Total Energy Control System) for Speed and Height Tuning Guide