激光SLAM第六章大學生禮儀高等院校公共課系列精品教材01SLAM的概念及分類PARTONE01SLAM的概念及分類

SLAM最早在機器人領域提出，它指的是機器人從未知環境的未知地點出發，在運動過程中通過重復觀測到的環境特征定位自身位置和姿態，再根據自身位置構建周圍環境的增量式地圖，從而達到同步定位和地圖構建的目的。SLAM系統使用的傳感器在不斷拓展，從早期的聲吶到后來的2D/3D激光雷達，再到單目、雙目、RGB-D、TOF等各種相機，以及與慣性測量單元等傳感器的融合。SLAM算法也從開始的基于濾波（擴展卡爾曼濾波、粒子濾波等）的算法向基于優化的算法轉變，技術框架也從開始的單一線程向多線程演進。01SLAM的概念及分類

五輪機器人的研究較少，僅有日本空間與航天科學研究所研究的Micro-5機器人和上海交通大研究的五輪鉸接式機器人。移動機器人要想實現自主運動能力，首先要知道行動的目標，避免無目的的行動，即對其進行控制和規劃。但是控制和規劃的前提是移動機器人首先知道自己周圍的環境及其自身的位置，然后才能進行相應的操作。SLAM技術作為實現移動機器人自主移動的關鍵技術之一，恰恰回答了這兩個問題。（1） 我在什么地方？一一定位。（2） 我周圍的環境是什么樣的？一一一建圖。SLAM的基本作用01SLAM的概念及分類

(1)激光SLAM激光雷達的優點是測量精確，能夠比較精準地提供角度和距離信息，可以達到<1。的角度精度及厘米級別的測距精度，掃描范圍廣（通常能夠覆蓋平面內270。以上的范圍），而且基于掃描振鏡的固態激光雷達（如Sick、Hokuyo等）可以達到較高的數據刷新率(20Hz以上），基本滿足實時操作的需求。其缺點是價格比較昂貴，安裝部署對結構有要求（要求掃描平面無遮擋）SLAM的分類(2)視覺SLAM視覺SLAM的優點是它所利用的豐富的紋理信息，如兩塊尺寸相同、內容卻不同的廣告牌，基于點云的激光SLAM無法區分它們，但視覺SLAM可以輕易分辨，這帶來了重定位、場景分類上無可比擬的巨大優勢。同時，視覺信息可以較為容易地被用來跟蹤和預測場景中的動態目標，如行人、車輛等，對于在復雜動態場景中的應用，這是至關重要的。01SLAM的概念及分類

(3)多傳感器融合的SLAM如果移動機器人除搭載以上某一種傳感器外還搭載了其他傳感器，如慣性測量單元、輪速計、GPS、毫米波雷達等，則為多傳感器融合的SLAM。多傳感器的融合能夠幫助我們應對比較復雜的場景和環境，降低使用成本。慣性測量單元由3個單軸的加速度計和3個單軸的陀螺儀組成，提供了一個相對的定位信息，即測量相對于起點物體所運動的路線，所以它并不能提供物體所在的具體位置的信息。因此，它常常和GPS一起使用，當在某些GPS信號微弱的地方時，慣性測量單元就可以發揮它的作用，可以讓汽車繼續獲得絕對位置信息，不至于“迷路”。SLAM的分類01SLAM的概念及分類

傳感器數據；主要用于采集實際環境中的各類型原始數據，包括激光掃播數據、視頻圖像數據、點云數據等。3.SLAM的基本框架前端視覺里程計：主要用于不同時刻間移動目標相對位置的估算，包括特征點匹配、直接配準等算法的應用。對傳感器送來的每幀數據進行處理，估計相鄰幀的位姿，給后端優化提供一個初始位姿。01SLAM的概念及分類

后端非線性優化：主要用于優化前端帶來的累積誤差，包括濾波、圖優化等算法的應用。接收前端送來的不同時刻的初始位姿及回環檢測的信息，對它們進行優化，得到全局一致的軌跡和地圖。回環檢測：主要用于空間累積誤差的消除。判斷運動主體是否進行一段SLAM任務后又回到了之前走過的位置，從而實現累積誤差的消除.建圖：用于三維地圖的構建。根據估計的軌跡，建立與任務要求一致的地圖。02SLAM的數學表述PARTONE02SLAM的數學表述

現實世界：客觀存在的世界就是現實世界，它獨立于人們的思想之外。現實世界存在無數的對象和事物，每一個對象或事物可以看成一個個體，每個個體有一項或多項特征信息。信息世界：信息世界是現實世界在人們頭腦中的反映，人的思維將現實世界中的對象或事物的特征抽象化后用文字符號表示出來，從而形成了信息世界。機器世界；機器世界又稱為數據世界。信息世界中的信息經過抽象和組織，以數據的形式存儲在計算機中，從而形成了機器世界。02SLAM的數學表述

通常，移動機器人會攜帶一個測量自身運動的傳感器，如碼盤或慣性傳感器，這個傳感器可以測量有關運動的讀數，但不一定直接是位置之差，還可能是加速度、角速度等信息。然而，無論是什么傳感器，我們都能使用一個通用的、抽象的數學模型：1.移動機器人運動的數學描述02SLAM的數學表述

以二維運動為例，假設移動機器人攜帶相機在平面中運動，那么它的位姿可以由兩個位置和一個轉角來描述，二維運動的位姿如圖6.2所示。02SLAM的數學表述

2.關于觀測的數學描述與運動方程相對應，還有一個觀測方程。觀測方程描述的是，當移動機器人在xx位置上看到某個路標點y,時，產生了一個觀測數據k;。同樣，我們用一個抽象的函數h來描述這個關系：02SLAM的數學表述

這兩個方程描述了最基本的SLAM問題，當我們知道運動測量的讀數v,以及傳感器的讀數z時，如何求解定位問題(估計x)和建圖問題)?這時，我們把SLAM問題建模成一個狀態估計問題：如何通過帶有噪聲的測量數據估計內部的、隱藏的狀態變量?狀態估計問題的求解，與兩個方程的具體形式，以及噪聲服從哪種分布有關。我們按照運動和觀測方程是否為線性、噪聲是否服從高斯分布進行分類，分為線性/非線性和高斯/非高斯系統。其中線性高斯(LinearGaussian,LG)系統是最簡單的，它的最優無偏估計可以由卡爾曼濾波器給出；而在復雜的非線性非高斯(Non-LinearNon-Gaussian,NLNG)系統中，我們會使用擴展卡爾曼濾波和非線性優化兩大類方法去求解它。直至21世紀早期，以擴展卡爾曼濾波為主的濾波方法占據了SLAM中的主導地位。03濾波激光SLAMPARTONE03濾波激光SLAM

