基于視覺輔助導航的小旋翼機群編隊穿越障礙技術

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

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

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

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

該研究內容參加了2018年由空軍裝備部舉辦的首屆“無人爭鋒”智能無人機集群系統挑戰賽科目1(SC-1)：密集編隊穿越競速賽。主要研究貢獻如下：

1)完成異構無人機編隊穿越仿真驗證，并構建了一套無人機集群編隊飛行實機軟硬件測試平臺；

2)設計一種基于單目視覺輔助的目標相對定位方法，實現了無人機編隊在線實時穿越與路徑規劃；

3)設計一套多傳感器融合的安全飛行策略，并設計一種基于激光定高數據的時域濾波器，為無人機編隊在低空復雜環境中的飛行定高提供保障。

1 無人機編隊穿越技術總體框架

總體框架研究將小旋翼無人機編隊穿越系統分為硬件、軟件和算法3個層面，如圖1所示。

圖1 無人機編隊穿越障礙技術框圖

以此技術框架為基礎，進一步研究基于視覺輔助的小旋翼無人機編隊穿越障礙技術，以解決主?從式異構旋翼機編隊飛行任務中的避障穿越、路徑規劃、安全飛行策略等技術問題。

2 無人機編隊硬件平臺構建

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

圖2 試飛階段實機展示圖

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

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

3 編隊飛行軟件設計

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

4 編隊避障穿越算法設計

4.1 視覺識別與輔助定位算法

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

圖3 視覺識別與避障算法流程圖

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

根據對極幾何理論可以知，設x1,x2為p1,p2特征點歸一化坐標，它們滿足：

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

4.2 編隊飛行與控制策略

4.2.1多傳感器定高保護算法

一方面，考慮到氣壓計和GPS信號融合獲得的高度信息通常存在0.3～0.8 m的誤差，因此采用激光雷達測高作為輔助手段。另一方面，為了防止激光定高數據抖動或者故障時旋翼機出現沖高等危險動作，采用激光、氣壓計、GPS信號同時對高度進行估計：

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

2)在12 m以上：加入Pixhawk內部融合的氣壓計和GPS信息作為保護，如果檢測到激光定高數據相對Pixhawk內部相對高度數據值偏離超過2 m即判定激光數據異常，則自動切換到Pixhawk內部氣壓計和GPS信號融合的定高數據。

4.2.2基于激光數據的時域濾波方法

考慮到在低空區域更多關注無人機集群相對地面的高度，針對激光定高數據面對地面變化時抖動大容易引起飛行控制環路產生過調節現象或者震蕩反饋等問題，給出了一種時域濾波方法：

式中， HFn為n時刻待求的無人機期望高度； HFn?1為n?1時 刻已知的無人機期望高度； HLn?1為n?1時刻的激光雷達數據。S定義為濾波補償項：

4.2.3編隊飛行安全距離策略

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

4.3 無人機集群編隊邏輯

結合“無人爭鋒”比賽的實驗試飛場地，編隊穿越飛行測試流程如下：1)無人機編隊利用機載地面站發布起飛指令后，一字型編隊飛行到各個門框障礙附近并搜索門框目標；2)利用主機的單目相機和LiDAR激光等傳感器實現障礙目標定位，解算穿越點的空間坐標和穿越航向角，并分配各個無人機的穿越路線；3)利用地面站軟件將最終航點下發到其他從機，無人機編隊進行穿越障礙并向下一個障礙區域編隊飛行；4)依次完成4個門框的穿越任務，返回起飛點完成安全降落。

此外，利用4.2節中的保護策略和pixhawk中的故障保護設計，一旦發生主機丟失、墜毀等嚴重事故，從機將自動切換一鍵返航，達到減小機群損失的目的。

5 仿真實驗與實機編隊飛行驗證

5.1 定高時域濾波器仿真

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

模擬飛行高度相對于地面為5 m，設定氣壓計噪聲為標準差0.20、方差1、均值0的高斯噪聲；激光噪聲為標準差0.01、方差1、均值0的高斯噪聲；在不同阻力因子R下的仿真激光數據的濾波輸出結果如圖4所示。

圖4 定高時域濾波器仿真

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

5.2 視覺定位仿真

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

圖5 室內視覺定位仿真示意

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

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

5.3 ROS無人機編隊穿越仿真

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

圖7 無人機集群飛行穿越障礙示意圖

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

5.4 室外實機編隊與穿越實驗

在完成無人機的LiDAR激光傳感器調試、單目相機與云臺測試、集群離線仿真編隊驗證后，旋翼機編隊在正式的測試場地進行了完整的穿越障礙飛行測試。表1為測試飛行場地中4個障礙門框區域(障礙門將在該區域附近隨機出現)的經緯度數據以及門框法線相對于正北順時針朝向的角度數據。

表1 障礙穿越區域中心坐標信息

旋翼機需要完成對目標障礙進行搜索、識別、編隊穿越。圖9和圖10展示了編號不同區域，主機獲取的視覺第一視角。圖11和圖12展示了不同位置觀測到無人機編隊穿越門框障礙的瞬間。

圖9 2號障礙主機第一視角

圖10 1號附近主機第一視角

圖11 1號障礙平視穿越瞬間

圖12 3號障礙仰視穿越瞬間

可以看出，2號門附近地面平坦整潔，1號門附近樹木坑洼較多。在不同的光照條件、地形條件下，無人機集群均能穩定地識別、估計定位并穿越障礙目標。

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

表2 給出了比賽場地測試的穿越次數、穿越航向角與成功率的相關實驗數據。

6 結束語

本文完整地構建了該小旋翼無人機編隊系統的軟硬件一體化平臺。利用視覺、LiDAR等傳感器實現了對無人機集群的輔助測高與目標障礙的定位與穿越，提出了一種小旋翼無人機編隊穿越障礙技術。針對復雜室外環境穿越障礙這一技術目標，提出基于單目視覺的連續跟蹤定位算法和基于激光雷達的時域濾波方法，提高了對待穿越障礙目標的定位精度和可靠性，并通過大量仿真測試和實機編隊飛行實驗驗證了該技術方案的可行性。