楊濤，蘆迪，翟凱，陳旭

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

0 引言

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

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

本文將采用多傳感器信息融合的方式，用激光雷達(dá)彌補(bǔ)單目相機(jī)無法獲取深度信息的缺陷，用單目相機(jī)彌補(bǔ)激光雷達(dá)場景稀疏的不足，研究融合單目相機(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ì)是分別在圖像和三維點云中獲得足夠的像素點坐標(biāo)和空間點云坐標(biāo)，找到其對應(yīng)關(guān)系，通過對多對匹配點的計算得出聯(lián)合標(biāo)定的外參結(jié)果[6～7]。

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

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

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

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

標(biāo)定過程中，需要對點云的視角進(jìn)行縮放和平移，將點云和圖像的視角進(jìn)行同步，在兩個視角中可以分別清晰地辨別出標(biāo)定版的位置和姿態(tài)，在每個定位點對兩個視角進(jìn)行捕獲，通過觀察點云視角上的點云信息并與圖片視角中標(biāo)定板的位置做比較，在其上標(biāo)注出標(biāo)定版的位置，通過標(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 激光點云的投影與信息融合

單目相機(jī)和激光雷達(dá)的聯(lián)合標(biāo)定為空間任意一點從三維點云到相機(jī)圖像平面的轉(zhuǎn)換提供了轉(zhuǎn)換矩陣，在此基礎(chǔ)上實現(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è)空間一點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)定所求出來的轉(zhuǎ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)系的問題，轉(zhuǎn)化為求解矩陣D的問題，其中相機(jī)內(nèi)參和焦距f由出廠數(shù)據(jù)可查到，坐標(biāo)系之間的轉(zhuǎn)換矩陣K由已經(jīng)由聯(lián)合標(biāo)定獲得，因此可以求解出矩陣D，從而實現(xiàn)將三維激光點云數(shù)據(jù)投影到二維圖像上。

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

3 三角測量

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

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

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

圖4 三角測量示意圖

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

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

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

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

4 圖像的反投影

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

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

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

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

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

公式化表示如下：

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

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

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

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

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

圖6 建圖平臺

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

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

圖8 實驗室一角建圖

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

6 結(jié)束語

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