俞毓鋒 趙卉菁

面向非結(jié)構(gòu)化環(huán)境的輪式移動機(jī)器人在民用及國防等領(lǐng)域有著重要的應(yīng)用需求,其技術(shù)發(fā)展得到了一些著名賽事的驗證和推廣(如美國DARPA 無人車挑戰(zhàn)賽1http://www.darpa.mil/ grandchallenge05/,歐洲ELROB室外機(jī)器人比賽2http://www.elrob.org/,國內(nèi)跨越險阻地面無人系統(tǒng)挑戰(zhàn)賽3http://www.mod.gov.cn/power/2016-10/18/content 4748815.htm等),獲得了社會各界的廣泛關(guān)注.

精確定位作為機(jī)器人導(dǎo)航的關(guān)鍵,在室外非結(jié)構(gòu)化環(huán)境面臨巨大挑戰(zhàn).環(huán)境復(fù)雜多變、光照、遮擋、缺乏結(jié)構(gòu)化信息等,為視覺里程計[1]或視覺SLAM[2]算法的特征提取與匹配帶來很多不確定性,影響定位精度和魯棒性.另一方面,激光雷達(dá)已被廣泛應(yīng)用于機(jī)器人系統(tǒng).基于三維激光雷達(dá)[3]的定位系統(tǒng)有較高精度,然而傳感器系統(tǒng)昂貴,限制了其廣泛的應(yīng)用.通過電機(jī)帶動單線激光雷達(dá)旋轉(zhuǎn)所構(gòu)建的三維掃描系統(tǒng)成本較低,但完成一次三維掃描的周期長,如何對周期內(nèi)的掃描數(shù)據(jù)進(jìn)行運動補償成為其關(guān)鍵因素.文獻(xiàn)[3]采取stop-scan-go 的方式,將數(shù)據(jù)掃描與機(jī)器人運動分離,嚴(yán)重影響了機(jī)器人運動的連續(xù)性;文獻(xiàn)[4]利用航位推算法對周期內(nèi)掃描數(shù)據(jù)進(jìn)行運動補償,僅適用于平坦地形環(huán)境;文獻(xiàn)[5]利用空間中直線和平面等強結(jié)構(gòu)信息對激光特征進(jìn)行約束,無法應(yīng)用于非結(jié)構(gòu)化環(huán)境.為了彌補單一傳感器信息的局限,融合相機(jī)與單線激光雷達(dá)的方案也被研究.文獻(xiàn)[6?7]利用激光數(shù)據(jù)構(gòu)建三維點云,為圖像中的特征點賦予深度信息,從而提高定位精度.然而三維點云的重建精度受到累計定位誤差的影響,長時間運行過程中特征點深度信息難以準(zhǔn)確估計.另一方面,前述研究中均未考慮到由于系統(tǒng)、光照、遮擋等因素對特征點提取與匹配所帶來的不確定性影響.

本文提出一種基于相機(jī)與搖擺激光雷達(dá)融合的定位算法,重點解決在非結(jié)構(gòu)化環(huán)境中的機(jī)器人位姿估計問題.本文結(jié)合激光雷達(dá)的深度信息和圖像的顏色紋理信息,構(gòu)建在時序幀間的特征點匹配關(guān)系;引入一種置信度評價方法,結(jié)合系統(tǒng)誤差、數(shù)據(jù)關(guān)聯(lián)、物體遮擋、特征跟蹤等因素對特征點及其匹配關(guān)系進(jìn)行評估,減少低質(zhì)量特征的影響;最終將定位問題轉(zhuǎn)化為特征點對的加權(quán)重投影誤差優(yōu)化問題予以解決.利用小型輪式移動機(jī)器人在越野和公園等典型非結(jié)構(gòu)化環(huán)境下進(jìn)行數(shù)據(jù)采集和定位實驗,驗證了本文算法的有效性.實驗結(jié)果表明,與前沿的雙目視覺定位算法相比,本文算法可有效提高在非結(jié)構(gòu)化環(huán)境中的定位精度.

本文結(jié)構(gòu)如下:第1 節(jié)圍繞室外機(jī)器人的定位問題調(diào)研國內(nèi)外研究現(xiàn)狀,并分析關(guān)鍵技術(shù)難點;第2 節(jié)給出本文的問題定義,將定位問題轉(zhuǎn)化為特征點重投影誤差優(yōu)化問題;第3 節(jié)介紹在時序幀間的特征點匹配關(guān)系構(gòu)建方法,包括從激光到圖像,圖像到激光等兩類匹配方法;第4 節(jié)針對系統(tǒng)誤差、數(shù)據(jù)關(guān)聯(lián)、物體遮擋、特征跟蹤等不確定性因素,建立特征點置信度評價方法;第5 節(jié)給出實驗結(jié)果與分析,包括越野和公園等兩類典型非結(jié)構(gòu)環(huán)境的定位結(jié)果,與航位推算法,單目或雙目視覺定位算法的精度對比,以及置信度評價的有效性分析等;第6 節(jié)給出總結(jié)和今后的研究展望.

1 相關(guān)研究

目前的無人車或室外移動機(jī)器人大多依賴于高精度衛(wèi)星定位與慣性導(dǎo)航組合系統(tǒng)(俗稱GPS 慣導(dǎo))并結(jié)合輪速儀等設(shè)備實現(xiàn)定位.然而在復(fù)雜路面,基于輪速儀的航位推算有很大誤差,而衛(wèi)星信號的遮擋,干擾等嚴(yán)重影響衛(wèi)星定位精度.通過匹配相機(jī),激光雷達(dá)等采集的環(huán)境信息進(jìn)行定位,可提高無人車或移動機(jī)器人在復(fù)雜環(huán)境中的定位精度.

視覺里程計[1]作為一個通用視覺定位算法,利用前后幀特征點的檢測跟蹤結(jié)果,解算機(jī)器人實際行走里程.由于缺乏深度信息,單目視覺里程計無法有效地解算尺度,成為定位誤差的重要原因之一.雙目相機(jī)系統(tǒng)[8]在兩個相機(jī)標(biāo)定準(zhǔn)確的前提下,利用左右圖像生成三維點.圖像中的二維特征點有唯一的三維點進(jìn)行對應(yīng),減小了求解三維深度的誤差,但相機(jī)之間的對應(yīng)點匹配受到環(huán)境光照、陰影等因素影響,在光照條件受限的情況下,容易產(chǎn)生定位偏差.

不同于相機(jī),激光雷達(dá)傳感器主動發(fā)出激光束進(jìn)行測距,受到光照的干擾小.在地面平坦的區(qū)域,直接使用水平放置的二維激光雷達(dá)即可進(jìn)行定位[9].在地面起伏明顯區(qū)域,三維激光雷達(dá)可通過點云匹配實現(xiàn)較高精度的定位[3],但傳感器價格昂貴限制了這類方法的應(yīng)用.為了構(gòu)建低成本三維掃描系統(tǒng),一些研究者將二維激光雷達(dá)安裝在旋轉(zhuǎn)電機(jī)上構(gòu)建低成本三維掃描系統(tǒng).然而這類系統(tǒng)完成一次三維掃描的周期長,需對周期內(nèi)的掃描數(shù)據(jù)進(jìn)行運動補償.Stop-scan-go 的方式將數(shù)據(jù)掃描與機(jī)器人運動分離,雖避免了掃描周期中的運動影響,但嚴(yán)重影響了機(jī)器人的運行速度[4];航位推算法對運動的補償可在平坦路面得到較高精度,但無法適用于非平坦路面[5];空間中直線和平面等強結(jié)構(gòu)信息可對激光特征進(jìn)行約束來估計掃描周期內(nèi)的運動,但環(huán)境的結(jié)構(gòu)假設(shè)過強,無法應(yīng)用于非結(jié)構(gòu)化環(huán)境[10].

在單一傳感器無法有效定位的情況下,多傳感器融合的定位方法可提高傳感器信息量,成為解決定位問題的一個可行途徑.一些研究者使用多個不同朝向的單目相機(jī)進(jìn)行融合定位[11],將一個相機(jī)向下傾斜觀察地面,一定程度上減少了光照的影響.另一些則在單目或雙目視覺定位系統(tǒng)中加入慣性導(dǎo)航單元,有效減少角度誤差[12?13],從而提高定位精度,但仍然無法避免光照、遮擋等因素干擾下,低質(zhì)量特征點對定位結(jié)果的影響.相機(jī)與激光雷達(dá)兩類傳感器的融合可有效提高特征的信息維度.傳統(tǒng)相機(jī)與激光雷達(dá)融合定位算法主要依靠激光傳感器生成的三維點云對圖像中特征點的深度進(jìn)行估計,再使用視覺特征進(jìn)行定位[6?7],提高定位精度.然而,受到非結(jié)構(gòu)化場景中地面顛簸的影響,三維點云重建結(jié)果隨著時間的推移存在明顯累計誤差,長期運行的偏差很大程度影響定位結(jié)果,需考慮直接在局部幀間進(jìn)行相機(jī)和激光雷達(dá)特征層上的匹配.另一方面,相機(jī)和激光雷達(dá)在融合時引入了更多系統(tǒng)誤差,容易造成錯誤匹配,需要分析特征的置信度給出權(quán)重,剔除誤差較大的特征點.

綜上,面向輪式移動機(jī)器人室外非結(jié)構(gòu)化環(huán)境的精確定位,本文提出一種基于相機(jī)和搖擺單線激光融合的視覺定位算法,構(gòu)建在時序幀間的特征點匹配關(guān)系,并引入一種結(jié)合系統(tǒng)誤差、數(shù)據(jù)關(guān)聯(lián)、物體遮擋、特征跟蹤等因素的置信度評價方法,最終將定位估計轉(zhuǎn)化為特征點對的加權(quán)重投影誤差優(yōu)化問題予以解決.

2 問題定義

機(jī)器人與相機(jī)為剛性連接,兩者的坐標(biāo)系可以通過外部標(biāo)定參數(shù)進(jìn)行轉(zhuǎn)換.因此,機(jī)器人的定位求解等價于相機(jī)的定位求解.定義相機(jī)坐標(biāo)系為基準(zhǔn)坐標(biāo)系,圖像幀率作為基準(zhǔn)幀率,第k幀圖像所對應(yīng)的相機(jī)坐標(biāo)系記為Ck,對應(yīng)的圖像坐標(biāo)系為Ik.由于激光與相機(jī)的幀率不一致,將激光幀與最接近的圖像幀進(jìn)行對應(yīng).假設(shè)激光幀l上的一個激光點所對應(yīng)的圖像幀為k,令三維激光點在基準(zhǔn)坐標(biāo)系(相機(jī)坐標(biāo)系)下定義為P k,對應(yīng)的二維圖像上的點為pk=KP k,其中K為相機(jī)的內(nèi)參矩陣.

圖1 問題定義Fig.1 Problem statement

如圖1 所示,(P k,i,pk,i)表示在第k幀下第i個特征匹配點對.一方面,通過跟蹤圖像點pk,i,可獲得其在相鄰幀上的圖像跟蹤點,j=···,k ?2,k ?1,k,k+1,···.另一方面,三維激光點P k,i可通過坐標(biāo)變換投影至對應(yīng)幀j.令Mk表示第k幀的相機(jī)坐標(biāo)系Ck到世界坐標(biāo)系的坐標(biāo)變換矩陣

其中,Rk為3×3 的旋轉(zhuǎn)矩陣,tk為平移向量.P k,i可先轉(zhuǎn)換至世界坐標(biāo)系MkP k,i,再轉(zhuǎn)換至第j幀的相機(jī)坐標(biāo)系Cj下最后投影到第j幀的圖像坐標(biāo)系Ij,得若定位無誤差,且特征對應(yīng)與跟蹤準(zhǔn)確,則在第j幀圖像上,基于圖像特征點跟蹤的結(jié)果和基于坐標(biāo)系轉(zhuǎn)換的投影結(jié)果為同一點.由于定位誤差的存在,或特征對應(yīng)與跟蹤時的誤差影響,跟蹤點與投影點之間存在偏差.定義第k幀中的第i個特征在第j幀中的跟蹤點與投影點之間的偏差為特征的重投影誤差,即

當(dāng)特征對應(yīng)與跟蹤準(zhǔn)確時,通過最小化重投影誤差,可進(jìn)行定位,即

針對特征對應(yīng)與跟蹤的誤差影響,假定誤差噪聲為白噪聲,通過置信度估計方法調(diào)整各個特征的權(quán)重,減少甚至消除特征誤差的影響.因此,在最小化重投影誤差函數(shù)中加入權(quán)重參數(shù),定義為

上式中所有的坐標(biāo)系變換矩陣M都需進(jìn)行優(yōu)化.隨著幀數(shù)的逐漸增加,優(yōu)化也會越來越慢.實際上,在線定位系統(tǒng)無需對過早的信息進(jìn)行優(yōu)化,只需優(yōu)化當(dāng)前一小段時間內(nèi)的定位結(jié)果.因此,最終所需優(yōu)化的代價函數(shù)如下優(yōu)化的過程為非線性優(yōu)化,可使用LM 算法(Levenberg-Marquardt[14])進(jìn)行迭代優(yōu)化.綜上,本文定位算法整體框架如圖2 所示.

圖2 系統(tǒng)流程Fig.2 System overview

1)對于第k幀圖像,已知k ?1 幀圖像的特征點集{pk?1,i}與對應(yīng)三維點集{P k?1,i},通過跟蹤圖像特征點得到當(dāng)前幀圖像特征點集

