面向自主靠泊的船舶航跡估計方法

鄒 雄, 肖長詩,b, 文元橋,b, 周春輝,b, 詹文強

(武漢理工大學(xué) a.航運學(xué)院； b.內(nèi)河航運技術(shù)湖北省重點實驗室， 武漢 430063)

目前，隨著無人航行器和自動駕駛越來越受到關(guān)注，無人艇(Unmanned Surface Vehicle，USV)作為航行器的一個分支，應(yīng)用也日趨廣泛，不僅應(yīng)用在軍事行動上，還應(yīng)用在民用領(lǐng)域中。USV非常適合執(zhí)行危險和耗時的任務(wù)，例如海上勘測、危險水域的搜索和救援、海岸線巡邏和水質(zhì)檢測等。[1]為使用USV執(zhí)行這些任務(wù)，精確導(dǎo)航是至關(guān)重要的能力。車載導(dǎo)航廣泛使用的工具之一是組合慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)和全球定位系統(tǒng)(Global Positioning System，GPS)的組合導(dǎo)航系統(tǒng)。INS通常以高頻率運行，但不可避免地具有漂移誤差，而GPS可提供絕對位置，其集成為海洋導(dǎo)航應(yīng)用提供理想的互補組合。然而，在大型建筑物附近，如橋梁、建筑物周圍以及內(nèi)河水域，GPS的使用常常受到限制。[2]

針對這些局限，激光里程計和視覺里程計(Visual Odometry，VO)迅速發(fā)展，與輪式里程計一樣，傳感器數(shù)據(jù)已被用來計算其相對運動軌跡。激光雷達方法的缺點是所使用的傳感器非常昂貴。此外，其測量范圍一般較小，不適合檢測遠距離海洋標(biāo)志物。[3]BANDYOPHADYAY等[4]提出由于水面波動的影響，激光傳感器在大于20 m的范圍內(nèi)相對無效。VO被定義為通過觀察其環(huán)境的圖像序列來估計相機的相對運動的過程，如果將單個或多個攝像機固定到載體上(車輛、人和機器人)，估計的位姿則為載體的相對運動。[5]而且，VO視覺方向性強，在視距好的情況下具有覆蓋距離很遠等諸多優(yōu)勢，所以VO在水上環(huán)境尤其是在近岸水域具有很好的發(fā)展?jié)摿Α?/p>

1 相關(guān)工作

MORAVEC提出利用視覺來估計無人車(Unmanned Ground Vehicle，UGV)的運動。MAIMONE等[6]提出視覺里程計的概念，并在美國國家航空航天局(National Aeronautics and Space Administration,NASA)開發(fā)的火星探測器上使用，其裝備兩個向下的相機組成雙目視覺，通過提取興趣點并采用跟蹤的方式完成相鄰幀的特征匹配，最后采用最大似然估計算法得到運動估計，并在30 m的軌道上能獲得2%的精度。NISTéR等[7]也對VO進行研究，首先檢測Harris角點，提取專用描述符，然后在一定的搜索區(qū)域內(nèi)完成特征匹配，在運動估計時提出單目和立體視覺兩種方案，而且在無人車上實施并驗證可行。隨后，KONOLIGE等[8]針對大尺度崎嶇不平的野外場景，提出雙目實時VO，融合慣性測量單元(Inertial Measurement Unit，IMU)來減少角度偏差的增長，最低誤差能達到0.36%。GEIGER等[9]提出雙目視覺里程計算法并將其開源，其采用自定義的檢測子和描述子并用專用指令集進行優(yōu)化，在前后幀的匹配上采用運動模型引導(dǎo)搜索，在“外點”去除上采用“環(huán)形”匹配予以驗證，最后通過特征點在左右圖像的重投影誤差和的最小化得到相機的運動估計。

