基于GPS定位的四旋翼無人機位置跟蹤控制

楊 帆，龍 坤

（1.武漢工程大學 電氣信息學院，武漢430205；2.湖北省視頻圖像與高清投影工程技術研究中心，武漢430205）

近年來，隨著智能儀器與自動化技術的逐步成熟，四旋翼無人機已經從最初軍用領域轉向了民用領域，在航拍、測繪、植保、消防等方面得到了越來越多的應用[1]。由于四旋翼無人機結構簡單，具有橫滾、俯仰、偏航、懸停、垂直等多種飛行模式，可最大程度地滿足用戶的需求。在四旋翼無人機飛行過程中，控制系統和導航技術則是重中之重，給四旋翼無人機加上GPS 模塊后，就可以利用GPS 的定位功能更加靈活地控制四旋翼無人機[2]。本文設計了一種串級PD 控制算法， 將其應用到四旋翼無人機控制系統中，相比于傳統PID 控制算法，四旋翼無人機飛行狀態更加穩定，通過上位機軟件可以實時觀察到四旋翼無人機的運動軌跡，真正實現四旋翼無人機軌跡跟蹤功能。

1 四旋翼無人機動力學建模

1.1 飛行原理

四旋翼無人機在分類上主要有“+”和“x”兩種形式，由于“x”型四旋翼無人機在結構上更加靈活，具有更好的飛行視野，故本文采用的是“x”型結構四旋翼無人機[3]，其簡化模型如圖1所示。在系統結構上，四旋翼無人機主要由飛控板、無刷直流電機、電調、電池、旋翼等組成，是一個典型的非線性欠驅動系統。沿逆時針方向將4 個旋翼分別編號為4 號，2號，3號，1 號， 其中1 號和2 號沿順時針方向旋轉，3 號和4 號沿逆時針方向旋轉，這樣可抵消四旋翼無人機做飛行運動時的反扭矩效應以及機身陀螺效應。

圖1 四旋翼無人機的結構Fig.1 Structure of quadrocopter drone

1.2 機體坐標系轉換

將四旋翼無人機繞x 軸產生的橫滾角記為?；繞y 軸產生的俯仰角記為θ； 繞z 軸產生的偏航角記為ψ。為了更加準確地描述四旋翼無人機在空間中的位置以及建模的方便， 先建立地球坐標系E（OE，XE，YE，ZE），將四旋翼無人機軸心點記為坐標原點OE， 以垂直于水平地面向上的方向記為ZE軸方向，再根據坐標系右手原則確定XE，YE軸的方向[4]。以四旋翼無人機軸心點為坐標系原點，建立機體坐標系B（OB，XB，YB，ZB），坐標原點OB即為四旋翼無人機軸心點，以垂直于機身水平面向上的方向記為ZB軸的方向，根據右手原則同樣可得到XB，YB軸的方向。兩種坐標系示意圖如圖2所示。

圖2 機體坐標系-地球坐標系Fig.2 Body coordinate system-Earth coordinate system

由于在建模過程中一般選用的參考標準是地球坐標系，而實際上得到的是機體坐標系，因此需要將四旋翼無人機機體坐標系轉換為地球坐標系，四旋翼無人機繞x，y，z 軸旋轉所對應的旋轉矩陣分別為

在四旋翼無人機飛行過程中，由于四旋翼無人機的姿態角是根據歐拉角解算得來的，在進行坐標系轉換時，必須要保證四旋翼無人機的3 個姿態角和歐拉角相同，即要求無人機按照X-Y-Z 的順序旋轉，這樣做便于進行坐標系轉換[5]。四旋翼無人機地球坐標系轉機體坐標系的矩陣RE-B＝C（?）·C（θ）·C（ψ），由旋轉矩陣的正交性可得到四旋翼無人機的地球坐標系矩陣為

1.3 牛頓-歐拉方程的建立

由牛頓第二定律可知四旋翼無人機在x，y，z 軸所受合外力可表示為

4 個電機產生的總的升力U1=F1+F2+F3+F4，由式（1）和式（2）可得到：

四旋翼無人機x，y，z 軸方向產生的力矩分別為

假設無人機進行小角度飛行運動，由歐拉方程M=Jε+ω·Jω 可知[6]，則有：

聯立式（4）和式（5）可得：

每個電機產生的升力可表示為

故牛頓-歐拉方程為

式中：CT為系統升力系數；CM為系統扭力系數；d 為電機到軸心的軸距；J 為轉動慣量矩陣；ω 為角速度；ε 為角加速度；b 為四旋翼無人機旋翼的拖動系數；為電機轉速。

