基于多傳感器融合的室內(nèi)自主飛行器系統(tǒng)

趙典 王新懷 徐茵 韋佳辰 徐逸飛

摘要：設(shè)計(jì)了以多旋翼無人機(jī)為平臺(tái)，基于多傳感器融合、ADRC自抗擾控制、YOLOv5目標(biāo)檢測(cè)神經(jīng)網(wǎng)絡(luò)等技術(shù)的室內(nèi)自主飛行器系統(tǒng)。系統(tǒng)以Jetson NANO為運(yùn)行平臺(tái)，采用ACFLY EDU飛行控制板運(yùn)行核心控制部分，基于Intel RealSense T265的位置信息，融合飛行控制板IMU、高度氣壓計(jì)、TOF等傳感器數(shù)據(jù)對(duì)無人機(jī)在室內(nèi)的狀態(tài)進(jìn)行估計(jì)和修正，使用ROS和MAVROS構(gòu)建通信系統(tǒng)，以提高無人機(jī)在室內(nèi)飛行的平穩(wěn)性。該系統(tǒng)可以根據(jù)控制命令或自主實(shí)現(xiàn)無人機(jī)一鍵起飛、動(dòng)態(tài)室內(nèi)定位、飛行姿態(tài)控制、對(duì)室內(nèi)目標(biāo)的檢測(cè)與精準(zhǔn)識(shí)別、航向控制與定點(diǎn)降落等功能。經(jīng)電子設(shè)計(jì)競(jìng)賽實(shí)際測(cè)試，本系統(tǒng)能夠不依賴傳統(tǒng)的GPS信號(hào)和光流傳感器等模塊進(jìn)行定位，同時(shí)具有較高的定位精度，可在室內(nèi)復(fù)雜條件下完成自主飛行器既定的設(shè)計(jì)功能。

關(guān)鍵詞：多傳感器融合;室內(nèi)自主飛行;ADRC自抗擾控制;T265;YOLOv5神經(jīng)網(wǎng)絡(luò);ROS

0引言

無人機(jī)在最近十年內(nèi)在民用領(lǐng)域有了長(zhǎng)足的發(fā)展，但由于室內(nèi)全球定位系統(tǒng)（global positioning system，GPS）信號(hào)弱或不可用，傳統(tǒng)平臺(tái)依賴的慣性導(dǎo)航誤差較大，并且室內(nèi)地面效應(yīng)比較嚴(yán)重，所以室內(nèi)無人機(jī)的發(fā)展進(jìn)度相對(duì)緩慢。筆者通過對(duì)室內(nèi)無人機(jī)飛行器比賽的訓(xùn)練和參與發(fā)現(xiàn)存在著如下問題：室內(nèi)定位常用的光流傳感器受環(huán)境光照強(qiáng)度和參照面特征點(diǎn)數(shù)量的影響;氣壓計(jì)高度讀數(shù)隨著樓層的變化，不能正常顯示飛行器相對(duì)地板高度;室內(nèi)地面效應(yīng)嚴(yán)重致使飛行器起飛時(shí)參數(shù)震蕩，傳統(tǒng)比例積分微分（proportional integral derivative，PID）控制參數(shù)難以調(diào)整等，因此對(duì)室內(nèi)自主飛行器系統(tǒng)的研究十分必要。

本系統(tǒng)以多旋翼無人機(jī)為平臺(tái)，基于多傳感器融合、ADRC（自抗擾控制）等技術(shù)，設(shè)計(jì)了一款室內(nèi)自主飛行器，在一般室內(nèi)環(huán)境下能夠完成一鍵起飛、動(dòng)態(tài)室內(nèi)定位、航向姿態(tài)控制、目標(biāo)檢測(cè)等功能，拓展了無人飛行器在室內(nèi)環(huán)境下的功能和可用性。

1系統(tǒng)設(shè)計(jì)

1.1系統(tǒng)組成部分

