1.1 機器人環境建模技術

對于智能移動機器人來說，其在完全未知的環境下獲得自主能力的基礎，是對環境模型進行創建——建立環境地圖，并在環境模型的基礎上同步實現定位，即同步定位與建圖技術（Simultaneous Localization and Mapping，SLAM）。SLAM也是機器人在環境中完成各種智能任務的前提。

1.1.1 基于SLAM的環境建模

SLAM作為機器人領域的技術難題，涉及地圖表示、不確定性信息處理、數據關聯、自定位、探索規劃等一系列高度相關的環節，根據地圖描述環境的范圍可分為局部地圖和全局地圖兩種。大部分移動機器人采用激光測距傳感器或可見光視覺傳感器來創建局部地圖，用于機器人的局部、自主避障。全局導航的目標點由用戶進行人工選擇、設置。全局地圖的創建將為高自主性的全局自定位和導航提供可能，如基于遞增極大似然法的全局混合地圖的創建方法、采用用戶人工引導方式快速創建大規模環境的拓撲和概率柵格混合地圖等。

對于移動機器人來說，其SLAM過程中遇到的最大問題是未知環境所帶來的挑戰，比如室內環境的擁擠、動態變化，室外環境的大范圍、特征稀疏等。區別于一般的智能汽車，警用機器人的應用場合包括機場、高鐵站、商場以及室外廣場等。在SLAM領域，前人已經做出了大量卓有成效的工作，其中具有代表性的有擴展卡爾曼濾波（Extended Kalman Filter，EKF）算法、擴展信息濾波（Extended Information Filter，EIF）算法以及粒子濾波（Rao-Blackwellized Particle Filters，RBPF）算法等。這些算法的核心思想是采用遞歸貝葉斯原理對系統狀態（機器人位姿及環境特征）的后驗概率進行估計，將SLAM問題本質等效為后驗概率估計問題，即

P（x_1：t，m|z_1：t，u_0：t-1）

式中，m是環境地圖；x_t是機器人在時刻t的位姿；z_t是激光測距傳感器的觀測；u_t是機器人控制量；x_1：t={x₁，…，x_t}是機器人從起始時刻到時刻t的行駛軌跡；u_1：t={u₁，…，u_t}是從起始時刻到時刻t的機器人控制序列。

在SLAM問題中，一般通過式（1-1）對環境地圖m和機器人位姿x進行估計

由式（1-1）可以看出，地圖m主要依賴移動機器人的行駛軌跡以及傳感器的觀測，只要確定機器人在各時刻的位姿x，就可以從（x_k，z_k）的信息組中解算出環境地圖m。因此，SLAM問題就轉變為一個軌跡估計問題

地圖創建過程并不需要求取地圖的完整分布P（m），只需求取地圖的極大似然估計m^*即可，即不需直接求取P（x_1：t，m|z_1：t，u_0：t-1）的完整分布，只需求取P（x_1：t，m|z_1：t，u_0：t-1）的極大似然估計，則可得到

在SLAM研究中，為了計算與分析的方便性，假設移動機器人運動學模型及傳感器觀測模型的噪聲均滿足高斯分布，即

這樣便可得到如下的關系

式中，⊕是坐標系的變換符號；T_ij和Σ_ij分別是位姿x_i與x_j間對應的坐標變換關系和協方差。對T_ij和Σ_ij進行分解，便可得出機器人的位姿以及環境地圖。

考慮復雜環境下一般場景規模大，且特征稀疏、動態因素較多等特點，為保證機器人在該環境下導航時的定位精度、降低算法的計算復雜度，本章節主要針對二維、三維激光雷達在移動機器人SLAM中的應用提出了基于分層匹配的增量式SLAM算法，將SLAM問題簡化為數據關聯和最小二乘優化兩個部分，然后通過以下步驟加以解決：首先，通過分層迭代最近點（ICP）匹配算法解決數據關聯問題，并且對傳感器觀測與局部地圖以及局部地圖之間進行匹配，匹配結果的不確定性采用Fisher信息矩陣描述；其次，采用增量式正交三角（QR）分解對機器人位姿進行優化；最后，將SLAM問題簡化為最小二乘問題，并通過增量優化算法求解，可提高計算效率。提出的建圖算法可保證在大規模室內外場景下的建圖精度，同時能夠滿足實時應用的需求。

在進行環境建模時，由于二維、三維激光雷達在數據量規模、地圖表示、應用場景等方面存在差異，下面將分別進行闡述。

1.1.2 基于二維激光雷達的環境建模

不論在室內環境還是室外環境，當范圍較大時，由于觀測噪聲以及匹配誤差的存在，通過傳感器觀測信息配準得到的環境地圖經常會出現不一致的描述。當機器人回到之前已經探索過的區域時，算法需要進行針對性處理，從而消除不一致性描述。在SLAM過程中，這個做法也被稱為環形閉合（Loop Closure）。環形閉合對于移動機器人的地圖創建非常重要。正確的環形閉合能夠修正建圖過程中產生的累積誤差，從而提高地圖精度；而錯誤的環形閉合將會在后續不確定性處理過程中引入累積誤差，甚至破壞已建立的環境地圖。

移動機器人環境建模需要機器人在定位、導航階段具有較高的精度（厘米級），這樣才能滿足用戶對巡檢、安保等任務執行，以及擁擠環境下安全性導航的要求。針對這些要求，提出基于分層匹配的增量式SLAM（Multilayer Matching based Incremental SLAM，M2ISLAM）算法（圖1-1）。該算法不提取環境特征，而采用基于圖優化融合的方法對所有已知信息（傳感器觀測和機器人運動軌跡等）進行估計，并在圖優化融合的方法中采用圖論的思想將環境特征和機器人位姿作為頂點，將傳感器觀測信息作為邊；作為邊的觀測信息描述了位姿間的空間約束關系，對特征點和位姿點的位置進行優化后，便可滿足邊所表示的約束關系。

從圖1-1可以看出，在M2ISLAM算法中，SLAM問題被簡化為數據關聯和最小二乘圖優化兩個部分。

首先對數據關聯進行求解。數據關聯的求解是該算法的關鍵部分，決定了算法的精度，因此采用SLAM中常見的迭代最近點（Iterative Closet Point，ICP）匹配算法。根據觀測z_t及z_1：t-1，估計x_t與x_1：t-1間的約束關系T_ij及其協方差Σ_ij，從而確定T_ij。同時，由于將z_t與z_1：t-1均進行ICP匹配在計算上是不可行的，所以采用分層匹配的方法進行z_t與局部地圖以及局部地圖與局部地圖間的匹配，從而在保證匹配精度的同時降低算法復雜度。不同于常見的匹配策略——僅將z_t與z_t-1進行匹配或將z_t與所有鄰域的觀測進行匹配，這種匹配策略既有效減小了累積誤差，也避免了算法在特征稀疏的環境中陷入局部極小。另外，由于ICP算法無法直接處理不確定性，所以Fisher信息矩陣被用來對匹配結果的不確定性進行定量估計，從而得到x_t與x_1：t-1間最終的協方差Σ_ij。

