基于INS/前視聲納組合的輸水隧洞AUV巡檢定位方法

楊文果，吳文啟，張 鷺，王茂松

(國防科技大學智能科學學院，長沙 410073)

0 引言

南水北調工程對于提高水資源的配置效率，促進可持續(xù)發(fā)展具有重要意義。輸水隧洞常年處于滿負荷狀態(tài)，隨著運行時間的延長，會出現(xiàn)局部破損等缺陷，使隧洞的安全運行受到嚴重威脅[1]。因此，定期進行輸水隧洞的問題排查和檢修是十分必要的。目前，國內對于輸水隧洞的檢測主要有人工檢測和水下機器人檢測兩類[2]。水下環(huán)境的隱秘與惡劣，會對潛水員的人身安全構成威脅，且人工檢測效率較低，因此，利用自主水下潛航器(Autonomous Underwater Vehicle，AUV)進行自動巡檢是未來輸水隧洞檢測的主流發(fā)展趨勢。

確定輸水隧洞的故障位置是AUV巡檢的主要任務，這就對AUV的環(huán)境感知能力和導航定位能力提出了很高的要求。巡檢時，AUV用于隧洞環(huán)境成像的傳感器通常是前視聲納，利用前視聲納對隧洞內結構破損和泥垢堆積等故障進行感知。當前視聲納檢測到故障部位時，確定AUV的位置也即確定了故障部位的位置，對AUV進行定位的最終目的是對隧洞故障進行定位。輸水隧洞位于地下，不能接收到衛(wèi)星導航信號，目前，AUV導航定位主要采用慣性導航系統(tǒng)/多普勒測速儀(Inertial Navigation System/Doppler Velocity Log，INS/DVL)組合導航的方式。但前視聲納和DVL都需要發(fā)射和接收聲波，且二者含有相同的頻率成分，因此DVL聲波會對聲納成像產生干擾，使得前視聲納對隧洞故障的感知變得困難。一旦前視聲納失去了對隧洞故障的成像能力，AUV對故障處的定位便無從談起。

為了解決AUV對隧洞故障的成像和定位相矛盾的問題，本文提出了一種基于慣性測量單元(Inertial Measurement Unit，IMU)和前視聲納組合導航的方法，即舍棄DVL，直接根據(jù)前視聲納成像原理恢復出載體速度，而后和慣性器件相組合，使前視聲納除了用于成像之外，也能發(fā)揮和DVL相接近的作用。目前，利用前視聲納實現(xiàn)水下組合導航的研究仍處于起步階段，研究熱點集中在同時定位與地圖構建(Simultaneous Localization and Mapping，SLAM)這一方向。張潔等使用最鄰近數(shù)據(jù)關聯(lián)算法(Nearest Neighbour Filter，NNF)實現(xiàn)了前視聲納圖像間的數(shù)據(jù)關聯(lián)，應用EKF-SLAM進行水下機器人的位姿估計與環(huán)境中障礙物特征的地圖構建[3]，但是僅在仿真環(huán)境中驗證了其算法的可行性，沒有在實際AUV平臺上進行測試。T.A.Huang等仿照光學相機的運動恢復結構(Structure from Motion，SfM)算法，推導了適用于前視聲納的SfM算法(Acoustic SfM, ASfM)，從聲納圖像幀間恢復出載體的位姿變化[4]，但是跳過了不同聲納圖像幀間特征自動匹配這一關鍵步驟，而是通過手動匹配特征點實現(xiàn)數(shù)據(jù)關聯(lián)。E.Westman等對ASfM算法進行了改進，討論了欠約束特征點的處理方法及其對位姿估計的影響[5]，但是僅在水池中進行了實驗，而且提前在水池壁上粘貼了磁鐵以便在聲納圖像中形成特征點，缺乏實際在未知水域中的實驗驗證?？傊?，利用前視聲納進行SLAM算法的設計會遇到各種各樣的挑戰(zhàn)。目前，利用前視聲納提取載體速度的研究尚未得到深入開展。

本文提出的IMU和前視聲納組合導航的方法，通過在相鄰聲納圖像幀間進行特征提取和匹配，根據(jù)慣導信息約束以及隨機抽樣一致(Random Sample Consensus，RANSAC)算法，剔除明顯的誤匹配點對，而后從剩下的配對點中解算出載體速度并與慣導信息進行組合。通過實際實驗，以隧洞內的接縫線作為定位參考基準，對INS/前視聲納組合結果的準確性進行了驗證。

