基于相機(jī)與搖擺激光雷達(dá)融合的非結(jié)構(gòu)化環(huán)境定位

俞毓鋒 趙卉菁

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

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

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

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

1 相關(guān)研究

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

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

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

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

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

2 問(wèn)題定義

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

圖1 問(wèn)題定義Fig.1 Problem statement

如圖1 所示,(P k,i,pk,i)表示在第k幀下第i個(gè)特征匹配點(diǎn)對(duì).一方面,通過(guò)跟蹤圖像點(diǎn)pk,i,可獲得其在相鄰幀上的圖像跟蹤點(diǎn),j=···,k ?2,k ?1,k,k+1,···.另一方面,三維激光點(diǎn)P k,i可通過(guò)坐標(biāo)變換投影至對(duì)應(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,得若定位無(wú)誤差,且特征對(duì)應(yīng)與跟蹤準(zhǔn)確,則在第j幀圖像上,基于圖像特征點(diǎn)跟蹤的結(jié)果和基于坐標(biāo)系轉(zhuǎn)換的投影結(jié)果為同一點(diǎn).由于定位誤差的存在,或特征對(duì)應(yīng)與跟蹤時(shí)的誤差影響,跟蹤點(diǎn)與投影點(diǎn)之間存在偏差.定義第k幀中的第i個(gè)特征在第j幀中的跟蹤點(diǎn)與投影點(diǎn)之間的偏差為特征的重投影誤差,即

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 置信度估計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4.3 物體遮擋

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

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

4.4 特征跟蹤

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

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

如果投影點(diǎn)pk,i所在區(qū)域?yàn)榉翘卣鼽c(diǎn)區(qū)域,需由投影點(diǎn)臨近的特征點(diǎn)進(jìn)行跟蹤.令pk,1,···,pk,n為周?chē)膱D像特征點(diǎn),在第j幀中,跟蹤這些圖像特征點(diǎn),并插值生成的投影點(diǎn)pk,i在第j幀上的跟蹤點(diǎn)如圖3 所示.影響跟蹤置信度的因素包含鄰近點(diǎn)的跟蹤置信度以及鄰近點(diǎn)相對(duì)位置的穩(wěn)定性?xún)煞矫?定義scx為第x個(gè)臨近特征點(diǎn)的跟蹤匹配度,dx,k為第x個(gè)鄰近點(diǎn)在k幀中與投影點(diǎn)的距離.由于在機(jī)器人運(yùn)動(dòng)過(guò)程中,圖像特征點(diǎn)尺度變化明顯,鄰近點(diǎn)的絕對(duì)位置變化較大,但相對(duì)位置穩(wěn)定,因此定義相對(duì)距離比例

跟蹤置信度vt定義如下:

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

5.1 概述

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

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

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

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

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

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

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

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

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

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

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

5.2 特征跟蹤與置信度

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

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

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

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

5.3 定位結(jié)果

5.3.1 越野環(huán)境

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

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

圖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)境路面平整度增加,加強(qiáng)了運(yùn)動(dòng)約束,但行人較多,給特征點(diǎn)的對(duì)應(yīng)帶來(lái)更多干擾.實(shí)驗(yàn)車(chē)逆時(shí)針行駛一段約250 米的閉合軌跡,結(jié)果如圖10 所示.

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

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

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

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

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

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

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

本文使用最小化重投影誤差的方式估計(jì)機(jī)器人位姿,并使用特征的置信度估計(jì)對(duì)特征進(jìn)行約束.為了證明重投影誤差與定位結(jié)果的相關(guān)性,在越野環(huán)境實(shí)驗(yàn)中,我們對(duì)不同算法的重投影誤差和定位結(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

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

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

6 總結(jié)與展望

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

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

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