一種基于激光矩陣的水下網衣巡檢機器人定位方法

秦 浩，楊仁友，楊 靚

(南方海洋科學與工程廣東省實驗室(湛江)，廣東 湛江，524002)

近年來，中國海水漁業養殖業發展迅猛，內灣與近海水域分布共計有100多萬只網箱[1-2]，為獲得更好的養殖質量[3]，開始嘗試將養殖水域向深遠海拓展[4-6]。網衣作為深水網箱的重要組成部分，受污損生物的攀附影響，堵塞部分網孔，降低網箱內溶氧含量[7-8]，增加環境載荷，降低網衣使用壽命[9]，破壞網衣結構，增加魚群逃逸風險[10-11]。因此，網衣巡檢成為深海網箱的關鍵性技術和推廣中需要解決的重要難點之一[12]。目前，深水網箱網衣巡檢依賴于專業潛水員下潛作業[13]。網衣受海浪等隨機性因素影響，會出現不可控的擺動及變形[14-15]，而基于靜態參量設置的導航程序難以適應不斷變化的網衣情況，無法做到高精度定位與導航，可能造成機器人在巡檢過程中定位丟失、遺漏檢測區域等問題，使得最終檢測結果與實際情況存在較大誤差。因此面向隨機運動網衣的動態定位是實現機器人水下高精度巡檢作業的關鍵技術問題。

現有的水下定位技術，主要包括船位推算、慣性導航(INS)/聲學組合導航、INS/多普勒計程儀(DVL)組合導航、視覺導航、地球物理導航[16-17]等。航位推算[17-18]主要依賴測速儀、姿態儀及深度傳感器等，算法簡單且實現方便，但其推算精度不高，養殖網箱中，網衣變形不易測量，故搭載機器人難以實現跟隨網衣的定位。慣導定位是一種根據載體的加速度和角速度值推算載體相對位置的導航方法[21]，自主性好、短時精度高，但是存在長期累積誤差，常用超短基線設備或DVL抑制長期漂移。超短基線等安裝位姿要沒入一定水深深度，難以獲取近水面的定位數據，且近水面受波浪背景噪聲及多途干擾，而且聲學設備對魚類養殖可能產生干擾，故養殖網箱不宜使用INS/聲學組合導航。類似地，DVL設備由于養殖魚群干擾會頻繁出現短時失效，同時受網箱運動干擾，很難獲取對底速度，無法消除慣導設備的累積誤差，因網衣變形難以測量，不適用于搭載機器人對網衣的相對定線航行和抵近檢測。在內使用聲學系統進行水下機器人的定位。視覺系統[22-24]通常與激光或慣導結合使用，但由于光在水中傳播損耗較大，養殖網箱隨海浪周期性和隨機性運動，視覺里程計會引入背景誤差，以及網衣受流變形導致視覺里程計難以實現定位。地球物理導航[25-26]主要依靠自身攜帶的傳感器對地球物理參數，如地形匹配、重力場、地磁場等，與預先勘探測繪的地球物理參考圖進行對比獲得水下定位，其無源性，精度高，不受時間限制，隱蔽性強，在軍用中有著重要前景，但也因其傳感器精度要求高，技術尚不成熟，不適用于養殖網箱尺度的水下環境。

本研究利用相位式激光在水下近距離測距的高精度、低干擾優勢，組成一套基于十字激光測距矩陣傳感器的水下機器人定位系統，并提出一種基于激光矩陣的水下網衣巡檢機器人定位方法實現養殖網箱系統內的相對定位，以及巡航過程中的相對航行姿態計算。

1 基于激光矩陣的水下網衣巡檢機器人定位系統

如圖1所示，該水下網衣巡檢機器人的定位系統[27]包括：處理器1001，例如CPU，網絡接口1004，用戶接口1003，存儲器1005，通信總線1002。其中，通信總線1002用于實現這些組件之間的連接通信。用戶接口1003包括顯示屏(Display)、輸入單元比如鍵盤(Keyboard)，還包括標準的有線接口、無線接口。網絡接口1004包括標準的有線接口、無線接口(如WI-FI接口)。存儲器1005可以是高速RAM存儲器，也可以是穩定的存儲器(non-volatile memory)，例如磁盤存儲器。存儲器1005可以是獨立于前述處理器1001的存儲裝置。

