韓澤凱 ,朱興華 ,韓曉軍 ,孫 凱 ,劉肖宇

(1.中國科學(xué)院沈陽自動化研究所 機器人學(xué)國家重點實驗室,遼寧 沈陽,110016;2.中國科學(xué)院 機器人與智能制造創(chuàng)新研究院,遼寧 沈陽,110169;3.中國科學(xué)院大學(xué),北京,100049)

0 引言

水下無人系統(tǒng)包括各類能夠執(zhí)行水下任務(wù)的無人水下航行器(unmanned undersea vehicle,UUV),其中自主水下航行器(autonomous undersea vehicle,AUV)因具有在隱蔽條件下自主執(zhí)行戰(zhàn)術(shù)任務(wù)的能力[1],已經(jīng)成為各國海軍裝備的重要研究方向。隨著新材料、新能源、人工智能等技術(shù)的不斷進步,大深度、遠航程、大載荷、自主回收成為 AUV 的發(fā)展趨勢。水下自主對接回收技術(shù)作為水下無人系統(tǒng)的關(guān)鍵技術(shù),能夠給予AUV 水下自主能源與信息傳輸能力,實現(xiàn)深遠海長期連續(xù)作業(yè)。

近年來,利用聲學(xué)信標進行遠距離初步導(dǎo)引,光學(xué)信標進行近距離精確導(dǎo)引的聲光聯(lián)合導(dǎo)引方法逐漸成為AUV 回收導(dǎo)引的主流方案[2]。Park 等[3]利用黑白相機捕捉五燈光陣列光源,采用傳統(tǒng)的分割與形狀處理方法成功進行了水池中15 m 距離位置姿態(tài)識別,但受限于光源的散射,沒有很好地提取出燈光中心,誤差較大。Palomeras 等[4]結(jié)合主被動信標,使用增強現(xiàn)實(augmented reality,AR)標記補充近距離視覺盲區(qū),在湖水中完成了對接實驗。Liu 等[5]提出檢測、分割和位姿估計的識別框架,將YOLO(you only look once)網(wǎng)絡(luò)應(yīng)用于水下基站識別,成功在湖水環(huán)境完成10 m 距離內(nèi)的光學(xué)導(dǎo)引,但該算法會在神經(jīng)網(wǎng)絡(luò)檢測丟失的情況下失效。Lin 等[6]利用單光源實現(xiàn)2 自由度定位,結(jié)合視線導(dǎo)引(line-of-sight,LOS)方案完成了湖上20～30 m 距離的光學(xué)導(dǎo)引。張偉等[7]提出視頻跟蹤算法來解決信標丟失問題,充分利用了視頻序列的一致性,但其信標布置與回收方式限制了算法的適用場景與工作距離。

聲光聯(lián)合導(dǎo)引方法在水池環(huán)境下已得到充分驗證,但實際環(huán)境下受擾流、地形等因素干擾,進入光學(xué)導(dǎo)引前,聲學(xué)導(dǎo)引給出的初始位置具有一定偏差,如果不能進入光學(xué)有效范圍且觀測到足夠多的信標,就需要耗費更多能量重新進行初步導(dǎo)引。因此如何提升光學(xué)導(dǎo)引對初始位置的魯棒性成為了對接成功的關(guān)鍵,如果光學(xué)導(dǎo)引的有效作業(yè)空間能夠包容聲學(xué)導(dǎo)引的末端誤差空間,將大大提升回收成功率。針對此,文中通過算法與光源陣列設(shè)計優(yōu)化來提升光學(xué)導(dǎo)引作用范圍。主要創(chuàng)新點在于: 充分利用傳統(tǒng)方法與神經(jīng)網(wǎng)絡(luò)方法優(yōu)勢,建立了改進的基于神經(jīng)網(wǎng)絡(luò)的檢測-跟蹤算法框架;提出長度裕量、角度裕量、工作空間度量指標與分析方法;利用水池與湖上的對接實驗驗證了檢測、跟蹤和位姿估計全流程算法的有效性。

1 AUV 回收裝置

AUV 對接基站主要由回收籠和電子艙構(gòu)成,基站整體為長2 m,寬1.2 m,高1.2 m 的長方體,如圖1 所示。回收籠內(nèi)置于基站,籠口末端安裝電磁鐵用于固定對接后的AUV?；緦?dǎo)引設(shè)備包括超短基線(ultra-short baseline,USBL)以及光學(xué)信標陣列。

