基于視覺輔助導(dǎo)航的小旋翼機(jī)群編隊(duì)穿越障礙技術(shù)

王梓豪，朱 波,2，王 奇，厚秉新，秦開宇*

(1.電子科技大學(xué)航空航天學(xué)院 成都611731；2.中山大學(xué)航空航天學(xué)院 廣州510006)

旋翼機(jī)集群編隊(duì)穿越障礙技術(shù)是一個(gè)多學(xué)科交叉的領(lǐng)域，涉及到了編隊(duì)飛行控制[1-2]、無人機(jī)集群自組織網(wǎng)絡(luò)[3-4]、機(jī)載傳感器數(shù)據(jù)融合[5-6]及空間環(huán)境智能感知[7-8]等。其中，旋翼機(jī)集群對(duì)目標(biāo)的感知定位與避障能力是其完成復(fù)雜飛行任務(wù)的重要基礎(chǔ)之一?？紤]到旋翼機(jī)的載荷能力，輕便型視覺傳感器是作為輔助定位與避障穿越任務(wù)的良好選擇。針對(duì)此類問題，國(guó)內(nèi)外已經(jīng)展開了相關(guān)研究，文獻(xiàn)[9-12]提供了目前常見的一些基于視覺輔助的旋翼機(jī)編隊(duì)導(dǎo)航方案；文獻(xiàn)[13-14]提供了基于主?從式(leader-follower)異構(gòu)旋翼機(jī)編隊(duì)的導(dǎo)航方案。此外，在近地面低空飛行的情況下，由于氣壓計(jì)不穩(wěn)定帶來的旋翼機(jī)與地面相對(duì)測(cè)高誤差也是一個(gè)需要解決的難題。隨著激光雷達(dá)的發(fā)展，小型LiDAR激光傳感器可作為旋翼機(jī)低空測(cè)高的良好選擇，研究者們也開始關(guān)注該領(lǐng)域的相關(guān)問題，如基于LiDAR的地形測(cè)量[15]、機(jī)載LiDAR輔助避障[16]、LiDAR與慣性導(dǎo)航器件融合定位[17]等。

旋翼機(jī)編隊(duì)技術(shù)應(yīng)用廣泛，如空中偵測(cè)、編隊(duì)作戰(zhàn)打擊、無人機(jī)物流等。這些應(yīng)用中涉及穿越復(fù)雜障礙[18-20]，如峽谷、高樓縫隙、大型物流倉(cāng)庫(kù)的門窗等。如何為無人機(jī)提供有效的環(huán)境感知和穿越障礙技術(shù)是一個(gè)亟待解決的問題?；谝陨纤伎?，本文提出一種飛行通道約束的概念，將街道兩邊建筑或者具有直線特征的障礙抽象成類似矩形門框的兩條豎邊，并精準(zhǔn)估計(jì)門框的中心點(diǎn)，以此作為期望航點(diǎn)實(shí)現(xiàn)對(duì)障礙的穿越。

該研究?jī)?nèi)容參加了2018年由空軍裝備部舉辦的首屆“無人爭(zhēng)鋒”智能無人機(jī)集群系統(tǒng)挑戰(zhàn)賽科目1(SC-1)：密集編隊(duì)穿越競(jìng)速賽。主要研究貢獻(xiàn)如下：

1)完成異構(gòu)無人機(jī)編隊(duì)穿越仿真驗(yàn)證，并構(gòu)建了一套無人機(jī)集群編隊(duì)飛行實(shí)機(jī)軟硬件測(cè)試平臺(tái)；

2)設(shè)計(jì)一種基于單目視覺輔助的目標(biāo)相對(duì)定位方法，實(shí)現(xiàn)了無人機(jī)編隊(duì)在線實(shí)時(shí)穿越與路徑規(guī)劃；

3)設(shè)計(jì)一套多傳感器融合的安全飛行策略，并設(shè)計(jì)一種基于激光定高數(shù)據(jù)的時(shí)域?yàn)V波器，為無人機(jī)編隊(duì)在低空復(fù)雜環(huán)境中的飛行定高提供保障。

1 無人機(jī)編隊(duì)穿越技術(shù)總體框架

總體框架研究將小旋翼無人機(jī)編隊(duì)穿越系統(tǒng)分為硬件、軟件和算法3個(gè)層面，如圖1所示。

圖1 無人機(jī)編隊(duì)穿越障礙技術(shù)框圖

