楊濤，蘆迪，翟凱，陳旭

（中國(guó)礦業(yè)大學(xué)（北京） 機(jī)電與信息工程學(xué)院，北京， 100083）

0 引言

精確和魯棒的環(huán)境地圖是移動(dòng)機(jī)器人進(jìn)行自主移動(dòng)的前提，通過(guò)構(gòu)建致密的環(huán)境地圖，可以為移動(dòng)機(jī)器人進(jìn)行的定位、導(dǎo)航和路徑規(guī)劃等操作提供良好的依據(jù)[1～4]。

建圖的方法根據(jù)使用傳感器的不同分為兩類(lèi)：一類(lèi)為視覺(jué)傳感器建圖，包括單目相機(jī)、雙目相機(jī)和深度相機(jī)等；另一類(lèi)為激光雷達(dá)建圖[5]。單一傳感器建圖存在著明顯的缺陷：使用單目相機(jī)建圖雖然能夠獲取豐富的環(huán)境紋理信息，卻無(wú)法得到具體的深度信息；雙目相機(jī)可以按照三角測(cè)量的原理，由極線匹配的方式實(shí)現(xiàn)深度值的估計(jì)，但是受到基線長(zhǎng)度的限制僅在近距離有良好的測(cè)距效果；深度相機(jī)具有良好的測(cè)距效果，但是在室外環(huán)境受光照的影響大；激光雷達(dá)通過(guò)發(fā)出的激光束能準(zhǔn)確地測(cè)量出深度信息，但是受到激光點(diǎn)云稀疏性的限制導(dǎo)致無(wú)法獲得致密地圖。

本文將采用多傳感器信息融合的方式，用激光雷達(dá)彌補(bǔ)單目相機(jī)無(wú)法獲取深度信息的缺陷，用單目相機(jī)彌補(bǔ)激光雷達(dá)場(chǎng)景稀疏的不足，研究融合單目相機(jī)、三維激光雷達(dá)的三維致密地圖構(gòu)建方法。

1 聯(lián)合標(biāo)定

單目相機(jī)與激光雷達(dá)的聯(lián)合標(biāo)定是進(jìn)行信息融合的前提，也是致密地圖構(gòu)建的基礎(chǔ)。良好的聯(lián)合標(biāo)定結(jié)果可以使信息融合更加準(zhǔn)確，從而使構(gòu)建的地圖能夠更好地還原和表達(dá)環(huán)境信息，為機(jī)器人的自主導(dǎo)航和路徑規(guī)劃提供精確的參考依據(jù)。激光雷達(dá)與相機(jī)的聯(lián)合標(biāo)定的本質(zhì)是分別在圖像和三維點(diǎn)云中獲得足夠的像素點(diǎn)坐標(biāo)和空間點(diǎn)云坐標(biāo)，找到其對(duì)應(yīng)關(guān)系，通過(guò)對(duì)多對(duì)匹配點(diǎn)的計(jì)算得出聯(lián)合標(biāo)定的外參結(jié)果[6～7]。

本文中進(jìn)行的聯(lián)合標(biāo)定主要是對(duì)標(biāo)定板進(jìn)行聯(lián)合標(biāo)定。標(biāo)定板選用的是8行6列的棋盤(pán)標(biāo)定板，標(biāo)定板中每一個(gè)小方格的邊長(zhǎng)為12cm，在進(jìn)行標(biāo)定時(shí)同時(shí)采集棋盤(pán)標(biāo)定板的激光和相機(jī)數(shù)據(jù)。

圖1 聯(lián)合標(biāo)定示意圖

聯(lián)合標(biāo)定在autoware的Calibration Toolkit模塊中進(jìn)行，標(biāo)定結(jié)束后自動(dòng)計(jì)算出標(biāo)定結(jié)果。這種標(biāo)定方式通過(guò)對(duì)標(biāo)定板識(shí)別，在抓取的激光點(diǎn)云中找到同一時(shí)間圖像中標(biāo)定版的位置，可以實(shí)時(shí)標(biāo)定，也可以通過(guò)錄制數(shù)據(jù)包實(shí)現(xiàn)離線標(biāo)定。其中離線標(biāo)定受到環(huán)境條件的影響較小，所以本文采用離線標(biāo)定的方式。

