基于單目視覺里程計(jì)的移動(dòng)機(jī)器人自定位方法

雷越，鄧斌，何沛恒，徐新，左榮

基于單目視覺里程計(jì)的移動(dòng)機(jī)器人自定位方法

雷越，鄧斌，何沛恒，徐新，左榮

（西南交通大學(xué) 先進(jìn)驅(qū)動(dòng)節(jié)能技術(shù)教育部工程研究中心，四川 成都 610031）

為解決在列車底部直線移動(dòng)的機(jī)器人對(duì)風(fēng)管進(jìn)行識(shí)別定位摘解時(shí)需到達(dá)兩節(jié)車廂之間的自定位問題，提出一種基于單目視覺里程計(jì)的自定位方法。該方法根據(jù)單目相機(jī)在列車底部豎直向上拍攝時(shí)圖像顏色較深，而當(dāng)相機(jī)將要到達(dá)兩節(jié)車廂之間時(shí)所拍攝圖像中會(huì)有明顯亮度變化區(qū)域的特點(diǎn)，選取關(guān)鍵幀圖像，通過圖像處理提取出圖像中目標(biāo)區(qū)域信息，根據(jù)所提出的運(yùn)動(dòng)估計(jì)算法估計(jì)相機(jī)自身的運(yùn)動(dòng)。最后，通過實(shí)驗(yàn)計(jì)算得到該方法的絕對(duì)誤差為0.059 s，相對(duì)誤差為1.16%，驗(yàn)證了該方法的可行性。

移動(dòng)機(jī)器人；單目視覺里程計(jì)；關(guān)鍵幀選取；圖像處理；運(yùn)動(dòng)估計(jì)

目前，列車風(fēng)管的摘解均采用人工方式操作，勞動(dòng)環(huán)境差、摘解效率低、危險(xiǎn)系數(shù)高。利用搭載視覺系統(tǒng)的移動(dòng)機(jī)器人對(duì)風(fēng)管進(jìn)行識(shí)別定位摘解能夠有效解決人工操作問題，而其中對(duì)在列車底部直線移動(dòng)的機(jī)器人進(jìn)行定位，使其能停在兩節(jié)車廂之間是移動(dòng)機(jī)器人實(shí)現(xiàn)風(fēng)管識(shí)別定位摘解的重要條件。

近年來，基于視覺里程計(jì)（Visual Odometry，VO）的定位技術(shù)已成為機(jī)器人導(dǎo)航領(lǐng)域的重要研究熱點(diǎn)[1-3]。視覺里程計(jì)利用視覺傳感器采集圖像，通過一系列算法來估計(jì)自身的運(yùn)動(dòng)軌跡，具有結(jié)構(gòu)簡單、信息量大、適應(yīng)性強(qiáng)等優(yōu)點(diǎn)[4]。按照系統(tǒng)所使用的相機(jī)個(gè)數(shù)可將視覺里程計(jì)分為單目視覺里程計(jì)和立體視覺里程計(jì)[5]。立體視覺里程計(jì)一般采用兩個(gè)或兩個(gè)以上相機(jī)作為載體獲取圖像，可通過對(duì)級(jí)約束關(guān)系和三角測量原理得到深度信息，重建空間結(jié)構(gòu)，但相機(jī)間標(biāo)定復(fù)雜，系統(tǒng)不穩(wěn)定，計(jì)算量大，實(shí)時(shí)性低且成本昂貴；單目視覺里程計(jì)采用單個(gè)相機(jī)為載體獲取圖像，雖難以獲得深度信息，但具備成本低廉、安裝方便、計(jì)算量小、實(shí)時(shí)性高的特點(diǎn)[6-7]。

