Ardupilot速率控制器rate_controller_run解析

rate_controller_run()在APM中是作为PID控制器将输入的期望角速率转换计算得到实际的电机控制指令的。下面将以Copter为例进行讲解。

---

所有无人机的PID速率控制器都是在fast_loop()这个以400Hz频率运行的函数中被实时调用的。而update_flight_mode()函数通常以100Hz频率被调用，速率控制器的运行频率高于姿态角控制器也是符合常识的。

在运行rate_controller_run()这个函数之前需要实时获取到IMU数据。

// Main loop - 400hz
void Copter::fast_loop()
{
    // update INS immediately to get current gyro data populated
    ins.update();

    // run low level rate controllers that only require IMU data
	attitude_control->rate_controller_run();
	...
}

 

PID速率控制器源码解析

rate_controller_run()

void AC_AttitudeControl_Multi::rate_controller_run()
{
	// 将油门与姿态混合移至所需位置（从此处调用，因为每次迭代都方便地调用它）
	// 此处与PID控制器关系不大，就不细展开了
    update_throttle_rpy_mix();

	// 获取到最新的陀螺仪角速度参数
    Vector3f gyro_latest = _ahrs.get_gyro_latest();

	// get_rate_xxx_pid()返回该通道上的pid控制器
	// update_all()：真正的PID控制器，根据输入的期望角速率和当前角速率进行PID运算
	// 运算得到电机控制指令并通过set_xxx()函数赋值给这个通道上的电机
	_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll));
	// get_ff()根据期望角速率计算前馈量
    _motors.set_roll_ff(get_rate_roll_pid().get_ff());

    _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch));
    _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());

    _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw));
    _motors.set_yaw_ff(get_rate_yaw_pid().get_ff());
    
	// 控制监视器更新
    control_monitor_update();
}

可以看到这个函数分别对Roll、Pitch和Yaw三个通道进行PID运算。以Roll通道为例，get_rate_roll_pid()返回该通道的PID控制器，然后通过update_all()接收期望和测量角速度计算得到电机控制指令，然后通过set_roll()函数实现电机控制指令的设置。set_roll_ff()中类似，只不过计算的是前馈量。

下面以Roll通道为例进行细节分解。

PID运算

get_rate_xxx_pid()函数原型如下，返回一个AC_AttitudeControl_Multi类中的AC_PID类对象，这个对象就是对应该通道的PID控制器（等多详细内容看：Ardupilot姿态控制器 PID控制流程）

    // pid accessors
    AC_PID& get_rate_roll_pid() override { return _pid_rate_roll; }
    AC_PID& get_rate_pitch_pid() override { return _pid_rate_pitch; }
    AC_PID& get_rate_yaw_pid() override { return _pid_rate_yaw; }

然后调用PID控制器的update_all()进行PID计算

//  update_all - set target and measured inputs to PID controller and calculate outputs
//  target and error are filtered
//  the derivative is then calculated and filtered
//  the integral is then updated based on the setting of the limit flag
float AC_PID::update_all(float target, float measurement, bool limit)
{
    // don't process inf or NaN
    if (!isfinite(target) || !isfinite(measurement)) {
        return 0.0f;
    }

    // 根据是否重启滤波器采用不同的计算方法
	if (_flags._reset_filter) {
		// 如果reset标志位置1
		// 首先将reset标志位置0
		// 期望和误差按照正常方式计算，但不进行滤波
		// 本次微分量置0
        _flags._reset_filter = false;
        _target = target;
        _error = _target - measurement;
        _derivative = 0.0f;
	} else {
		// 如果不需要重置滤波器
		// 首先保存上一次计算的误差量
        float error_last = _error;
		// 计算本次输入期望与上一次保存的期望之间的偏差
		// 经过T滤波器滤波之后叠加到上一次保存的期望以获取到本次期望
        _target += get_filt_T_alpha() * (target - _target);
		// 计算本次期望与测量值之间的误差，并与上一次保存的误差作差求出偏差量
		// 经过E滤波器滤波之后叠加到上一次保存的误差上获得本次误差
        _error += get_filt_E_alpha() * ((_target - measurement) - _error);

        // 计算并且滤波微分项，操作与上面相同
        if (_dt > 0.0f) {
            float derivative = (_error - error_last) / _dt;
            _derivative += get_filt_D_alpha() * (derivative - _derivative);
        }
    }
    
	// 运算得到I项
	// 根据limit是否为true选择是否仅允许积分量向变小的方向运算
    update_i(limit);

    float P_out = (_error * _kp);	// 计算P项
    float D_out = (_derivative * _kd);	// 计算D项
	
	// 参数保存到日志的PID信息中
    _pid_info.target = _target;
    _pid_info.actual = measurement;
    _pid_info.error = _error;
    _pid_info.P = P_out;
    _pid_info.D = D_out;

	// 返回PID运算量
    return P_out + _integrator + D_out;
}

