px4的垂向控制

简易流程

例如手控的垂向控制，在手控高度规划 FlightTaskManualAltitudeSmoothVel 中，根据垂向的目标速度，与xy轴的速度轨迹规划一样，也是将目标速度带入速度S曲线，生成相应的轨迹(加加速度，加速度，速度，位置)等垂向信息，之后作为控制输入，代入旋翼的位置控制类 MulticopterPositionControl ，在此类的主循环中，更新位置与悬停油门(推力)的估计数据，调用位置控制类 PositionControl 进行 PVA 控制。

根据位置速度加速度的控制目标以及前馈叠加，得到最终的加速度控制量，单说垂向的话，根据垂向的加速度期望以及悬停油门(推力)，计算出垂直向上的推力期望值，之后转化为最终的油门值给到混控器。

高度控制模式

参数 MPC_ALT_MODE 控制不同的高度模式：

*设置为0---高度控制模式，可控制相对于起飞原点的高度。由于传感器漂移，该原点可能在飞行中上下移动。
*设置为1---地形跟随模式，可控制相对于飞机到地面的距离的高度。飞机将随地形高度变化而上下移动。需要与地面传感器的距离。如果local_position.distance_bottom_valid 消息为假，则到地面的距离估计值无效，则高度控制器将恢复为使用起飞原点以上的高度。
*设置为2---地形保持模式，可控制悬停静止时相对于地面的高度（需要距离传感器），水平移动时相对于起飞原点的高度。速度阈值由 MPC_HOLD_MAX_XY 参数控制。

锁高逻辑

锁高的完整过程在两个地方都有涉及，组合起来实现锁高逻辑，一个是飞行任务层的锁高判断 FlightTaskManualAltitude::_updateAltitudeLock()，另一个是规划层的锁高判断。

在规划层，高度规划上，沿用的是速度S曲线规划的模式，在速度S曲线的规划底层中，虽然也会计算位置状态 state.x ，但在飞机运动过程中位置规划并不会作为控制输入，只有在飞机没有输入且接近悬停状态时，才会将位置规划量作为前馈。

从执行顺序上，是先执行飞行任务层的锁高判断，再执行规划层的锁高处理

飞行任务层

在飞行任务层的锁高判断中，有两个标志，一个是判断用户是否想要刹车，即判断有无外部输入，另一个是判断当前飞机是否已经被执行了停止(此时可能飞机还有速度，并没有完全停下来)

// Check if user wants to break
//如果摇杆没有输入，则判断用户想要刹车(锁高)
const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON;

// Check if vehicle has stopped
//检测飞机是否已经被停止(此时飞机可能还有一定的速度)
//_param_mpc_hold_max_z 表示悬停时的最大垂直速度 设置值在0.5~1.5m/s之间
const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());

后面只论正常的高度控制模式：

1）正常飞行时，高度目标不设置，即为NAN

2）用户想要锁高，但是当前已经有高度控制量了，此时将控制量重置为当前高度

3）用户想要锁高，并且当前飞机已经被停止，并且没有指定高度目标(飞行时是不指定高度目标的)的情况下，高度期望指定为当前高度，此时飞机可能还有速度，根据参数设置，一般参数为 0.5~1.5m/s，也就是说，在一定的速度下，才会传入高度控制量

规划层

_updateSetpoints 函数在飞行任务层有一个，但注意这个函数在规划层的子类中会覆盖，因此会执行两个类的此函数，在S曲线规划的 checkPositionLock 中，会根据当前的规划加速度，速度，以及期望值是否都小到了一定的程序，当小到一定程度时，说明用户想要锁高，之后在 _position_lock_active 锁高标志置位，并且规划的高度将赋值为S曲线的规划值：

_position_setpoint_locked = _state.x;  //最终传出的位置控制量是 _position_setpoint_locked

在正常飞行时，状态 state.x 都会被赋值为当前位置，因此速度S曲线的位置规划量，只有在要刹车时，才会进行规划输出，平时的位置规划状态都是赋值的当前位置：

