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

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

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

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

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

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

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

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

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

本文的主要貢獻(xiàn)是將激光雷達(dá)與基于2.5D柵格地圖的半直接單目視覺方法相結(jié)合，將其應(yīng)用于復(fù)雜的室外環(huán)境，大大降低了復(fù)雜環(huán)境數(shù)據(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)的起點。

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

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

圖1 SLO 算法整體框架

1.3 創(chuàng)建高度圖

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

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

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

1.4 跟蹤線程初始化

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

1.5 位姿估計

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

圖3 跟蹤線程流程

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

式中

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

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

式中：

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

然后，用高斯牛頓法求出擾動

式中。H=JTJ

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

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

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

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

相比于基于角點的圖像對齊粗測，基于點的特征對齊選取了8×8的圖像塊范圍來比較均值差，可以得到亞像素級別的精度。

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

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

2 SLO 算法驗證實驗

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

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

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

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

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

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

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

表3 ICP 和NDT 的全程累計誤差

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

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

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

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

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

圖8 東北大學(xué)324 室實驗室場景圖匹配實驗

圖9 東北大學(xué)教學(xué)樓3 樓走廊場景圖匹配實驗

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

表4 點云精簡效果

3 結(jié)束語

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

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