圖1 對接基站Fig.1 Docking station

實驗中使用的便攜型AUV(見圖2),長2 m,直徑240 mm,最大航速5 kn,質(zhì)量80 kg。AUV 裝備慣性測量單元、USBL、NVIDIA Jetson TX2 計算板卡及Nano Sea Cam 單目彩色水下相機。水下相機采集基站的燈光導(dǎo)引信標圖片通過Jetson TX2 進行識別與位姿估計。

圖2 便攜型AUVFig.2 Portable AUV

回收過程中水下視覺導(dǎo)引采用環(huán)形作為信標燈陣分布結(jié)構(gòu),考慮到水體對光的吸收,采用傳輸損耗較小的450 nm 波長藍色發(fā)光二極管(light emitting diode,LED)光源,經(jīng)過水池測試,導(dǎo)引信標束散角為44°。信標燈陣均勻安裝于回收籠四周,呈正六邊形分布,邊長為60 cm。

2 視覺導(dǎo)引算法

為了解決信標缺失、傳統(tǒng)方法識別率低等問題,提出基于神經(jīng)網(wǎng)絡(luò)的改進的檢測-跟蹤算法框架,結(jié)合神經(jīng)網(wǎng)絡(luò)檢測準確率高與傳統(tǒng)分割算法效率高的優(yōu)勢,利用重投影機制引入信標分布結(jié)構(gòu)信息,實現(xiàn)實時檢測跟蹤與神經(jīng)網(wǎng)絡(luò)檢測并行。

2.1 視覺導(dǎo)引算法框架

視覺導(dǎo)引算法主要由神經(jīng)網(wǎng)絡(luò)目標檢測、基于神經(jīng)網(wǎng)絡(luò)檢測結(jié)果的目標區(qū)域分割、匈牙利匹配、PnP-P3P 位姿估計、卡爾曼濾波以及重投影模塊構(gòu)成,算法框架如圖3 所示。通過相機采集RGB(red-green-blue)圖像進行基于神經(jīng)網(wǎng)絡(luò)的目標檢測,隨后對燈陣進行光源提取與跟蹤,最后利用PnP-P3P 算法獲取AUV 相機相對于基站的位置姿態(tài)信息。

圖3 檢測-跟蹤算法框架Fig.3 Framework of detecting-tracking algorithm

圖中:D=(u,v,w,h)∈Scamera;L=(u,v)∈Scamera;Q=(x,y,z,yaw,pitch,roll);上標n代表第n幀圖像的數(shù)據(jù)。

算法具體步驟如下:

1)彩色相機的RGB 圖像作為輸入,利用卷積神經(jīng)網(wǎng)絡(luò)對單張圖片進行檢測識別,得到圖像是否存在基站的置信度以及基站在圖像中的位置與大小D(n);

7)輸出具有時空信息的位姿Q(n)。

2.2 基于YOLOv5 的水下基站目標檢測

目標檢測是指從圖像中識別目標是否存在并獲取相應(yīng)位置與大小的方法。水下復(fù)雜環(huán)境引起的圖像退化是導(dǎo)致水下目標檢測準確率低的主要原因,表現(xiàn)為顏色失真、圖像模糊和霧化[8-9]。為了提高檢測的成功率,文中引入YOLOv5[10]神經(jīng)網(wǎng)絡(luò)目標檢測算法和高性能計算平臺來完成目標檢測任務(wù)。與人工特征提取方法相比,神經(jīng)網(wǎng)絡(luò)方法通過學(xué)習(xí)水下真實圖像的特征分布,可以有效從退化的圖像中識別水下目標。

YOLOv5 將檢測目標直接看成一個回歸問題來處理,在處理速度上占據(jù)優(yōu)勢的同時保持了良好的檢測精度。首先將圖片分成G×G個方格,隨后在每個方格中預(yù)測B個不同尺寸大小的目標,獲得G×G×B個相應(yīng)的置信度、位置和大小,最后通過非極大值抑制去除置信度較小的目標至預(yù)設(shè)的理想目標數(shù)量,從而獲得最優(yōu)目標。

目標檢測的輸出參與下一步光源提取與跟蹤的輸入,同時可以矯正跟蹤循環(huán)中重投影的結(jié)果。

2.3 光源提取與跟蹤