圖1 水下網衣巡檢機器人硬件運行環境的系統結構示意圖

如圖2所示，在水下巡檢機器人的航行過程中，使水下巡檢機器人上安裝的多個激光傳感器發射激光至目標網衣上，并通過多個呈十字型排布的激光傳感器持續接收激光反射信號，以獲取初始回波信號圖。激光傳感器如圖2所示的方式間隔均勻排布并朝向目標網衣表面，數量可根據實際需要設置；網衣由網線編織而成，其包括多個網目，激光傳感器發射的激光若打網線上，將被網線反射回同一激光傳感器上，而若打在網目位置，激光將穿過網目而不發生反射。激光傳感器接收到反射激光的時間點即為回波信號點，隨著水下巡檢機器人繞目標網衣表面不斷航行，激光傳感器不斷接收到目標網衣反射的反射激光。將每個激光傳感器的回波信號點在時間軸上逐一標示出來，即形成了矩陣激光回波信號圖。通過計算分析信號圖，可以獲得水下網衣巡檢機器人的相對定位。

圖2 激光傳感器的排布示意圖

2 水下網衣巡檢機器人定位方法

如圖3所示，提出了一種基于激光測距矩陣的水下網衣巡檢機器人定位方法，該方法包括以下步驟：基于網箱物理模型，利用地址編碼給網箱結構、網衣結點、網線、網孔建立編碼地址索引庫；水下巡檢機器人上安裝的多個激光傳感器發射激光至目標網衣上并接收；通過激光信號接收器，獲取回波信號圖；通過回波信號圖得到機器人航行的網目位置；通過公式計算出航行距離、豎直偏移距離、橫向偏移距離和橫向偏移角度；從而確定水下巡檢機器人在水下網衣的定位地址。深遠海養殖網箱中，由于機器人在水下巡檢，水下環境的復雜，水體濁度對水下相位式激光測距儀精度影響較小，且在近距離測距中測量精度理想[28]，所以采用相位式激光傳感器的相位式激光進行定位。

圖3 水下網衣巡檢機器人定位方法流程圖

2.1 相位式激光測距原理

相位式激光測距的基本原理是對激光強度進行幅度正弦波調制，并將傳播后的飛行時間(Time of flight)轉換為相位差[29]。可以通過式(1)得到相位差Δφ：

(1)

式中：ω為調制光的角頻率，rad/s；τ為激光傳輸時間，s；f0為調制光的頻率，Hz；d為激光發射器到目標的距離，m；c為激光傳播速度，m/s。

采用基于光外差相位檢測技術進行相位測量時，具有良好的分辨率是非常重要的[30]。利用光外差技術，相位差又可以寫成Δφ=2πTX/TIF，其中TIF為中頻信號的周期，Tx為相位檢測器電路輸出處得到的矩形波列高脈沖寬度所對應的時間。所以激光發射器到目標的距離d可以由式(2)計算得到:

(2)

式中：λ0為激光波長，m；λ0=c/f0。

相位差測量的精度受到串擾效應、激光二極管驅動器信號頻率的精度和穩定性、相位檢測器計數器的局部頻率和時鐘脈沖頻率等因素的影響。而在水下測量時，水的折射率決定了光波長、密度、溫度以及水的壓力。同時，水的鹽分、溫度、壓力及速率分布會引起折射率的非均勻分布，這會很大程度上影響激光測距儀的結果。光的吸收損耗會導致光束能量的減少，也會導致傳輸距離的減少。光的散射作用會影響光束的方向，并且會極大地降低測距精度。而使用的相位式激光測距能一定程度上避免水體濁度對精度的影響，但測距動態范圍隨著水體濁度的增大呈現指數衰減現象。經定標校正后，水下相位式激光測距儀在水下3.5 m范圍內測距誤差平均值不超過3 mm，可用于水下目標近距離的精確測距[28]，本研究中漁網巡檢機器人的貼網距離設定為0.5 m，理論測距誤差滿足定位需求。