在得到x_t與x_1：t-1間的約束關系T_ij及其協方差Σ_ij之后，再采用增量式QR分解對機器人位姿進行優化，從而得到一致的地圖描述及機器人位姿信息。每個模塊的算法描述具體如下。

1.1.2.1 數據關聯

在移動機器人SLAM問題求解過程中，數據關聯是至關重要的部分。在將傳感器觀測信息與已知地圖信息融合建立新的地圖描述后，數據關聯將成形并不能再被修改。也就是說，數據關聯是不能動態調整的，穩定、精確且可靠的數據關聯將保證地圖描述的一致性與準確性，但是錯誤、松散的數據關聯就將帶來較大的誤差和不一致的地圖描述，甚至會導致地圖創建的失敗。因此，SLAM算法中關鍵的一部分就是解決提高數據關聯的穩定性與精度問題，尤其在遇到Loop Closure問題時，數據關聯將顯得更為重要。

以幾何地圖為例，假設機器人通過傳感器檢測并獲得環境中的特征為D={d₁，…，}，那么數據關聯算法的作用就是建立一個集合H={j₁，…，j_p}，使得每一環境特征d_i均能與環境地圖中的實際路標m_ji相對應（如果傳感器對環境的觀測與環境中的特征無關，則j_i=0）。圖1-2所示為一種樹狀的數據關聯結構，它表現的是傳感器對環境的觀測、環境特征d_i以及地圖中的實際路標之間的關系。

對于圖1-2中的任意一層，所有節點均表示環境路標d_i可能與實際路標存在數據關聯；“*”表示環境中不存在任何實際路標與環境特征d_i相關聯；M={m₁，…，m_q}為地圖實際存在的路標。每個節點均會有p+1個分枝，由此便可形成（p+1）^q種可能的數據關聯。而數據關聯解算算法就是通過某種搜索算法，從全部可能的關聯中找出需要的關聯組合。由于計算量巨大，不可能將所有的觀測信息均進行匹配，通常需要進行簡化：

1）只考慮時間上相近的觀測：由于只考慮了與當前時刻相近的觀測信息，將不可避免地產生累積誤差。因此，當環境范圍逐漸增大時，地圖描述的不一致性也將增大。

2）只考慮在空間上相近的觀測：與上一簡化方法相近，也忽略了部分觀測信息，但這種方法可減小累積誤差，策略更為合理。不過，當環境復雜或特征稀少時，空間上相近的觀測信息間的差異度不大，使得匹配算法陷入局部極小。

3）將匹配對離散化，只將觀測與地圖信息進行匹配：此方法本質上與第二種簡化方法類似，空間上不相近的觀測對于匹配結果不會產影響，但該方法不需要限定鄰域范圍，較第二種方法具有一定的優勢。然而，該方法并沒有解決地圖不一致描述的問題。

針對上述所分析的數據關聯問題，采用分層匹配的方法，在觀測信息與局部地圖、局部地圖與局部地圖間分別進行匹配；匹配算法均采用迭代最近點匹配算法。由于ICP算法并不能直接處理信息的不確定性，故采用Fisher信息量來估計匹配協方差。下文將分別對基于ICP的迭代最近點匹配、分層匹配方法以及不確定性估計等部分進行詳細的描述。

1.1.2.2 基于ICP的最近鄰域匹配

最近鄰域（Nearest Neighbor，NN）匹配方法，是通過將傳感器對環境特征的觀測與根據歷史信息得到的環境特征預測進行比較，判斷二者所得到的位置信息是否足夠接近（一般由量化指標來表示），從而判定該特征是否與已知地圖路標存在關聯。

給定特征參考點集M={m₁，…，m_p}與觀測點集D={d₁，…，d_p′}間的相對坐標變換關系為T=（R，t），其中，R是旋轉矩陣，t是平移向量。基于ICP的迭代最近點匹配算法步驟如下：

Step1：通過最近鄰域原則建立M與D之間的對應關系如下

式中，D_{inline_thread}是常數，需要根據傳感器的噪聲進行設定。

Step2：計算M與D的匹配誤差，即

Step3：尋找最優的（R，t）使得上述的配準誤差最小，即

Step4：重復進行上述步驟，直至R^*、t^*不再變化或達到最大迭代次數時，終止迭代過程。

根據已有的算法推導，上述過程中的Step3可采用奇異值分解（SVD）方法有效求解。同時，該迭代過程的算法耗時主要在于匹配誤差的計算，因此采用K-D樹對參考點集M進行再組織從而降低算法復雜度。另外，由于噪聲以及未知障礙物等原因的影響，需要將無法匹配的觀測值舍棄，即時，在Step2及Step3中將該組觀測值舍棄。

1.1.2.3 分層匹配方法

假設在時刻t，機器人采集的觀測信息為z_t，則z_t所包含的環境信息與z_1：t-1存在關聯，此關聯根據時間特性分為兩類（連續與不連續）。考慮圖1-3所示的位姿約束情景，機器人從x₀出發，在t=9的時刻，機器人回到之前已經探索過的區域（x₀），此時（x₉，z₉）同時與（x₈，z₈）及（x₀，z₀）存在約束關聯。

對于時間上連續的位姿約束，可將z_t與鄰域間觀測構成的局部地圖l進行匹配，如圖1-4所示。其中，匹配的初始坐標變換可采用里程計信息或x_t-1，局部地圖l為z_t-m：t-1信息的合集

下面考慮機器人回到τ時刻已探索的區域時的情形：在機器人行駛較長距離時，誤差的累積將導致匹配算法迭代初值與真實值相差較大。因此，僅依靠單幀觀測進行匹配易陷入局部極小。為此，將z_t-m：t構成的l_t與z_{τ-q-n：τ-q}的信息合集l_τ-q進行匹配，當l_t與l_τ-q描述的環境范圍出現重合時，即機器人回到已探索區域，建立x_t與x_τ-n：τ之間的約束。

局部地圖的大小決定了匹配算法的復雜度，這里根據機器人行駛路徑的長度離散化τ，即τ∈{τ_i}，其中τ_i滿足

當l_t與l_τ-q（τ∈{τ_i}）出現重疊，即兩者所描述邊界的凸包（Convex Hull）的交集大于一定面積時，進行l_t與l_τ間的ICP匹配。

圖1-4形象地描述了分層ICP匹配算法過程，該算法可表述為如下幾個步驟：

Step1：初始化，d=0，t=0，L_τ=Φ，l=Φ，q′=0。

Step2：l=l∪{（x_t，z_t）}，并采用K-D樹進行描述，t=t+1。

