PX4源码阅读笔记：姿态内环PID控制器：RateControl类

简介

这个类是姿态控制内环的PID控制器（即输入为期望角速度，输出为三轴力矩Torque）的具体实现。
定义在文件RateControl.hpp中，具体函数的实现则在RateControl.cpp中。

具体定义

类定义如下：

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

    void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
    void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
    void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
    void setSaturationStatus(const MultirotorMixer::saturation_status &status);
    matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp,const matrix::Vector3f &angular_accel, const float dt, const bool landed);
    void resetIntegral() { _rate_int.zero(); }
    void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);

private:
    void updateIntegral(matrix::Vector3f &rate_error, const float dt);

    // Gains
    matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z
    matrix::Vector3f _gain_i; ///< rate control integral gain
    matrix::Vector3f _gain_d; ///< rate control derivative gain
    matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
    matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters

    // States
    matrix::Vector3f _rate_int; ///< integral term of the rate controller

    bool _mixer_saturation_positive[3] {};
    bool _mixer_saturation_negative[3] {};
};

成员函数

setGains

给PID参数赋初值。
输入参数类型为Vector3f，即姿态内环PID控制器的参数值。

void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
    _gain_p = P;
    _gain_i = I;
    _gain_d = D;
}

setIntegratorLimit

为所有轴设置积分器的最大绝对值。

void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };

setFeedForwardGain

设定转矩前向增益的直接速率（Set direct rate to torque feed forward gain），即前馈增益。

void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };

setSaturationStatus

设置饱和状态
MultirotorMixer是与Mixer相关的类，猜测这里应该是把执行器是饱和相关信息从那边拿过来，具体后面去看MultirotorMixer的代码。

void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status)
{
    _mixer_saturation_positive[0] = status.flags.roll_pos;
    _mixer_saturation_positive[1] = status.flags.pitch_pos;
    _mixer_saturation_positive[2] = status.flags.yaw_pos;
    _mixer_saturation_negative[0] = status.flags.roll_neg;
    _mixer_saturation_negative[1] = status.flags.pitch_neg;
    _mixer_saturation_negative[2] = status.flags.yaw_neg;
}

resetIntegral

把积分项为置零0，目的是避免饱和（Set the integral term to 0 to prevent windup）
具体操作为把私有变量_rate_int置零。这个变量存储的是积分项（integral term of the rate controller）。

void resetIntegral() { _rate_int.zero(); }

getRateControlStatus

获取控制器状态信息，用于日志记录/调试

void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
{
    rate_ctrl_status.rollspeed_integ = _rate_int(0);
    rate_ctrl_status.pitchspeed_integ = _rate_int(1);
    rate_ctrl_status.yawspeed_integ = _rate_int(2);
}

updateIntegral

这是一个私有方法，作用是更新计算出积分项，存在私有变量_rate_int中，控制算法会循环调用这段代码。
在角速率误差值增大的时候降低积分项。
输入参数为角速率误差（rate_error=rate_sp-rate)，执行后改变了_rate_int的值。

void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
{
    for (int i = 0; i < 3; i++) {
        // prevent further positive control saturation
        if (_mixer_saturation_positive[i]) {
            rate_error(i) = math::min(rate_error(i), 0.f);
        }

        // prevent further negative control saturation
        if (_mixer_saturation_negative[i]) {
            rate_error(i) = math::max(rate_error(i), 0.f);
        }

        // I term factor: reduce the I gain with increasing rate error.
        // This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
        // change (noticeable in a bounce-back effect after a flip).
        // The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
        // with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
        // and up to 200 deg error leads to <25% reduction of I.
        float i_factor = rate_error(i) / math::radians(400.f);
        i_factor = math::max(0.0f, 1.f - i_factor * i_factor);

        // Perform the integration using a first order method
        float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt;

        // do not propagate the result if out of range or invalid
        if (PX4_ISFINITE(rate_i)) {
            _rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
        }
    }
}

update

核心代码，即PID控制器的实现

输入：

rate：当前机体角速度
rate_sp：期望机体角速度
angular_accel：当前机体角加速度
dt：时间片
landed：代表是否着陆

输出:

vector3f型变量，机体坐标系下的三轴扭矩, 源码注释中说返回的是[-1,1]的归一化矢量，还没找到这个归一化在哪里实现的

Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel, const float dt, const bool landed)
{
    // 得到角速度误差
    Vector3f rate_error = rate_sp - rate;

    // 带有前馈的PID控制
    // torque=p*rate_error(比例项)+_rate_int(积分项)-d*angular_accel(微分项)+ff*rate_sp(前馈)
    // 其中_rate_int的值每次在下面的updateIntegral(rate_error, dt)中计算
    const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);

    // update integral only if we are not landed
    // 如果没有降落，更新积分值
    if (!landed) {
        updateIntegral(rate_error, dt);
    }

    return torque;
}