基于激光雷達(dá)和視覺融合的機(jī)器人SLAM應(yīng)用研究

摘 要 ：通過研究激光雷達(dá)和視覺傳感器融合 SLAM，實(shí)現(xiàn)雙目視覺傳感器對(duì)單線激光雷達(dá)點(diǎn)云的補(bǔ)充，以提高建圖精度。實(shí)現(xiàn)方案以 2D 激光雷達(dá)點(diǎn)云數(shù)據(jù)為主，雙目視覺傳感器作為激光雷達(dá)點(diǎn)云盲區(qū)的補(bǔ)充，搭建 SLAM實(shí)驗(yàn)平臺(tái)，完成機(jī)器人實(shí)時(shí)地圖構(gòu)建并獲取當(dāng)前位置信息，同時(shí)降低攜帶傳感器的成本。

0 引 言

隨著機(jī)器人技術(shù)的發(fā)展，越來越多的機(jī)器人已經(jīng)實(shí)現(xiàn)自主運(yùn)動(dòng)，但可靠性和制作成本依然制約著機(jī)器人的普及。通過使用相較3D激光雷達(dá)傳感器成本更低的2D激光雷達(dá)和視覺傳感器進(jìn)行點(diǎn)云數(shù)據(jù)融合，實(shí)現(xiàn)平臺(tái)搭建，優(yōu)化機(jī)器人的SLAM技術(shù)，解決當(dāng)前2D激光雷達(dá)存在的感知單一平面點(diǎn)云數(shù)據(jù)不足、視覺傳感器數(shù)據(jù)計(jì)算量大等問題［1-2］，提升地圖精度，減少SLAM應(yīng)用過程中的不確定性。

1 實(shí)驗(yàn)平臺(tái)搭建

研究基于ROS構(gòu)建由2D激光雷達(dá)和深度相機(jī)點(diǎn)云融合的SLAM［3］平臺(tái)，通過NVIDIA Jetson TX2運(yùn)算平臺(tái)和STM32微控制器通信傳送數(shù)據(jù)，實(shí)現(xiàn)機(jī)器人自主運(yùn)動(dòng)。實(shí)驗(yàn)平臺(tái)包括點(diǎn)云數(shù)據(jù)識(shí)別部分和運(yùn)動(dòng)部分，整體結(jié)構(gòu)如圖1所示。

云數(shù)據(jù)識(shí)別部分包括激光雷達(dá)獲取平面點(diǎn)云數(shù)據(jù)和雙目相機(jī)深度點(diǎn)云計(jì)算。運(yùn)行時(shí)激光雷達(dá)獲取2D平面內(nèi)障礙物點(diǎn)云信息，雙目相機(jī)獲取機(jī)器人正面范圍的影像［4］，通過Stereo Matching 計(jì)算可視范圍內(nèi)的深度點(diǎn)云數(shù)據(jù)，并對(duì)其進(jìn)行范圍剔除和距離篩選 ；最后將雙目相機(jī)獲得的點(diǎn)云與激光雷達(dá)點(diǎn)云序列進(jìn)行比較更新，融合點(diǎn)云數(shù)據(jù)后根據(jù)占據(jù)柵格地圖算法構(gòu)建地圖［5］。點(diǎn)云數(shù)據(jù)融合過程如圖1所示。

運(yùn)動(dòng)部分通過單片機(jī)STM32F4從NUC獲取運(yùn)動(dòng)速度向量或從遙控中獲取控制信息，經(jīng)PID解算將電流或電壓值發(fā)送給底盤電機(jī)，電機(jī)連接聯(lián)軸器帶動(dòng)麥克納姆輪轉(zhuǎn)動(dòng)，從而實(shí)現(xiàn)全方向移動(dòng)［6］。機(jī)器人整體設(shè)計(jì)如圖1所示。

2 研究與測(cè)試

圍繞ROS節(jié)點(diǎn)話題進(jìn)行研究和測(cè)試。雙目相機(jī)點(diǎn)云生成節(jié)點(diǎn)（cameraNode），將相機(jī)點(diǎn)云（/camera_cloud）傳送給點(diǎn)云數(shù)據(jù)處理節(jié)點(diǎn)（/transNode）處理，得到深度數(shù)據(jù)（/depth）；激光雷達(dá)點(diǎn)云生成節(jié)點(diǎn)，得到點(diǎn)云數(shù)據(jù)（/laser），二者由點(diǎn)云合成節(jié)點(diǎn)（/mergeNode）融合得到點(diǎn)云數(shù)據(jù)（/scan），再將其輸出到 HectorMapping 的節(jié)點(diǎn)。點(diǎn)云融合建圖部分 ROS 節(jié)點(diǎn)與話題如圖2 所示 ［7］。

2.1 雙目校正與像素匹配

