無(wú)人機(jī)在室內(nèi)環(huán)境中自主飛行與避障

呂 倩，陶 鵬，吳 宏，胡向陽(yáng)，張璦涵

（上海工程技術(shù)大學(xué)電子電氣工程學(xué)院，上海 210600）

0 引言

微型飛行器（Micro Air Vehicle，MAV）與其它空中機(jī)器人相比，由于四旋翼飛行器機(jī)械和控制的簡(jiǎn)單性、高機(jī)動(dòng)性及低入門(mén)、低成本的特點(diǎn)，備受學(xué)術(shù)界和工業(yè)界青睞，近年來(lái)成為研究熱點(diǎn)。隨著MAV 飛行控制技術(shù)的日趨成熟及GPS 技術(shù)在民用領(lǐng)域的普及，MAV 在空中操縱［1］、智能監(jiān)視和偵察（Intelligent Surveillance and Reconnaissance，ISR）［2］、航空攝影、結(jié)構(gòu)檢測(cè)［3］、機(jī)器人應(yīng)急［4］、建筑［5］和計(jì)算機(jī)視覺(jué)［6］等領(lǐng)域應(yīng)用廣泛，相關(guān)研究也推動(dòng)了四旋翼飛行器的進(jìn)一步發(fā)展。

無(wú)人機(jī)高度依賴周圍環(huán)境實(shí)際測(cè)量數(shù)據(jù)的準(zhǔn)確性。在室外環(huán)境中可利用良好的GPS 信號(hào)，在實(shí)驗(yàn)室環(huán)境中可使用超寬帶（Ultra Wide Band，UWB）技術(shù)［7］或通過(guò)適當(dāng)放置的特殊標(biāo)簽獲悉環(huán)境實(shí)況。然而在飛行過(guò)程中，實(shí)際環(huán)境是動(dòng)態(tài)的，GPS 信號(hào)也可能因建筑物遮擋等因素中斷或處于較弱的信號(hào)狀態(tài)，而放置標(biāo)簽又只能在已知環(huán)境下采用。針對(duì)以上問(wèn)題，本文提出一種僅依靠無(wú)人機(jī)機(jī)載計(jì)算機(jī)在無(wú)GPS 的未知環(huán)境中，實(shí)現(xiàn)無(wú)人機(jī)自主飛行、規(guī)避障礙物與穿越特定障礙物的方法。

計(jì)算能力有限的純機(jī)載傳感平臺(tái)地圖構(gòu)建工作可以使用激光測(cè)距儀實(shí)現(xiàn)［8］。由于激光掃描匹配限制及計(jì)算能力有限，該方法僅限于慢速飛行情況，且由無(wú)人機(jī)攜帶的體積小、重量輕的激光測(cè)距儀僅能測(cè)量單個(gè)平面中的距離，因此該方法需要對(duì)環(huán)境進(jìn)行簡(jiǎn)化假設(shè)。

隨著計(jì)算能力的增長(zhǎng)和更高效算法的提出，將視覺(jué)用于無(wú)人機(jī)狀態(tài)估計(jì)成為可能［9］。為了最小化無(wú)人機(jī)機(jī)體重量與飛行過(guò)程中的功率消耗，可以僅使用單個(gè)向下或向前的攝像機(jī)。MAV 中大多數(shù)單目視覺(jué)算法［10］依賴于并行跟蹤與建圖（Parallel Tracking and Mapping，PTAM）。然而，PTAM 主要應(yīng)用于增強(qiáng)現(xiàn)實(shí)，在大規(guī)模復(fù)雜環(huán)境中操作還需多次修改。隨著視覺(jué)算法的改進(jìn)和小型計(jì)算機(jī)可用計(jì)算能力的增長(zhǎng)，相機(jī)現(xiàn)已成為無(wú)人機(jī)檢測(cè)與識(shí)別環(huán)境障礙物的首選傳感器。

