四旋翼飛行姿態測試平臺設計及穩定性分析

高佳佳 高嵩 曹凱 陳超波

摘 要：四旋翼飛行器非線性、強耦合以及欠驅動的特性極大地增加了算法設計與姿態控制的難度。為了減少四旋翼飛行器飛行過程中不必要的干擾因素和事故率，自主設計了四旋翼飛行姿態測試平臺。通過對系統進行受力分析和動態建模，采用能夠快速穩定的雙閉環串級PID控制算法設計了姿態控制器，并使用卡爾曼濾波算法進行姿態估計，然后分別在Matlab環境和平臺上驗證了飛行姿態的穩定性。實驗結果表明了平臺設計的合理性和可行性，是一種有效的四旋翼飛行姿態測試平臺。

關鍵詞：四旋翼飛行器；測試平臺；姿態控制；雙閉環串級PID控制；卡爾曼濾波

中圖分類號：TP29 文獻標識碼：A

Abstract： The quadrotor aircraft has the characteristics of nonlinearity，strong coupling and underactivity，which greatly increase the difficulty of algorithm design and the attitude control.In order to reduce the non-essential disturbance factors and accident rate during the flight of the aircraft，and independently designing a test platform to test quadrotor flight attitude.Through the force analysis and dynamic modeling of the system，the attitude controller is designed by a double closed-loop cascaded PID control algorithm which can stabilize quickly，and the attitude is estimated using Kalman filter algorithm.The stability of flight attitude is verified respectively on Matlab environment and the platform.The experimental results show that the platform design is reasonable and feasible，and it is an effective test platform.

Keywords： quadrotor aircraft；test platform；attitude control；double closed-loop cascade PID control；Kalman filter

1 引 言

四旋翼飛行器具有機動靈活、操縱方便、垂直起降的優點，可執行的任務相對復雜化和多樣化，諸如監視、營救、電力巡線和物流快遞等繁瑣危險的任務[1]。但是在飛行過程中存在許多不確定的因素，具有復雜的空氣動力學特性，因此，學者們利用各種控制算法來設計飛行器控制系統[2-4]，如反步控制、非線性魯棒控制、LQR控制器等。

為了減少四旋翼飛行器飛行過程中的非必要干擾因素及事故率，遵循國家限定飛行條例，四旋翼飛行器室內測試平臺的設計變得尤為重要。文獻[5]中在室內分布一圈紅外攝像頭和Qball-X4飛行器構建實驗測試環境，利用紅外攝像頭對飛行器進行定位來確定飛行器飛行是否穩定，如圖1（a）所示，測量精度高和實時響應速度快，但是定位測量的方法較為復雜，利用無線網絡進行通信，容易出現信號干擾甚至中斷，成本也相當昂貴。文獻[6]中三軸半實物仿真轉臺主要是模擬飛行器運動的三維姿態，快速驗證控制算法的有效性，但是結構復雜，體積龐大，造價較高，如圖1（b）所示。文獻[7][8]中測試平臺使用一根剛性支桿固定在地面底座上，頂端采用三個萬向節和拉繩固定連接飛行器，如圖

1（c）所示。雖然飛行器的姿態沒有受到較大影響，但是限制了X和Y方向上的一部分運動，形成小規模的擾動，從而影響控制穩定性。文獻[9]的測試平臺是在地面底座安裝180°萬向球柱，剛性桿與飛行器直接固定連接，如圖1（d）所示。雖然可以解放X和Y方向上的一部分運動，但是對飛行姿態的影響較大，還會受到剛性桿的一部分反作用力，建模分析和采用控制律時，需要考慮更多的影響因素，增加了控制的復雜度。

本文中，搭建了一種成本低廉，又盡可能不影響飛行器飛行狀態的測試平臺，在地面底座上安裝2自由度球頭柱關節，形成X、Y軸的運動，抵消一部分剛性桿產生的反作用力，減少系統擾動，采用360°萬向節連接飛行器，解決姿態限制問題，雖然鎖定了一部分自由度，但是降低了控制的復雜度。