2 四旋翼無人機仿真測試

利用MATLAB/Simulink 搭建出牛頓-歐拉方程模型，以電機轉速作為輸入對象，四旋翼無人機的3 個歐拉角?，θ，ψ 作為輸出對象， 建立的歐拉角模型如圖3所示。

圖3 牛頓-歐拉方程模型Fig.3 Newton-euler equation model

其中姿態計算模塊中的3 個子模塊分別表示求?¨，θ¨，ψ¨的過程，由于PD 控制存在比較性，需使得角度在2π 弧度內變化，故fcn 模塊表示對3 個歐拉角取2π 的余數，方便后續算法的計算。通過姿態計算得到的歐拉角可反解出四旋翼無人機在x，y，z 軸的加速度，最終得到牛頓-歐拉方程的仿真模型。

2.1 四旋翼無人機PD 控制算法結構

結合PID 控制原理以及給定的四旋翼無人機軌跡，通過位置控制后，輸出?d，θd，ψd姿態角信號；再通過姿態控制輸出轉力矩信號給電機，當作用力改變后，電機轉速ω?也隨之改變；然后再將轉速信號輸出給四旋翼無人機數學模型，最后將姿態信號，位置信號分別反饋到姿態控制和位置控制模塊[7]，其控制框圖如圖4所示。

圖4 四旋翼無人機控制系統的結構原理Fig.4 Structural principle of quadrocopter drone control system

2.2 系統內外環建模仿真

通過圖4可以看出，系統的內環控制為姿態角控制，外環為位置控制，由于PID 控制算法應用廣泛，在本控制系統中，采用PID 控制后發現最后得到的系統仿真曲線會失真，即x，y，z 軸方向上的運動軌跡無法趨于穩定[8]，原因在于積分環節I 一定程度上降低了系統的穩定性。采用串級PD 控制可以產生早期的修正信號，增加阻尼程度，有效地增強系統的穩定性。因此，系統內外環控制均采用串級PD 控制，以此來改善系統的穩定性。外環輸入為xd，yd，zd，x，y，z，然后依次經過P，D 環節，最后輸出得到x，y，z 軸方向上的力Ux，Uy，Uz； 內環輸入為?d，θd，ψd，?，θ，ψ 由于姿態角在輸入時存在較大的波動性，故先通過微分環節D 處理得到早期的待穩定信號，然后在將信號傳給比例環節P，最后輸出得到F2，F3，F4。建立好的系統仿真模型如圖5所示。

圖5 系統內外環控制仿真模型Fig.5 Simulation model of inner and outer ring control of system

若給定psai_d 等于0.2，x，y，z 為輸入的階躍響應曲線，且幅值范圍在10～15 m 之間，仿真時間為100 s，經過多次調整P，D 參數后最終能夠得到穩定的系統位置變化曲線。由于在仿真模型里引入了四旋翼無人機質量m，升力系數CT以及扭力系數CM，根據四旋翼無人機的實物參數，再結合北京航空航天大學所研發的飛行測評網站（www.flyeval.com）就可以算出CT和CM的值分別為1.116×10-5和1.474×10-7[9]。四旋翼無人機位置曲線如圖6所示。

由圖6可知，x，z 方向上的運動軌跡最終能夠達到穩定狀態，y 方向上出現的波動在經過一段時間后也能回到原始的位置。采用串級PD 控制算法所建立的仿真模型能夠得到較好的四旋翼無人機位置變化曲線， 避免了積分環節所帶來的曲線失真，為下一步四旋翼無人機的軌跡仿真提供了有效應對辦法。

圖6 四旋翼無人機位置曲線Fig.6 Position curve of quadrocopter drone

最后利用旋轉矩陣Cbn以及所得到的四旋翼無人機的實際位置x，y，z，通過編寫簡單的建模程序，將仿真模型的信息傳到MATLAB 工作區就可以繪制出四旋翼無人機仿真軌跡圖。同時還可以得到四旋翼無人機跟蹤軌跡曲線變化圖，如圖7所示。無人機在經過短暫的上升后最終懸停在10 m 左右的高度，與上述仿真結果一致。四旋翼無人機的軌跡跟蹤曲線分別如圖8和圖9所示。

圖7 四旋翼無人機完整航跡Fig.7 Complete track of quadrocopter drone

圖8 四旋翼無人機歐拉角曲線仿真Fig.8 Simulation of euler angle curve of quadrocopter drone