視覺(jué)導(dǎo)航的出現(xiàn)解決了無(wú)GPS 信號(hào)或弱GPS 信號(hào)下無(wú)法準(zhǔn)確獲取無(wú)人機(jī)位置與姿態(tài)的難題，但僅依賴于相機(jī)傳感器識(shí)別障礙物準(zhǔn)確度不高，而且會(huì)由于光線明暗、距離遠(yuǎn)近不同等原因?qū)е伦R(shí)別錯(cuò)誤。本文通過(guò)融合更多機(jī)載傳感器測(cè)得的數(shù)據(jù)，更真實(shí)地反映無(wú)人機(jī)及其周圍環(huán)境的狀況，從而獲得真實(shí)準(zhǔn)確的信息，為無(wú)人機(jī)在室內(nèi)等無(wú)GPS 或弱GPS 信號(hào)下的飛行與避障提供保障。

1 硬件平臺(tái)設(shè)計(jì)

硬件平臺(tái)設(shè)計(jì)的指導(dǎo)原則是保證無(wú)人機(jī)快速靈活地飛行。為達(dá)到20m/s 的速度，要求飛行器必須在大多數(shù)傳感器檢測(cè)距離（約20～25m）內(nèi)使無(wú)人機(jī)速度減為0，這意味著平臺(tái)必須能達(dá)到10m/s2的加速度。為保證飛行器可在保持高度的同時(shí)達(dá)到該加速度，飛行器推力重量比約為1.5。為了在高加速階段保證飛行器有一定的控制力，該平臺(tái)需具有足夠的推力，故在滿載時(shí)提供給飛行器的推重比需超過(guò)2.0。

在對(duì)現(xiàn)有平臺(tái)進(jìn)行調(diào)研的基礎(chǔ)上，最終選擇DJI M600飛行平臺(tái)，包括飛行器、電機(jī)、螺旋槳和速度控制器，該組合非常符合硬件平臺(tái)性能要求。每個(gè)DJI 3510 發(fā)動(dòng)機(jī)和螺旋槳組合具有大約1.6kgf 的額定最大推力，則四旋翼配置總推力約為6.4kgf。

飛行平臺(tái)采用DJI Guidance 視覺(jué)傳感器導(dǎo)航系統(tǒng)，該系統(tǒng)視覺(jué)處理模塊為64g，視覺(jué)傳感模塊為43g，每根VBUS 線纜為11.6g。該平臺(tái)相機(jī)采用大疆禪思X4S，型號(hào)為ZENMUSE X4S，重量為253g。飛行平臺(tái)還使用了加速計(jì)，陀螺儀，氣壓計(jì)等其它傳感器。板載計(jì)算機(jī)采用妙算MANIFOLD。基座平臺(tái)由DJI M100 飛行器及其飛控系統(tǒng)A1 組成，質(zhì)量約為1.6kg；加上傳感器和計(jì)算負(fù)載，平臺(tái)重量為2.1kg。為達(dá)到飛行時(shí)間要求，需選擇合適的電池，同時(shí)需保證平臺(tái)最大總質(zhì)量在3kg 左右。

由于無(wú)人機(jī)電池是能量和功率密度高的基于鋰聚合物化學(xué)的電池。DJI M100 推進(jìn)系統(tǒng)需額定電壓約為22.2V。鑒于此，可用的主要設(shè)計(jì)選擇是電池容量。典型的鋰聚合物電池比能值在130～140Wh/kg，四旋翼機(jī)懸停所需功率約為200W/kg，所以對(duì)于總質(zhì)量在3～3.5kg 左右的平臺(tái)，功耗為600～700W。假設(shè)電池整體效率約為60%，從電池供電到螺旋槳機(jī)械功率輸出，電池5min 飛行時(shí)間能量容量約為69.4～83.4Wh。在實(shí)踐中，為了保護(hù)電池使用壽命，不會(huì)消耗電池全部容量，需為不可預(yù)見(jiàn)的情況預(yù)留一些容量。假設(shè)使用電池額定容量的80%，則需要電池能量容量為86.8～104.1Wh。使用135Wh/kg 的平均比能值，期望電池質(zhì)量在0.64～0.77kg 之間，這與整個(gè)平臺(tái)總質(zhì)量預(yù)算非常吻合。根據(jù)可用電池容量，選擇TB47D 型號(hào)電池，以便在性能和續(xù)航能力方面提供一定靈活性。該型號(hào)電池容量為4 500mAh，電池整體重量為600g，電池能量為99.9Wh。無(wú)人機(jī)飛行平臺(tái)如圖1 所示。

