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

楊 翊，周星群，胡志強，范傳智，王志超，傅殿友，鄭 權(quán)

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

0 引言

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

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

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

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

1 水下視覺定位技術(shù)研究

1.1 視覺定位系統(tǒng)硬件平臺設(shè)計

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

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

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

1.2 圖像預(yù)處理算法

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

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

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

1.3 光線檢測算法

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

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

圖3 光線檢測結(jié)果Fig.3 Light detection result

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

1.4 位姿估計算法

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

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

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

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

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

2 水下視覺定位技術(shù)試驗驗證

2.1 實驗測試平臺

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

圖5 水下機器人整體結(jié)構(gòu)圖Fig. 5 Overall structure of the UUS

2.2 實驗測試數(shù)據(jù)

2.2.1 靜態(tài)目標(biāo)定位精度測試實驗

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

圖6 靜態(tài)目標(biāo)實驗場景Fig.6 Experiment scene of static target

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

表1 視覺定位精度數(shù)據(jù)Tab. 1 Visual positioning accuracy data

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

2.2.2 動態(tài)目標(biāo)定位實驗

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

圖7 目標(biāo)絕對距離變化曲線圖Fig. 7 Curve of absolute target distance

3 水下編隊技術(shù)研究

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

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

3.1 領(lǐng)航–跟隨模式幾何構(gòu)型

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

圖9 領(lǐng)航者與跟隨者的幾何構(gòu)型Fig. 9 Geometric configuration of the leader and follower

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

領(lǐng)航者與跟隨者的航向角偏差eα定義為

3.2 跟隨者水下機器人運動學(xué)控制器

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

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

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

3.3 編隊控制系統(tǒng)工作模式

水下機器人根據(jù)獲得目標(biāo)水下機器人的位姿信息結(jié)合編隊任務(wù)中設(shè)定的水下機器人所在空間位置，計算水下機器人自身與目標(biāo)位置之間的相對距離與旋轉(zhuǎn)角度，進(jìn)而控制載體運動至目標(biāo)位置。若在某一時間或任務(wù)節(jié)點后，水下機器人編隊隊形任務(wù)發(fā)生變化，則水下機器人以新任務(wù)重新計算目標(biāo)位置，完成隊形變換任務(wù)。編隊控制系統(tǒng)啟動時，執(zhí)行以下步驟。

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

2）領(lǐng)航者執(zhí)行區(qū)域檢測優(yōu)先級步驟拍攝圖像，進(jìn)行圖像處理，判斷跟隨者是否跟隨，若長時間未發(fā)現(xiàn)跟隨者或跟隨者距離大于設(shè)定值時減速行駛；

3）跟隨者執(zhí)行區(qū)域檢測優(yōu)先級步驟拍攝圖像，進(jìn)行圖像處理，定位集群內(nèi)領(lǐng)航者和其他成員位置，計算其在當(dāng)前集群內(nèi)的移動控制參數(shù)。

4 水下編隊實驗驗證

4.1 實驗條件及配置情況

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

圖11 試驗環(huán)境及參試設(shè)備Fig. 11 Experiment environment and equipment

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

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

4.2 實驗結(jié)果

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

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

02號與01號水下機器人的相對距離變化值如圖14所示。03號與02號水下機器人的相對距離變化值實驗數(shù)據(jù)如圖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

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

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

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

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

5 結(jié)束語

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

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