以此技術(shù)框架為基礎(chǔ)，進(jìn)一步研究基于視覺輔助的小旋翼無人機(jī)編隊(duì)穿越障礙技術(shù)，以解決主?從式異構(gòu)旋翼機(jī)編隊(duì)飛行任務(wù)中的避障穿越、路徑規(guī)劃、安全飛行策略等技術(shù)問題。

2 無人機(jī)編隊(duì)硬件平臺(tái)構(gòu)建

旋翼機(jī)分為主機(jī)和從機(jī)兩個(gè)型號(hào)，主機(jī)采用550 mm軸距機(jī)架和14寸槳葉，質(zhì)量為630 g，最大載重6 kg。從機(jī)采用360 mm軸距機(jī)架和9寸槳葉，機(jī)架總重量為267 g，最大載重3 kg。此外，主機(jī)將額外搭載機(jī)載英特爾迷你i7處理器、機(jī)載路由器、單目視覺傳感器和云臺(tái)，擔(dān)負(fù)起地面站軟件控制、WIFI廣播、障礙感知與定位等重要任務(wù)。圖2展示了試飛準(zhǔn)備階段的主機(jī)和從機(jī)實(shí)物圖。

圖2 試飛階段實(shí)機(jī)展示圖

主機(jī)搭載3自由度云臺(tái)的單目視覺傳感器為避障穿越提供輔助視覺輸入信息。該攝像頭具有輕便、低成本、安裝便捷等特點(diǎn)，最大分辨率1 600×1 200，焦距2.8 mm，刷新率30 Hz，模塊尺寸30 mm×30 mm。采用采樣率500 Hz工業(yè)級(jí)激光傳感器LiDAR Lite輔助定高，實(shí)現(xiàn)低空飛行條件下對(duì)地面的精確相對(duì)定高。根據(jù)光照條件不同，實(shí)測(cè)該激光雷達(dá)有效距離為16～40 m之間。

主機(jī)的機(jī)載型地面站可以通過WiFi路由器實(shí)現(xiàn)和從機(jī)的WiFi收發(fā)模塊進(jìn)行通信，實(shí)現(xiàn)位置信息、控制指令的交互。相比于傳統(tǒng)的固定地面站，由于主機(jī)采用機(jī)載地面站和機(jī)載WiFi路由器的形式，較好地保障了與附近從機(jī)指令交互的實(shí)時(shí)性。此外，地面控制人員若發(fā)現(xiàn)異常，可以利用遠(yuǎn)距離數(shù)傳模塊實(shí)現(xiàn)編隊(duì)一鍵返航功能，進(jìn)一步提升了集群飛行的安全性。

3 編隊(duì)飛行軟件設(shè)計(jì)

平臺(tái)采用的地面站軟件是由北京飛行魔方科技有限公司利用Mission Planner二次開發(fā)的無人機(jī)編隊(duì)控制地面站軟件。該地面站主要由飛行器管理、任務(wù)設(shè)計(jì)、實(shí)時(shí)控制、實(shí)時(shí)數(shù)據(jù)查看等部分組成。本文利用Visual Studio 2015開發(fā)了基于視覺避障算法和數(shù)傳通信的專用開發(fā)包，該開發(fā)包基于編隊(duì)控制軟件提供的服務(wù)使用命名管道實(shí)現(xiàn)雙向通訊，保證了兩個(gè)進(jìn)程通訊的可靠性和時(shí)效性，通過管道發(fā)送命令字符控制編隊(duì)。利用開發(fā)的視覺避障模塊，可以向該地面站發(fā)布最新航點(diǎn)數(shù)據(jù)，實(shí)現(xiàn)無人機(jī)集群的在線路徑規(guī)劃與避障穿越功能；利用遠(yuǎn)距離數(shù)傳模塊實(shí)現(xiàn)緊急返航等功能。利用開發(fā)包和無人機(jī)地面站軟件，可以實(shí)時(shí)監(jiān)控?zé)o人機(jī)集群的基本飛行狀態(tài)、定位數(shù)據(jù)、航跡規(guī)劃軌跡等信息。

4 編隊(duì)避障穿越算法設(shè)計(jì)

4.1 視覺識(shí)別與輔助定位算法

當(dāng)旋翼機(jī)編隊(duì)發(fā)現(xiàn)并接近待穿越的障礙區(qū)域時(shí)，主機(jī)利用視覺傳感器獲取目標(biāo)障礙的圖像信息后求解其幾何穿越點(diǎn)的相對(duì)3D空間位置。利用低精度的GPS獲取了多幀圖像間的基線，采用三角定位初步估計(jì)目標(biāo)穿越點(diǎn)并模擬出穿越路徑。由于GPS模塊往往存在1～5 m的定位估計(jì)誤差，需要在飛行過程中利用多幀2D圖像進(jìn)行連續(xù)跟蹤，修正穿越點(diǎn)的空間位置坐標(biāo)使得其保持在2D圖像的中心區(qū)域，算法流程如圖3所示。