針對單目的尺度不確定性問題，SCARAMUZZA等[10]在假設(shè)地面平坦的基礎(chǔ)上，將全景相機放在車輛頂部并結(jié)合車輛的運動學(xué)模型，提出一種基于隨機抽樣一致(Random Sample Consensus, RANSAC)的單點算法，能夠最大程度地降低迭代次數(shù)和運行時間。KITT等[11]利用相機高度和車輛附近地面的平面假設(shè)得到尺度信息并恢復(fù)運動估計。相機由于具有體積、重量、成本等諸多優(yōu)點，其是無人機(Unmanned Aerial Vehicle，UAV)中最具競爭力的傳感設(shè)備。BROCKERS等[12]修改第一個多線程實時的同步定位與建圖系統(tǒng)(Parallel Tracking and Mapping，PTAM)用于無人機的自主導(dǎo)航，得以在機載嵌入式計算機上以30幀/s的速度估計無人機的6D姿態(tài)。FORSTER等[13]提出一種半直接視覺里程計算法(Semi-Direct Monocular Visual Odometry，SVO)，在一臺嵌入式計算機上以55幀/s的速度運行，并輸出稀疏的3D環(huán)境地圖。FU等[14]采用嵌入式系統(tǒng)實現(xiàn)無人機的立體視覺預(yù)處理系統(tǒng)，其中雙目視覺里程計由一個基線為12 cm的單獨模塊實現(xiàn)，該系統(tǒng)可實時估計六自由度無人機姿態(tài)以及重構(gòu)無人機的周圍環(huán)境，在室內(nèi)0.8 m/s的速度下，估計精度小于3 cm。 HUANG等[15]將深度相機用于四旋翼無人機的自主飛行，采用FAST特征提取和9×9大小的像素塊作為描述符通過絕對誤差和算法找到匹配特征點，實現(xiàn)基于深度相機的VO，能夠在靜態(tài)室內(nèi)環(huán)境中估計自身位姿并生成豐富的環(huán)境地圖。STEINBRüCKER等[16]針對深度相機提出能量最小化方法得到圖像間相機的相對運動，該方法在較小的相機位移和靜態(tài)環(huán)境下的性能良好。KERL等[17]在此基礎(chǔ)上提出一種更高精度和更廣用途的擴展版本(Depth Visual Odometry，DVO)，通過最小化光度誤差得到兩個圖像間的相機運動關(guān)系。然而與視覺導(dǎo)航在UGV和UAV的應(yīng)用如此廣泛有所不同，關(guān)于VO與USV結(jié)合的研究，尤其是在海事環(huán)境中，面向USV的視覺導(dǎo)航的實現(xiàn)還有很多未知領(lǐng)域，而且針對UGV和UAV開發(fā)的視覺里程計算法均不能高效地適合USV的水域環(huán)境。VO算法需要檢測每幀圖像中的特征點，但是這不適合于特征很少的開放海洋環(huán)境。[18]在水域環(huán)境中VO的性能受到快速變化的水面、反射等情況的嚴(yán)重限制。[19]面對這些困難，相關(guān)學(xué)者研究船舶的視覺導(dǎo)航和水域環(huán)境的重建。HUNTSBERGER等[20]針對海洋環(huán)境設(shè)計一種具有4個攝像機的立體視覺系統(tǒng)，以增加視覺范圍，實現(xiàn)對水中目標(biāo)和障礙物的檢測。LEEDEKERKEN等[21]提出既能在水面又能在水下場景都能建圖和導(dǎo)航的方法，該方法利用開放水域中的GPS數(shù)據(jù)和GPS受限區(qū)域的其他傳感器數(shù)據(jù)。

本文主要研究利用視覺傳感器在無需外部位置信息的情況下,在水域環(huán)境中生成USV的軌跡和三維稀疏點云。特別地，在VO算法中引入水岸線檢測算法，以提高軌跡精度。本文的創(chuàng)新之處在于不像傳統(tǒng)的VO需要在所有圖像幀中完成特征檢測，本算法只需在關(guān)鍵幀中檢測特征，幀間通過特征點的跟蹤匹配進行關(guān)聯(lián)，不僅可提高精度，還可加快初始化速度。

2 算法設(shè)計