//当前垂向速度与加速度规划以及速度目标都比较小时，判断当前需要锁高，将 _position_lock_active 置1
//置1之后，才会将规划的位置目标复制并传递出去   锁高标志为0时，高度目标都不设置，为NAN
//非高度锁定时，规划的位置状态就一直传入当前位置
if (fabsf(_state.v) < 0.1f &&  //0.1
    fabsf(_state.a) < .2f &&   //0.2
    fabsf(velocity_target) <= FLT_EPSILON) {
        // Lock position
        _position_lock_active = true;
        _position_setpoint_locked = _state.x;

} else {
        // Unlock position
        if (_position_lock_active) {
                // Start the trajectory at the current velocity setpoint
                _trajectory.setCurrentVelocity(_velocity_setpoint_feedback);
                _state.v = _velocity_setpoint_feedback;
                resetPositionLock();
        }
        //在非位置锁定模式下，规划的位置就一直传入当前实际位置
        _trajectory.setCurrentPosition(_position_estimate);
}

垂向推力计算

如上图所示，已知垂向的加速度期望 acc_sp，需要计算的是最终的向上推力(油门) T 值。

假设期望加速度 acc_sp 对应的合力 Fm = T - m*g = m * acc_sp ，则：

T = m * acc_sp + m * g

同时通过悬停油门估计器估计出来的悬停油门 Th 是已知的，Th = m * g，m = Th / g ，再次代入，得：

T = acc_sp * (Th / g) - Th

float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;

不过悬停推力，向上推力等信息，在 px4 中都是使用归一化后的油门值来表示。

而悬停油门估计，就是估计飞控归一化的油门值与飞机重力之间的对应关系

悬停油门估计

悬停油门估计使用单状态的卡尔曼滤波器，在《卡尔曼滤波》笔记中，有相关的推导与最终的公式结果，就不再叙述，悬停油门估计中的关键步骤，对照卡尔曼滤波的五个公式就可以一一对应起来，不过有几个需要注意的地方：

1）油门值是归一化的[0,1]的值，并不能直接表示重力大小，因此在求解 加速度测量 z(k) 与 悬停油门 x(k) 的关系矩阵 H 时，采用的是比例的方式，并且由于当前的加速度是动态变化的，因此当前加速度与悬停油门的关系矩阵 H 也并不是一个定值，需要实时计算：

//计算加速度z(k) 与 悬停油门值x(k) 的关系矩阵H    z(k) = H * x(k)
//方向向下的加速度acc = 油门与悬停油门(重力)的比值 * g   相当于是当前油门是重力的多少倍 就产生多少的g
inline float ZeroOrderHoverThrustEkf::computeH(const float thrust) const
{
        return -CONSTANTS_ONE_G * thrust / (_hover_thr * _hover_thr);
}

当前油门与悬停油门的比值，就表示当前油门相当于是重力的多少倍，即：
当前向上的力 = thrust / _hover_thr * mg
产生的方向向上的加速度 acc = 当前向上的力
px4 中向下为正，因此 acc 取负号，则 acc = - g * ( thrust / _hover_thr)
acc 与 悬停油门 _hover_thr 的关系为： acc = - g * ( thrust / (_hover_thr * _hover_thr)) * _hover_thr
H矩阵就是： - g * ( thrust / (_hover_thr * _hover_thr))

2）悬停油门估计中的加速度测量误差的方差，是一个变化值，里面有一个系数_acc_var_scale，加速度测量误差的方差最终会乘以这个系数：

const float R = _acc_var * _acc_var_scale;  //测量误差的协方差矩阵

当速度小于设定的参数时，此系数为1，当速度越来越大时，此系数也会线性变化，越来越大，也就是说，速度越大，测量误差的方差就越大，估计器就越不相信测量值

//_param_hte_vz_thr 表示灵敏度降低的垂直速度阈值
//当垂直速度大于此阈值时，测量噪声线性增加 降低估计器对测量值的灵敏度(噪声越大，估计器就越不相信此值)
const float meas_noise_coeff_z = fmaxf((fabsf(local_pos.vz) - _param_hte_vz_thr.get()) + 1.f, 1.f);
const float meas_noise_coeff_xy = fmaxf((matrix::Vector2f(local_pos.vx,
                                        local_pos.vy).norm() - _param_hte_vxy_thr.get()) + 1.f,
                                        1.f);

//计算加速度测量误差的方差系数
_hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z));

垂直速度的参数阈值 _param_hte_vz_thr 为 2：

PARAM_DEFINE_FLOAT(HTE_VZ_THR, 2.0);

水平速度的参数阈值 _param_hte_vxy_thr 为 10：

PARAM_DEFINE_FLOAT(HTE_VXY_THR, 10.0);

两者在计算加速度测量噪声误差时，取的是最大值，无论水平速度还是垂直速度，越大，都会影响测量噪声系数。