室內(nèi)自主飛行器系統(tǒng)硬件部分包括：機(jī)載主控核心板NVIDIA Jetson NANO，ACfly EDU飛行控制板、Intel RealSense T265 追蹤深度實(shí)感攝像頭、RealSense D435i追蹤深度實(shí)感攝像頭、Tiger動(dòng)力套裝、地面站等。

室內(nèi)自主飛行器系統(tǒng)軟件部分包括：Linux操作系統(tǒng)、機(jī)器人操作系統(tǒng)ROS、通信協(xié)議MAVLINK、自抗擾控制ADRC算法、YOLOv5目標(biāo)檢測(cè)神經(jīng)網(wǎng)絡(luò)等。系統(tǒng)組成結(jié)構(gòu)如圖1所示。

1.2系統(tǒng)工作過程

機(jī)載主控核心板NANO實(shí)時(shí)將T265采集獲得的深度位置數(shù)據(jù)與飛行控制模塊采集的慣性測(cè)量單元（inertial measurement unit，IMU）及氣壓計(jì)數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)對(duì)無人飛行器的當(dāng)前位姿數(shù)據(jù)進(jìn)行糾正和估計(jì)，發(fā)送到飛控板，飛控板獲得位姿數(shù)據(jù)后根據(jù)控制指令對(duì)無人飛行器位置和姿態(tài)進(jìn)行控制。機(jī)載D435i深度相機(jī)可以通過部署在Jetson NANO上的YOLOv5神經(jīng)網(wǎng)絡(luò)對(duì)目標(biāo)進(jìn)行識(shí)別。獲得目標(biāo)的相對(duì)位置和坐標(biāo)，并將數(shù)據(jù)上傳到飛控板。

Jetson NANO與飛行控制板使用MAVLINK協(xié)議通過串口實(shí)現(xiàn)通信功能;通過WIFI連接遠(yuǎn)程上位機(jī)部署調(diào)試界面NoMachine進(jìn)行非接觸式調(diào)試和任務(wù)布置工作。通過各個(gè)模塊的有機(jī)結(jié)合，最終實(shí)現(xiàn)了基于多傳感器融合的室內(nèi)自主飛行器系統(tǒng)，可以借助NoMachine完成遠(yuǎn)程命令下達(dá)實(shí)現(xiàn)自主飛行及功能操作。

2系統(tǒng)功能實(shí)現(xiàn)

2.1硬件部分

系統(tǒng)采用Nvidia Jetson NANO作為板載計(jì)算機(jī)。Jetson NANO的浮點(diǎn)計(jì)算性能達(dá)到472 GFLOPS的同時(shí)，它能并行運(yùn)行多個(gè)神經(jīng)網(wǎng)絡(luò)并同時(shí)處理多個(gè)高分辨率的傳感器。并且它具有小型化、輕量化和低功耗的特點(diǎn)，適合在無人機(jī)環(huán)境中進(jìn)行部署和開發(fā)。

它作為無人機(jī)系統(tǒng)的核心，承擔(dān)著信息處理和數(shù)據(jù)交互等工作，我們?cè)谒南到y(tǒng)之上搭建了ros（實(shí)時(shí)操作系統(tǒng)）平臺(tái)，此平臺(tái)可以以節(jié)點(diǎn)的方式高效的處理來自各個(gè)傳感器的數(shù)據(jù)，同時(shí)在ros平臺(tái)上可以很方便的調(diào)用各個(gè)模塊廠家發(fā)布的模塊包，簡(jiǎn)化，方便我們獲取來自飛行時(shí)間（time of flight，ToF），攝像頭，飛控等各個(gè)模塊傳遞過來的信息數(shù)據(jù)，板載計(jì)算機(jī)進(jìn)行綜合處理之后可以通過串口，發(fā)送特定協(xié)議指令與飛控進(jìn)行通信，從而控制無人的各種姿態(tài)運(yùn)動(dòng)。

