Ardupilot姿态控制器 PID控制流程

---

APM的控制器主要分为attitude姿态控制器和pos位置控制器。这系列文章主要来讲一下姿态控制器内部的实现。本文主要目的是为了解析APM源码然后方便自己将其实现在自己的水下航行器控制器上的，因此将会以ArduCopter（毕竟最官方了）和ArduSub为例进行解析，其他无人机类型也是类似的，也可用作参考。

---

一、PID姿态控制器

1.1 Copter姿态控制官方原图

在真正开始解析源码之前，可以看一下Ardupilot官方网站上开发者手册对于直升机姿态控制器的描述（虽然官方的文档已经很久没更新了，下图根据论坛上描述应该是Copter3.2版本的，4.x版本之后的PID控制器详见我的上一篇博文）。

可以看到是一个典型的串级PID控制器。

• 首先获取到期望角度和实际角度的误差，然后使用P控制器将其转换为期望的角速度值，在此控制过程中会根据具体控制器内部程序设定判断是否开启速率前馈控制FF，以及是否启用平方根控制器等。
• 如果开启前馈控制FF，将会叠加到P控制器的输出上以获取控制量，使用前馈的主要目的是使角度控制器自动校准系统的干扰量和缺陷，提高动态响应速度。
• 然后对输出的期望欧拉角姿态变化速率（这个描述比较准确，后面谈到角速率和角速度基本上默认是欧拉角姿态变化速率，后面太多改起来有些麻烦了，这边统一说一下）和实际姿态变化速率作差获得转速误差，输入到PID控制器中得到电机控制量完成整个控制过程。

以上内容针对于ROLL、PITCH和YAW的单轴控制。

官方描述：Below is a high level diagram showing how the attitude control is done for each axis. The control is done using a P controller to convert the angle error (the difference between the target angle and actual angle) into a desired rotation rate followed by a PID controller to convert the rotate rate error into a high level motor command. The “square root controller” portion of the diagram shows the curved used with the angle control’s P controller.

前馈控制（顺馈控制）
查了一下，知乎上说在自动控制里面这两个是一个意思，那这边就当做这样。
关于前馈控制基础概念，看这两篇差不多了：
前馈控制+PID
PID控制器开发笔记之九：基于前馈补偿的PID控制器的实现
看完上面的对前馈有基本了解之后在看下面的：
以下内容节选自：https://zhidao.baidu.com/question/71949941.html和https://zhidao.baidu.com/question/647133996316370805.html
前馈控制分为两种
1.基于扰动的前馈补偿。就是以某种扰动作为前馈控制通路的给定，再将计算得到的补偿值叠加到系统给定中去；
2.基于给定的前馈补偿。为了提高控制系统的响应速度，可以将系统给定经过一个前馈通道，叠加到系统的控制量上。
由此判定采用的是基于给定的前馈补偿

1.2 ArduCopter V4.X STABILIZE

• Input Translation输入整形器：首先根据摇杆位置（stick postion）进行输入转化，这个输入可能来自stick、navigationcontroller或者position controller。该转换器以400Hz频率运行，获得加速度和加速条件下的目标速度，并将其以向量形式输出。此处纵向上有3个方块表示ROLL、PITCH和YAW。
• Angle Controller角度控制器：然后针对期望速率向量，分两路进行处理：
  • 一路向下首先进行积分并进行四元数运算获取到期望角度四元数，并将此期望角度四元数反馈回输入整形器，同时将期望值与测量获得的实际值计算获得误差，对加速度进行限幅，通过P控制器计算得到期望角速度控制量输出，对应于第一张图中的主流程。
  • 另一路向右转换到机体坐标系上，作为前馈控制量叠加到P控制器的输出中，叠加之后的输出量作为最终输入进角速率PID控制器的期望角速率。