2.2 激光測距傳感器網格計數方法

在理想狀況下，水下巡檢機器人始終平行于橫向網線航行(具體為呈十字型的激光傳感器始終平行于橫向網線)，由于每個網目的尺寸相同，因此每個激光傳感器接收到豎向網線反射的反射光的時間節點是基本相同的，即此時回波信號圖中每個激光傳感器的每個回波信號點在時間軸上的位置是基本相同的，在此情況下，當各個激光傳感器的回波信號點在某一時間節點上同時出現，且在下一時間節點上再次同時出現時，則可認為水下巡檢機器人在兩個時間節點之間走過了一個網目的距離，因此在已知網目尺寸的情況下，通過統計總共走過的網目數量，可方便地獲知水下巡檢機器人相對目標網衣表面的總航行距離。

具體計數步驟如下：

①將與豎向部上的激光傳感器相對應的校正回波時間軸篩選出來，以作為豎向回波時間軸；

②設定時間閾值Δt(s)，在目標航行時間段內，統計多個豎向回波時間軸在一個Δt范圍內均出現回波信號點的總次數，以作為水下巡檢機器人在目標航行時間段內的經歷網目數；

③基于經歷網目數，計算出校正航行距離ΔY。

時間閾值Δt具體可根據式(3)計算：

Δt=l/2v

(3)

式中：l為目標網衣上單個網目的邊長，m；v為水下巡檢機器人的航行速度，m/s。

如圖4所示的情況，在目標航行時間段內，多個豎向回波時間軸在一個時間閾值范圍內均出現回波信號點的總次數為5次，則可認為水下巡檢機器人在目標航行時間段內等效走過了5個網目的距離。

圖4 矩陣激光傳感器信號接收示意圖

由此可根據公式(4)計算出水下巡檢機器人的航行網目距離Δy。

Δy=N×l

(4)

式中：N為水下巡檢機器人經歷的網目數，個。

2.3 網目內相對定位方法

通過激光測距傳感器網格計數方法僅能得到水下巡檢機器人經歷過的網衣格數以及精度為l的距離信息，巡檢機器人以“網目”為單位進行的定位，即網衣格柵定位，不能確定具體的相對位置信息，故需通過以下方法對巡檢機器人再次定位，提高定位精度。

如圖5所示，巡檢機器人P在巡檢過程中某時刻，十字矩陣激光中心P’掃過網目內，通過第一激光線AB可以確定P在繞第二激光線OO’旋轉面上的旋轉角pitch(rad)，即γ，以及第一激光線AB上的相對位移ΔY，即AP’(BP’)。

圖5 水下網衣巡檢機器人網目內定位方法示意圖

同理，通過第二激光線OO’可以確定P在繞第一激光線AB旋轉面上的旋轉角yaw(rad)，以及第二激光線AB上的相對位移ΔX。

根據示意圖中的結構信息，可以根據式(5)、(6)計算出γ及AP’(BP’)。

(5)

(6)

由此可以計算出巡檢機器人相對網衣的航行位置X、Y，以及航向角pitch、yaw，從而提高巡檢機器人的相對定位精度。

在實際中激光傳感單元的排列、角度、傳感噪聲、機器運動都會導致誤差量Δl1、Δl2、Δα、Δβ的產生，下面通過誤差計算來分析誤差量對最終計算結果的影響。

(7)

(8)

由于在每次試驗在定標矯正后才開始試驗，故Δα→0、Δβ→0，在仿真試驗中Δα=0、Δβ=0，只有距離測量值會在傳感器噪聲以及運動下產生誤差值Δl1、Δl2。

故式(7)、(8)，可化簡成：

(9)

(10)

式中：l′=l1/l2，Δl′=(l1+Δl1)/(l2+Δl2)。

由式(9)、(10)可見，Δγ～Δ(tanγ)～Δl′，計算中航向角誤差主要來源于測量值的比值Δl′，而航行位置誤差主要由各自的測量值Δl1、Δl2產生。

