相關專業術語：

IMU—(Inertial measurement unit，簡稱 IMU）是測量物體三軸姿態角(或角速率)以及加速度的裝置。

AHRS—（attitude heading reference system ）航姿參考系統       姿態和方位參照系統;

UAV — （Unmanned Aerial Vehicle）  無人駕駛飛機，簡稱無人機；

pitch（俯仰角）、roll（橫滾角）、yaw（航向角）；

       四軸飛行器是微型飛行器的其中一種，相對於固定翼飛行器，它的方向控制靈活、抗干擾能力強、飛行穩定，能夠攜帶一定的負載和有懸停功能，因此能夠很好地進行空中拍攝、監視、偵查等功能，在軍事和民用上具備廣泛的運用前景。 四軸飛行器關鍵技術在於控制策略。

1、 四軸飛行器的結構與基本飛行原理

        四軸飛行器結構主要由主控板和呈十字交叉結構的4個電子調速器、電機、旋漿組成，電機由電子調速器控制，主控板主要負責解算當前飛行姿態、控制電調等功能。 以十字飛航模式為例，l號旋翼為頭，1、3號旋翼逆時針旋轉，2、4號旋翼順時針旋轉，如圖1所示。

        參照飛行狀態表1變化電機轉速，由於四個電機轉速不同，使其與水平面傾斜一定角度，如圖l所示。四個電機產生的合力分解為向上的升力與前向分力。當重力與升力相等時，前向分力驅動四軸飛行器向傾斜角度的方向水平飛行。空間三軸角度尤拉角分為仰俯角、橫滾角、航向角：傾斜角是仰俯角時，向前、向後飛行；傾斜角是橫滾角時，向左、向右飛行；而傾斜航向角時，向左、右旋轉運動，左(右)旋轉是由於順時針兩電機產生的反扭矩之和與逆時針兩電機產生的反扭矩之和不等，即不能相互抵消，機身便在反扭矩作用下繞z軸自旋轉。

2 、姿態解算

        四軸飛行器運用姿態解算計算出空間三軸尤拉角。結構框架如圖2所示，陀螺儀取樣三軸角速度值，加速度感測器取樣三軸加速度值，而磁力感測器取樣得到三軸地磁場值，將陀螺儀、加速度感測器、磁力感測器取樣後的資料進行標定、濾波、校正後得到三軸尤拉角度，其中陀螺儀和加速度感測器選用MPU6050晶片，磁力感測器選用HMC5883L晶片，採用IIC匯流排與主控板通訊。

                                                                       圖2 姿態解算結構圖

       由於感測器存在器件誤差，因此在使用前需要標定。陀螺儀在靜止時，角速度為0；但實際情況由於器件誤差並不為0，因此可在靜止時取樣500次資料，再求平均，得出偏移量，標定陀螺儀資料；加速度感測器可依據在靜止時，三軸重力加速度平方和的開方為重力加速度的標定方程，利用最小二乘法求出標定偏移值和誤差值進行標定。而磁力感測器校正，可將器件靜置於桌面旋轉一週找出最小值和最大值，通過電子羅盤校正計算公式計算出標定偏移值和誤差值。 

        由於陀螺儀長時間採集角速度會有飄移，因此需要使用加速度感測器的值與磁力感測器的值進行校正。將加速度的測量向量和磁場的測量向量與參考向量做叉積後相加 

        式(1)中：ex、ey、ez為兩叉積之和，ax、ay、az為加速度的測量向量，mx、my、mz為磁場的測量向量，axref、ayref、azref為加速度的參考向量，mxref、myref、mzref為磁場的參考向量，參考向量是通過實時四元數值與本次測量值計算出來。 再將叉積修正角速度漂移值：

        式(2)中ωx(t)、ωy(t)、ωz(t)為角速度，kpex(t)為比例項修正，....為積分修正項。 將校正後的角速度通過二階畢卡演算法轉化為四元數，公式如式(3)。 

3、 高度計算

        高度計算是通過氣壓感測器採集的大氣壓值計算出來，將氣壓感測器採集值進行校正後，在通過溫度二階補償，得到準確的大氣壓值，最後經過氣壓轉換為高度公式 

        式(6)中Altitude為計算出來的實際高度，CurrentPressure為當前氣壓值，StartPressure為起飛之前氣壓值。氣壓感測器選用MS5611晶片，其中集成了溫度感測器和氣壓感測器，採用IIC匯流排與主控板通訊。

4 、PID控制

4．1 雙閉環PID控制

        當四軸飛行器正常飛行時，突遇外力(風等)或磁場干擾，使加速度感測器或磁力感測器採集資料失真，造成姿態解算出來的尤拉角錯誤，只用角度單環情況下，使系統很難穩定執行，因此可以加入角速度作為內環，角速度由陀螺儀採集資料輸出，採集值一般不存在受外界影響情況，抗干擾能力強，並且角速度變化靈敏，當受外界干擾時，回覆迅速；同理，高度環中氣壓感測器同樣也會受到外界干擾，引入z軸加速度環可有效避免外界干擾造成的影響，增強了系統的魯棒性。 四軸飛行器雙閉環PID控制，如圖3、圖4所示。角度作為外環，角速度作為內環，進行姿態PID控制；當需要定高時，高度作為外環，z軸加速度作為內環， 進行高度PID控制。其中，PID輸出為油門值，油門給定電子調速器值，電子調速器控制電機使空間三軸尤拉角和高度變化。

                                                                      圖3姿態PID控制總體流程圖

                                                                        圖4高度PID控制總體流程圖

PID控制演算法採用位置式數字PID控制：

           式(7)中u(t)為PID輸出值，e(t)為期望值與實際值之差，為積分量，為微分量，kp，、ki、kd。為比例、積分、微分系數。 在將積分量，微分量離散化得到PID計算公式 

式(8)中T為更新時間。 基於公式(8)，姿態PID控制演算法

        式(9)為角度環PID計算公式，(10)為角速度環PID計算公式。AngelPIDOut(t)為角度環PID輸 出，AngelRatePIDOut(t)為角速度環PID輸出，e(t)=期望角度一實際角度，e＇(t)=AngelPIDOut(t) - 實際角速度。 同理高度PID控制演算法： 

          式(11)為高度環PID計算公式，公式(12)為加速度環PID計算公式，AltitudePIDOut(t)為高度環PID輸 出，AcceleratePIDOut(t)為加速度環PID輸出。e(t)=期望高度一實際高度，e＇(t)=AltitudePIDOut(t) - (z軸加速度 - 重力加速度值)。

4．2 油門輸出計算

        PID輸出值先經過限幅處理，再輸出給油門，防止某些時刻輸出油門值過大，造成過沖，使系統難以穩定，公式如下 AngelOut=AngelPIDOut(t)(限制範圍 - 100到100) (13) AltitudeOut=AcceleratePIDOut(t)(限制範圍 - 10到10) (14) 式中AngelOut。為尤拉角輸出值，AltitudeOut為高度輸出值。 最後經過十字飛航模式油門輸出公式，計算出4個電機輸出油門

        式(15)中throttlelOut到throttle4Out為油門1到油門4輸出值，依據公式(13)輸出的AngelOut分為三軸角度：pitchOut為仰俯角輸出值，rollOut為橫滾角輸出值，yawOut為航向角輸出值。

4．3 油門補償

        以十字飛航模式為例，PID控制條件為線性環境，而給出的油門值與轉速的關係為非線性，且兩對向電機再同一油門下轉速存在差異。運用光電感測器測出不同油門量對應的轉速，通過matlab軟體繪製出該曲線，其中一對向電機油門與轉速的關係如圖5所示，兩電機在相同油門下轉速存在差異且不平行，因此將油門曲 線分段，並通過計算公式 補償油門值 =α(實際油門值 - β) (16) 進行油門補償，式(16)中α、β為補償偏移係數，得出曲線如圖5(b)所示。

                                  圖5 油門 - 轉速曲線圖

5、 測試

       本次測試姿態解算更新週期T ≈ 4 mS（250Hz），PID控制更新週期T ≈ 10 ms（100Hz），期望值為水平0度。

      將四軸飛行器採集的尤拉角值通過無線模組NRF24L01傳送到PC機上，接收的資料顯示到上位機進行分析。上位機顯示橫滾角波形如圖7所示，波形圖橫座標單位為10 ms，縱座標單位為度。 

                                                                 圖 7雙閉環PID控制橫滾角波形圖

         由圖7可知，橫滾角波形經過一大波一小波震盪後近似歸為期望值零點，且超調量較小，系統很快進入穩定狀態。 其他尤拉角測試結果類似。