基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)

馮 強，齊 偉

(1.西安明德理工學院，西安 710124; 2.西安航天動力機械有限公司，西安 710025)

0 引言

近年來無人機的應(yīng)用越來越廣泛，因其獨特的體積小、隱蔽性強、飛行穩(wěn)定等優(yōu)點，在軍事及民用領(lǐng)域都發(fā)揮著極其重要的作用，尤其在搶險救災(zāi)及影視行業(yè)中都扮演了十分重要的角色[1-2]。四旋翼無人機系統(tǒng)不同于普通無人機，采用非線性系統(tǒng)，另因其特殊的驅(qū)動也對無人機的航跡跟蹤控制帶來了困難[3-4]。

無人機的飛行環(huán)境復雜，這些問題都對四旋翼無人機的控制提出了很高的要求，控制系統(tǒng)會直接影響四旋翼無人機的飛行狀態(tài)和任務(wù)執(zhí)行的優(yōu)勢性。為了更好的實現(xiàn)四旋翼無人機的航跡跟蹤和控制，本文基于MPC航跡規(guī)劃算法和航跡跟蹤算法對硬件和軟件部分進行設(shè)計，通過數(shù)據(jù)通訊實現(xiàn)地面控制與空中飛行控制的數(shù)據(jù)交換，提出了一種基于MPC控制的四旋翼無人機跟蹤控制系統(tǒng)，并通過實驗驗證了該控制系統(tǒng)的有效性。

1 四旋翼無人機航跡跟蹤控制系統(tǒng)硬件結(jié)構(gòu)設(shè)計

根據(jù)四旋翼無人機的飛行荷載有限的特點，通過空中和地面兩個部分實現(xiàn)控制系統(tǒng)的設(shè)計，結(jié)構(gòu)如圖1所示。

圖1 基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)硬件結(jié)構(gòu)

根據(jù)圖1可知，根據(jù)不同的任務(wù)需求，無人機航跡跟蹤控制系統(tǒng)分為 3 個部分：空中飛行自主控制模塊、地面航跡規(guī)劃系統(tǒng)、緊急情況下的人工干預(yù)模塊，空中控制器主要負責穩(wěn)態(tài)飛行和航跡跟蹤控制，地面控制器主要負責飛行的動態(tài)航線規(guī)劃，并通過無線通信技術(shù)將兩部分結(jié)果進行數(shù)據(jù)連接，該控制系統(tǒng)中，地面控制決定了無人機的航跡規(guī)劃，要求系統(tǒng)能夠?qū)χ車h(huán)境進行甄別并有效規(guī)避障礙物[5-6]?？罩锌刂苿t要求能夠準確執(zhí)行地面控制系統(tǒng)所規(guī)劃的航線。

1.1 四旋翼無人機平臺設(shè)計

四旋翼無人機平臺主要硬件包括：飛行器、電機、螺旋槳。為避免在調(diào)試過程中因摔機造成機架損壞，選用主體采模塊化設(shè)計、便于拆卸和更換的F450機架。為了要最大限度的提升無人機的系統(tǒng)性能，要合理搭配電機和螺旋槳，電機在選擇上主要考慮無人機的最大飛行荷載，電機自重應(yīng)小于電機最大動力的2/5。螺旋槳的選擇主要考慮電機的電壓值，一般電壓值較大時優(yōu)先選擇小型螺旋槳；當電壓值較小時，優(yōu)先選用大型螺旋槳。本文設(shè)計使用4 s電池，電壓為22 V，搭配APC1540螺旋槳[7-8]。

1.2 飛行控制器設(shè)計

飛行控制器的主控計算機是整個系統(tǒng)的核心，要負責傳感器數(shù)據(jù)的采集與分析、自主飛行控制算法的計算、輸出指令和其他模塊間無線數(shù)據(jù)傳輸，因此，主控計算機應(yīng)具有強大的計算能力，多個通信接口，較低的功耗，合適的重量和體積。因此本文采用法國 ST 公司 STM32F4 系列芯片主板，如圖2所示。

圖2 STM32F4 系列芯片主板