在現(xiàn)階段VO領(lǐng)域，關(guān)于利用單目視覺里程計(jì)來解決實(shí)際問題的研究越來越多。文獻(xiàn)[8]為解決移動(dòng)機(jī)器人的避障問題，提出一種可實(shí)時(shí)獲取移動(dòng)機(jī)器人位姿的單目視覺里程計(jì)算法，根據(jù)實(shí)驗(yàn)結(jié)果表明該算法實(shí)時(shí)性高，但誤差仍較大，還需進(jìn)一步改進(jìn)。文獻(xiàn)[9]為解決在相機(jī)快速運(yùn)動(dòng)時(shí)單目視覺里程計(jì)精度差和不穩(wěn)定的問題，提出一種基于樹結(jié)構(gòu)的特征匹配和實(shí)時(shí)地圖更新的單目視覺里程計(jì)算法，通過公開數(shù)據(jù)集對(duì)算法進(jìn)行對(duì)比驗(yàn)證，結(jié)果表明該算法具有不錯(cuò)精度和魯棒性，但存在同一幀圖像曝光時(shí)刻不同產(chǎn)生較大誤差的問題。

本文針對(duì)移動(dòng)機(jī)器人在列車底部行走的特點(diǎn)，提出一種基于單目視覺里程計(jì)的自定位方法，其流程如下，首先通過問題描述，建立相機(jī)運(yùn)動(dòng)模型，其次在對(duì)單目相機(jī)進(jìn)行標(biāo)定矯正后，對(duì)單目相機(jī)所采集的圖像進(jìn)行一定手段的處理，提取出目標(biāo)區(qū)域信息，然后通過理論分析，基于VO原理提出相機(jī)運(yùn)動(dòng)估計(jì)算法，最后搭建簡易實(shí)驗(yàn)平臺(tái)驗(yàn)證該方法的的精度和可行性。

1 問題描述

在列車底部直線移動(dòng)的機(jī)器人，如圖1所示，從鐵軌端頭進(jìn)入兩節(jié)鐵軌之間，在列車車廂底部直線行走，當(dāng)移動(dòng)機(jī)器人到達(dá)兩節(jié)車廂之間時(shí)停留，對(duì)風(fēng)管進(jìn)行識(shí)別定位摘解，完成后移動(dòng)至下兩節(jié)車廂繼續(xù)摘解，直至摘解完成。

圖1 移動(dòng)機(jī)器人模型

針對(duì)移動(dòng)機(jī)器人為到達(dá)兩節(jié)車廂之間需要對(duì)其運(yùn)動(dòng)進(jìn)行定位使其能在兩節(jié)車廂之間停留的問題，建立如圖2所示的相機(jī)運(yùn)動(dòng)模型。將單目相機(jī)水平放置，垂直向上方采集圖像。其中相機(jī)到車廂底部距離為1，到鐵軌枕木距離為2，相機(jī)焦距為，相機(jī)視場角為，相機(jī)運(yùn)動(dòng)速度為，兩節(jié)車廂間距為，相機(jī)通過設(shè)定相應(yīng)幀數(shù)，在不斷水平移動(dòng)中進(jìn)行圖像采集，當(dāng)相機(jī)在列車底部時(shí)，所拍攝照片整體背景較深，而當(dāng)相機(jī)將要移動(dòng)到車廂之間時(shí)，所采集的圖像會(huì)存在明顯的亮度區(qū)域變化，因此通過對(duì)圖像進(jìn)行一定處理計(jì)算即可對(duì)相機(jī)的運(yùn)動(dòng)進(jìn)行估計(jì)，實(shí)現(xiàn)移動(dòng)機(jī)器人自身的定位。

圖2 相機(jī)運(yùn)動(dòng)模型

2 相機(jī)標(biāo)定及圖像處理

2.1 單目相機(jī)模型

首先建立相機(jī)投影模型如圖3，其中為相機(jī)坐標(biāo)系，為像素坐標(biāo)系，為相機(jī)光心，其中點(diǎn)像素坐標(biāo)為（1,2），設(shè)為現(xiàn)實(shí)世界中的三維點(diǎn)，在成像平面上的投影為，其坐標(biāo)為[,]T，其在世界坐標(biāo)系下的坐標(biāo)為[X,Y,Z]T。通過世界坐標(biāo)系與相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系，以及圖像坐標(biāo)系與相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系，結(jié)合投影透視模型可得到世界坐標(biāo)系與圖像坐標(biāo)系轉(zhuǎn)換方程式為：

式中：ff和c、c分別為方向的焦距和偏移量；、分別為旋轉(zhuǎn)矩陣和平移向量；1為相機(jī)內(nèi)部的參數(shù)矩陣；2為相機(jī)外部的參數(shù)矩陣。