• Body Frame Controllers机体控制器：PID速率控制器，分别对欧拉角各自单独的轴进行控制。由前面的P控制器输出的期望角速度和IMU测得的实际角速度会先作差获得误差。
  • 在ROLL和PITCH控制器中对于P项和I项会直接输入，而对于D项则是会先经过一个低通滤波器再输入。官方视频中说是因为ROLL和PITCH的改变需要等待RPM正确变化，考虑到D项的作用是使输出快速的跟定输入，与系统的动态特性相关，或许因此在D项前加入低通滤波器（此处我也还没弄懂，瞎说的）。
  • 在YAW控制器中对于所有项都会先通过一个低通滤波器。考虑到可能是因为需要倾转分离的缘故，需要ROLL和PITCH快速响应，而YAW角的调整较慢
  • 所有角速度的PID控制器都会在积分项计算过程中对其进行积分量限制操作（具体代码后面再说），然而在具体阅读过程中笔者尚未完全确定这个一个抗积分饱和PID控制器还是一个积分分离PID控制器，根据特性个人认为更加偏向于是一个抗积分饱和PID控制器。
  • 后续操作就是对PID控制器的输出进行限幅然后获得最终的电机控制量的输出，传递给ESC电子调速器，再传递到机体框架进行DCM控制（DCM理论部分还不太熟悉，可以先看看这篇博文：Pixhawk之姿态解算篇（1）_入门篇（DCM
    Nomalize））

最后，让我们重申，Ardupilot的姿态控制器本质上是一个三轴的Angle P-> Rate PID控制器。 姿态控制器为一个四元数控制器，而其实际上是一个P角控制器。它使用角度误差来计算角速度并传递给PID速率控制器。

 

二、姿态控制器类实现

在ardupilot/libraries/AC_AttitudeControl路径下实现了姿态控制器的相关内容，主要是在AC_AttitudeControl.h/.cpp文件中，这部分内容实际上是作为基类，而对应的会根据具体的无人机类型派生出如AC_AttitudeControl_Multi.h/.cpp（用于Copter）和AC_AttitudeControl_Sub.h/.cpp（用于Submarine）文件中的派生类。

2.1 类成员解析

首先还是具体来说一下姿态控制器的相关类的内容，具体的我已经整理在了下面这张图中（花了我老长时间…如果有人要转载这张图拜托一定要标明出处啊）

具体看了一下，AC_AttitudeControl作为基类派生出的AC_AttitudeControl_Multi类和AC_AttitudeControl_Sub类基本内容是一致的，然而用于直升机的AC_AttitudeControl_Heil类内容差异较大。

2.1.1 类成员变量

此处不会把所有内容都讲，具体还是看上图的概括，这边只写一些我觉得需要注意的地方，更具体的内容建议直接查看源码

重点关注一下角度PID控制器的实现和参数的获取是实现在AC_AttitudeControl这个基类内部，而对应的角速度PID控制器则是实现在具体的派生类中。

PID相关变量

在AC_AttitudeControl类中，注意到PID控制器的变量都是AC_P的类对象。

// angle controller P objects
AC_P                _p_angle_roll;
AC_P                _p_angle_pitch;
AC_P                _p_angle_yaw;

而在AC_AttitudeControl_Multi等具体车辆姿态控制器的类中，PID控制器的变量都是AC_PID的类对象

AC_PID                _pid_rate_roll;
AC_PID                _pid_rate_pitch;
AC_PID                _pid_rate_yaw;

原因之前也说过，主要是角度控制是通过P控制器来完成的，而角速率控制器则是通过PID控制器完成，在APM内部针对P控制器和PID控制器分别声明了不同的类进行定义。

期望值

在AC_AttitudeControl这个基类中相关期望值都是以Vector3向量形式（x,y,z）或者Quaternion四元数形式（q1，q2，q3，q4）进行储存和运算的。

    // This represents a 321-intrinsic rotation in NED frame to the target (setpoint)
    // attitude used in the attitude controller, in radians.
    Vector3f            _attitude_target_euler_angle;

    // This represents the angular velocity of the target (setpoint) attitude used in
    // the attitude controller as 321-intrinsic euler angle derivatives, in radians per
    // second.
    Vector3f            _attitude_target_euler_rate;

    // This represents a quaternion rotation in NED frame to the target (setpoint)
    // attitude used in the attitude controller.
    Quaternion          _attitude_target_quat;

    // This represents the angular velocity of the target (setpoint) attitude used in
    // the attitude controller as an angular velocity vector, in radians per second in
    // the target attitude frame.
    Vector3f            _attitude_target_ang_vel;

    // This represents the angular velocity in radians per second in the body frame, used in the angular
    // velocity controller.
    Vector3f            _rate_target_ang_vel;

    // This represents a quaternion attitude error in the body frame, used for inertial frame reset handling.
    Quaternion          _attitude_ang_error;

