在真正开始解析源码之前，可以看一下Ardupilot官方网站上开发者手册对于直升机姿态控制器的描述（虽然官方的文档已经很久没更新了，下图根据论坛上描述应该是Copter3.2版本的，4.x版本之后的PID控制器详见我的上一篇博文）。
可以看到是一个典型的串级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.基于给定的前馈补偿。为了提高控制系统的响应速度，可以将系统给定经过一个前馈通道，叠加到系统的控制量上。
由此判定采用的是基于给定的前馈补偿
最后，让我们重申，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）文件中的派生类。
首先还是具体来说一下姿态控制器的相关类的内容，具体的我已经整理在了下面这张图中（花了我老长时间…如果有人要转载这张图拜托一定要标明出处啊）
具体看了一下，AC_AttitudeControl作为基类派生出的AC_AttitudeControl_Multi类和AC_AttitudeControl_Sub类基本内容是一致的，然而用于直升机的AC_AttitudeControl_Heil类内容差异较大。
此处不会把所有内容都讲，具体还是看上图的概括，这边只写一些我觉得需要注意的地方，更具体的内容建议直接查看源码
重点关注一下角度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;
此处不会把所有内容都讲，具体还是看上图的概括，这边只写一些我觉得需要注意的地方，更具体的内容建议直接查看源码
在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运算的过程。
以上函数实现内容以及姿态控制器的调用流程会在后面进行详述，这边先有个大概的印象即可。
如果是有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这个基类的。同时，如上述可知在声明这个类对象的时候就已经完成了姿态控制器的初始化过程，我们具体来看一下。
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（最大角度），控制的电机，积分采样时间。
随后通过列表初始化的方式实现参数赋值过程：
#define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw
// 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
此处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描述，删除一些错误及不必要的重点。