细节部分以注释，需要注意在对期望值_target，误差量_error以及微分项_derivative 计算时会经过一个滤波器，其内部只调用了get_filt_alpha()这个函数，只不过是输入的频率不同。

// get_filt_alpha - calculate a filter alpha
float AC_PID::get_filt_alpha(float filt_hz) const
{
    if (is_zero(filt_hz)) {
        return 1.0f;
    }

    // calculate alpha
    float rc = 1 / (M_2PI * filt_hz);
    return _dt / (_dt + rc);
}

函数的数学原型表示为
$$rc=\frac{1}{2\pi f}$$$$filter=\frac{dt}{dt+rc}$$

根据不同的对象输入的频率也会有所差异

积分限制update_i()

update_all()中可以看到对于积分项I单独调用了一个函数进行计算。函数源码解析如下

// 积分量运算
// 如果limit=1则仅允许积分量朝变小的方向运算，根据积分量和误差的正负确定进入if还是else中
// 如果limit=0则表明允许积分量运算上下浮动，只能进入if语句不断进行积分量累加
void AC_PID::update_i(bool limit)
{
    if (!is_zero(_ki) && is_positive(_dt)) {
        // 确保仅在输出饱和时才能减小积分器
		// 仅当（limit=0）或者（积分累积量为正但是当前误差为负）或者（积分累加量为负但是当前误差为正）的时候进行积分项的累加
		// 积分累加量和当前误差的正负相反表明积分量是朝着减小的方向计算的
		// 即仅允许积分量朝着减小的方向进行累加运算
        if (!limit || ((is_positive(_integrator) && is_negative(_error)) || (is_negative(_integrator) && is_positive(_error)))) {
            _integrator += ((float)_error * _ki) * _dt;
			// 并对计算得到的积分量进行限幅
            _integrator = constrain_float(_integrator, -_kimax, _kimax);
        }
	} else {
		// 如果积分量和误差方向相同，为了防止PID的输出由此不断累积扩大，本次积分作用取消
        _integrator = 0.0f;
    }
    _pid_info.I = _integrator;
}

在阅读这个积分量计算函数时博主是对其是一个抗积分饱和操作还是积分分离操作有一定的疑问的。然而最后还是觉得这是一个经过修改的抗积分饱和计算。

虽然一般的抗积分饱和操作都是根据设定的最大最小值来判断本次积分量进不进行累加，然而在APM中则是判断积分量 _integrator 和 _error 的正负，仅当两者相反，也就是积分量朝着变小的方向才进行积分量累加（一般limit=1），之后输出进行PID控制。但是当两者正负号相同时则是直接将积分量 _integrator 置0，也就是取消了P项，最后进行的是PD控制。

因此个人觉得这是一个不完全标准的抗积分饱和PID控制器（如有错误欢迎指出）。

---

get_ff()

这个函数的作用就是根据期望角速率计算出前馈控制量并且返回

float AC_PID::get_ff()
{
    _pid_info.FF = _target * _kff;	// 计算前馈控制量并保存到日志参数表中
    return _target * _kff;		// 返回前馈量
}

set_xxx()

电机指令获取函数。

    // set_roll, set_pitch, set_yaw, set_throttle
    void                set_roll(float roll_in) { _roll_in = roll_in; };        // range -1 ~ +1
    void                set_roll_ff(float roll_in) { _roll_in_ff = roll_in; };    // range -1 ~ +1
    void                set_pitch(float pitch_in) { _pitch_in = pitch_in; };    // range -1 ~ +1
    void                set_pitch_ff(float pitch_in) { _pitch_in_ff = pitch_in; };  // range -1 ~ +1
    void                set_yaw(float yaw_in) { _yaw_in = yaw_in; };            // range -1 ~ +1
    void                set_yaw_ff(float yaw_in) { _yaw_in_ff = yaw_in; };      // range -1 ~ +1

内容补充：函数中陀螺仪数据的获取