圖3 視覺識(shí)別與避障算法流程圖

假設(shè)兩張圖中同一特征角點(diǎn)空間坐標(biāo)為P(X,Y,Z)，其投影在2D圖像的坐標(biāo)為p2(x2,y2,z2)和p1(x1,y1,z1)。兩幀間相機(jī)運(yùn)動(dòng)為[R,t]，其中R為旋轉(zhuǎn)矩陣，t為平行向量。由于相機(jī)安裝了云臺(tái)，可知旋轉(zhuǎn)矩陣R為單位矩陣，因此兩幀圖像的空間變化由位移向量t表征，可得為基線長(zhǎng)度即為t。

根據(jù)對(duì)極幾何理論可以知，設(shè)x1,x2為p1,p2特征點(diǎn)歸一化坐標(biāo)，它們滿足：

上式左側(cè)為0，可以看做是未知數(shù)為z2的方程進(jìn)行求解，同理可以求得z1。已知目標(biāo)深度后，可以利用單目針孔模型獲得穿越點(diǎn)空間位置，并將穿越航點(diǎn)下發(fā)到地面站。利用多幀實(shí)時(shí)解算連續(xù)修正穿越點(diǎn)定位誤差，以提高成功穿越障礙的概率。當(dāng)發(fā)現(xiàn)穿越點(diǎn)計(jì)算偏離原值大于0.1 m時(shí)，進(jìn)行實(shí)時(shí)路徑規(guī)劃修正。

4.2 編隊(duì)飛行與控制策略

4.2.1多傳感器定高保護(hù)算法

一方面，考慮到氣壓計(jì)和GPS信號(hào)融合獲得的高度信息通常存在0.3～0.8 m的誤差，因此采用激光雷達(dá)測(cè)高作為輔助手段。另一方面，為了防止激光定高數(shù)據(jù)抖動(dòng)或者故障時(shí)旋翼機(jī)出現(xiàn)沖高等危險(xiǎn)動(dòng)作，采用激光、氣壓計(jì)、GPS信號(hào)同時(shí)對(duì)高度進(jìn)行估計(jì)：

1)在0～16 m：正常飛行避障條件下，激光作為主要參考，利用濾波后的激光數(shù)據(jù)作為無人機(jī)相對(duì)地面的測(cè)高值；

2)在12 m以上：加入Pixhawk內(nèi)部融合的氣壓計(jì)和GPS信息作為保護(hù)，如果檢測(cè)到激光定高數(shù)據(jù)相對(duì)Pixhawk內(nèi)部相對(duì)高度數(shù)據(jù)值偏離超過2 m即判定激光數(shù)據(jù)異常，則自動(dòng)切換到Pixhawk內(nèi)部氣壓計(jì)和GPS信號(hào)融合的定高數(shù)據(jù)。

4.2.2基于激光數(shù)據(jù)的時(shí)域?yàn)V波方法

考慮到在低空區(qū)域更多關(guān)注無人機(jī)集群相對(duì)地面的高度，針對(duì)激光定高數(shù)據(jù)面對(duì)地面變化時(shí)抖動(dòng)大容易引起飛行控制環(huán)路產(chǎn)生過調(diào)節(jié)現(xiàn)象或者震蕩反饋等問題，給出了一種時(shí)域?yàn)V波方法：

式中， HFn為n時(shí)刻待求的無人機(jī)期望高度； HFn?1為n?1時(shí) 刻已知的無人機(jī)期望高度； HLn?1為n?1時(shí)刻的激光雷達(dá)數(shù)據(jù)。S定義為濾波補(bǔ)償項(xiàng)：

4.2.3編隊(duì)飛行安全距離策略

考慮到異構(gòu)無人機(jī)編隊(duì)采用的是主?從結(jié)構(gòu)，且視覺傳感器搭載在主機(jī)上。因此，編隊(duì)采用了一字型方案，主機(jī)在前，從機(jī)跟隨排列，從而提供更可靠和寬闊的視覺輸入信息?？紤]到GPS誤差等因素，設(shè)定單個(gè)飛機(jī)水平安全防碰撞區(qū)域?yàn)?.5 m，機(jī)間水平方向間隔保持5 m。垂直地面方向考慮有激光傳感器，定位精度相對(duì)較高，設(shè)定單個(gè)飛機(jī)垂直安全防碰撞區(qū)域?yàn)? m，考慮到氣流氣壓等因素，機(jī)間垂直方向安全距離為4 m。