光源提取是指從原始圖像中提取出光源的圖像坐標。光源跟蹤是指通過匹配上一幀的預(yù)測坐標與當前幀的光源坐標來跟蹤光源的序號等信息。與傳統(tǒng)全局提取與跟蹤算法不同,由于引入了神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu),文中提出了基于目標區(qū)域的分割策略,充分利用了神經(jīng)網(wǎng)絡(luò)的檢測結(jié)果。在跟蹤模塊上,通過重投影機制引入了結(jié)構(gòu)信息。

所提出的光源提取跟蹤算法在軌跡初始化后能夠獨立于神經(jīng)網(wǎng)絡(luò)檢測模塊運行,運行速度不受神經(jīng)網(wǎng)絡(luò)限制,算法框架不受神經(jīng)網(wǎng)絡(luò)檢測失效的影響,增強了系統(tǒng)魯棒性。

1)基于目標區(qū)域的分割

現(xiàn)有的光源分割方法在全局進行二值化分割，需要考慮到背景光、水面反射和后向散射等干擾因素[11]。而針對特定場景優(yōu)化的算法對圖像分布有一定要求，不能夠解決多種環(huán)境干擾。

文中利用檢測階段或者預(yù)測給出的目標區(qū)域進行分割，直觀上去除了大部分非目標背景干擾，簡單有效，相比于直方圖和頻域濾波等方法，更有利于保留目標特征。在更新目標區(qū)域時，將神經(jīng)網(wǎng)絡(luò)目標檢測結(jié)果與重投影結(jié)果融合。

對于目標區(qū)域D，第n幀目標區(qū)域D的值為：有目標檢測信息，取n幀檢測D(n)；無目標檢測信息，取n-1 幀預(yù)測(n+1)。

光源提取的結(jié)果是不帶有序號匹配信息的各光源中心在圖像中的2D 坐標，然而位姿估計算法需要將2D 坐標與3D 坐標匹配，因此需要進一步進行排序與跟蹤。

2)改進的檢測-跟蹤算法

SORT(simple online and realtime tracking)作為主流多目標跟蹤算法[12],主要采用檢測-跟蹤策略。但SORT 算法存在以下問題: 將第1 階段檢測結(jié)果進行匹配跟蹤,較為依賴直接檢測結(jié)果,在檢測失效時將在當前幀失去跟蹤目標;將每個信標視為獨立目標,由于多目標跟蹤直接應(yīng)用于信標會丟失整體結(jié)構(gòu)信息,因此在信標數(shù)量發(fā)生變化時將導(dǎo)致匹配失效。

通過重投影機制將位姿估計的空間信息加入算法框架,利用上一幀目標的卡爾曼濾波預(yù)測位置信息來進行匹配,由于信標排列被視為一個整體基站,因此局部的丟失不受影響,能夠適應(yīng)信標的缺失和數(shù)量變化。在系統(tǒng)初始化后,跟蹤模塊可以與檢測模塊并行實現(xiàn)實時跟蹤,獲得用于位姿估計的2D-3D 點對。

2.4 PnP-P3P 位姿估計

主流的PnP 算法包括EPnP[13]、RPnP[14]等,算法在n≥4 時具有唯一解。n=3 時,退化為P3P 問題,具有4 個解。在對接過程中,隨著AUV 接近基站,部分光源進入相機盲區(qū),n逐漸減少。提出的PnP-P3P 算法將傳統(tǒng)單幀圖像的姿態(tài)估計轉(zhuǎn)化為跟蹤解序列問題。考慮到對接過程中視頻序列的一致性,因此當可觀測信標數(shù)量降低到3 個時,可以直觀地從前一幀的解中獲取信息。

PnP 問題是通過匹配n對真實世界3D 坐標在視覺平面的2D 投影,還原出相機坐標與真實世界坐標的位置和姿態(tài)的算法。對于L=(u,v)∈Scamera,R=(r1,r2,r3),T=(t1,t2,t3),Lworld=(xr,yr,zr)∈Sworld,則有

式中:λ為焦距;K為相機內(nèi)參矩陣;Xc為相機平面3D 坐標;Xr為世界坐標。相機內(nèi)參矩陣被事先標定,通過L與Lworld求解Q中的6 個未知數(shù)。

當點對的數(shù)量逐漸降低至3 時,采用最小距離原則從P3P 的4 個解中找出最接近當前位置的解序列,實現(xiàn)連續(xù)的位姿估計。

3 實驗結(jié)果