系統(tǒng)采用Intel RealSense T265作為多傳感器融合的核心模塊，利用其內(nèi)部的魚眼鏡頭、IMU等模塊獲取環(huán)境中的視覺特征，而其內(nèi)部帶有Movidius Myriad 2視覺處理單元（VPU）可以直接在攝像頭內(nèi)部運(yùn)行V-SLAM算法，減輕板材計(jì)算機(jī)數(shù)據(jù)處理的壓力，可以直接從攝像頭獲得其位姿數(shù)據(jù)，在未知空間可精確追蹤其路徑。在預(yù)期使用環(huán)境下它可以達(dá)到低于1%的閉環(huán)漂移誤差且也具有低功耗、小型化、輕量化等特點(diǎn)，有利于無人機(jī)在GPS數(shù)據(jù)不可用的位置進(jìn)行穩(wěn)定飛行操作。

項(xiàng)目采用ACFLY EDU開源飛控做為飛行控制板，其采用TM4C123GH6PM作為核心芯片，能夠兼容MAVLINK通信協(xié)議，飛控內(nèi)部帶有陀螺儀、磁羅盤、氣壓計(jì)等模塊，并支持外部傳感器模塊的拓展，能對(duì)多傳感器數(shù)據(jù)進(jìn)行檢測(cè)和融合，得到無人機(jī)當(dāng)前自身的姿態(tài)，速度等數(shù)據(jù)信息。飛控通過四路PWM波控制電調(diào)出力，進(jìn)而控制各個(gè)電機(jī)的轉(zhuǎn)速，完成對(duì)無人飛行器的位姿和速度控制。

ToF模塊，通過激光反射時(shí)間差獲取距離信息，它具有高精度，小體積，低功耗等特點(diǎn)，其距離分辨率可達(dá)到1 cm，同時(shí)模塊通信方式支持串口模式，IIC模式，modbus模式，用戶可根據(jù)自身系統(tǒng)配特點(diǎn)對(duì)其進(jìn)行配置，該模塊可以與飛控通信，飛控再將其數(shù)據(jù)與自身帶有的氣壓計(jì)等模塊實(shí)現(xiàn)無人機(jī)飛行高度的控制和穩(wěn)定。

2.2軟件部分

軟件部分主要負(fù)責(zé)控制無人機(jī)姿態(tài)位置、板際通信實(shí)現(xiàn)、傳感器數(shù)據(jù)采集、目標(biāo)檢測(cè)、遠(yuǎn)程控制等。

為了增加無人機(jī)姿態(tài)控制的精度以對(duì)抗室內(nèi)環(huán)境的地面效應(yīng)，筆者使用ADRC自抗擾控制算法代替?zhèn)鹘y(tǒng)的PID控制算法以完成室內(nèi)飛行控制的目的。傳統(tǒng)PID 算法是由飛行控制板從IMU模塊中獲取四元數(shù)，對(duì)四元數(shù)濾波后進(jìn)行轉(zhuǎn)換得到歐拉角來獲取當(dāng)前的姿態(tài)信息，通過構(gòu)建PID模型的比例、積分、微分、三個(gè)量值來獲得最終輸出。然而傳統(tǒng)PID算法的不足在于無法自適應(yīng)外界干擾來調(diào)整比例、積分、微分三個(gè)量值使其能夠始終控制姿態(tài)穩(wěn)定，這在室內(nèi)環(huán)境下尤其重要。因此筆者采用ADRC方法即使用跟蹤微分器TD、擴(kuò)張狀態(tài)觀測(cè)器ESO和狀態(tài)誤差反饋控制律（NLSEF）三個(gè)模塊對(duì)無人機(jī)進(jìn)行控制。

