姿態(tài)解算

　　姿態(tài)解算(attitude algorithm),是指把陀螺儀，加速度計(jì), 羅盤等的數(shù)據(jù)融合在一起，得出飛行器的空中姿態(tài)，飛行器從陀螺儀器的三軸角速度通過四元數(shù)法得到俯仰，航偏，滾轉(zhuǎn)角，這是快速解算，結(jié)合三軸地磁和三軸加速度得到漂移補(bǔ)償和深度解算。

　　姿態(tài)的數(shù)學(xué)模型坐標(biāo)系

　　姿態(tài)解算需要解決的是四軸飛行器和地球的相對姿態(tài)問題。地理坐標(biāo)系是固定不變的，正北，正東，正上構(gòu)成了坐標(biāo)系的X，Y，Z軸用坐標(biāo)系R表示，飛行器上固定一個(gè)坐標(biāo)系用r表示，那么我們就可以適用歐拉角，四元數(shù)等來描述r和R的角位置關(guān)系。

　　姿態(tài)的數(shù)學(xué)表示

　　姿態(tài)有多種數(shù)學(xué)表示方式，常見的是四元數(shù)，歐拉角，矩陣和軸角。在四軸飛行器中使用到了四元數(shù)和歐拉角,姿態(tài)解算的在于旋轉(zhuǎn)。姿態(tài)解算中使用四元數(shù)來保存飛行器的姿態(tài)，包括旋轉(zhuǎn)和方位。在獲得四元數(shù)之后，會(huì)將其轉(zhuǎn)化為歐拉角，然后輸入到姿態(tài)控制算法中。姿態(tài)控制算法的輸入?yún)?shù)必須要是歐拉角。AD值是指MPU6050的陀螺儀和加速度值，3個(gè)維度的陀螺儀值和3個(gè)維度的加速度值，每個(gè)值為16位。AD值必須先轉(zhuǎn)化為四元數(shù)，然后通過四元數(shù)轉(zhuǎn)化為歐拉角。在四軸上控制流程如下圖：

　　下面是用四元數(shù)表示飛行姿態(tài)的數(shù)學(xué)公式，從MPU6050中采集的數(shù)據(jù)經(jīng)過下面的公式計(jì)算就可以轉(zhuǎn)換成歐拉角，傳給姿態(tài)PID控制器中進(jìn)行姿態(tài)控制.

　　PID控制算法

　　先簡單說明下四軸飛行器是如何飛行的,四軸飛行器的螺旋槳與空氣發(fā)生相對運(yùn)動(dòng)，產(chǎn)生了向上的升力，當(dāng)升力大于四軸的重力時(shí)四軸就可以起飛了。四軸飛行器飛行過程中如何保持水平：我們先假設(shè)一種理想狀況：四個(gè)電機(jī)的轉(zhuǎn)速是完全相同的是不是我們控制四軸飛行器的四個(gè)電機(jī)保持同樣的轉(zhuǎn)速，當(dāng)轉(zhuǎn)速超過一個(gè)臨界點(diǎn)時(shí)（升力剛好抵消重力）四軸就可以平穩(wěn)的飛起來了呢？答案是否定的，由于四個(gè)電機(jī)轉(zhuǎn)向相同，四軸會(huì)發(fā)生旋轉(zhuǎn)。我們控制四軸電機(jī)1和電機(jī)3同向，電機(jī)2電機(jī)4反向，剛好抵消反扭矩，巧妙的實(shí)現(xiàn)了平衡, 但是實(shí)際上由于電機(jī)和螺旋漿本身的差異，造成我們無法做到四個(gè)電機(jī)產(chǎn)生相同的升力，這樣飛行器起飛之后就會(huì)失去平衡。

　　PID控制器

　　PID控制器通過PID三個(gè)參數(shù)來對被控對象進(jìn)行控制，是應(yīng)用廣泛一種的控制器

　　比例(P)控制器