本文的目的是面對靠近岸邊的水域環(huán)境，利用船載視覺傳感器獲取圖像，提取和跟蹤圖像中比較穩(wěn)定的特征點，從而估計出船舶的航跡。提出的算法框架見圖1，其算法框架分為兩部分：一部分為水岸線檢測；另一部分為航跡估計也叫運動估計。其中水岸線檢測的目的是將快速變化的水面給予去除，運動估計通過VO的方式實現(xiàn)。

圖1 算法框架

2.1 水岸線檢測

面對近岸水域，獲取的水岸線圖像分為3部分見圖2:圖像上方是天空背景，往往最亮(白天)，其灰度變化平緩；圖像下方是水面背景，其灰度明顯小于天空背景，而且水面區(qū)域往往有岸邊景物的倒影和受風(fēng)浪流影響的波紋；圖像中間是岸邊區(qū)域，主要是草地、樹木和建筑，其灰度值最低。岸上區(qū)域和水面區(qū)域的交界線稱為水岸線，其是梯度較大值的連線。由于艇載視覺的高度加上水面的水平特性，所以通常水岸線在圖像中為直線，但由于無人艇航行和艇載視覺的朝向，水岸線有一定的傾角。

a) 長江水域

b) 東湖水域

本文借用文獻[22]的思想，提出的水岸線檢測算法分為3步如圖1所示。

2.1.1邊緣檢測

邊緣是圖像中周圍像素灰度值顯著變化的像素點的集合，邊緣檢測就是提取這些顯著變化的點，常用的邊緣檢測方法有一階微分算子Roberts、Sobel和二階微分算子Laplacian、Marr等。這些方法的優(yōu)點是原理簡單，速度快，但是對噪聲敏感。諸多的邊緣檢測算子中，Canny算子是最佳邊緣檢測算子，具有非常好的檢測效果，其具體步驟為

(1) 采用高斯濾波器對圖像進行平滑;

(2) 通過鄰域范圍的有限差分，計算梯度幅值和方向;

(3) 對梯度幅值圖像進行非極大值抑制，只保留局部梯度幅值最大的點;

(4) 采用雙閾值法不斷地收集邊緣直到將圖像邊緣閉合。

2.1.2線段提取

通過采集和分析水岸線圖像，其有如下特點：

(1) 水岸線往往貫穿整個圖像，即使有時被遮擋，但其長度、連續(xù)性還是好于水面波紋。

(2) 岸邊建筑物在圖像中呈直線狀，且與水岸線垂直。

(3) 統(tǒng)計來說，水面波紋呈現(xiàn)的直線段長度大概率小于5個像素(圖像為640×480)。

(4) 統(tǒng)計來說，水岸線往往與圖像水平夾角在45°之內(nèi)。也就是說：通過角度和線段長度的判斷可把絕大多數(shù)的岸上建筑物和水面波紋的干擾消除。所以本方法采用霍夫變換提取線段，其中只考慮長度為5像素以上并且直線斜率在[-1，1]之間的線段。

2.1.3直線擬合

采用RANSAC算法進行直線擬合，其迭代次數(shù)為

(1)

式(1)中：p為正確概率；w為每次從數(shù)據(jù)集中選取一個局內(nèi)點的概率。

為保證工程應(yīng)用的實時性和可靠性，該方法針對RANSAC算法做了細節(jié)上的修改：受CHUM等[23]的啟發(fā)，從最可信的匹配中挑選樣本，在樣本的選擇上采取如下規(guī)則：

(1) 第一個點的選取，越靠近圖像中心水平線選擇優(yōu)先級越高;

(2) 第二個點的選取，以第一個點為原點，水平移動一定距離(100像素)得到中心點，越靠近該中心點優(yōu)先級越高；抽樣次數(shù)，不需要遍歷所有可能情況，保證至少有一個優(yōu)樣本(樣本數(shù)據(jù)都是“內(nèi)點”的樣本)即可。據(jù)統(tǒng)計計算量可降低60%。

2.2 運動估計

