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

楊 帆，龍 坤

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

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

1 四旋翼無人機動力學(xué)建模

1.1 飛行原理

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

圖1 四旋翼無人機的結(jié)構(gòu)Fig.1 Structure of quadrocopter drone

1.2 機體坐標(biāo)系轉(zhuǎn)換

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

圖2 機體坐標(biāo)系-地球坐標(biāo)系Fig.2 Body coordinate system-Earth coordinate system

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

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

1.3 牛頓-歐拉方程的建立

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

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

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

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

聯(lián)立式（4）和式（5）可得：

每個電機產(chǎn)生的升力可表示為

故牛頓-歐拉方程為

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

2 四旋翼無人機仿真測試

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

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

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

2.1 四旋翼無人機PD 控制算法結(jié)構(gòu)

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

圖4 四旋翼無人機控制系統(tǒng)的結(jié)構(gòu)原理Fig.4 Structural principle of quadrocopter drone control system

2.2 系統(tǒng)內(nèi)外環(huán)建模仿真

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

圖5 系統(tǒng)內(nèi)外環(huán)控制仿真模型Fig.5 Simulation model of inner and outer ring control of system

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

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

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

最后利用旋轉(zhuǎn)矩陣Cbn以及所得到的四旋翼無人機的實際位置x，y，z，通過編寫簡單的建模程序，將仿真模型的信息傳到MATLAB 工作區(qū)就可以繪制出四旋翼無人機仿真軌跡圖。同時還可以得到四旋翼無人機跟蹤軌跡曲線變化圖，如圖7所示。無人機在經(jīng)過短暫的上升后最終懸停在10 m 左右的高度，與上述仿真結(jié)果一致。四旋翼無人機的軌跡跟蹤曲線分別如圖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可知，四旋翼無人機的姿態(tài)角始終變化在合理區(qū)間內(nèi)，其中橫滾角? 大約在57 s 時趨于穩(wěn)定，俯仰角θ 大約在51 s 時趨于穩(wěn)定，偏航角ψ 大約在10 s 時趨于穩(wěn)定；由圖9可知，x，y，z 的實際位置與給定的參考位置曲線基本相符，經(jīng)過一段時間運行后最終都能達到穩(wěn)定狀態(tài)。通過仿真測試進一步證明了串級PD 控制算法的穩(wěn)定性。

3 四旋翼無人機調(diào)試實驗

3.1 APM 飛控板參數(shù)校正

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

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

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

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

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

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

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

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

4 結(jié)語

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