多旋翼无人机

        我们知道在无人机系统中，测量单元对角速度的感知要比角度更加灵敏，因此在串级反馈控制中，将角速度控制作为内环，将角度控制作为外环，组成一个双环串级反馈控制系统。整个姿态控制如下图所示：

        角度期望：包括俯仰角、滚转角和航向角。这3个角的期望值来源于其他模块。遥控器的控制杆量、位置控制模块的输出量、地面站系统的期望值等等。角度的期望值就是系统中希望无人机达到的某一个角度，这也就是姿态控制中的目标状态，并且我们希望无人机在这个目标状态下保持稳定。例如，通过拨动遥控器的俯仰、滚转和航向控制杆，给出的俯仰、滚转和航向角分别为5°、5°、10°这3个角度则是姿态控制器的角度期望。但是在控制程序中往往使用的是弧度制，为了便于阅读和理解，我们还是采用角度制来进行说明。

        测量角度：测量角度为无人机当前的姿态角。即，俯仰角、滚转角和航向角，通常用四元数来表示。我们知道这俯仰和滚转角是通过陀螺仪积分所得到的，航向角需要通过卡尔曼滤波将陀螺仪的积分值和磁罗盘的测量值进行融合才能得到。因此测量角度并不是直接从传感器得到的，而是由状态估计模块计算所得到的最优估计值。请读者注意，后续我们所介绍在控制系统中所需要用到的状态来源，包括角度、角速度、位置、速度等所有内容均来自于卡尔曼滤波的最优状态估计，而不是直接来自于传感器。

        角度控制器：外环P控制器，其输入内容为角度期望减去测量角度，也就是角度误差。然后通过“比例”控制方法对角度误差进行计算，得到其输出内容。需要注意的是，角度控制器的输入为角度误差，而输出却是角速度期望。也就是系统希望达到这样的角速度值，并且保持稳定。

        角速度期望：外环控制器的输出，并与内环测量角速度计算得到角速度误差，并作为内环控制器的输入。

        测量角速度：测量角速度为无人机当前的旋转角速度，其来源与陀螺仪的读数，但是也需要通过卡尔曼滤波器得到它们的最优估计值。

        角速度控制器：内环PID控制器，其输入内容为角速度期望减去测量角速度，也就是角速度误差。然后通过“比例-积分-微分”控制方法对角速度误差进行计算，得到其输出内容。与角度控制器类似的，角速度控制器的输入为角速度期望，而输出却是角速度控制量。

        角速度控制量：角速度控制量是一个控制数值，是角速度控制的输出值，这个控制量并不是角速度。这个控制量也不能直接作为PWM输出给电调，还需要经过无人机的混合控制器来进行混合调节控制，最后才能输出成最终的PWM信号。

        接下来我们来看看姿态控制的相关程序代码：

ParamFloat _roll_p,                    //滚转角比例参数
ParamFloat _roll_rate_p,               //滚转角速度比例参数
ParamFloat _roll_rate_i,               //滚转角速度积分参数
ParamFloat _roll_rate_d,               //滚转角速度微分参数

ParamFloat _pitch_p,                   //俯仰角比例参数
ParamFloat _pitch_rate_p,              //俯仰角速度比例参数
ParamFloat _pitch_rate_i,              //俯仰角速度积分参数
ParamFloat _pitch_rate_d,              //俯仰角速度微分参数

ParamFloat _yaw_p,                     //航向角比例参数
ParamFloat _yaw_rate_p,                //航向角速度比例参数
ParamFloat _yaw_rate_i,                //航向角速度积分参数
ParamFloat _yaw_rate_d,                //航向角速度微分参数

matrix::Vector3f _attitude_p;          //姿态角比例参数向量
matrix::Vector3f _rate_p;              //角速度比例参数向量
matrix::Vector3f _rate_i;              //角速度积分参数向量
matrix::Vector3f _rate_d;              //角速度微分参数向量

matrix::Vector3f _rates;               //当前角速度向量
matrix::Vector3f _rates_sp;            //角速度期望向量
matrix::Vector3f _rates_int;           //角速度积分向量
matrix::Vector3f _att_control;         //姿态控制量

parameters_updated()
{
    //取得角度的P参数
    _attitude_p(0) = _roll_p.get();
    _attitude_p(1) = _pitch_p.get();
    _attitude_p(2) = _yaw_p.get();

    //取得角速度的PID参数
    _rate_p(0) = _roll_rate_p.get();
    _rate_i(0) = _roll_rate_i.get();
    _rate_d(0) = _roll_rate_d.get();
    _rate_p(1) = _pitch_rate_p.get();
    _rate_i(1) = _pitch_rate_i.get();
    _rate_d(1) = _pitch_rate_d.get();
    _rate_p(2) = _yaw_rate_p.get();
    _rate_i(2) = _yaw_rate_i.get();
    _rate_d(2) = _yaw_rate_d.get();
}

//姿态控制
void control_attitude(float dt)
{
    //取得当前姿态的四元数（测量值）
    Quatf q(_v_att.q);
    //取得姿态期望的四元数（期望值）
    Quatf qd(_v_att_sp.q_d);
    //归一化
    q.normalize();
    qd.normalize();
    //四元数使用缩放旋转轴的方式作为姿态误差
    Quatf qe = q.inversed() * qd;
    Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();
    //误差与比例参数_attitude_p相乘，得到最终的角速度期望_rates_sp
    _rates_sp = eq.emult(_attitude_p);

    //其他代码略
}

//角速度控制
void control_attitude_rates(float dt)
{
    //取得当前角速度信息（测量值）
    rates(0) -= _sensor_bias.gyro_x_bias;
    rates(1) -= _sensor_bias.gyro_y_bias;
    rates(2) -= _sensor_bias.gyro_z_bias;
    //将期望值减去测量值得到角速度误差
	Vector3f rates_err = _rates_sp - rates;
    //对角速度做PID控制
    _att_control = _rate_p.emult(rates_err) + 
	               _rates_int + 
	               _rate_d.emult(rates_err - _rates_err_prev) / dt;
    //更新积分项
    _rates_int = _rates_int + _rate_i.emult(rates_err) * dt;

    //其他代码略
}