4.3 無人機(jī)集群編隊(duì)邏輯

結(jié)合“無人爭(zhēng)鋒”比賽的實(shí)驗(yàn)試飛場(chǎng)地，編隊(duì)穿越飛行測(cè)試流程如下：1)無人機(jī)編隊(duì)利用機(jī)載地面站發(fā)布起飛指令后，一字型編隊(duì)飛行到各個(gè)門框障礙附近并搜索門框目標(biāo)；2)利用主機(jī)的單目相機(jī)和LiDAR激光等傳感器實(shí)現(xiàn)障礙目標(biāo)定位，解算穿越點(diǎn)的空間坐標(biāo)和穿越航向角，并分配各個(gè)無人機(jī)的穿越路線；3)利用地面站軟件將最終航點(diǎn)下發(fā)到其他從機(jī)，無人機(jī)編隊(duì)進(jìn)行穿越障礙并向下一個(gè)障礙區(qū)域編隊(duì)飛行；4)依次完成4個(gè)門框的穿越任務(wù)，返回起飛點(diǎn)完成安全降落。

此外，利用4.2節(jié)中的保護(hù)策略和pixhawk中的故障保護(hù)設(shè)計(jì)，一旦發(fā)生主機(jī)丟失、墜毀等嚴(yán)重事故，從機(jī)將自動(dòng)切換一鍵返航，達(dá)到減小機(jī)群損失的目的。

5 仿真實(shí)驗(yàn)與實(shí)機(jī)編隊(duì)飛行驗(yàn)證

5.1 定高時(shí)域?yàn)V波器仿真

針對(duì)4.2.2節(jié)的濾波器，仿真中對(duì)兩種地形進(jìn)行模擬與分析：1)具有連續(xù)坑洼和樹木的地形，在該類地形條件下，通常希望濾波器能過濾激烈的數(shù)據(jù)抖動(dòng)，保持平穩(wěn)飛行。2)具有臺(tái)階特征的地形，在該類地形條件下，通常希望無人機(jī)能保持和地面的相對(duì)高度，并在臺(tái)階處作平滑上升動(dòng)作，減少激烈的陡然攀升動(dòng)作。

模擬飛行高度相對(duì)于地面為5 m，設(shè)定氣壓計(jì)噪聲為標(biāo)準(zhǔn)差0.20、方差1、均值0的高斯噪聲；激光噪聲為標(biāo)準(zhǔn)差0.01、方差1、均值0的高斯噪聲；在不同阻力因子R下的仿真激光數(shù)據(jù)的濾波輸出結(jié)果如圖4所示。

圖4 定高時(shí)域?yàn)V波器仿真

由圖4a可得到，隨著R的減小，過濾效果越來越明顯，且相比于緩慢變化的氣壓計(jì)定高，利用激光相對(duì)定高數(shù)據(jù)更具穩(wěn)定性。由圖4b可以得，隨著R的減小，通過陡變平臺(tái)的平滑效果越來越明顯，平滑過渡防止了無人機(jī)控制環(huán)路過高的調(diào)節(jié)量，既節(jié)約了能源，其飛行過程也更加安全平穩(wěn)。

5.2 視覺定位仿真

考慮到無人機(jī)飛行測(cè)試成本，首先在室內(nèi)設(shè)計(jì)按比例縮小的門框障礙，利用機(jī)載單目相機(jī)離線測(cè)試視覺定位算法可靠性。相較于室內(nèi)，室外環(huán)境更多地受到光照和成像質(zhì)量的影響。本文在戶外實(shí)驗(yàn)中還原了比賽場(chǎng)地障礙設(shè)置并進(jìn)行了實(shí)際測(cè)量，該門框障礙高2.4 m，寬7.2 m。室內(nèi)和室外的測(cè)試效果如圖5和圖6所示。

圖5 室內(nèi)視覺定位仿真示意

圖6 室外視覺定位仿真示意

因研究背景是無人機(jī)集群穿越障礙，將問題轉(zhuǎn)換為尋找合適的障礙穿越角度，其中定義a ngle角為門框法線方向與門框中點(diǎn)和鏡頭連線的夾角，即該時(shí)刻無人機(jī)應(yīng)該選擇的穿越航向角。分別進(jìn)行的20次室內(nèi)、室外仿真測(cè)試中，穿越角度平均誤差分別為0.85°和1.87°，滿足無人機(jī)集群的穿越條件。