其中相機(jī)內(nèi)部參數(shù)只與相機(jī)內(nèi)部結(jié)構(gòu)有關(guān)，相機(jī)外部參數(shù)與相機(jī)所在位置有關(guān)，相機(jī)在不同位置拍攝時(shí)的外部參數(shù)都不相同。通過相機(jī)標(biāo)定得到相機(jī)內(nèi)外參數(shù)具體數(shù)值后，便可利用相機(jī)內(nèi)部參數(shù)和外部參數(shù)來確定空間物體上某一點(diǎn)的三維坐標(biāo)與在圖像上對(duì)應(yīng)點(diǎn)之間的相互關(guān)系。

實(shí)際情況中，由于鏡頭制作工藝等原因，攝像機(jī)鏡頭會(huì)產(chǎn)生徑向和切向畸變[10]，導(dǎo)致所采集的圖像失真，其中切向畸變產(chǎn)生于相機(jī)組裝過程等，徑向畸變產(chǎn)生于透鏡形狀等。因此通過相機(jī)標(biāo)定得到內(nèi)參系數(shù)后，使用內(nèi)參系數(shù)對(duì)圖像進(jìn)行矯正，可提高后續(xù)對(duì)圖像進(jìn)行處理時(shí)的精度，減小誤差，也是做好后續(xù)工作的重要環(huán)節(jié)和前提。

圖3 相機(jī)透視投影模型

2.2 單目相機(jī)標(biāo)定及矯正

為了得到相機(jī)的內(nèi)部參數(shù)矩陣?yán)肕atlab工具箱對(duì)相機(jī)進(jìn)行標(biāo)定。單目相機(jī)采集的不同位置標(biāo)定板棋盤格圖像數(shù)量為20張，部分如圖4所示，棋盤格規(guī)格為9×10，正方形邊長為10 mm。

圖4 棋盤格采集圖像

通過系統(tǒng)計(jì)算得到標(biāo)定結(jié)果，相機(jī)的內(nèi)部參數(shù)矩陣為：

相機(jī)徑向畸變系數(shù)1、2、3為[0.0663,-0.0805,0]；

相機(jī)切向畸變系數(shù)1、2為[0,0]。

得到相機(jī)內(nèi)部參數(shù)后，將各參數(shù)代入opencv圖像矯正算法中進(jìn)行圖像矯正，處理結(jié)果如圖5所示。

圖5 圖像矯正

2.3 圖像處理

為后續(xù)工作的順利展開，在對(duì)該單目相機(jī)所采集的圖像進(jìn)行矯正后，根據(jù)本方法的思路，需要對(duì)采集的圖片進(jìn)行一定處理，處理流程如圖6所示。

圖6 圖像處理流程圖

首先將所采集的圖像進(jìn)行預(yù)處理，其中包括將圖像灰度化和圖像灰度直方圖獲取以及全局閾值分割和開運(yùn)算，完成預(yù)處理后對(duì)對(duì)目標(biāo)進(jìn)行信息提取處理，得到所需參數(shù)進(jìn)行計(jì)算。

利用全局即整個(gè)圖像的信息，如整個(gè)圖像的灰度直方圖，在整個(gè)圖像中只使用一個(gè)閾值對(duì)圖像進(jìn)行分割，可以將整個(gè)圖像分割成兩個(gè)區(qū)域，包括目標(biāo)區(qū)域和背景部分。對(duì)于物體和背景對(duì)比較為明顯的圖像使用全局閾值法能很好的對(duì)圖像進(jìn)行分割。具體操作為將所采集的圖像進(jìn)行灰度化處理，利用Matlab等軟件繪制出灰度直方圖，選定直方圖中波谷處即最低點(diǎn)的灰度值作為全域閾值分割值，對(duì)圖像進(jìn)行全局閾值分割，可以將目標(biāo)區(qū)域和背景區(qū)域進(jìn)行分離。

通過形態(tài)學(xué)中的腐蝕與膨脹組合運(yùn)算可以對(duì)二值圖像進(jìn)行減噪處理，本方法采用形態(tài)學(xué)開運(yùn)算即先腐蝕后膨脹運(yùn)算，消除圖像上細(xì)小的噪聲，平滑物體的邊界，減小計(jì)算的誤差。