Step3：z_t匹配，將z_t與l匹配，匹配結果即T_tj（t-n＜j＜t），如圖1-4a所示。

Step4：d=d+||x_t-x_t-1||，如果d≥d_threshold，則轉至Step5，否則轉至Step2。

Step5：判斷l_τ-q∈L_τ與l_t的凸包是否重合，若重合轉至Step6，否則轉至Step7。

Step6：進行局部地圖l_t與l_τ-q之間的ICP匹配，匹配結果即T_tj（τ-q-n′＜j＜τ-q），并且q=q+1，如圖1-4d所示。

Step7：L_τ=L_τ∪{l_t}，d=0，l=Φ，轉至Step2。

在機器人未回到和回到之前已經探索過的某個區域時，算法的時間復雜度分別為常數和O（n）+T_ICP，這不僅滿足實時運用，而且在地圖閉合時能夠產生可靠的約束T。

1.1.2.4 不確定性估計

在完成基于ICP的迭代最近鄰域匹配之后，需要對匹配結果進行不確定性估計。準確的不確定性估計將會消除地圖創建的誤差，而不準確估計將加大地圖的不一致性，不僅不會提高地圖的精度，甚至有可能破壞已建立的地圖。

假定匹配結果的不確定性滿足均值為零的高斯分布，那么只需確定其協方差Σ即可，這里采用Fisher信息矩陣的逆作為協方差Σ。

定位問題中的Fisher信息矩陣定義為期望激光數據及激光掃描到的環境表面斜率的函數，將其離散化后可用于匹配結果的不確定性估計。觀測z_t的Fisher信息矩陣為

式中，r_i是激光測距傳感器的第i個激光束到障礙物的長度；σ²是激光數據的噪聲方差；s_i是第i個激光束的影響因子，即對匹配的“貢獻”大小，是觀測值r_i的函數。

r_i越偏離在局部地圖中的期望值，匹配結果的不確定性越高。因此，可將s_i定義為激光測距傳感器觀測的概率分布函數

根據Cramér-Rao界（CRB）定理可知，I（T）以矩陣的形式描述了當前定位的整體性能及方向性，為機器人匹配結果概率分布的協方差下界（不妨假設匹配結果達到下界），即有

當機器人工作在二維平面環境中時，有

分析式（1-14）及式（1-16）可知，只需確定即可確定協方差Σ。

考慮，一個關鍵問題是如何估計假定約束T′=T+ΔT成立時的觀測。圖1-5所示為模擬二維激光雷達掃描示例，當約束為T時，傳感器獲得觀測{r_i}；即假定約束為T′時，傳感器的觀測應為，關系為

式中，θ是機器人轉角；Δx是機器人在x方向的位移增量；Δy是機器人在y方向的位移增量；f（i）是對應于觀測值r_i的掃描角度；f^-1是f（i）的逆函數。

考慮位姿及觀測的序列{（x₁，z₁），（x₂，z₂），…，（x_t，z_t）}，掃描匹配算法根據觀測z_t及z_1：t-1建立x_t與x_1：t-1間的約束關系T_ij。

1.1.2.5 基于QR分解的位姿優化

在通過分層匹配算法建立機器人位姿之間的約束之后，需要對位姿估計進行優化。這里采用圖優化算法對其進行優化，從而得到全局一致的位姿估計。圖優化算法是決定該地圖創建算法能否滿足實時運用的關鍵，特別是當圖的節點上升時是否會出現維數災難。采用基于QR分解的增量式平滑和建圖方法（Incremental Smoothing and Mapping，iSAM）對圖進行優化，以控制算法復雜度，滿足實時運用的目的。

在二維情形下，x=（x，y，θ）^T，T可表示為（x_T，y_T，θ_T）^T，則⊕運算的具體形式可以表示為

于是，式（1-3）可更新為

令f_j（x_i）=x_i⊕T_ij，其為非線性函數，為方便處理需要將其線性化，即有

式中，是f_j（x_i）的雅可比矩陣。

在線性化之后，便可得到線性最小二乘優化問題

式（1-24）是式（1-22）的簡化，其中。

記，則式（1-24）可轉換成一般表達形式

式中，Q（R 0）^T是A的QR分解，這里采用增量式方法進行QR分解。

下面將對QR分解做進一步的討論：由于QR分解的復雜度較高，而且是增量式的，一般是當出現新的約束或者添加新的變量時才進行完整的QR分解，這將導致算法的復雜度持續增加，從而無法滿足實時使用的要求。

考慮t時刻出現新的約束或有新的變量添加，有

由于每次迭代過程中A的變化不大，即t-1時刻的A_t-1與t時刻的A_t中大部分的元素均是相同的，不妨采用上一時刻QR分解的吉文斯旋轉（Givens Rotation）矩陣作為迭代初值（在兩者不同時可直接向A_t-1添加元素0進行擴充），則有

此時，QR分解是增量進行的，而上三角陣求逆的復雜度較小，因此該方法可滿足實時運用。

1.1.3 基于三維激光雷達的環境建模

經過近幾十年的發展，可以認為SLAM技術在理論及概念層面已得到基本解決，尤其在室內結構化場景中，基于SLAM技術的移動機器人完成定位導航任務已經得到產品級應用，但是在大范圍室外環境中仍然存在大量問題亟待解決，因此仍需致力解決如下問題：

1）大規模場景下，隨著機器人行走距離的延長，數據關聯不可避免的累積誤差會導致機器人重新回到已訪問區域或對路徑的重復探索時，破壞地圖的一致性，需要尋找合理的方式來解決此時的數據關聯問題。同時，場景的大尺度對于后端優化的計算復雜度也是一大挑戰。

2）室外環境相比室內復雜性更高，場景中特征匱乏與無規則（自然地形）、動態（廣場、道路）與噪聲數據（樹木較多的場景）較多時，如何保證數據關聯的魯棒性與位姿估計的精度也是要解決的問題。

為解決上述問題，本節提出了針對三維激光雷達的基于多層ICP匹配的SLAM算法（Multilayer ICP Matching based SLAM，MIM_SLAM），其框架如圖1-6所示。

從MIM_SLAM算法框架可以看出：

1）首先，一種點云降采樣策略被提出。考慮激光點云數據量太大，同時場景中可能存在噪聲且動態的物體，因此該方法可有效提取環境中穩定的幾何特征來參與匹配，以減小匹配過程的不確定性。

2）其次，SLAM的數據關聯問題通過多層ICP匹配來解決。該算法充分考慮了時間相鄰幀以及地域相鄰幀的數據關聯問題；結合Scan-to-Scan（10Hz頻率）以及Scan-to-SubMap（1Hz頻率）的匹配方式，可以構建實時低漂移的激光里程計，以針對激光里程計不可避免的累積誤差；利用提出的Scan-to-KeyScan匹配來解決機器人重新訪問已經歷區域時的數據關聯問題，其中的關鍵技術是提出一種基于隨機森林學習的場景識別算法；經過上述步驟，可以得到位姿x_i與位姿x_j之間的約束關系T_ij以及不確定性Σ_ij，并且以位姿圖（頂點即表示位姿，邊表示位姿約束）的形式保存。