3)通過插值生成k幀圖像相鄰激光幀的位姿,并將激光點投影至對應(yīng)幀,構(gòu)造最新的激光點與圖像點特征對應(yīng);

4)使用局部若干幀圖像,通過優(yōu)化代價函數(shù)(5),優(yōu)化當(dāng)前時刻附近的相機(jī)位姿.

在上述四個模塊中,模塊一圖像特征跟蹤部分可使用任意的特征點跟蹤方法;模塊二當(dāng)前幀的位姿預(yù)測使用當(dāng)前幀的最小化重投影誤差方法,本質(zhì)上與模塊四一致,而優(yōu)化方法可使用開源的非線性優(yōu)化方法.因此,本文的重點內(nèi)容在于求解模塊三的特征對應(yīng)與構(gòu)造模塊四的優(yōu)化函數(shù)(5).這兩部分內(nèi)容分別在第3 節(jié)特征對應(yīng)求解和第4 節(jié)置信度估計中詳述.

3 特征對應(yīng)求解

三維激光點與二維圖像點的特征對應(yīng)關(guān)系包含以下難點.1)三維激光點在對應(yīng)幀的投影很可能是一個無特征或特征較少的區(qū)域,投影點可能無法跟蹤或跟蹤誤差很大;2)三維激光點云非常稀疏,且一幀圖像所對應(yīng)的激光數(shù)據(jù)也只集中在圖像中的一小塊區(qū)域內(nèi),如果僅使用三維點到二維點的投影,則特征過少且過于集中.因此,在特征對應(yīng)中不僅需要考慮三維激光點到二維圖像點的正向?qū)?yīng),還需要考慮二維圖像點到三維激光點的逆向?qū)?yīng).本文特征對應(yīng)同時包含激光雷達(dá)到圖像(三維到二維)和圖像到激光雷達(dá)(二維到三維)兩部分內(nèi)容.

