基于視覺定位的水下機器人無通信高精度編隊技術研究

楊 翊，周星群，胡志強，范傳智，王志超，傅殿友，鄭 權

（1. 中國科學院沈陽自動化研究所機器人學國家重點實驗室，遼寧 沈陽 110016；2. 中國科學院機器人與智能制造創新研究院，遼寧 沈陽 110169）

0 引言

按照工作方式不同，無人水下機器人（Unmanned Underwater Vehicle，UUV）可分為自主水下機器人（Autonomous Underwater Vehicle，AUV）和遙控水下機器人（Remote Operated Vehicle，ROV）[1]。其中，由于自主水下機器人具有隱蔽性好、可拓展性強等優點，被廣泛應用于軍事[2]、環境和科學考察[3]，以及石油工業和海洋采礦等領域[4]。

近年來，水下機器人集群技術研究及相關應用成為水下機器人技術領域的發展熱點之一[5-6]，可以顯著解決單一水下機器人任務范圍小和工作效率低等局限性，極大地提升和拓展水下作業的能力和范圍[7]。但是，受到水體介質的影響，電磁波等通信方式難以在水下使用，水聲通信基本成為水下唯一的遠距離穩定通信方式。但由于聲波傳播速度比電磁波要低5個數量級，導致信息傳輸速度低，時延嚴重，還有丟包及衰減快等問題[8]。同時，水下機器人對水下環境以及其它水下機器人載體的感知能力也受到水體介質的較大影響。除了常用的海洋溫、鹽、深以及流速傳感器等海洋環境感知設備，側掃聲吶、地層剖面聲吶等海底地質勘探設備，以及用于水下目標探測的各種類型的主被動聲吶設備等，用于水下機器人之間感知的傳感器手段非常有限。

由于受到上述因素的限制，現有的水下機器人編隊均無法達到空中、地面或水面上各類無人系統的編隊隊形精度。以中國科學院沈陽自動化研究所2020年 9月開展的編隊試驗為例，試驗采用了 3臺微小型水下機器人，實現了水下一字形和三角形編隊及隊形切換。試驗中采用領航者編隊算法，基于水聲通信實現水下機器人之間位置與控制信息的交互，編隊間距設定值為 20 m，編隊航速設定值為2 kn，編隊時長7 min。試驗結果表明：編隊平均誤差大于20%，并且發現水聲通信的通信頻率和速率過低，跟不上領航者運動的變化；水聲通信較高的丟包率也導致水下編隊的隊形維持非常困難。為了避免水下機器人之間相撞，編隊的間距難以進一步縮小。對比而言，目前空中無人機集群編隊表演時每臺無人機的位置誤差均可達到厘米級，編隊中無人機之間的間距可小于1 m，且能夠在空中準確組成任意三維圖案并實現不同圖案的連續變化。

針對水下集群編隊的整體技術現狀，本文提出了基于水下機器人之間的近距離視覺定位技術，實現了水下機器人之間的高效、高精度感知，在此基礎上研究不依賴于通信的水下近距離密集編隊技術，編隊的密集程度和精度與傳統方式相比大幅提升，在不大于10 m編隊間距的條件下，實現不大于10%的編隊精度。

1 水下視覺定位技術研究

1.1 視覺定位系統硬件平臺設計

在深水黑暗環境下，使用標志燈作為視覺定位目標是一種經典的方案。然而受到水下機器人的體積限制，標量光源在載體上最大分布距離有限，無法實現遠距離定位，多用于水下機器人對接[9-11]，因此在本文中設計了矢量燈示位方案。矢量激光艙段如圖1所示，將若干矢量標志燈組，設于激光艙段內，激光器穿過載體透射窗向周圍水域環境發射激光，使得在水下機器人載體軸向視角方向構成角度可調的矢量光學圖像。且矢量圖案邊界點可置于機器人之外，極大提高了視覺定位距離。

圖1 矢量光源發射激光光線示意圖Fig.1 Schematic diagram of laser lights emitted by vector light source

為了捕捉其它水下機器人的光線圖案，在水下機器人的艏段設置前視廣角攝像機，用來捕捉水下機器人前方較大范圍內其它水下機器人的光線圖像。在水下機器人的艉段后部設置后視相機，采集水下機器人的后方圖像信息。前視和后視圖像數據均通過網絡傳輸至水下機器人載體內的嵌入式平臺上進行處理，計算其它水下機器人的位姿數據。