大量實驗結果表明，非線性魯棒控制可以取得較好的飛行控制效果，反步控制方法在姿態角控制方面表現更佳。但是都依賴于精準的數學模型，工程計算量大，數據的實時處理要求較高，實現起來具有一定困難。為此本文設計了一種易于實現，不需要精準的模型，能夠快速穩定的雙閉環串級PID控制算法對姿態角進行控制，并在自主設計的測試平臺上進行受力分析和動態建模，驗證了基于雙閉環串級的四旋翼飛行器姿態控制的穩定性。仿真實驗和實際試飛結果表明，在自主設計的測試平臺上，基于該算法的四旋翼飛行器的姿態控制具有較好的控制效果。

2 四旋翼飛行器動作原理

四旋翼飛行器在飛行時的動作可分為6種，即沿軸向的3種平行運動和圍繞軸向的3種旋轉運動。平行運動為沿著x，y，z方向的運動，主要研究飛行器的位置，旋轉運動則可分為橫滾運動（Roll）、俯仰運動（Pitch）、偏航運動（Yaw），主要分析飛行器的姿態。參考系一般分為大地坐標系和機體坐標系。大地坐標系（慣性坐標系或者導航坐標系，用e或者G表示）用于研究飛行器相對大地的運動狀態以及空間位置坐標。機體坐標系（用b或B）坐標原點取機體的重心，用于研究飛行器相對于重心的旋轉運動。四旋翼飛行器動力模型如圖2所示，xB方向指向飛行器的前（橫滾軸）yB方向指向飛行器的右（俯仰軸）和zB方向指向飛行器的下（偏航軸）方向[10]。

四旋翼飛行器產生基本動作的原理為：為了抵消旋翼飛行器陀螺效應和空氣動力扭矩效應，四旋翼飛行器上兩兩相鄰的電機轉速方向是相反的，保證4個電機轉速一致時機身不發生轉動。電機1和2轉速同時增大或減小，與此同時電機3和4轉速減小或者增大，四旋翼飛行器產生橫滾運動。電機1和4轉速減小或者增大，同時電機2和3轉速增大或者減小，四旋翼飛行器產生俯仰運動。電機1和3轉速同時增大或者減小，電機2和4電機轉速同時減小或者增大，四旋翼飛行器產生偏航運動。4個電機轉速同時增大或這減小，升力大于重力向上運動，升力小于重力向下運動[11]。

3 四旋翼飛行器姿態解算和動力計算

3.1 姿態解算

歐拉角是基于飛行器本身軸旋轉得到的（與參考坐標系無關），然而得到的飛行器姿態卻是相對于參考坐標系而言的。四旋翼飛行器飛行時機體坐標和大地坐標系之間的變化可視為三次繞著定軸的旋轉，繞軸順序為Z-Y-X，如圖3所示[12]。旋轉之后的矩陣即為機體坐標系b到大地坐標系e的轉換矩陣，如公式1所示。

歐拉角的微分方程包含大量的三角函數運算，給四旋翼飛行器姿態解算造成不便，況且“萬向節死鎖”的存在，導致歐拉角方法并不適用于全姿態四旋翼飛行器的姿態確定[13]。由此我們引入四元數姿態解算，而四元數線性方程組計算量小，易于操作，比較貼近工程實際。

推導得到的四旋翼飛行器模型與實際飛行中的動態模型略有簡化，如忽略空氣阻力、球面萬向節摩擦，輕質拉桿的重量等，加入的動態模型影響參數越多，對應的控制算法的復雜程度也會隨之增加，因此忽略了一部分對系統影響較小的因素，有利于控制算法的設計。

4 四旋翼飛行器測試平臺

在旋翼飛行器開發測試階段，因為仿真實驗平臺的大量缺乏，導致飛行器復發性故障如傳感器飽和、電機故障以及傳輸信號中斷導致失控。其次，國家頒布的限制飛行區域、天氣影響和遠距離數據傳輸的數據包缺失等對室外飛行的限制和影響條件的增加。