該主板的芯片內(nèi)核處理器為 Cortex-M4，輔以運算單元FPU，主要頻率為169 MHz[9-10]。根據(jù)圖2可知，該芯片連接PWM輸出接口，電源接口及調(diào)試接口，保證STM32F4系列芯片主板的穩(wěn)定運行，通過遙控數(shù)據(jù)接口實現(xiàn)飛控數(shù)據(jù)的接收機傳輸；通過導航數(shù)據(jù)接口及無線數(shù)據(jù)接口實現(xiàn)飛控數(shù)據(jù)的處理，通過姿態(tài)數(shù)據(jù)接口實現(xiàn)無人機姿態(tài)控制數(shù)據(jù)的傳輸，并在芯片連接的液晶顯示屏上展示。

1.3 導航控制器設(shè)計

導航控制器主流的GPC定位系統(tǒng)精度不夠，存在很大的誤差，無法滿足四旋翼無人機的導航要求，所以本文基于借助 Ublox 6H GPS定位技術(shù)，采用MS5803數(shù)字式高度計來實現(xiàn)導航定位。

本文設(shè)計的導航控制器如圖3所示。

圖3 導航控制器

圖3為導航控制器結(jié)構(gòu)圖，接收機接收四旋翼無人機航跡數(shù)據(jù)，通過數(shù)字氣壓計及主控板實現(xiàn)外界大氣壓力，溫度，濕度和海拔高度的測量，通過Ublox 6H GPS定位技術(shù)實現(xiàn)導航定位。

1.4 姿態(tài)指引儀

姿態(tài)指引儀包括陀螺儀、加速度計和磁力計。加速度計的主要功能是通過測量重力加速度[11-12]來校正陀螺儀的俯仰和橫滾角，輸出三軸加速度和角速度，然后將其反饋到控制系統(tǒng)作為對照參考。磁力計用于測量地磁場的漂移以校正偏航角[13]。本位采用 MTI 姿態(tài)參考系統(tǒng)設(shè)計姿態(tài)指引儀，參考儀內(nèi)部分別集成三軸加速度計、軸磁力計、角速率陀螺和溫度傳感器。采用卡爾曼濾波算法得到姿態(tài)估計參數(shù)，分辨率為 0.1，數(shù)據(jù)更新頻率為256 Hz，俯仰角輸出范圍為±90°，角速度輸出角度±300°，通信波特率921 700 bps。

1.5 數(shù)據(jù)通信器

數(shù)據(jù)通信器為地面控制系統(tǒng)和空中控制系統(tǒng)間的數(shù)據(jù)交換紐帶，本文采用 MaxStream 公司的9XTend OEMRF模塊，通信距離最遠可大70 km，采用5 V的串行接口，天線接口采用RPSMA，植入抗干擾技術(shù)，避免無線電干擾，靈敏度110 dBm,工作頻率為900 MHz，數(shù)據(jù)處理量為240 Kbps。

1.6 人工干預(yù)器

地面控制模塊的主要任務(wù)是規(guī)劃航線，本文設(shè)計采用高配置終端計算機。當無人機發(fā)生突發(fā)意外情況時，可以切換到人工遙控模式，人工干預(yù)主要通過 FUTABA T8FG 遙控器和RS5803SB 接收器實現(xiàn)，F(xiàn)UTABA 遙控器和接收器采用SBus傳輸協(xié)議，該協(xié)議遵循串口數(shù)據(jù)流傳輸模式的基本數(shù)據(jù)流協(xié)議，以100 bps 的頻率對數(shù)據(jù)單元進行發(fā)送。根據(jù)Sbus 協(xié)議的數(shù)據(jù)單元進行有效傳輸，每次有效傳輸?shù)臄?shù)據(jù)包括25個字節(jié)，且以低速高速交替形式進行周期性傳輸。Futaba T8FG 遙控器每個指令占用11 位的空間傳輸空間，總共包含了 16 通道的控制指令。

2 基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)軟件設(shè)計