2.4 仿真環境搭建

2.4.1 養殖網箱編碼先驗地圖建立

根據深遠海養殖網箱建立網箱的物理模型，如圖6所示。基于網箱物理模型，利用地址編碼給網箱結構、網衣結點、網線、網孔建立編碼地址索引庫。網衣結點定義點編碼地址索引庫，網線定義線編碼地址索引庫，網孔(網目)建立面編碼地址索引庫，同時利用網箱結構定義編碼地址邊界條件。例如，某一網目編碼如圖7所示。

圖6 深遠海養殖網箱物理模型

圖7 基于網箱模型的編碼示意圖

圖7編碼L0203H0102X06Y20中L后接數字表示結構立柱02與03，H后接數字表示結構桁架01與02，X、Y后接數字表示在該結構立柱與結構桁架為邊界條件的情況下，該平面區域中，網目的二維編碼位置。

2.4.2 仿真模型構建

通過Gazebo仿真建模，利用水下機器人搭載本研究提出的十字矩陣激光傳感器，進行該區域水下網衣巡檢的仿真試驗，仿真主機的硬件配置如表1所示。

表1 仿真試驗環境

仿真過程中，某時刻Gazebo的顯示界面如圖8所示。

圖8 Gazebo仿真環境中水下網衣巡檢機器人作業圖

如圖9所示，十字激光矩陣激光傳感器在橫向及豎向上分別設置了32個激光器，激光器間隔角度為0.033 45 rad，從帶編碼標記立柱的確定位置開始，通過控制仿真機器人在規劃路徑上進行適網航行巡檢作業，水下環境有水霧設定以模擬光在水下的衰減情況，以及通過給矩陣激光傳感器加入高斯噪聲達到傳感器的噪聲模擬，噪聲參數基于公布的Hokuyo激光器規范，在小于10 m的范圍內達到“+30mm”的精度。本研究中設置均值為0.0 m，標準差為0.01 m。

圖9 水下網衣巡檢機器人巡航定位示意圖

3 結果與分析

3.1 激光矩陣信號分析

如圖10、圖11所示，水下網衣巡檢機器人從基準點開始，依照規劃路徑作業巡航17.5 s，采集到的激光矩陣回波信號圖。其中圖10為豎向部上部分激光信號回波圖，圖11為橫向部上部分激光信號回波圖。

圖10 豎向部激光傳感器部分信號回波圖

圖11 橫向部激光傳感器部分信號回波圖

根據網目統計方法，從豎向部激光信號回波圖可以統計得到在17.5 s內，巡檢機器人沿X方向經歷了8個網目；從橫向部激光信號回波圖統計，時間閾值Δt內并未同時出現回波信號，而是依序出現，故巡檢機器人沿Y方向經歷了0個網目。值得注意的是，從豎向部激光信號回波圖中的第六激光器回波圖中，可以觀察到一段連續的回波點，這是因為巡檢機器人在巡航過程中，路徑偏離了規劃路徑，激光發射器發射的激光正好打在了網衣的網線上。

3.2 定位計算結果分析

通過對激光器的回波信號圖中數據的處理，依據上文計算方法，可以計算得到巡檢機器人的具體偏航信息，如圖12所示為巡檢機器人在17.5 s內，XY方向上位移計算值與實際位移的對比。由圖12可見，本研究提出的算法計算出的位移，基本符合實際移動趨勢以及位移量，可以分析出，巡檢機器人在網衣巡檢作業過程中，在y方向上偏離了約0.008 m。

圖12 XY方向位移計算值與實際位移值對比圖

依據本研究提出的偏航角γ值計算pitch以及yaw的值，實際計算中，由于巡檢機器人在pitch上偏轉較小，數值趨近于0，如圖14所示，且由于yaw上變化量依舊很小，對數據進行了簡單的均值濾波處理后，得到了如圖13的yaw實際值與真實值的對比圖。

