基於ROI的NDT重定位鐳射雷達視場設定方法
期刊：MDPI
單位：山東理工大學
摘要: 在高精度地圖自動導航的重定點陣圖中，鐳射雷達布放和視場選擇起到檢測車輛相對位置和位姿的作用。當鐳射雷達視場被遮擋或鐳射雷達位置錯位時，很容易導致重定位丟失或重定位精度低。針對遮擋視場過大時NDT重定位誤差大、位置丟失的問題，提出一種基於高精度地圖正態分佈變換（NDT）重定位的鐳射雷達佈局與視場選擇方法。為了模擬真實的放置環境以及鐳射雷達被障礙物遮擋的情況，採用ROI演算法對鐳射雷達點雲進行切割，得到不同尺寸的鐳射雷達點雲資料。切點雲資料首先被下采樣，然後重新定位。 NDT重定位的下采樣點被記錄為有效匹配點。使用 RMSE 值和有效匹配點最佳化 LiDAR 點雲資料的方向和角度設定。結果表明，在路況複雜的城市場景中，單位角度內前後匹配點數量多於左右匹配點數量。 NDT重定位演算法的匹配點越多，重定位精度越高。增加前後鐳射雷達視野可防止重新定位丟失。透過增加左右鐳射雷達視場可以提高重定位精度。
關鍵詞：正態分佈變換；高精度地圖；自動導航
1. 簡介
在過去的二十年裡，遷移在汽車駕駛應用中發揮了至關重要的作用。重定位用於根據構建的地圖獲取車輛的全球位置和位姿。利用全球導航衛星系統（GNSS）[1-3]、慣性測量單元（IMU）[4,5]、攝像頭、鐳射雷達和其他感測感測器，各種重新定位方法已成功解決此類問題。 GNSS 和 IMU 的整合是解決這些問題的常見解決方案[6-8]。然而，GNSS 很容易出現訊號干擾或丟失的情況，例如在城市建築中。當前的解決方案是使用里程資訊來補償 GNSS 測量結果。然而，在自動駕駛相關應用中的效能仍然普遍不足。相機因其固有的高資訊內容、低成本和小尺寸而成為最有吸引力的重定位感測器之一[9-11]。視覺重定位利用攝像頭提供的大量資訊來估計機器人位置。由於在GPS訊號較弱的情況下需要準確可靠的定位，鄭志勇提出了一種新的陸地車輛定位視覺測量框架[12]。然而，對於戶外大規模搬遷來說，確保其穩健執行仍然非常具有挑戰性，尤其是在不斷變化的環境和惡劣的天氣條件下。 Kaixin Yang提出了一種由語義資訊和機率資料關聯組成的協調定位策略，提高了動態交通環境中SLAM（同步定位和建圖）的準確性[13]。然而，城市中的許多牆壁和窗戶都是由玻璃或乙烯基製成的，在白天很容易暴露在光線下。
解決城市問題的一個越來越流行的解決方案是完全停止使用 GNSS 和相機測量，而依靠鐳射雷達來測量環境的 3D 掃描。與相機和GNSS相比，鐳射雷達具有更好的穿透性和抗干擾特性。 LiDAR 使用距離測量來檢測物體和表面，距離測量是根據反射光脈衝的飛行時間計算得出的[14-16]。此外，鐳射雷達系統獲取的資料量比相機少。因此，鐳射雷達感測器經常用於定位和物體檢測。然而，大多數採用現有點雲地圖的基於鐳射雷達的定位解決方案都假設道路場景相對恆定，因此新建築、路邊植被、物體變化造成的部分遮擋可能會嚴重損害其魯棒性。因此，一個有趣但懸而未決的問題是鐳射雷達是否可以用於大規模變化環境中的魯棒重定位。
在大規模、動態的城市環境中，基於高精度地圖的正態分佈變換（NDT）演算法是汽車駕駛應用中廣泛使用的主要重定位演算法之一[17,18]。與 SLAM 演算法相比，NDT 演算法對於大型點雲更有效。 NDT配准演算法耗時且穩定，與初始值相關性很小，並且在初始誤差較大時能夠很好地進行修正。名古屋大學開源的Autoware演算法主要採用NDT演算法。 Autoware演算法利用NDT匹配配准演算法來獲取自身在LiDAR點雲地圖中的位置和位置資訊。然而，對於環境的顯著幾何變化或過於意外或動態的物體，NDT 演算法並不穩健[19,20]。這些缺點嚴重影響了基於高精度地圖的掃描匹配的效能。 NDT重定位演算法可以處理一些環境變化，例如物體的意外遮擋（LiDAR感測器被樹葉或其他車輛部分遮擋），這在城市中是不可避免的。當環境與地圖存在差異時，無損檢測的定位精度會降低。在實際應用中，多采用多鐳射雷達進行定位。鐳射雷達的位置直接決定了鐳射雷達的視場角。鐳射雷達視場的不同視角直接影響車輛的定位精度。許多研究並未具體評估不同位置的點雲遮擋與基於高精度地圖的無創檢測效能之間的關係。
2. 演算法原理
2.1. Loam
LOAM（Lidar odometry and Mapping in Real Time）是 Ji Zhu 等人提出的一種基於 3D LiDAR 的高精度實時定位與建圖演算法。 [21,22]。 LOAM框架的核心分為兩部分，高頻里程計和低頻對映，如圖1所示。
圖1 LOAM演算法的系統結構圖。
里程計透過高頻、少量的點雲進行掃描匹配，估計兩幀之間的運動關係，並將結果輸出給建圖演算法；使用掃描地圖匹配方法 [23,24]，對映以 1 Hz 的頻率將未失真的點雲與地圖進行匹配和對齊。最後，綜合兩種演算法建立的姿態變換，得到LiDAR的變換輸出，其對地圖的姿態約為10 Hz。我們首先從 LiDAR 雲中提取特徵點 pk。
我們選擇銳邊和平表面塊上的特徵點。令 i 為 pk 中的一個點，i ∈ pk 並令 S 為 LiDAR 掃描器在同一掃描中返回的 i 的連續點的集合。這定義了一個評估區域性表面平滑度的術語：
本文根據上述公式計算點的曲率。實際中，我們只需要比較一個點的曲率，就可以在一個點周圍五個點之差的平方座標中求出該點的曲率。這樣，我們就可以求出每個點的曲率c，透過比較曲率，我們可以選擇曲率較大的邊緣點和曲率較小的平面點。為了防止特徵點聚類，將每個掃描點雲分為四部分，其中選擇曲率最大的兩個點作為邊緣點，選擇曲率最小的四個點作為平面點。在選擇點時，我們要避免選擇已經選擇的點周圍的點或鐳射雷達線接近平行平面的點，這些點通常被認為是不可靠的，因為它們無法隨時看到。我們還希望避免可能的模糊點。
里程計演算法可估計 LiDAR 在掃描過程中的運動。令 tk 為掃描 k 的開始時間。在每次掃描結束時，掃描期間感知的點雲 pk 被重新投影到時間戳 tk+1。我們將重新投影的點雲表示為 _ pk。在下一次掃描期間，k + 1, _ pk 與新接收的點雲 pk+1 一起使用來估計 LiDAR 的運動。接下來就是尋找對應關係，即匹配兩個點雲的特徵點； pk+1 的角點與 _ pk 的角線重合，pk+1 的平面點與 _ pk 的平面重合。有了點到線和點到面的對應關係，我們就可以計算出點到線和點到面的距離：
然後我們就可以求出該平面點到對應平面的距離：
LiDAR 運動在掃描過程中以恆定的角速度和線速度進行建模。這使我們能夠在掃描內線性插值不同時間接收到的點的姿態變換。如果我們讓 t 為當前時間戳，並回想一下 tk+1 是掃描 k + 1 的開始時間，則線性插值公式為
為了獲得本幀資料中的點與前一幀資料中的點的對應關係，我們使用旋轉矩陣R和平移量T
由於旋轉矩陣的求導非常複雜，因此透過Rodrigues公式將旋轉矩陣R展開如下：
這使得推導旋轉矩陣變得容易。
現在我們有了點到線和點到面的距離，就可以得到最佳化的誤差函式：
f 中的每條線代表一個特徵點。下一個要求是求解雅可比矩陣。最後採用LM方法進行最佳化：
由於上一步的解是根據本地鐳射雷達觀測座標系TL得到的結果，因此它解決了相鄰幀之間的變換。然而，為了同時進行定位和建圖，需要解決全域性座標系TW下的變換。因此，當我們獲得相鄰幾幀的姿態變換資訊時，需要將其與全域性地圖進行匹配，並將其新增到全域性地圖中。
最後，透過使用rviz等軟體，將鐳射雷達里程計解決方案獲得的姿態資訊與建圖獲得的資訊進行轉換和整合。
2.2. NDT重定位演算法
為了識別LiDAR在離線地圖中的位置，我們將LiDAR掃描的點雲與離線地圖的點雲進行比較。在重定位過程中，LiDAR 掃描的點雲可能與離線地圖的點雲不同，這可能是因為 LiDAR 視野被遮擋，或者是因為車輛僅使用了部分 LiDAR 點雲。對於有偏差的地圖重定位，我們使用NDT對齊演算法，該演算法不比較兩個點雲之間的差異，而是將參考點雲地圖轉換為多維變數的正態分佈[25-27]。如果變換引數能夠使兩組 LiDAR 資料良好匹配，則參考系統中變換點的機率密度會很大。因此，可以考慮採用最佳化方法來找到使兩組LiDAR點雲資料最匹配時機率密度之和最大化的變換引數。
第一步是將3D離線點雲圖網格化，使用小立方體劃分掃描點的整個空間，對於每個網格，根據網格內的點計算其機率密度函式。這可以描述為：
其中 → μ 為離線地圖網格正態分佈的均值，m 表示離線地圖網格中的點數，q 表示離線地圖網格中的第 q 個點，y → q=1,... ,m 為離線地圖網格中所有掃描點，Σ 表示離線地圖網格的協方差矩陣。網格的機率密度函式可以描述為：
使用正態分佈來表示離散的離線點雲圖有很多好處。這種分塊、平滑的表示是連續可導的，每個晶格的機率密度函式可以被認為是區域性表面的近似，它不僅描述了表面在空間中的位置，而且還包含有關表面的方向和平滑度的資訊。表面。
使用 NDT 對齊時，目標是找到當前 LiDAR 掃描的位姿，以最大化當前掃描點位於離線地圖表面上的可能性。然後我們需要最佳化的引數是當前掃描的 LiDAR 點雲的變換（旋轉、平移等），我們使用變換引數 → h 來描述。當前掃描是點雲 X = {→x 1, . 。 。 , →x n}，給定掃描點集合 X 和變換引數 → h ，使得空間變換函式 T(→ h , →x q) 表示使用位姿變換 → h 來移動點 →x q ，組合與之前的一組狀態密度函式（每個網格的機率密度函式），那麼最佳變換引數 → h 應該是最大化似然函式的位姿變換：
那麼，最大化似然也相當於最小化負對數似然 − log θ；
然後使用最佳化演算法來調整變換引數 → h 以最小化這種負對數似然。 YNDT演算法使用牛頓法進行引數最佳化。這裡機率密度函式 f (→x ) 不必服從正態分佈；任何反映掃描表面結構資訊並對異常掃描點具有魯棒性的機率密度函式就足夠了。
2.3.點雲資料預處理
x軸正方向為鐳射雷達的前方，y軸正方向為鐳射雷達的左側，如圖2所示。透過提取不同的鐳射雷達點雲區域ROI（感興趣區域），在前、後、左、右預留不同大小的鐳射雷達點雲區域，模擬鐳射雷達視場不同程度的變化。可以描繪 ROI 以進行進一步處理。每個區域的LiDAR點雲可以描述為：
LiDAR點雲區域的前、後、左、右分別用ff(α)、fb(α)、fl(α)、fr(α)表示。鐳射雷達點雲內部的點用(a,b)表示，α為切割鐳射雷達點雲的視角，β為0°到90°，如圖3所示。
體素下采樣根據輸入點雲資料建立 3D 體素網格（將體素網格視為空間 3D 立方體的集合）[28-31]。然後在每個體素（3D立方體）內，體素中的其他點透過體素中所有點的重心來近似，使得體素中的所有點都由一個重心點表示。體素下采樣根據輸入點雲資料建立 3D 體素網格（將體素網格視為空間 3D 立方體的集合）。然後，在每個體素（3D 立方體）內，使用體素的重心來近似體素中的所有點，以揭示體素中的其他點，以便體素中的所有點都由單個重心點表示。從而減少了點的數量，提高了匹配速度，點雲的形狀特徵基本保持不變，並且保留了空間結構資訊。體素網格選擇越大，取樣的點雲越小，處理速度越快；然而，原始點雲會過於模糊。較小的體素網格選擇將產生相反的效果。同時，需要記錄解取樣後不同LiDAR點雲資料的點數，如圖4所示。
3. KITTI資料集測試
為了測試LiDAR不同視場對NDT重定位演算法的影響，我們使用全長864.831 m、持續時間117 s的KITTI資料集。測試平臺是一輛配備 Velodyne HDL-64E 的車輛。所有實驗均在該平臺上進行。車輛的平均速度約為2.5 m/s。所有評估實驗均在配備 AMD R7-4800H 處理器、16 GB RAM 和單個 NVIDIA GeForce GTX1650ti GPU 的計算機上執行。 Velodyne HDL-64E 是直接安裝在移動底盤上方的 64 線數字鐳射雷達，具有 360° 水平視場、5–15 Hz 旋轉速度、26.8° 垂直視場（+2° 至 -24.8°） ，垂直角解析度0.4°，水平角解析度0.08°，點雲計數高達130萬點/秒，最大範圍100m，測距精度±2cm。如圖5所示，利用loam演算法對KITTI資料集構建了高精度地圖。 NDT匹配點是原始點雲資料經過ROI處理和下采樣後的點，如圖6所示。由於體素下采樣後原始點雲密度不均勻，每個方向的NDT匹配點數量為也不規則，因此各個方向的無損檢測匹配點的定位精度是不同的。本文程式碼設定的下采樣因子為3.0。變換epsilon為0.05，步長為0.1，解析度為2.0，最大迭代次數為30。本文采用均方根誤差（RMSE）來衡量定位精度。 RMSE 是預測值與真實值之間的偏差的平方與觀測值數量之比的平方。
如表1所示，比較了不同方向90°、180°、270°鐳射雷達點雲重定位軌跡得到的RMSE。可見，在同一方向上，鐳射雷達的視角越大，NDT演算法的匹配點越有效，定位精度越高，誤差越小。當鐳射雷達左右視場角為90度時，無法重新定位鐳射雷達。當前後鐳射雷達視場角為90°時，可以重新定位。當鐳射雷達四個方向視場角放大時，正面和背面的精度提升較大，而左右兩側的精度提升較小。
為了進一步探討LiDAR匹配點雲方位和NDT匹配點數量對點雲中定位精度和權重的影響，將定位軌跡漂移的影響在中間90°擴充套件到270° ° 位於左側和右側。實驗以30度不同角度進行。如表2所示，透過比較鐳射雷達左右兩側NDT匹配點的數量可以看出，由於正面和後面的NDT匹配點雲數量較多，並且在90°左右°~270°，1°時，NDT前後匹配點數量的比例對定位軌跡的精度起著決定性作用。同時也證明NDT匹配點越多，定位精度越高，漂移程度越小。正常駕駛模式下，由於雷達點雲前後的點雲比較豐富，因此建議多使用前後點雲。
在無損檢測匹配點數量大致的情況下，只需要較小的鐳射雷達視場角即可使用之前的無損檢測匹配點。正面NDT匹配點也存在較大誤差。這體現了前後端NDT匹配點的魯棒性。
4.城市資料集測試
配備64線鐳射雷達的履帶式車輛在真實環境中使用，如圖7所示。這裡使用的鐳射雷達是Ouster OS1-64，測量範圍為150 m，精度為±2 cm ，垂直視角30°，水平視角360°，垂直角度解析度0.52°，水平角度解析度0.09°，旋轉速率10Hz。此次測試是在特徵明顯的複雜城市環境中進行的。如圖8所示，履帶的左右兩側有彎路、直路，且有明顯的大型障礙物。資料集中的道路長度為1秒，持續時間為23秒。所有評估實驗均在配備 AMD R7-4800H 處理器、16 GB RAM 和單個 NVIDIA GeForce GTX1650ti GPU 的計算機上執行。
從圖9和圖10可以看出，當NDT匹配點數量小於100時，重定位誤差會急劇增大，當NDT匹配點數量小於50時，極易丟失定位。每幀NDT匹配數的理論值設定為100。由於NDT匹配點的數量為3109，因此NDT匹配點的數量為249,2189。因此，無損檢測有效匹配數不能低於總數的12.4%，且單幀NDT有效匹配不能少於100個。然而，路段的複雜性（有直線路段和彎路路段）導致NDT匹配點相對於點雲幀體積的波動。根據實際實驗，得到了可靠的範圍，為14.7%~16.3%，如表4所示。
從圖9和圖10可以看出，每組資料都有4個拐點，變化較大。如圖11和圖12所示，隨著車輛轉彎，兩側鐳射雷達效果匹配點數量急劇減少，導致重定位精度下降，如圖13和圖14所示。右側變大，有效匹配點的數量急劇增加，從而導致更高的重定位精度，如圖15和圖16所示。
如圖17和圖18所示，在正常的直線段中，障礙物分佈不均勻且距離較近。輕鬆縮小左右LiDAR視場視覺的影響，導致NDT有效匹配點數量在50到100之間，導致重定位精度降低，甚至丟失位置。在正常直線段，由於前後鐳射雷達資料大部分為地面點，重定位精度受到嚴重影響。左右鐳射雷達點多為有效特徵點，特徵豐富。如圖19和圖20所示，在直線段，正面和背面鐳射雷達資料的定位精度較差。如圖21和圖22所示，左右兩側LiDAR資料的定位效果良好。
5.結論
本文提出了一種基於高精度地圖NDT重定位的LiDAR佈局和視場選擇方法，以解決視場過大被遮擋時NDT重定位誤差大和位置丟失的問題。為了模擬真實的放置環境和遮擋的LiDAR，採用ROI演算法對LiDAR點雲進行切割，得到不同尺寸的LiDAR點雲資料。一、切入點雲資料被下采樣，然後重新定位。 NDT重定位的下采樣點被記錄為有效匹配點。使用 RMSE 值和有效匹配點來最佳化 LiDAR 點雲資料的方向和角度設定。結果表明，在路況複雜的城市場景中，單位角度內前後匹配點數量多於左右匹配點數量。 NDT重定位演算法的匹配點越多，重定位精度越高。增加前後 LiDAR 視野可防止重新定位丟失。單幀鐳射雷達資料的有效匹配點大於設定閾值。透過增加左右鐳射雷達的視場，可以提高重定位精度。您還需要與兩側的障礙物保持安全距離。未來的研究計劃是改進NDT演算法，以加快其處理速度，提高定位精度，並使其能夠在鐳射雷達資料稀疏的情況下進行重新定位。