3）最終，增量QR分解為核心的增量優化被用來優化該位姿圖，降低計算復雜度。

整個算法可保證實時性要求，各部分算法模塊將在后續章節詳細介紹。

1.1.3.1 點云降采樣策略

通常，三維激光雷達一幀的數據量非常大。例如，自動駕駛領域較常用的16線激光雷達（Velodyne VLP-16傳感器）一幀掃描具有約30000個激光點，而另一款64線激光雷達（Velodyne HDL-64E傳感器）一幀掃描具有約250000個激光點。如此龐大的數據量顯然不適合直接匹配，這會導致算法無法達到實時性的要求，而常見的降采樣算法有：

1）距離濾波：激光探測范圍很遠。由激光觀測方式可知，距離越遠的激光點越稀疏，因此距離過遠的激光點并不能正確地描述觀測物體的整體形狀，可根據距離剔除。

2）體素濾波：對于密集點云進行降采樣的方式。首先對輸入的點云數據進行三維柵格的劃分，然后遍歷每個體素，用體素中所有點的重心來近似顯示該體素中的其他點。該方法可通過柵格分辨率調節降采樣后的點云數量，同時保證點云的整體形狀特征。

3）統計濾波器：這是一種去除離群點的濾波方式。對于輸入數據中的每個點，計算到所有K個臨近點的平均距離與方差（假設滿足高斯分布，其形狀是由均值和標準差決定的），不在標準范圍內的點被認為是離群點進行剔除。

上述幾種方式雖然簡單且計算量小，但在復雜環境下卻有很大的局限性，因為場景中會有大量的動態（行人、車輛等）干擾、噪聲（樹葉、草坪等）等數據，顯然這些點云數據對于接下來的匹配算法是不利的，而上述降采樣方法無法處理該問題。學者們曾給出一種更好的采樣策略——利用激光點云的協方差矩陣來尋找幾何穩定點，然而該方法在匹配過程中是非常耗時的，很難在線應用。受其啟發，這里提出了一種快速的點云降采樣策略。

點云匹配的實質是尋找一個變換矩陣，使得輸入點云轉移到參考點云坐標系下，并具有最小的關聯誤差。對于移動機器人處于三維的場景中，該變換矩陣（旋轉矩陣+平移向量）是6自由度（Degree of Freedom，DOF）的，即（t_x，t_y，t_z，θ_roll，θ_pitch，θ_yaw）。因此這里提出的快速點云降采樣策略的實質是尋找利于匹配求解該6DOF位姿的點云數據，例如地面點云對決定（t_z，θ_roll，θ_pitch）是有幫助的，平行于激光雷達X軸坐標方向的墻面對決定（t_y，θ_yaw）是有幫助的。

圖1-7所示為所提出的點云降采樣算法框圖。首先，對于三維激光雷達輸入的點云數據P，提取地面點云信息為P_g，對去地面點云P_s進行基于深度圖像的點云分割，得到一系列分割好的點云塊{S_i|i=1，…，n}，然后對其進行穩定點云塊{T_i|i=1，…，m}的提取，最終與地面點云一起作為輸出點云數據。如圖1-7左下圖所示，輸入點云含有大量的噪聲數據，包括大量樹葉以及遠處的稀疏點，經過上述處理后得到的輸出點云（其中紅色表示穩定點云塊集合，藍色表示地面點云）如圖1-7右下圖所示，僅包含地面、柱子、墻面、靜態車輛等有助于匹配的靜態穩定點云數據。

由于三維激光雷達產生的點云數據具有無序性，所以還需將其轉換為深度圖像進行有序化處理。圖1-8所示為Velodyne VLP-16 型號激光雷達的觀測方式，絕大多數的國產三維激光雷達的觀測方式也相同。其中，垂直視域范圍為-15°～15°，角分辨率為2°，可離散化為16條掃描線；水平視域范圍為360°，角分辨率為0.18°。對周圍環境感知的一幀觀測數據如圖1-8c所示。

每個激光點均可以用（α，ω）來表示，其中α∈（-90°，90°），ω∈（0°，360°）。α表示該點投影到XY平面上與Y軸方向的夾角，ω表示該點與XY平面的夾角。如此，一幀點云數據便可以轉換為深度圖像。對于Velodyne VLP-16產生的點云數據，轉換為深度圖像是16×2000的矩陣，每個矩陣元素存儲的是該激光點的坐標以及距離信息，映射到行與列的計算方式如下

式中，vert_bottom是最下面一層掃描線的垂直角度；vert_res是垂直角分辨率；horz_res是水平角分辨率。

（1）地面點提取

將激光雷達的點云數據進行有序化處理過后，即可方便提取水平地面點。本節考慮采用一種快速且簡單的地面提取方法，相比網格法和分類器法，在基于掃描線的稀疏點云中表現更好。因為是應用在移動平臺上，激光雷達的架設方式一般為水平放置，所以對于垂直角度大于水平視角的掃描線不作考慮。對于深度圖像中的每個激光點p_c，尋找上一行對應列的激光點記為p_u，則計算如下

當上式求解結果小于一定閾值angThre時，即被認為是地面點。具體的算法流程見算法1-1。

（2）基于深度圖像的點云分割

從三維點云數據中進行物體分割是移動機器人領域的一大研究課題。當機器人在環境中進行導航時，需要知道物體是否改變或移動，因此對場景中的物體進行分割是首要前提。由此，利用點云分割后的結果提取穩定、利于匹配的點云數據，可以看出，準確的物體分割對于后續處理是至關重要的。這里使用了前人研究所提出的一種快速且精準的點云分割算法，該算法用于判斷兩個相鄰（水平或垂直）激光束是否來自同一個物體表面。該點云分割算法示意如圖1-9所示。

假設激光雷達所在位置為O，OA和OB分別表示兩個相鄰（水平或垂直）激光束，AB連線可能在同一個物體表面也可能不在。該算法斷言：如果β＞θ（θ是固定閾值），則表示兩束激光打在同一物體表面，否則沒有打在同一物體表面。圖1-9b中，紫色與黃色的兩塊點云是激光雷達掃描到的兩個行人，圖中下部是進行的俯視圖投影，綠色線表示兩束激光來自同一物體（β＞θ），而紅色線的β值小于一定閾值，則兩束激光被標記為來自不同的物體。算法中β值的計算也非常簡單，即

式中，d₁是兩束激光中距離值大的那個；d₂是兩束激光中距離值小的那個；α是水平角分辨率（兩束激光水平相鄰）或垂直角分辨率（兩束激光垂直相鄰）。

