基于雙目視覺的無人船障礙物探測與地圖構建

23

(1.南通大學 計算機科學與技術學院，江蘇 南通 226019；2.南通先進通信技術研究院，江蘇 南通 226019;3.西交利物浦大學 計算機科學與軟件工程系，江蘇 蘇州 215123)

0 引言

在智能化水質監測任務中，無人船扮演著重要角色[1-2]。無人水質監測船在其自主航行作業時需要自動巡航并自主避障，周圍環境感知是先決條件，因此智能化識別周圍環境是實現避障的關鍵技術[3]。由于現有無人水質監測船一般采用超聲波避障，其只能探測船體前方障礙物，且探測距離和精度較低、避障功能弱，而雷達系統在近距離內又存在盲區，因此無人船真正實現自動巡航和自主避障需要引進視覺系統[4-6]。

如今，實時視覺測距(VisualOdometry，VO)和視覺實時定位與地圖構建(Visual Simultaneous Localization And Mapping，VSLAM)已成為熱門的研究課題。第一個實時VSLAM出現是在2000年左右，其核心思想是選擇一系列的關鍵點，跟蹤它們在幀間的運動，聯合推斷它們的3D位置和相機運動。其后，出現了性能不斷提升的解決方案，包括計算硬件和算法理念的升級。例如當前主流前沿算法ORB-SLAM 算法，其能夠在小尺度和大尺度、室內和室外環境實時完成位姿確定的同時構建稀疏特征點地圖，執行寬基線的閉環檢測和重定位，并且能夠實現全自動的初始化[7]。

傳統的SLAM算法是基于嘗試選擇關鍵點的，而后出現的直接法旨在直接使用圖像計算幾何結構和運動，跳過中間的關鍵點選取步驟。例如大規模單目直接法LSD-SLAM，其核心思想是跟蹤相機增量運動，同時執行位姿圖優化以保持軌跡的全局一致性[8]。SVO屬于半直接法，僅在前端的部分使用了直接法，之后的位姿估計等則仍舊使用傳統的最小化重投影誤差的方式[9]。DSO是少數使用純直接法計算視覺里程計的系統之一，其無論是在精確度還是穩健性上都超過了基于特征方法的ORB-SLAM。但DSO等直接方法在沒有光度校準的情況下性能會明顯下降，單目DSO存在無法估計重建場景的尺度和相機運動的單位、估計的軌跡有較大的尺度漂移等問題[10]。

本文采用的雙目DSO算法相較于單目DSO，通過引入額外的傳感器信息優化上述缺點，提供了更精確并且更快收斂的尺度估計，對卷簾曝光或沒有光度標定敏感度降低[14]。當前該算法多運用于陸地環境，本文將其運用于水面建圖，并針對水上環境的特殊情況(如光強不斷變化、存在較多反光等)對算法功能進行了優化，實現了更適用于水上環境的地圖構建方法。

1 雙目視覺測距

1.1 雙目測距原理

如圖1所示，設點P沿垂直于相機中心的直線上下移動，則其在左右相機上成像點的位置將隨之不斷改變，即視差的大小會不斷改變。點P與相機間的距離Z和視差d之間存在反比關系，視差d可通過左右相機的中心距T減去點P在左右相機上的投影點到各自中心點的距離值獲得。因此，只要獲得左右相機的中心距T，即可估計出點P到相機的距離。

上文所述有個前提，就是在左右相機的成像上要定位到同一點P，即要把左右兩個圖像上的點進行匹配，這就需要雙目校正的操作。假如根據左圖像上一個點的特征到右圖像上匹配對應點，該過程將非常耗時。因此利用極限約束來減少搜索匹配的運算量。如圖2所示，空間中任意一點在圖像平面上的投影點，必然處于由該點和兩個相機中心組成的對極平面上。對于圖像上的某一特征點，其在另一視圖上的匹配點必處于對應的極線上，稱為極線約束原理[11]。極線約束使得只需在一維空間中搜索匹配對應點，從而顯著提升了運算效率并減少了誤匹配。

圖2 極線約束原理

此時用雙目校正把左右兩幅圖像在水平方向上嚴格對齊，使得左右圖像的對極線在同一水平線上。根據極線約束原理，此時一幅圖像上任意特征點與其在另一圖像上的匹配點必然在同一行上，因此只需要在該行搜索即可匹配到對應點。

1.2 雙目攝像頭參數標定