3.1 神經(jīng)網(wǎng)絡(luò)檢測與訓(xùn)練

1)檢測數(shù)據(jù)集

水下基站訓(xùn)練數(shù)據(jù)集包括20 515 張在水池與湖水環(huán)境中采集的真實水下退化圖像,包括模糊、偏移和環(huán)境光等因素的影響。每張水下基站圖片標注8 個信標燈的位置。

水下基站測試集包括在水池與湖上采集的5 組連續(xù)對接視頻,其中有1 組視頻僅包含背景,共有2 382 幀圖像、542 幀背景圖像。

2)實驗環(huán)境

系統(tǒng)環(huán)境為Ubuntu20.04,深度學(xué)習(xí)框架為Pytorch1.11.0、CUDA11.3 和cuDNN8.2.1,GPU 為NVIDIA RTX A6000,顯存48 GB,CPU 為2x Intel Xeon Gold 6234,線程數(shù)32。

3)網(wǎng)絡(luò)訓(xùn)練

通過水下基站訓(xùn)練數(shù)據(jù)集對網(wǎng)絡(luò)進行訓(xùn)練,相關(guān)參數(shù)如表1 和表2 所示,訓(xùn)練損失函數(shù)如圖4 所示。在驗證集上,YOLOv5 的平均精度均值(mean average precision,mAP)達99.42%。

表1 水下基站數(shù)據(jù)集參數(shù)Table 1 Dataset parameters of underwater station

表2 訓(xùn)練參數(shù)Table 2 Parameters of training

圖4 YOLOv5 訓(xùn)練過程軌跡Fig.4 Training process trajectory of YOLOv5

圖5 YOLOv5 檢測結(jié)果Fig.5 Detection results of YOLOv5

3.2 光源提取與跟蹤

當全局圖像存在背景光、前景噪聲、信標丟失等干擾,傳統(tǒng)方法無法有效提取與跟蹤光源。針對以上干擾,對水下基站測試集添加了高斯斑點、遮擋等噪聲,以測試提出的算法效果。

如圖6 所示,傳統(tǒng)分割算法在背景光與前景干擾下無法有效提取光源,提出的基于目標區(qū)域的分割充分利用了神經(jīng)網(wǎng)絡(luò)的檢測結(jié)果,能夠有效去除水面?zhèn)斡啊⒈尘肮飧蓴_以及前景高斯點狀光源噪聲。

圖6 基于目標區(qū)域的圖像分割結(jié)果Fig.6 Image segmentation results based on region

傳統(tǒng)SORT 跟蹤算法在目標受到遮擋時會丟失信標匹配信息,對于再次進入視場的光源檢測效果較差,容易誤匹配或丟失目標ID。然而,在對接過程中,大偏移量導(dǎo)致的光源超出視界等情況十分常見。對于跟蹤任務(wù),將水下基站測試集進行遮擋處理,采用多目標跟蹤(multi-objective tracking,MOT)指標對傳統(tǒng)SORT 與文中方法進行比較,在原測試集與遮擋測試集中的指標如表3 所示。

表3 多目標跟蹤性能指標Table 3 Multi-objective tracking performance indexes

如圖7 所示,在遮擋過程后,SORT 算法失去原序列信息,導(dǎo)致后續(xù)的匹配全部錯誤。而重投影跟蹤能夠在剩下3 個燈的情況下利用結(jié)構(gòu)信息估計出其他信標的位置。

圖7 不同算法在遮擋數(shù)據(jù)集上的跟蹤過程Fig.7 Tracking process of different algorithms on obscured datasets

3.3 PnP-P3P 定位

在一次真實情況下的大偏移量航行中,光源識別跟蹤結(jié)果如圖8 所示。AUV 從籠口左側(cè)經(jīng)過,過程中基站有效觀測信標燈從6 個逐漸遞減為1 個。一次對接過程XY-Z軌跡如圖9 所示,在僅觀測到3 個信標燈時,仍然能夠有效完成六自由度位姿估計。在切換PnP-P3P 時曲線出現(xiàn)一定偏移,隨后通過目標誤差函數(shù)最小原則獲得最優(yōu)解。

圖8 PnP-P3P 算法跟蹤結(jié)果Fig.8 Tracking results of PnP-P3P algorithm

圖9 對接過程XY-Z曲線Fig.9 XY-Zcurves in docking process

3.4 工作空間仿真

