基于半直接法的高度激光里程計(jì)

高珂，肖輝勇，姜宇凡，李瓊峰，楊濟(jì)源，王斐

1. 東北大學(xué) 信息科學(xué)與工程學(xué)院，遼寧 沈陽(yáng) 110819

2. 湖南柿竹園有色金屬有限責(zé)任公司，湖南 郴州 423037

3. 西北工業(yè)大學(xué) 瑪麗女王工程學(xué)院，陜西 西安 710129

4. 東北大學(xué) 機(jī)器人工程學(xué)院，遼寧 沈陽(yáng) 110819

隨著國(guó)家對(duì)礦山安全要求指標(biāo)不斷提高和對(duì)井下作業(yè)人員安全的重視，目前的井下作業(yè)系統(tǒng)已經(jīng)不足以滿足信息化的需求。物聯(lián)網(wǎng)、無(wú)人駕駛以及5G 等新一代技術(shù)日趨成熟，基于同步定位與建圖(simultaneous localization and mapping，SLAM)技術(shù)的井下作業(yè)自動(dòng)導(dǎo)向車(chē)(automated guided vehicle，AGV)的研究逐漸成為熱點(diǎn)。對(duì)于井下作業(yè)AGV 來(lái)說(shuō)，在降低計(jì)算成本的同時(shí)，保證AGV 運(yùn)動(dòng)的穩(wěn)定性也非常重要。在SLAM 系統(tǒng)中，最重要的部分之一是前端部分，它通常執(zhí)行逐幀姿態(tài)估計(jì)，從而形成里程計(jì)子系統(tǒng)[1]。目前主流的里程計(jì)分為視覺(jué)里程計(jì)（visual odometry，VO）和激光雷達(dá)里程計(jì)（liDAR odometry，LO）2類(lèi)[2]。視覺(jué)里程計(jì)在紋理不豐富、光照變化明顯的環(huán)境下魯棒性較差；相比之下，激光里程計(jì)通過(guò)匹配激光點(diǎn)云數(shù)據(jù)完成幀間的匹配從而進(jìn)行運(yùn)動(dòng)估計(jì)，在光照條件不足的情況下魯棒性較強(qiáng)，是更加穩(wěn)定的選擇。但是激光雷達(dá)采集的數(shù)據(jù)中過(guò)多的點(diǎn)云數(shù)量造成了激光里程計(jì)計(jì)算成本過(guò)高的問(wèn)題[3]。于是，研究人員開(kāi)始嘗試將激光與視覺(jué)傳感器的優(yōu)勢(shì)融合在一起，形成激光雷達(dá)視覺(jué)里程計(jì)，這樣的結(jié)合可能在各種場(chǎng)景環(huán)境中尤其是在特殊結(jié)構(gòu)或者光照不足的環(huán)境中會(huì)具有更強(qiáng)的魯棒性[4]。但是使激光里程計(jì)融合1 臺(tái)或多臺(tái)攝像機(jī)需要更復(fù)雜的設(shè)置，他們的融合并不容易；并且，同時(shí)使用2 種以上的傳感器很難使系統(tǒng)速度更快。

目前現(xiàn)有的視覺(jué)SLAM 與激光SLAM 方法根據(jù)其地圖的表示方式可分為2D、3D 和2.5D 這3 種[5]。其中由于2D 能體現(xiàn)的信息較少，因此不能應(yīng)用于許多開(kāi)放的室外環(huán)境；而3D 地圖的信息量雖然較大，但一般效率較低[6]；采用2.5D 的表示方法，可以縮小點(diǎn)云的維數(shù)和計(jì)算代價(jià)，從而實(shí)現(xiàn)精度與計(jì)算代價(jià)之間的平衡[7]。

