本文主要寫一下自己對(duì)于APM傾轉(zhuǎn)分離（軸角分離）函數(shù)的一些學(xué)習(xí)筆記及思考,。

ZING已經(jīng)很好地分析了APM軸角分離函數(shù),，以下是我參考的他的一些博文。

[飛控]姿態(tài)誤差(四)-APM如何計(jì)算姿態(tài)誤差
[飛控]傾轉(zhuǎn)分離(一)-APM的傾轉(zhuǎn)分離竟然沒(méi)有效果,？
[飛控]傾轉(zhuǎn)分離(補(bǔ)充)-等效旋轉(zhuǎn)矢量(軸角)與旋轉(zhuǎn)矩陣

后續(xù)分析過(guò)程會(huì)主要引用來(lái)自上面三篇,，侵刪,。

什么是軸角分離

引文來(lái)源：https://zinghd./tilt_torsion_3/
1.起源
姿態(tài)中,，roll 和 pitch 的改變來(lái)自于靠槳直接的力矩調(diào)整，調(diào)整很快,，十幾個(gè)毫秒就能到位,。
而yaw的改變是靠槳速度差產(chǎn)生的旋轉(zhuǎn)力矩來(lái)調(diào)整的調(diào)整比較慢，要快一百個(gè)毫秒才能到位。

2.問(wèn)題
通常旋翼飛機(jī)的80%的能量用于油門控制抵抗重力,，20%的能量用于控制姿態(tài),。
當(dāng) roll pitch yaw 都有較大誤差時(shí)，20%的能量由三個(gè)控制器共同使用,，但是由于 yaw 響應(yīng)較慢,，會(huì)導(dǎo)致 yaw 的誤差一直都比較大，占用大部分姿態(tài)控制的能量,，反而影響了整個(gè)姿態(tài)控制,。

3.思路
因?yàn)樾矸€(wěn)定飛行的第一要素是保證槳平面( roll pitch )的精確控制，即保證槳平面沒(méi)有誤差 ,
yaw是不是沒(méi)有誤差并不重要,。那么我們計(jì)算出真實(shí)姿態(tài)誤差時(shí),，把誤差分成兩個(gè)部分tilt（pitch和roll）和 torsion（yaw），
但是呢,，不直接把 torsion 給控制器
做一點(diǎn)限制，比如,，「限幅」或者「衰減」
然后重新合成一個(gè)處理后的誤差,。
這樣控制器就好認(rèn)為，yaw 的誤差并不大 ,，大部分能量可以留給 tilt（pitch和roll）,。

4.算法步驟
① 通過(guò)對(duì)齊 當(dāng)前機(jī)體坐標(biāo)系 z 軸 和 期望機(jī)體坐標(biāo)系 z 軸 得到 tilt 誤差
② 把 tilt 誤差 轉(zhuǎn)到 地理系 或者 轉(zhuǎn)到 機(jī)體 ③ 總
系誤差 - tilt 誤差 = torsion 誤差
④ 限制 torsion 誤差 可以使用「限幅」方法 或者「衰減」方法
⑤ 把限制后的 torsion 和 tilt 組合成新的總誤差

源碼分析

以下內(nèi)容備注來(lái)自我自己的一些考慮

// 將NED坐標(biāo)系簡(jiǎn)稱n系，機(jī)體坐標(biāo)系簡(jiǎn)稱b系
// 期望姿態(tài)表示為tb,，當(dāng)前姿態(tài)表示為cb（注意cb即為當(dāng)前機(jī)體坐標(biāo)系b系）
// 注意四元數(shù)表示的是三維空間中的旋轉(zhuǎn)而不是點(diǎn)