具體的點云分割算法和標記函數見算法1-2和算法1-3。

圖1-10所示為應用該算法在三個場景進行點云分割的結果，其中閾值參數θ取10°。

1）在室內結構化場景中，場景信息比較單一，只存在墻面和一些動態的行人，分割難度低，由圖1-10a可以看出室內環境的分割效果很好。

2）圖1-10b所示為在室外道路上的測試結果。該場景中存在較多環境因素，如行人、車輛、樹木、草坪、墻面等信息，也均被正確地分割。

3）也在較為空曠的結構化變電站場景中進行了測試，如圖1-10c所示。近處的冬青樹以及墻面、路沿石被成功分割，環境中存在大量較細的變電柱也可以很好地被分割，對于遠處的墻和樹葉也具有較好的分割效果。

（3）穩定點云提取

從上一小節可以看出環境中的物體被正確分割，但是其中包含了很多動態的、不穩定的環境因素，例如樹葉（是不穩定的特征，因為同一片樹葉很難在連續的兩幀觀測中存在）、行人（不是靜態的，對于估計連續兩幀觀測間的位姿是不利的）等。因此本小節對于分割后的結果提出一種穩定點云的提取方法。

由于降采樣后的點云數據是輸入匹配算法中用于求解連續兩幀數據間的變換矩陣，故針對移動機器人在三維場景的應用進行位姿估計，該變換矩陣為6DOF的，即（t_x，t_y，t_z，θ_roll，θ_pitch，θ_yaw）。因此本小節提出的算法實質是尋找有助于求解該6DOF位姿的點云數據，例如地面點云對決定（t_z，θ_roll，θ_pitch）是有幫助的，平行于激光雷達X軸坐標方向的墻面對決定（t_y，θ_yaw）是有幫助的。

首先，為保證算法運行的實時性要求，對于提取的地面點云降采樣2s個激光點進行保存，因為地面點云對決定（t_z，θ_roll，θ_pitch）是有幫助的。

其次，由上一小節進行點云分割提取的點云塊集合{S_Label}，對于每個點云塊S_Label={p_i|i=1，…，N}計算均值和協方差矩陣：

對式（1-33）中協方差矩陣C進行特征值分解，得到特征向量e₀、e₁、e₂，對應的三個特征值為λ₀、λ₁、λ₂，特別地，λ₀≥λ₁≥λ₂且被歸一化。由主成分分析（Principal Component Analysis，PCA）可知，對于場景中物體的三種特征類型具有不同的特征值表示，若λ₀≈λ₁≈λ₂，則表示散亂特征；若λ₀＞＞λ₁≈λ₂，則表示線性特征；若λ₀≈λ₁＞＞λ₂，則表示平面特征。定義線性度與平面度如下

對每個點云塊計算如下三個值

式中，X_v是X軸單位向量；Y_v是Y軸單位向量；Z_v是Z軸單位向量；f₁是該點云塊為平面特征且對決定（t_x，θ_yaw）貢獻量的大小，f₂是該點云塊為平面特征且對決定（t_y，θ_yaw）量的大小；f₃是該點云塊為線性特征且對決定（t_x，t_y）貢獻量的大小。

其目的是盡量提取環境中對定位（t_x，t_y，θ_yaw）方向有幫助的平面特征（建筑墻面等）以及線性特征（樹干、柱子等）。對于所有點云塊，這三個值按照降序進行排列，每一列只取前s個激光點保存。

最終，提取出環境中有利于定位（t_z，θ_roll，θ_pitch）方向的地面點，以及有利于定位（t_x，t_y，θ_yaw）方向的墻面、樹干、柱子等點云數據。共計有5s個激光點云數據，實驗中一般設置s=1000。將降采樣后的點云數據輸入后文匹配算法中完成位姿估計。

為驗證所提出的點云降采樣策略的有效性，與傳統的體素濾波降采樣算法開展對比工作，為保證兩種算法降采樣后的點云數量的一致性，算法的關鍵參數s設置為1000，體素濾波的體素柵格設置為0.8m。兩種算法在兩個場景下點云降采樣的示例如圖1-11所示，直觀上兩種算法基本保證了環境的整體外觀形狀特征。

1）圖1-11a與圖1-11b所示為室外道路的場景下測試。其中，藍色點云所示是激光雷達觀測到的一幀完整數據，可以看出包含了較多的不穩定環境因素，例如草坪、樹葉等。圖1-11a中的紅色點云是經過所提出的點云降采樣策略后的輸出點云，僅包含墻面、柱子、樹干、地面等靜態、穩定的特征；反觀圖1-11b室外道路環境中的紅色點云，卻包含了大量的樹葉、遠處雜點等噪聲數據，這對于匹配算法顯然是不利的。

2）圖1-11c與圖1-11d所示為結構化變電站場景下的測試。由圖1-11c可以看出算法的輸出點云只保留了環境中利于匹配的特征，如墻面、地面、變電柱、變電設備等，圖1-11d所示的體素濾波后的點云輸出中包含較多打在近處冬青樹木上的激光點。

因此，所提出的點云降采樣策略是非常有效的，可以避免保留不利于匹配的樹葉、草坪、行人等動態噪聲數據，對于提升匹配精度是至關重要的。

1.1.3.2 多層ICP匹配方法

對輸入點云進行降采樣處理后，可以得到一幀用于匹配的靜態穩定點云數據。本節介紹MIM_SLAM算法中的多層ICP匹配方法，如圖1-6所示。這里所說的多層匹配，包括Scan-to-Scan匹配、Scan-to-SubMap匹配和Scan-to-KeyScan匹配等。其中利用Scan-to-Scan和Scan-to-SubMap相結合的匹配方式，可構建實時低漂移的激光里程計，用于解決在稀疏、特征匱乏場景下時間相鄰幀的數據關聯不可靠的問題。但是對于復雜場景下的應用，激光里程計不可避免的累積誤差問題可能導致地圖的不一致性，于是Scan-to-Key-Scan匹配被用來解決地域相鄰幀的數據關聯問題，也被稱為回環檢測問題。下面分別從三個方面來闡述。

（1）Scan-to-Scan匹配

假設已知t-1時刻機器人的位姿，對于t時刻輸入的一幀激光觀測數據，如何確定當前位姿即為Scan-to-Scan匹配。很多杰出的算法被提出來解決該類問題，例如ICP、NDT、柵格相關性等。其中，迭代最近點（Iterative Closest Point，ICP）算法發展最為迅速。近年來，各種ICP變種算法也被提出。