3.1 激光雷達(dá)到圖像的對應(yīng)

給定一個在第k幀的三維激光點P k,i,通過標(biāo)定參數(shù)與局部運動估計,可投影至相鄰某幀j幀中,得到投影點假定標(biāo)定與同步的參數(shù)準(zhǔn)確,其對應(yīng)幀k的圖像投影點即為真實k幀圖像的特征對應(yīng)點pk,i.如果對應(yīng)幀投影點為圖像特征點pk,i,則可直接進(jìn)行圖像特征點跟蹤,獲取在j幀的跟蹤點跟蹤點與投影點即為投影點P k,i在第j幀的特征對應(yīng),代入式(5)中進(jìn)行定位.

如果對應(yīng)幀投影點pk,i本身所包含的特征信息較弱,直接對其進(jìn)行圖像特征跟蹤會產(chǎn)生較大誤差.為了解決這一問題,可采用如圖3 所示的處理方法.假定圖像特征點pk,1,···,pk,n為激光投影點pk,i的鄰近點,通過鄰近點的跟蹤,可獲得投影點的跟蹤.假定為這些鄰近點在第j幀的跟蹤點,投影點pk,i在第j幀的跟蹤點可以通過線性插值求解,得同時建立在第j幀的投影點與跟蹤點的對應(yīng)關(guān)系