圖9 四旋翼無人機軌跡跟蹤仿真Fig.9 Quadrocopter drone tracking simulation

由圖8可知，四旋翼無人機的姿態角始終變化在合理區間內，其中橫滾角? 大約在57 s 時趨于穩定，俯仰角θ 大約在51 s 時趨于穩定，偏航角ψ 大約在10 s 時趨于穩定；由圖9可知，x，y，z 的實際位置與給定的參考位置曲線基本相符，經過一段時間運行后最終都能達到穩定狀態。通過仿真測試進一步證明了串級PD 控制算法的穩定性。

3 四旋翼無人機調試實驗

3.1 APM 飛控板參數校正

實驗平臺的硬件結構主要包括F450 機架、APM2.8 飛控板、好盈20 A 電機、30 A 電調、9450 自鎖槳、MC6 遙控器、接收機、電池、UBEC 供電模塊、數傳模塊、M8NGPS 模塊以及減震板等[10]。APM2.8飛控板主處理器為Arduino Mega2560 單片機，APM2.8 內置六軸MEMS 傳感器MPU6000， 氣壓計MS-5611，三軸磁力計HMC5883，一般還會配置GPS模塊接口，可以實現四旋翼無人機定位懸停功能[11]。其中，MPU6000 整合了三軸陀螺儀和三軸加速度計。MS-5611 通過測量氣壓得到高度，輔助GPS 定位。HMC5883 通過測量地磁場得到方位，輔助無人機定向[12]。飛控采集并融合多種傳感器的數據，計算并校正四旋翼無人機的位姿。

利用USB 接口將Mission Planner 地面站軟件與APM 飛控進行連接，通過APM 官網進行固件的下載， 將串級PD 控制算法和傳統PID 算法分別以代碼的方式導入到APM 板中，比較二者的優劣性。固件下載完成后，接著對APM 板上的羅盤，加速度計，遙控器參數以及飛行模式參數進行校正，羅盤校正主要是檢測APM 飛控板內置羅盤的性能，加速度計校正主要是對APM 飛控板方向進行讀取，飛行模式校正主要是選擇無人機的飛行模式。在本次實驗過程中， 設置了3 種飛行模式： 自穩模式Stabilize，懸停模式Loiter，返航模式RTL。當飛控板參數調試完成后，最后對電調進行校正，以便于四旋翼無人機能正常起飛。

3.2 四旋翼無人機室外飛行測試

為驗證串級PD 控制算法的穩定性， 實驗地點選在武漢工程大學流芳校區土操場，實驗當天風速大概為2 m/s，風力等級2 級。預先為無人機規劃好航點，并利用數傳模塊讀取航點，飛行時首先將通道5 開關撥到Stabilize 處， 當四旋翼無人機上升到10 m 左右的高度時再切換到Loiter 模式，此時無人機可利用GPS 完成定點， 空中懸停效果如圖10所示。然后將遙控器的通道5 開關撥到RTL 模式，當到達最后一個航點時無人機會自動沿原路返回。從上位機軟件獲取的四旋翼無人機的姿態，位置數據如表1所示。

圖10 四旋翼無人機懸停圖Fig.10 Hover map of quadrocopter drone

表1 四旋翼無人機位置和姿態記錄Tab.1 Quadcopter position and attitude recordings

通過地面站數傳模塊可以觀察到四旋翼無人機歐拉角及其速度變化，如圖11所示。傳統PID 控制算法得到的實驗數據存在較大的波動性，無法使得四旋翼無人機穩定飛行， 而串級PD 控制算法得到的實驗數據比較穩定，能使得四旋翼無人機保持穩定飛行。

圖11 四旋翼無人機地面站控制Fig.11 Quadrocopter drone ground station controlling

4 結語

本文通過對四旋翼無人機系統結構以及動力學模型的分析，以MATLAB/Simulink 為平臺，以串級PD 控制為核心對四旋翼無人機內外環控制進行了建模仿真， 通過理論上的建模仿真實驗發現，串級PD 控制算法對四旋翼無人機有較好的控制效果， 使得四旋翼無人機在飛行過程中能保持穩定。最后通過室外實驗測試， 在串級PD 控制下， 搭載GPS 模塊的四旋翼無人機能夠穩定飛行并實現軌跡跟蹤功能。由于串級PD 控制始終存在一定的誤差，如果采用更穩定的控制算法以及控制單元，可以進一步減小誤差，提高四旋翼無人機的使用效率。