雙舵輪AGV視覺里程計的設計方法①

梅瑞冬,張 捷,李 強,劉 超,王書亭

1(華中科技大學 能源與動力工程學院,武漢 430074)

2(華中科技大學 機械科學與工程學院,武漢 430074)

3(廣東省智能機器人研究院,東莞 523000)

隨著電商的崛起和“工業(yè)4.0”的提出,基于AGV(Automated Guided Vehicle,自動巡航小車)的智能物流系統(tǒng)相較于傳統(tǒng)的人工分揀具有精度高,安全可靠,運行成本低廉的優(yōu)勢.并且AGV 小車可以24 小時無間斷地工作,大大提高了倉庫的分揀效率[1].

AGV 按照驅(qū)動方式大概可以分為三種:單舵輪,差動輪,雙舵輪.其中雙舵輪式AGV 具有非常高的靈活性,可做出原地旋轉(zhuǎn),平移,側(cè)移等全向的移動.底盤模型如圖1所示.

雙舵輪的AGV 小車具有良好的發(fā)展前景,是目前研究的熱點,但是因為使用兩個舵輪,其運動模型復雜且導航定位精度不高.雙舵輪AGV 小車還不能大規(guī)模、多領域地投入使用.

圖1 雙舵輪底盤模型

導航定位是AGV 小車控制的核心部分[2],能實時地上傳AGV 小車精確的坐標位置,是后續(xù)路徑規(guī)劃算法和避障算法的保障,目前的導航方法主要有依賴環(huán)境的:光學導航、磁帶導航、二維碼標志等,和依賴于AGV 自身傳感器的:激光導航、視覺導航等.基于自身傳感器的AGV 小車使用范圍更加廣泛,成為目前研究的熱點.其中激光導航技術相對成熟但是精密的激光雷達成本非常高昂商業(yè)運用價值低[3].隨著技術的發(fā)展,視覺在各個領域的應用逐漸增多,比如:三維重建[4]、SLAM[5]等前沿技術,而且其傳感器價格低廉具有很好的前景.

基于自身傳感器定位的AGV 導航系統(tǒng)不管是視覺導航還是激光導航都有一個非常重要的功能模塊——里程計.其作用是記錄小車每個時刻和前一微小時刻的運動增量,然后將這個增量傳遞給后臺來計算出AGV 小車的運動軌跡,得到小車的精確定位.目前工業(yè)運用的兩輪差速AGV 大多采用輪式編碼器來記錄里程,然后融合激光雷達進行定位.但是雙舵輪AGV 小車運用編碼器來記錄里程會導致定位誤差大,在路面崎嶇的情況下尤為明顯.本文用視覺里程計(Visual Odometry,VO)去計算里程,避免對地盤的直接運動學建模提高雙舵輪AGV 小車的定位精度.本文將詳細地論述在實際工程中VO 的原理和搭建過程,選取的傳感器是微軟的R-GBD 攝像頭kinect1,搭載在雙舵輪的移動重載搬運平臺上.如圖2所示為算法流程 圖.

圖2 算法流程圖

1 相機模型

1.1 針孔相機模型

相機采集的數(shù)據(jù)是一幀一幀的數(shù)字圖像,它的本質(zhì)是一個離散的數(shù)值矩陣,這個矩陣記錄了各個像素點灰度值(或者RGB 值).但是矩陣和圖像都是二維的,而相機周圍的環(huán)境是三維的.因此必須建立相機模型,找到三維空間點和像素坐標點的對應關系.如圖3所示是針孔相機模型.

圖3 針孔相機模型

在針孔相機模型中存在著4 個坐標系,一個是世界坐標系,另一個是以相機的光心O 為原點的相機坐標系,還有一個是以相機內(nèi)部成像平面中心為原點的物理成像平面.最后一個是像素坐標系,像素和物理成像平面在一個平面上,不過其原點在平面的左上角并且度量單位為像素.

現(xiàn)實世界中一個空間點P 在如圖所示的位置,其通過相機的光心O 投影在相機的物理成像平面上的P’點處,于是得到了兩個相似三角形,可以得到以下關系:

由上式可以看出成的是倒立的像,相機會將圖片自動翻轉(zhuǎn),因此上式變?yōu)?

這個公式反映了三維空間點和其在相機投影平面上的位置關系.將點在相機坐標系(注意是相機坐標系,和世界坐標系相差一個平移和旋轉(zhuǎn))中的坐標映射到物理成像平面,但是相機操作的是像素,因此需要將物理成像平面的坐標再轉(zhuǎn)換成像素坐標(相差一個平移和縮放),最后得到如下的關系:

其中,(X,Y,Z)表示空間點在相機坐標系下的三維坐標,(u,v,1)表示投影點在像素坐標系下的齊次坐標.參數(shù)K 表示的是空間點和相機中投影點的對應關系,這個參數(shù)在相機出廠之后是固定的,稱之為相機的內(nèi)參(可由相機標定得到).由此建立起3D-2D 的對應關系.