圖3 激光特征點到圖像的對應(yīng)求解Fig.3 LiDAR-to-camera correspondence generation

3.2 圖像到激光雷達(dá)的對應(yīng)

由于三維激光點的稀疏性,三維激光點到二維點的對應(yīng)數(shù)量較少,僅使用這些對應(yīng)點很難進(jìn)行定位求解.而圖像的特征點更為稠密,利用圖像到激光雷達(dá)的特征對應(yīng),可有效地提高特征點數(shù)目.

如圖4 所示,給定第k幀圖像上的一個特征點pk,i,需找到其在三維上的對應(yīng)點.通常情況下,由于激光點的稀疏性,圖像特征點無法直接與激光點對應(yīng),因此需通過插值求解.對于第k幀圖像,假設(shè)相鄰一段時間內(nèi)[k1,k2]的激光點投影到圖像Ik上,根據(jù)其掃描規(guī)律,不同幀的激光點處于不同的區(qū)域.圖4 所示的一幀激光的投影點可形成一個區(qū)域.在此區(qū)域內(nèi),可忽略激光點之間的同步誤差.對于圖像Ik上的點pk,i,其鄰近點均為第j幀所對應(yīng)的激光點投影對應(yīng)的三維點為P j,1,···,P j,n.通過擬合三維點形成平面或曲面,再根據(jù)平面與圖像點所對應(yīng)射線的角點,即可求解出pk,i到第j幀的三維對應(yīng)點P j,i.將其投影到其他幀,可以得到對應(yīng)的投影點另一方面,圖像特征點pk,i也可跟蹤至幀,得到對應(yīng)的跟蹤點最終建立第k幀圖像特征點在第幀的跟蹤點與投影點之間的對應(yīng),代入式(5)中進(jìn)行定位.

4 置信度估計

特征對應(yīng)存在不同程度的誤差,給定一個特征對應(yīng),其誤差來源可分為以下四個方面:

1)硬件系統(tǒng)誤差(vh),包括激光雷達(dá)與相機(jī)的標(biāo)定與時鐘同步誤差;

圖4 圖像特征點到激光的對應(yīng)求解Fig.4 Camera-to-LiDAR correspondence generation

2)數(shù)據(jù)關(guān)聯(lián)誤差(vf),包括數(shù)據(jù)插值誤差與不同源數(shù)據(jù)間投影的誤差;

3)物體遮擋影響(vo),為兩個傳感器視角不同產(chǎn)生遮擋導(dǎo)致的誤差;

4)特征跟蹤誤差(vt),包括單個圖像特征點跟蹤誤差與多個特征點聯(lián)合跟蹤誤差.

通過以上四個方面的置信度分析,定義特征對應(yīng)綜合置信度如下:

4.1 系統(tǒng)標(biāo)定與同步

對于一個使用多傳感器的系統(tǒng),標(biāo)定誤差和時鐘同步誤差無法避免.因此,系統(tǒng)置信度vh包含相機(jī)與激光之間的標(biāo)定產(chǎn)生的置信度vh(c)與時鐘同步產(chǎn)生的置信度vh(s).