貝葉斯濾波器包括卡爾曼濾波器、擴展卡爾曼濾波器、粒子濾波器等，應用貝葉斯迭代狀態估計理論，即先對移動機器人的運動進行建模，構造出貼合物理場景的運動方程和觀測方程，如輪式移動機器人常使用基于速度的運動模型。之后，采用卡爾曼濾波的5條公式，進行狀態預測和測量更新。狀態預測依靠運動方程，從當前狀態估計出下一時刻的移動機器人位姿；而測量更新，則是在移動機器人觀測到新的點時，對之前的預測值進行修正，可以看到，該過程是一個遞歸估計過程，為從k時刻到k+1時刻的估計。由于卡爾曼濾波算法是針對線性高斯系統的最優無偏估計，而在實際場景中，移動機器人的運動并不滿足線性特性，且噪聲項不滿足高斯分布，因此使用卡爾曼濾波不能精確地計算出結果。擴展卡爾曼濾波可以將線性系統的約束擴展到非線性系統，獲得更好的結果。然而其依舊不能逃出高斯分布的限制，因此實際中使用粒子濾波代替上述方案。粒子濾波類SLAM不依賴參數化的運動方程，使用大規模粒子點去模擬無參數化的分布，理論上可以近似各種分布，如早年業界流行的Gmapping算法，即采用了該方案。03濾波激光SLAM

激光SLAM系統模型假設移動機器人在未知環境中運動，并通過激光雷達獲取周圍環境信息。由于激光雷達本身是以離散時間序列來采集數據的，所以只考慮在這些離散時刻移動機器人所處的位置和周圍環境的地圖。將移動機器人連續時間的運動離散化，記t=1,2,…,N。SLAM模型通過運動方程和觀測方程來描述每一時刻移動機器人在三維空間中的運動及其周圍環境的信息，如下所示。03濾波激光SLAM

考慮到傳感器的測量數據會受到噪聲的影響，所以SLAM模型的運動方程及觀測方程均會受到噪聲的影響，故移動機器人的位姿x及環境中的路標y(統稱狀態變量)應該看作隨機變量。因此，針對SLAM問題，從概率角度出發提出的解決思路是，當已知運動數據u和觀測數據z時，如何確定狀態變量x的分布，以及當新的傳感器數據到來之后，狀態變量x的分布又將如何變化。假設狀態變量x及傳感器噪聲均服從高斯分布，則在設計算法時，只需要計算出狀態變量的均值及協方差矩陣。均值可以看成對狀態變量的最佳估計，而協方差矩陣則是對該估計不確定性的度量，從而問題變為當存在一些運動數據和觀測數據時，如何去估計狀態變量的高斯分布。03濾波激光SLAM

首先，由于移動機器人的位姿是待估計的狀態變量，令x,為t時刻的移動機器人的位姿，同時，把：時刻的所有觀測量記為z。于是，SLAM的運動方程和觀測方程的形式如下。對于接下來的推理步驟有兩種方法；一種方法是假設馬爾可夫性，認為：時刻的狀態只與t-1時刻的狀態有關，而與之前的狀態無關。基于該假設，產生了以擴展卡爾曼濾波為代表的基于濾波方法的SLAM問題的解決方案。另一種方法是考慮所有時刻狀態之間的影響，產生基于非線性優化方法為主體的SLAM問題的解決方案。03濾波激光SLAM

貝葉斯濾波首先介紹貝葉斯濾波中涉及的假設。(1)馬爾可夫假設。馬爾可夫性質主要在于,t時刻的狀態由t-1時刻的狀態和t時刻的動作決定,t時刻的觀測僅與t時刻的狀態有關。(2)系統所處環境是靜態環境,即假設對象周邊的環境是不變的。(3)觀測噪聲、模型噪聲等是彼此獨立的。03濾波激光SLAM