　　比例控制是一種簡單的控制方式。其控制器的輸出與輸入誤差信號成比例關(guān)系。當(dāng)僅有比例控制時(shí)系統(tǒng)輸出存在穩(wěn)態(tài)誤差。

　　積分(I)控制器

　　在積分控制中，控制器的輸出與輸入誤差信號的積分成正比關(guān)系。對一個(gè)自動(dòng)控制系統(tǒng)，如果在進(jìn)入穩(wěn)態(tài)后存在穩(wěn)態(tài)誤差，則稱這個(gè)控制系統(tǒng)是有穩(wěn)態(tài)誤差的 或簡稱有差系統(tǒng)（System with Steady-state Error）。為了消除穩(wěn)態(tài)誤差，在控制器中必須引入“積分項(xiàng)”。積分項(xiàng)對誤差取決于時(shí)間的積分，隨著時(shí)間的增加，積分項(xiàng)會(huì)增大。這樣，即便誤差很小，積 分項(xiàng)也會(huì)隨著時(shí)間的增加而加大，它推動(dòng)控制器的輸出增大使穩(wěn)態(tài)誤差進(jìn)一步減小，直到等于零。因此，比例+積分(PI)控制器，可以使系統(tǒng)在進(jìn)入穩(wěn)態(tài)后無穩(wěn)態(tài)誤差。

　　微分(D)控制器

　　微分調(diào)節(jié)就是偏差值的變化率。使用微分環(huán)節(jié)能夠?qū)崿F(xiàn)系統(tǒng)的超前控制。如果輸入偏差值線性變化，則在調(diào)節(jié)器輸出側(cè)疊加一個(gè)恒定的調(diào)節(jié)量。大部分控制系統(tǒng)不需要調(diào)節(jié)微分時(shí)間。因?yàn)橹挥袝r(shí)間滯后的系統(tǒng)才需要附加這個(gè)參數(shù)。如果畫蛇添足加上這個(gè)參數(shù)反而會(huì)使系統(tǒng)的控制受到影響。

　　四軸飛行器中的控制器

　　目前四軸飛行器中所使用的是增量是PD控制器，下面以ROLL方向?yàn)槔?

　　測得ROLL軸向偏差

　　偏差 = 目標(biāo)期望角度 – 傳感器實(shí)測角度

　　DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.Roll;

　　比例項(xiàng)計(jì)算

　　比例項(xiàng)輸出 = 比例系數(shù)P * 偏差

　　Proportion = PID_Motor.P * DIF_ANGLE.x;

　　微分計(jì)算

　　微分輸出 = 微分系數(shù) × 角速度

　　整合結(jié)果

　　ROLL方向總控制量 = 比例項(xiàng)輸出+微分量輸出

　　電機(jī)輸出

　　Motor[2] = (int16_t)(Thr - Pitch - Roll - Yaw );    //M3

　　Motor[0] = (int16_t)(Thr + Pitch + Roll - Yaw );    //M1

　　Motor[3] = (int16_t)(Thr - Pitch + Roll + Yaw );    //M4

　　Motor[1] = (int16_t)(Thr + Pitch - Roll + Yaw );    //M2

　　旋轉(zhuǎn)規(guī)則

　　Roll方向旋轉(zhuǎn)，則電機(jī)1電機(jī)2同側(cè)出力，電機(jī)0電機(jī)3反向出力

　　Pitch方向旋轉(zhuǎn)，則電機(jī)2電機(jī)3同側(cè)出力，電機(jī)0電機(jī)1反向出力

　　Yaw方向旋轉(zhuǎn)，則電機(jī)1電機(jī)3同側(cè)出力，電機(jī)0電機(jī)2反向出力

　　目前二十二個(gè)無人機(jī)飛控群又細(xì)分了軟件、硬件、培訓(xùn)、視覺等群，可說明方向加管理員微信 Gadministraor 向組織靠攏。