相機(jī)與激光之間的標(biāo)定參數(shù)包含旋轉(zhuǎn)矩陣R和平移向量t,其相對誤差分別為eR和et.對于一個激光掃描結(jié)果,投影到電機(jī)所在坐標(biāo)系下為P=[X,Y,D]T,轉(zhuǎn)換到相機(jī)坐標(biāo)系下的過程中,旋轉(zhuǎn)和平移誤差產(chǎn)生不同的效果.旋轉(zhuǎn)標(biāo)定誤差所帶來的實際誤差與距離D成正比,而平移標(biāo)定誤差所帶來的實際誤差為定值.因此,標(biāo)定置信度函數(shù)建立如下:

激光雷達(dá)與相機(jī)有不同的掃描頻率,為了將兩個數(shù)據(jù)進(jìn)行融合,需要進(jìn)行插值.在插值的過程中,兩者時間相差越大,所帶來的誤差也越大.因此,同步置信度定義如下:

4.2 數(shù)據(jù)關(guān)聯(lián)

本文算法所述的特征對應(yīng)求解包含激光雷達(dá)到圖像(三維到二維)和圖像到激光雷達(dá)(二維到三維)兩部分.一方面為了將兩者放入同一個目標(biāo)函數(shù)下,另一方面由于激光點的稀疏性所帶來的插值誤差以及圖像投影點的低紋理性帶來的跟蹤誤差,需進(jìn)行數(shù)據(jù)關(guān)聯(lián)上的置信度估計vf.

在三維激光點到二維的對應(yīng)求解上,給定一個第k幀的激光點P k,i,如果對應(yīng)投影點pk,i為圖像特征點,那么兩者進(jìn)行直接關(guān)聯(lián),置信度為1.如果對應(yīng)投影點pk,i不是圖像特征點,需找到鄰近區(qū)域的特征點pk,1,pk,2,···,pk,n,用這些特征點的跟蹤實現(xiàn)投影點的跟蹤.在數(shù)據(jù)關(guān)聯(lián)置信度上,通過計算投影點周圍一小塊圖像塊的紋理特征顯著度來決定置信度的大小.我們以3×3 區(qū)域內(nèi)的梯度與亮度值作為描述子,比較投影點pk,i與投影點周圍像素之間的相似度,加權(quán)平均得到平均相似度ˉs.置信度定義如下:

在二維圖像點到三維點的對應(yīng)求解上,給定一個第k幀的圖像點pk,i,如果需要通過插值第j幀的激光點得到對應(yīng)三維點,那么就包含同步以及插值誤差.同步誤差包含在系統(tǒng)置信度中,因此只需考慮插值所產(chǎn)生的誤差.在對應(yīng)點求解的過程中,我們采用平面擬合的方式確定三維點所在平面,并通過平面與圖像點對應(yīng)射線的交點來確定三維點.因此,平面擬合的準(zhǔn)確度就決定了三維點的置信度.我們假設(shè)在擬合平面后,這n個三維激光點與平面的距離分別為d1,···,dn,則數(shù)據(jù)關(guān)聯(lián)的置信度與距離的均方相關(guān),定義為:

4.3 物體遮擋

由于激光掃描儀與相機(jī)的視角不同,以及環(huán)境內(nèi)移動物體的干擾,激光與相機(jī)的數(shù)據(jù)關(guān)聯(lián)上可能產(chǎn)生錯誤匹配.圖5為數(shù)據(jù)示例.左上圖為相機(jī)圖像,左下圖為激光投影到圖像中的結(jié)果.在正確標(biāo)定的基礎(chǔ)上,觀察立柱上的激光點,有部分點偏離立柱,原因是激光與相機(jī)在水平位置上有偏差(激光偏右),導(dǎo)致兩個傳感器所觀察到的內(nèi)容有所不同.圖5 右圖為對應(yīng)示意.其中圓形點為激光和相機(jī)正確匹配的點,方形點為錯誤匹配的點,包含激光掃描到前景,而相機(jī)看到背景的點;以及激光掃描到背景,相機(jī)看到前景的點.從圖中可以看到,物體邊界處所帶來的遮擋影響較大,因此以局部深度差作為置信度估計的相關(guān)量.

給定一個圖像點p,令其對應(yīng)激光點為P,并且深度值為D.假設(shè)是圖像點周圍一小塊區(qū)域內(nèi)激光點的平均深度值,物體遮擋的置信度vo與深度差相關(guān),定義為:

4.4 特征跟蹤

對于一個三維激光點P k,i,其對應(yīng)幀的二維投影點為pk,i.如果pk,i是一個特征點,那么在第j幀上的跟蹤點是直接通過特征跟蹤得到.跟蹤所得的匹配度sc即為特征跟蹤置信度vt.