Fig.1 UAV hardware platform圖1 無(wú)人機(jī)硬件平臺(tái)

2 傳感器數(shù)據(jù)融合

在無(wú)人機(jī)平臺(tái)上有多個(gè)傳感器，每個(gè)傳感器均提供機(jī)器人狀態(tài)信息。例如加速計(jì)測(cè)量無(wú)人機(jī)加速度，經(jīng)積分運(yùn)算后可獲得無(wú)人機(jī)速度與位移；陀螺儀測(cè)量角速度，積分運(yùn)算后可獲得角度；氣壓計(jì)與高度傳感器可測(cè)量飛行高度等。此外，傳感器以不同速率提供輸出，例如，以40hz 的速度運(yùn)行單目攝像機(jī)，以20hz 的速度運(yùn)行向下的高度傳感器，以200hz 的速度運(yùn)行加速計(jì)。主要利用卡爾曼濾波器變體完成傳感器融合任務(wù)，將這些信息合并成無(wú)人機(jī)整體狀態(tài)的一致估計(jì)。四旋翼飛行器旋轉(zhuǎn)自由度決定了無(wú)人機(jī)是一個(gè)非線性系統(tǒng)，需使用擴(kuò)展卡爾曼濾波器（Extended Kalman Filter，EKF）或無(wú)跡卡爾曼濾波器（Unscented Kal?man Filter，UKF）。由于UKF 具有較好的處理系統(tǒng)與非線性的優(yōu)點(diǎn)，且計(jì)算量增加較少，所以選擇UKF 執(zhí)行傳感器融合任務(wù)［11］。

圖2 為運(yùn)行在無(wú)人機(jī)上的UKF 模塊數(shù)據(jù)輸入和輸出流程。根據(jù)相機(jī)傳感器拍攝到的視頻數(shù)據(jù)識(shí)別環(huán)境中的障礙物，再根據(jù)加速計(jì)、氣壓計(jì)、大疆視覺(jué)傳感導(dǎo)航系統(tǒng)DJI Guidance、陀螺儀等其它機(jī)載傳感器測(cè)量得到的數(shù)據(jù)，經(jīng)無(wú)跡卡爾曼濾波融合數(shù)據(jù)后控制無(wú)人機(jī)飛行方向、距離與飛行速度，從而避開(kāi)障礙物。

Fig.2 Data flow of UAV unscented Kalman filter圖2 無(wú)人機(jī)無(wú)跡卡爾曼濾波數(shù)據(jù)流

UKF 使用的狀態(tài)向量為：

其中，p 是無(wú)人機(jī)在真實(shí)世界的位置坐標(biāo)，p˙為無(wú)人機(jī)在真實(shí)世界的速度，bα是加速度計(jì)偏移，bω是陀螺儀偏移。使用ZYX 公約的歐拉角φ、θ 和ψ 表示方向，φ、θ 和ψ分別是橫滾、俯仰和偏航，歐拉角十分簡(jiǎn)潔，因此用以表示方向。由于四旋翼無(wú)人機(jī)橫滾和俯仰角不會(huì)大于90°，故在該情況下使用歐拉角時(shí)不會(huì)受萬(wàn)向鎖問(wèn)題影響。

UKF 包括預(yù)測(cè)步驟和多個(gè)更新步驟。根據(jù)慣性測(cè)量單元（Inertial Measurement Unit，IMU）等傳感器測(cè)得的數(shù)據(jù)進(jìn)行預(yù)測(cè)，更新步驟用于對(duì)預(yù)測(cè)值進(jìn)行修正。由于加速度計(jì)與陀螺儀的測(cè)量在機(jī)體坐標(biāo)系中，而位置和速度在世界坐標(biāo)系中，所以預(yù)測(cè)步驟是非線性的，需要利用估計(jì)的方向?qū)y(cè)量從機(jī)體坐標(biāo)系轉(zhuǎn)換到世界坐標(biāo)系。

