基于MSP432與OptiTrack的無人機(jī)室內(nèi)導(dǎo)航系統(tǒng)設(shè)計

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

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

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

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

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

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

無人機(jī)由MSP432R401R Launchpad 作為飛行控制模塊，并搭載電機(jī)驅(qū)動模塊，HC-SR04 超聲波測距模塊，光流模塊等。飛行控制系統(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 位差動1MSPS SAR ADC，兩個比較器、高級加密標(biāo)準(zhǔn)（AES256）加速器、CRC、DMA、32 位硬件乘法器、4 個16 位、2 個32 位定時計數(shù)器、通信接口豐富，多達(dá)4 個I2C、8個SPI、4 個UART[8]。

1.2 驅(qū)動模塊

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

1.3 超聲波測距模塊

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

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

2.1 搭建區(qū)域選擇

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

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

2.2 固定相機(jī)位置

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

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

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

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

2.4 標(biāo)定

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

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

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

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

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

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

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

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

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

4 無人機(jī)定位精度測量

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

4.1 對于無人機(jī)定高（z 軸）的數(shù)據(jù)檢測

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

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

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

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

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

5 結(jié)語

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