圖5 數(shù)據(jù)融合時,傳感器視點不同造成的物體遮擋示意Fig.5 Example of occlusion in camera-LiDAR fusion caused by different point of view

如果投影點pk,i所在區(qū)域為非特征點區(qū)域,需由投影點臨近的特征點進(jìn)行跟蹤.令pk,1,···,pk,n為周圍的圖像特征點,在第j幀中,跟蹤這些圖像特征點,并插值生成的投影點pk,i在第j幀上的跟蹤點如圖3 所示.影響跟蹤置信度的因素包含鄰近點的跟蹤置信度以及鄰近點相對位置的穩(wěn)定性兩方面.定義scx為第x個臨近特征點的跟蹤匹配度,dx,k為第x個鄰近點在k幀中與投影點的距離.由于在機(jī)器人運動過程中,圖像特征點尺度變化明顯,鄰近點的絕對位置變化較大,但相對位置穩(wěn)定,因此定義相對距離比例

跟蹤置信度vt定義如下:

5 實驗結(jié)果

5.1 概述

實驗使用小型輪式機(jī)器人作為實驗平臺,在越野和公園兩個典型非結(jié)構(gòu)化環(huán)境進(jìn)行實驗,如圖6 所示.平臺所含設(shè)備包括一個Bumblebee 雙目相機(jī),一個Hokuyo 單線激光(UTM-30LX),一個旋轉(zhuǎn)電機(jī),一個單點GPS,一個低精度IMU 和輪速編碼器等.其中,單點GPS 在所述實驗環(huán)境無法收到衛(wèi)星信號,因此未在實驗中使用.Bumblebee 雙目相機(jī)的分辨率為1 024×768,最高幀率為18 Hz.單線激光水平掃描視場角為180 度,角分辨率為0.5 度,幀率約為37 Hz.電機(jī)帶動單線激光進(jìn)行上下周期性搖擺運動,周期約為2 秒.實驗過程中,機(jī)器人通過手動遙控的方式進(jìn)行控制.考慮到電機(jī)搖擺周期的限制,實驗過程中機(jī)器人速度不超過1.2 m/s,在轉(zhuǎn)彎時,機(jī)器人速度不超過0.5 m/s.

實驗采用本文提出的相機(jī)和搖擺激光的融合定位算法,并考慮了式(6)所述的四項置信度.其中系統(tǒng)和數(shù)據(jù)關(guān)聯(lián)置信度聯(lián)合定義為S,物體遮擋置信度定義為O,特征跟蹤置信度定義為T.實驗比較了本文算法與雙目libviso[15]視覺里程計算法,單目或雙目orb-SLAM[2]算法和航位推算法(odom)等定位算法的精度誤差,說明定位結(jié)果的有效性.其中雙目libviso 通過左右相機(jī)的特征點匹配,生成單幀的三維點云,并通過前后幀的特征點匹配,計算幀間位姿估計.算法只包含前后幀的匹配定位,不包含多幀捆集調(diào)整.orb-SLAM通過orb 特征,先匹配前后幀特征點生成幀間位姿估計,再通過局部的捆集調(diào)整優(yōu)化位姿估計,最后間隔一定幀數(shù)選取關(guān)鍵幀,生成詞袋模型用于閉環(huán)檢測.本文提出的融合定位算法通過相機(jī)與激光雷達(dá)的特征對應(yīng),最小化局部重投影誤差進(jìn)行幀間位姿估計.算法利用激光雷達(dá)的距離信息生成局部三維點云,并將特征點投影至相鄰圖像幀上用于特征對應(yīng)與位姿估計,不顯式生成三維點云地圖.因此,算法只進(jìn)行局部幀間位姿估計,不通過地圖進(jìn)行閉環(huán)檢測.

圖6 非結(jié)構(gòu)化環(huán)境和機(jī)器人平臺Fig.6 Off-road environments and robot platform

為保證各個定位算法所使用數(shù)據(jù)的一致性,實驗采用數(shù)據(jù)仿在線回放的方式進(jìn)行算法測試.通過數(shù)據(jù)回放模塊,利用同步采集的相機(jī)、激光雷達(dá)、電機(jī)、慣導(dǎo)、編碼器等數(shù)據(jù),模擬數(shù)據(jù)采集過程中的傳感器數(shù)據(jù)流,對定位算法進(jìn)行測試.該測試方式一方面保證了各個定位算法所使用數(shù)據(jù)一致性,另一方面通過數(shù)據(jù)回放模塊和算法模塊的分離,達(dá)到仿在線測試目的.

此外,為了說明本文提出的置信度定義對特征點的篩選和定位的有效作用,實驗中還對比了添加各類置信度的后特征點數(shù)目與定位結(jié)果比較.在此,我們定義不加置信度定義的方法為our,添加本文所有置信度內(nèi)容的方法為our +SOT.在置信度的參數(shù)λ上,所有包含λ參數(shù)的置信度定義公式,如式(8)等,形式上均為:

