一個魯棒的基于圖優化的精確雷達建圖
摘要
工業環境中的移動機器人需要準確的地圖以執行導航和服務任務。 本文介紹了一個SLAM框架，該框架由不同的優化組件組成，旨在構建非常精確的地圖。 我們在前端提供高級水平的為數據關聯提供先驗的特征點提取器。 由于感知混疊，2D激光表示可能導致錯誤的閉環檢測，這可以通過后端中的現有圖優化技術可靠地解決。最后的后處理步驟使用包含激光測距儀特定特征的先進傳感器模型生成高度精確的地圖。 所獲得的框架在從典型工業環境獲得的數據集上進行評估。 給定遍歷環境的地面實況測量值來估計最終的地圖精度。
簡介
![]()
圖1
移動機器人在工業環境中的自主操作需要平臺相對于全局參考系的精確位置和方向。設置諸如視覺標記之類的人造地標可以容易地解決該問題，然而，這需要修改環境。相比之下，使用板載傳感器構建的先驗地圖使機器人能夠獨立于標記的存在進行導航。提供遍歷區域的路徑是已知的，可以容易地構建地圖。對于大多數情況，這些路徑是難以獲得的且對于室內環境而言難以獲得高精度的路徑。如果地圖和路徑都是未知的，則機器人必須同時保持關于其位置的估計以及可通過的環境，這被稱為同時定位和建圖（SLAM）問題。本文提出了一個SLAM框架，可以使用激光測距儀構建高度精確的地圖，如圖1所示。這包括一個通過局部運動和閉環提供機器人姿態空間關系的前端。這些信息被傳遞到后端，該后端維持姿勢關系并估計機器人行進的路徑。在給定估計的軌跡之后執行關節位姿和地圖優化。所提出的框架利用g2o [1]，一種通用的廣泛用于機器人和計算機視覺的圖優化后端。特別是基于優化的SLAM問題方法正在積極研究中。因此，存在大量前期的工作，這些工作在摘錄中概述。Olson等人介紹了一種使用梯度下降進行圖優化的有效方法[2]。 Grisetti等，提出了一個基于樹的實現，創造了TORO，它明確地將圖優化與地圖的大小聯系起來而不是軌跡的長度[3]。在[4]中，Kretzschmar和Stachniss建議通過信息理論進一步降低姿勢圖的復雜性。Blanco等人提出了混合度量拓撲SLAM的通用接口，構建了基于占用網格的子圖的圖形，非常適合于映射大規模環境[5]。基于度量的SLAM系統，例如RBPF [6]，通過位置識別方法補充，例如，FAB-MAP [7]。 iSAM [8]給出了一種基于SLAM的在線應用優化解決方案，這種方法的關鍵思想是，只需要更新因子圖中的一小部分變量以進行增量操作。 Kaess等人。進一步擴展了他們的算法，使用貝葉斯樹實現增量變量重新排序和重新線性化[9]。在[9]中，證明了iSAM在實際數據集上優于其他最先進的方法，如HOG-MAN [10]和Spa2D [11]。然而，我們的框架更側重于構建一個準確的地圖，該地圖不一定必須在線執行，因為數據集是為此目的而進行批量處理的。
上面提到的基于優化的SLAM方法在準確性方面提供了實質性結果，因為它們試圖根據相關約束找到給定圖表示的最優解。然而，假設所有回環檢測都是正確的，這對前端中的數據關聯提出了很高的要求。在視覺SLAM系統中常見的幾何一致性檢查，例如[12]中描述的，可以最小化錯誤檢測的數量，但通常不能避免它們。這是因為一些地方由于感知混疊而難以區分，特別是對于存在許多重復結構的2D激光掃描，例如在工業環境中。除此之外，由于更悲觀的評估，還需要昂貴的數據關聯技術，這些技術也可能拒絕真正的回環。另一方面的虛假約束可能會導致圖優化中的分歧，這些不能通過諸如[13]中所示的偽Huber誤差函數的魯棒方法來充分減輕。在[14]中，Sünderhauf和Protzel提出了一種強大的優化方法，使后端能夠通過切換回環約束來修改姿勢圖。這允許前端中非常簡單的數據關聯技術將更多數量的回環候選和相關的置信度傳遞給后端，后者能夠禁用錯誤的約束。 由于該技術被包含在所提出的框架中，因此在本文的其余部分中給出了更詳細的描述。
預計從魯棒的后端將獲得拓撲一致的姿勢圖(pose graph)。 然后根據該位姿圖估計最終地圖。 在[15]中，正如Ruhnke等人所證明的那樣，有利的是對激光束的物理進行建模，以便最大化地圖的局部精度。 這具體地解決了光束的范圍和入射角。 通過聯合優化光束測量的特性，局部環境和原點傳感器姿態，可以獲得高精度的地圖表示[15]。 本文通過僅使用2D激光測距儀和車輪里程計的移動機器人繪制工業環境，展示了該想法的好處。
本文的其余部分安排如下。 第2節和第3節描述了框架的前端和后端及其組件。 我們在第4節中提出實驗結果，然后在第5節結束。
2. 建圖前端
本節描述了建圖框架的前端。 它提供了關于連續運動機器人姿勢的初始估計。 除此之外，前端負責識別先前觀察到的位置，從而將回環候選傳遞給后端。 這是通過從激光范圍掃描(range scan)中提取與先前掃描獲得的特征相匹配的特征來完成的。(通過匹配特征)
A. 估計初始化
機器人連續軌跡間的姿態相對變換由里程計測量值(車輪)給出。 由于車輪滑動，這些估計可能會迅速發生變化。 我們使用激光掃描匹配來校正從里程計獲得的位置估計。 應用基于點到線度量的ICP算法[16]。
B. 特征提取
我們使用快速激光興趣區域變換(Fast Laser Interest Region Transform，FLIRT)[17]，靈感來自于先前工作中的計算機視覺中尺度不變點特征檢測器。 FLIRT將激光的范圍數據(range data)視為被映射成多尺度表示的一維曲線。 因此，我們獲得了由變化高斯核來平滑的所有尺度的曲線。 對平滑核進行歸一化以使其對采樣密度不變。 檢測每個尺度的局部極值。 這種數學推導在[18]中給出。 最后，我們獲得梯度高于某個閾值的一組興趣點作為局部最大值。 這些點通常指的是2D掃描中的角點。(不太懂)
為了將被檢測的興趣點與先前觀測的興趣點相互關聯，需要進一步給興趣點分配描述子。類似于計算機視覺中的常見特征檢測器Tipaldi等提出的用特征描述符捕獲感興趣點的局部環境[17]，其能夠識別后續掃描中觀察到的特征。諸如LRF的范圍測量傳感器給出關于由傳感器的視場（FOV）覆蓋的自由空間和占用空間的明確信息。因此，將其用描述子是有利的。在每個興趣點周圍建立了一個極坐標的計算方法。這與占用柵格建圖類似，但是我們僅在興趣點周圍生成高分辨率網格，而不是描述激光掃描覆蓋的整個區域。由于FLIRT興趣點具有高度的獨特性，并且不假設特定的幾何圖元，因此它們非常適合于許多非特征性的環境類型（例如長走廊）。關于特征檢測器和描述符的更多細節可以在[17]中找到。
C. 回環檢測
SLAM系統最重要的需求是能夠檢測和合并已經訪問過的地方的信息，這些信息被稱為回環。 通過回環能夠減少累積姿勢不確定性隨時間增加。 現在已經進行了針對2D范圍掃描中的回環檢測的大量研究，并且可以將其分為柵格地圖(gird map)和基于特征的方法。
基于柵格地圖方法通常不能隨著地圖尺寸的增加而很好地擴展，因為它們編碼了大量的自由空間。 對于基于Rao-Blackwellized粒子濾波器（例如[6]）的方法尤其如此，因為每個粒子保持整個軌跡的單獨圖。 另一方面基于特征的方法通常假設在諸如角點的環境中的特定結構，這妨礙了它們在無結構環境中的使用。 然而，作為本文特別關注的工業環境通常包括由諸如架子或機器之類的物體引起的精細結構。 通過從每次激光掃描中提取的FLIRT興趣點很好地考慮這些結構。 這些與使用對稱χ2度量和最近鄰策略的與先前掃描的特征進行匹配。(文章假設了環境存在角點)一組對應的特征點被傳遞給RANSAC進行幾何一致性檢查。每對描述子距離小于一定閾值的興趣點被推定為潛在的匹配。從對應集中隨機抽取最少的兩對，然后如下進行RANSAC迭代。計算推定匹配的空間距離。如果這些距離的偏移低于閾值，則通過估計這兩個點之間的變換來生成假設。此變換應用于所有觀察到的特征，以便將這些特征投影到參考掃描的坐標系中。因此，為每個觀察到的特征點分配參考掃描的最接近的對應特征點。將所有對的平方距離相加，其用作兩次掃描的匹配分數。只有距離低于閾值的那些對被添加到內部集合中。在基于RANSAC的數據關聯中，前述的過程重復進行。如果剩余的一組內點高于閾值tmin，則生成回環約束。tmin的值高度依賴于優化后端的穩健性，特別是它處理數據關聯錯誤的能力。(這一段翻譯的特別亂，下附原文，這一段大概在將RANSAC的作用)
出于性能原因，許多SLAM前端不考慮關聯軌跡的所有位置數據。 例如，[11]中提出的使用基于圖優化實現Karto，定義了當前位姿周圍的固定搜索半徑以減少操作時間。 然而，這對于大規模環境建圖是至關重要的，因為先驗位姿可能由于累積的測距誤差而顯著偏離真實位姿。 因此，應該根據位姿不確定性動態設置該半徑。 由于表現結果和錯誤關聯的風險， Grisetti等人建議限制在重認中考慮的地點數量。 由于我們的框架后處理數據集，我們不一定受性能限制的影響。 基于[14]提出的魯棒優化后端，我們能夠成功識別錯誤的閉環約束，這在第3節中有詳細解釋。
3. 優化后端
由于前端，我們給出了機器人姿勢和表達空間約束的關系的初始圖。 此圖的優化是后端的任務。 本節的其余部分簡要介紹位姿圖SLAM，解釋如何處理錯誤回環檢測并構建最終地圖。
A. 位姿圖(pose graph) SLAM
提供的移動機器人導航是在2維空間中進行的，其狀態向量可以用x =（x，y，φ）T來描述。 位姿圖圖SLAM僅優化給定軌跡的機器人姿勢xi，省略分別優化地標掃描點。 該圖由位姿xi和位姿xi和xj之間邊界給出的頂點組成。 并且執行動作ui =Δ（x，y，φ）以便從狀態xi到xi+1根據運動模型合并：
B. 魯棒的優化
Tipaldi等人在[20] 中，在SLAM框架中展示了FLIRT興趣點的魯棒性和高重復性。 然而，作者使用TORO [3]作為優化后端，在存在偽回環檢測時這種后端不魯棒。 對于具有多個重復結構的環境而言，這是一個顯著的缺點，這些結構難以通過2D激光掃描來區分。 提出了將位姿搜索空間限制在當前估計周圍的局部區域的滑動窗口以最小化該問題。 然而，它阻礙了在具有大環路和較差的先驗位姿估計的環境中的應用，例如通過車輪里程計獲得。
對于SLAM問題的通常基于最小二乘的優化方法保持所提供的位姿圖的結構固定，假設所有回環約束是正確的。 Sünderhauf和Protzel提出了可切換約束，通過修改的目標函數來解決優化問題。他們的工作使用表示可切換回環約束的圖形模型中的因子。基于此，圖的拓撲結構不是固定的，但也可以進行優化。以這種方式可以禁用表示為開關變量sij的閉環。回環對優化的貢獻可以通過其初始值γij和相應的協方差Ξij來合并。例如，這些估計器可以通過前端的地點識別系統的一定程度的信任來給出。這些開關先驗是必要的，以避免優化器關閉所有回環，如[14]所示。為了在優化中包括可切換約束，目標函數（式5）被修改如下：(本文加入了可以切換的回環約束)
C. 地圖構建
稀疏表面調整（SSA）是一個聯合優化問題，旨在尋找機器人位姿和激光測量值的最佳設置。這種方法由Ruhnke等人在[15]中提出，與計算機視覺中的稀疏束調整（SBA）非常相似。 SBA通過最小化點的重投影誤差來優化一組相機位姿和圖像序列的3D點。類似地，SSA優化機器人位姿和激光點，然而SBA需要點對點，和數據關聯中的點對面對應的SSA相反。進一步假設環境由光滑表面組成，這對于人造結構是非常常見的。改進的傳感器模型明確地解釋了激光測距儀的距離測量中的不確定性，其主要由于入射角和距離變化的局部表面特征而發生。這是通過將激光掃描描述為一組表面切線來完成的，每個表面切線代表一個激光束。切線被建模為高斯的，其中平均值μik以機器人的激光束k為中心，位姿i和基于周長點的協方差估計∑ik。表面沿著它們的切線方向平滑地移動，并且沿著面向機器人姿勢的法線方向更加剛性。機器人姿態X和表面貼片(?surface patches)M的聯合優化問題公式如下[15]：
更詳細推導見參考文獻[5]
4. 實驗
為了評估所提出的框架，進行了許多實驗。 為此，我們通過倉庫手動操縱移動機器人捕獲激光范圍掃描和車輪里程計。 以下部分簡要介紹了用于實驗的移動平臺，并討論了獲得的結果。 首先，證明了強大的優化后端的必要性。 第二部分顯示了在典型的工業環境中使用所提出的框架實現的最終建圖精度。
A. 平臺
使用由德國Metralabs GmbH開發的Scitos G5平臺（參見圖2）。 它配備了四個攝像頭，兩個激光測距儀（Hokuyo URG04-LX，SICK S300），一個TOF攝像頭（PMD Camcube 3.0）和車輪里程計。 只有前向SICK S300用于地圖構建。
B. 貢獻魯棒的后端
盡管FLIRT興趣點非常獨特，但回環檢測錯誤的數量很高。 這主要是由于2D激光掃描的有限場景表示引起的感知混疊。 幾何一致性檢查無法在所有情況下阻止這種情況，因為環境中存在大量重復結構，例如貨架和盒子。 圖3顯示了位姿圖優化的結果，強調了魯棒的后端的重要性。 我們使用了Switchable Constraints [14]實現，創造具有線性切換功能。 此外，我們使用了通用圖優化后端g2o [1]。 這兩種實現都是從openslam2平臺獲得的。
參考文獻相關論文閱讀
參考文獻：
[1] R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g2o: A general framework for graph optimization,” in ICRA, Shanghai, 2011.
[2] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of pose graphs with poor initial estimates,” 2006, pp. 2262–2269.
[3] G. Grisetti, S. Grzonka, C. Stachniss, P. Pfaff, and W. Burgard, “Efficient estimation of accurate maximum likelihood maps in 3d,” San Diego, CA (USA), 2007, pp. 3472–3478.
[4] H. Kretzschmar and C. Stachniss, “Information-theoretic compression of pose graphs for laser-based slam,” vol. 31, pp. 1219–1230, 2012.
[5] J.-L. Blanco, J.-A. Fernandez-Madrigal, and J. Gonzalez, “A new approach for large-scale localization and mapping: Hybrid metrictopological slam,” in Robotics and Automation, 2007 IEEE International Conference on, 2007, pp. 2061 –2067.
[6] G. Grisetti, C. Stachniss, and W. Burgard, “Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling,” in Robotics and Automation, 2005. Proc. of the 2005 IEEE International Conference on, Apr. 2005, pp. 2432 – 2437.
[7] M. Cummins and P. Newman, “FAB-MAP: Probabilistic Localization and Mapping in the Space of Appearance,” The International Journal of Robotics Research, vol. 27, no. 6, pp. 647–665, 2008.
[8] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental smoothing and mapping,” IEEE Trans. on Robotics (TRO), vol. 24, no. 6, pp. 1365–1378, Dec. 2008.
[9] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping using the Bayes tree,” Intl. J. of Robotics Research (IJRR), vol. 31, pp. 217–236, Feb. 2012.
[10] G. Grisetti, R. Kummerle, C. Stachniss, U. Frese, and C. Hertzberg, “Hierarchical optimization on manifolds for online 2d and 3d mapping,” in Robotics and Automation (ICRA), 2010 IEEE International Conference on, 2010, pp. 273–278.
[11] K. Konolige, G. Grisetti, R. Kümmerle, W. Burgard, B. Limketkai, and R. Vincent, “Efficient sparse pose adjustment for 2d mapping,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), Taipei, Taiwan, October 2010.
[12] D. Galvez-Lopez and J. D. Tardos, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1188–1197, October 2012.
[13] N. Sünderhauf, “Robust optimization for simultaneous localization and mapping,” Ph.D. dissertation, 2012.
[14] N. Sünderhauf and P. Protzel, “Towards a robust back-end for pose graph slam,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on, may 2012, pp. 1254 –1261.
[15] M. Ruhnke, R. Kümmerle, G. Grisetti, and W. Burgard, “Highly accurate maximum likelihood laser mapping by jointly optimizing laser points and robot poses,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2011.
[16] A. Censi, “An ICP variant using a point-to-line metric,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), May 2008.
[17] G. Tipaldi and K. Arras, “Flirt - interest regions for 2d range data,” in Robotics and Automation (ICRA), 2010 IEEE International Conference on, 2010, pp. 3616 –3622.
[18] R. Unnikrishnan and M. Hebert, “Multi-scale interest regions from unorganized point clouds,” in Workshop on Search in 3D (S3D), IEEE Conf. on Computer Vision and Pattern Recognition (CVPR), June 2008.
[19] M. W. M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrantwhyte, and M. Csorba, “A solution to the simultaneous localization and map building (slam) problem,” IEEE Transactions on Robotics and Automation, vol. 17, pp. 229–241, 2001.
[20] G. D. Tipaldi, M. Braun, and K. O. Arras, “Flirt: Interest regions for 2d range data with applications to robot navigation,” in Proceedings of the International Symposium on Experimental Robotics (ISER), Dehli, India, 2010.
[21] E. Einhorn, C. Schroter, and H. Gross, “Finding the adequate resolution for grid mapping - cell sizes locally adapting on-the-fly,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on, 2011, pp. 1843–1848.