ICP算法常被用來解決多視點云之間的空間對齊問題，是一種基于最小二乘的點云最優配準方式。該算法的核心思想是對于輸入的兩幀點云數據，通過計算空間變換矩陣，使得它們可以統一到同一坐標系下，完成點云間的數據拼接。一般的計算步驟包括數據點對的關聯、計算空間變換矩陣、迭代此過程直至滿足收斂條件。這里使用的是ICP的一種改進算法即Point-to-Plane ICP，相比標準ICP具有更好的配準精度與魯棒性，其算法流程見算法1-4。ICP算法的迭代初值對算法的收斂情況影響很大，實驗中由輪式里程計或慣性測量單元（IMU）給定。利用ICP算法可以很容易地求解出兩幀數據間的變換矩陣（也即兩個位姿間的位姿約束），卻很難描述位姿間的不確定性，而在后端優化中需要明確給定該不確定性，實驗中采用配準距離S_fit（計算方式參見算法1-4）來描述，該配準距離越小表示不確定性越小，否則不確定性越大。

雖然上述Scan-to-Scan匹配可以用作構建激光里程計，實現同步定位與建圖，但是會帶來較大的累積誤差，尤其是在特征較為匱乏的場景中很容易導致匹配陷入局部極小，最終破壞了地圖的一致性。于是提出Scan-to-SubMap匹配來進一步減小該誤差，從而達到構建實時、低漂移激光里程計的目的。

（2）Scan-to-SubMap匹配

Scan-to-SubMap匹配過程如圖1-12所示，M_i表示i時刻已構建的全局地圖，表示i時刻全局坐標系下的機器人位姿，表示i+1時刻到i時刻機器人位姿的變換矩陣（即Scan-to-Scan匹配的輸出結果），z_i+1表示i+1時刻的激光觀測數據。由圖1-12可以看出，Scan-to-Scan匹配存在的誤差可能導致地圖的不一致性（橙色與黑色存在偏差）。

歸納Scan-to-SubMap匹配過程如下：

1）首先，對全局點云地圖用八叉樹結構描述。將z_i+1轉換到全局坐標系下，并對每個激光點找到地圖中一定半徑范圍內的近鄰點，將所有近鄰點轉移到局部坐標系下并保存作為m_i+1。

2）其次，為加速搜索，使用PCL庫中近似最近鄰（Approximate Nearest Neighbor，ANN）算法。

3）最后，將z_i+1和m_i+1作為算法1-4的輸入，可得到完成兩幀點云數據配準的變換矩陣T_opt。優化后的機器人在i+1時刻的位姿為：。

考慮SLAM算法的實時性問題，上述Scan-to-SubMap匹配過程不可能對每幀數據都進行優化，這會加重算法運行的負擔。由于三維激光雷達的數據發布頻率為10Hz，所以一般也對Scan-to-Scan匹配采用10Hz處理頻率，而Scan-to-SubMap匹配采用1Hz的處理頻率，即位姿發布為10Hz的頻率，全局地圖發布為1Hz的頻率，如此可保證算法的實時性要求。

經過上述Scan-to-Scan匹配與Scan-to-SubMap匹配結合的方式，可構建實時低漂移的激光里程計。利用該激光里程計算法進行建圖的示例如圖1-13所示。

圖1-13a與圖1-13b是在小范圍室內及長直走廊中的測試結果，初步驗證了算法的有效性；圖1-13c所示是在室內辦公區域（場景規模約80m×50m）內，遙控機器人沿走廊行走一圈進行激光里程計測試。此時并沒有利用回環檢測進行優化處理，由建圖結果可以看出地圖的一致性較高；圖1-13d是在室外道路環境下遙控機器人沿道路行走100m的測試結果，道路旁的車輛、建筑物、樹木等清晰可見，并沒有地圖疊加錯亂等現象。

由此可見，所提出的Scan-to-Scan匹配與Scan-to-SubMap匹配結合的激光里程計算法的有效性得到了驗證，在多個室內外小場景范圍內具有較好的建圖效果，但是在大范圍室外場景下還需要考慮回環優化以避免激光里程計的累積誤差，具體將在后文詳細介紹。

（3）Scan-to-KeyScan匹配

通過之前構建的激光里程計算法，已經可以實現在小范圍室內外環境下保證SLAM建圖的一致性。但是在復雜環境下，激光里程計不可避免的累積誤差會破壞地圖的一致性，主要表現在當機器人重新回到已訪問過的區域時。如果機器人知道該場景曾經訪問過，那么便可以修正當前估計的位姿，否則不正確的位姿所關聯的觀測數據必將破壞已建立的地圖。該問題即是SLAM中尤為關鍵的回環檢測問題，也是在復雜環境下建立全局一致性地圖的關鍵。在視覺領域，圖像檢索技術常被用于回環檢測，其中基于詞袋模型的方法已被廣泛應用，并取得了很好的效果。而在激光雷達領域，由于信息豐富度較視覺匱乏，所以回環檢測一直是激光雷達的短板。此外，由于三維激光雷達輸出的點云數據量太大，基于點云配準的方式很難保證實時運行，近年來大多數工作也都是融合視覺信息來實現回環檢測。因此，針對三維激光雷達的回環檢測問題，提出Scan-to-KeyScan匹配算法來解決機器人重新回到已訪問區域時的數據關聯。

算法流程見算法1-5。算法輸入為關鍵幀InScan以及處理頻率f。其中，關鍵幀的定義為當機器人行走一定距離后（實驗中設置該距離為0.5m）才認為是一幀關鍵幀；為考慮算法的時效性，對于輸入的關鍵幀滿足處理頻率f（實驗中設置f=6）才進行后續的Scan-to-KeyScan匹配，否則直接返回（算法1-5中的3—5行）。在進行匹配時，會與所有歷史關鍵幀進行遍歷匹配。為避免相鄰幀的相似性太高從而導致誤檢測，要忽略近鄰的幾幀觀測數據（7行）。這里，首先會進行基于隨機森林的場景識別檢測（詳細闡述見后文），尋找與當前機器人所處環境最為相似的歷史關鍵幀（8 行），然后應用算法1-4 介紹的Point-to-Plane ICP求解變換矩陣以及配準距離（10行），只有配準距離小于一定閾值才被加入到回環的候選集合中（12行）。

對于當前輸入的點云數據，檢測到的回環候選集合可能包含0個、1個或多個回環信息。若為0個，則表示沒有檢測到回環信息（17—19行）；若不為0個，則挑選具有最小配準距離S_fit的一組回環信息，因為當前的回環幀對具有最好的配準效果。但此時還不能直接將其輸出作為最終檢測到的回環信息，因為環境中可能存在高度相似的場景，這可能是誤檢測，因此在輸出前會進行幾何驗證（21行）。

回環檢測中的幾何驗證過程如圖1-14所示。其中，圓圈表示關鍵幀，圓圈間的箭頭表示運動過程。對于機器人觀測到第i′-f、i′、i′+f個關鍵幀時，對應的第i-f、i、i+f個關鍵幀是檢測到的候選回環，只有滿足如下條件才認為第i′+f個與第i+f個關鍵幀是檢測到的回環幀對