程序開(kāi)始//
// thrust_heading_rotation_angles() - 計(jì)算兩個(gè)有序旋轉(zhuǎn),，以將att_from_quat四元數(shù)移動(dòng)到att_to_quat四元數(shù)。
// 第一個(gè)旋轉(zhuǎn)校正推力矢量,，第二個(gè)旋轉(zhuǎn)校正航向矢量,。
///上面的是官方注釋
// att_to_quat：傳入?yún)?shù)，期望姿態(tài)四元數(shù),。方向：tb->n
// att_from_quat：傳入?yún)?shù),，當(dāng)前姿態(tài)四元數(shù)。方向：cb->n,。同時(shí)也表示從機(jī)體b系向n系的旋轉(zhuǎn),。
// att_diff_angle：傳入傳出參數(shù)，姿態(tài)誤差
// thrust_vec_dot：傳入傳出參數(shù),，按軸角法進(jìn)行Z軸對(duì)其時(shí)的旋轉(zhuǎn)角
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
{
	///獲取姿態(tài)的期望Z軸和當(dāng)前Z軸/

	// 從四元數(shù)att_to_quat計(jì)算出旋轉(zhuǎn)矩陣att_to_rot_matrix
	// 旋轉(zhuǎn)矩陣att_to_rot_matrix用于將期望姿態(tài)旋轉(zhuǎn)到n系
	// 并把期望姿態(tài)的Z軸轉(zhuǎn)換到n系下的期望Z軸att_to_thrust_vec
	// 旋轉(zhuǎn)方向：tb->n
    Matrix3f att_to_rot_matrix; 
    att_to_quat.rotation_matrix(att_to_rot_matrix); 
    Vector3f att_to_thrust_vec = att_to_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);	// 期望姿態(tài)的標(biāo)準(zhǔn)z軸與旋轉(zhuǎn)矩陣相乘得到n系下的期望z軸表示

	// 從四元數(shù)att_from_quat計(jì)算出旋轉(zhuǎn)矩陣att_from_rot_matrix
	// 旋轉(zhuǎn)矩陣att_from_rot_matrix用于將當(dāng)前機(jī)體坐標(biāo)系cb轉(zhuǎn)換到n系
	// 并將當(dāng)前姿態(tài)的Z軸轉(zhuǎn)換到NED坐標(biāo)系下的期望Z軸att_from_thrust_vec
	// 旋轉(zhuǎn)方向：cb->n
    Matrix3f att_from_rot_matrix;
    att_from_quat.rotation_matrix(att_from_rot_matrix);
    Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);

	///下面通過(guò)軸角法和四元數(shù)配合,，以Z軸誤差計(jì)算tilt傾角誤差/
	// 構(gòu)建軸角（n系下）

	// 叉乘獲取旋轉(zhuǎn)向量（轉(zhuǎn)軸）thrust_vec_cross（叉乘后不一定是單位向量）
	// 如c=a×b，表示繞著向量c將a旋轉(zhuǎn)到b,，|c|=|a|*|b|,，向量c正交于向量a和b
	// thrust_vec_cross即為轉(zhuǎn)軸
	// 旋轉(zhuǎn)方向：att_from_thrust_vec -> att_to_thrust_vec
    Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;

	// 點(diǎn)乘獲取旋轉(zhuǎn)角thrust_vec_dot
	// 表示向量att_from_thrust_vec轉(zhuǎn)向向量att_to_thrust_vec的夾角
	// 由于均為單位向量因此直接求arccos，之后限定在-1~1之間即可
    thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec, -1.0f, 1.0f));

    // 單位化旋轉(zhuǎn)向量thrust_vec_cross（轉(zhuǎn)換為單位向量）
    float thrust_vector_length = thrust_vec_cross.length();
    if (is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)) {
        thrust_vec_cross = Vector3f(0, 0, 1);
        thrust_vec_dot = 0.0f;
    } else {
        thrust_vec_cross /= thrust_vector_length;
}

	// 軸角構(gòu)造完畢，轉(zhuǎn)軸為單位向量thrust_vec_cross,，轉(zhuǎn)角為thrust_vec_dot
	// 旋轉(zhuǎn)方向：n系下向量att_from_thrust_vec（當(dāng)前姿態(tài)z軸）-> 向量att_to_thrust_vec（期望姿態(tài)z軸）

	// 下面開(kāi)始將Z軸對(duì)準(zhǔn),，即通過(guò)Z軸誤差獲取傾角誤差（不考慮Yaw轉(zhuǎn)角）

	// 軸角轉(zhuǎn)換到四元數(shù)
	// 四元數(shù)thrust_vec_correction_quat表示在n系下以thrust_vec_cross為轉(zhuǎn)軸，thrust_vec_dot為轉(zhuǎn)角的旋轉(zhuǎn)過(guò)程
	// 此處似乎是對(duì)軸角和四元數(shù)的旋轉(zhuǎn)方向有爭(zhēng)議
    Quaternion thrust_vec_correction_quat;	
    thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);

	// 將傾角誤差由n系轉(zhuǎn)向機(jī)體坐標(biāo)b系
	// 四元數(shù)att_from_quat表示有機(jī)體b系向n系旋轉(zhuǎn)的過(guò)程,，n = q×b×inv(q)
	// b = inv(q) ×n×q
	// 此處q為單位四元數(shù),，因此其逆等于共軛
	// 注意此處并非標(biāo)準(zhǔn)的四元數(shù)旋轉(zhuǎn)描述，因?yàn)椴皇?標(biāo)量四元數(shù),，詳見(jiàn)下方
	thrust_vec_correction_quat = att_from_quat.inverse() * thrust_vec_correction_quat * att_from_quat;

	// 機(jī)體b系下的傾角誤差獲取完畢

	// 計(jì)算b系下的yaw轉(zhuǎn)角誤差
	// 注意：旋轉(zhuǎn)矩陣B的逆 * 旋轉(zhuǎn)矩陣A 等價(jià)于 旋轉(zhuǎn)A與旋轉(zhuǎn)B作差,，由A減去B（但不是減法的意思，只是表明旋轉(zhuǎn)姿態(tài)的差異）
	// 依次左乘：att_from_quat.inverse() * att_to_quat = 期望姿態(tài) – 當(dāng)前姿態(tài) = 姿態(tài)總誤差（total error）
	// thrust_vec_correction_quat.inverse() * total error = 姿態(tài)總誤差 – 傾角誤差（tilt error） = yaw轉(zhuǎn)角誤差（torsion error）
    Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * att_from_quat.inverse() * att_to_quat;

	// 傾角誤差四元數(shù)轉(zhuǎn)換回軸角
	// 并通過(guò)att_diff_angle獲取到roll和pitch上的誤差角
    Vector3f rotation;
    thrust_vec_correction_quat.to_axis_angle(rotation);
    att_diff_angle.x = rotation.x;
    att_diff_angle.y = rotation.y;

	// 轉(zhuǎn)角誤差四元數(shù)轉(zhuǎn)換回軸角形式
	// 通過(guò)att_diff_angle獲取到y(tǒng)aw上的誤差角（roll和pitch上的應(yīng)該為0）
    yaw_vec_correction_quat.to_axis_angle(rotation);
	att_diff_angle.z = rotation.z;

	// 到此處獲得了傾轉(zhuǎn)的所有姿態(tài)誤差,，然而并沒(méi)有對(duì)YAW角誤差進(jìn)行限制

    // Todo: Limit roll an pitch error based on output saturation and maximum error.

    // 基于最大加速度限制偏航誤差-更新以包括輸出飽和度和最大誤差
    // 當(dāng)前,，該限制基于使用SQRT控制器的線性部分的最大加速度
    // 應(yīng)該將其更新為基于角度限制，飽和度或基于用戶定義的參數(shù)
    if (!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()) {
        att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP());
        yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f, 0.0f, att_diff_angle.z));
        att_to_quat = att_from_quat * thrust_vec_correction_quat * yaw_vec_correction_quat;
    }
}

