在開始閱讀本文之前,，建議可以先看一下前面幾篇博文，當然你基礎(chǔ)好直接看也是沒事的~
詳解APM的開方控制器sqrt_controller
Ardupilot傾轉(zhuǎn)分離函數(shù)thrust_heading_rotation_angles
Ardupilot前饋及平滑函數(shù)input_euler_angle_roll_pitch_yaw解析

源碼解析

以n系簡稱NED坐標系,，b系簡稱機體坐標系,，tb表示期望姿態(tài)，cb表示當前姿態(tài),。

// 根據(jù)姿態(tài)誤差計算得出期望角速率
void AC_AttitudeControl::attitude_controller_run_quat()
{
	// 檢索四元數(shù)車輛姿態(tài)
	// 獲取到當前姿態(tài)的四元數(shù)并保存到attitude_vehicle_quat
    Quaternion attitude_vehicle_quat;
    _ahrs.get_quat_body_to_ned(attitude_vehicle_quat);

	// 計算姿態(tài)誤差
	// _attitude_target_quat：輸入?yún)?shù),，在n系下期望姿態(tài)的四元數(shù)，旋轉(zhuǎn)方向tb->n
	// attitude_vehicle_quat：輸入?yún)?shù),，在n系下當前姿態(tài)的四元數(shù),，旋轉(zhuǎn)方向cb->n
	// attitude_error_vector：傳入傳出參數(shù)，姿態(tài)誤差向量
	// _thrust_error_angle：傳入傳出參數(shù),，期望z軸和當前z軸的誤差（詳見軸角分離）,，即推力方向上的偏差角
	// 該函數(shù)通過前兩個輸入的四元數(shù)參數(shù)計算得到各軸的姿態(tài)偏差
	// 經(jīng)過此函數(shù)后得到的參數(shù)均在b系下
    Vector3f attitude_error_vector;
    thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);

	// P控制器，輸入為姿態(tài)誤差,，輸出為期望角速率
	// 函數(shù)內(nèi)部根據(jù)是否開啟了sqrt_controller來區(qū)分計算各通道的期望角速率_rate_target_ang_vel
	// 是,，則使用開方調(diào)整過的P控制器
	// 否，則使用常規(guī)的Kp*error
    _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);

	// 對P控制器輸出疊加前饋項，嘗試確保側(cè)傾和俯仰誤差隨車身框架而不是參考框架旋轉(zhuǎn)
	// x軸的前饋項 = y軸的姿態(tài)誤差（限幅后的）*陀螺儀獲取的z軸角速度
	// y軸的前饋項 = x軸的姿態(tài)誤差（限幅后的）*陀螺儀獲取的z軸角速度
	// 注意此處并沒有對z軸的期望角速率進行處理
    // todo: this should probably be a matrix that couples yaw as well.
    _rate_target_ang_vel.x += constrain_float(attitude_error_vector.y, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;
    _rate_target_ang_vel.y += -constrain_float(attitude_error_vector.x, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;

	// 期望角速率限幅
    ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));

	// 角速度前饋旋轉(zhuǎn)到機體坐標中（對該處有疑問的詳見本文后面）
	// _attitude_target_ang_vel是一個全局變量,，表示期望姿態(tài)的期望角速率,，是由input_euler_angle_roll_pitch_yaw等類似函數(shù)獲取到的前饋速率
	// attitude_target_ang_vel_quat在此處表明是一個向量[0,v]
	Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
	// 運算過程注意四元數(shù)和旋轉(zhuǎn)矩陣一樣是可以上下標相互約去的，此處以Q(a)(b)形式表明上標為a,，下標為b,，表示b向a的旋轉(zhuǎn)過程
	// attitude_vehicle_quat前面說過是表示cb->n旋轉(zhuǎn)的單位四元數(shù)，求逆后表示n->cb
	// to_to_from_quat = Q(cb)(n)*Q(n)(tb)=Q(cb)(tb)
	// 表明獲取的四元數(shù)to_to_from_quat表示tb->cb的旋轉(zhuǎn),，計算公式也可視作期望姿態(tài)與當前姿態(tài)的作差
	Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
	// 將tb下的期望角速率旋轉(zhuǎn)至cb下獲取當前姿態(tài)的角速率期望前饋疊加量desired_ang_vel_quat
    Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;

	// 校正推力矢量并平穩(wěn)添加前饋和偏航輸入
	// 下述過程根據(jù)推力角誤差大小分配在Z軸YAW角上的數(shù)學(xué)計算結(jié)果和傳感器測量值之間的確信度來確定Z軸的期望角速度
	// 同時將前饋控制量添加到各軸
	// 當推力角誤差特別大的時候,，表明計算出來的期望值不可信,，此時z軸期望角速度取陀螺儀測量值
    if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) {
        _rate_target_ang_vel.z = _ahrs.get_gyro().z;
	} else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) {
		// 當推力角誤差較大時,，根據(jù)之前計算出來的推力角誤差_thrust_error_angle計算出前饋因子
		// 對各軸的旋轉(zhuǎn)角速度各自疊加前饋控制量
        float feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE);
        _rate_target_ang_vel.x += desired_ang_vel_quat.q2 * feedforward_scalar;
        _rate_target_ang_vel.y += desired_ang_vel_quat.q3 * feedforward_scalar;
        _rate_target_ang_vel.z += desired_ang_vel_quat.q4;
		// 校正推力矢量，feedforward_scalar表示對計算出的數(shù)學(xué)結(jié)果的確信度,，(1- feedforward_scalar)則表示對陀螺儀測得的角速度的確信度
		// 兩者綜合得到最后的期望角速率
        _rate_target_ang_vel.z = _ahrs.get_gyro().z * (1.0 - feedforward_scalar) + _rate_target_ang_vel.z * feedforward_scalar;
	} else {
		// 推力角誤差較小時,，表明當前數(shù)學(xué)計算得到的期望角速率可信度較高，直接進行前饋疊加
        _rate_target_ang_vel.x += desired_ang_vel_quat.q2;
        _rate_target_ang_vel.y += desired_ang_vel_quat.q3;
        _rate_target_ang_vel.z += desired_ang_vel_quat.q4;
    }

    if (_rate_bf_ff_enabled) {
        // 當開啟機體速率前饋時,，實時計算更新期望姿態(tài)四元數(shù),，每一次更新都需要將其單位化
        Quaternion attitude_target_update_quat;
        attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
        _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
        _attitude_target_quat.normalize();
    }

    // 確保四元數(shù)已經(jīng)被單位化
    _attitude_target_quat.normalize();

	// 記錄姿態(tài)誤差以處理EKF重置
	// 等式相當于將_attitude_target_quat表示的期望姿態(tài)減去attitude_vehicle_quat表示的當前姿態(tài)
    _attitude_ang_error = attitude_vehicle_quat.inverse() * _attitude_target_quat;
}