在基于直接法的SLAM 算法中，瑞士蘇黎世大學(xué)的研究人員[8]提出了基于半直接法的半直接視頻里程計(jì)（semi-direct monocular visual odometry，SVO）。SVO 算法通過(guò)對(duì)關(guān)鍵幀提取（features from accelerated segment test，F(xiàn)AST）視覺(jué)點(diǎn)特征來(lái)構(gòu)建新地圖點(diǎn)，并采用深度濾波器進(jìn)行地圖點(diǎn)的深度估計(jì)。因?yàn)樗惴ㄗ饔糜谙∈璧南袼攸c(diǎn)，因此能夠在機(jī)載嵌入式計(jì)算機(jī)上以55 f/s 的速度運(yùn)行。同濟(jì)大學(xué)的研究人員[1]使用基于2.5D 網(wǎng)格圖的直接LO 方法用于室外環(huán)境中的自動(dòng)駕駛汽車(chē)。該方法借鑒了SVO 結(jié)構(gòu)，將視覺(jué)中的直接方法與ORB (oriented fast and rotated brief)方法[9]中的FAST 關(guān)鍵點(diǎn)相結(jié)合，專(zhuān)注于解決運(yùn)算性能問(wèn)題。該方案是基于視覺(jué)方法的高效激光測(cè)距里程計(jì)，不僅充分利用高度圖信息，也在一定程度上降低了直接法的不確定性；并且提出的方法使用激光雷達(dá)代替相機(jī)，解決了傳統(tǒng)VO 受光照影響較大的問(wèn)題，可以在大多數(shù)系統(tǒng)上實(shí)現(xiàn)高效定位。到目前為止，研究人員已經(jīng)在這一領(lǐng)域進(jìn)行了大量的研究工作，但是想要在復(fù)雜的環(huán)境中保持系統(tǒng)的輕量化與高效率仍然面臨較大的挑戰(zhàn)[10]。

受到以上研究啟發(fā)，本文專(zhuān)注于使用純?nèi)S激光點(diǎn)云數(shù)據(jù)完成快速精確的定位，而無(wú)需任何額外的視覺(jué)傳感器或強(qiáng)度信息。同時(shí)，為了節(jié)省計(jì)算資源，本文提出了基于半直接法的高度激光里程計(jì)(height laser odometer based on semi-direct method，SLO)方法，將三維激光點(diǎn)云投影至指定平面上，然后存儲(chǔ)每個(gè)點(diǎn)云在世界坐標(biāo)系中的高度信息，將其點(diǎn)云高度平均值作為圖像中該像素點(diǎn)的灰度值，生成2.5D 深度圖像，從而避免了對(duì)三維點(diǎn)云的直接計(jì)算。因?yàn)橐曈X(jué)方法中的灰度不變假設(shè)是一個(gè)很強(qiáng)的假設(shè)，在實(shí)際應(yīng)用中很難成立。對(duì)此，本文算法將灰度不變假設(shè)改進(jìn)為高度不變假設(shè)，在激光里程計(jì)中使用視覺(jué)里程計(jì)中的方法，基于高度不變假設(shè)[11]進(jìn)行位姿估計(jì)，這在現(xiàn)實(shí)中比灰度不變假設(shè)的魯棒性更強(qiáng)。

本文的主要貢獻(xiàn)是將激光雷達(dá)與基于2.5D柵格地圖的半直接單目視覺(jué)方法相結(jié)合，將其應(yīng)用于復(fù)雜的室外環(huán)境，大大降低了復(fù)雜環(huán)境數(shù)據(jù)計(jì)算成本，保持了系統(tǒng)的輕量化和高效率。

1 SLO 系統(tǒng)概述

1.1 預(yù)處理

首先給定圖像序列和激光雷達(dá)序列，其次假設(shè)已經(jīng)完成了激光數(shù)據(jù)球坐標(biāo)向笛卡爾坐標(biāo)的變換。本文將激光的局部坐標(biāo)設(shè)置為本體坐標(biāo)，將世界坐標(biāo)設(shè)置為本體坐標(biāo)的起點(diǎn)。

1.2 系統(tǒng)總體框架

系統(tǒng)的整體框架如圖1 所示，該框架包含位姿估計(jì)與局部地圖構(gòu)建2 個(gè)運(yùn)行線程。在運(yùn)動(dòng)估計(jì)過(guò)程中，SVO 將運(yùn)動(dòng)估計(jì)與建圖分成了2 個(gè)并行計(jì)算的線程。它基于構(gòu)建的稀疏地圖點(diǎn)，首先通過(guò)最小化圖像之間的光度誤差，初步計(jì)算了每幀圖像的位姿；然后通過(guò)優(yōu)化視覺(jué)點(diǎn)特征位置并最小化重投影誤差，進(jìn)一步優(yōu)化了位姿估計(jì)。由于算法作用于稀疏的圖像像素點(diǎn)，因此它能夠在CPU 上以很高的幀率運(yùn)行。本文提出的SLO 算法參照了SVO 的架構(gòu)，綜合了直接法和特征點(diǎn)法的優(yōu)點(diǎn)，設(shè)計(jì)了3 步策略。此方案能夠很好地處理稀疏的3D 點(diǎn)云數(shù)據(jù)，從而比一般的激光里程計(jì)更加輕量，比基于直接法的視覺(jué)里程計(jì)更加魯棒。