在實際工程運用中空間點的坐標往往是世界坐標Pw,和相機坐標系下的坐標P相差一個旋轉(zhuǎn)R和一個平移t.因此有如下等式:

最后結合上面兩式并化為齊次坐標:

1.2 相機標定

雖然通過相機模型可解出空間點和像素點的對應關系,但是攝像頭是光學原件,存在著透視失真如:徑向畸變、切向畸變.畸變會扭曲圖像(比如現(xiàn)實中的一條直線因為畸變的原因在相機圖像中變成了一條曲線)導致最后實驗的結果產(chǎn)生很大的誤差[6].

對相機進行標定,修正相機的參數(shù)是計算精確里程前必須要做的一步,將像素坐標轉(zhuǎn)換為極坐標并且引入5 個畸變糾正參數(shù),可以得到如下關系.

徑向畸變:

切向畸變:

聯(lián)立上述方程可以解出這5 個系數(shù),進而糾正相機的畸變找到空間點和圖像投影點正確的對應關系.本文采用OpenCV 結合棋盤格對kinect1 進行標定,結果如圖4所示.

圖4 相機標定結果

通過棋盤格標定精確地得到了相機的內(nèi)參數(shù)和5 個糾正系數(shù).標定后的相機修正了畸變帶來的誤差,為后面視覺里程計的搭建提供了條件.

2 特征提取和匹配

2.1 特征提取

相機模型可以得出三維空間點和圖像投影點之間的對應關系,但是一副圖像中包含了大量的像素點,而計算機的計算能力有限,不足以對每個像素點都進行計算.因此需要找到一些比較特殊的像素點代表這幅圖像,只需對這些特殊的點進行處理,并計算對應空間點的相對運動[7].這個過程叫做特征提取,這些特殊的點叫做“特征點”,大多數(shù)特征點都由兩部分構成:關鍵點和描述子.

常見的特征提取算子有很多,比如經(jīng)典的Harris角點檢測算子、SIFT 算子、SURF 算子、FAST 算子,和根據(jù)FAST 改進的ORB 算子等等[8].每種算子都有自己適合的運用場景,如SIFT 算子具有旋轉(zhuǎn)不變性,尺度不變性,對光照和噪聲不敏感等優(yōu)點,適合復雜的環(huán)境[9].但是其對計算機性能要求高,耗時長,不適合本文的運用場景.結合本文實際的運用場合選取ORB 算子來進行特征提取,其最大的特點是提取速度非?？?ORB 算子由Oriented FAST 關鍵點和BRIEF 描述子構 成.如圖5所示為Oriented FAST 關鍵點的原理圖:

圖5 Oriented FAST 原理圖

Oriented FAST 關鍵點是基于灰度檢測的,如果一個像素點與其周圍的絕大多數(shù)的像素點的灰度值都差距較大,則將這個點定義為關鍵點,認為這個點是這幅圖像的一個特征.其具體的算法實現(xiàn)步驟如下:

(1)選取一個點P,其灰度值為IP,并以P為圓形畫一個半徑為3 的圓.

(2)設定一個閾值T(一般設為IP的20%),然后依次比較圓上16 個點的灰度值和IP的大小,如果有連續(xù)12 個點的灰度值大于IP+T或者小于IP-T,則將P記為關鍵點.

(3)對每一個像素都進行以上兩步操作,得出所有的關鍵點.

(4)對得出的所有關鍵點進行非極大值抑制,防止關鍵點扎堆.

本文通過OpenCV 實現(xiàn)上述算法,并驗證了算法的可行性,如圖6所示,對于kinect1 采集的圖像,此算法能準確快速地提取出Oriented FAST 關鍵點.

2.2 特征匹配

Oriented FAST 可以找到每個關鍵點在圖像中的位置,視覺里程計需要通過兩幅圖像計算相機的運動增量.因此要將兩幅圖像中的相同的關鍵點進行匹配,需要給關鍵點加上一個獨一無二的id:描述子.描述子描述了每個特征點和其周圍像素點的一種關系,因此不同特征點的描述子是不一樣的.而且在不同的圖像中,相同特征點的描述子差距不大,計算機可以通過描述子對相同的特征點進行匹配.BRIEF 描述子的基本原 理如圖7所示.

圖6 Oriented FAST 關鍵點

圖7 BRIEF 描述子

