一種基于單目相機與激光雷達信息融合構(gòu)建致密地圖方法的實現(xiàn)

楊濤，蘆迪，翟凱，陳旭

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

0 引言

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

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

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

1 聯(lián)合標定

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

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

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

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

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

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

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

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

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

2 激光點云的投影與信息融合

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

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

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

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

由相機坐標系向圖像坐標系的變換表示為：

由圖像坐標系向像素坐標系的變換表示為：

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

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

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

3 三角測量

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

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

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

圖4 三角測量示意圖

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

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

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

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

4 圖像的反投影

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

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

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

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

圖5 世界坐標向圖像坐標的投影

公式化表示如下：

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

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

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

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

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

圖6 建圖平臺

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

圖7 對標定板的建圖

圖8 實驗室一角建圖

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

6 結(jié)束語

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