通信方面，系統(tǒng)NANO主控板環(huán)境中運(yùn)行Ubuntu 18.04，并安裝ROS Melodic版本，更新MAVROS擴(kuò)展包。MAVROS通信協(xié)議被廣泛應(yīng)用于無人機(jī)通信，其是一種為微形飛行器設(shè)計(jì)的非常輕巧的、只由頭文件構(gòu)成的信息編組庫。無人機(jī)的控制、狀態(tài)、位置等信息都可編譯為MAVROS數(shù)據(jù)包。機(jī)載電腦內(nèi)部ROS節(jié)點(diǎn)調(diào)用MAVROS包進(jìn)行信息的訂閱和發(fā)布，將ROS數(shù)據(jù)與支持MAVLINK的飛行控制板進(jìn)行連接并傳輸相關(guān)數(shù)據(jù)。

目標(biāo)檢測(cè)方面，系統(tǒng)依賴于NANO的TensorRT深度學(xué)習(xí)加速引擎部署YOLOv5模型。無人機(jī)通過D435i 深度相機(jī)獲取目標(biāo)圖片，并將圖片傳入該模型中進(jìn)行檢測(cè)獲取目標(biāo)中心點(diǎn)位置信息以及目標(biāo)在圖片中的相對(duì)坐標(biāo)信息。將這些信息與深度相機(jī)獲得的深度點(diǎn)云圖結(jié)合，再通過opencv功能包進(jìn)行一系列的圖形學(xué)形態(tài)學(xué)操作處理，可以得到目標(biāo)物形態(tài)、分類，以及與無人機(jī)的距離位置等信息。板載電腦根據(jù)此類信息，結(jié)合當(dāng)前運(yùn)行的任務(wù)要求，發(fā)送不同的無人機(jī)運(yùn)動(dòng)控制指令，完成需要實(shí)現(xiàn)的目標(biāo)。

系統(tǒng)任務(wù)下達(dá)與遠(yuǎn)程調(diào)試依賴于NoMachine圖形遠(yuǎn)程界面Linux版本，通過Wi-Fi將地面站上位機(jī)與Jetson NANO進(jìn)行連接。可以在地面站上位機(jī)觀察NANO的輸出值，對(duì)其進(jìn)行實(shí)時(shí)調(diào)試;也可通過上位機(jī)向機(jī)載電腦發(fā)送控制命令以控制無人機(jī)動(dòng)作。

3實(shí)際項(xiàng)目設(shè)計(jì)

3.1項(xiàng)目設(shè)計(jì)需求

本項(xiàng)目參加了2021年全國大學(xué)生電子設(shè)計(jì)競(jìng)賽G 賽題植保飛行器。賽題要求無人機(jī)按以下播撒區(qū)域飛行并做出指示：

賽題要求飛行器在“十”字起降點(diǎn)垂直起飛，升空至150±10cm巡航高度，尋找播撒作業(yè)起點(diǎn)，從“A”所在區(qū)塊開始“撒藥”作業(yè)，必須在360秒內(nèi)完成對(duì)圖7中所有綠色區(qū)塊進(jìn)行全覆蓋播撒，作業(yè)完成后穩(wěn)定準(zhǔn)確降落在起降點(diǎn);飛行器幾何中心點(diǎn)與起降點(diǎn)中心距離的偏差不大于士10 cm。

在作業(yè)區(qū)中放置一只高度為150 cm、直徑3.5±0.5 cm的黑色桿塔，桿塔上套有圓環(huán)形條形碼（放條碼的高度為120～140cm）;作業(yè)中或返航途中，飛行器識(shí)別條形碼所表征的數(shù)字，用LED閃爍次數(shù)顯示數(shù)字，間隔數(shù)秒后再次閃爍顯示。

最后以起降點(diǎn)“十”字中心為圓心，以上述桿塔二維碼中識(shí)別的數(shù)字乘10 cm為半徑，飛行器在該圓周上穩(wěn)定降落;飛行器幾何中心點(diǎn)與該圓周最近距離的偏差不大于±10 cm。