5.3 ROS無人機(jī)編隊(duì)穿越仿真

利用ROS(robot operating system)系統(tǒng)的Rviz仿真工具進(jìn)行了無人機(jī)編隊(duì)飛行邏輯驗(yàn)證。其可視化的特點(diǎn)方便分析飛行邏輯，圖7和圖8給出了編隊(duì)飛行邏輯示意圖和Rviz下的仿真軌跡，紅點(diǎn)為隨機(jī)出現(xiàn)的門框障礙區(qū)域，黃點(diǎn)為主機(jī)，黑點(diǎn)為從機(jī)。

圖7 無人機(jī)集群飛行穿越障礙示意圖

圖8 Rviz仿真無人機(jī)集群飛行穿越障礙

5.4 室外實(shí)機(jī)編隊(duì)與穿越實(shí)驗(yàn)

在完成無人機(jī)的LiDAR激光傳感器調(diào)試、單目相機(jī)與云臺(tái)測(cè)試、集群離線仿真編隊(duì)驗(yàn)證后，旋翼機(jī)編隊(duì)在正式的測(cè)試場(chǎng)地進(jìn)行了完整的穿越障礙飛行測(cè)試。表1為測(cè)試飛行場(chǎng)地中4個(gè)障礙門框區(qū)域(障礙門將在該區(qū)域附近隨機(jī)出現(xiàn))的經(jīng)緯度數(shù)據(jù)以及門框法線相對(duì)于正北順時(shí)針朝向的角度數(shù)據(jù)。

表1 障礙穿越區(qū)域中心坐標(biāo)信息

旋翼機(jī)需要完成對(duì)目標(biāo)障礙進(jìn)行搜索、識(shí)別、編隊(duì)穿越。圖9和圖10展示了編號(hào)不同區(qū)域，主機(jī)獲取的視覺第一視角。圖11和圖12展示了不同位置觀測(cè)到無人機(jī)編隊(duì)穿越門框障礙的瞬間。

圖9 2號(hào)障礙主機(jī)第一視角

圖10 1號(hào)附近主機(jī)第一視角

圖11 1號(hào)障礙平視穿越瞬間

圖12 3號(hào)障礙仰視穿越瞬間

可以看出，2號(hào)門附近地面平坦整潔，1號(hào)門附近樹木坑洼較多。在不同的光照條件、地形條件下，無人機(jī)集群均能穩(wěn)定地識(shí)別、估計(jì)定位并穿越障礙目標(biāo)。

在條件允許的情況下盡可能多的在4個(gè)區(qū)域進(jìn)行了編隊(duì)穿越障礙測(cè)試。通過表2可以看出，由于各個(gè)穿越區(qū)地形特點(diǎn)與測(cè)試難度不同，飛行成功率也存在差異。其中，2號(hào)門區(qū)域地面平坦、環(huán)境簡(jiǎn)單的特點(diǎn)提高了其穿越成功率。1、4號(hào)門地面坑洼，樹木繁多，且該區(qū)域飛行時(shí)測(cè)試時(shí)出現(xiàn)磁羅盤誤差較大，因此穿越成功率有所下降?？傮w來說，在實(shí)機(jī)編隊(duì)穿越測(cè)驗(yàn)中穿越總體成功率達(dá)到77%左右，說明該技術(shù)具備可行性和進(jìn)一步研究的價(jià)值。

表2 給出了比賽場(chǎng)地測(cè)試的穿越次數(shù)、穿越航向角與成功率的相關(guān)實(shí)驗(yàn)數(shù)據(jù)。

6 結(jié)束語

本文完整地構(gòu)建了該小旋翼無人機(jī)編隊(duì)系統(tǒng)的軟硬件一體化平臺(tái)。利用視覺、LiDAR等傳感器實(shí)現(xiàn)了對(duì)無人機(jī)集群的輔助測(cè)高與目標(biāo)障礙的定位與穿越，提出了一種小旋翼無人機(jī)編隊(duì)穿越障礙技術(shù)。針對(duì)復(fù)雜室外環(huán)境穿越障礙這一技術(shù)目標(biāo)，提出基于單目視覺的連續(xù)跟蹤定位算法和基于激光雷達(dá)的時(shí)域?yàn)V波方法，提高了對(duì)待穿越障礙目標(biāo)的定位精度和可靠性，并通過大量仿真測(cè)試和實(shí)機(jī)編隊(duì)飛行實(shí)驗(yàn)驗(yàn)證了該技術(shù)方案的可行性。