2.1.2 类成员函数

此处不会把所有内容都讲，具体还是看上图的概括，这边只写一些我觉得需要注意的地方，更具体的内容建议直接查看源码

在AC_AttitudeControl基类中相关成员函数大多以虚函数形式为派生类提供接口。

获取PID参数

如前所述，AC_AttitudeControl基类可以获取角度PID控制量相关值，但是为派生类提供了虚函数接口

// pid accessors
AC_P& get_angle_roll_p() { return _p_angle_roll; }
AC_P& get_angle_pitch_p() { return _p_angle_pitch; }
AC_P& get_angle_yaw_p() { return _p_angle_yaw; }
virtual AC_PID& get_rate_roll_pid() = 0;
virtual AC_PID& get_rate_pitch_pid() = 0;
virtual AC_PID& get_rate_yaw_pid() = 0;

而AC_AttitudeControl_Multi类中则是速率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; }

控制器

姿态控制器实现在AC_AttitudeControl类中，其中以四元数计算为主

// Calculates the body frame angular velocities to follow the target attitude
void attitude_controller_run_quat();

在其内部重点需要关注的是这个函数，其内部实现了姿态误差的计算以及倾转分离。

thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);

而实际上attitude_controller_run_quat()这个函数的调用是在下述形式的函数中调用

// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);

// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
virtual void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw);
...

所以宏观上来说，实际上姿态控制是从input_euler_angle_roll_pitch_euler_rate_yaw()形式的函数开始的。

速率控制器则实现在具体的派生类中，如AC_AttitudeControl_Multi类和AC_AttitudeControl_Sub类中

// run lowest level body-frame rate controller and send outputs to the motors
void rate_controller_run() override;

内部是对输入角速率误差进行PID运算的过程。

以上函数实现内容以及姿态控制器的调用流程会在后面进行详述，这边先有个大概的印象即可。

2.2 构造函数

如果是有C++基础的同学，应该知道当你在C++代码中新建一个类对象的时候，编译器会自动帮你调用类的构造函数进行初始化。我个人觉得有必要讲一下姿态控制器的声明以及初始化的过程。

首先是在具体的车辆文件夹路径下面，车辆类如 class Copter 或者 class Sub中声明一个姿态控制器。

// 在ArduCopter文件夹中
#define AC_AttitudeControl_t AC_AttitudeControl_Multi
...
AC_AttitudeControl_t *attitude_control;

// 在ArduSub文件夹中
// Attitude, Position and Waypoint navigation objects
// To-Do: move inertial nav up or other navigation variables down here
AC_AttitudeControl_Sub attitude_control;

在前面说过，这些AC_AttitudeControl_Multi和AC_AttitudeControl_Sub 都是继承自AC_AttitudeControl这个基类的。同时，如上述可知在声明这个类对象的时候就已经完成了姿态控制器的初始化过程，我们具体来看一下。

2.2.1 AC_AttitudeControl构造函数

AC_AttitudeControl构造函数如下，实现在AC_AttitudeControl.h中：

class AC_AttitudeControl {
public:
    AC_AttitudeControl( AP_AHRS_View &ahrs,
                        const AP_Vehicle::MultiCopter &aparm,
                        AP_Motors& motors,
                        float dt) :
        _p_angle_roll(AC_ATTITUDE_CONTROL_ANGLE_P),
        _p_angle_pitch(AC_ATTITUDE_CONTROL_ANGLE_P),
        _p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P),
        _dt(dt),
        _angle_boost(0),
        _use_sqrt_controller(true),
        _throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
        _throttle_rpy_mix(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
        _ahrs(ahrs),
        _aparm(aparm),
        _motors(motors)
        {
            AP_Param::setup_object_defaults(this, var_info);
        }
    ...
}