假設(shè)迭代狀態(tài)為k，xk（維數(shù)n）具有均值和協(xié)方差Pk，用具有均值和協(xié)方差Qk的過(guò)程噪聲（維度p）對(duì)其進(jìn)行擴(kuò)充，從而創(chuàng)建增廣狀態(tài)與協(xié)方差矩陣。

再將無(wú)跡變換應(yīng)用于增廣態(tài)，生成一組sigma 點(diǎn)。

其中L=n+p是增強(qiáng)狀態(tài)的維度，λ是縮放參數(shù)。

然后，利用過(guò)程模型傳播這些sigma 點(diǎn)，其中輸入為加速度計(jì)與陀螺儀測(cè)量值。

每當(dāng)新的傳感器測(cè)量值Yk+1到達(dá)時(shí)，即運(yùn)行濾波器更新步驟。首先，以與預(yù)測(cè)步驟相同的方式生成一組新的sigma 點(diǎn)。

除了用于預(yù)測(cè)步驟的IMU 之外，對(duì)于UKF 的每個(gè)傳感器輸入均有一個(gè)單獨(dú)的測(cè)量函數(shù)h(x,n)，當(dāng)從任何一個(gè)傳感器接收到輸入時(shí)，將執(zhí)行完整的更新步驟。

考慮到在跨越障礙物時(shí)高度傳感器的跳躍問(wèn)題，UKF保持了一個(gè)內(nèi)部基本高度參數(shù)。如果高度傳感器輸出與期望值相比有跳躍，則UKF 假設(shè)基礎(chǔ)高度已改變，并使用新的基礎(chǔ)高度作為高度傳感器的參考級(jí)別，較好地處理了傳感器輸出跳變。該方法局限性在于當(dāng)高度變化緩慢時(shí)，機(jī)器人只會(huì)相對(duì)于基礎(chǔ)高度保持所需高度，并且會(huì)隨著基礎(chǔ)高度的升降而上下移動(dòng)。

由機(jī)載傳感器獲得的數(shù)據(jù)經(jīng)無(wú)跡卡爾曼濾波進(jìn)行數(shù)據(jù)融合后，可使無(wú)人機(jī)獲取的環(huán)境信息更詳細(xì)，若只依靠單個(gè)傳感器片面測(cè)量飛行器某方面數(shù)據(jù)，則無(wú)法感知飛行器與環(huán)境的整體狀態(tài)，無(wú)人機(jī)也無(wú)法精確判斷障礙物和控制飛行方向及飛行速度，這將導(dǎo)致無(wú)人機(jī)無(wú)法精準(zhǔn)避障，因此需與機(jī)載傳感器信息融合。

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

3.1 仿真平臺(tái)實(shí)驗(yàn)

為測(cè)試系統(tǒng)性能，在開(kāi)源自動(dòng)駕駛仿真平臺(tái)Air?Sim［12-13］上進(jìn)行仿真實(shí)驗(yàn)，仿真環(huán)境如圖3 所示。該平臺(tái)模擬真實(shí)世界復(fù)雜未知環(huán)境，無(wú)GPS 與全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS）信號(hào)，無(wú)人機(jī)需要避開(kāi)大型建筑物、密集樹(shù)木、廢舊汽車、路障等障礙物，并按照數(shù)字順序依次穿越紅色圓環(huán)，保證不碰撞圓環(huán)和其它障礙物，不漏穿錯(cuò)穿圓環(huán)。

Fig.3 Simulation test environment圖3 仿真測(cè)試環(huán)境

機(jī)載相機(jī)傳感器采集經(jīng)過(guò)處理的仿真場(chǎng)景圖像，如圖4—圖6 所示。圖4 為實(shí)物圖，圖5 為深度圖，圖6 為分割圖。將相機(jī)采集到的視頻數(shù)據(jù)進(jìn)行分割［14］，如圖6 所示，可區(qū)分可供無(wú)人機(jī)自由飛行的空曠區(qū)域、無(wú)人機(jī)需穿越的障礙圈與其它必須避開(kāi)的無(wú)關(guān)障礙物。

Fig.4 Simulation platform scene acquired by camera圖4 相機(jī)獲取的仿真平臺(tái)場(chǎng)景

Fig.5 Depth image圖5 深度圖

