移動機器人SLAM技術(shù) 課件 【ch06】激光SLAM

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

04Gmapping算法

04Gmapping算法

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

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

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

3.scan_match

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

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

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

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

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

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

04Gmapping算法

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

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

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

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

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

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

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

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

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

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

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

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

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

06Cartographer算法

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