雙目標定的第一步是分別獲取左右相機的內外參數，然后通過立體標定對左右兩幅圖像進行立體校準和對齊，最后一步是確定兩個相機之間的相對位置關系，即中心距。為了消除畸變以及得到內外參數矩陣，雙目相機需要標定的參數有內參數矩陣、畸變系數矩陣、本征矩陣、基礎矩陣、旋轉矩陣和平移矩陣。內參數矩陣與焦距相關，內參數矩陣得到鏡頭的信息，并消除畸變，使得到的圖像更為準確，它是一個從平面到像素的轉換。為了最終的測距，通過外參數矩陣得到相機相對于世界坐標的聯系，實現相機坐標系與世界坐標系的轉換，畸變參數一般包含在內參數矩陣中[12]。

本文采用傳統的棋盤作為標定板，規格為5×7方格，棋格尺寸間距為4.2cm×4.2cm，通過左右攝像機(間距8厘米)采集圖像對。雙目攝像機內部參數矩陣標定結果如表1所示。

表1 內部參數標定結果

右攝像機相對于左攝像機的平移外部參數矩陣標定結果如表2所示。

表2 外部參數標定結果

由以上兩表中數據可知，攝像機的旋轉矩陣接近于單位矩陣，所以攝像機的位置較為理想。

1.3 立體匹配與測距

在攝像機標定結束后，開始對拍攝的圖像進行匹配。立體匹配算法計算基準圖像中所有像素的視差值，得到視差圖，從而得到基準圖像的三維信息。由于水面情況復雜(如障礙物特征不明顯，低紋理區較多，水面存在倒影等)，而局部立體匹配對噪聲或特征要求較高，所以采用全局立體匹配算法。

根據視差圖上的深度信息及攝像機內參即可計算任一像素點與攝像機間的距離。

2 雙目直接稀疏里程計

2.1 雙目DSO系統

直接稀疏里程計(Direct Sparse Odometry，DSO)是近年來發展起來的一種新方法，它利用非線性優化算法，通過最小化由直接圖像對齊得到的光度誤差來估計相機的位姿和像素深度[13]。本文運用的是雙目立體擴展的DSO算法實現無人船所處水上環境的實時三維重建。

雙目DSO系統概述如圖3所示。系統使用固定的雙目立體匹配獲得的深度估計。基于直接圖像對齊公式，首先新的雙目幀會對參考關鍵幀進行跟蹤，所得到的位姿估計用于調整和改進最近選定點的深度。然后系統檢查當前活動窗口是否需要插入新的關鍵幀，如果不需要，則將創建一個普通幀，否則將生成一個新的關鍵幀并添加到當前的活動窗口。活動窗口中的所有關鍵幀聯合優化位姿，調整亮度參數以及所有可觀察到的三維點的深度。為了保持活動窗口為固定尺寸，使用Schur方法邊緣化舊的關鍵幀和三維點[14]。

圖3 系統概述

2.2 直接圖像對齊

系統通過直接圖像對齊獲得光度誤差，進而通過最小化光度誤差來估計相機的位姿和像素深度。假設參考幀Ii中的點集Pi在另一幀Ij中被觀察到，則直接圖像對齊的基本思想可表示光度損失能量函數為：

(1)

式中，‖·‖γ是Huber范數，ωp權值會減弱梯度大的地方的影響，p′是p在Ij中的投影。

雙目DSO每幀選取固定數量的點，均勻分布在所有區域并且梯度明顯，然后在每個點周圍的很小鄰域內計算光度誤差。由于光度誤差是直接根據像素強度計算的，對幀間光照變化比較敏感。因此對每幀圖像引入了兩個參數ai和bi建模亮度變化，修改后的光度損失能量函數如下：

(2)

2.3 幀管理與窗口優化

每當有新的雙目幀時，系統使用直接圖像對齊跟蹤它和活動窗口內的最新關鍵幀。活動窗口內所有的點都被投影到新幀，在深度值保持固定的情況下，通過最小化能量函數計算新幀的位姿[14]。

為了構造新的關鍵幀，需要從圖像上選擇一系列的稀疏點，稱為候選點。當時間較長的點從活動窗口邊緣化時，候選點會被激活并加入聯合優化。每一個激活點存儲在一個關鍵幀中并且可以被活動窗口內的其它一些關鍵幀觀測。每當一個激活點被另外的關鍵幀觀測到時，就會創造光度損失函數內部的光度能量因子，公式如下：

(3)

將所有的因素聯合起來，在窗口優化中最小化的最終能量函數為：

(4)

式中，F是當前窗口的關鍵幀集，obs(p)是F中的可觀測到p的關鍵幀。

為了保持活動窗口大小不變，時間較久的幀需要Schur邊緣化。在關鍵幀邊緣化之前，需先將兩個最新的關鍵幀未觀測到的全部激活點邊緣化。

3 系統功能優化

3.1 三維點云過濾