式中，‖ ‖是二范數；T是兩個關鍵幀間的位姿變換矩陣；trans函數是取變換矩陣的平移部分；rot函數是取變換矩陣的旋轉部分；t與r分別是平移與旋轉閾值，實驗中一般分別設置為0.1m和0.035rad。該方法可有效避免因誤檢測情況的發生而導致地圖的一致性破壞。

最終，整個多層ICP匹配算法結束。經過Scan-to-Scan匹配、Scan-to-SubMap匹配以及Scan-to-KeyScan匹配構建的位姿及位姿約束與不確定性估計，統一保存在位姿圖結構中，并通過增量優化算法進行全局位姿優化。至此，整個SLAM問題得到求解。

1.1.3.3 基于隨機森林學習的場景識別

由上述可知，使用Scan-to-KeyScan匹配方法來解決SLAM中的回環檢測問題，其中的關鍵步驟便是基于隨機森林學習的場景識別。下面詳細闡述該算法流程。

圖1-15所示為MIM_SLAM所使用的基于隨機森林的場景識別算法（以下稱為RF-PR算法）框架。具體地，在離線部分采集用于訓練的數據集，并且基于隨機森林的學習方法訓練模型參數，這里的輸入為兩幀激光觀測數據的特征向量表示，輸出為兩幀數據是否來自同一場景。在線部分對于輸入的激光觀測先用特征向量表示，并分別與數據庫中所有的特征向量表示構建成對（Make Pair），輸入到隨機森林完成分類，最終輸出為1表示兩幀觀測數據來自同一場景。若用此方法解決SLAM中的回環檢測問題，則這里的“特征向量表示數據庫”為SLAM中所有歷史關鍵幀的特征向量表示。

（1）特征向量表示

由于三維激光雷達一幀數據量太大，不可能直接輸入到隨機森林分類器中進行訓練，需要用一組特征向量來表示，故提出了圖1-16所示的三類特征描述方式。三類特征均具有旋轉不變性，分別介紹如下。

1）投影特征：f₁～f₆₀。該特征是將點云數據分別向XY、XZ、YZ平面投影。以投影到XY平面為例（圖1-16a），輸入點云為S，對于其中每個激光點p_i=（x_i，y_i，z_i）計算到原點（即激光中心）的距離為，然后將平面中到原點距離在[d_min，d_max]范圍內的區域劃分為20個等間隔的圓環{I_j|j=1，…，20}，最后統計落在每個圓環中的激光點數目，并歸一化處理，即得到

同理，投影到XZ、YZ平面上也可以分別得到一個包含20個值的特征向量。該類特征描述了一幀觀測數據的所有激光點的分布情況，是一種全局描述子。

2）法向量曲率特征：f₆₁～f₁₀₀。表示方式如圖1-16b所示，輸入點云為S，對于其中每個激光點p_i=（x_i，y_i，z_i）計算法向量

式中，v₁是當前激光點指向上一條掃描線對應水平角度的激光點；v₂是當前激光點指向同一掃描線其后第三個激光點；v₃是當前激光點指向下一條掃描線對應水平角度的激光點；v₄是當前激光點指向同一條掃描線其前第三個激光點。

對法向量進行單位化處理，并取投影在Z軸的值：。將該區間劃分為20個子區間{I_j|j=1，…，20}，對所有激光點計算值，并統計落在每個子區間的點云數目，進行歸一化處理

對法向量曲率特征表示中每個激光點p_i=（x_i，y_i，z_i）計算曲率

式中，S是激光點p_i同一條掃描線最近鄰的8個激光點組成的點集。

定義c_i取值范圍為[c_min，c_max]，將該區間劃分為20個子區間{I_j|j=1，…，20}。對所有激光點計算c_i值，并統計落在每個子區間的點云數目，進行歸一化處理

該類特征可以很好地描述機器人周圍環境中分布物體表面的朝向、起伏等特性，是一種局部描述子。

3）基于掃描線特征：f₁₀₁～f₁₄₈。表示方式如圖1-16c所示。對于每條掃描線計算如下三個值，表示該掃描線上所有連續激光點距離之和，表示該掃描線覆蓋面積，表示該掃描線上所有激光點的距離標準差。

式中，N_i是第i條掃描線的激光點數目；x、y、z分別是某個激光點在X軸、Y軸、Z軸方向的坐標；r是某個激光點的距離（或深度）值；α是該激光點投影在XY平面與Y軸方向的夾角；L是掃描線的條數。

該類特征是基于掃描線的特征描述方式得到的，它可以很好地描述每條掃描線的激光點分布特性；同時對于環境中存在動態障礙時的描述性更好，因為動態物體通常只改變幾條掃描線的狀態，而大多數掃描線的狀態是不受影響的。具體實現時，若采用具有16條掃描線的Velodyne VLP-16激光雷達，則該類特征包含48個值即f₁₀₁～f₁₄₈；若采用具有64條掃描線的Velodyne HDL-64E，則選擇每隔4條掃描線取1條處理，因此最終也包含48個值即f₁₀₁～f₁₄₈。最終，均可得到包含148個值的特征向量來表示一幀三維激光觀測數據。

（2）隨機森林學習

對于兩幀觀測數據z_k和z_k+1，利用特征向量表示方法可表示為，，N=148。定義如下函數

則n組訓練數據可表示如下

式中，y_i∈{0，1}是二進制變量，y_i=0表示兩幀激光觀測不來自同一場景，否則表示來自同一場景。

在獲取訓練數據集之后，便可以使用隨機森林學習方法進行訓練，其算法流程見算法1-6。隨機森林算法利用集成學習的思想，通過將多個學習器進行結合，可獲得比單一學習器更顯著的泛化性能；此外，自助采樣法以及在決策樹的訓練過程中引入隨機屬性選擇相結合，使得隨機森林算法中基學習器的多樣性不僅來自于樣本擾動還有屬性擾動，對于分類的抗干擾性大大加強，最終集成的泛化性能可通過個體學習器之間差異度的增加而進一步提升。

（3）場景識別性能

在訓練隨機森林分類器之前需要獲取訓練數據集，傳統的手工標注方式是困難且煩瑣的，因此考慮一種更為簡單的方式獲取用于訓練的數據集。選取室內地下停車場場景來構建訓練數據集，由于缺乏地面真值數據，故利用前述的低漂移的激光里程計算法來產生位姿。為保證位姿的精度，控制機器人以較慢的速度（約為0.5m/s）行駛，同時考慮激光里程計會隨著機器人行駛距離的增加會有較大的累積誤差，因此控制每次行駛距離也較短。考慮激光雷達頻率較高，任意相鄰的兩幀觀測可被認為來自同一場景，同時通過刻意控制機器人重復訪問之前經歷的場景以產生更多的正樣本，對于判斷是否兩幀觀測數據來自同一場景可用如下方式