總結(jié)：attitude_controller_run_quat()函數(shù)在內(nèi)部通過調(diào)用thrust_heading_rotation_angles()函數(shù)計算出姿態(tài)誤差，然后根據(jù)姿態(tài)誤差調(diào)用了P控制器得到期望角速率,，并進行了前饋疊加及平滑操作,。

細節(jié)講解

thrust_heading_rotation_angles()

這個函數(shù)主要的作用就是根據(jù)輸入的期望姿態(tài)和當前姿態(tài)計算出姿態(tài)誤差，詳細介紹請看我之前的博文：Ardupilot傾轉(zhuǎn)分離函數(shù)thrust_heading_rotation_angles

update_ang_vel_target_from_att_error()

這個函數(shù)內(nèi)部代碼比較簡單,，但是還是說一下,。

主要就是P控制器的作用，將輸入的姿態(tài)誤差處理之后轉(zhuǎn)換為期望角速率輸出,。

可以很清楚看到內(nèi)部會根據(jù)是否啟用_use_sqrt_controller來進行不同的處理,，如果_use_sqrt_controller = 1，那么會調(diào)用整定后的P控制器sqrt_controller進行計算,；而如果_use_sqrt_controller = 0,，那么就會按照傳統(tǒng)計算方式 Kp*error 進行計算。

關(guān)于sqrt_controller詳見：詳解APM的開方控制器sqrt_controller

// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f &attitude_error_rot_vec_rad)
{
    Vector3f rate_target_ang_vel;
    // Compute the roll angular velocity demand from the roll angle error
    if (_use_sqrt_controller) {
        rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
    } else {
        rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x;
    }
    // todo: Add Angular Velocity Limit

    // Compute the pitch angular velocity demand from the pitch angle error
    if (_use_sqrt_controller) {
        rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
    } else {
        rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y;
    }
    // todo: Add Angular Velocity Limit

    // Compute the yaw angular velocity demand from the yaw angle error
    if (_use_sqrt_controller) {
        rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt);
    } else {
        rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z;
    }
    // todo: Add Angular Velocity Limit
    return rate_target_ang_vel;
}

角度前饋量旋轉(zhuǎn)解釋

有人可能會對這段代碼的旋轉(zhuǎn)過程有所疑惑,，然而實際上在我上一篇博文（Ardupilot前饋及平滑函數(shù)input_euler_angle_roll_pitch_yaw解析）已經(jīng)對這個問題進行了解釋,，這邊再細說一下。

	// 角速度前饋旋轉(zhuǎn)到機體坐標中（對該處有疑問的詳見本文后面）
	// _attitude_target_ang_vel是一個全局變量,，表示期望姿態(tài)的期望角速率,，是由input_euler_angle_roll_pitch_yaw等類似函數(shù)獲取到的前饋速率
	// attitude_target_ang_vel_quat在此處表明是一個向量[0,v]
	Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
	// 運算過程注意四元數(shù)和旋轉(zhuǎn)矩陣一樣是可以上下標相互約去的，此處以Q(a)(b)形式表明上標為a,，下標為b,，表示b向a的旋轉(zhuǎn)過程
	// attitude_vehicle_quat前面說過是表示cb->n旋轉(zhuǎn)的單位四元數(shù)，求逆后表示n->cb
	// to_to_from_quat = Q(cb)(n)*Q(n)(tb)=Q(cb)(tb)
	// 表明獲取的四元數(shù)to_to_from_quat表示tb->cb的旋轉(zhuǎn)，計算公式也可視作期望姿態(tài)與當前姿態(tài)的作差
	Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
	// 將tb下的期望角速率旋轉(zhuǎn)至cb下獲取當前姿態(tài)的角速率期望前饋疊加量desired_ang_vel_quat
    Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;

可以看到這段代碼將前饋期望速度從期望姿態(tài)轉(zhuǎn)換到了當前姿態(tài)下,，那么問題就是,，為什么這個期望速度是在期望姿態(tài)下而不是原本就在當前姿態(tài)的呢？

因為在之前的input_euler_angle_roll_pitch_yaw()函數(shù)（其他類似）中,，通過對本次輸入的期望姿態(tài)和自駕儀中當前保存的未更新前的期望姿態(tài)作差求出姿態(tài)誤差,，然后再得到了_attitude_target_ang_vel這個期望角速率。

而在現(xiàn)在我們這個函數(shù)中,，我們是通過期望姿態(tài)與當前姿態(tài)作差得到的姿態(tài)誤差來求取期望角速率_rate_target_ang_vel,。

因此_attitude_target_ang_vel是在期望姿態(tài)tb下的期望角速率，而_rate_target_ang_vel則是在當前姿態(tài)cb下的期望角速率,，疊加前需要將_attitude_target_ang_vel從tb轉(zhuǎn)換到cb下,。

姿態(tài)更新部分

    if (_rate_bf_ff_enabled) {
        // 當開啟機體速率前饋時，實時計算更新期望姿態(tài)四元數(shù),，每一次更新都需要將其單位化
        Quaternion attitude_target_update_quat;
        attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
        _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
        _attitude_target_quat.normalize();
    }

該部分仍然跟之前講解的input_euler_angle_roll_pitch_yaw()及類似函數(shù)有關(guān),，在input_euler_angle_roll_pitch_yaw()中如果開啟了前饋，那么會計算出前饋速率,，但是并沒有將本次輸入的期望姿態(tài)更新到當前保存的期望姿態(tài)中,；而如果沒有開啟前饋，則會將本次輸入的期望姿態(tài)更新到當前期望姿態(tài),，然后將前饋速率置0,。

因此，此處解決了我們之前說過的如果開啟前饋,，姿態(tài)更新將在attitude_controller_run_quat()中更新的疑惑,。

參考資料：

如何用四元數(shù)表示反向旋轉(zhuǎn)，類似于旋轉(zhuǎn)矩陣的轉(zhuǎn)置表示反向旋轉(zhuǎn),？

[飛控]姿態(tài)誤差(三)-四元數(shù)和軸角求誤差