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

23

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

0 引言

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

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

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

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

1 雙目視覺測距

1.1 雙目測距原理

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

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

圖2 極線約束原理

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

1.2 雙目攝像頭參數(shù)標(biāo)定

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

本文采用傳統(tǒng)的棋盤作為標(biāo)定板，規(guī)格為5×7方格，棋格尺寸間距為4.2cm×4.2cm，通過左右攝像機(jī)(間距8厘米)采集圖像對。雙目攝像機(jī)內(nèi)部參數(shù)矩陣標(biāo)定結(jié)果如表1所示。

表1 內(nèi)部參數(shù)標(biāo)定結(jié)果

右攝像機(jī)相對于左攝像機(jī)的平移外部參數(shù)矩陣標(biāo)定結(jié)果如表2所示。

表2 外部參數(shù)標(biāo)定結(jié)果

由以上兩表中數(shù)據(jù)可知，攝像機(jī)的旋轉(zhuǎn)矩陣接近于單位矩陣，所以攝像機(jī)的位置較為理想。

1.3 立體匹配與測距

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

根據(jù)視差圖上的深度信息及攝像機(jī)內(nèi)參即可計(jì)算任一像素點(diǎn)與攝像機(jī)間的距離。

2 雙目直接稀疏里程計(jì)

2.1 雙目DSO系統(tǒng)

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

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

圖3 系統(tǒng)概述

2.2 直接圖像對齊

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

(1)

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

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

(2)

2.3 幀管理與窗口優(yōu)化

每當(dāng)有新的雙目幀時，系統(tǒng)使用直接圖像對齊跟蹤它和活動窗口內(nèi)的最新關(guān)鍵幀?；顒哟翱趦?nèi)所有的點(diǎn)都被投影到新幀，在深度值保持固定的情況下，通過最小化能量函數(shù)計(jì)算新幀的位姿[14]。

為了構(gòu)造新的關(guān)鍵幀，需要從圖像上選擇一系列的稀疏點(diǎn)，稱為候選點(diǎn)。當(dāng)時間較長的點(diǎn)從活動窗口邊緣化時，候選點(diǎn)會被激活并加入聯(lián)合優(yōu)化。每一個激活點(diǎn)存儲在一個關(guān)鍵幀中并且可以被活動窗口內(nèi)的其它一些關(guān)鍵幀觀測。每當(dāng)一個激活點(diǎn)被另外的關(guān)鍵幀觀測到時，就會創(chuàng)造光度損失函數(shù)內(nèi)部的光度能量因子，公式如下：

(3)

將所有的因素聯(lián)合起來，在窗口優(yōu)化中最小化的最終能量函數(shù)為：

(4)

式中，F(xiàn)是當(dāng)前窗口的關(guān)鍵幀集，obs(p)是F中的可觀測到p的關(guān)鍵幀。

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

3 系統(tǒng)功能優(yōu)化

3.1 三維點(diǎn)云過濾

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

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

3.2 生成二維網(wǎng)格圖

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

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

4 實(shí)驗(yàn)結(jié)果與分析

4.1 實(shí)驗(yàn)結(jié)果

圖5為無人船在河上行駛時某一時刻的左右攝像機(jī)拍攝的圖像以及此時刻的關(guān)鍵幀選點(diǎn)圖。

圖5 左右攝像機(jī)拍攝圖像(左圖)和關(guān)鍵幀選點(diǎn)(右圖)

以圖中拍攝到的橋?yàn)槔?，此時無人船距離橋較遠(yuǎn)，為23 m，系統(tǒng)未完整構(gòu)建橋的空間模型，如圖6所示。

圖7為無人船繼續(xù)向前行駛后左右攝像機(jī)拍攝的圖像以及此時刻的關(guān)鍵幀選點(diǎn)圖，由圖可知此時與橋的距離縮短，為8.5 m。

圖7 左右攝像機(jī)拍攝圖像(左圖)和關(guān)鍵幀選點(diǎn)(右圖)

如圖8所示，此時系統(tǒng)基本構(gòu)建完成橋的三維模型，并估計(jì)出了這段距離船的行駛軌跡。

圖8 三維重建點(diǎn)云圖(2)

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

圖9 二維網(wǎng)格地圖(1)

二維地圖中船體行進(jìn)方向?yàn)橛勺笾劣?，圖中二維數(shù)組中1用紅色標(biāo)記，表示有障礙物。建圖是動態(tài)的且只收集關(guān)鍵幀所有活動點(diǎn)的信息，而不是每次都計(jì)算整張地圖，這意味著數(shù)組將在每一組關(guān)鍵幀插入之后輸出和刷新，如圖10即為圖9下一組關(guān)鍵幀輸出的二維地圖。

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

圖10 二維網(wǎng)格地圖(2)

4.2 實(shí)驗(yàn)分析

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

5 結(jié)束語

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

在未來的研究中，可以將重點(diǎn)放在優(yōu)化從圖像中選擇關(guān)鍵點(diǎn)的算法，通過光度校準(zhǔn)來減少來自不同光照強(qiáng)度下的動態(tài)物體和預(yù)處理圖像。