王 演，王 鑫，曹龍輝，康健賓

（天津理工大學(xué) 工程訓(xùn)練中心，天津 300384）

近年來(lái)，無(wú)人機(jī)技術(shù)在國(guó)內(nèi)外的應(yīng)用領(lǐng)域越來(lái)越廣泛，得到了科研和工程領(lǐng)域的重點(diǎn)關(guān)注[1]。無(wú)人機(jī)的廣泛應(yīng)用可以節(jié)省大量的勞動(dòng)力，而且對(duì)于一些危險(xiǎn)和不便的地方，應(yīng)用無(wú)人機(jī)可以輕松實(shí)現(xiàn)。無(wú)人機(jī)編隊(duì)飛行已在多個(gè)領(lǐng)域發(fā)揮重要作用。多架無(wú)人機(jī)編隊(duì)飛行，協(xié)同偵察、作戰(zhàn)的模式，可以在一定程度上提高單機(jī)單次作戰(zhàn)任務(wù)的成功概率[2]。無(wú)人機(jī)編隊(duì)飛行是各國(guó)軍隊(duì)和科研工作者一直以來(lái)就追求的技術(shù)，其關(guān)鍵技術(shù)問(wèn)題，主要包括隊(duì)形設(shè)計(jì)、氣動(dòng)耦合。

航跡規(guī)劃、信息互換以及編隊(duì)飛行控制策略等問(wèn)題，是各個(gè)無(wú)人機(jī)研發(fā)強(qiáng)國(guó)的研究熱點(diǎn)[3-4]，但是近年來(lái)，無(wú)論是國(guó)內(nèi)外，對(duì)于無(wú)人機(jī)在室內(nèi)的編隊(duì)飛行技術(shù)始終無(wú)法突破，究其原因是因?yàn)槭覂?nèi)環(huán)境具有更多的時(shí)變性和不可預(yù)測(cè)性，而用于定位的如GPS 等則無(wú)法在室內(nèi)使用或是定位精度不夠[5-6]。

本文使用OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)建立坐標(biāo)，并加以對(duì)地光流以輔佐完成無(wú)人機(jī)在室內(nèi)的尋跡避障功能[7]，使用“從-主”的編隊(duì)飛行模式來(lái)擴(kuò)大搜查范圍以及提高效率，并改進(jìn)超聲波測(cè)距模塊的自動(dòng)目標(biāo)識(shí)別方法，來(lái)對(duì)室內(nèi)的復(fù)雜地形情況和位置進(jìn)行識(shí)別與確認(rèn)，再通過(guò)主機(jī)將所偏移的誤差發(fā)送到上位機(jī)進(jìn)行進(jìn)一步的校準(zhǔn)，進(jìn)而實(shí)現(xiàn)無(wú)人機(jī)在室內(nèi)的精準(zhǔn)導(dǎo)航飛行。

1 無(wú)人機(jī)硬件設(shè)計(jì)

無(wú)人機(jī)由MSP432R401R Launchpad 作為飛行控制模塊，并搭載電機(jī)驅(qū)動(dòng)模塊，HC-SR04 超聲波測(cè)距模塊，光流模塊等。飛行控制系統(tǒng)硬件組成如圖1所示。

圖1 飛行控制系統(tǒng)硬件框圖Fig.1 Hardware block diagram of flight control system

1.1 飛行控制模塊

MSP432 Launchpad 包含一顆低功耗、高性能MSP432P401R MCU。32 位ARM Cortex M4F 內(nèi)核，主48 MHz 帶浮點(diǎn)單元和DSP 加速功能，256 KB Flash、64 KB RAM，功耗低，95 μA/MHz 工作功耗和850 nA RTC 待機(jī)操作功耗，24 通道14 位差動(dòng)1MSPS SAR ADC，兩個(gè)比較器、高級(jí)加密標(biāo)準(zhǔn)（AES256）加速器、CRC、DMA、32 位硬件乘法器、4 個(gè)16 位、2 個(gè)32 位定時(shí)計(jì)數(shù)器、通信接口豐富，多達(dá)4 個(gè)I2C、8個(gè)SPI、4 個(gè)UART[8]。

1.2 驅(qū)動(dòng)模塊

由于無(wú)刷電機(jī)具有噪聲小、轉(zhuǎn)速穩(wěn)定、動(dòng)力足等優(yōu)點(diǎn)，在飛行時(shí)避免了由于噪聲產(chǎn)生對(duì)編隊(duì)飛行的影響故選擇無(wú)刷電機(jī)和電調(diào)模塊驅(qū)動(dòng)四旋翼。

1.3 超聲波測(cè)距模塊

采用HC-SR04，鑒于該模塊性能穩(wěn)定，測(cè)度距離精確，模塊高精度，盲區(qū)小，探測(cè)距離最高可達(dá)450 cm，精度可達(dá)0.2 cm，滿足無(wú)人機(jī)編隊(duì)飛行的測(cè)距要求，同時(shí)具有GPIO、串口等通信方式，故可以與無(wú)人機(jī)實(shí)現(xiàn)實(shí)時(shí)通訊。