式中，P₁和P₂分別是兩幀觀測的位置坐標；d_thre是距離閾值，選擇3m。

由此，可產生大量的正負樣本來訓練隨機森林分類器。在這個訓練數據集中，最終生成了21700的正樣本以及21700的負樣本。為了保證測試的合理性，在該場景下，又用同樣方式生成了包含12000正樣本和12000負樣本的測試數據集。

利用隨機森林學習方法對構建好的訓練數據集進行訓練，在得到模型參數后，在測試數據集上驗證所提出的RF-PR場景識別算法的有效性，并與基于激光幀描述的方法進行對比：

1）Small-Size Signature（S-SS）：該算法對三維激光雷達產生的點云數據提取法向量特征構建直方圖，并采用EMD距離（Earth Mover’s Distance）度量兩個直方圖之間的相似性。

2）Fast Histogram（FH）：該算法對三維激光雷達產生的點云數據提取高度特征構建直方圖，并采用卡方距離度量兩個直方圖之間的相似性。

為定量評估場景識別精度，引入監督學習方法中兩個重要的評價指標，分別為分類精度和受試者工作特征（Receiver Operating Characteristic，ROC）曲線。此外，該數據集使用的激光雷達型號為Velodyne VLP-16，因此所提出算法的特征向量表示的長度為148，對于另外兩種對比算法直方圖的bin的個數也設置為148。

1）分類精度：為了評價在測試數據集上的預測結果，引入如下錯誤率指標

式中，I（·）是一個函數，當為真時返回1，否則返回0；y_i是真值結果；是預測結果；m是樣本總數。

給定不同的d_thre，三種算法的分類精度見表1-1，顯然RF-PR算法具有更低的分類錯誤率。

2）ROC曲線：ROC曲線橫坐標為假正例率（False Positive Rate，FPR），即實際為負樣本中預測為正樣本的比例；縱坐標為真正例率（True Positive Rate，TPR），即實際為正樣本中預測為正樣本的比例。曲線下方圍成的面積被稱為AUC（Area Under ROC Curve），該面積越大表示分類性能越好。

三種算法的ROC曲線如圖1-17所示。圖1-17a中的黃色曲線相比藍色（0.87）和紅色（0.80）有更大的AUC值（0.95），綠色曲線表示隨機猜想，顯然所提算法（RF-PR）的表現更好。對于兩幀觀測數據，當關聯的位姿距離值小于閾值d_thre時認為是正樣本（即來自同一場景），不同的d_thre值可能影響算法性能，因此選擇幾個離散的不同距離值：d_thre=2m，d_thre=3m，d_thre=5m，d_thre=10m，并在S-SS和所提算法開展對比測試（圖1-17b）。顯然，當該距離閾值越大時識別效果越差，因為此時很難描述兩幀觀測數據之間的相似性或差異性；然而該取值也不能取太小，以防止實際為同一場景卻無法識別的情況。由此，實驗中一般設置為d_thre=3m。

通過上述對比實驗，在采集的大范圍地下停車場場景下的數據集上，由定量結果可以驗證所提出的場景識別算法相比另外兩個算法具有更好的識別性能。

1.1.3.4 算法性能測試

為驗證MIM_SLAM算法整體性能，首先在室外道路場景中進行測試。在實驗中，遙控移動機器人以1m/s的速度行駛了約950m，共采集10472幀數據。機器人的行駛路徑如圖1-18a所示（場景規模約180m×150m），紅色實線顯示的路徑序列為ABCDEFGCAHFGBH，其間經歷了多次重復路徑。

MIM_SLAM算法建立的場景點云地圖如圖1-18b所示。由圖可以看出地圖創建精度較高，地圖一致性較好。主要原因是該場景結構化信息較為豐富，MIM_SLAM算法構建的低漂移的激光里程計均保證了較小的累積誤差，所以地圖的一致性較高，幾乎不存在點云的錯亂累積現象。

接下來，在公開數據集KITTI上對提出的MIM_SLAM算法進行了驗證。實驗中，特別挑選了其中3種典型類型的場景，并且與標準ICP（Standard ICP）、LOAM算法進行定量對比分析。

1）鄉村道路（Sequence 03）：道路較窄，動態的行人、車輛等較少，周圍存在大量的田園樹木、自然地形等。該數據集被用來驗證提出的MIM_SLAM方法可以在室外特征稀疏的鄉村田園場景下實現低漂移的位姿估計。建圖結果如圖1-19a所示，無人車平臺的運行路徑及真值數據如圖1-20a所示。由于在該場景下穩定特征點較少，尤其是在運行約420m之后，實際的運行軌跡與真值存在些許偏差，但整體建圖效果并沒有存在錯亂等現象，仍然保持了較高的一致性。為了定量評估算法的性能，這里引入了KITTI中評測測距數據集的方法，它對所有的可能的子序列長度（100m，200m，…，800m）計算平均平移及旋轉誤差。圖1-21a所示為該場景下的位姿估計誤差，相比標準ICP算法，LOAM算法與MIM_SLAM算法均具有較高的激光里程計精度，可保證平移誤差在1%以內，旋轉誤差在0.005°/m以內。

2）城市道路（Sequence 07）：道路上動態行人、車輛等較多，周圍具有較多的墻面、建筑物等顯著特征。該數據集被用來驗證提出的算法可有效處理回環區域的數據關聯問題，并通過增量優化算法建立全局一致性高的地圖。如圖1-19b所示，MIM_SLAM算法建立的全局點云地圖中建筑物、柱子等場景信息清晰可見，幾乎不存在錯誤累積，點云地圖一致性很高。圖1-20b所示的運行軌跡與地面真值軌跡也幾乎是重合的。如圖1-21b所示，MIM_SLAM算法相比LOAM算法平移與旋轉誤差更低。

3）高速道路（Sequence 06）：車速較快，道路上動態行人、車輛等較多。該數據集是城市道路場景，場景中具有較高的動態、噪聲數據，且規模較大。KITTI無人車以6.2m/s的速度行走約600m停止。MIM_SLAM算法建立的點云地圖結果如圖1-19c所示，KITTI無人車平臺的運行軌跡及地面真值數據如圖1-20c所示。直觀上看，點云地圖的輪廓是清晰可見的，地圖一致性較高，估計的位姿和地面真值數據幾乎是重合的，具有較高的建圖精度。位姿估計的誤差如圖1-21c所示。由此可見，MIM_SLAM算法在動態因素較高的城市道路場景上也具有較好的表現，幾乎不輸LOAM算法。

由此可見，在公開數據集KITTI上再一次驗證了MIM_SLAM算法的有效性與較高的建圖精度，相比該數據集上的榜首算法LOAM也具有相似或更好的表現性能，可滿足在大范圍室外場景下的實時高精度建圖的應用。