對比相關飛行器測試平臺資料，文獻[5]中測試平臺的測量精度和準確性較高，但是定位測量算法復雜，無線網傳輸容易受到干擾，成本也相當昂貴。文獻[6]的半實物仿真平臺一般是模擬飛行器的飛行狀態，驗證控制算法的有效性。文獻[7][8]中測試平臺限制了飛行器X和Y方向的運動，會對橫滾和俯仰角度形成小規模擾動。文獻[9]測試平臺一定程度上限制了飛行姿態，還容易受到飛行器對桿的反作用力。因此，針對于它們存在的一些問題，搭建了如圖5所示的測試平臺。

測試平臺主要由以下五個部分組成：①四旋翼飛行器機架（聚酯纖維復合材料，重量大約為450 g）；②飛行控制板（集成慣性測量單元、GPS模塊等）；③電機模塊（直流無刷電機和20 A電子調速計）；④帶有360°萬向節的輕質拉桿；⑤2自由度球頭柱關節。

考慮實驗場地、成本以及易于操作等因素，本文在地面底座上加裝2自由度球頭柱關節，以便于可以解放一部分飛行器在X和Y軸的運動，抵消剛性桿對飛行器的部分反作用力，從而減少系統誤差。采用360°萬向節連接固定飛行器，解決飛行姿態限制問題，飛行器可以靈活的進行姿態調節，從而增強控制穩定性。這種設計可以帶來兩方面的好處，一方面是測試平臺可以幫助鎖定X-Y-Z方向上的一部分自由度，簡化了控制模型，降低了飛行器控制的復雜程度，并且減少了飛行過程中飛行器因為設定的參數不正確或者通信錯誤出現失控而造成的損壞。另一方面，操作人員可以直觀地觀察到飛行過程中飛行器的飛行狀態，以及飛行的穩定性。如果飛行器在誤差范圍內可以穩定懸停，輕質拉桿和萬向節都會隨著保持直立，即當系統穩定運行時，只是約束了其在Z方向上的運動。反之，則說明當前系統不具有穩定性，需要重新調整參數。

系統設計中，預先對應地設計了手動操作和自動操作兩套測試方案，手動操作方案通過操控2.4 GHz無線遙控器來控制四旋翼飛行器懸停在控制，這種方案的優點是通過操作員的視覺反饋，迅速將飛行器上升并懸停，但是手動操作很可能使飛行器沿Z軸方向的力較大，從而飛行器會受到支桿對于它的反作用力，最終影響穩定性。自動操作方案使用PC端通過串口對帶有慣性測量單元（3軸加速度計+3軸陀螺儀）的MCU（微控制器）下發飛行指令，然后MCU給電機下發實時控制指令，最終達到懸停位置，這種方案實現了完全意義上的無人機自主飛行，但是調整時間較長，到達目標位置需要依靠較多的傳感器回傳的數據，在多傳感器數據融合使用時，就容易出現誤差或冗余數據。

當飛行器穩定懸停后，飛行狀態信息以及姿態信息可以通過無線數傳模塊（WIFI、藍牙等方式）以及有線串口進行傳輸，為了減輕機身重量，本文采用控制器自帶的MicroUSB接口進行有線傳輸。控制器將飛行器目前的姿態數據（三維方向角Pitch、Roll、Yaw、加速度計、氣壓計、磁力計、陀螺儀）通過串口傳輸到電腦端，并自動繪制成相應的曲線，通過對比分析曲線可以清晰的判斷四旋翼飛行器飛行狀態的穩定性。

5 四旋翼飛行器控制算法

5.1 雙閉環串級PID控制

