基于MSP432與OptiTrack的無人機室內導航系統設計

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

（天津理工大學 工程訓練中心，天津 300384）

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

航跡規劃、信息互換以及編隊飛行控制策略等問題，是各個無人機研發強國的研究熱點[3-4]，但是近年來，無論是國內外，對于無人機在室內的編隊飛行技術始終無法突破，究其原因是因為室內環境具有更多的時變性和不可預測性，而用于定位的如GPS 等則無法在室內使用或是定位精度不夠[5-6]。

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

1 無人機硬件設計

無人機由MSP432R401R Launchpad 作為飛行控制模塊，并搭載電機驅動模塊，HC-SR04 超聲波測距模塊，光流模塊等。飛行控制系統硬件組成如圖1所示。

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

1.1 飛行控制模塊

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

1.2 驅動模塊

由于無刷電機具有噪聲小、轉速穩定、動力足等優點，在飛行時避免了由于噪聲產生對編隊飛行的影響故選擇無刷電機和電調模塊驅動四旋翼。

1.3 超聲波測距模塊

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

2 OptiTrack 運動捕捉系統

2.1 搭建區域選擇

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

圖2 OptiTrack 系統場地搭建Fig.2 OptiTrack system site construction

2.2 固定相機位置

將相機等距環繞分布在捕獲區域，且必須保證至少有2 臺相機捕獲到Marker 點。對于低高度的目標，相機放置于較低高度處，聚焦在低處位置。相機固定位置如圖3所示。

圖3 OptiTrack 相機固定Fig.3 OptiTrack camera fixed

2.3 相機角度與對焦

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

2.4 標定

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

放置Marker 點需保證點不在同一平面，形成一個立體形狀。對于多個剛體，要保證每個剛體唯一。創造唯一剛體的關鍵思想是避免Motive 內多個剛體的幾何一致性。

在無人機上安裝Marker 點，進行檢測觀察坐標數據，如圖4所示。

圖4 無人機在OptiTrack 運動捕捉系統中的坐標數據Fig.4 Coordinate data of UAV in OptiTrack motion capture system

2.5 坐標數據數據傳輸

OptiTrack 運動捕捉系統數據傳輸過程為攝像頭采集到無人機上Marker 點位置，在Motive 軟件主界面中顯示，無人機的實時位置便可直觀地通過3D 圖像顯示出來，并在已經進行標定的固定坐標系中讀出實時位置數據，精確度可達0.1 mm；使用MATLAB 軟件編寫相關程序通過串口將數據打包發送給無人機，并提供給無人機目標位置的坐標數據，無人機便可從當前位置飛行至目標位置。數據傳輸過程如圖5所示。

圖5 數據傳輸流程Fig.5 Flow chart of data transmission

3 無人機編隊飛行設計

無人機編隊飛行模式采用從-主的飛行模式，在該模式中，首先設定無人機編隊系統中的一架作為領航無人機，其他個體為跟隨無人機，在編隊飛行時，跟隨無人機實時跟隨領航無人機進行飛行。在此種控制算法中，由于有領航無人機和跟隨無人機的這種相對運動模式，可以將無人機編隊系統的隊形控制問題轉換成跟隨無人機跟隨領航者的位置運動情況。因此可以將復雜的多個個體之間的問題轉換成單個個體的運動情況研究，簡化了控制結構，且具有較好的控制效果。

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

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

4 無人機定位精度測量

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

4.1 對于無人機定高（z 軸）的數據檢測

通過對OptiTrack 運動捕捉系統中所傳輸的z軸數據匯總，得出無人機高度數據變化如圖7所示。在設定高度為60 刻度時，無人機的高度在400 ms～800 ms 時為衰減振蕩的過程，最終趨于平穩，并最終穩定在60 刻度值處，可見無人機最終可以實現平穩的定高。

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

4.2 對于無人機懸停（x 與y 軸）的數據檢測

通過由OptiTrack 運動捕捉系統中所傳輸的x、y 軸坐標數據，得出無人機懸停數據變化如圖8所示。無人機由一開始的（-530，120）開始運動，最終較為平穩地懸停在（-650，780）坐標附近，可見最終無人機能夠實現較為穩定的懸停功能。

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

5 結語

基于MSP432 與OptiTrack 的無人機室內導航系統設計在編隊飛行方面采用了“從-主”的編隊飛行模式，相較于“鏈式領航者-跟隨者”模式，由于編隊無人機數目較少，化簡了控制結構，有更好的穩定性。并且在定高以及懸停方面結合了具有更高精度的OptiTrack 運動捕捉系統，能夠較好地完成無人機在室內的精準定位。因此，將無人機通過結合OptiTrack 運動捕捉系統并應用于實際工廠中，有望實現較高精度的巡邏。