3.2無人機(jī)運(yùn)動(dòng)控制邏輯

針對(duì)賽題要求，我們采取無人機(jī)以固定軌跡路線進(jìn)行播撒作業(yè)的方案，此方案可以有效的實(shí)現(xiàn)所有綠色區(qū)域檢測(cè)并避開桿子防止區(qū)域，減少邏輯復(fù)雜度，簡(jiǎn)化系統(tǒng)流程。

運(yùn)行軌跡如圖8箭頭所示。

想要完整實(shí)現(xiàn)的場(chǎng)地的遍歷，需要建立在無人機(jī)對(duì)于自身的精確定位，而在實(shí)際測(cè)試過程中，無人機(jī)由于地面效應(yīng)等原因，在無人機(jī)起飛過程中會(huì)有不可控的震蕩，這容易造成T265數(shù)據(jù)的失效，因此在每次任務(wù)運(yùn)行前，通過攜帶無人機(jī)遍歷場(chǎng)地獲取更多有效的視覺數(shù)據(jù)，可以有效減少T265自身數(shù)據(jù)出錯(cuò)的概率。整個(gè)過程中無人機(jī)使用ARDC自抗擾控制進(jìn)行姿態(tài)自穩(wěn)和位置校正。這一系列過程中系統(tǒng)調(diào)用了無人機(jī)姿態(tài)控制接口、位置控制接口、通過MAVLINK協(xié)議完成了上述控制。

3.3無人機(jī)功能控制邏輯

無人機(jī)在繞場(chǎng)飛行中，通過獲取自身的定位數(shù)據(jù)，判斷需要進(jìn)行的功能模塊，在無人機(jī)遍歷綠色區(qū)域塊的過程中，將會(huì)調(diào)用無人機(jī)下方的攝像頭獲取圖片，再通過顏色閾值判斷等方法對(duì)圖片進(jìn)行檢測(cè)，判斷下方作物顏色，以確定是否需要激光筆的閃爍。

而再遍歷完所有綠色區(qū)域的，返航時(shí)（即圖六中藍(lán)色箭頭所示行進(jìn)路線），將會(huì)打開無人機(jī)側(cè)方攝像頭獲取圖片，并通過顏色閾值提取，膨脹腐蝕，霍夫直線檢測(cè)等一系列圖形學(xué)處理算法，再結(jié)合t265獲取到的深度圖共同判斷，可以有效的獲取桿子的距離以及方位，通過PID等算法控制無人機(jī)向桿子位置靠近，最后再通過深度學(xué)習(xí)等算法獲取二維碼所在的位置，調(diào)節(jié)無人機(jī)姿態(tài)向其靠近，接近一定距離后讀取出相關(guān)條形碼，根據(jù)條形碼上的有效數(shù)據(jù)進(jìn)行降落操作。

整個(gè)系統(tǒng)模塊的流程圖如圖9所示。

4項(xiàng)目測(cè)試

本項(xiàng)目在自行搭建的，全封閉的保護(hù)網(wǎng)內(nèi)進(jìn)行實(shí)驗(yàn)測(cè)試，我們分別測(cè)試了無人機(jī)的失敗播撒（“漏撒”和“誤撒”）的次數(shù)，下降位置的準(zhǔn)確度，以及二維碼識(shí)別成功率。

經(jīng)過實(shí)際測(cè)試，此套無人機(jī)系統(tǒng)可以實(shí)現(xiàn)無人機(jī)在室內(nèi)精確定位以及穩(wěn)定飛行。此項(xiàng)目方案流程可以較為完整的完成題目要求的各個(gè)功能，而完成度與無人機(jī)穩(wěn)定性及周圍環(huán)境密切相關(guān)。在一個(gè)較為寬闊且光線可控的封閉空間中，無人機(jī)的穩(wěn)定性和完成度可以得到大幅度的提升。

5結(jié)語