1.2 圖像預處理算法

水下機器人的相機獲取如圖2所示的圖像后，需要進行一系列處理，然后求解待定位機器人的位姿。

圖2 不同距離下目標機器人水下原始圖像Fig.2 Original underwater image of the target robot at different distances

為實現精確位姿估計，首先對圖像畸變問題進行處理。完成圖像獲取后首先依據相機標定參數進行圖像校正，保證初始數據的準確性。輸入 RGB圖像進行通道轉換，轉換為HSV圖像并根據色調、飽和度和亮度進行多區間限制，將非藍色噪聲極大的去除。而后進行均值濾波，濾除圖像中可能出現的高亮雜點。而后進行伽馬變換，進一步提高圖像中光線與背景亮度差異。根據閾值進行二值化后得到二值圖像，需要采取腐蝕、膨脹等形態學操作，使矢量燈的模態圖骨骼化，使圖像中光線調整為單像素，不僅有利于后續進行的光線檢測，亦可去除非線性高亮區域對光線檢測造成的影響。

1.3 光線檢測算法

水下實際環境中可能出現圖像模糊、顏色偏移、對比度偏移、鏡面圖像、非均勻照明和噪聲光源等現象，這對視覺定位算法提出了極高的要求。因此，針對視覺定位算法，快速、準確和抗噪是基本需求。

圖像預處理后進行激光光線檢測，使用曲線擬合算法來擬合激光光線，以進行后續位姿計算。相機采集到的原始圖像信息經過形態學處理后，進行激光光線檢測。在嘗試多種基于特征提取的直線檢測算法后發現，實際的水下環境中可能存在多種未知干擾，導致光學標志缺失或出現未知形狀的高亮區域干擾，導致檢測器輸出錯誤的檢測結果。因此，采用隨機采樣一致性算法（Random Sample Consensus，RANSAC）[12]，對圖像中可能存在的直線進行多次擬合。由于RANSAC從一組包含離群值的觀測數據中估算數學模型的參數，因此，可以準確檢測激光光線并且具有很強的魯棒性。光線檢測結果如圖3所示，光線所在位置影響后續的位姿計算，從而使用不同顏色進行標記。

圖3 光線檢測結果Fig.3 Light detection result

多臺機器人編隊過程中必然出現多條激光光線互相干擾的情況，為將不同載體的光線進行區分，采用基于光線亮度進行識別的方式，僅檢測距離最近載體所發出光線，從而進行光線檢測。

1.4 位姿估計算法

根據每條光線所在圖像位置與空間中實際光線之間的映射關系，兩兩計算光線交點坐標。計算得到光線交點像素坐標后，結合空間中光線交點實際三維空間坐標與相機標定獲得的相機內參，使用PNP（Perspective-n-Point）算法計算目標水下機器人的相對水下機器人自身相機的位置和姿態。

位姿估計采用3種坐標系，如圖4所示：圖像坐標系、相機坐標系和世界坐標系。圖像坐標系是建立在圖像平面上的一個以像素為單位的二維坐標系。相機坐標系和世界坐標系都是以毫米為單位的三維坐標系。相機坐標的原點位于攝像機的光學中心上。世界坐標系的原點位于激光器的中心點。

圖4 位姿估計算法中坐標系示意圖Fig.4 Schematic diagram of the coordinate system in the pose estimation algorithm

由于攝像機是剛性固定在水下機器人的頭部上，只考慮剛體運動，因此計算水下機器人與目標水下機器人之間的位姿等價于確定攝像機坐標系與世界坐標系之間的變換。以針孔攝像機為例，計算圖像坐標與攝像機坐標之間的變換。

式中：(xr, yr, zr)為世界坐標；(R, T)分別為旋轉、平移矩陣，即為所求位姿信息；(R, T)的求解利用二維圖像點與三維坐標之間的4個對應點，這里采用了PNP算法，不做贅述。

2 水下視覺定位技術試驗驗證

2.1 實驗測試平臺

為了對所設計的水下視覺定位系統及后續編隊控制系統性能，開發了一個如圖5所示的微型水下機器人。水下機器人艏段安裝了高清相機，可采集RGB信息，幀率為65.2幀/秒。艏段除設置相機外，配置了NVIDIA JETSON TX2處理平臺，以進行實時圖像處理。矢量光源艙段發射激光光線以示意其他水下機器人自身所在位置。艉段相機采集水下機器人后方區域圖像信息以輔助進行水下機器人編隊。

