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

馮 強(qiáng)，齊 偉

(1.西安明德理工學(xué)院，西安 710124; 2.西安航天動(dòng)力機(jī)械有限公司，西安 710025)

0 引言

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

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

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

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

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

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

1.1 四旋翼無人機(jī)平臺(tái)設(shè)計(jì)

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

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

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

圖2 STM32F4 系列芯片主板

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

1.3 導(dǎo)航控制器設(shè)計(jì)

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

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

圖3 導(dǎo)航控制器

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

1.4 姿態(tài)指引儀

姿態(tài)指引儀包括陀螺儀、加速度計(jì)和磁力計(jì)。加速度計(jì)的主要功能是通過測量重力加速度[11-12]來校正陀螺儀的俯仰和橫滾角，輸出三軸加速度和角速度，然后將其反饋到控制系統(tǒng)作為對(duì)照參考。磁力計(jì)用于測量地磁場的漂移以校正偏航角[13]。本位采用 MTI 姿態(tài)參考系統(tǒng)設(shè)計(jì)姿態(tài)指引儀，參考儀內(nèi)部分別集成三軸加速度計(jì)、軸磁力計(jì)、角速率陀螺和溫度傳感器。采用卡爾曼濾波算法得到姿態(tài)估計(jì)參數(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模塊，通信距離最遠(yuǎn)可大70 km，采用5 V的串行接口，天線接口采用RPSMA，植入抗干擾技術(shù)，避免無線電干擾，靈敏度110 dBm,工作頻率為900 MHz，數(shù)據(jù)處理量為240 Kbps。

1.6 人工干預(yù)器

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

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

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)結(jié)果與分析

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

表1 實(shí)驗(yàn)參數(shù)

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

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

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

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

(1)

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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