利用去除水面區(qū)域后的圖像來估計船舶航跡，分為3個部分：首先對圖像特征檢測并跟蹤，得到兩個關(guān)鍵幀的特征對應(yīng)關(guān)系;然后估計兩相機的相對運動;最后采用滑動窗口局部優(yōu)化。

2.2.1特征匹配

到目前為止，研究者陸續(xù)提出許多局部特征方法，如Harris、FAST、SURF、SIFT、ORB等，其中前兩種是角點，后幾種特征包含關(guān)鍵點和描述符。通常有匹配特征點的描述子和跟蹤兩種方式得到圖像間的特征點對應(yīng)關(guān)系。本文采取多尺度FAST特征加上Lucas光流(Kanade-Lucas-Tomasi,KLT)跟蹤的方式實現(xiàn)圖像間的特征匹配。其中尺度因子取1.2，當(dāng)檢測到的特征數(shù)量太少時，將閾值(默認(rèn)值20)減半，以確保檢測到足夠多的特征點數(shù)量。KLT屬于一種光流法，假設(shè)：相鄰幀之間的亮度恒定；相鄰幀之間的運動微??；相鄰點有相似運動。

針對研究的水域環(huán)境，與UGV和UAV相比，船舶的運動較慢，而且水域環(huán)境中的運動物體較少，所以這種匹配方式較為適合，而且最為關(guān)鍵的是不需要每一幀圖像都進行水岸線檢測，這對算法的運行效率相對較高。

2.2.2位姿估計

在初始化完成之后，已經(jīng)具備全局坐標(biāo)系和初始化的場景結(jié)構(gòu)，每當(dāng)提取一個關(guān)鍵幀，通過特征跟蹤，得到特征點的匹配關(guān)系，以及在當(dāng)前圖像中是可見的初始場景點，這構(gòu)成2D-3D的對應(yīng)，可通過n點透視問題(Perspective-n-Point, PnP)求解。換句話說，已經(jīng)知道的場景點(X1,X2,…,Xj,…,Xn)和其在圖像中的特征位置以及在目前的圖像幀的對應(yīng)位置，能夠估計當(dāng)前的相機在全局坐標(biāo)系中的位姿。

由于在位姿估計中通常會有少量的“外點”干擾，采用RANSAC方案得到最大的一致集以及位姿估計的初值。隨后通過場景點在圖像中的重投影誤差構(gòu)建最小二乘優(yōu)化問題，通過光速法平差(Bundle Adjustment, BA)對估計值進行調(diào)整。

2.2.3局部優(yōu)化

每當(dāng)提取一個關(guān)鍵幀，一方面為得到更加精細的位姿估計，需要對前面的相機位姿和場景點進行優(yōu)化，另一方面為控制一定的計算量，所以設(shè)定只在一定的滑動窗口內(nèi)執(zhí)行優(yōu)化操作[24]，具體如下：取窗口大小M和最新關(guān)鍵幀Nk，其他的相機位姿保持固定不變，使用LOURAKIS等[25]提出的稀疏束調(diào)整進行實現(xiàn)。在局部優(yōu)化完成后，通過當(dāng)前關(guān)鍵幀與前后的兩個關(guān)鍵幀，采用三視角的三角化方案[26]得到場景3D點。

3 稀疏集中調(diào)整

3.1 關(guān)鍵幀的選擇

隨著程序的運行，因為有些場景移出視線，所以能夠跟蹤成功的特征點的數(shù)量越來越少。當(dāng)該數(shù)量下降到一定閾值時，選擇一個新的關(guān)鍵幀。但是，如果關(guān)鍵幀過于密集，不僅會影響程序的運行速度，還會增加空間點三角測量的不確定性。[27]所以選擇關(guān)鍵幀，既希望他們之間有盡可能多的相機運動，又需要有足夠的匹配。[11]

3.1.1初始關(guān)鍵幀的選擇