圖5 水下機器人整體結構圖Fig. 5 Overall structure of the UUS

2.2 實驗測試數據

2.2.1 靜態目標定位精度測試實驗

實驗中將2臺水下機器人固定于支架上，在水池進行圖像數據采集。為降低圖像中激光光線區分難度，調整一側激光發射角度，避免左右對稱，并從多個視角（–45°，0°，45°）進行數據采集，如圖6所示。

圖6 靜態目標實驗場景Fig.6 Experiment scene of static target

使用上述算法進行了目標水下機器人位姿計算。由于水下機器人運動過程中實際位姿無法獲取，因此在靜態環境下采用實時獲取視頻信息并進行圖像處理與位姿計算，對實驗結果取均值后與實際測量值進行對比。根據采集得到的數據進行了視覺定位算法的精度測試，計算結果如表1所示。

表1 視覺定位精度數據Tab. 1 Visual positioning accuracy data

由表中數據可知，相對距離誤差平均值為2.6%。

2.2.2 動態目標定位實驗

動態目標定位實驗中，1臺水下機器人觀察另一臺水下機器人由近及遠運動，記錄定位數據。目標相對距離數據以及目標相對三維坐標如圖7所示。由圖中曲線可知，目標位置變化曲線符合實際運動情況，能夠實現不小于2 Hz的持續定位數據輸出，滿足載體航行自主航行控制的數據量要求。實驗中極限定位距離為 12 m，滿足水下機器人密集編隊需求，使用更高功率的激光器可實現更遠距離的定位距離。

圖7 目標絕對距離變化曲線圖Fig. 7 Curve of absolute target distance

3 水下編隊技術研究

水下機器人密集編隊由大于3臺的任意數量水下機器人組成，每臺水下機器人設置有位于載體艏段和艉段用于實現視覺定位的高清攝像頭以及位于載體上的矢量及標量標志燈組，每個水下機器人通過視覺捕捉其他水下機器人矢量及標量標志燈組構成的圖像進行位姿計算，并按照當前編隊任務中的隊形需求控制自身的姿態、航向及速度，以完成多水下機器人的水下集群密集編隊。具體流程如圖8所示。

圖8 編隊控制系統整體工作流程圖Fig. 8 Overall work flow chart of the formation control system

3.1 領航–跟隨模式幾何構型

水下機器人編隊控制算法采用領航者算法。圖9展示了在笛卡爾坐標系內領航者與跟隨者之間的幾何構型。圖中：L代表領航者；F代表跟隨者；Fd代表跟隨者的期望位置；lLF為領航者和跟隨者的歐氏距離，φLF為領航者和跟隨者的連線與大地坐標系X軸正方向的夾角，二者的期望值為

圖9 領航者與跟隨者的幾何構型Fig. 9 Geometric configuration of the leader and follower

將跟隨者實際位置F與期望位置Fd的誤差轉換到領航者機體坐標系中得到1(, )Txy E=e e ，其中：

領航者與跟隨者的航向角偏差eα定義為

3.2 跟隨者水下機器人運動學控制器

求解方法如圖10所示。跟隨者的期望朝向由實際位置F指向期望位置Fd。

圖10 期望航向角計算方法示意圖Fig. 10 Schematic diagram of calculation method of expected heading angle

當跟隨者到達期望點時其朝向角與領航者一致，即eα= 0 。期望航向角的計算公式如下，其中b∈R+。

3.3 編隊控制系統工作模式

水下機器人根據獲得目標水下機器人的位姿信息結合編隊任務中設定的水下機器人所在空間位置，計算水下機器人自身與目標位置之間的相對距離與旋轉角度，進而控制載體運動至目標位置。若在某一時間或任務節點后，水下機器人編隊隊形任務發生變化，則水下機器人以新任務重新計算目標位置，完成隊形變換任務。編隊控制系統啟動時，執行以下步驟。

1）領航者和跟隨者的控制器均輸出指令控制矢量標志燈組、標量標志燈組的開啟，控制水下攝像機拍攝圖像；