在錄制數(shù)據(jù)包的時(shí)候，標(biāo)定板的站位以及姿態(tài)也要滿足要求。棋盤(pán)標(biāo)定板放置在距離鏡頭大約5m左右的位置，在鏡頭的視野中對(duì)標(biāo)定板設(shè)置三個(gè)定位點(diǎn)，分別位于相機(jī)視野的左側(cè)、中間和右側(cè)。為了使標(biāo)定結(jié)果更加準(zhǔn)確，需要抓取不同姿態(tài)下的標(biāo)定板。因此在三個(gè)定位點(diǎn)上要進(jìn)行不同姿態(tài)的擺放，其姿態(tài)依次為：正向、下俯、上仰和向內(nèi)側(cè)偏移。在標(biāo)定過(guò)程中，通過(guò)對(duì)這12組姿態(tài)的信息進(jìn)行標(biāo)定，可以得到準(zhǔn)確的聯(lián)合標(biāo)定效果。

標(biāo)定過(guò)程中，需要對(duì)點(diǎn)云的視角進(jìn)行縮放和平移，將點(diǎn)云和圖像的視角進(jìn)行同步，在兩個(gè)視角中可以分別清晰地辨別出標(biāo)定版的位置和姿態(tài)，在每個(gè)定位點(diǎn)對(duì)兩個(gè)視角進(jìn)行捕獲，通過(guò)觀察點(diǎn)云視角上的點(diǎn)云信息并與圖片視角中標(biāo)定板的位置做比較，在其上標(biāo)注出標(biāo)定版的位置，通過(guò)標(biāo)定可以得到相機(jī)外參矩陣、相機(jī)內(nèi)參矩陣、畸變參數(shù)和圖像的尺寸。

具體結(jié)果如圖2所示。

圖2 聯(lián)合標(biāo)定結(jié)果

由標(biāo)定結(jié)果可知，相機(jī)坐標(biāo)系與雷達(dá)坐標(biāo)系進(jìn)行轉(zhuǎn)換的旋轉(zhuǎn)矩陣R和平移向量t分別為：

2 激光點(diǎn)云的投影與信息融合

單目相機(jī)和激光雷達(dá)的聯(lián)合標(biāo)定為空間任意一點(diǎn)從三維點(diǎn)云到相機(jī)圖像平面的轉(zhuǎn)換提供了轉(zhuǎn)換矩陣，在此基礎(chǔ)上實(shí)現(xiàn)傳感器數(shù)據(jù)的信息融合。圖3為激光雷達(dá)坐標(biāo)系LO-XL-LY-ZL，相機(jī)坐標(biāo)系cO-Xc-cY-Zc、圖像坐標(biāo)系O-X-Y和像素坐標(biāo)系u-v之間的關(guān)系。

圖3 坐標(biāo)系關(guān)系

設(shè)空間一點(diǎn)P，其在雷達(dá)坐標(biāo)系下的坐標(biāo)為(X L,YL,ZL)，在相機(jī)坐標(biāo)系下的坐標(biāo)為（X C,YC,ZC），在圖像坐標(biāo)系下的坐標(biāo)為（x,y），在像素坐標(biāo)系下的坐標(biāo)為（u，v）。

由聯(lián)合標(biāo)定所求出來(lái)的轉(zhuǎn)換矩陣可得，點(diǎn)P從雷達(dá)坐標(biāo)系向相機(jī)坐標(biāo)系的變換表示為：

由相機(jī)坐標(biāo)系向圖像坐標(biāo)系的變換表示為：

由圖像坐標(biāo)系向像素坐標(biāo)系的變換表示為：

將式(3)、式(4)、式(5)聯(lián)立，可得激光雷達(dá)轉(zhuǎn)換為像素坐標(biāo)系的關(guān)系表示為：

由此將激光雷達(dá)坐標(biāo)系轉(zhuǎn)換為像素坐標(biāo)系的問(wèn)題，轉(zhuǎn)化為求解矩陣D的問(wèn)題，其中相機(jī)內(nèi)參和焦距f由出廠數(shù)據(jù)可查到，坐標(biāo)系之間的轉(zhuǎn)換矩陣K由已經(jīng)由聯(lián)合標(biāo)定獲得，因此可以求解出矩陣D，從而實(shí)現(xiàn)將三維激光點(diǎn)云數(shù)據(jù)投影到二維圖像上。

