梅瑞冬,張 捷,李 強(qiáng),劉 超,王書(shū)亭

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

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

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

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

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

雙舵輪的AGV 小車(chē)具有良好的發(fā)展前景,是目前研究的熱點(diǎn),但是因?yàn)槭褂脙蓚€(gè)舵輪,其運(yùn)動(dòng)模型復(fù)雜且導(dǎo)航定位精度不高.雙舵輪AGV 小車(chē)還不能大規(guī)模、多領(lǐng)域地投入使用.

圖1 雙舵輪底盤(pán)模型

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

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

圖2 算法流程圖

1 相機(jī)模型

1.1 針孔相機(jī)模型

相機(jī)采集的數(shù)據(jù)是一幀一幀的數(shù)字圖像,它的本質(zhì)是一個(gè)離散的數(shù)值矩陣,這個(gè)矩陣記錄了各個(gè)像素點(diǎn)灰度值(或者RGB 值).但是矩陣和圖像都是二維的,而相機(jī)周?chē)沫h(huán)境是三維的.因此必須建立相機(jī)模型,找到三維空間點(diǎn)和像素坐標(biāo)點(diǎn)的對(duì)應(yīng)關(guān)系.如圖3所示是針孔相機(jī)模型.

圖3 針孔相機(jī)模型

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

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

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

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

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

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

最后結(jié)合上面兩式并化為齊次坐標(biāo):

1.2 相機(jī)標(biāo)定

雖然通過(guò)相機(jī)模型可解出空間點(diǎn)和像素點(diǎn)的對(duì)應(yīng)關(guān)系,但是攝像頭是光學(xué)原件,存在著透視失真如:徑向畸變、切向畸變.畸變會(huì)扭曲圖像(比如現(xiàn)實(shí)中的一條直線因?yàn)榛兊脑蛟谙鄼C(jī)圖像中變成了一條曲線)導(dǎo)致最后實(shí)驗(yàn)的結(jié)果產(chǎn)生很大的誤差[6].

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

徑向畸變:

切向畸變:

聯(lián)立上述方程可以解出這5 個(gè)系數(shù),進(jìn)而糾正相機(jī)的畸變找到空間點(diǎn)和圖像投影點(diǎn)正確的對(duì)應(yīng)關(guān)系.本文采用OpenCV 結(jié)合棋盤(pán)格對(duì)kinect1 進(jìn)行標(biāo)定,結(jié)果如圖4所示.

圖4 相機(jī)標(biāo)定結(jié)果

通過(guò)棋盤(pán)格標(biāo)定精確地得到了相機(jī)的內(nèi)參數(shù)和5 個(gè)糾正系數(shù).標(biāo)定后的相機(jī)修正了畸變帶來(lái)的誤差,為后面視覺(jué)里程計(jì)的搭建提供了條件.

2 特征提取和匹配

2.1 特征提取

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

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

圖5 Oriented FAST 原理圖

Oriented FAST 關(guān)鍵點(diǎn)是基于灰度檢測(cè)的,如果一個(gè)像素點(diǎn)與其周?chē)慕^大多數(shù)的像素點(diǎn)的灰度值都差距較大,則將這個(gè)點(diǎn)定義為關(guān)鍵點(diǎn),認(rèn)為這個(gè)點(diǎn)是這幅圖像的一個(gè)特征.其具體的算法實(shí)現(xiàn)步驟如下:

(1)選取一個(gè)點(diǎn)P,其灰度值為IP,并以P為圓形畫(huà)一個(gè)半徑為3 的圓.

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

(3)對(duì)每一個(gè)像素都進(jìn)行以上兩步操作,得出所有的關(guān)鍵點(diǎn).

(4)對(duì)得出的所有關(guān)鍵點(diǎn)進(jìn)行非極大值抑制,防止關(guān)鍵點(diǎn)扎堆.

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

2.2 特征匹配

Oriented FAST 可以找到每個(gè)關(guān)鍵點(diǎn)在圖像中的位置,視覺(jué)里程計(jì)需要通過(guò)兩幅圖像計(jì)算相機(jī)的運(yùn)動(dòng)增量.因此要將兩幅圖像中的相同的關(guān)鍵點(diǎn)進(jìn)行匹配,需要給關(guān)鍵點(diǎn)加上一個(gè)獨(dú)一無(wú)二的id:描述子.描述子描述了每個(gè)特征點(diǎn)和其周?chē)袼攸c(diǎn)的一種關(guān)系,因此不同特征點(diǎn)的描述子是不一樣的.而且在不同的圖像中,相同特征點(diǎn)的描述子差距不大,計(jì)算機(jī)可以通過(guò)描述子對(duì)相同的特征點(diǎn)進(jìn)行匹配.BRIEF 描述子的基本原 理如圖7所示.

圖6 Oriented FAST 關(guān)鍵點(diǎn)

圖7 BRIEF 描述子

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

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

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

2.3 視覺(jué)里程計(jì)

通過(guò)前文的步驟,可以準(zhǔn)確快速地提取相機(jī)圖像中的特征點(diǎn),并對(duì)相鄰兩幅圖像中的特征點(diǎn)進(jìn)行匹配,然后找到與之對(duì)應(yīng)的空間點(diǎn)坐標(biāo).最后,可以通過(guò)這些空間點(diǎn)的幾何關(guān)系去求解出相機(jī)在這兩幅圖片中的運(yùn)動(dòng)增量.本文采用ICP(Iterative Closest Point,迭代最近點(diǎn) )的方法來(lái)求解這個(gè)運(yùn)動(dòng)R和t[10,11].

圖8 ORB 算子特征匹配圖

選取一對(duì)已經(jīng)經(jīng)過(guò)特征匹配的空間點(diǎn):

定義一個(gè)誤差項(xiàng)ei,可以建立一個(gè)最小二乘的問(wèn)題:

對(duì)于這個(gè)最小二乘的問(wèn)題,求解R,t使誤差平方和最小.

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

圖9 視覺(jué)里程計(jì)

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

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

將視覺(jué)里程計(jì)裝載上圖10所示的雙舵輪AGV 小車(chē) 臺(tái)架上和激光雷達(dá)數(shù)據(jù)融合測(cè)試并對(duì)比定位精度.

圖10 試驗(yàn)臺(tái)架

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

圖11 平坦路面定位散點(diǎn)圖

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

圖12 崎嶇路面定位散點(diǎn)圖

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

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

圖13 編碼器定位精度時(shí)間軸

圖14 視覺(jué)里程計(jì)定位精度時(shí)間軸

4 結(jié)論

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