2）領航者執行區域檢測優先級步驟拍攝圖像，進行圖像處理，判斷跟隨者是否跟隨，若長時間未發現跟隨者或跟隨者距離大于設定值時減速行駛；

3）跟隨者執行區域檢測優先級步驟拍攝圖像，進行圖像處理，定位集群內領航者和其他成員位置，計算其在當前集群內的移動控制參數。

4 水下編隊實驗驗證

4.1 實驗條件及配置情況

2021年4月，基于3臺“探索MINI”水下機器人開展了集群編隊實驗，如圖11所示。其中01號載體為領航者，搭載矢量光源艙段。02號載體和03號載體搭載矢量光源艙段和視覺艙段。02號載體為01號載體的跟隨者，同時是03號載體的領航者。所有載體均不搭載水聲通信設備，因此在整個水下編隊航行過程中水下機器人之間沒有通信，僅靠視覺定位數據作為2臺跟隨者水下機器人的航行控制輸入。

圖11 試驗環境及參試設備Fig. 11 Experiment environment and equipment

實驗在100 m×20 m×20 m試驗水池進行。3臺水下機器人采用一字隊形，實驗編隊隊形以及航行任務軌跡如圖12所示。在直線航行過程中啟動編隊控制模式，2臺跟隨者水下機器人各自跟隨其領航者水下機器人運動；轉彎過程中啟動任務航行模式，所有水下機器人自主航行。

圖12 編隊隊形以及航行任務軌跡Fig. 12 Formation and navigation mission trajectory

4.2 實驗結果

3臺水下機器人的編隊航行水池實驗實際效果如圖13所示，水下機器人的定位間距設置為3.5 m（對應的載體艏艉間距為2 m）。在一個控制周期內，跟隨者水下機器人基于視覺定位結果結合領航者編隊算法，發送速度及航向角控制指令至水下機器人主控進而執行命令。

圖13 水池編隊實驗航行效果Fig. 13 Sailing effect of formation experiment in the pool

02號與01號水下機器人的相對距離變化值如圖14所示。03號與02號水下機器人的相對距離變化值實驗數據如圖15所示。

圖14 02號相對01號距離變化曲線Fig. 14 Variation curve of distance between No. 02 and No. 01

圖15 03號相對02號距離變化曲線Fig. 15 Variation curve of distance between No. 03 and No. 02

由于水下機器人編隊過程中出發階段與轉彎階段相對位置與相對角度快速變化，編隊中每個水下機器人的相對距離存在大幅變化，因此在計算編隊保持誤差時以連續3次視覺定位距離與編隊控制設定值誤差小于0.5 m時開始進行數據統計，如圖14、圖15中箭頭所指示區域。

02號和03號水下機器人編隊實驗數據如表2、表3所示，02號載體的編隊誤差為7.4%，03號載體的編隊誤差為 9.2%，整體編隊誤差小于 10%。03號編隊誤差較大的原因是由于視野內存在多個光源，且跟蹤過程會受到02號水下機器人的跟蹤過程運動調整影響。

表2 02號水下機器人編隊數據Table 2 No. 02 underwater robot formation data

表3 03號水下機器人編隊數據Table 3 No. 03 underwater robot formation data

5 結束語

本文針對目前水下機器人集群研究中存在的通信能力與感知手段受限，編隊精度較低等問題，提出了基于水下視覺定位且不依賴于通信的適用于密集編隊的集群編隊方法。通過實驗可知，視覺定位能夠達到不大于3%的定位精度，且能夠實現不小于2 Hz的定位頻率。與以往基于通信的水下編隊相比，基于視覺定位的3臺水下機器人能夠在間距10 m以內更小的尺度上形成密集編隊，編隊精度也有較大提升，達到了小于10%的隊形保持精度，與基于通信的水下編隊相比，編隊精度提高1倍以上。

然而，本文開展的實驗工作均是在水池環境下開展的，受到水池長度限制，無法通過長時間編隊航行來驗證編隊的真實精度。水下機器人航行至水池一端時，受限于水池的寬度，無法以編隊狀態回轉，導致已經組成的編隊解散，所以水下機器人完成回轉后需要航行一段距離才能再次組成編隊。為了進一步驗證和改進視覺定位和集群編隊的相關算法和控制參數，下一步將繼續開展湖上和海上試驗，最終實現在深海條件下開展基于視覺定位的編隊實驗。