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

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

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

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

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

1 相關工作

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

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

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

2 算法設計

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

圖1 算法框架

2.1 水岸線檢測

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

a) 長江水域

b) 東湖水域

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

2.1.1邊緣檢測

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

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

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

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

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

2.1.2線段提取

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

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

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

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

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

2.1.3直線擬合

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

(1)

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

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

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

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

2.2 運動估計

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

2.2.1特征匹配

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

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

2.2.2位姿估計

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

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

2.2.3局部優化

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

3 稀疏集中調整

3.1 關鍵幀的選擇

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

3.1.1初始關鍵幀的選擇

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

3.1.2關鍵幀的選擇

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

3.2 初始化過程

初始化過程在初始關鍵幀中實現，其步驟如下：

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

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

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

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

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

(2)

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

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

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

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

4 試 驗

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

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

圖3 iNaV-I型USV

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

4.1 水岸線檢測試驗

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

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

a) 東湖原始圖像

b) 一橋原始圖像

c) 二橋原始圖像

圖4 水岸線檢測結果

4.2 運動估計試驗

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

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

系統運行圖見圖6，右上角為實時的船舶運動估計和三維場景點，右下角為船舶軌跡。針對本實驗，特征點個數和L0、L1、L3分別取200、180、160。滑動窗口中的窗口大小M和最新關鍵幀N分別取7和3。

圖6 系統運行圖

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

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

5 結束語

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