最后通過連通區(qū)域獲取算法找到目標(biāo)區(qū)域，繪制感興趣區(qū)域，并用紅框標(biāo)記，通過面積提取算法提取出目標(biāo)區(qū)域的面積、邊長等信息作后續(xù)計(jì)算，其中邊長通過面積除以圖像寬度像素得到。

取一張?jiān)趯?shí)驗(yàn)現(xiàn)場所拍攝的圖片作為示例，首先對(duì)其進(jìn)行預(yù)處理，將其灰度化后得到灰度直方圖，根據(jù)灰度直方圖取得全局閾值為240，利用全局閾值分割法將圖像背景與目標(biāo)區(qū)域分割開，并顯示出所求目標(biāo)區(qū)域，預(yù)處理后的結(jié)果圖如圖7，再經(jīng)過上述所說算法進(jìn)行一定處理提取出目標(biāo)區(qū)域信息，如圖8，通過算法得到目標(biāo)區(qū)域面積，通過計(jì)算獲得其邊長長度，為后續(xù)計(jì)算做準(zhǔn)備。

圖7 預(yù)處理結(jié)果

圖8 目標(biāo)區(qū)域提取結(jié)果

3 運(yùn)動(dòng)估計(jì)理論分析

3.1 VO原理

相機(jī)在運(yùn)動(dòng)中以固定幀率采集圖像時(shí)，如圖9所示，設(shè)其在-1時(shí)刻和時(shí)刻所采集的圖像分別為I-1和I。為現(xiàn)實(shí)世界中的三維點(diǎn)，在I-1、I中的成像點(diǎn)為p-1、p。通過這兩點(diǎn)的坐標(biāo)以及投影方程可得到相機(jī)在-1時(shí)刻到時(shí)刻的位姿變換矩陣T,t-1。其中T,t-1∈4*4，該矩陣可表示為：

式中：R,t-1為相機(jī)的旋轉(zhuǎn)矩陣；Q,t-1為相機(jī)的平移矩陣。

將所得一系列圖像進(jìn)行計(jì)算估計(jì)得到變換矩陣，通過集成法集成所有矩陣可得到相機(jī)的全部運(yùn)動(dòng)軌跡。這也意味著VO是通過增量式一個(gè)位姿接著一個(gè)位姿重建軌跡。

圖9 幀間運(yùn)動(dòng)估計(jì)原理圖

3.2 運(yùn)動(dòng)估計(jì)算法

由于視覺里程計(jì)是通過增量式方法重建運(yùn)動(dòng)軌跡，所以會(huì)產(chǎn)生累積誤差，需要對(duì)其進(jìn)行優(yōu)化得到更加精確的運(yùn)動(dòng)軌跡[11]，這也是視覺里程計(jì)在精確性方面面臨的主要問題。而本文基于單目視覺里程計(jì)的原理提出的新方法只需要對(duì)相機(jī)所拍攝的關(guān)鍵三至四幀圖片進(jìn)行處理并估計(jì)其運(yùn)動(dòng)軌跡，因此累計(jì)誤差很小。

通過之前的相關(guān)工作，對(duì)相機(jī)所采集的圖片進(jìn)行關(guān)鍵幀選取并進(jìn)行圖像處理完成后，根據(jù)圖10所示，找到經(jīng)處理后剛出現(xiàn)白色區(qū)域的圖片設(shè)為1，前一幀圖片設(shè)為0，后一幀圖片設(shè)為2，然后依次設(shè)至，如圖11所示。單目相機(jī)在幀間運(yùn)動(dòng)時(shí)，相機(jī)在每幀時(shí)間內(nèi)移動(dòng)距離雖相等，但根據(jù)相機(jī)投影成像特性有1＞2，導(dǎo)致白色區(qū)域面積并不是線性增加，因此需要采用一種算法來進(jìn)行運(yùn)動(dòng)估計(jì)?；谶@一特點(diǎn)，為通過反推計(jì)算在1時(shí)刻白色區(qū)域邊長長度1在總區(qū)域長度0即一幀中所占用的時(shí)間，如圖12，提出一種推導(dǎo)算法。