在四旋翼飛行器正常飛行時，遇到磁場干擾或者強外力時，會使磁力計和加速度傳感器采集到的數據部分失真或者有大的偏差，導致歐拉角解算錯誤，進一步的控制它的角速度，可以增加飛行器的穩定性（增加阻尼）并提高它的控制效果，于是角度-角速度串級PID控制算法應運而生。雙閉環系統的引入就解決了此問題，由于陀螺儀采集到的角速度數據，抗干擾能力強，并且角速度變化靈敏。同樣的高度環中氣壓傳感器也會受到外界干擾，因而在內環中引入Z軸的加速度分量，增強了系統的魯棒性[15]。

四軸飛行器雙閉環串級PID控制，圖6（a）為姿態環PID控制整體框圖、圖6（b）是高度環PID控制整體框圖。角度作為外環，角速度作為內環，進行姿態PID控制；定高模式下，高度作為外環，Z軸加速度作為內環，進行高度PID控制。其中，PID輸出為油門值，并通過電子調速器控制電機使空間三軸歐拉角和高度變化[16]。

計算出來實際四旋翼飛行器飛行高度，其中BP為當前氣壓值、BS為起始氣壓值，選用的氣壓傳感器中集成了溫度傳感器和氣壓傳感器。

5.2 濾波算法

四旋翼飛行器系統中如果使用一些簡單的數據處理方式很難滿足精度和實時性上的要求。本文中，采用卡爾曼濾波將加速度傳感器和陀螺儀的數據進行融合，抑制噪聲的干擾使得四旋翼飛行器達到穩定狀態，提高測量精度[18]。

卡爾曼濾波是一種最優線性狀態估計方法，輸入當前的測量值和上一個周期的估計值，估計當前的狀態，然后估計當前傳感器數據和上上一個狀態的數據，為當前最優估計。例如位置估計時，就是位置坐標輸入的傳感器數據，最后輸出的最優估計用來作為四旋翼飛行器當前位置被導航算法以外的程序調用。算法的核心思想就是根據傳感器當前的測量值和上一時刻的預測量和誤差，計算當前的最優量，再預測下一時刻的值[19][20]。

6 實驗分析與結論

穩定懸停的指標可以通過三個軸上的加速度和陀螺儀上橫滾、俯仰、偏航角度反映出來。橫滾和俯仰的角度偏差越小以及偏航角度的變化量越小說明系統的穩定性越好。為了更加真實地模擬飛行器的穩定懸停，在Matlab2016a中構建了模擬飛行器，設定高度為1 m，橫滾角和俯仰角為0°（0 rad），偏航角為30°（0.5236 rad），，在啟動仿真后，可以看到模擬飛行器起飛到穩定懸停的過程，如圖7所示。

Matlab仿真輸出的橫滾角度、俯仰角度和偏航角度分別如圖8（a）、（b）、（c）所示。每個圖分為左中右三部分，分別表示實際測量值、期望值以及角度控制的輸出值。以圖8（a）為例，測量的橫滾角度出現波動是因為飛行器在接近期望值時進行微調，同時可以看到角度控制輸出值也在實時響應。

在室內測試平臺上進行實際穩定懸停時，使用手動控制，起飛穩定后轉換為穩定懸停檔，使飛行器可以穩定懸停。圖9（a）、（b）、（c）分別為四旋翼飛行器在實際飛行中橫滾角度、俯仰角度、偏航角度。由于飛行器在飛行時受到反向氣流等外界影響，實驗結果與期望值存在誤差也是在所難免。

由圖9（a）和（b）可以直觀地看到平穩飛行狀態下橫滾角度和俯仰角度分別穩定在1.62°（0.028rad）和0.32°（0.005rad）左右，均在穩定誤差范圍以內，滿足實驗要求。從圖9（c）可知，平穩飛行狀態下偏航角度誤差基本穩定在-78.95°附近，單位時間的角度變化量為±0.5°（0.0087rad），在穩定誤差范圍以內，滿足實驗要求。