圖1 SLO 算法整體框架

1.3 創(chuàng)建高度圖

三維激光里程計(jì)獲取的大量點(diǎn)云數(shù)據(jù)可以顯示環(huán)境物體的位置和高度，但點(diǎn)云數(shù)據(jù)是在重建過(guò)程中采集的，這消耗了大量的處理器硬件資源。因此，本文主要關(guān)注點(diǎn)云表征數(shù)據(jù)，以減少CPU 的計(jì)算壓力。為了識(shí)別地面，提出了2 個(gè)假設(shè)：一是假設(shè)傳感器大致安裝在水平移動(dòng)底座上；二是假設(shè)地面的曲率很低。

采用灰度值保存高度信息的方法，在不犧牲物體高度信息的情況下，將3D 點(diǎn)云轉(zhuǎn)換為2D 圖像，保留了2D 網(wǎng)格圖易于存儲(chǔ)和構(gòu)建的優(yōu)點(diǎn)，同時(shí)也保留了高度信息，生成的高度圖如圖2 所示。

圖2 激光點(diǎn)云數(shù)據(jù)轉(zhuǎn)化

1.4 跟蹤線程初始化

算法首先初始化變換矩陣T為單位矩陣，檢測(cè)第1 張灰度圖像的特征點(diǎn)，若灰度圖像中檢測(cè)的特征點(diǎn)數(shù)量超過(guò)設(shè)定的閾值，就把這張圖像作為第1 個(gè)關(guān)鍵幀。然后從第1 幀圖像開(kāi)始，使用LK (lucas-kanade)光流法持續(xù)跟蹤特征點(diǎn)，并計(jì)算從第1 幀圖像開(kāi)始的連續(xù)圖像與第1 幀圖像的視差，如果視差中位數(shù)大于閾值，就認(rèn)為初始化成功，并將與此對(duì)應(yīng)的幀設(shè)定為第2 個(gè)關(guān)鍵幀，將提取的特征點(diǎn)作為地圖點(diǎn)加入局部地圖，進(jìn)入正常跟蹤模式。

1.5 位姿估計(jì)

在運(yùn)動(dòng)估計(jì)線程中，SVO 算法基于構(gòu)建的稀疏地圖點(diǎn)，通過(guò)最小化圖像之間的光度誤差，初步計(jì)算每幀圖像的位姿；然后通過(guò)最小化重投影誤差優(yōu)化了視覺(jué)點(diǎn)特征的位置，進(jìn)一步優(yōu)化了位姿估計(jì)[12]。本文SLO 算法位姿估計(jì)跟蹤線程流程如圖3 所示。

圖3 跟蹤線程流程

1）基于角點(diǎn)的圖像對(duì)齊。使用稀疏直接法粗略計(jì)算當(dāng)前幀的位置和姿態(tài)，并建立損失函數(shù)(最小化光度誤差)，利用高斯–牛頓（Gauss-Newton，GN）優(yōu)化算法得到幀間的2 個(gè)位姿變換。目標(biāo)函數(shù)為

式中

其中：π-1(u,du)為根據(jù)高度圖像素位置u逆投影到三維空間后得到的三維點(diǎn)坐標(biāo)p，T·π-1(u,du)為將三維坐標(biāo)點(diǎn)旋轉(zhuǎn)平移到當(dāng)前幀坐標(biāo)系下，π(T·π-1(u,du))為將當(dāng)前幀坐標(biāo)系下的三維坐標(biāo)點(diǎn)投影到當(dāng)前幀高度圖像坐標(biāo)，I(u)為特征點(diǎn)周?chē)?×4的像素塊的高度值。由于這一步是一個(gè)粗略的計(jì)算，為了加快計(jì)算速度，因此沒(méi)有進(jìn)行仿射變換。

利用GN 法求解2 個(gè)高度映射的轉(zhuǎn)移矩陣T，即2 個(gè)柵格地圖之間的最小高度差誤差[1]。根據(jù)直接法和鏈?zhǔn)椒▌t，雅克比矩陣Ji可分為3 部分。

式中：

像素梯度計(jì)算使用雙線性插值方法計(jì)算，即

然后，用高斯牛頓法求出擾動(dòng)

式中。H=JTJ

