固定翼的位置控制是一个很重要问题,它不同于旋翼的控制,需要对速度和高度进行解耦控制,并且其不能像旋翼那样进行悬停,其转弯的时候有一个转弯半径.本博客不会对源码进行详细的解读,主要是分享一些自己读源码时的资料,自己读的过程中也有注释,想要的同学私信我.话不多说,分享资料!
在固定翼位置控制中,主要涉及到两个控制算法:L1控制和TECS(total energy control system),这两个算法是干嘛的呢?L1算法在源码中主要是为了得到飞机需要偏转的滚转角和偏航角,其原理主要是控制飞机的横向向心加速度.TECS算法是为了飞机的速度和高度解耦,在源码中主要是得到了期望的油门和俯仰角.通过这两个算法,最终得到了期望的油门和三轴角度(俯仰角,滚转角,偏航角),然后内环的姿态控制就交给了姿态控制器了.
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的原理,我看的是一片博士论文: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;
}
}
自己画了一个简单的框图如下:
参考资料:TECS (Total Energy Control System) for Speed and Height Tuning Guide