一些細(xì)節(jié)補(bǔ)充

關(guān)于數(shù)學(xué)原理部分,，我之前已經(jīng)有一篇博文匯總過(guò)了：APM姿態(tài)旋轉(zhuǎn)理論基礎(chǔ),。因此其中一些數(shù)學(xué)原理的函數(shù)如軸角與四元數(shù)、四元數(shù)與旋轉(zhuǎn)矩陣之間的相互轉(zhuǎn)換此處不再補(bǔ)充,。

另外關(guān)于叉乘和點(diǎn)乘的定義,，可以參看這篇：向量?jī)?nèi)積（點(diǎn)乘）和外積（叉乘）概念及幾何意義

// 軸角轉(zhuǎn)換到四元數(shù)
// 四元數(shù)thrust_vec_correction_quat表示在n系下以thrust_vec_cross為轉(zhuǎn)軸，thrust_vec_dot為轉(zhuǎn)角的旋轉(zhuǎn)過(guò)程
// 此處似乎是對(duì)軸角和四元數(shù)的旋轉(zhuǎn)方向有爭(zhēng)議
Quaternion thrust_vec_correction_quat;	
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);

關(guān)于上面這段函數(shù),，參考的博文：姿態(tài)誤差(四)-APM如何計(jì)算姿態(tài)誤差,，認(rèn)為軸角轉(zhuǎn)換到四元數(shù)后旋轉(zhuǎn)方向改變，軸角旋轉(zhuǎn)：當(dāng)前->期望,，而轉(zhuǎn)換成四元數(shù)后：期望->當(dāng)前,。理由是軸角法描述的是向量的旋轉(zhuǎn)，而四元數(shù)描述的是坐標(biāo)系的旋轉(zhuǎn),。