可以看到，这个构造函数接收4个参数：姿态传感器（AP_AHRS_View 这个类允许代码的不同部分从不同的角度查看姿态），AP_Vehicle::MultiCopter类型的aparm（最大角度），控制的电机，积分采样时间。

随后通过列表初始化的方式实现参数赋值过程：

• _p_angle_roll、_p_angle_pitch和_p_angle_yaw这三个P控制器的Kp赋值为AC_ATTITUDE_CONTROL_ANGLE_P = 4.5；

#define AC_ATTITUDE_CONTROL_ANGLE_P                     4.5f             // default angle P gain for roll, pitch and yaw

• 积分采样时间_dt从接收的参数中获得初始化的默认值；
• 用于倾斜补偿的油门增加_angle_boost设置为0；
• _use_sqrt_controller指定姿态控制器在姿态校正中开启平方根控制器，确保对P项进行整定；
• _throttle_rpy_mix_desired和_throttle_rpy_mix这个还没怎么弄懂，先把官方的注释放在下面，等弄懂了再更新吧

// desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float               _throttle_rpy_mix_desired;

// mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float               _throttle_rpy_mix;
/
#define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT             0.5f  // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control.  Higher favours Attitude over pilot input

• _ahrs、_aparm和_motors这三个变量获得输入参数的给定值。
• 最后是在构造函数内部实现了对于参数列表的加载，具体的var_info参数列表实现在AC_AttitudeControl.h中，内容过多就不放进来了。

2.2.2 具体车辆派生类的构造函数

此处Copter和Sub基本一致，我以Sub为例（内容相对较长，请大家务必仔细看完）

在AC_AttitudeControl_Sub.h中 声明了构造函数：

class AC_AttitudeControl_Sub : public AC_AttitudeControl {
public:
    AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt);
...
}

并在AC_AttitudeControl_Sub.cpp中进行了实现

AC_AttitudeControl_Sub::AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) :
    AC_AttitudeControl(ahrs, aparm, motors, dt),
    _motors_multi(motors),
    _pid_rate_roll(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ, dt),
    _pid_rate_pitch(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ, dt),
	_pid_rate_yaw(AC_ATC_SUB_RATE_YAW_P, AC_ATC_SUB_RATE_YAW_I, AC_ATC_SUB_RATE_YAW_D, 0.0f, AC_ATC_SUB_RATE_YAW_IMAX, AC_ATC_SUB_RATE_YAW_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_YAW_FILT_HZ, dt)
{
    AP_Param::setup_object_defaults(this, var_info);

    // Sub-specific defaults for parent class
    _p_angle_roll.kP().set_default(AC_ATC_SUB_ANGLE_P);
    _p_angle_pitch.kP().set_default(AC_ATC_SUB_ANGLE_P);
    _p_angle_yaw.kP().set_default(AC_ATC_SUB_ANGLE_P);

    _accel_yaw_max.set_default(AC_ATC_SUB_ACCEL_Y_MAX);
}

和AC_AttitudeControl一样接受了四个相同的参数，同时使用列表初始化将这四个参数赋值给AC_AttitudeControl类完成类初始化，并且对RPY的3个PID控制器的参数进行赋值，如果有人忘了的话，这边再提一下这三个变量在.h文件中以protected形式：

AC_PID                _pid_rate_roll;
AC_PID                _pid_rate_pitch;
AC_PID                _pid_rate_yaw;

---

横线框选内容部分讲一下PID的构造，从这边开始，不想看的同学直接下拉到横线结束即可。

上面调用了AC_PID类的构造函数对这3个类对象进行初始化，AC_PID的构造函数定义如下：

/// @class	AC_PID
/// @brief	Copter PID control class
class AC_PID {
public:

    // Constructor for PID
    AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt);
    ...
}

来仔细看一下，里面的参数对应于