最后，判斷此幀是否滿足關(guān)鍵幀要求，若滿足則插入局部地圖，并在這個(gè)新的關(guān)鍵幀上檢測(cè)新的特征點(diǎn)作為地圖點(diǎn)。

2）基于點(diǎn)的特征對(duì)齊。關(guān)鍵幀中提取的特征點(diǎn)在被賦予深度后得到的三維點(diǎn)被稱(chēng)為地圖點(diǎn)，由于本文使用的灰度圖為激光點(diǎn)云投影生成的圖片，激光關(guān)鍵幀中的關(guān)鍵點(diǎn)信息伴隨著深度信息，他們的灰度值就是深度，即整個(gè)圖片相當(dāng)于“上帝視角”的圖片，故本文算法去除了SVO 中的深度估計(jì)步驟，地圖點(diǎn)經(jīng)過(guò)篩選后可直接放入到局部地圖中[13]。

完成角點(diǎn)的圖像對(duì)齊后，算法獲得了當(dāng)前幀雷達(dá)的位姿，因此可以通過(guò)重投影將地圖點(diǎn)投影到當(dāng)前幀中，而正是由于這種幀與幀之間的位姿估計(jì)會(huì)有累計(jì)誤差，所以投影后的地圖點(diǎn)在新的圖像幀中的投影位置也會(huì)隨之產(chǎn)生誤差。因此算法可以根據(jù)高度不變假設(shè)構(gòu)造一個(gè)殘差，對(duì)于每個(gè)特征點(diǎn)單獨(dú)考慮，對(duì)特征預(yù)測(cè)位置進(jìn)行優(yōu)化，即

式中：Ik(ui′)為當(dāng)前第K幀圖像中第i個(gè)特征點(diǎn)的亮度值（即高度值）；后一部分為Ir(ui)而不是Ik-1(ui)，因?yàn)樗歉鶕?jù)局部地圖投影的地圖點(diǎn)追溯到這個(gè)地圖點(diǎn)所在的關(guān)鍵幀中的像素值，而不是相鄰幀。

相比于基于角點(diǎn)的圖像對(duì)齊粗測(cè)，基于點(diǎn)的特征對(duì)齊選取了8×8的圖像塊范圍來(lái)比較均值差，可以得到亞像素級(jí)別的精度。

3）位姿迭代優(yōu)化。利用優(yōu)化后的特征塊預(yù)測(cè)位置，使用直接法再次對(duì)位姿構(gòu)建優(yōu)化函數(shù)進(jìn)行估計(jì)：

最后建立局部地圖，判斷這個(gè)幀是否是關(guān)鍵幀，如果是關(guān)鍵幀，提取新的特征點(diǎn)。

2 SLO 算法驗(yàn)證實(shí)驗(yàn)

采用KITTI 數(shù)據(jù)集[14]與AGV 移動(dòng)平臺(tái)來(lái)驗(yàn)證所提出的算法。在本實(shí)驗(yàn)中，使用HDL-16 激光雷達(dá)在東北大學(xué)渾南校區(qū)收集數(shù)據(jù)來(lái)進(jìn)行實(shí)驗(yàn)測(cè)試。

將圖像分成固定大小的單元（像素值大小為800×800）。算法中的參數(shù)設(shè)置為：處理點(diǎn)云數(shù)據(jù)截取的窗口的寬度Iwidth=400、高度Iheight=400，灰度圖的每個(gè)柵格的大小fx=0.1、fy=0.1，像素值倍增系數(shù)cx=200、cy=200。

正態(tài)分布變換與迭代最近點(diǎn)方法是傳統(tǒng)的處理三維激光點(diǎn)云的算法，因此本文首先對(duì)比了他們的性能。本部分實(shí)驗(yàn)全部為完成畸變補(bǔ)償?shù)那闆r下進(jìn)行，在實(shí)驗(yàn)中分別比對(duì)了PCL (point cloud library)中集成的迭代最近點(diǎn)(iterative closest point，ICP)算法、正態(tài)分布變換 (normal distributions transform, NDT)算法以及改進(jìn)的基于高斯牛頓算法的ICP 算法。實(shí)驗(yàn)記錄了每50 和100 m 的階段誤差以及全程的累計(jì)誤差，階段誤差實(shí)驗(yàn)結(jié)果對(duì)比如圖4 所示。數(shù)據(jù)如表1 和表2 所示，由數(shù)據(jù)可以看出在3 種算法的每50 m 均方根階段誤差(root mean squared error, RMSE)和每100 m 的均方根階段誤差的對(duì)比實(shí)驗(yàn)中，NDT 算法的效果最好，基于高斯牛頓的ICP 算法次之。