本文軟件本設(shè)計系統(tǒng)中，地面控制中心的航線規(guī)劃系統(tǒng)是基于 windows 系統(tǒng)的 PC 計算機開發(fā)的， 自主航線跟蹤控制系統(tǒng)是基于 STM32 微處理器進行裸機開發(fā)。提供了完善的 C++語言，C ++語言庫函數(shù)進行應(yīng)用層軟件的開發(fā)和調(diào)試，極大地降低了程序的開發(fā)難度。航跡跟蹤控制中心采用基于芯片STM32F417 的裸機開發(fā)，無操作系統(tǒng)，在IAR Embedded Workbench for ARMv6.4 集成開發(fā)環(huán)境下進行裸機開發(fā)，在該環(huán)境下 ST 公司開發(fā)了不同類型芯片的操作庫文件，這樣極大地降低了應(yīng)用程序開發(fā)的難度[14-15]。

MPC是一種進階過程控制方法，這是一種多變量控制策略，以最優(yōu)動態(tài)軌跡為控制目標，設(shè)定航線規(guī)劃軟件流程如圖4所示。

圖4 航線規(guī)劃軟件流程

觀察圖4可知，采用空間質(zhì)點模型來實現(xiàn)飛行控制系統(tǒng)的航線規(guī)劃，根據(jù)三維方向上的速度變化率得出其動態(tài)性能約束。通過網(wǎng)絡(luò)云端數(shù)據(jù)獲取障礙物數(shù)據(jù)信息，建立航線規(guī)劃系統(tǒng)環(huán)境模型，利用鼠標在地圖上標記目標地點，從地面網(wǎng)站獲取目標地點位置，規(guī)劃到系統(tǒng)目錄下生成文本文檔。在規(guī)劃航線時，通過讀取地面共享信息得出具體障礙物數(shù)據(jù)信息，對障礙物空間進行規(guī)劃，利用無線數(shù)據(jù)傳輸獲取無人機的實時飛行狀況，實行航線動態(tài)規(guī)劃[16-18]。

對航線規(guī)劃問題建模，采用MPC控制方法，以AMPL標準模型語言對多目標優(yōu)化問題描述，然后調(diào)用CPLEX軟件對目標函數(shù)求解，同時忽略滾動時域，把第一個點作為飛行目標點，求出N個時域的最優(yōu)解航線，具體流程如圖5所示。

圖5 最優(yōu)解航線流程

接口數(shù)據(jù)接收程序接收的數(shù)據(jù)主要有：MTI姿態(tài)系統(tǒng)數(shù)據(jù)、GPS導航系統(tǒng)數(shù)據(jù)、無線鏈路數(shù)據(jù)、遙控器控制數(shù)據(jù)。其中MTI數(shù)據(jù)，GPS數(shù)據(jù)和無線鏈路數(shù)據(jù)是通過硬件接口開進行數(shù)據(jù)交互的，遙控數(shù)據(jù)為Futaba配套的特殊SBUs通信協(xié)議，通過接口改進，可以轉(zhuǎn)換為一般的通信協(xié)議。在數(shù)據(jù)傳輸中，MTI姿態(tài)數(shù)據(jù)和遙控器數(shù)據(jù)格式大體相同，每次傳輸?shù)臄?shù)據(jù)包中包含一前端數(shù)字，N個字節(jié)的傳輸數(shù)據(jù)集和末端的數(shù)據(jù)驗證。

高度計采集板上的導航定位信息是處理過的，可以實現(xiàn)輸出數(shù)據(jù)的格式調(diào)節(jié)，因此所有的數(shù)據(jù)接收程序都接口中斷用于觸發(fā)MDA數(shù)據(jù)接收，以完成中斷形式。 當緩沖區(qū)中有要訪問的數(shù)據(jù)時，觸發(fā)USART接收，檢測到數(shù)據(jù)包前端數(shù)位后，關(guān)閉 USART 數(shù)據(jù)接收通道，觸發(fā)MDA 直接接收 N 個字節(jié)的數(shù)據(jù)，DMA 接收完成觸發(fā)中斷打開 USART 數(shù)據(jù)接收通道[19-20]。

通信數(shù)據(jù)鏈設(shè)計采用 XTend OEMRF模塊，考慮到無人機的飛行環(huán)境復雜多變，所以該通信模塊設(shè)計為拓撲結(jié)構(gòu)，地面Base隨時掌握飛機的狀態(tài)信息，包括經(jīng)緯位置，速度和飛行姿態(tài)等等。地面控制站能夠?qū)⒁?guī)劃航線控制指令上傳到無人機，Remote根據(jù)根據(jù)接受的指令信息作出反應(yīng)，實現(xiàn)Base和Remote間的數(shù)據(jù)交互，無人機根據(jù)指令執(zhí)行飛行航線?？紤]到信息傳輸過程中會出現(xiàn)數(shù)據(jù)錯誤和丟失的情況，在通行模式中設(shè)置base和Remote的應(yīng)答機制，準確可靠地實現(xiàn)地面站與空中多飛行器的數(shù)據(jù)間通信。