由公式(7)可以較為精確地將三維點(diǎn)云中的點(diǎn)與二維圖像上的像素進(jìn)行對(duì)應(yīng)，通過(guò)計(jì)算將點(diǎn)云投影到圖像平面，利用與像素的對(duì)應(yīng)關(guān)系，將每個(gè)激光點(diǎn)云點(diǎn)所攜帶的深度信息融合到相應(yīng)的像素點(diǎn)中，完成深度賦值，使每個(gè)像素點(diǎn)包含了(x,y,z,R,G,B)六個(gè)參數(shù)信息，即在圖像上的位置信息、深度信息和環(huán)境彩色信息，完成信息的融合。

3 三角測(cè)量

由于激光雷達(dá)獲取的點(diǎn)云數(shù)據(jù)是稀疏的，導(dǎo)致激光雷達(dá)與單目相機(jī)的信息融合使部分像素點(diǎn)獲得了深度信息。為了能夠構(gòu)建致密的環(huán)境地圖，本文利用三角測(cè)量對(duì)其余的像素深度進(jìn)行估計(jì)。

三角測(cè)量的方法最初被廣泛運(yùn)用于單目相機(jī)地圖構(gòu)建當(dāng)中，因?yàn)樵诮▓D時(shí)，僅憑單張圖像是無(wú)法獲取環(huán)境深度值的，需要通過(guò)兩張甚至多張對(duì)目標(biāo)進(jìn)行深度估計(jì)。三角測(cè)量具體指的是通過(guò)兩個(gè)不同的視角對(duì)同一個(gè)目標(biāo)進(jìn)行觀測(cè)，根據(jù)兩個(gè)位置的觀測(cè)夾角以及兩個(gè)位置之間的距離計(jì)算出觀測(cè)目標(biāo)的距離。三角測(cè)量最早由高斯提出，如今在多個(gè)領(lǐng)域都發(fā)揮著重要的作用[8]。

設(shè)I1和I2分別為兩個(gè)圖像平面，矩陣T為從平面I1到平面I2的變換矩陣。兩個(gè)平面內(nèi)分別有一個(gè)特征點(diǎn)p1和p2，O1和O2分別為相機(jī)在兩個(gè)位置拍攝時(shí)的光心。如果不考慮其他因素的影響，理論上直線O1p1與O2p2會(huì)相交于特征點(diǎn)p1和p2在三維場(chǎng)景中的真實(shí)位置，即圖4中的點(diǎn)P。但是由于環(huán)境噪聲等因素的影響，直線O1p1與O2p2往往不會(huì)相交于點(diǎn)P。

圖4 三角測(cè)量示意圖

根據(jù)極幾何中的定義中，它們滿足：

式中s1和s2分別為特征點(diǎn)的深度；x1和x2為特征點(diǎn)的歸一化坐標(biāo)，R和t為兩個(gè)圖像平面之間的旋轉(zhuǎn)矩陣和平移向量。

現(xiàn)在已經(jīng)知道了R,t，想要求解的是s1和s2。當(dāng)然這兩個(gè)深度是可以分開(kāi)求的。如果要算s2，那么先對(duì)上式兩側(cè)左乘一個(gè)得：

當(dāng)式(9)左側(cè)為零時(shí)，右側(cè)就只含有s2一個(gè)未知數(shù)，可直接求出s2。當(dāng)s2已知時(shí)，s1也可以求出。據(jù)此得到點(diǎn)P的深度。以上情況是不考慮環(huán)境噪聲的影響而求出來(lái)的深度值?？紤]到噪聲的存在，式(9)不能精確得為零。所以在實(shí)際中，需要通過(guò)最小二乘法來(lái)估計(jì)。

4 圖像的反投影

為了使構(gòu)建的環(huán)境地圖更加直觀清晰，需要將信息融合后的圖像反投影回三維空間形成三維點(diǎn)云地圖。由單目相機(jī)和激光雷達(dá)的聯(lián)合標(biāo)定結(jié)果可以得到各傳感器坐標(biāo)系之間的轉(zhuǎn)化關(guān)系，通過(guò)這種轉(zhuǎn)化關(guān)系能夠找到二維圖像中的像素點(diǎn)在空間中所對(duì)應(yīng)的三維點(diǎn)的位置，并且將其所攜帶的深度信息和彩色信息表示在三維空間，形成三維點(diǎn)云。

二維圖像投影到三維空間其本質(zhì)還是坐標(biāo)的變換，其公式為：

其中x，y，z是點(diǎn)云坐標(biāo)系，x′，y′是圖像坐標(biāo)系，D為深度值。