采用不需要精準數學模型，可以快速穩定的雙閉環串級PID控制算法進行位姿的閉環控制，一方面，驗證了四旋翼飛行器在自主設計的測試平臺上的姿態穩定性，另一方面，證明了經典控制器在小擾動情況下控制方位角的能力。仿真數據與實際飛行獲取的數據相比，可以看出由于室內反向氣流、測試平臺的機械誤差等因素的影響，理論數據和實際數據之間雖然存在一定的誤差，但是對于四旋翼飛行器的姿態控制依舊具有良好的控制效果，各項指標均可以控制在誤差范圍之內，表明了該平臺設計的合理性和可行性，是一套有效的四旋翼飛行姿態測試平臺。未來的目標是優化姿態控制器和平臺的設計，在此基礎上開發一套完全自主飛行器。

參考文獻

[1] 何道敬，杜曉，喬銀榮.無人機信息安全研究綜述[J/OL].計算機學報，2017：1—21.

[2] 張鐳，李浩.四旋翼飛行器模糊PID姿態控制[J].計算機仿真，2014，31（08）：73—77.

[3] 甄紅濤，齊曉慧，夏明旗，等.四旋翼無人機魯棒自適應姿態控制[J].控制工程，2013，20（05）：915—919.

[4] XU G，MO Z.Modified adaptive flight control of quadrotor based on single neuron PID[C]. International Conference on Information Science and Technology.Yangzhou，China，IEEE，2013：313—316.

[5] 張友民，余翔，劉丁，等.四旋翼無人系統的容錯控制算法設計與實現[J].控制工程，2016，23（12）：1874—1882.

[6] 高鵬飛，鄭云龍.某型無人直升機飛行控制系統半實物仿真[J].兵工自動化，2017，36（06）：90-92+96.

[7] BOUABDALLAH S，SIEGWART R.Backstepping and sliding-mode techniques applied to an indoor micro quadrotor[C].IEEE International Conference on Robotics and Automation.2006，3：2247—2252.

[8] BOUABDALLAH S，SIEGWART R.PID vs LQ control techniques applied to an indoor micro quadrotor[C].IEEE Internationel Conference on Intelligent Robots and Systems 2004，3：2451—2456.

[9] TAYEBI A，MCGILVRAY S. Attitude stabilization of a VTOL quadrotor aircraft[J].IEEE Transactions on Control Systems Technology，2006，14（3）：562-571.

[10] 宋宇，宋雋煒.四旋翼無人機的姿態穩定控制器設計仿真[J].計算機仿真，2017，（08）：85—88.

[11] 趙常均，宮勛，白越，等.四旋翼飛行器中升力波動的干擾與抑制[J].光學精密工程，2014，22（09）：2431—2437.

[12] 張帥，王志萍，夏飛.四旋翼飛行器姿態的非線性控制仿真研究[J].計算技術與自動化，2017，36（03）：1—5.

[13] 李漢博，秦文華.四旋翼慣性導航姿態解算算法的改進與仿真[J].計算機測量與控制，2016，24（07）：187-189+194.

[14] MAHONY R，KUMAR V，CORKE P.Multirotor aerial vehicles：Modeling，estimation，and control of quadrotor[J].IEEE Journal of Robotics and Automation.2012，19（3）：20-32.

[15] 戴啟浩，馬國梁.四旋翼飛行器的自抗擾控制方法研究[J].計算技術與自動化，2017，36（02）：46—50.

[16] 薛佳樂，程珩.基于串級PID四旋翼飛行器控制系統研究[J].電子技術應用，2017，43：134-137+142.

[17] 吳宏鑫，沈少萍.PID控制的應用與理論依據[J].控制工程，2003，10（1）：38—42.

[18] 王立，章政，孫平.一種自適應互補濾波姿態估計算法[J].控制工程，2015，22（ 5） ：881—886.

[19] 張國云，李亞斌，向爛群，等.四旋翼飛行器懸停算法設計與實現[J].計算技術與自動化，2016，35（01）：77—81.

[20] XIONG Jing-jing，ZHENG En-hui.Optimal Kalman filter for state estimation of a quadrotor UAV[J].Optik-International Journal for Light and Electron Optics，2015，（11）：2862—2868.