針對光學(xué)導(dǎo)引算法指標,文中首次提出利用水下有效導(dǎo)引空間仿真進行度量。通過PnP-P3P 算法,結(jié)合信標燈分布進行仿真。采取左右對稱均勻燈陣分布,表4 列出了當信標燈數(shù)量為n時,相對于基站直徑允許的最大長度觀測裕量?？梢钥闯?6 個燈的分布在具有一定觀測裕量優(yōu)勢的同時,保持了較少的光源數(shù)量,因此能夠在同樣的燈光條件與直徑下,允許更大的單光源光功率,擴大了燈光導(dǎo)引距離。

表4 不同信標數(shù)量下的長度觀測裕量Table 4 Length observation margins of different beacon numbers

在AUV 對接過程中,考慮到相機有效視場角為40°,定義角度觀測裕量為

式中,β為AUV 對m個目標信標的最小觀測夾角,若夾角大于40°,則AUV 進入視覺盲區(qū)。

圖10 為α在X-Z平面的分布,隨著AUV 接近基站,角度裕值減少。所設(shè)計燈光在水下散射角度為44°,假設(shè)燈光在水下符合Lambert 分布[15],則每個位置的光強為

圖10 角度裕量在X-Z平面分布云圖Fig.10 Contours of angle margin in X-Zplane

式中: μ為衰減系數(shù);I0為光源光強;θ為輻射角;z為距離。

定義工作空間強度值為

工作空間強度在X-Z平面的分布如圖11 所示。

圖11 有效工作空間在X-Z平面分布云圖Fig.11 Contours of effective workspace in X-Zplane

同光場分布下,分別定義最小n點觀測指標及最小m閾值工作空間指標

提出的算法工作空間指標參見表5。

表5 工作空間指標Table 5 Workspace indexes

仿真結(jié)果證明,PnP-P3P 算法能夠提升視覺導(dǎo)引有效工作空間,補充了近距離的對接視場,提高了整體對接魯棒性。

3.5 對接實驗

1)水池光學(xué)對接

實驗水池寬10 m,長25 m,深7 m?；镜闹行牡醴胖了戮嚯x水面2 m 的位置,AUV 在距離基站徑向20 m 時從水面進行下潛對接任務(wù),前向速度為0.5 m/s。整個對接過程僅使用視覺方法完成。一次成功的對接如圖12 所示。

圖12 水池對接過程圖像Fig.12 Image of docking process on pool

2)湖上聲光聯(lián)合導(dǎo)引對接

在實際應(yīng)用場景中,受到環(huán)境光和擾流等的影響,對導(dǎo)引算法提出了更高的要求。湖上實驗的目的是在水池實驗的基礎(chǔ)上驗證算法的魯棒性以及聲光導(dǎo)引的系統(tǒng)穩(wěn)定性。選取較為平緩的地形,將基站提前放置于湖底并保證其穩(wěn)定性,基站中心距離水面15 m。AUV 在距離基站1 000 m 距離從水面下潛,首先進行聲學(xué)的粗導(dǎo)引,在航行器接近基站15 m 時進入末端光學(xué)導(dǎo)引狀態(tài)。在執(zhí)行末端對接時,前向速度為0.5 m/s。圖13 與圖14 為一次成功對接的圖像與聲學(xué)導(dǎo)引軌跡。

圖13 湖上對接過程圖像Fig.13 Image of docking process on Lake

圖14 湖上對接聲學(xué)導(dǎo)引軌跡Fig.14 Acoustic guidance trajectory of docking on lake

4 結(jié)束語

文中針對籠式水下基站回收設(shè)計信標方案,通過仿真驗證了信標工作空間有效性。提出改進的基于神經(jīng)網(wǎng)絡(luò)的檢測-跟蹤算法與PnP-P3P 算法,將觀測數(shù)量降低到3 個信標,實現(xiàn)對基站的魯棒跟蹤。文中研究的優(yōu)點在于將神經(jīng)網(wǎng)絡(luò)目標識別的高泛化性優(yōu)勢與傳統(tǒng)算法分割的高效率優(yōu)勢結(jié)合,通過水池與湖上聲光聯(lián)合導(dǎo)引驗證了該方法在實際應(yīng)用中的可行性,但目前提出的光導(dǎo)引工作空間分布僅分析了二維平面,下一步工作將考慮從多光源空間分布設(shè)計出發(fā)建立信標燈光三維空間分布及其優(yōu)化理論。