以下为补充内容，如无需求可直接跳过。

Vector3f gyro_latest = _ahrs.get_gyro_latest();

首先来看一下相关类的关系及初始化。姿态控制器类的实现及初始化看这篇博文的第二章：Ardupilot姿态控制器 PID控制流程，这边就不再多说了。

在姿态控制器AC_AttitudeControl中声明了一个protected类对象const AP_AHRS_View& _ahrs;
AP_AHRS_View类的主要作用就是创建一个获取机体姿态的窗口。什么意思呢？

我们首先还是来看一下AP_AHRS，这个类作为接口直接与硬件上的姿态传感器作用，用于获取最原始的数据，如磁力计、陀螺仪、加速度计等。同时在这个类中声明了AP_AHRS_View作为其友元类，这已经是很高的权限了，基本上可以访问到AP_AHRS类中绝大部分数据。

那么由此可以得知，AP_AHRS_View这个类的作用就是提供一个接口用以访问获取获取IMU上的传感器数据，并且提供一些数据二次处理的功能，因此其内部的大部分函数都是以get_xxx打头的。

回到类中，在AP_AHRS_View.h中定义了AP_AHRS_View类的构造函数：

// Constructor
AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0);

在AP_AHRS_View.cpp文件中对构造函数进行了具体实现。第一个参数表示监视的传感器，第二个参数表示传感器的旋转角度，如果是正常水平安装则为ROTATION_NONE，否则之后的计算都会将传感器数据进行角度变换，第三个参数应该是俯仰角的修整度（老实说我也还没弄懂，不过在此处也不是我们的重点）

AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim_deg) :
    rotation(_rotation),	// 传感器旋转角度
    ahrs(_ahrs)				// 保存到AP_AHRS类对象ahrs中，后续访问都是通过这个对象实现
{
    switch (rotation) {
    case ROTATION_NONE:
        y_angle = 0;
        break;
    case ROTATION_PITCH_90:
        y_angle = 90;
        break;
    case ROTATION_PITCH_270:
        y_angle =  270;
        break;
    default:
        AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation);
    }

    _pitch_trim_deg = pitch_trim_deg;
    // Add pitch trim
    rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0);
    rot_view_T = rot_view;
    rot_view_T.transpose();

    // setup initial state
    update();
}

回到函数中

// 使用最新的ins数据返回平滑和校正的陀螺仪矢量（EKF可能尚未使用过）
Vector3f AP_AHRS_View::get_gyro_latest(void) const {
    Vector3f gyro_latest = ahrs.get_gyro_latest();		// 调用ahrs对象的单数获取最新陀螺仪数据
    gyro_latest.rotate(rotation);	// 陀螺仪数据进行角度旋转
    return gyro_latest;				// 返回处理后的陀螺仪数据
}

可以看到内部调用了AP_AHRS类对象ahrs的方法

// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f AP_AHRS::get_gyro_latest(void) const
{
	// 获取当前主陀螺仪传感器的索引，及当前健康且处于使用状态的传感器编号_primary_gyro
	const uint8_t primary_gyro = get_primary_gyro_index();
	// 根据索引获取到陀螺仪数据
	// 并且叠加陀螺仪漂移的当前估计
    return AP::ins().get_gyro(primary_gyro) + get_gyro_drift();
}

rotate()函数内部会对输入的rotation进行判断，并且将获取到的角速度向量旋转对应角度。

// rotate a vector by a standard rotation, attempting
// to use the minimum number of floating point operations
template <typename T>
void Vector3<T>::rotate(enum Rotation rotation)
{
    T tmp;
    switch (rotation) {
    case ROTATION_NONE:
    case ROTATION_MAX:
        return;
    case ROTATION_YAW_45: {
        tmp = HALF_SQRT_2*(float)(x - y);
        y   = HALF_SQRT_2*(float)(x + y);
        x = tmp;
        return;
	}
	...
}

关于这个rotation枚举类型变量，通常是在具体的车辆类中定义的。

如在Copter中，首先是在Copter这个类中声明了一个private类型的对象

AP_AHRS_View *ahrs_view;

然后在system.cpp中实现了对rotation的赋值，默认为不旋转

ahrs_view = ahrs.create_view(ROTATION_NONE);

再比如说在ArduSub中，直接在Sub类的构造函数中实现了类对象初始化

ahrs_view(ahrs, ROTATION_NONE),

 

如有错误请及时指出