作為經典的狀態推斷方法，貝葉斯濾波方法是很多算法的基礎，如卡爾曼濾波算法、擴展卡爾曼濾波算法、粒子濾波算法等。值得一提的是，這里的Bel只是求出了：時刻移動機器人的位姿，即解決了SLAM問題中的定位問題。如果我們要完全解決SLAM問題，則我們需要根據位姿x與對應的觀測數據構建地圖，這個比較容易實現。03濾波激光SLAM

擴展卡爾曼濾波SLAM算法卡爾曼濾波以貝葉斯濾波為基礎,假設Bel(xz)服從高斯分布,在每個時刻只需要計算出均值和方差,就可以完成對Bel()的描述。卡爾曼濾波是一種遞歸算法,它的優點是計算復雜度低,缺點是不能處理非線性估計問題,因此引出了擴展卡爾曼濾波,一種通過線性近似得到系統狀態估計的方法。擴展卡爾曼濾波對非線性函數的泰勒展開式進行一階線性化截斷,忽略其余高階項,從而將非線性問題轉換成線性問題。03濾波激光SLAM

擴展卡爾曼濾波算法用多元高斯函數表示移動機器人的估計，在SLAM問題中可表示如下。03濾波激光SLAM

擴展卡爾曼濾波SLAM算法首先利用運動模型估計移動機器人的新位姿，并通過觀測模型預估可能觀測到的環境特征，計算出實際觀測和估計觀測間的誤差，然后綜合系統協方差計算卡爾曼濾波參數，并用其對之前估計的移動機器人位姿進行校正，最后將新觀測到的環境特征加入地圖。移動機器人在移動過程中循環不斷地估計一校正，盡量消除累積誤差，獲得盡可能準確的定位和地圖信息。圖6.5所示為一個擴展卡爾曼濾波SLAM的仿真過程。圖中，康線表示移動機器人的路徑，移動機器人從初始位姿或坐標系原點開始運動。當它移動時，它自身姿態的不確定性就會增大，即圖中半徑不斷擴大的黑色橢圓。在移動的同時，移動機器人不斷感知環境中的目標(如地標),并在環境對象上將固定的測量不確定度與增加的姿態不確定度結合起來，因此，地標位置的不確定性將會隨時間的推移而增大，在圖中用半徑不斷增加的白色橢圓表示，03濾波激光SLAM

之后如圖6.5(d)所示，當移動機器人觀察到它在初始位姿觀測到的地標時，可以看到移動機器人的姿態誤差減小了。最終，移動機器人的姿態誤差橢圓很小，同時地圖上其他地標的不確定性也減小了。這是由于移動機器人在獲取到之前觀察到的地標信息時，會相應地修正逐步累積的誤差。03濾波激光SLAM

粒子濾波針對擴展卡爾曼濾波實現較困難等缺陷，使用粒子濾波定位能夠適應任意噪聲分布，并且其容易實現。粒子濾波是一種基于蒙特卡羅模擬的非線性濾波方法，其基本思想是：用一組粒子預估移動機器人的可能位姿(處于該位姿的概率),每個粒子對應一個位姿，利用觀測結果對每個粒子進行加權傳播，從而使最有可能的位姿的概率越來越高。1999年，Carpenter等人在文獻中正式提出粒子濾波這一名稱。其利用一套粒子表示后驗概率，其中每個粒子表示這個系統可能存在的一種潛在狀態。狀態假設表示為一個有n個加權隨機樣本的集合$。03濾波激光SLAM

權重為非0值，且所有權重的總和為1,樣本集S可用于模擬任意分布，這些樣本是從被近似的分布中采樣而來的。粒子濾波器利用一套樣本集對多模態分布模型建模的能力較其他系列的濾波器有更大的優勢。03濾波激光SLAM

重采樣技術解決了粒子退化問題。粒子退化問題如圖6.6所示，當沒有圖6.7所示重采樣過程時，隨著迭代次數的增加，只有少數粒子的權重比較大，其余粒子的權重變得很小。粒子權重的方差隨著時間的推移而增大，狀態空間中的有效粒子數減少。隨著無效粒子數的增加，大量計算浪費在無效粒子上，估計性能下降03濾波激光SLAM

因為權重小的粒子基本是無用的，所以需要舍棄這些粒子，同時，為了保持粒子的數量不變，需要生成新的粒子來取代它們，這便是重采樣的基本思想。一個簡單的生成新粒子的方法是將權重大的粒子按照自己權重所占的比例分配需要生成的新粒子數目。但這樣又會帶來一個新的問題：粒子耗散，即粒子的多樣性降低。粒子濾波是一種非參數表示，可以輕松地表示多模態分布，但粒子濾波也存在粒子耗散、維數災難等問題，04Gmapping算法PARTONE04Gmapping算法

FastSLAM算法FastSLAM算法獨辟蹊徑，采用RBPF算法，將SLAM算法分解成兩個問題：一個是移動機器人的定位問題，另一個是已知移動機器人位姿進行地圖構建的問題。分解過程的公式推導如下，04Gmapping算法