圖10 相機(jī)關(guān)鍵幀選取

圖11 幀間圖像原理圖

圖12 I1幀反推原理圖

其步驟如下：

（1）首先對(duì)前幀的圖像進(jìn)行處理并通過計(jì)算得到1,2,3, ...,x，取值為3～4即可；

（2）根據(jù)公式Δx-1＝x－x1計(jì)算一階差分，得到Δ1, Δ2, Δ, ..., Δx；

（3）根據(jù)Δx的值列出一階差分表格，取得合理值Δ0；

（4）在1這一幀內(nèi)采用線性處理方法，白色區(qū)域在總區(qū)域長度中所占用的時(shí)間為：

式中：為相機(jī)一幀所用時(shí)間，s。

（5）得到后，可得：

4 實(shí)驗(yàn)驗(yàn)證

4.1 簡易實(shí)驗(yàn)平臺(tái)搭建

為進(jìn)行實(shí)驗(yàn)驗(yàn)證，根據(jù)移動(dòng)機(jī)器人自身移動(dòng)的特點(diǎn)，搭建如圖13所示的簡易實(shí)驗(yàn)平臺(tái)，其中基座用于模擬移動(dòng)機(jī)器人高度，電機(jī)絲杠組合模擬移動(dòng)機(jī)器人的水平直線移動(dòng)，圖像采集程序如圖14，通過電腦控制單目相機(jī)進(jìn)行圖像采集，采集格式為數(shù)據(jù)流視頻格式，顏色空間為YUY2，輸出大小為640×480默認(rèn)值。

圖13 實(shí)驗(yàn)平臺(tái)搭建圖

圖14 圖像采集程序

通過測量現(xiàn)場其余具體數(shù)據(jù)，有相機(jī)到車廂底部距離1為610 mm，相機(jī)到鐵軌枕木距離2為240 mm，兩節(jié)車廂間距為1000 mm。相機(jī)成像角度為46°，相機(jī)焦距為4.3 mm，幀數(shù)為30，絲杠運(yùn)動(dòng)速度為0.15 m/s。

為驗(yàn)證該方法的精度和可行性，將相機(jī)在0處距離車廂外側(cè)的水平距離設(shè)為定值，這里設(shè)置為270 mm，直接讓相機(jī)從0出發(fā)，可得：

4.2 圖像處理和誤差計(jì)算

在系統(tǒng)采集完圖像后，如圖15所示，對(duì)這些圖像進(jìn)行圖像處理，首先找到經(jīng)處理后首次出現(xiàn)白色區(qū)域的圖像1，根據(jù)運(yùn)動(dòng)估計(jì)算法，取的值為3，然后提取出1前一幀圖像0以及1后的兩幀圖片分別命名為2、3，經(jīng)過圖像處理提取出目標(biāo)區(qū)域，計(jì)算其面積并獲得其邊長，提取結(jié)果圖如圖16所示。

圖15 采集原圖

在計(jì)算得到各幀目標(biāo)區(qū)域邊長后，建立一階差分表，如表1，根據(jù)表中數(shù)據(jù)取Δ0為33.8000 p，按照式（3）計(jì)算出＝0.0143 s，然后根據(jù)式（4）得到＝5.0738 s，因此可得到該方法的絕對(duì)誤差為|t－|＝0.059 s，由下式得：

（a）0（b）I

（c）I2 （d）I3

表1 一階差分表

5 結(jié)論

針對(duì)在列車底部直線移動(dòng)機(jī)器人的自定位問題，基于單目視覺里程計(jì)的原理，提出了一種解決移動(dòng)機(jī)器人自定位的新方法。本文通過建立相機(jī)運(yùn)動(dòng)模型，對(duì)單目相機(jī)進(jìn)行標(biāo)定矯正后，提出對(duì)所采集的圖像的處理方法，通過理論分析提出一種運(yùn)動(dòng)估計(jì)算法，最后通過搭建簡易實(shí)驗(yàn)平臺(tái)對(duì)該方法的精度和可行性進(jìn)行驗(yàn)證。根據(jù)實(shí)驗(yàn)結(jié)果計(jì)算出該方法的絕對(duì)誤差為0.059 s，相對(duì)誤差為1.16%，其中產(chǎn)生誤差的主要原因有：