1 DVL對前視聲納圖像的干擾

由于水下光線昏暗以及水質混濁等問題，基于光學原理的相機難以在水下進行遠距離成像，目前用于水下成像的設備通常是各類聲納產品[6]。其中，前視聲納主要用于水下機器人前方范圍的感知和成像。圖1所示為前視聲納在未受干擾時所記錄的某一輸水隧洞中的圖像(藍線框出的黑色弧線為隧洞內的接縫線)。

圖1 前視聲納在輸水隧洞中拍攝到的圖像

DVL和前視聲納都是AUV上常用的傳感器，當兩者在輸水隧洞內同時開啟，前視聲納受干擾嚴重時采集到的圖像如圖2所示。

圖2 前視聲納受到DVL干擾時形成的圖像

輸水隧洞內空間狹小，DVL發(fā)射的聲波經隧洞壁來回反射后，有不少會被聲納換能器接收，在圖像中形成噪聲干擾。對比圖1和圖2可知，雖然聲納圖像的清晰度不如陸地上普遍使用的光學相機，但在圖1未受DVL干擾的情況下，可以分辨出隧洞壁上的凹槽和接縫線等特征。而在受干擾嚴重的圖2中，除了隧洞壁邊緣(兩條豎直亮斑)可以辨析出來，其他的圖像細節(jié)大都湮沒在噪聲之中。圖中藍線框出的區(qū)域為接縫線所在位置，由于噪聲污染已變得非常模糊。而且在不同的圖像幀之間，噪聲斑千變萬化，時大時小，時而出現(xiàn)時而消失，沒有規(guī)律可循。因此，如果在AUV上同時使用DVL和前視聲納，則通過前視聲納感知隧洞故障會變得困難，影響巡檢任務的完成。

2 由前視聲納圖像提取載體速度

2.1 前視聲納成像原理

針對上述問題，本文提出的解決方案是從AUV上卸載DVL，直接根據(jù)聲納圖像的成像原理以及AUV在隧洞中的運動特點，從聲納圖像中提取出載體速度。

前視聲納工作時，換能器基陣周期性地向前方一定角度范圍發(fā)射聲波，而后接收障礙物反射的回波。回波方位對應于障礙物相對聲納的方位，接收回波的時間對應于障礙物相對聲納的距離，回波強度對應于障礙物在圖像上的像素值大小[7]。前視聲納的視域為前方的扇形空間，如圖3所示。

圖3 前視聲納三維視域范圍

圖3中，前視聲納位于原點O處，以O點指向聲納正前方為x軸，指向右側為y軸，指向下側為z軸建立聲納坐標系。描述聲納視域的參數(shù)為[8]：發(fā)射波束水平開角θs，垂直開角φs，最大探測距離rmax(O到曲面M1M2M3M4上任一點的距離)，最小探測距離rmin(rmin一般接近于0，圖中沒有表示出來)。如果以x軸正向為基準建立球坐標系，則對于坐標系內任何障礙點，只要其距離r、方位角θ、仰角φ這3個參數(shù)滿足

則其位于聲納的視域范圍內，如果該點與O點的連線上沒有其他障礙點，則可以投射到聲納圖像上。

前視聲納圖像是二維圖像，視域中的三維坐標點投射到二維圖像上時，會有一個維度的信息丟失，具體的投射關系如圖4所示。聲納圖像的成像面為xOy平面與視域范圍相交所形成的二維扇形。對于空間中的一個障礙點，假設其位于聲納視域中[r,θ,φ]這一位置，通過該點反射的回波可以得到r和θ這2個參數(shù)，則其沿著圖中由r和θ所確定的紅色圓弧投射到圓弧與成像扇面的交點位置，其像素大小由回波強度決定。也就是說，投影到成像平面內的點，其距離和方位角這2個參數(shù)得以保留，而仰角信息丟失，根據(jù)聲納圖像無法確定成像點的仰角大小[9]。前視聲納和光學相機在成像時，都會有一個維度的信息丟失。不同的是，光學相機成像于與圖4中yOz平面平行的歸一化平面上，而前視聲納成像于xOy平面；光學相機丟失了空間點的距離信息，而前視聲納丟失的是空間點的仰角信息。

圖4 前視聲納圖像投影模型