以關鍵點P為中心選取一個固定大小的正方形區(qū)域,然后根據(jù)一定的概率分布隨機地選取兩個點A和B組成一個點對,比較這兩個點灰度值的大小,如果A大于B則輸出1,反之輸出0.按照此方法重復地選取n個點對,將最后得到的結果記錄在一個相應大小的數(shù)組中(采用這樣類似于二進制編碼的描述方式,計算機讀取描述子的速度非?？?.然后重復以上操作對每一個關鍵點都求其描述子.

上述方法得到的特征點對旋轉(zhuǎn),尺度這兩個因素敏感,當場景發(fā)生改變時容易丟失.通過搭建圖像金字塔來改善尺度不變性,通過求圖像的質(zhì)心來解決旋轉(zhuǎn)不變性.最后通過計算漢明距離結合快速近似最近鄰(FLANN)算法對兩幅有微小旋轉(zhuǎn)和平移的圖像進行特征匹配.以上算法全在OpenCV 下實現(xiàn),然后在kinect1 上去檢測算法的效果,結果如圖8所示.

ORB 算子能有效地提取兩幅圖像中的特征點,并將相同的特征點進行特征匹配.

2.3 視覺里程計

通過前文的步驟,可以準確快速地提取相機圖像中的特征點,并對相鄰兩幅圖像中的特征點進行匹配,然后找到與之對應的空間點坐標.最后,可以通過這些空間點的幾何關系去求解出相機在這兩幅圖片中的運動增量.本文采用ICP(Iterative Closest Point,迭代最近點 )的方法來求解這個運動R和t[10,11].

圖8 ORB 算子特征匹配圖

選取一對已經(jīng)經(jīng)過特征匹配的空間點:

定義一個誤差項ei,可以建立一個最小二乘的問題:

對于這個最小二乘的問題,求解R,t使誤差平方和最小.

至此,一個基于RGB-D 攝像頭的視覺里程計就搭建完成了.最后在Linux+ROS 平臺下對上述算法進行代 碼的實現(xiàn).如圖9是視覺里程計效果圖.

圖9 視覺里程計

左邊的箭頭表示相機當前的位姿,始終指向相機坐標系的正前方,右邊的箭頭實時地接收里程計返回的數(shù)據(jù)并可視化的表示出來.

3 實驗結果與分析

將視覺里程計裝載上圖10所示的雙舵輪AGV 小車 臺架上和激光雷達數(shù)據(jù)融合測試并對比定位精度.

圖10 試驗臺架

本文采用重復定位的方式來測試定位精度,在系統(tǒng)運行穩(wěn)定后,分別重復20 次定位并繪制出散點圖,和基校點比較得出定位精度.圖11為路面平坦的情況下編碼器做里程和視覺做里程的定位精度對比圖,編碼器的定位精度為19.46 mm,而視覺里程計的定位精度 為16.7 mm,可以看出視覺里程計的定位精度更高.

圖11 平坦路面定位散點圖

但是實際工廠中的環(huán)境往往更加復雜,AGV 小車一般會在路面崎嶇的環(huán)境下工作,針對崎嶇的路面對比兩種方案的定位精度,結果如圖12所示,可以看出在崎嶇的路面中輪子顛簸和打滑加上運動模型復雜,編碼器不能很好的記錄里程信息.定位誤差增大為31.46 mm.視覺里程計避開了直接求解運動模型而且路面對其影響較小,定位效果有了明顯的改善,重復定位精度為19.65 mm,相對于平坦路面的情況下定位精度變化不大.

圖12 崎嶇路面定位散點圖

最后為了對比系統(tǒng)的魯棒性,在崎嶇的路面上分別應用兩種方案連續(xù)運行8 小時并繪制時間軸.可以看出視覺做里程的情況下,系統(tǒng)的魯棒性相對穩(wěn)定,在連續(xù)運行8 小時的情況下定位精度幾乎保持在一個穩(wěn)定的范圍類,而編碼器做里程其對路面非常敏感,定位精度不高累計誤差大.

通過圖13、圖14對比可以看出在雙舵輪平臺上,運用視覺做里程定位精度較高,在不同路面環(huán)境下都能保持在一個穩(wěn)定的范圍內(nèi),同時魯棒性較好,和現(xiàn)在傳統(tǒng)的編碼器做里程相比,避免了對底盤的直接運動學 建模并且價格低廉具有很好的發(fā)展?jié)摿?

圖13 編碼器定位精度時間軸

圖14 視覺里程計定位精度時間軸

4 結論

針對雙舵輪AGV 在地面崎嶇不平的情況下編碼器失效的問題.本文提出一種使用價格低廉的RGBD 相機做視覺里程計的方案,避免對雙舵輪AGV 的直接運動學建模來解決編碼器航跡推算累積誤差過大的問題.并在不同路面條件下對視覺里程計和編碼器里程計兩種方案的定位精度做了對比,視覺里程計的定位精度更高特別是在崎嶇路面的條件下,而且系統(tǒng)的魯棒性相對穩(wěn)定,在連續(xù)運行8 小時的情況下定位精度保持在一個穩(wěn)定的范圍類.而編碼器做里程對路面非常敏感,定位精度不高累計誤差大.結果表明使用RGB-D 相機做視覺里程計運用于雙舵輪AGV 具有很好的實際運用價值.