構(gòu)建了一種基于多傳感器融合的室內(nèi)自主飛行器系統(tǒng)，使用Intel RealSense T265攝像頭作為室內(nèi)定位傳感器，在NVIDIA Jetson NANO上搭載ROS，采用ACFLY EDU飛控進(jìn)行無人機(jī)姿態(tài)和位置控制，基于MAVROS-MAVLINK進(jìn)行通信，使用Intel Realsense T265深度相機(jī)在Jetson NANO上部署YOLOv5進(jìn)行目標(biāo)識(shí)別，通過無人機(jī)任務(wù)管理系統(tǒng)實(shí)現(xiàn)無人機(jī)位置和姿態(tài)控制。通過參加電子設(shè)計(jì)競(jìng)賽無人機(jī)賽題試驗(yàn)測(cè)試自主飛行器系統(tǒng)的性能。比賽實(shí)驗(yàn)結(jié)果表明：

該系統(tǒng)實(shí)現(xiàn)了無人機(jī)在室內(nèi)一鍵起飛、自主懸停、自主飛行、目標(biāo)檢測(cè)和自主降落功能。

無人機(jī)能夠按照預(yù)定飛行路線自主飛行、傳感器融合達(dá)到了室內(nèi)定位所需的精度要求、能夠按照要求完成既定任務(wù)。

該系統(tǒng)定位精度高、穩(wěn)定性強(qiáng)、能夠完成識(shí)別工作，對(duì)于室內(nèi)智能無人機(jī)的實(shí)現(xiàn)具有很好的借鑒意義。

參考文獻(xiàn)：

[1]姚光樂，王守雷.Pixhawk飛控系統(tǒng)的四旋翼無人機(jī)室內(nèi)飛行技術(shù)研究[J].電子世界，2021（07）：86-88.

[2]藺曉俊，薛濤，孫習(xí)貞.基于視覺慣性里程計(jì)的室內(nèi)自主飛行無人機(jī)系統(tǒng)設(shè)計(jì)[J].機(jī)械設(shè)計(jì)，2021：99-103.

[3]李佳佳.四旋翼飛行器一致性控制研究[D].陜西：西安電子科技大學(xué).2018.

[4]王晶晶，顧得.無人機(jī)室內(nèi)自主定位方法研究[J].計(jì)算機(jī)與數(shù)字工

程，2022（01）：801-806.

[5]祁江鑫，吳玲，盧發(fā)興，等.基于改進(jìn)YOLOv4算法的無人機(jī)目標(biāo)檢測(cè)[J].兵器裝備工程學(xué)報(bào)，2022.

[6]李健，劉孔宇，任憲盛，等.基于自適應(yīng)閾值的Canny算法在MRI邊緣檢測(cè)中的應(yīng)用[J].吉林大學(xué)學(xué)報(bào)，2021（02）：22-25.

[7]MAGNUS LIE HETLAND. Python基礎(chǔ)教程[M].北京：人民郵電出版社，2018.

[8]葛暢，何振鵬，年哲，等.四旋翼無人機(jī)姿態(tài)測(cè)試平臺(tái)設(shè)計(jì)及運(yùn)動(dòng)控制研究[J].機(jī)械設(shè)計(jì)，2019，36（1）：136-140.

[9]林曉華，王茂森，戴勁松.基于ROS的無人機(jī)占據(jù)地圖構(gòu)建方案設(shè)計(jì)[J].兵器裝備工程學(xué)報(bào)，2021，42（5）：215-220.

[10]趙麗榮，楊毅.無人機(jī)室內(nèi)定位技術(shù)綜述[C].第十四屆全國信號(hào)和智能信息處理與應(yīng)用學(xué)術(shù)會(huì)議論文集，北京，2021：270-273.

[11]張熙恒，張建強(qiáng).多旋翼飛行器DIY入門與時(shí)間[M].西安：西安電子科技大學(xué)出版社，2018：144-146.