前視聲納圖像是對其正前方扇形成像區(qū)域的等比例縮放，圖中亮度越大的地方，其反射回波越強，相鄰2個像素點間所對應的實際物理距離用于衡量圖像的分辨率。例如，聲納的最大探測距離設置為10m，若圖像中原點與圓弧邊緣間隔500個像素點，則圖像分辨率為0.02m/pixel。

2.2 載體速度提取模型

圖5 前視聲納在相鄰兩幀圖像間的運動

(1)

(2)

特征點在導航系中的坐標變化，與AUV在導航系中的位移存在對應關系

(3)

則在t1和t2這2個時刻之間，根據(jù)特征點匹配解算出的AUV在導航系中的速度為

(4)

式(4)是在特征點坐標可測量的情況下建立起的方程，但是特征點的仰角是未知的，因此需要先確定特征點在聲納坐標系中的仰角值才能提取載體速度。特征點仰角的計算是前視聲納用于導航的一大障礙，在前視聲納垂直開角不大的情況下，可以直接假設所有特征點仰角為固定值，此時引入的誤差并不是很大。但是為了實現(xiàn)更高精度的INS/前視聲納組合導航，就必須對特征點的仰角進行計算。

本文根據(jù)AUV巡檢時的運動特點，提出了一種針對水底為局部平坦區(qū)域的特征點仰角估算方法。通常，IMU會嚴格平行于AUV主軸安裝，即b系x軸指向AUV正前方，y軸指向AUV右側。為了確保前視聲納能掃描到隧洞底部，聲納基陣會斜向下發(fā)射聲波，即聲納坐標系的x軸是指向AUV的前下方，s系和b系之間通過繞y軸旋轉φs角能使三坐標軸平行，如圖6所示。

圖6 特征點仰角計算幾何示意圖

AUV巡檢時，通常將其設置為巡航模式，將深度傳感器和羅盤、IMU測得的信息融入控制回路，使AUV所處深度基本保持不變，姿態(tài)維持穩(wěn)定，因此俯仰角和橫滾角維持在0°左右，變化幅度不大。圖6中，水底的AB區(qū)域被聲納視域所覆蓋，可以反射聲波，形成圖像，設A點距坐標原點S的距離為RSA，在聲納視域中比RSA更近的區(qū)域內沒有障礙物，不會形成圖像。因此，聲納圖像中會出現(xiàn)一條較明顯的分界線，該線為聲波面SA與實際環(huán)境相交形成的，近似為弧線，如圖7所示。分界線以內區(qū)域不能形成回波，因此像素值很小，圖1中原點附近即有一條較明顯的弧形分界線。

圖7 聲納圖像分界線示意圖

當水底為起伏不大的平坦區(qū)域時，設聲納坐標系原點與分界線頂端距離為RSA，則在AUV維持水平姿態(tài)的情況下，SA與豎直方向的夾角是固定的，因此可以計算出AUV距水底的高度

(5)

對于水底成像視域內的一點P，其與坐標原點的距離是已知的，設為RSP，SP與水平面的夾角為

(6)

則P點在聲納坐標系中的仰角為

φP=φSP-φs

(7)

對于圖像中的分界線，可以通過圖像二值化和梯度檢測等手段提取出來。由于AUV設置為定深模式，如果提前獲取水深信息，在估算仰角過程中也可以將AUV距水底高度設為常值。當輸水隧洞底部為圓弧狀時，不能大范圍運用水平面假設，只能在底部小區(qū)域內進行水平面近似。但是當水底區(qū)域起伏較大或者AUV自穩(wěn)能力變差時，上述方法求解特征點仰角的誤差會增大。

與仰角計算相比，更能影響速度解算結果的便是特征點的匹配精度。目前沒有專門針對前視聲納圖像的特征提取和匹配方法，設計導航系統(tǒng)時只能使用針對光學圖像的ORB、SIFT和KAZE等算法，由于聲納圖像的低分辨率，提取的配對點精度普遍不高，而且含有大量的誤匹配。如果誤匹配特征點出現(xiàn)在方程(4)中，則最后求解的載體速度和實際情況偏差較大。針對這個問題，本文設計的解決方案如下。

步驟一：