使用金乾象工業(yè)相機(jī)中的兩個(gè)自制雙目深度相機(jī)，需要保證左右畫面的同步和亮度，否則會(huì)導(dǎo)致深度圖的計(jì)算結(jié)果不佳。因此，在相機(jī)點(diǎn)云生成（/camera_cloud）之前，需要對(duì)左右相機(jī)讀取的原圖像進(jìn)行雙目校正和像素匹配。

雙目校正根據(jù)攝像頭標(biāo)定后獲得的單目內(nèi)參（焦距、成像原點(diǎn)、畸變系數(shù)）和雙目相對(duì)位置關(guān)系（旋轉(zhuǎn)矩陣和平移向量），分別對(duì)左右視圖進(jìn)行消除畸變和行對(duì)準(zhǔn)，使得左右視圖的成像原點(diǎn)坐標(biāo)一致、攝像頭光軸與對(duì)極線行對(duì)齊。原圖像與校正后圖像對(duì)比情況如圖3所示。

像素匹配通過匹配代價(jià)計(jì)算、代價(jià)聚合、視差計(jì)算和視差優(yōu)化等步驟 ［8］，對(duì)左右視圖上對(duì)應(yīng)的像素點(diǎn)進(jìn)行匹配得到視差圖。依靠視差圖，結(jié)合雙目相機(jī)的內(nèi)參，計(jì)算得到正交空間下的點(diǎn)云數(shù)據(jù)。

2.2 點(diǎn)云數(shù)據(jù)處理

由于雙目相機(jī)參數(shù)的差異，幀速率不能完全同步，獲取的點(diǎn)云數(shù)據(jù)存在一定誤差，需要先通過點(diǎn)云數(shù)據(jù)處理節(jié)點(diǎn)（/transNode）變換和剔除數(shù)據(jù)，再通過點(diǎn)云合成節(jié)點(diǎn)（/mergeNode）融合同種格式的數(shù)據(jù)。

正交空間下的點(diǎn)云通過透視除法的逆變換得到相機(jī)空間下的點(diǎn)云數(shù)據(jù)，獲得相對(duì)于相機(jī)實(shí)際位置的坐標(biāo)。坐標(biāo)變換與效果如圖4所示。

數(shù)據(jù)轉(zhuǎn)換部分將點(diǎn)云投影到二維平面，并轉(zhuǎn)換為激光雷達(dá)點(diǎn)云格式的數(shù)據(jù)，即角度和對(duì)應(yīng)方向的深度值［9］。轉(zhuǎn)換時(shí)，通過反正切函數(shù)，根據(jù)點(diǎn)云位置的水平偏移與深度值計(jì)算出角度和距離值，篩選取得各個(gè)角度上距離最近的點(diǎn)云，效果如圖5所示。在融合點(diǎn)云之前，應(yīng)當(dāng)剔除視錐內(nèi)不需要的部分，效果如圖5中截取點(diǎn)云后的投影圖所示 ；否則地面和天花板將對(duì)投影后的點(diǎn)云造成影響，效果如圖5中的原點(diǎn)云投影圖所示?？紤]到雙目左右圖像的邊緣缺少完整的匹配信息，點(diǎn)云可能出現(xiàn)極不穩(wěn)定的深度值，效果如圖5中未進(jìn)行邊緣剔除的投影圖所示。因此，在能夠反映完整深度值的情況下，剔除部分邊緣深度值，效果如圖5中邊緣剔除后的投影圖所示。

2.3 點(diǎn)云融合建圖

點(diǎn)云合成節(jié)點(diǎn)將深度數(shù)據(jù)和激光雷達(dá)點(diǎn)云數(shù)據(jù)融合，并篩選出各個(gè)角度距離最近的點(diǎn)云輸出到 HectorMapping 的節(jié)點(diǎn)。

經(jīng)過測(cè)試，增加雙目相機(jī)后的建圖能夠獲取單線雷達(dá)掃描平面外的細(xì)節(jié)，自身位姿估計(jì)和建圖結(jié)果更加精確，測(cè)試對(duì)比結(jié)果如圖6所示。但與工業(yè)級(jí)產(chǎn)品相比穩(wěn)定性差，建圖過程中會(huì)有微小偏差。

3 結(jié) 語

本文通過激光雷達(dá)提供大范圍點(diǎn)云信息，視覺傳感器提供機(jī)器人正面區(qū)域補(bǔ)充激光雷達(dá)缺失的點(diǎn)云，較單線雷達(dá)大范圍稀疏點(diǎn)云提高了計(jì)算準(zhǔn)確度，使用Hector SLAM算法融合點(diǎn)云數(shù)據(jù)，更好地實(shí)現(xiàn)機(jī)器人點(diǎn)云獲取和建圖［10］。

審核編輯 ：李倩