3 實驗結(jié)果與分析

為了驗證本文提出的基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)的有效性，設(shè)計對比實驗，選用本文提出的跟蹤控制系統(tǒng)與傳統(tǒng)的基于云臺相機的四旋翼無人機航跡跟蹤控制系統(tǒng)，基于數(shù)據(jù)挖掘的的四旋翼無人機航跡跟蹤控制系統(tǒng)進行實驗對比。設(shè)計實驗參數(shù)如表1所示。

表1 實驗參數(shù)

根據(jù)上述實驗參數(shù)，選用本文基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)和基于云臺相機的四旋翼無人機航跡跟蹤控制系統(tǒng)、基于數(shù)據(jù)挖掘的四旋翼無人機航跡跟蹤控制系統(tǒng)進行實驗。

預(yù)計的跟蹤控制結(jié)果如圖6所示。

圖6 預(yù)計的跟蹤控制曲線

利用下式求解不同無人機航跡跟蹤控制系統(tǒng)的跟蹤結(jié)果：

(1)

式中，M為四旋翼無人機航跡跟蹤坐標，X、Y、Z分別為橫縱坐標及垂直坐標的預(yù)計位移值，通過上式求解航跡跟蹤控制結(jié)果。

基于云臺相機的四旋翼無人機航跡跟蹤控制系統(tǒng)得到的跟蹤控制結(jié)果如圖7所示。

圖7 基于云臺相機的四旋翼無人機航跡跟蹤控制結(jié)果

基于數(shù)據(jù)挖掘的四旋翼無人機航跡跟蹤控制系統(tǒng)得到的跟蹤控制結(jié)果如圖8所示。

圖8 基于數(shù)據(jù)挖掘的四旋翼無人機航跡跟蹤控制結(jié)果

本文提出的基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)得到的跟蹤控制結(jié)果如圖9所示。

圖9 基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)得到的跟蹤控制結(jié)果

根據(jù)上述實驗結(jié)果可知，本文基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)得到的跟蹤控制結(jié)果與預(yù)期的跟蹤控制曲線重合度更高，平均誤差控制在1 cm以內(nèi)。本文提出的控制系統(tǒng)具有顏色特征識別功能，MPC在跟蹤控制過程中，會隨著目標的移動而移動，對于無人機旋轉(zhuǎn)角度有很好的跟蹤能力，視線范圍之內(nèi)始終跟著跟蹤目標。

綜上所述，基于MPC的四旋翼無人機航跡跟蹤控制系統(tǒng)具有很好的控制能力，可以在地面和空中實現(xiàn)對無人機精確穩(wěn)定的跟蹤控制。

4 結(jié)束語

本文根據(jù)綜合控制系統(tǒng)的需求出發(fā)，設(shè)計了四旋翼無人機航線跟蹤控制的硬件平臺，然后基于MPC航跡規(guī)劃算法和航跡跟蹤算法對軟件部分進行設(shè)計，最后通過數(shù)據(jù)通訊模塊實現(xiàn)了無人機和地面控制系統(tǒng)間的數(shù)據(jù)通信，完成整個基于MPC的四旋翼航線跟蹤控制系統(tǒng)。本文雖然基于MPC針對無人機航跡跟蹤和控制提出了系統(tǒng)的方法，但還存在很大的改善空間，特別是對于姿態(tài)控制的分析中，弱化了各通道間的耦合，姿態(tài)動力學模型簡化為線性化小擾動模型。在實際情況中，小型四旋翼無人機的飛行路徑控制效果尚不理想。所以對航跡的控制研究將是今后的工作重點，對于小型旋翼飛行器來說能夠自主避障的控制系統(tǒng)是未來發(fā)展的必然趨勢，希望在未來的研究中能更好的實現(xiàn)對四旋翼無人機的航跡跟蹤和控制。