（1）一階差分法的估計(jì)誤差；

（2）圖像處理誤差；

（3）現(xiàn)場測量數(shù)據(jù)誤差，后續(xù)工作還需繼續(xù)對(duì)其優(yōu)化。

通過實(shí)驗(yàn)結(jié)果表明該方法可行且精度較高，該方法也為其它需要進(jìn)行自定位的移動(dòng)機(jī)器人提供了一種可行參考。

[1]Nister D , Naroditsky O , Bergen J R . Visual odometry[C]. Proceedings of the 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, 2004.

[2]F. Fraundorfer，D. Scaramuzza. Visual Odometry: Part II: Matching, Robustness, Optimization, and Applications[J]. IEEE Robotics & Automation Magazine，2012，19（2）：78-90.

[3]李宇波，朱效洲，盧惠民，等. 視覺里程計(jì)技術(shù)綜述[J]. 計(jì)算機(jī)應(yīng)用研究，2012，29（8）：2801-2805，2810.

[4]Cadena C，Carlone L，Carrillo H，et al. Simultaneous Localization And Mapping: Present， Future， and the Robust-Perception Age[J]. IEEE Transactions on Robotics，2016，32（6）：1309-1332.

[5]R. Mur-Artal，J. D. Tardós. ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras[J]. IEEE Transactions on Robotics，2017（5）：1255-1262.

[6]趙文愷，李剛. 基于雙目相機(jī)的視覺里程計(jì)[J]. 計(jì)算機(jī)工程與設(shè)計(jì)，2020，41（4）：1133-1138.

[7]烏萌，郝金明，高揚(yáng)，等. 一種場景穩(wěn)健的單目視覺里程計(jì)算法[J]. 測繪科學(xué)技術(shù)學(xué)報(bào)，2019，36（4）：364-370.

[8]趙黎明，陳寧. 基于平面單應(yīng)性的單目視覺里程計(jì)設(shè)計(jì)[J]. 計(jì)算機(jī)工程與應(yīng)用，2016，52（17）：243-246，252.

[9]龐智華，齊臣坤. 一種魯棒的單目視覺里程計(jì)算法[J]. 計(jì)算機(jī)工程，2020，46（12）：80-87.

[10]汪佳麗，王煒杰，馬振玲. 相機(jī)畸變的混合模型迭代檢校法[J].測繪通報(bào)，2019（4）：103-106.

[11]祝朝政，何明，楊晟，等. 單目視覺里程計(jì)研究綜述[J]. 計(jì)算機(jī)工程與應(yīng)用，2018，54（7）：20-28，55.

Self-Positioning Method of Mobile Robot Based on Monocular Visual Odometer

LEI Yue，DENG Bin，HE Peiheng，XU Xin，ZUO Rong

( Engineering Research Center of Advanced Drive Energy Saving Technologies, Ministry of Education, Southwest Jiaotong University, Chengdu610031, China )

In order to solve the self-positioning problem that the robot moving linearly at the bottom of the train is expected to reach the middle of two carriages when identifying, locating and disassembling the air duct, a self-positioning method based on monocular visual odometer is proposed. On the basis of the feature that the color of the image is darker when the monocular camera shoots vertically upwards from the bottom of the train, and when the camera is about to reach the middle of the two carriages, there will be obvious brightness changes in the image, the key frame image is selected, and the target area information in the image is extracted through image processing, and the camera's own motion is estimated according to the proposed motion estimation algorithm. It turns out that the absolute error of the method is 0.059s and the relative error is 1.16%, which verifies the feasibility of the method.

mobile robot；monocular visual odometer；key frame selection；image processing；motion estimation

TP24

A

10.3969/j.issn.1006-0316.2021.04.009

1006－0316 (2021) 04－0055－07

2020－10－29

雷越（1997－），男，四川成都人，碩士研究生，主要研究方向?yàn)闄C(jī)器視覺、機(jī)械結(jié)構(gòu)設(shè)計(jì)，E-mail：292755756@qq.com；鄧斌（1964－），男，湖北荊門人，博士，教授，主要研究方向?yàn)闄C(jī)電液一體化。