2 OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)

2.1 搭建區(qū)域選擇

搭建區(qū)域選擇在陽(yáng)光較少，無(wú)反光的空間內(nèi)，并保證所搭建的區(qū)域內(nèi)的障礙物較少。其場(chǎng)地搭建如圖2所示。

圖2 OptiTrack 系統(tǒng)場(chǎng)地搭建Fig.2 OptiTrack system site construction

2.2 固定相機(jī)位置

將相機(jī)等距環(huán)繞分布在捕獲區(qū)域，且必須保證至少有2 臺(tái)相機(jī)捕獲到Marker 點(diǎn)。對(duì)于低高度的目標(biāo)，相機(jī)放置于較低高度處，聚焦在低處位置。相機(jī)固定位置如圖3所示。

圖3 OptiTrack 相機(jī)固定Fig.3 OptiTrack camera fixed

2.3 相機(jī)角度與對(duì)焦

首先要確保相機(jī)位置擺放正確，之后調(diào)節(jié)相機(jī)的角度、對(duì)焦等內(nèi)容（按下相機(jī)后按鈕進(jìn)行）。選擇相機(jī)為灰度模式，提高曝光時(shí)間（EXP），LED 亮度（LED）。在此基礎(chǔ)上，將Marker 點(diǎn)放在捕捉區(qū)域內(nèi)，調(diào)整為相機(jī)正對(duì)角位置并查看圖像，確保捕獲空間在相機(jī)覆蓋區(qū)域中，放大Marker 點(diǎn)的圖像，使其充滿畫面，調(diào)整相機(jī)焦距，重復(fù)以上步驟直至調(diào)整完所有相機(jī)。

2.4 標(biāo)定

Motive 中的Rigid Body 是由安裝在被測(cè)物體上的Marker 點(diǎn)創(chuàng)建的，在Rigid Body 中可以獲得位置（X、Y、Z）和姿態(tài)（Pitch、Yaw、Roll），Motive 中Rigid Body 通常由3 個(gè)以上的Marker 點(diǎn)組成，且Marker 點(diǎn)相對(duì)位置關(guān)系不會(huì)發(fā)生改變，即安裝的Marker 點(diǎn)之間的空間位置關(guān)系保持不變，且Marker點(diǎn)之間的距離不會(huì)超出對(duì)應(yīng)Rigid Body 下設(shè)置的偏移公差范圍，否則可能無(wú)法重建出Rigid Body。

放置Marker 點(diǎn)需保證點(diǎn)不在同一平面，形成一個(gè)立體形狀。對(duì)于多個(gè)剛體，要保證每個(gè)剛體唯一。創(chuàng)造唯一剛體的關(guān)鍵思想是避免Motive 內(nèi)多個(gè)剛體的幾何一致性。

在無(wú)人機(jī)上安裝Marker 點(diǎn)，進(jìn)行檢測(cè)觀察坐標(biāo)數(shù)據(jù)，如圖4所示。

圖4 無(wú)人機(jī)在OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)中的坐標(biāo)數(shù)據(jù)Fig.4 Coordinate data of UAV in OptiTrack motion capture system

2.5 坐標(biāo)數(shù)據(jù)數(shù)據(jù)傳輸

OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)數(shù)據(jù)傳輸過(guò)程為攝像頭采集到無(wú)人機(jī)上Marker 點(diǎn)位置，在Motive 軟件主界面中顯示，無(wú)人機(jī)的實(shí)時(shí)位置便可直觀地通過(guò)3D 圖像顯示出來(lái)，并在已經(jīng)進(jìn)行標(biāo)定的固定坐標(biāo)系中讀出實(shí)時(shí)位置數(shù)據(jù)，精確度可達(dá)0.1 mm；使用MATLAB 軟件編寫相關(guān)程序通過(guò)串口將數(shù)據(jù)打包發(fā)送給無(wú)人機(jī)，并提供給無(wú)人機(jī)目標(biāo)位置的坐標(biāo)數(shù)據(jù)，無(wú)人機(jī)便可從當(dāng)前位置飛行至目標(biāo)位置。數(shù)據(jù)傳輸過(guò)程如圖5所示。

圖5 數(shù)據(jù)傳輸流程Fig.5 Flow chart of data transmission

3 無(wú)人機(jī)編隊(duì)飛行設(shè)計(jì)