整個過程以一個粒子為例，其中每個粒子都攜帶著上一時刻的位姿、權重、地圖。從式(6-25>我們可以看出，上一時刻移動機器人的軌跡通過里程計的狀態傳播之后，我們得到了該粒子的預測位姿。根據預測位姿在觀測模型的作用下，我們得到了該粒子代表的當前移動機器人軌跡，也就是完成了該粒子的移動機器人位姿估計。通過式(6-24),根據移動機器人的軌跡并結合觀測數據，即可閉式得到該粒子代表的地圖。這樣每一個粒子都存儲了一個移動機器人軌跡及一張環境地圖。04Gmapping算法

Gmapping算法概述根據上一節的描述，我們已經知道SLAM問題被分解成了兩個問題；移動機器人的軌跡估計問題和已知移動機器人軌跡后的地圖構建問題。FastSLAM算法中的移動機器人軌跡估計問題使用的是粒子濾波算法，由于使用的是粒子濾波算法，將不可避免地帶來兩個問題：第一個問題，當環境大或移動機器人里程計的誤差大時，需要更多的粒子才能得到較好的估計，這時將造成內存爆炸第二個問題，粒子濾波算法避免不了使用重采樣以確保當前粒子的有效性，然而重采樣帶來的問題就是粒子耗散、粒子多樣性的丟失。由于這兩個問題出現，導致FastSLAM算法理論上可行實際上卻不能實現。針對以上問題，Gmapping算法提出了兩種針對性的解決方法，也就是說，Gmapping算法基于FastSLAM算法將RBPF算法變成了現實04Gmapping算法

1.降低粒子數蛋問題由來：每一個粒子都包含自己的柵格地圖，對一個稍微大一點的環境來說，每一個粒子都會占用比較大的內存。如果移動機器人里程計的誤差比較大，即proposal分布(提議分布)跟實際分布相差較大，那么需要較多的粒子才能比較好地表示移動機器人位姿的估計，這樣將會造成內存爆炸。分析：里程計的概率模型比較平滑，有一個比較大的范圍，如果對整個范圍來樣將需要很多粒子，如果能找到一個位姿優值，在其周圍進行小范圍采樣，這樣就可以降低粒子數量。04Gmapping算法

方法一：直接采用極大似然估計的方式，根據粒子位姿的預測分布和與地圖的匹配程度，通過掃描匹配找到粒子的最優位姿參數，將該位姿參數直接當作新粒子的位姿。如下式所示，用極大似然估計提升采樣的質量，也就是Gmapping算法中的做法。04Gmapping算法

方法二；Gmapping算法通過最近一幀的觀測把提議分布限制在一個狹小的有效區域內，再正常地對提議分布進行采樣。由圖6.8所示的激光雷達匹配與里程計估計的位姿模型可知，激光雷達匹配的精度比里程計測量的精確很多，因為其方差要小得多。04Gmapping算法

如果提議分布用激光雷達匹配來表示，那么可以把采樣范圍限制在一個比較小的區城內，因此可以用更少的粒子覆蓋移動機器人位姿的概率分布，也就是說，提議分布從里程計的觀測模型變換到了激光雷達觀測模型。04Gmapping算法

從圖6.8可以看出激光雷達觀測模型的方差較小，假設其服從高斯分布，使用多元正態分布公式即可進行計算，于是求解高斯分布是下面需要做的。同方法一，首先通過極大似然估計找到局部極值x,認為該局部極值距離高斯分布的均值比較近，于是在位姿x,附近取N個位姿；然后對這N個位姿進行打分(位姿匹配),這樣我們就具有N個位姿，以及它們與地圖匹配的得分；最后我們又假設它們服從高斯分布，計算出它們的均值和方差之后，即可使用多元正態分布，對移動機器人位姿的估計進行概率計算。04Gmapping算法

2,緩解粒子耗散問題由來：Gmapping算法采用粒子濾波算法對移動機器人的軌跡進行估計，必然少不了粒子重采樣的過程。隨著采樣次數的增多，會出現所有粒子都從一個粒子復制而來的情況，這樣粒子的多樣性就完全喪失了。目的：緩解粒子耗散。分析：Gmapping算法在算法原理上使用粒子濾波算法，不可避免地要進行粒子重采樣，所以，問題實實在在地存在，不能解決，只能從減少重采樣的思路走。方法：Gmapping算法提出選擇性重采樣的方法，根據所有粒子自身權重的離散程度(權重方差)來決定是否進行粒子重采樣的操作。于是就出現了式(6-28),當N小于某個閾值時，說明粒子的差異性過大，進行重采樣，否則，不進行04Gmapping算法

Gmapping算法流程Gmapping算法的總體思路如下。接收激光信息、里程計信息，并根據里程計信息構建誤差模型。初始化粒子群，遍歷上一時刻的粒子群，獲取位姿、權重、地圖。通過里程計進行位姿更新，通過極大似然估計求得局部極值，獲取最優粒子。4.若未找到局部極值，可通過觀測模型對位姿更新。5.若找到局部極值，可選取周圍k個粒子位姿，計算其高斯分布、均值、權重、方差，近似新位姿，并對粒子位姿權重進行更新。6.擴充地圖、粒子群。7.根據粒子權重的離散程度選擇是否對新的粒子位孌重采樣。04Gmapping算法

04Gmapping算法

04Gmapping算法

1.粒子數據Gmapping算法是基于粒子濾波的激光SLAM算法，在粒子濾波部分，我們對粒子進行了簡單的介紹，但實際使用時，粒子包含的內容更加廣泛。Gmapping算法會初始化一群粒子，用這群粒子的分布來表示移動機器人位姿的概率分布。每一個粒子都包含如下數據：該粒子當前的地圖，僅占據部分(柵格地圖分占據、空閑、未知三種狀態，后面有詳解);該粒子的當前最優位姿；該粒子的權重及累計權重；該粒子的整條軌跡，表示移動機器人的軌跡04Gmapping算法

2、激光雷達運動畸變激光雷達運動畸變在以激光雷達為外部傳感器的移動機器人系統中是比較常見的一個問題。激光雷達運動畸變會直接影響移動機器人SLAM系統的建圖效果，進而影響移動機器人定位的準確性。它產生的原因是激光雷達安裝在移動機器人上，當移動機器人運動時，激光的測量伴隨著移動機器人的運動，因為激光對環境當中的點云信息不是同一時刻獲取的，一幀數據的首末位獲取是存在時間差的，當激光雷達的幀率較低時或移動機器人運動較快時，激光雷達運動畸變就會產生較大的影響。激光雷達運動畸變的原理示意圖如圖6.10所示，假設移動機器人由位置A運動到位置B,激光的掃描方向為逆時針。在位置A時掃描到空間某點C,由于移動機器人一直在運動，當移動機器人在當前幀激光掃描結束的時間內運動到位置B時，移動機器人觀測到空間某點D,而此時移動機器人的位置并沒有更新，移動機器人依然認為觀測到的點D是在位置A時得到的數據，這就造成了數據的觀測誤差。04Gmapping算法

有了激光雷達運動畸變就要去除這種畸變帶來的影響，常見的激光雷達運動畸變去除方法是利用傳感器輔助來去除，這里采用的是里程計來輔助去除激光雷達運動畸變，它的優勢主要有以下3點：①里程計具有很高的位姿更新頻率，一般都能達到100～200Hz,可以比較精確地反映出移動機器人多個時刻的運動狀態；②具有較高精度的移動機器人局部位姿估計；③可以和激光雷達對位姿的狀態估計完全解耦，從而不會影響到后期SLAM算法的運行。根據激光雷達運動畸變產生的原因，可以得知，去除激光雷達運動畸變的目標就是要求得移動機器人在運動過程中，當前幀激光數據中每一個激光點所對應的移動機器人的位姿，然后再將所求得的位姿轉換到同一坐標系下，最后再重新封裝成一幀激光數據發布出去。假設已知當前頓激光數據的起始時間分別是T1、Tn,,兩個激光束之間的時間間隔設為04Gmapping算法

3.scan_match

scan_match主要有兩個功能：第一，為每個粒子的位姿找到一個最優值，即利用爬山算法將粒子的位姿作為初值，尋找最優的一個值來對其進行更新；第二，為每個粒子更新權重及累計權重，這里使用scan-to-map的方法對權重進行更新。04Gmapping算法

4.粒子差異性計算04Gmapping算法

5.重采樣到目前為止，新的粒子群是根據提議分布進行采樣的，并且用觀測模型計算權重，而最終目的是用粒子群分布來近似后驗概率分布，故需要進行重采樣，即權重高的位置，粒子數多；權重低的位置，粒子數少。重采樣前后的粒子權重及分布變化如圖6所示，g（表示先驗分布，表示后驗分布。根據上面提到的粒子濾波的原理，運動模型傳播，即對粒子的位姿進行計算，將位姿由g(x）變為烈到，但粒子的分布依然是提議分布，提議分布采樣如圖6所示，下方的黑色豎線表示粒子，豎線高度表示粒子權重，豎線的分布表示粒子的分布狀態，即離散狀態。通過觀測模型（這里是scan-match)進行權重計算后，粒子的權重發生變化，重采樣后的粒子分布如圖6所示。重采樣的目的是用粒子群的分布來近似后驗概率分布，即對每個粒子以其權重的概率來接受該粒子，從而使粒子的分布與權重聯系起來。04Gmapping算法

(1)重采樣示例圖如圖6.13所示，圖6.13中有3個粒子，不同權重用不同灰色區域標識，并進行了編號。黑色虛線將權重之和均分為3份。右側箭頭表示第一次隨機抽到的目標權重，黑色箭頭表示粒子權重的累加值，最先開始位于0位置，即圖6.13中黑色箭頭所示的位置，(2)兩個箭頭都順時針旋轉，黑色箭頭每次增加當前粒子權重的大小。若黑色箭頭超過右側箭頭，則將右側箭頭所在區域的粒子采樣一次，否則不采樣。每次采樣后，右側箭頭向前旋轉粒子權重均值的大小。04Gmapping算法

6.構建占據柵格地圍第一，構建柵格地圖需要使用激光雷達傳感器。第二，激光雷達傳感器是有噪聲存在的，通俗地說，“不一定準”。舉個例子，移動機器人在同一位置下的不同時刻，通過激光雷達對一個固定的障礙物的探測距離不一致，一幀為5m;一幀為5.1m,我們難道要把5m和5.1m的位置都標記為障礙物嗎?這也是使用占據柵格地圖構建算法的原因。為了解決這一問題，引入占據柵格地圖的概念。我們將地圖柵格化，對于每一個柵格的狀態要么占據，要么空閑，要么未知(即初始化狀態)。占據柵格地圖的示意圖如圖04Gmapping算法

對于每一個柵格，我們用P(s=1)來表示空閑狀態的概率，我們用P(s=0)來表示占據狀態的概率，兩者之和為1。用兩個值表示一個柵格的狀態，有點不妥，所以我們引入兩者的比值表示柵格的狀態：04Gmapping算法

04Gmapping算法

我們通過激光雷達數據柵格進行判斷，如果判定柵格是空閑的，就執行式(6-38);如果判定柵格是占據的，就執行式(6-39)。在經過許多幀激光雷達數據的洗禮之后，每一個柵格都存儲了一個值，此時我們可以自己設定闕值與該值比較，來做柵格最終狀態的判定。05圖優化SLAM算法PARTONE05圖優化SLAM算法圖優化SLAM算法概述圖優化SLAM算法最早由Lu和Milios在1997年提出，由于使用標準技術解決誤差最小化問題具有相當高的復雜性，因此在當時并沒有受到重視直到多年后，隨著對SLAM問題結構的深入研究及稀疏線性代數領城的發展，解決優化問題成為了可能。圖優化SLAM算法目前已經在速度和準確性方面屬于最先進的技術之一。圖優化SLAM算法應用所有保存的觀測數據估計移動機器人所有的軌跡，這解決了SLAM問題。圖優化SLAM算法從觀測數據提取一組軟約束，用一個稀疏圖表示，05圖優化SLAM算法圖優化SLAM算法將SLAM問題劃分為前端與后端兩個部分。前端根據傳感器數據來構建圖并計算約束，約束由兩種形式構成；一種是通過相鄰的幾個連續觀測的對應關系得到的；另一種是通過閉環檢測，檢測移動機器人是否回到曾經訪何過的位置從而添加閉環約束的。后端根據前端提供的約束對圖進行優化，它通過將這些約束條件解析為全局一致的估計值來獲得地圖和移動機器人路徑。由于新的觀測加入到已有的位姿圖中時只是考慮最近的幾次觀測，因此位姿圖會逐漸累積誤差，通過全局優化可以將累積誤差去除，得到在給定約束條件下的移動機器人運動軌跡的最優估計。05圖優化SLAM算法圖優化SLAM算法原理圖優化SLAM算法交錯執行前端和后端，圖優化SLAM算法框架如圖6.16所示。前端需要在部分優化的地圖上進行操作，以限制誤差的累積，當前位姿估計越精確，前端生成的約束條件的魯棒性就越強，得到的地圖就越精準。05圖優化SLAM算法這類方法能夠用圖的方式直觀表達，所得的圖稱為位姿圖。在位姿圖中，節點表示移動機器人的位姿，邊由位姿之間的關系組成。圖優化SLAM算法可以描述構建圖和優化圖的過程。(1)構建圖。將移動機器人的位姿作為頂點，位姿間的關系作為邊，這一步通常被稱為前端，一般是傳感器信息的堆積。(2)優化圖。調節移動機器人的位姿頂點，以盡量滿足邊的約束，使得預測與觀測的誤差達到最小，這一步稱為后端。圖優化中圖的示意圖如圖6.17所示，用一個圖來表示SLAM問題，用圖中的節點來表示移動機器人的位姿，用兩個節點之間的邊表示兩個位姿的空間約束(相對位姿關系及對應方差或線性矩陣),邊分為了兩種邊：幀間邊為連接的前后，時間上是連續的；回環邊為連接05圖優化SLAM算法的前后，時間上是不連續的，但是為兩個位姿的空間約束。構建了回環邊才會有誤差出現，沒有回環邊沒有誤差。圖優化的目標是找出最優的各節點位姿的配置，使得誤差最小。05圖優化SLAM算法圖優化SLAM算法的主要思想是；構建非線性最小二乘的目標函數，將移動機器人的位姿與特征點作為待優化變量，利用牛頓法、Leveuberg-Marquardt算法等迭代估計最優解。05圖優化SLAM算法KartoSLAM算法作為第一個以圖優化為基本架構的開源SLAM算法于2010年被提出，該算法主要針對系統稀疏性問題。從KartoSLAM算法出來之后，眾多學者看到了圖優化的開發前景，基于濾波的激光SLAM算法的研究少了許多，KartoSLAM算法在一定程度上替代了基于濾波的激光SLAM算法。雖然系統的稀疏性問題有被考慮到，但是該算法在前端匹配局部子圖的過程中，需要預先創建子圖，所以需要較多的計算時長。如果用全局匹配的算法代替的話，碰到較為空曠的外部環境，需要擴大探測領域進而所需的時間也將增加。05圖優化SLAM算法HectorSLAM算法也是圖優化SLAM算法中的一種，于2011年被提出，該算法僅用激光雷達傳感器就可對周圍環境完成SLAM過程。與Cartographer算法部分過程類似，HectorSLAM算法流程中也可分為前端及后端兩個部分，前端主要用來預測移動機器人的運動，后端對位姿進行優化，但后端缺少了閉環檢測環節。在建圖過程中，首先前端進行激光掃描，獲得柵格地圖。每當激光雷達獲得新的數據時，將其與上一時刻的地圖進行匹配。為使激光雷達數據映射到柵格地圖中，采用雙線性插值的方法來獲得連續的柵格地圖。后端采用高斯牛一頓法對鄰近幀進行匹配，使地圖數據誤差最小，得到優化的地圖。06Cartographer算法PARTONE06Cartographer算法

Cartographer算法是基于激光葡達的開源SLAM算法。該算法的重點分為兩個部分，其中一個是局部子圖在形成過程中，結合了多個不同的傳感器數據，另一個是靈活地將掃描匹配用于閉環檢測中。Cartographer算法前端中的掃描匹配算法是通過將相關掃描匹配及梯度優化進行融合完成的。每當局部閉環之前都會通過數據構建子地圖，生成所有子地圖之后，為了達到減少閉環檢測時間的目的，在全局的閉環檢測中使用分支定界及提前推算網格的方案。Cartographer算法在實現方面有著操作簡便的優點，且主要表現在對硬件，如激光雷達這樣的設備的要求不是很嚴格上。不僅如此，借助ROS這個高效的平臺，可以通過激光雷達在室內完成地圖的創建，進而對周圍環境實現SLAM過程。在室內環境下，該算法相對于其他算法在便捷性上存在一定的優勢，主要體現在激光雷達可以在人工移動的情況下完成建圖。06Cartographer算法

在對周圍環境的適應性方面，因為Cartographer算法在前端部分通過對激光掃描外部環境得到的數據進行處理得到了大量的局部子地圖，所以當需要在較大的室內環境下建圖時，Cartographer算法能夠較好地完成。在計算復雜度方面，該算法在后端部分使用基于圖與圖之間配對的閉環檢測方法，提高了計算速度，進而使工作效率有效提高。基于圖優化的二維激光SLAM特點如表6.2所示。06Cartographer算法

Cartographer算法的理論流程圖如圖6.18所示，該圖將整個SLAM過程大致分為三個部分：激光掃描匹配、閉環檢測及位姿圖優化。在進行大范圍地圖構建時，維護整個地圖的高階協方差矩陣具有非常龐大的計算復雜度，而且距離較遠的特征間的約束不大。因此，將整個環境地圖劃分為一個個的子地圖可以減小計算復雜度，通過構建子地圖及子地圖之間的位姿變換來構成整個環境地圖。當子地圖構建完成后，不會再插入新的掃描，Cartagrepher算法將參與閉環檢測的掃描匹配。06Cartographer算法

激光掃描匹配位姿圖的構建主要通過激光掃描匹配建立位姿節點之間的約束關系。掃描與掃描間的匹配經常用于計算激光SLAM算法中的相對位姿變化，但是，這種方法累積誤差很快。掃描與地圖間的匹配有助于限制這種誤差的累積，然而隨著建圖范圍的增加，這種方法的計算復雜度將顯著提升。掃描與子地圖間的匹配可以得到更好的效果。將每一幀的掃描數據通過澈光掃描匹配插入子地圖中的最佳估計位姿處，由于移動機器人短時間內不會劇烈移動，因此激光掃描匹配是針對最近的子地圖進行的。掃描與子地圖的匹配方式使用非線性優化的方法，將每個連續掃描與子地圖進行匹配，從而得到該掃描在子地圖中的最佳位姿估計。06Cartographer算法

子地圖的構建是反復匹配掃描坐標系與子地圖坐標系的迭代過程。在將掃描插入子地圖之前，激光掃描匹配器負責找到該掃描在子地圖中概率最大的位姿，通過非線性最小二乘優化進行求解得到最優匹配，見式(6-45)。06Cartographer算法

團環檢測由于激光掃描匹配不可能完全準確，會隨著時間的推移逐漸累積誤差，未閉環檢測的位姿圖如圖6.19(a)所示，未環路閉合檢測需要通過閉環約束將累積誤差消除，經過閉環檢測修正后的位姿圖如圖6.19(b)所示。閉環檢測指的是通過傳感器的數據判斷移動機器人是否來到了曾經探索過的地方，用以檢測當前觀測與已經構建好的地圖之間的關系。如果匹配到相同的地方說明環境中存在閉環的結構，可以在位姿圖中添加新的閉環約束。正確的閉環約束可以消除系統中的累積誤差，從而得到一致性更好的位姿圖。06Cartographer算法

子地圖構建完成后，不再有新的掃描被插入，它將參與閉環檢測的掃描匹配，所有完成的子地圖和掃描都會被用來進行閉環檢測。如果檢測到當前掃描與子地圖中某處足夠相似，則將其作為閉環約束添加到優化問題中。每隔幾秒進行一次匹配搜索，以使得在訪問到曾經訪問過的位置時，環路會立即閉合。因此，閉環掃描匹配的頻率必須比掃描更新更快，否則會出現明顯的滯后，可能會導致地圖不一致，通過使用分支定界法(BranchandBoundMethod)來加速這種搜索匹配可以實現在較大的搜索空間中高效地計算匹配位姿。06Cartographer算法

分支定界法在混合整數線性規劃的背景下提出，其主要思想是將可行解的子集表示為樹中的節點，其中根節點表示所有可行解的空間，每個節點的子節點形成它們父節點的一個分支，稱為分支節點，對應著一個可行解的部分集合，是該分支能否成為拓展分支的關鍵。整個分支的最底端被稱為葉節點，它是全部可行解的集合。分支定界法搜索樹的示意圖如圖6.20所示。06Cartographer算法

通常，分支定界法首先將全部可行解的集合不斷地分成越來越小的集合，即分支；之后，為每個小集合計算出一個目標上界或下界，即定界；在進行分支時，那些集合的界限超過了已知可行解界限的將不再進行分支，即剪枝，因為那些集合中不存在比目前最優的可行解更好的解。對于SLAM過程，就是要找到最佳的匹配從而確定閉環，即確定當前掃描與以前構建的子地圖為同一地點的最大概率，因而需要求得上界。解決SLAM閉環問題的分支定界法的過程如下。06Cartographer算法

第一步：根據計算復雜度及上界質量設定邊界，根據最初節點拓展其所有子節點，在當前可行解集合中選擇邊界，作為初始邊界，接下來每步的上界由當前所有可行解集合的最大值決定(定界)。第二步：每個未被拓展的節點有且僅有一次機會成為拓展節點，當其目標函數的值大于當前上界時即可成為拓展節點。對拓展節點進行分支，生成當前拓展節點的所有子節點(分支),在所有子節點中根據邊界舍棄那些不可能產生最優解的節點(剪枝)。06Cartographer算法

圖6.21所示為閉環優化的整體過程。移動機器人從起點開始繞著某房間運動了一圈，當移動機器人再次回到起點時，其路徑中出現了如圖6.21(a)所示閉環優化前的閉環誤差，即在同一位置處，新的激光掃描數據與最初構建的地圖匹配不上。應用分支定界掃描匹配的方式加快了激光掃描數據的匹配過程，閉環檢測能夠每隔幾秒進行一次，使得陰環誤差一經出現就可以被檢測到。圖6.21(b)所示為下一時刻進行了閉環優化后的地圖，可以看到整體地圖在經過閉環優化之后成功地消除了閉環誤差，使得新得到的激光掃描數據與地圖能夠匹配上。06Cartographer算法

位姿圖優化優點(1)它使用了約束中的協方差信息，從而得到更加準確的解。(2)SPA算法對初始化具有很強的魯棒性，對于增量和批量處理都具有非常低的失敗率。(3)因此它只用Levenberg-Marquardt算法進行較少次數的迭代，這使它的收斂速度非常快：(4)SPA算法是完全非線性的。在每一次迭代中，它將當前姿態周圍的所有約束線性化06Cartographer算法

06Cartographer算法

SPA算法過程如下(1)首先初始化輸入的值，若為0,則應用上一次迭代儲存的值。由于傳感器的距離限制，移動機器人只能觀測到附近一定距離的特征，因此，位姿圖呈現出非常稀疏的連按結構。該算法針對位姿圖的稀疏性，通過只儲存和處理非0元素可以大福度降低儲存空間及計算復雜度。非0元素要通過壓縮存儲數據結構來儲存，因此需要將位姿圖節點c和約束e創建出的稀疏上三角矩陣H轉換成這種格式。(2)建立線性系統式(6-50),通過求解該式得到增量△x與△e。(3)根據式(6-51)更新位姿，得到更新后的位姿。(4)如果誤差變小了，那么返回更新后的位姿，并把λ的值縮小一半，并保存。(5)如果誤差變大了，那么返回原始位姿，并把λ擴大一倍，并保存。(6)SPA算法的輸入為位姿c、約束e及對角線增量A,輸出為更新后的位姿c。該算法的每次迭代是為了尋找一個合適的對角線增量λ,當λ很小時，SPA算法就變成了牛頓-歐拉法的最優步長計算式，當很大時，SPA算法就蛻化為梯度下降法的最優步長計算式。07激光SLAM的案例分析PARTONE07激光SLAM的案例分析工業控制計算機工業控制計算機當移動機器人剛剛進入未知環境時，由于沒有關于環境的先驗信息，移動機器人無法知道自身所處的位置及其周圍的環境情況。首先需要獲取移動機器人自身所攜帶的里程計及激光雷達數據；然后，采用粒子濾波算法，使用里程計及激光雷達數據實現移動機器人的定位。在確定了移動機器人的位姿之后，基于該位姿，構建移動機器人的周圍環境地圖，從而解決SLAM問題，需要特別說明的是，環境地圖構建包含不同形式的地圖，具體使用哪類地圖，需要根據移動機器人所要完成的具體任務而定。07激光SLAM的案例分析1.里程計運動模型傳感器模型里程計是移動機器人進行相對定位的有效傳感器，可以說是許多移動機器人平臺的標配。其主要工作原理是檢測安裝在移動機器人的輪子上的光電編碼器對輪子在單位時間內轉過的弧度，從而推算出移動機器人的相對運動。由于移動機器人在二維地面上運動，所以其位姿可以通過二維坐標和方向角來描述，里程計運動模型如圖6.22所示。07激光SLAM的案例分析里程計的運動模型可以分為直線型與圓弧型，考慮到移動機器人在進行自主探索時，常常伴隨著轉動，所以針對里程計運動模型，決定采用圓弧型，其表現形式如圖622所示。下面給出基于圓弧型的里程計運動模型的具體形式：07激光SLAM的案例分析本書中使用的激光雷達是思嵐科技所生產的2D激光雷達RPLDAR-A1,如圖6.23所示。其采用激光三角測距技術，能夠進行每秒8000次以上的測距動作，從而能夠讓移動機器人更快、更精確地構建地圖。此外，該激光雷達可以實現360°的全方位掃描測距檢測，從而獲得移動機器人周圍環境的輪廓圖，并且該雷達的測意半徑最大可達12m,掃描頻率可通過輸送給電機的PWM信號進行控制，最高可達10Hz。2.激光雷達觀測模型07激光SLAM的案例分析基于粒子濾波的移動機器人定位1.狀態變置的概率分布傳感器模型一節給出了SLAM模型運動方程的具體形式，如下所示。假設上述的噪聲服從零均值高斯分布，即Wk～N(0,R)。從而，我們可以得到運動模型的概率分布形式。07激光SLAM的