在k-1時刻，經卡爾曼濾波得到了載體的姿態(tài)、速度和位置，到了下一個濾波時刻k時，首先根據(jù)慣導解算出的未經濾波修正的速度，對每對特征點進行初步判定。由于2個濾波時刻之間的間隔較短，純慣導解算出的速度在其間不會出現(xiàn)大的誤差累積，有一定的可信度。圖8所示為水下機器人搭載的某型光纖慣導系統(tǒng)在靜止狀態(tài)下的水平速度誤差絕對值曲線。第100s之前，以零速為觀測量進行卡爾曼濾波，解算出的水平速度維持在0附近；100s之后不再進行濾波修正，純慣導解算出的水平速度誤差絕對值會逐漸偏離0，但在斷開濾波后的短時間內，慣導解算值與真值偏離得并不遠，因此可用于對特征點數(shù)據(jù)進行初步篩選，剔除明顯的誤匹配點。

圖8 純慣導水平速度誤差絕對值曲線

則先將該對特征點保留，否則舍棄。其中，Δd是聲納圖像中的距離分辨率，本文實驗部分所用前視聲納的分辨率為0.013m/pixel，T為時間間隔，k為一個篩選參數(shù)，與匹配噪聲和慣導精度有關，若慣導系統(tǒng)精度較差，可將k值增大，防止原本正確的配對點被剔除。

步驟二：

經步驟一剔除了明顯的野值之后，對于剩下的誤差較大的配對點，本文仿照純視覺導航中RANSAC算法的思路進行剔除，具體操作如下：

輸入:連續(xù)兩幀特征點集,最大迭代次數(shù)kmax,速度誤差閾值Δvmax,nmax=0,k=0輸出:特征內點集當knmax,則nmax=n,記錄此時的內點集k=k+1,并重復執(zhí)行以上過程結束返回最后記錄的內點集,即為篩選出的特征內點

步驟三：

對于篩選出的N對特征點，通過取均值的方式求解最后的載體速度，計算公式為

(8)

求出載體速度后，便可將其作為觀測量進行卡爾曼濾波。

3 INS/前視聲納組合導航模型

在INS/前視聲納組合導航系統(tǒng)中，取系統(tǒng)狀態(tài)變量為

X=[φNφEφDδvNδvEδvD

δLδλδh]T

(9)

其中，φN、φE、φD分別代表沿北向、東向和地向的姿態(tài)失準角；δvN、δvE、δvD分別為北向速度誤差、東向速度誤差和地向速度誤差；δL為緯度誤差；δλ為經度誤差；δh為高度誤差。

姿態(tài)失準角方程為

(10)

速度誤差方程為

(11)

位置誤差方程為

(12)

依據(jù)北向和東向速度誤差建立的觀測方程為

(13)

(14)

另外，由定深模式下設定的深度值，可對狀態(tài)變量中的高度誤差建立觀測

(15)

其中，υdepth是高度觀測噪聲。

聯(lián)立方程(10)、(11)、(12)、(14)和(15)，便能得到連續(xù)時間狀態(tài)下的卡爾曼濾波模型。將連續(xù)時間模型離散化后，在濾波周期內，經時間更新、量測更新、增益系數(shù)計算等過程后，便能得到系統(tǒng)狀態(tài)變量的最優(yōu)估計，而后對姿態(tài)、速度和位置進行修正。

4 實驗驗證

4.1 實驗設計

用于實驗驗證的數(shù)據(jù)來源于南水北調工程中穿黃隧洞內的AUV測試數(shù)據(jù)，該隧洞位于河南焦作境內，地圖上位置如圖9所示。

圖9 實驗地點圖

實驗時所用的慣導系統(tǒng)為某型光纖慣性導航系統(tǒng)，其技術指標如表1所示。

表1 慣導系統(tǒng)技術指標

巡航模式下，AUV航向為沿著隧洞出口方向，根據(jù)IMU和前視聲納采集的數(shù)據(jù)，對INS/前視聲納組合算法性能進行驗證和評估。在實際工程中，隧洞故障在河段上的具體位置是迫切需要獲取的信息，因此AUV需要精確測量故障處離隧洞入口的距離，即需要精確測量檢測到故障時自身在隧洞前向上運行了多長距離。輸水隧洞中接收不到全球定位系統(tǒng)(Global Positioning System，GPS)信號，因此不能利用衛(wèi)星定位信息對INS/前視聲納組合定位算法進行精度評估，對此，本文將隧洞內接縫線作為位置基準。實驗隧洞中的接縫線是均勻分布的，每2條接縫線之間間距為10m，可在事后經過對聲納圖像數(shù)據(jù)的甄別，將其中的隧洞接縫線標注出來，并作為定位基準，對組合算法精度進行評估。以AUV經過的第一條接縫線為起始線，而后經過每條接縫線時，AUV的位置便依次為距起始線10m、20m、30m……以此為評估基準。