除去場景模糊等之外的情況，通常取第一幀為關(guān)鍵幀K1，進行水岸線檢測，去除水面區(qū)域，然后提取L0個特征點，采取持續(xù)跟蹤的方式選擇第二關(guān)鍵幀K2，使得在K1和K2之間至少存在L1個匹配點，然后選擇第三個關(guān)鍵幀K3，使得在K1和K3之間至少有L2個匹配點。

3.1.2關(guān)鍵幀的選擇

在初始化完成之后，對最近的關(guān)鍵幀圖像提取L0個特征點，之后每獲取一幀圖像，都采用KLT跟蹤并統(tǒng)計特征點數(shù)量，當(dāng)特征點數(shù)量降為L2個，設(shè)置當(dāng)前幀為關(guān)鍵幀，并進行去除水面操作，然后提取L0個特征，依次往復(fù)。

3.2 初始化過程

初始化過程在初始關(guān)鍵幀中實現(xiàn)，其步驟如下：

1) 得到3個初始關(guān)鍵幀K1、K2、K3，在去除水面之后，K1與K2、K1與K3之間各有L1和L2個對應(yīng)特征點。

2) 設(shè)定K1為全局坐標(biāo)系，在K1與K2和K1與K3之間通過對極幾何在RANSAC框架下分別計算相對位姿。

3) 基于本質(zhì)矩陣E(K1與K3)，得到K3與全局坐標(biāo)的相對位姿(R，t)，其中R為相機的旋轉(zhuǎn)，t為相機的位移。

4) 使用K3的相機位姿和對應(yīng)的特征點，先三角化得到三維點，再利用其與K2中的對應(yīng)特征采用PnP算法得到K2幀的相機位姿。

5) 在這3個關(guān)鍵幀中，通過場景點在圖像中的投影位置構(gòu)建重投影誤差為

(2)

至此，初始化完成后，位姿估計算法的過程為

(1) 對于每一幀，判斷是否為關(guān)鍵幀，如果不是，重復(fù)(1);

(2) 采用PnP算法估計相機位姿的初值，三角化得到場景點，將該幀加入窗口;

(3) 在窗口中構(gòu)建重投影誤差，采用(2)中的初值進行優(yōu)化，并返回(1)。

4 試 驗

試驗分為水岸線檢測試驗和船舶運動估計試驗兩部分。試驗設(shè)備有：iNav-II型USV和相關(guān)設(shè)備，視覺系統(tǒng)由工業(yè)相機(200 W pixel，3.2 μm像素尺寸)和工業(yè)透鏡(2.8～12.0 mm，1.0∶1.4)組成，捕獲的圖像以30幀/s分辨率記錄，分辨率為1 280×720像素。PC機具有雙核英特爾I5處理器和4 GB存儲器，Win 7系統(tǒng)，Visual C++編輯器等。

iNav-I型USV是我們自主設(shè)計的4.15 m長和1.6 m寬的無人艇見圖3。該USV采用單吊艙推進器，偏轉(zhuǎn)角限制在-25°～25°。整個船舶動力和設(shè)備動力由兩組48伏鋰電池提供。USV系統(tǒng)由兩部分組成：USV子系統(tǒng)和岸基支撐系統(tǒng)。USV子系統(tǒng)主要包括差分GPS移動臺、慣性導(dǎo)航、電子羅盤、船載無線電、風(fēng)速風(fēng)向儀、應(yīng)急操縱裝置和控制計算機。該子系統(tǒng)使用差分GPS移動臺的每秒脈沖(Pulse Per Second,PPS)來實現(xiàn)精確定時。船載系統(tǒng)中各傳感器和設(shè)備的數(shù)據(jù)通過MavLink協(xié)議進行封裝，然后通過交換機和無線電臺進行傳輸和通信。岸基支持系統(tǒng)主要包括岸基無線電臺、岸基差分基站和岸基地面站等3個部分，前兩個對應(yīng)于USV子系統(tǒng)，岸基地面站負責(zé)船舶運行狀態(tài)數(shù)據(jù)的顯示和存儲以及控制命令的發(fā)布。

圖3 iNaV-I型USV

