简介

实现了基于四元数的姿态外环PID控制器（实际上是P+前馈）。
类的定义及实现存放在：mc_att_control/AttitudeControl
控制器输入为NED坐标系下四元数表示的当前姿态q和期望姿态qd，输出为机体坐标系下三轴的角速度。
如何由NED坐标系下表示的q和q_d得到在机体坐标系下的姿态误差是关键，本来这个误差由q.inversed() * qd就可以得到，但是考虑了优先控制roll和pitch，就有一些复杂，下面进行解释。
注：网上查到的这部分的源码解读大部分是旧版本的，本文基于V1.10.1版本。

类的定义

class AttitudeControl
{
public:
    AttitudeControl() = default;
    ~AttitudeControl() = default;

    /**
     * Set proportional attitude control gain
     * @param proportional_gain 3D vector containing gains for roll, pitch, yaw
     */
    void setProportionalGain(const matrix::Vector3f &proportional_gain);

    /**
     * Set hard limit for output rate setpoints
     * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw
     */
    void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; }

    /**
     * Run one control loop cycle calculation
     * @param q estimation of the current vehicle attitude unit quaternion
     * @param qd desired vehicle attitude setpoint
     * @param yawspeed_feedforward [rad/s] yaw feed forward angular rate in world frame
     * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller
     */
    matrix::Vector3f update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward);

private:
    matrix::Vector3f _proportional_gain;
    matrix::Vector3f _rate_limit;
    float _yaw_w = 0.0f; /**< yaw weight [0,1] to prioritize roll and pitch */
}

setProportionalGain

作用是给P参数赋值。
由于控制策略是优先控制roll和pitch，所以并不是直接把proportional_gain赋值给私有变量_proportional_gain。
具体方式为:

• 把输入参数proportional_gain的前两项（roll和pitch的P参数）直接赋值给_proportional_gain的对应位置。
• 对输入参数proportional_gain的前两项（roll和pitch的P参数）求均值，赋值给_proportional_gain中的yaw的P参数的位置。
• 用私有变量_yaw_w去储存给定yaw的P参数与求均值计算出来的yaw的P参数的比值（经过0-1constrain），用来表征优先控制roll和pitch的程度（后面会介绍）。

void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain)
{
    _proportional_gain = proportional_gain;

    // prepare yaw weight from the ratio between roll/pitch and yaw gains
    const float roll_pitch_gain = (proportional_gain(0) + proportional_gain(1)) / 2.f;
    _yaw_w = math::constrain(proportional_gain(2) / roll_pitch_gain, 0.f, 1.f);

    _proportional_gain(2) = roll_pitch_gain;
}

setRateLimit

给控制器输出的角速度设置limit

void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; }

update

PID控制器的实现。

输入

• matrix::Quatf q：当前姿态在NED坐标系下的四元数表示，由姿态解算得到
• matrix::Quatf qd：期望姿态在NED坐标系下的四元数表示，不同模式下来源不同
• const float yawspeed_feedforward：yaw的前馈增益

输出

• 机体坐标系下的三轴角速度（Vector3f）

matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, const float yawspeed_feedforward)
{
    // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN
    q.normalize();
    qd.normalize();

    // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
    const Vector3f e_z = q.dcm_z();
    const Vector3f e_z_d = qd.dcm_z();
    Quatf qd_red(e_z, e_z_d);

    if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
        // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
        // full attitude control anyways generates no yaw input and directly takes the combination of
        // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
        qd_red = qd;

    } else {
        // transform rotation from current to desired thrust vector into a world frame reduced desired attitude
        qd_red *= q;
    }

    // mix full and reduced desired attitude
    Quatf q_mix = qd_red.inversed() * qd;
    q_mix.canonicalize();
    // catch numerical problems with the domain of acosf and asinf
    q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
    q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
    qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

    // quaternion attitude control law, qe is rotation from q to qd
    const Quatf qe = q.inversed() * qd;

    // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
    // also taking care of the antipodal unit quaternion ambiguity
    const Vector3f eq = 2.f * qe.canonical().imag();

    // calculate angular rates setpoint
    matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

    // Feed forward the yaw setpoint rate.
    // yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,
    // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
    // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
    // and multiply it by the yaw setpoint rate (yaw_sp_move_rate).
    // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
    // such that it can be added to the rates setpoint.
    rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward;

    // limit rates
    for (int i = 0; i < 3; i++) {
        rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
    }

    return rate_setpoint;
}

下面逐行分析

    q.normalize();
    qd.normalize();

将q和qd单位化

    const Vector3f e_z = q.dcm_z();
    const Vector3f e_z_d = qd.dcm_z();
    Quatf qd_red(e_z, e_z_d);

q.dcm_z()得到的是将q转化为方向余弦矩阵，并取第三列形成的向量。
所以根据方向余弦的定义，e_z和e_z_d即为当前机体坐标系和期望机体坐标系的z轴在NED坐标系下的表示。