當(dāng)δ=0 時,v=1 置信度最大.隨著δ增加,v不斷減小.當(dāng)λ·δ=1 時,v=0.5.在本實驗中,采用手工調(diào)整λ使得v=0.5 時,δ取到了合適的經(jīng)驗值.具體定義如下:

1)式(8)中λc的經(jīng)驗值為距離D=30 m;

2)式(10)中λc的經(jīng)驗值為時間差δt=100 ms;

3)式(12)中λfI的經(jīng)驗值為距離方差

4)式(14)中λo的經(jīng)驗值為深度差

5)式(16)中λt的經(jīng)驗值為相對距離方差ˉd2=0.2.

5.2 特征跟蹤與置信度

典型非結(jié)構(gòu)化環(huán)境如圖6(a)和圖6(b)所示,環(huán)境無明顯結(jié)構(gòu)特性,地面崎嶇不平,且光照條件一般.由于車載電腦配置受限,相機(jī)的幀率降至5 Hz,相鄰的激光幀數(shù)據(jù)將直接關(guān)聯(lián)到對應(yīng)圖像幀.單數(shù)據(jù)投影如圖7(a)所示.由于激光和相機(jī)視角不同,激光的部分觀測在圖像中被前景遮擋,但在單幀激光數(shù)據(jù)中無法有效提取(如圖7(a)中矩形框區(qū)域內(nèi),部分激光觀測被圖像中的樹干遮擋).當(dāng)一次電機(jī)上下?lián)u擺周期內(nèi)的激光點全部投影至同一幀圖像中時,可有效地去除被遮擋區(qū)域(如圖7(b)矩形框區(qū)域所示),生成物體遮擋置信度圖O.

圖7 激光點投影到視頻結(jié)果Fig.7 Projection results of LiDAR points

特征置信度估計是本方法的另一重要因素.圖8 所示為特征跟蹤與置信度估計結(jié)果.所示特征點的大小表示跟蹤時間的長(越大表示跟蹤越久),而亮度則表示置信度短(越亮表示置信度越高).從圖中可以看出,特征點主要集中在圖像中下方區(qū)域,也是機(jī)器人導(dǎo)航的最重要區(qū)域.

圖8 特征跟蹤與置信度結(jié)果Fig.8 Feature tracking and confidence measure results

5.3 定位結(jié)果

5.3.1 越野環(huán)境

越野實驗環(huán)境如圖6(a)所示,越野環(huán)境地面起伏不平,光照條件較差.由于非結(jié)構(gòu)化環(huán)境下軌跡真值標(biāo)注的困難性,實驗順時針行駛一段約35 米的閉合軌跡,以軌跡首尾的閉合誤差作為度量方式.比較航位推算法(odom),orb-SLAM(含單目orb-mono 和雙目orb-stereo),雙目libviso(viso-stereo)和本文含置信度估計的融合定位算法(our+SOT)等方法結(jié)果,如圖9 所示.

在圖9(a)所示二維閉合誤差上,航位推算法在地面起伏不平的路面下精度較低,閉合誤差大;單目視覺定位算法由于不包含尺度信息,閉合誤差較大;本文相機(jī)與激光融合定位整體軌跡與雙目視覺定位算法類似,在軌跡首尾閉合精度上略高于雙目視覺定位算法.考慮到越野環(huán)境的三維運動,僅使用二維閉合誤差無法準(zhǔn)確衡量定位的精度.因此,實驗還對比了各個方法之間的高度(Z)值,如圖9(b)所示.從圖中結(jié)果來看,本文算法在最終高度閉合估計上優(yōu)于其他算法(如圖中點虛線所示).

圖9 越野環(huán)境定位結(jié)果Fig.9 Localization results in off-road environment

5.3.2 公園環(huán)境

公園環(huán)境如圖6(b)所示,相比于越野環(huán)境,公園環(huán)境路面平整度增加,加強了運動約束,但行人較多,給特征點的對應(yīng)帶來更多干擾.實驗車逆時針行駛一段約250 米的閉合軌跡,結(jié)果如圖10 所示.

從圖10(a)中的二維閉合誤差處分析,航位推算法由于地面起伏,閉合精度無法有效提高;單目視覺定位算法無法有效保證全局尺度的一致性,帶來較大定位偏差;viso-stereo算法在第一次拐彎后誤差一直增加,最終的閉合誤差超過10米;orb-stereo 算法雖然在閉合上精度上較高,但第一次拐彎處的形狀與其他方法有很大偏差;本文方法結(jié)果在整體形狀上與其他的多數(shù)方法類似,最終閉合精度略高于orb-stereo算法.

從圖10(b)中高度信息進(jìn)行分析,整體軌跡起伏明顯,達(dá)到10 米以上.本文算法與orb-stereo 雖然在閉合處高度接近,但機(jī)器人在p1至p2間行駛的過程中大致在平路上,而orb-stereo 高度結(jié)果產(chǎn)生明顯起伏.

圖10 公園環(huán)境定位結(jié)果Fig.10 Localization results in park area

5.3.3 數(shù)值結(jié)果分析