無(wú)人機(jī)避開(kāi)障礙物、穿越障礙圈的過(guò)程為：保證遠(yuǎn)離除圓環(huán)之外的障礙物，并且需識(shí)別圓環(huán)；在開(kāi)始之后進(jìn)行4次判斷，首先判斷是否成功起飛，如果失敗則進(jìn)入起飛失敗策略重新起飛，如果成功，則攝像頭開(kāi)始啟動(dòng)；在飛機(jī)前行的同時(shí)根據(jù)攝像頭拍攝的圖像開(kāi)始識(shí)別與檢測(cè)，判斷是否發(fā)現(xiàn)圓環(huán)，若未發(fā)現(xiàn)圓環(huán)，飛機(jī)向左或向右移動(dòng)，如果發(fā)現(xiàn)圓環(huán)，則飛機(jī)向圓環(huán)方向移動(dòng)并開(kāi)始穿越圓環(huán)；最后判斷是否穿越成功，如果穿越成功則記入成功數(shù)目，否則進(jìn)入穿越圓環(huán)失敗策略。仿真流程圖如圖7 所示。

Fig.6 Segmentated image圖6 分割圖

Fig.7 Simulation test flow圖7 仿真測(cè)試流程

為完成該測(cè)試，系統(tǒng)不僅需準(zhǔn)確避障，還需識(shí)別場(chǎng)景中特定紅色障礙圈。在仿真環(huán)境中所有障礙圈的顏色為紅色，并形成一塊特殊標(biāo)識(shí)的連通區(qū)域，采用兩邊掃描法識(shí)別并標(biāo)記圖片中所有連通區(qū)域。利用HSV 顏色空間識(shí)別紅色區(qū)域，再使用霍夫變換等方法檢測(cè)需要穿越的圓環(huán)位置與半徑，確保無(wú)人機(jī)盡可能從圓環(huán)中心穿過(guò)，避免與障礙物碰撞。圖8 為無(wú)人機(jī)在仿真平臺(tái)中穿越紅色障礙圈時(shí)刻。

Fig.8 Moment of UAV successfully crossing red obstacle circle in simulation environment圖8 仿真環(huán)境中無(wú)人機(jī)成功穿越紅色障礙圈時(shí)刻

3.2 硬件飛行實(shí)驗(yàn)

在現(xiàn)實(shí)世界中無(wú)人機(jī)穿越障礙圈的時(shí)刻如圖9 所示。由于現(xiàn)實(shí)中有諸多不穩(wěn)定因素，包括光線明暗變化、無(wú)人機(jī)傳感器精度有限等一系列問(wèn)題，造成穿越障礙圈的動(dòng)作不能保證百分之百成功，但在現(xiàn)實(shí)世界中若測(cè)試參數(shù)設(shè)置合理，光線明暗適中，則可準(zhǔn)確識(shí)別并成功穿過(guò)障礙圈。

Fig.9 Moment of UAV successfully crossing red obstacle circle in reality圖9 現(xiàn)實(shí)世界中無(wú)人機(jī)穿越障礙圈時(shí)刻

4 結(jié)語(yǔ)

本文設(shè)計(jì)了一種基于單目攝像機(jī)與IMU、DJI Guidance 等傳感器的無(wú)人機(jī)自主飛行及避障系統(tǒng)，并使用無(wú)跡卡爾曼濾波融合多傳感器信息，實(shí)現(xiàn)無(wú)人機(jī)與周圍環(huán)境信息區(qū)分、識(shí)別與檢測(cè)。在仿真平臺(tái)使用虛擬無(wú)人機(jī)與傳感器，在現(xiàn)實(shí)世界中使用自主搭建的硬件平臺(tái)，分別針對(duì)自主起飛、避開(kāi)障礙物、穿越障礙圈等任務(wù)進(jìn)行實(shí)驗(yàn)。經(jīng)微軟AirSim 仿真平臺(tái)實(shí)驗(yàn)與現(xiàn)實(shí)世界實(shí)驗(yàn)驗(yàn)證，該系統(tǒng)可精確避開(kāi)無(wú)關(guān)障礙物，穩(wěn)定完成穿越指定障礙圈、目標(biāo)識(shí)別、場(chǎng)景搜索、自主起飛與降落等飛行任務(wù)。