第三行调用了Quatf类的其中一种构造函数，qd_red代表的是从e_z旋转到e_z_d的四元数。
可以这样理解：当前姿态坐标系经过qd_red代表的旋转之后，两个坐标系（当前姿态坐标系和期望姿态坐标系）的z轴是对齐的。我们把这个z轴对齐的状态定义为一个中间状态。可以认为qd_red包含了roll和pitch的误差信息。
注意此时qd_red只是代表了一个旋转（q->qd），并不像q和qd代表了姿态（NED->q和NED->qd）。
由于于e_z和e_z_d是在NED坐标系下的向量，所以得到的qd_red也是在NED下表示的。

    if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
        // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
        // full attitude control anyways generates no yaw input and directly takes the combination of
        // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
        qd_red = qd;

    } else {
        // transform rotation from current to desired thrust vector into a world frame reduced desired attitude
        qd_red *= q;
    }

接下来是对一种极端情况的考虑

• 先看else里的一般情况：qd_red=qd_red*q，由四元数乘法的几何意义可知，此时得到的qd_red即代表了NED系表示下的中间状态（z轴对齐的状态，NED->q_red）。
• 再看if里的情况：if里代表了一种极端情况，此时q_red接近(0,1,0,0)或者(0,0,1,0)，由四元数代表旋转的意义知道，q_red代表的旋转角度为180°，这时不考虑偏航误差，直接把qd赋值给q_red。

    Quatf q_mix = qd_red.inversed() * qd;

q_mix代表从中间状态qd_red到期望姿态qd的旋转四元数在qd_red系下的表示，可以表征yaw的误差。下面进行解释。

• 首先设代表了从中间状态qd_red到期望姿态qd的旋转在NED系下的表示，那么由四元数乘法的几何意义，有，因此。
• 那么从到q_mix差一个从NED系到qd_red系的坐标系变换，因此，即为上面代码所示。
• 注意要区分向量旋转与坐标系变换的区别！

    q_mix.canonicalize();

对q_mix规范化。
因为同一个旋转可以由互为相反数的两个四元数表示。为了避免奇异，进行统一表示。

    q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
    q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
    qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

由前面我们知道，q_mix代表从中间状态qd_red到期望姿态qd的旋转四元数在qd_red系下的表示，可以表征yaw的误差。，由于中间状态qd_red坐标系与期望姿态qd坐标系的z轴是重合的，所以q_mix在qd_red系下的表示应该是，为旋转角，即只有q_mix(0)和q_mix(3)的值是我们需要的。

• 前两行对q_mix进行constrain
• 第三行实际为：
  ,其中=_yaw_w，前面说了，这是AttitudeControl类的私有变量，在setProportionalGain方法里进行了计算， 用来表征优先控制roll和pitch的程度，范围是[0,1],这里就可以看出解释:
  因为在0到1之间，，经过这样操作，yaw的误差被缩小计算了。

所以经过了_yaw_w参数作用后的，在反推回来得到的qd与最开始给定的qd以经有所变化，即最后算出来的roll和pitch的期望角速度相同，但计算出来的yaw的期望角速度根据_yaw_w的大小会有所减小，这样就实现了优先控制roll和pitch。

    const Quatf qe = q.inversed() * qd;

qe即为从q旋转到qd的的四元数在机体坐标系下的表示，即误差。

    const Vector3f eq = 2.f * qe.canonical().imag();

使用sin(alpha/2)缩放的旋转轴作为姿态误差(参见轴角定义的四元数)
还要考虑对映单元四元数的歧义性

    matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);
    rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward;

得到期望的三轴角速度（用P参数乘以误差），并且加上yaw的前馈。
yawspeed_feedforward是绕NED系z轴旋转的前馈增益，但是我们需要在机体坐标系中应用它。
因此，我们通过取R的最后一列来推断出世界z轴(在body frame中表示)。将其取逆(== q.inversed)并将其乘以偏航设定值率(yawspeed_feedforward)。
这就产生了一个在机体系下表示的前馈的向量。

    for (int i = 0; i < 3; i++) {
        rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
    }

    return rate_setpoint;

限幅，返回期望角速度。

总结

整个过程大致为：

• 把通过四元数运算，把得到仅表征roll&pitch误差的四元数qd_red和仅表示yaw误差的四元数q_mix。
• 用_yaw_w去改变q_mix，使计算出来的yaw的误差偏小，导致最后计算出来的yaw的期望角速度变小。
• 再用qd_red和q_mix合成新的期望姿态，用新的期望姿态计算总误差，并进行P+前馈，得到期望角速度。

这样一系列操作，可以达到优先控制roll&pitch的效果。

关于四元数乘法的总结

有惯性坐标系，以及表示在下的两个单位四元数，此时若存在，满足：


那么有什么几何意义?

• 
  为从到的旋转在系下的表示，这可以由四元数乘法的几何意义得到，即在系下先进行旋转，在进行旋转等于直接进行旋转，期间没有涉及坐标系转换。
• 
  为从到的旋转在代表的坐标系下的表示，推导如下：
  因为，因此，在坐标变换的意义下即为将转换到了代表的坐标系下。

初次接触，如有错误欢迎批评指正。