選擇3個位點作為試驗場所。地點1是中國第二大城市湖泊武漢東湖水域，場地水域?qū)掗煏惩o阻。地點2和地點3分別在武漢長江大橋附近和二橋附近。由于長江的嚴(yán)格管理，USV被禁止，因此，視頻是在渡船上使用視覺系統(tǒng)采集的，差分全球定位系統(tǒng)(Differential Global Position System,DGPS)信息通過基準(zhǔn)站R6和移動站R93T組合得到，可達到厘米級精度。

4.1 水岸線檢測試驗

為驗證本文水岸線檢測算法的有效性和實用性，采取實船驗證的方式：第一列為利用iNav-I型USV在東湖獲取的水岸線圖像，試驗中將攝像機傳感器放置在無人艇上，其方向和無人艇的船頭方向保持一致；第二列和第三列分別是通過視覺傳感器在輪渡上采集的，地點分別為武漢長江大橋附近和二橋附近。水岸線檢測結(jié)果見圖4。

圖4中:圖4a、圖4b和圖4c為采集的原始圖像;圖4d、圖4e和圖4f為邊緣檢測后的圖像;圖4g、圖4h和圖4i為提取線段后的圖像放在原圖中的效果;圖4j、圖4k和圖4l為更加清晰顯示的二值圖像;圖4m、圖4n和圖4o為直線擬合后的結(jié)果。由圖4可知：本文的水岸線檢測算法基本可滿足要求，也為后續(xù)位姿估計提供較好的條件。

a) 東湖原始圖像

b) 一橋原始圖像

c) 二橋原始圖像

圖4 水岸線檢測結(jié)果

4.2 運動估計試驗

本文研究的最終目的是針對內(nèi)河領(lǐng)域，估計船舶在靠泊過程中的運動軌跡。所提出的VO系統(tǒng)利用OpenCV SDK3等庫在Visual C++2015中實現(xiàn)。試驗是在中華路碼頭到武漢港碼頭的渡船上，將視覺傳感器放置渡船側(cè)面，采集岸邊視頻數(shù)據(jù)。輪渡的航行軌跡和采集的圖像見圖5，水面上的路徑是渡船的軌跡，黑白圖像是對應(yīng)點捕獲的圖像。

圖5 輪渡的航行軌跡和采集的圖像

系統(tǒng)運行圖見圖6，右上角為實時的船舶運動估計和三維場景點，右下角為船舶軌跡。針對本實驗，特征點個數(shù)和L0、L1、L3分別取200、180、160?；瑒哟翱谥械拇翱诖笮和最新關(guān)鍵幀N分別取7和3。

圖6 系統(tǒng)運行圖

估計軌跡和實際軌跡的對比見圖7。圖7中顯示的是估計的船舶航跡節(jié)點(小方框)和真實數(shù)據(jù)(Real Time Kinematic,RTK)值(實線)的對比。由圖7可知:在近80 m范圍之內(nèi)精度很高，本文算法可正常運行，目測精度良好，在超過該距離(圖7中的小叉點)后，船舶已經(jīng)轉(zhuǎn)向，相機對著開闊水域，算法無法獲得精確估計，誤差已逐漸變大。這也是后續(xù)需要研究的方面。本試驗初始化耗時3 s，而ORB-SLAM無法完成初始化。其中，本文算法沒有尺度信息，80 m是通過RTK與算法估計軌跡對比得到的，在這段期間，相機采集1 418幀，有26個關(guān)鍵幀，特征點平均生存6.5個關(guān)鍵幀，這也從另一方面說明滑動窗口取7比較適合。

圖7 估計軌跡和實際軌跡的對比

5 結(jié)束語

本文針對船舶在靠泊過程中自身軌跡估計和周圍環(huán)境重建的需求，提出一種基于水面區(qū)域去除的船舶航跡估計方法。通過現(xiàn)場試驗以及與RTK數(shù)據(jù)的對比結(jié)果表明，該方法切實可行，但是針對船舶具體靠泊會涉及轉(zhuǎn)向，而單目普通相機無論是視野還是尺度都無法滿足這種需求，這也是下一步研究的方向。