具體可以參照這篇解釋博文：[飛控]傾轉(zhuǎn)分離(補(bǔ)充)-等效旋轉(zhuǎn)矢量(軸角)與旋轉(zhuǎn)矩陣

然而參照軸角轉(zhuǎn)換為四元數(shù)以及四元數(shù)表示旋轉(zhuǎn)的公式（懶得打了）,，我個(gè)人依舊認(rèn)為旋轉(zhuǎn)方向是不變的（望打臉）。

不過(guò)此處相對(duì)來(lái)說(shuō)并不那么重要,，先了解姿態(tài)誤差計(jì)算流程即可,。

效果顯示及進(jìn)一步修改

MATLAB代碼來(lái)自：[飛控]傾轉(zhuǎn)分離(一)-APM的傾轉(zhuǎn)分離竟然沒(méi)有效果？

如上圖所示,，期望姿態(tài)歐拉角表示為(60, 60, 25),，att_diff_angle誤差為(0.8194, 1.0325, -0.2072)

綠色表示當(dāng)前姿態(tài)，與黑色NED坐標(biāo)系重合,，紅色表示期望姿態(tài),，藍(lán)色表示APM中軸角分離之后得到的att_diff_angle誤差，然而可以看到其與期望姿態(tài)的Z軸并沒(méi)有對(duì)齊,，但是APM程序中明明是按照對(duì)齊Z軸進(jìn)行姿態(tài)誤差計(jì)算的,，這是為什么呢,？

因?yàn)锳PM程序中并沒(méi)有對(duì)傾轉(zhuǎn)分離后的z軸誤差做處理，忽略處理了傾斜時(shí)產(chǎn)生的z軸誤差,，抑制效果不好,。

正確做法是對(duì)torsion誤差（YAW）按一定的比例進(jìn)行縮小，限制旋轉(zhuǎn),，然后用四元數(shù)乘法,，重新疊加兩個(gè)旋轉(zhuǎn)，得到新的總旋轉(zhuǎn),。

在att_diff_angle.z = rotation.z; 這段代碼后面,，可添加如下更新

// 以下內(nèi)容未經(jīng)過(guò)驗(yàn)證，僅提供思路
att_diff_angle.z = 0.5 * att_diff_angle.z,；// torsion誤差比例縮小進(jìn)行衰減
yaw_vec_correction_quat.from_axis_angle(att_diff_angle.z);	// torsion誤差轉(zhuǎn)換為四元數(shù)形式
Quaternion att_diff_angle_quat = thrust_vec_correction_quat * yaw_vec_correction_quat;	// torsion和tilt旋轉(zhuǎn)疊加
att_diff_angle_quat.to_axis_angle(rotation);	// 轉(zhuǎn)換為軸角
att_diff_angle.x = rotation.x;
att_diff_angle.y = rotation.y;
att_diff_angle.z = rotation.z;

MATLAB實(shí)現(xiàn)效果如下,，黑色部分為經(jīng)過(guò)torsion誤差衰減之后的att_diff_angle。誤差表示為(0.7651, 1.0740, -0.0881)

結(jié)果對(duì)比可發(fā)現(xiàn)經(jīng)調(diào)整之后的黑色坐標(biāo)系相對(duì)于藍(lán)色坐標(biāo)系,，Z軸已經(jīng)完成對(duì)準(zhǔn),。