水面建圖的一個重要問題是水面反光的影響。河岸、船只或雜草的倒影會嚴重誤導建圖過程，造成“虛擬障礙”問題，在實驗中會發現有些點出現在河流下方，如圖4所示。為了消除河岸建筑在水面上的倒影的影響，系統在生成點云之前對點進行了過濾。由于相機只能觀察水面，所以所有實際物體的點都應該在水面上。因此，任何具有負y坐標的點都可以確定為反射點。通過比較點云的坐標，可以估算出曲面的y坐標。然后，通過刪除所有相關值較低的點來過濾點云[15]。實際實驗表明該方法有效且不會影響表面障礙物的探測。

圖4 水面倒影造成“虛擬障礙”

3.2 生成二維網格圖

水面建圖的另一個問題是，不同于無人機等運動范圍相對較大的自動巡航設備，無人船僅在固定河道航行，因此可以假設無人船是在二維平面(x，z)上運動的，具有相同(x，z)坐標而y坐標不相同的點對于無人船的障礙意義相同。所以將三維點云圖轉換成二維網格圖可以簡化計算，并且不影響無人船實現避障功能[16]。

對于二維網格圖而言，超出船體高度的點不僅會增加計算量，有時(尤其是穿越橋體時)會對建圖以及標記障礙物產生誤導。因此將高出船體一定距離的空間忽略，僅取船體前方10×10×2的空間，將該空間垂直分割成20×20個的方格，若方格內存在關鍵點，則標記此空間為存在障礙物，最終得到一個20×20的二維數組，存在障礙物的區域標記為1，不存在障礙物的區域標記為0，即為二維網格圖。

4 實驗結果與分析

4.1 實驗結果

圖5為無人船在河上行駛時某一時刻的左右攝像機拍攝的圖像以及此時刻的關鍵幀選點圖。

圖5 左右攝像機拍攝圖像(左圖)和關鍵幀選點(右圖)

以圖中拍攝到的橋為例，此時無人船距離橋較遠，為23 m，系統未完整構建橋的空間模型，如圖6所示。

圖7為無人船繼續向前行駛后左右攝像機拍攝的圖像以及此時刻的關鍵幀選點圖，由圖可知此時與橋的距離縮短，為8.5 m。

圖7 左右攝像機拍攝圖像(左圖)和關鍵幀選點(右圖)

如圖8所示，此時系統基本構建完成橋的三維模型，并估計出了這段距離船的行駛軌跡。

圖8 三維重建點云圖(2)

與圖8相對應，系統輸出此時的二維網格地圖，運用二維數組M存儲二維網格地圖的信息。其中每個元素表示該空間是否被占用，即在相機坐標系下的坐標為(x，z)其對應二維網格中的M(i，j)，示例輸出如圖9所示。

圖9 二維網格地圖(1)

二維地圖中船體行進方向為由左至右，圖中二維數組中1用紅色標記，表示有障礙物。建圖是動態的且只收集關鍵幀所有活動點的信息，而不是每次都計算整張地圖，這意味著數組將在每一組關鍵幀插入之后輸出和刷新，如圖10即為圖9下一組關鍵幀輸出的二維地圖。

由圖9與圖10可以看出，由于預先過濾了超出船體一定高度(本文設定為2 m)的點云，在生成的二維網格圖中，對應橋洞位置的M(7,17)、M(8,17)、M(9,17)的值均為0，并未被標記為障礙物，因此二維地圖在傳輸數據給避障系統后，避障系統會判定此處為可通行，證明了此方法的可行性。

圖10 二維網格地圖(2)

4.2 實驗分析

本文系統對真實河流環境進行重建。通過優化改進算法，過濾點云，解決了水面反光造成的“虛擬障礙”問題，最終生成了準確、魯棒的相關點云，使其適用于水上環境重建。其次根據三維點云圖生成二維網格圖，優化了避障所需的計算量，同時解決了二維建圖所帶來的“障礙誤導”問題，即在壓縮過程中將整個橋體壓縮至平面導致避障系統判定此路不通的情況。最終該系統生成的二維地圖以二維數組的形式傳輸給避障系統用來進行規避障礙物與路徑規劃。實驗結果證明了本文系統的可行性以及穩定性。

5 結束語

本文采用雙目DSO系統，主要用于無人船自動巡航。作為一種可視化的、實時的環境重建方法，其在深度的精確化、光線的魯棒性等方面都具備了較好的性能。在此基礎上，本文優化了系統功能，解決了水上環境構建存在的“虛擬障礙”等問題，并且優化了地圖信息，由三維點云圖生成二維網格圖，簡化了計算量，使其更適用于水上環境的地圖構建，最終為避障系統提供數據信息。

在未來的研究中，可以將重點放在優化從圖像中選擇關鍵點的算法，通過光度校準來減少來自不同光照強度下的動態物體和預處理圖像。