在進行特征點提取和匹配時，本文的設計是在聲納圖像的正前方區(qū)域截取一個固定的小窗口，從小窗口中提取特征進行數(shù)據(jù)關聯(lián)，將小窗口所對應的實際水底區(qū)域近似為水平面，便于進行特征點仰角估計，圖像其他區(qū)域的信息統(tǒng)統(tǒng)舍棄。

用于光學圖像特征提取和匹配的算法有很多，經實際測試，用于前視聲納圖像時每一種都會出現(xiàn)許多明顯的誤匹配點，目前沒有具體數(shù)據(jù)能夠衡量各種算法在匹配精度上的優(yōu)劣。從算法的復雜性和實現(xiàn)實時系統(tǒng)的可行性的角度考慮，本文采用了一種快速特征點提取和描述(Oriented FAST and Rotated BRIEF，ORB)算法。ORB在平移、旋轉、縮放的變換下仍有良好的表現(xiàn)，廣泛應用于基于光學相機的實時SLAM系統(tǒng)中[10]，但目前將其用于聲納圖像的嘗試還比較少。

4.2 實驗結果

整個實驗過程中，AUV的狀態(tài)可分為“緩慢前進-靜止-緩慢前進”3個階段，共進行了約1800s的IMU和前視聲納數(shù)據(jù)采集，前進過程中平均速度約為0.3m/s。除了進行INS/前視聲納組合的數(shù)據(jù)采集外，為便于對比，本文實驗也在相同區(qū)域進行了INS/DVL組合的數(shù)據(jù)采集，運用兩種組合定位方法分別對同一區(qū)域的接縫線進行定位，比較不同算法的定位效果。AUV經過第n條接縫線時，其相對于第一條接縫線的參考位置為10(n-1)m，圖10所示分別為兩種算法對各條接縫線的定位相對于參考位置的誤差，表2列出了兩種算法的均方根誤差(RMS)、最大誤差(MAX)和終點誤差(END)。

(a)INS/DVL組合算法對各條接縫線定位誤差

表2 兩種算法的誤差統(tǒng)計

實驗中，INS/前視聲納組合的最大定位誤差為3.0m，小于總行程的1%。從圖表中可以看出，輸水隧洞內，INS/前視聲納組合與INS/DVL組合的性能非常相似，兩種算法的定位精度沒有明顯的優(yōu)劣之分。但是在實際巡檢工作中，DVL聲波會對前視聲納圖像產生干擾，使得聲納圖像損失大量的成像細節(jié)，從圖像中辨析出隧洞故障的難度會增大。因此，在巡檢時卸載DVL、運用INS/前視聲納組合定位具有以下優(yōu)勢：1)從根本上消除DVL聲波的干擾，提高聲納圖像質量，確保故障定位任務的完成；2)降低巡檢成本，卸載DVL能減小AUV功耗，節(jié)省蓄電池電能，進而延長AUV在水下持續(xù)工作的時間。

本文所提出的INS/前視聲納組合算法能為解決AUV導航定位問題提供新的思路，除了在輸水隧洞巡檢時可以替代INS/DVL組合之外，在其他諸如水下堤壩檢修和水下地形探測等一些對聲納圖像質量要求高的任務中，需要規(guī)避DVL干擾時，也可以采取INS/前視聲納組合替代INS/DVL組合。但是當AUV姿態(tài)自穩(wěn)能力變差，或者前視聲納成像區(qū)域起伏較大，不能進行水平面近似時，本文提出的方法在估算特征點仰角時會存在一定誤差，當AUV進行長航時、遠距離的運動時，這種簡化方式會使導航定位精度有所降低。

5 結論

本文針對AUV在輸水隧洞巡檢過程中存在的DVL對前視聲納圖像產生干擾的問題，提出了一種從前視聲納圖像提取載體速度，進而和IMU進行組合導航的方法。經實際的輸水隧洞測試，驗證了其具有較高精度，能發(fā)揮和INS/DVL組合相接近的作用，具有實際應用價值。在其他需要隔絕DVL對聲納圖像干擾的應用場合，也可以選擇卸載DVL，采用INS/前視聲納組合定位的方法。但其也存在需進一步解決的問題，例如本文在估算特征點仰角時，對水底進行了局部平面假設，如果隧洞橫截面為弧形，這種假設只對弧形底部小范圍內的特征點近似成立，如果大范圍使用，需要進一步引入弧形底面的幾何約束。