由圖13可見，yaw上計算值的變化趨勢較符合yaw的真實值，經過17.5 s后巡檢機器人在yaw方向上偏轉了約0.14 rad。

圖13 計算偏航角度與實際值對比圖

在具有網衣編碼的先驗地圖中模擬一組定位試驗，如圖14所示。

圖14 仿真試驗下水下網衣巡檢機器人巡檢路徑

本次仿真試驗在8×6網格中進行網目計數定位算法驗證，網目大小為250 mm×250 mm，由定位結果可見，格柵定位路徑能準確反映了巡檢機器人在網目中的相對巡檢路徑， 即在該巡檢時段中，巡檢機器人貼網航行的方向，以及航行的網格數目，且在開始一段定線巡檢中，激光矩陣中心傳感器如圖10中第6傳感器所示，維持了較長一段定線巡航穩定性。

在穿越網線時也能檢測出巡線路徑的改變，在本研究方法中，只有當網目計數偏離半個網目，才會出現偏航計數誤差，故本次仿真網衣格柵定位誤差為0個網目。如表2所示，依照基于編碼地址的先驗地圖信息，巡檢路徑能以編碼信息輸出，在巡線航行中，編碼以兩面編碼表示面-面相交處網線編碼，在編碼信息中同時記錄養殖網箱系統中網衣的實際情況。

表2 巡檢路徑編碼

3.3 誤差分析

本仿真試驗中，誤差主要來源為由于水下環境造成的建模網衣的輕微振動以及巡檢機器人在航行過程中運動穩定性受環境干擾，導致的激光矩陣在測距上出現的測量誤差，以及人為設定的傳感器高斯噪聲產生的誤差。由圖15可見，在X位移上誤差較小，而在Y位移上誤差較大，誤差均呈現由大到小的分布趨勢，X位移上由初始的0～2 s內平均誤差7.52%提高到14.5～16.5 s內的3.72%，Y位移上0～2 s內平均誤差由51.35%提高到14.5～16.5 s內的8.15%，這是因為在仿真試驗中，矩陣激光傳感器中加入了高斯噪聲以及機器人在水環境中運動測量的干擾，在位移量較小時，高斯噪聲造成的傳感器測量誤差遠大于運動造成的誤差，隨著位移量的增加，傳感器測量誤差占比逐漸小于運動誤差，而本研究提出的計算方法計算出的相對定位值在運動誤差影響下符合理想預期，故整體位移誤差均隨位移量增加呈下降趨勢，其中X方向上位移量增加比Y方向上要大，X位移誤差下降比Y方向快。

圖15 XY方向位移計算誤差圖

如圖16所示，通過計算誤差進行進一步分析。由yaw誤差曲線圖可見，yaw初始時段2～4 s內平均誤差由62.48%提高到14.5～16.5 s內的19.18%，yaw的誤差如前文所述，在偏轉角度較小時，受傳感器噪聲誤差影響較大，故誤差異常偏高，隨著偏轉角度的增加，誤差影響逐漸變小，主要呈現運動誤差，而計算的轉角值符合理想情況，運動誤差遠小于傳感器噪聲誤差，所以總誤差隨著偏轉角的增大而出現下降趨勢。

圖16 yaw上計算偏航角度誤差圖

4 結論

提出的水下網衣巡檢機器人定位方法能夠準確定位出巡檢機器人相對網衣巡航的網目定位，并根據網目內相對定位算法計算出的巡檢機器人相對位移值與相對偏航角度，0.4 m距離處計算平均誤差3.72%，0.08 m距離處計算平均誤差8.15%，0.14 rad偏轉計算平均誤差19.18%，并在位移量及偏轉量增大時誤差逐步降低，符合實際位移及偏轉情況，在網衣格柵定位中誤差為0個網目，達到了水下巡檢機器人在非聲非慣性系下相對養殖網箱定位要求，巡檢路徑以編碼信息輸出，并記錄養殖網箱巡檢情況。本研究下一步將探索多源異構傳感器在深遠海養殖網箱系統中的定位，并實現水下網衣巡檢機器人的污損網衣識別檢測與定位。