無(wú)人機(jī)編隊(duì)飛行模式采用從-主的飛行模式，在該模式中，首先設(shè)定無(wú)人機(jī)編隊(duì)系統(tǒng)中的一架作為領(lǐng)航無(wú)人機(jī)，其他個(gè)體為跟隨無(wú)人機(jī)，在編隊(duì)飛行時(shí)，跟隨無(wú)人機(jī)實(shí)時(shí)跟隨領(lǐng)航無(wú)人機(jī)進(jìn)行飛行。在此種控制算法中，由于有領(lǐng)航無(wú)人機(jī)和跟隨無(wú)人機(jī)的這種相對(duì)運(yùn)動(dòng)模式，可以將無(wú)人機(jī)編隊(duì)系統(tǒng)的隊(duì)形控制問(wèn)題轉(zhuǎn)換成跟隨無(wú)人機(jī)跟隨領(lǐng)航者的位置運(yùn)動(dòng)情況。因此可以將復(fù)雜的多個(gè)個(gè)體之間的問(wèn)題轉(zhuǎn)換成單個(gè)個(gè)體的運(yùn)動(dòng)情況研究，簡(jiǎn)化了控制結(jié)構(gòu)，且具有較好的控制效果。

在無(wú)人機(jī)編隊(duì)飛行過(guò)程中，無(wú)人機(jī)由OptiTrack運(yùn)動(dòng)捕捉系統(tǒng)所提供的坐標(biāo)數(shù)據(jù)來(lái)判斷自己的位置，并根據(jù)所處位置來(lái)調(diào)節(jié)自身飛行的高度、俯仰角度等，同時(shí)從機(jī)由主機(jī)的飛行情況作出相應(yīng)更改。無(wú)人機(jī)在飛行的過(guò)程中對(duì)目標(biāo)路徑障礙進(jìn)行實(shí)時(shí)檢測(cè)，若發(fā)現(xiàn)障礙，則記錄下此時(shí)的坐標(biāo)數(shù)據(jù)，并通過(guò)串口向上位機(jī)發(fā)送所記錄的數(shù)據(jù)，完成自主避障導(dǎo)航功能。編隊(duì)飛行定位懸停原理如圖6所示。

圖6 編隊(duì)飛行定位懸停原理圖Fig.6 Schematic diagram of formation flying positioning hovering

4 無(wú)人機(jī)定位精度測(cè)量

應(yīng)用OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)，在四旋翼無(wú)人機(jī)上布置Marker 點(diǎn)，通過(guò)對(duì)Marker 點(diǎn)的數(shù)據(jù)來(lái)近似代替無(wú)人機(jī)的飛行數(shù)據(jù)，得到無(wú)人機(jī)定高（z 軸）與懸停（x、y 軸）的相應(yīng)數(shù)據(jù)，數(shù)據(jù)分析如下所述。

4.1 對(duì)于無(wú)人機(jī)定高（z 軸）的數(shù)據(jù)檢測(cè)

通過(guò)對(duì)OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)中所傳輸?shù)膠軸數(shù)據(jù)匯總，得出無(wú)人機(jī)高度數(shù)據(jù)變化如圖7所示。在設(shè)定高度為60 刻度時(shí)，無(wú)人機(jī)的高度在400 ms～800 ms 時(shí)為衰減振蕩的過(guò)程，最終趨于平穩(wěn)，并最終穩(wěn)定在60 刻度值處，可見無(wú)人機(jī)最終可以實(shí)現(xiàn)平穩(wěn)的定高。

圖7 無(wú)人機(jī)定高（z 軸）的數(shù)據(jù)檢測(cè)Fig.7 Data detection of UAV fixed height（z-axis）

4.2 對(duì)于無(wú)人機(jī)懸停（x 與y 軸）的數(shù)據(jù)檢測(cè)

通過(guò)由OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)中所傳輸?shù)膞、y 軸坐標(biāo)數(shù)據(jù)，得出無(wú)人機(jī)懸停數(shù)據(jù)變化如圖8所示。無(wú)人機(jī)由一開始的（-530，120）開始運(yùn)動(dòng)，最終較為平穩(wěn)地懸停在（-650，780）坐標(biāo)附近，可見最終無(wú)人機(jī)能夠?qū)崿F(xiàn)較為穩(wěn)定的懸停功能。

圖8 無(wú)人機(jī)懸停（x 與y 軸）的數(shù)據(jù)檢測(cè)Fig.8 Data detection of UAV hovering（x and y axes）

5 結(jié)語(yǔ)

基于MSP432 與OptiTrack 的無(wú)人機(jī)室內(nèi)導(dǎo)航系統(tǒng)設(shè)計(jì)在編隊(duì)飛行方面采用了“從-主”的編隊(duì)飛行模式，相較于“鏈?zhǔn)筋I(lǐng)航者-跟隨者”模式，由于編隊(duì)無(wú)人機(jī)數(shù)目較少，化簡(jiǎn)了控制結(jié)構(gòu)，有更好的穩(wěn)定性。并且在定高以及懸停方面結(jié)合了具有更高精度的OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)，能夠較好地完成無(wú)人機(jī)在室內(nèi)的精準(zhǔn)定位。因此，將無(wú)人機(jī)通過(guò)結(jié)合OptiTrack 運(yùn)動(dòng)捕捉系統(tǒng)并應(yīng)用于實(shí)際工廠中，有望實(shí)現(xiàn)較高精度的巡邏。