通過比較閉環(huán)起始點和終止點之間的偏差,定量分析定位精度,兩組實驗的數(shù)值結(jié)果如表1 所示.航位推算法無法估計三維精度,在二維精度上與雙目定位算法相似.單目視覺定位算法由于尺度估計不準(zhǔn)確,偏差較大,尤其是公園環(huán)境.在越野環(huán)境中,viso-stereo 精度較高;在公園環(huán)境中,orb-stereo 精度較高,原因可能是libviso 的特征描述子受到光照干擾較小,而orb-SLAM 具有三維重建與定位優(yōu)化,行人的影響較小.本文算法在兩處實驗的精度均高于雙目視覺定位算法,說明了算法的有效性.

表1 不同定位算法閉合誤差(m)Table 1 Results of different localization algorithms(m)

5.4 重投影誤差與定位結(jié)果相關(guān)性

本文使用最小化重投影誤差的方式估計機(jī)器人位姿,并使用特征的置信度估計對特征進(jìn)行約束.為了證明重投影誤差與定位結(jié)果的相關(guān)性,在越野環(huán)境實驗中,我們對不同算法的重投影誤差和定位結(jié)果進(jìn)行整理,結(jié)果如表2 所示.此處,硬件系統(tǒng)和數(shù)據(jù)關(guān)聯(lián)置信度統(tǒng)一為系統(tǒng)置信度,記做S,物體遮擋置信度記做O,特征跟蹤置信度記做T.

表2 重投影誤差與定位結(jié)果相關(guān)性Table 2 Correlation of re-projection errors and localization error

首先,對于雙目定位結(jié)果(orb-stereo 和viso-stereo),由于激光與相機(jī)本身存在標(biāo)定與配準(zhǔn)誤差,而定位結(jié)果(雙目)和用于重投影誤差解算的傳感器(單目+激光)不一致,導(dǎo)致重投影誤差較大.對于本文基準(zhǔn)算法,即不加任何置信度估計的算法,特征點數(shù)目與雙目結(jié)果接近,平均重投影誤差比雙目小,但定位誤差卻更大,原因是激光雷達(dá)與視覺融合的系統(tǒng)誤差比雙目系統(tǒng)高.在加入系統(tǒng)誤差置信度分析后(our+S),特征點數(shù)目少量減少,重投影誤差也相應(yīng)減少,定位誤差也隨之略微減少.通過逐漸加入遮擋置信度(O)和跟蹤置信度(T),整體特征點數(shù)目減少至244.52,重投影誤差減少至3.40,定位誤差減少至0.109 米,與雙目系統(tǒng)相比有了顯著的提高.

綜上,盡管激光雷達(dá)與相機(jī)融合定位系統(tǒng)的硬件誤差比雙目相機(jī)大,在加入置信度分析后,基于相機(jī)與激光的定位算法減少了無效的特征點數(shù)據(jù),有效地減少了重投影誤差,提高了定位精度.

6 總結(jié)與展望

針對非結(jié)構(gòu)化環(huán)境的定位難點,本文提出了一種基于相機(jī)和搖擺激光雷達(dá)融合的定位算法.算法結(jié)合激光雷達(dá)的深度信息以及圖像中的顏色紋理信息,通過激光雷達(dá)到圖像,圖像到激光雷達(dá)的兩種映射,構(gòu)建在時序幀間的特征點匹配關(guān)系;引入一種結(jié)合系統(tǒng)誤差、數(shù)據(jù)關(guān)聯(lián)、物體遮擋、特征跟蹤等因素的置信度評價方式,量化評價特征點匹配中的不確定性,最終將定位問題轉(zhuǎn)化為特征點對的加權(quán)重投影誤差優(yōu)化問題予以解決.

實驗利用小型輪式移動機(jī)器人在越野和公園等典型非結(jié)構(gòu)化環(huán)境下進(jìn)行的數(shù)據(jù)采集和結(jié)果分析,對提出的基于相機(jī)和搖擺激光雷達(dá)融合的定位算法進(jìn)行驗證.結(jié)果表明,與前沿的單目或雙目視覺定位算法相比,本文算法可提高在非結(jié)構(gòu)化環(huán)境中的定位精度.與此同時,實驗還進(jìn)行置信度與定位精度的相關(guān)性分析,說明本文置信度評價方法可有效地抑制數(shù)據(jù)融合,特征提取與匹配等不確定性因素對定位精度的影響.

考慮到在不同環(huán)境(包括路面、衛(wèi)星定位、環(huán)境特征等條件)不同定位方案各有其優(yōu)劣,后續(xù)的工作將研究基于環(huán)境評估的環(huán)境自適應(yīng)融合定位解決方案.另一方面,由于激光搖擺周期過長,一定程度上限制了機(jī)器人的移動速度,尤其是轉(zhuǎn)彎速度,今后的研究也將嘗試搖擺激光的控制中加入反饋機(jī)制,確保激光雷達(dá)的掃描范圍處于最重要的區(qū)域內(nèi).