作為學控制的，多軸的姿態控制簡單了解一些，當初看px4代碼時，對其姿態控制算法表示不能理解，又很難找到一些資料詳細解釋，經過仔細的推導，大概對其有了一定的認識，因此記錄下思路，大概解釋下算法代碼，如有錯誤，歡迎指出。

概述：px4的姿態控制，是根據SO3數學基礎，推導旋轉矩陣模型，采用串級PID控制，控制角度環和角速度環。實際上將姿態控制問題轉換成平滑軌跡跟蹤問題。

如圖所示，該姿態控制主要關注如何平滑的轉動。理論上來講，該算法機動性強。
算法代碼詳解： 1.構建姿態設計旋轉矩陣R_sp和當前姿態旋轉矩陣R；R_sp.set(_v_att_sp.R_body);R.set(_v_att.R);2.取出兩個矩陣的Z軸向量，math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));根據這兩個z軸計算出誤差向量（參考坐標系），并轉換到機體坐標系；math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);注：%已經符號重載過了，并不是求余，這里指的是向量叉積運算，后面的*，表示點積。3.計算兩個z軸的誤差角度；由公式a×b=︱a︱︱b︱sinθ，a?b=︱a︱︱b︱cosθ，這里的R_z和R_sp_z又是單位向量，模為1，因此誤差向量e_R?的模就是sinθ，點積就是cosθ。float e_R_z_sin = e_R.length();float e_R_z_cos = R_z * R_sp_z;4.計算yaw誤差角度的權重；yaw_w = R_sp(2, 2) * R_sp(2, 2);5.因為多軸的yaw響應一般比roll/pitch慢一倍，因此將兩者解耦，先補償roll/pitch的變化，計算R_rp；
判定兩個z軸是否存在誤差if (e_R_z_sin > 0.0f)將e_R轉成單位向量；?e_R = e_R_z_axis * e_R_z_angle;計算e_R_z_axis?的叉積矩陣，根據歐拉旋轉公式，求出R_rp；R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));6.現在z軸都已經重合了，只需求yaw的誤差角度；取出兩個矩陣的x軸（現在只有x軸存在誤差）；math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));同樣根據向量的叉積和點積求出誤差角度的正弦和余弦，再反正切求出角度；e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;7.以上旋轉方法，適用于小角度的誤差，當轉動的角度偏大時，還需另一種方法；直接計算參考系到機體系的旋轉矩陣，并轉換成四元數形式；q.from_dcm(R.transposed() * R_sp);math::Vector<3> e_R_d = q.imag();取出虛部，并歸一化，根據e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));同樣得到一個誤差角度向量；求出互補系數direct_w = e_R_z_cos * e_R_z_cos * yaw_w;更新e_R，包含兩種旋轉方法，互補；e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;8.求出角速度的期望值，供內環角速度控制使用；_rates_sp = _params.att_p.emult(e_R);9.yaw響應較慢，因此再加入一個前饋控制；_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
上述用到的相關理論，讀者自己查閱書籍。參考資料：pixhawk源碼 mc_att_control；High performance full attitude control of a quadrotor on SO(3)；捷聯慣性導航；

總結

以上是生活随笔為你收集整理的pixhawk的姿态控制算法解读的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。