1 软件的基本组成
/*
该程序提供以下功能：
- 组合控制速度和高度，使用油门控制总能量，使用俯仰角控制潜在能量和动能之间的交换。
- 在计算俯仰角时，可选择速度优先模式或高度优先模式。
- 当没有速度测量可用时，使用回退模式，基于高度速率需求设置油门，并将俯仰角控制切换到高度优先模式。
- 低速保护功能要求最大油门并将俯仰角控制切换到速度优先模式。
- 通过使用直观的时间常数、积分器和阻尼增益以及易于测量的飞机性能数据，相对容易进行调试。
结合理论知识，飞控软件开发包括几个关键的函数：
① update_speed(load_factor)
此函数主要目的是计算出真实的速度数值，详细可以参见ArduPilot—AP_TECS—更新速度。
② update_speed_demand();
此函数主要目的是计算出下一步需要用到的速度目标值以及速度速率（微分项）目标值，
这里用到了真实速度和总能量速率的最大最小值来对速度目标值进行一些速率限制，以防超过物理性能上的限制。
③ update_height_demand();
此函数主要目的是计算出下一步需要用到的飞机高度目标值以及高度速率（微分项）目标值。
④ update_energies();
此函数主要目的是计算出下一步需要用到的总能量，更新势能、动能以及势能微分和动能微分的目标值和实际估计值。
⑤ update_throttle_with_airspeed();
此函数主要目的是计算出下一步需要用到的油门目标值（使用速度计的情况，这里暂且不讨论不使用速度计的情况）。
采用总能量的误差作为控制器的输入，通过PID+前馈控制器，得到油门的目标值。这里与理论中讲到的也不太一致，
理论中用总能量的误差作为积分器的输入，用总能量的反馈作为P的输入，是为了防止较大的超调，
但是这里因为加入了D阻尼，所以对超调有一点的抑制作用。
油门的目标值将直接作为飞机油门的通道的输出，提高飞机螺旋桨旋转，进一步提供飞机的推力。
⑥ update_pitch();
此函数主要目的是计算出下一步需要用到的pitch角的目标值，理论上应该求到的是航迹角，但是一般情况认为飞机的攻角比较小，
从而通过将升降舵的偏角作为pitch角的目标值，以此来更好地控制航迹角。
这里计算了势能和动能的均衡值，即对动能和势能均进行加权，两个权重之和为2，如果动能的权重为2，
那么势能的权重为0，此时飞机更加偏向动能，也就更加逼近速度目标。反之，飞机更加倾向势能，也就更加逼近飞机的高度目标，
如果权重为1，说明双方兼顾，能量倾向更均衡。
// Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
进一步通过均衡能量的目标值与估计值做差，计算均衡能量误差及其微分值。
最终的pitch的目标值也是利用均衡能量误差作为控制输入，经过PID控制器输出的。
更新得到的pitch目标值就会进入pitch controller里面，与导航观测到的pitch估计值做差，进一步做姿态角的控制，
进而改变升降舵的偏移量，从而控制飞机的势能与动能的均衡。
*
*/
2 算法数据来源和融合
void AP_TECS::update_50hz(void)
{
// 用于高度和高度速率的三阶互补滤波器。
// estimated height rate = _climb_rate 估计的高度速率等于爬升速率(_climb_rate)。
// estimated height above field elevation = _height 估计的离场高度等于场地高度以上的高度(_height)。
// Reference Paper :
// Optimizing the Gains of the Baro-Inertial Vertical Channel
// Widnall W.S, Sinha P.K,
// AIAA Journal of Guidance and Control, 78-1307R
/*
首先，需要强调的是，这里的3阶互补滤波（Complementary Filter）是在飞控无法获取EKF（Extend Kalman Filter）
提供的北东地三个方向上速度的情况下才使用的，意思是首选扩展卡尔曼滤波，
扩展卡尔曼滤波不能正常工作时才使用3阶互补滤波。
3阶互补滤波:(融合气压计的高度信息与加速度计的垂直方向数据）
omega2 = omega * omega，（omega-互补滤波的频率 - 由全参数给定，默认为3 radius / s）
error_h = baro_h - filter_h，（h-高度，baro-气压计，filter-互补滤波器）
Integ1 = error_h * omega2 * omega，（第一阶段，可以理解为高度方向上的加加速度 - 加速度的微分）
dd_h = Integ1 * dt + dd_h，（通过积分，获得下一时刻的dd_h-滤波器估计出的高度的加速度）
Integ2 = dd_h + acc_h + error_h * omega2 * 3，（第二阶，可以理解为高度方向上校正后的加速度）
climb_rate = Integ2 * dt + climb_rate，（积分后得到下一时刻的爬升速率）
Integ3 = error_h * omega * 3 + climb_rate，（第三阶段，可以理解为高度方向上校正后的速度）
filter_h = Integ3 * dt + filter_h，（积分，估计出下一刻的高度）
其中，acc_h = - （acc_z + g）。acc_z =加速度计获取的z方向（垂直向下）数据，g - 重力加速度
dt = 采样间隔
所以不采用2阶互补滤波，而采用3阶互补滤波，是不想忽略加速度偏差或者重力计算偏差导致的误差，进一步精确估计结果。
*/
/*
如果从扩展卡尔曼滤波器(EKF)中有垂直位置的估计值输出，则使用该数值；
否则使用气压高度。
*/
_ahrs.get_relative_position_D_home(_height);
_height *= -1.0f;
// 计算距离上一次更新的时间（以秒为单位）。
uint64_t now = AP_HAL::micros64();
float DT = (now - _update_50hz_last_usec) * 1.0e-6f;
if (DT > 1.0f) {
_climb_rate = 0.0f;
_height_filter.dd_height = 0.0f;
DT = 0.02f; // 当首次启动纵向飞行控制系统（TECS）时，使用一个较小的时间常数。
}
_update_50hz_last_usec = now;
// 如果有惯性导航系统的垂直速度和高度信息可用，则使用它们。
Vector3f velned;
if (_ahrs.get_velocity_NED(velned)) {
// _ahrs.get_velocity_NED 返回以米/秒为单位的地面速度，顺序为北/东/下。
// 只有在have_inertial_nav()为true时才能调用此函数。
// 如果可能，可以使用扩展卡尔曼滤波器（EKF）的垂直速度信息。
_climb_rate = -velned.z; // 爬升速度 向上爬升为正，下降为负
} else {
/*
使用补偿滤波器来计算爬升速率。
该滤波器的设计旨在减小滞后。
*/
// 获取气压高度
const float baro_alt = AP::baro().get_altitude();
// 获得高度加速度。
float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
/*
其中，acc_h = - （acc_z + g）。acc_z =加速度计获取的z方向（垂直向下）数据，g - 重力加速度
*/
// 转换等价的速度为真实的速度
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
// 使用向后欧拉积分进行滤波计算，选择系数，使得所有三个滤波器极点都位于角频率 omega 处。
float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
// _hgtCompFiltOmega 高度互补滤波器频率（弧度/秒） （omega-互补滤波的频率 - 由全参数给定，默认为3 radius / s）
float hgt_err = baro_alt - _height_filter.height; // 高度差=气压高-滤波高 baro-气压计，filter-互补滤波器
float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega; // （第一阶段，可以理解为高度方向上的加加速度 - 加速度的微分）
_height_filter.dd_height += integ1_input * DT; // 通过积分，获得下一时刻的dd_h-滤波器估计出的高度的加速度
float integ2_input = _height_filter.dd_height + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
// 第二阶，可以理解为高度方向上校正后的加速度
_climb_rate += integ2_input * DT; // 积分后得到下一时刻的爬升速率
float integ3_input = _climb_rate + hgt_err * _hgtCompFiltOmega * 3.0f;
// 第三阶段，可以理解为高度方向上校正后的速度
// If more than 1 second has elapsed since last update then reset the integrator state to the measured height
// 如果距离上次更新已经过去超过1秒，则将积分器状态重置为测量的高度值。
if (DT > 1.0f) {
_height_filter.height = _height;
} else {
_height_filter.height += integ3_input*DT; // 积分，估计出下一刻的高度
}
}
// Update and average speed rate of change Get DCM
// 从 DCM 获取更新、平均速度变化率。
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
// 计算速度的变化率
float temp = rotMat.c.x * GRAVITY_MSS + AP::ins().get_accel().x;
// take 5 point moving average 对数据进行5点移动平均处理。
/*
移动平均是一种平滑数据的方法，它通过计算相邻数据点的平均值来减少噪声和突变。
在5点移动平均中，每个数据点的值都是其自身及其前后两个数据点的平均值。
以下是计算5点移动平均的步骤：
1) 对于第一个数据点，使用自身及其后面四个数据点的平均值作为平均值。
2) 对于中间的数据点，使用当前数据点及其前后两个数据点的平均值作为平均值。
3) 对于最后一个数据点，使用自身及其前面四个数据点的平均值作为平均值。
*/
_vel_dot = _vdot_filter.apply(temp); // 高度速度变化率
}
void AP_TECS::_update_speed(float load_factor)
{
/*
此函数主要目的是计算出真实的速度数值。
从程序中可以看到主要就是一些逻辑判断和这个二阶互补滤波。
_spdComFiltOmega 是用于融合X轴（向前）加速度和速度的互补滤波器的交叉频率，为了获得较低的速度噪声和滞后估计;
integDTAS_input 可以理解为速度的二次微分项，即空气加加速度;
TAS_input 可以理解为速度的一次微分项，即速度加速度。
_vel_dot =重力加速度在水平X轴上的分量与加速度计x轴的数值之和。
*/
// 计算更新时间/采样间隔
// 计算距离上次更新的时间（以秒为单位）。
uint64_t now = AP_HAL::micros64();
float DT = (now - _update_speed_last_usec) * 1.0e-6f;
_update_speed_last_usec = now;
// 将等效速度转换为真实速速。
float EAS2TAS = _ahrs.get_EAS2TAS(); // 表速与真速度的比例
_TAS_dem = _EAS_dem * EAS2TAS; // _EAS_dem 等效速度需求 //_TAS_dem 当前真速度需求
_TASmax = aparm.airspeed_max * EAS2TAS;
_TASmin = aparm.airspeed_min * EAS2TAS;
if (aparm.stall_prevention) {
_TASmin *= load_factor;
}
if (_TASmax < _TASmin) {
_TASmax = _TASmin;
}
if (_TASmin > _TAS_dem) {
_TASmin = _TAS_dem;
}
// 重置自上次更新以来的时间状态。
if (DT > 1.0f) {
_TAS_state = (_EAS * EAS2TAS); // 实际速度，经过下面的二阶滤波器，使得速度的测量值更加平滑准确
_TAS_state = MAX(_TAS_state, min_airspeed);
_integDTAS_state = 0.0f;
DT = 0.1f; // when first starting TECS, use a small time constant
// 在首次启动TECS（时间连续系统）时，使用一个较小的时间常数。
}
// 执行一个二阶互补滤波器，以获取平滑的速度估计值，速度估计值保存在_TAS_state中。
// 使得速度的测量值更加平滑准确
float aspdErr = (_EAS * EAS2TAS) - _TAS_state; // 速度误差值
float integDTAS_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega; // _spdCompFiltOmega 速度互补滤波器频率（弧度/秒）。
// 防止状态出现过度累积（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; // _vel_dot速度变化率
_TAS_state = _TAS_state + TAS_input * DT;
_TAS_state = MAX(_TAS_state, min_airspeed);
/*
从程序中可以看到主要就是一些逻辑判断和这个二阶互补滤波，它的作用是为了消除速度计的测量误差。
_spdComFiltOmega 是用于融合X轴（向前）加速度和速度的互补滤波器的交叉频率，为了获得较低的速度噪声和滞后估计;
integDTAS_input可以理解为速度的二次微分项，即空气加加速度;
TAS_input可以理解为速度的一次微分项，即空气加速度。
_vel_dot =重力加速度在水平X轴上的分量与加速度计x轴的数值之和。
// Update and average speed rate of change
// Get DCM
const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned();
// Calculate speed rate of change
float temp = rotMat.c.x * GRAVITY_MSS + _ahrs.get_ins().get_accel().x;
// take 5 point moving average
_vel_dot = _vdot_filter.apply(temp);
load_factor = aerodynamic_load_factor，初始值为1。后续被update_flight_mode函数中的除去垂起相关模式以及手动、stabilize、Acro等其余模式下每400Hz更新一次，具体更新如下代码：
float demanded_roll = fabsf(nav_roll_cd*0.01f);
if (demanded_roll > 85) {
// 限制roll的目标需求最大为85度，以防临近90出现数据错误（奇异问题）
demanded_roll = 85;
}
aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));
*/
}
1820
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