参数名	含义	赋值（以YAW为例）	具体值
initial_p	初始定义的Kp	AC_ATC_SUB_RATE_YAW_P	0.180f
initial_i	初始定义的Ki	AC_ATC_SUB_RATE_YAW_I	0.180f
initial_d	初始定义的Kd	AC_ATC_SUB_RATE_YAW_D	0.0f
initial_ff	初始定义的前馈因子	0.0f	0.0f
initial_imax	Ki最大值	AC_ATC_SUB_RATE_YAW_IMAX	0.222f
initial_filt_T_hz	设置目标滤波器hz	AC_ATC_SUB_RATE_YAW_FILT_HZ	5.0f
initial_filt_E_hz	设置误差滤波器hz	0.0f	0.0f
initial_filt_D_hz	设置微分滤波器hz	AC_ATC_SUB_RATE_YAW_FILT_HZ	5.0f
dt	采样间隔时间	dt	dt

AC_PID构造函数具体实现如下（可查看AC_PID.cpp文件）。注意会获取到dt值并且赋值给_dt，然后会在初始化的时候将积分量_integrator，误差值_error和微分量_derivative给置零，然后对PID的各个参数进行赋值，重启滤波器并且将原来的变量表给清零。

// Constructor
AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt) :
    _dt(dt),
    _integrator(0.0f),
    _error(0.0f),
    _derivative(0.0f)
{
    // load parameter values from eeprom
    AP_Param::setup_object_defaults(this, var_info);

    _kp = initial_p;
    _ki = initial_i;
    _kd = initial_d;
    _kff = initial_ff;
    _kimax = fabsf(initial_imax);
    filt_T_hz(initial_filt_T_hz);
    filt_E_hz(initial_filt_E_hz);
    filt_D_hz(initial_filt_D_hz);

    // reset input filter to first value received
    _flags._reset_filter = true;

    memset(&_pid_info, 0, sizeof(_pid_info));
}

此处是PID构造内容的结束

---

回到AC_AttitudeControl_Sub的构造函数中，其中赋值的各个参数定义于.h文件中。

// default angle controller PID gains
// (Sub-specific defaults for parent class)
#define AC_ATC_SUB_ANGLE_P             6.0f
#define AC_ATC_SUB_ACCEL_Y_MAX         110000.0f

// default rate controller PID gains
#define AC_ATC_SUB_RATE_RP_P           0.135f
#define AC_ATC_SUB_RATE_RP_I           0.090f
#define AC_ATC_SUB_RATE_RP_D           0.0036f
#define AC_ATC_SUB_RATE_RP_IMAX        0.444f
#define AC_ATC_SUB_RATE_RP_FILT_HZ     30.0f
#define AC_ATC_SUB_RATE_YAW_P          0.180f
#define AC_ATC_SUB_RATE_YAW_I          0.018f
#define AC_ATC_SUB_RATE_YAW_D          0.0f
#define AC_ATC_SUB_RATE_YAW_IMAX       0.222f
#define AC_ATC_SUB_RATE_YAW_FILT_HZ    5.0f

构造函数内容为：

① 初始化变量表

AP_Param::setup_object_defaults(this, var_info);

② 对AC_AttitudeControl父类中的Kp值进行重新赋值

_p_angle_roll.kP().set_default(AC_ATC_SUB_ANGLE_P);
_p_angle_pitch.kP().set_default(AC_ATC_SUB_ANGLE_P);
_p_angle_yaw.kP().set_default(AC_ATC_SUB_ANGLE_P);

③ 设置在地坐标系下YAW角旋转的最大加速度

_accel_yaw_max.set_default(AC_ATC_SUB_ACCEL_Y_MAX);

Copter内容与上面基本一致，实际上还更加简化一些。

 
目前先写到这边吧，内容太多看着也不太方便，具体的姿态控制函数再另开一篇着重写一下。

参考资料：
Copter Attitude Control
Arducopter quaternion based attitude control

推荐阅读：
High performance full attitude control of a quadrotor on SO(3)
中文翻译：四旋翼无人机SO(3)高性能全姿态控制
上面这篇论文很好地说明了为什么使用四元数，并且简要介绍了姿态误差的计算，基本上是最基础的。

以上内容目的为个人学习记录分享用途，如有错误请指正

2020/10/30更新：添加部分PID描述，删除一些错误及不必要的重点。