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

楊 帆，龍 坤

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

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

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

1.1 飛行原理

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

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

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

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

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

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

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

1.3 牛頓-歐拉方程的建立

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

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

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

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

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

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

故牛頓-歐拉方程為

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

2 四旋翼無人機(jī)仿真測試

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

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

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

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

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

圖4 四旋翼無人機(jī)控制系統(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)生早期的修正信號，增加阻尼程度，有效地增強(qiáng)系統(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)位置變化曲線。由于在仿真模型里引入了四旋翼無人機(jī)質(zhì)量m，升力系數(shù)CT以及扭力系數(shù)CM，根據(jù)四旋翼無人機(jī)的實物參數(shù)，再結(jié)合北京航空航天大學(xué)所研發(fā)的飛行測評網(wǎng)站（www.flyeval.com）就可以算出CT和CM的值分別為1.116×10-5和1.474×10-7[9]。四旋翼無人機(jī)位置曲線如圖6所示。

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

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

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

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

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

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

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

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

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

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

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

3.2 四旋翼無人機(jī)室外飛行測試

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

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

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

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

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

4 結(jié)語

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