圖5為圖像中任意點(diǎn)m（u，v）向世界坐標(biāo)系進(jìn)行投影的過(guò)程。設(shè)投影后在世界坐標(biāo)系下的坐標(biāo)為M（Xw，Yw，Zw）。

圖5 世界坐標(biāo)向圖像坐標(biāo)的投影

公式化表示如下：

式（11）中u和v為像素坐標(biāo)系中任意一點(diǎn)的坐標(biāo)，u0和v0為像素坐標(biāo)系的中心坐標(biāo)，xω，yω，zω表示在世界坐標(biāo)系中任意一點(diǎn)的坐標(biāo)，zc表示相機(jī)坐標(biāo)的z軸值，即目標(biāo)到相機(jī)的距離。

在本文中，由于相機(jī)坐標(biāo)系與世界坐標(biāo)系三個(gè)軸的指向都相同，為了減少計(jì)算量，將世界坐標(biāo)系與相機(jī)坐標(biāo)系設(shè)為了同一個(gè)坐標(biāo)系，即相機(jī)沒(méi)有旋轉(zhuǎn)和平移，同時(shí)在相機(jī)坐標(biāo)系和世界坐標(biāo)系下任何的環(huán)境深度都相同，即zc=zω，于是公式可進(jìn)一步簡(jiǎn)化為：

由式（12）的變化，可以計(jì)算得到像素點(diǎn)(u,v)到世界坐標(biāo)點(diǎn)(xω,yω,zω)的變換公式：

5 致密地圖構(gòu)建

本次實(shí)驗(yàn)所用的傳感器系統(tǒng)如圖6所示。本實(shí)驗(yàn)中使用的移動(dòng)平臺(tái)為Scout移動(dòng)機(jī)器人，整個(gè)系統(tǒng)包括搭載了計(jì)算平臺(tái)為Intel Core i7-8700K CPU的迷你主機(jī)、單目相機(jī)和激光雷達(dá)等傳感器。其中，3D激光雷達(dá)VLP-16安放在移動(dòng)平臺(tái)前端鋼梁的最頂層，這能夠使得激光雷達(dá)具有良好的掃描視角，在其工作進(jìn)行掃描時(shí)，避免了車(chē)身系統(tǒng)的遮擋物對(duì)掃描的干擾；單目相機(jī)安裝在鋼架的第二層，激光雷達(dá)的正下方，可以和激光雷達(dá)保持相同的視野，在聯(lián)合標(biāo)定以及后續(xù)建圖時(shí)提供了很大的方便。

圖6 建圖平臺(tái)

如圖7和圖8所示，單目相機(jī)只能獲得環(huán)境的紋理信息和彩色信息，無(wú)法獲取深度信息，只能構(gòu)建二維地圖，激光雷達(dá)獲取了環(huán)境深度信息，但其稀疏性使得對(duì)環(huán)境表達(dá)不夠豐富。將兩種傳感器獲取的信息進(jìn)行融合致密化后得到的三維點(diǎn)云地圖既包含了環(huán)境的深度信息，又能準(zhǔn)確地表達(dá)環(huán)境的紋理和色彩，構(gòu)建的地圖具有較高的還原性和魯棒性。

圖7 對(duì)標(biāo)定板的建圖

圖8 實(shí)驗(yàn)室一角建圖

圖9為對(duì)實(shí)驗(yàn)室內(nèi)一個(gè)邊長(zhǎng)大約6m的場(chǎng)景進(jìn)行建圖。整體效果展現(xiàn)較好。由三維點(diǎn)云地圖可清楚地看到信息融合以及深度估計(jì)都取得良好效果，場(chǎng)景紋理信息還原度較高，實(shí)驗(yàn)桌上書(shū)架、顯示器、水杯等物品輪廓容易辨別，環(huán)境表達(dá)清晰，證明本文提出的致密地圖構(gòu)建方法可行。

6 結(jié)束語(yǔ)

單一傳感器受自身特性的局限性無(wú)法單獨(dú)構(gòu)建三維環(huán)境的致密地圖。而多傳感器的信息融合能夠使兩種傳感器進(jìn)行優(yōu)勢(shì)互補(bǔ)，彌補(bǔ)單一傳感器建圖的不足，并且該建圖方法能夠適用于大場(chǎng)景和復(fù)雜的室外場(chǎng)景，具有良好的研究?jī)r(jià)值。