**px4的姿態控制分為角度環(外環)和角速度環(內環)，角度環使用p控制，角速度環使用pid控制，由於偏航通道響應較慢(多旋翼飛行器的俯仰和滾轉運動由旋翼的公升力力矩產生，偏航運動由旋翼的反扭矩來產生，而公升力力矩要比反扭矩大得多(可從旋翼的公升力係數和反扭矩係數中看出)，這造成了偏航運動能力相比滾轉和俯仰運動能力要弱)，故在偏航通道內環加入前饋環節；
px4的姿態控制部分使用的是roll-pitch和yaw分開控制的（是為了解耦控制行為），即tilt和torsion兩個環節。如下圖：
根據經驗所得，控制toll-pitch比控制yaw更容易實現。比如同樣是實現10°的變化，roll-pitch需要60ms左右；但是yaw控制器卻需要接近150ms。**使用分步控制的優點在於解耦控制行為，即分別執行響應較快的動作和響應較慢的動作；同時，相比傳統的rpy三軸解耦姿態控制，三個姿態通道的姿態動作範圍更小，能耗更小。**下圖則比較形象地表示了這一點：
;此處，r_sp表示期望姿態角所對應的期望機體系到地理座標系的旋轉矩陣；r：根據當前的姿態角得到當前機體系到地理座標系的旋轉矩陣；
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
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))
;
/* axis and sin(angle) of desired rotation */
math:
:vector<
3> e_r = r.
transposed()
*(r_z % r_sp_z)
;
/* calculate angle error */
float e_r_z_sin = e_r.
length()
;float e_r_z_cos = r_z * r_sp_z;
/* calculate weight for yaw control */
float yaw_w =
r_sp(2
,2)*
r_sp(2
,2);
/* calculate rotation matrix after roll/pitch only rotation */
math:
:matrix<3,
3> r_rp;
if(e_r_z_sin >
0.0f
)else
/* r_rp and r_sp has the same z axis, calculate yaw error */
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;
if
(e_r_z_cos <
0.0f
)
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.
emult
(e_r)
;//外環p控制
/* limit yaw rate */
_rates_sp(2
)= math:
:constrain
(_rates_sp(2
),-_params.yaw_rate_max, _params.yaw_rate_max)
;//限幅
/* feed forward yaw setpoint rate */
_rates_sp(2
)+= yaw_sp_move_rate * yaw_w * _params.yaw_ff;
//前饋
向量點乘
a•b=︱a︱︱b︱cosθ
叉乘運算元
將向量叉乘形式表示為叉乘矩陣與向量的乘積
pixhawk之姿態控制篇
pixhawk原生韌體px4之位姿控制演算法解讀
關於px4原始碼中固定翼姿態控制
在看完px4固定翼的姿態控制 後，我有幾點思考 1.我們知道程式中姿態控制的方法是將控制角度轉化為控制角速度。那pitch來說，先求出設定的pitch和當前的pitch的差值 delta pitch 然後將delta pitch除以時間常數得到目標pitch rate。接著將目標pitch rate...
PX4原始碼分析4 PX4軟體結構
px4自動駕駛儀軟體可分為三大部分 實時作業系統 中介軟體和飛行控制棧。提供posix style的使用者操作環境，進行底層的任務排程。px4中介軟體執行於作業系統之上，提供裝置驅動和乙個微物件請求 micro object request broker，uorb 用於駕駛儀上執行的單個任務之間的非...
PX4二次開發 基於mahony演算法的姿態估計
px4裡面有兩種姿態估計演算法，一種是基於ekf的，還有一種是基於mahony的，雖然mahony演算法簡單而且計算量少，但效能並不輸ekf多少。這裡我們講一下基於mahony的姿態估計演算法，本來我還想在正式開始前為大家補充一下座標系 四元數 尤拉角和旋轉矩陣等一些基礎知識，不過筆者最近又是期末考...