表1 ICP 和NDT 的分段距離誤差/50 m

表2 ICP 和NDT 的分段距離誤差/100 m

圖4 3 個(gè)匹配算法的位姿估計(jì)結(jié)果與真值的對(duì)比

累計(jì)誤差的實(shí)驗(yàn)結(jié)果圖如圖5 所示，在全程的累計(jì)誤差中，基于高斯牛頓的改進(jìn)ICP 算法顯示出了更好的優(yōu)勢(shì)，數(shù)據(jù)如表3 所示。

表3 ICP 和NDT 的全程累計(jì)誤差

本文提出的SLO 算法在預(yù)處理部分參考了DLO[1]中的方法，算法使用KITTI 數(shù)據(jù)集中序列07 的軌跡中的某幀點(diǎn)云來(lái)測(cè)試提出的投影方法，算法生成灰度圖后使用LK 光流跟蹤算法[15]跟蹤的結(jié)果如圖6 所示。

圖6 某幀灰度圖LK 光流法的效果

算法針對(duì)相鄰兩幀點(diǎn)云進(jìn)行了ORB 特征點(diǎn)[16]的提取與匹配，特征點(diǎn)的匹配結(jié)果如圖7 所示，可以看出匹配結(jié)果絕大多數(shù)是正確的。

圖7 某幀灰度圖的ORB 特征點(diǎn)匹配結(jié)果

本文在真實(shí)環(huán)境中分別進(jìn)行了灰度圖的生成以及匹配實(shí)驗(yàn)，實(shí)驗(yàn)選用的平臺(tái)為AGV 移動(dòng)平臺(tái)，該平臺(tái)配置16 線激光雷達(dá)，雷達(dá)測(cè)量距離為100 m 左右，垂直測(cè)量角度30°（±15°），掃描過(guò)程中從下往上計(jì)數(shù)，-15°記為初始線（第0 線），一共16 線。使用鐳神16 線激光雷達(dá)進(jìn)行實(shí)驗(yàn)的結(jié)果如圖8 和圖9 所示。

圖8 東北大學(xué)324 室實(shí)驗(yàn)室場(chǎng)景圖匹配實(shí)驗(yàn)

圖9 東北大學(xué)教學(xué)樓3 樓走廊場(chǎng)景圖匹配實(shí)驗(yàn)

SLO 算法將點(diǎn)云處理成高度圖階段的精簡(jiǎn)效果如表4 所示。由表4 中數(shù)據(jù)可以看出，本文提出的投影方法有效地縮少了點(diǎn)云的原始數(shù)據(jù)；并且在對(duì)高度圖進(jìn)行特征點(diǎn)提取后，算法可以在保證場(chǎng)景環(huán)境特征的基礎(chǔ)上完成對(duì)數(shù)據(jù)大數(shù)量級(jí)的精簡(jiǎn)。

表4 點(diǎn)云精簡(jiǎn)效果

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

由于井下作業(yè)AGV 平臺(tái)的應(yīng)用場(chǎng)景可以是室內(nèi)也可以是室外，因此本文選擇了一種不受環(huán)境因素影響的激光里程計(jì)來(lái)估計(jì)平臺(tái)的姿態(tài)?？紤]到激光數(shù)據(jù)處理需要大量的計(jì)算量，本文選擇將點(diǎn)云數(shù)據(jù)轉(zhuǎn)換為高信息特征點(diǎn)，它將三維點(diǎn)云轉(zhuǎn)換為灰度，可大大節(jié)省數(shù)據(jù)存儲(chǔ)和計(jì)算，并且通過(guò)應(yīng)用半直接法，提高了精度，使整個(gè)過(guò)程輕量化。此外，當(dāng)圖像特征匹配時(shí)，使用迭代最近點(diǎn)法執(zhí)行姿勢(shì)估計(jì)，這使得最小二乘問(wèn)題實(shí)際上具有解析解，并且不需要迭代優(yōu)化。

本文提出的SLO 方法合理地結(jié)合了視覺(jué)里程計(jì)和激光里程計(jì)的優(yōu)點(diǎn)。在未來(lái)的研究中，基于高度圖提取點(diǎn)和線特征會(huì)更容易，基于投影構(gòu)建拓?fù)鋱D也是一個(gè)值得嘗試的思路。