機器人SLAM技術及其ROS系統(tǒng)應用 第2版 課件 第1、2章 緒論、SLAM算法簡介與實現(xiàn)

第一章緒論目錄CONTENTS01SLAM算介02SLAM數(shù)學模型03SLAM分類04ROS簡介05基于ROS系統(tǒng)的SLAM技術06SLAM發(fā)展前景01SLAM簡介SLAM（SimultaneousLocalizationAndMapping）中文稱“同步定位及建圖”是目前在機器人定位方面的熱門研究課題。到目前為止，也取得了不少進展。SLAM問題可以描述為:機器人在未知環(huán)境中從一個未知位置開始移動,在移動過程中根據(jù)位置估計和傳感器數(shù)據(jù)進行自身定位,同時建造增量式地圖一般來說，機器人自主移動到指定位置的過程可以分解為這三個任務:（1）路徑規(guī)劃（2）定位（3）建圖。而SLAM的目的就是解決“定位”與“地圖構建”這兩個問題定位(localization):機器人必須知道自己在環(huán)境中位置。建圖(mapping):機器人必須記錄環(huán)境中特征的位置（如果知道自己的位置）02SLAM數(shù)學模型

由于噪聲的存在，將SLAM問題數(shù)學建模成一個狀態(tài)估計問題，通過帶有噪聲的觀測數(shù)據(jù)，估計狀態(tài)變量。03SLAM分類SLAM算法輸出為一張地圖，地圖可分為度量圖，拓撲圖，混合圖或語義圖。度量圖可以將環(huán)境表示為柵格地圖，特征地圖或幾何信息地圖。地圖度量地圖柵格地圖特征地圖幾何信息地圖拓撲地圖混合地圖柵格地圖表示法即將整個環(huán)境分為若干相同大小的柵格，對于每個柵格各指出其中是否存在障礙物。柵格地圖●可以持續(xù)地獲取環(huán)境信息特征地圖通過指定的標記點、特征等精確度量位置來表示環(huán)境。幾何信息地圖表示法是指機器人收集對環(huán)境的感知信息，從中提取更為抽象的幾何特征，例如線段或曲線，使用這些幾何信息描述環(huán)境。特征地圖幾何信息地圖●直觀形象，便于導航●對其傳感器要求較高●相對數(shù)據(jù)存儲量和運算量比較小拓撲地圖抽象度高，特別在環(huán)境大而簡單時。這種方法將環(huán)境表示為一張拓撲意義中的圖(graph)，圖中的節(jié)點對應于環(huán)境中的一個特征狀態(tài)、地點。如果節(jié)點間存在直接連接的路徑則相當于圖中連接節(jié)點的弧。●緊湊，需要的存儲空間小●傳感器信息模糊很難構建大環(huán)境下的地圖，也可能產(chǎn)生的路徑不是最佳路徑拓撲地圖混合地圖包含地圖的拓撲和度量信息。例如，一些度量圖通過拓撲關系連接起來。混合地圖中的每個節(jié)點可以是小型度量地圖，或者是某個地方的定性信息，也可兩者都有。●根據(jù)用途不同可以訪問不同級別的信息●典型如語義地圖實現(xiàn)人與機器人在概念上就能交互混合地圖04ROS簡介

ROS是面向機器人的開源的元操作系統(tǒng)（meta-operatingsystem）。它能夠提供類似傳統(tǒng)操作系統(tǒng)的諸多功能，如硬件抽象、底層設備控制、常用功能實現(xiàn)、進程間消息傳遞和程序包管理等。此外，它還提供相關工具和庫，用于獲取、編譯、編輯代碼以及在多個計算機之間運行程序完成分布式計算。

ROS的基礎框架ROS的總體設計一個目標?提高機器人研發(fā)中的軟件復用率五個特點?點對點設計

?多語言支持

?架構精簡、集成度高

?組件化工具包豐富

?免費并且開源四位一體?ROS=通訊機制+開發(fā)工具+應用功能+

生態(tài)系統(tǒng)05

基于ROS系統(tǒng)的SLAM技術基于激光的SLAM技術最底層機器人本身的電機驅(qū)動和控制部分中間通信層通信通路決策層例Gmapping包1、基于激光的SLAM技術Gmapping包是在ROS里對開源社區(qū)openslam下Gmapping算法的C++實現(xiàn)，該算法采用一種高效的Rao-Blackwellized粒子濾波將收取到的激光測距數(shù)據(jù)最終轉換為柵格地圖。粒子濾波其核心思想是通過從后驗概率（觀測方程）中抽取的隨機狀態(tài)粒子來表達其分布，是一種循序重要性采樣法。粒子濾波主要步驟如下：

（1）初始化階段

（2）轉移階段

（3）決策階段

（4）重采樣階段

（5）濾波

（6）地圖生成2、基于視覺的SLAM技術

根據(jù)相機工作方式的不同，可分為以下三種:單目只能反映三維場景的二維圖像雙目通過左右眼圖像的差異來判斷場景中物體的遠近，能從直接提取完整的特征數(shù)據(jù)。RGB-D可同時獲取圖像彩色信息和深度信息。

視覺SLAM經(jīng)典方法的比較激光SLAM與視覺SLAM的優(yōu)劣勢06SLAM發(fā)展前景

視覺與慣性導航融合SLAM和深度學習技術結合。基于超寬帶（ultrawideband,UWB）的定位技術基于線/面特征的

SLAM動態(tài)場景下的SLAM多機器人的SLAM。。。SLAM算法簡介與實現(xiàn)第二章目錄CONTENTS01SLAM算法簡介02基于矢量的SLAM經(jīng)典算法03基于隨機有限集的SLAM算法01SLAM算法簡介1.SLAM算法分類基于矢量的SLAM算法：應用最早和最廣泛的算法，它將每一時刻機器人的位姿信息和地圖特征都表示成地圖、軌跡聯(lián)合狀態(tài)矢量(即向量序列)的形式，并通過遞歸公式對地圖、軌跡聯(lián)合狀態(tài)后驗概率密度進行遞歸估計?；跀U展卡爾曼濾波（ExtendedKalmanFilter,EKF）的EKF-SLAM算法和基于序貫蒙特卡羅的FastSLAM算法都屬于比較典型的基于矢量的SLAM算法?；陔S機有限集的SLAM算法：這類算法與傳統(tǒng)的算法相比，最大的不同之處是以隨機有限集的形式來代替矢量表示。RFS是集合形式的隨機變量，其元素的數(shù)量和順序都是隨機的。利用RFS理論對機器人SLAM問題進行建模，即用隨機有限集合而不是矢量形式來表示每一時刻的地圖特征狀態(tài)和觀測信息，這使得多特征－多觀測狀態(tài)通過這種方式得到更加有效的表達。2.1基于矢量的SLAM算法特點機器人采用條件概率表示置信度，置信度是以獲得數(shù)據(jù)為條件的關于狀態(tài)變量的后驗概率，用

表示在執(zhí)行完后，進行觀測之前所計算的后驗概率密度表示為：該概率也常被稱為預測。是基于以前狀態(tài)的后驗，在綜合時刻的觀測之前，預測了時刻的狀態(tài)。EKF優(yōu)勢：●既簡單又高效的計算速率缺陷：●計算復雜度過高。其復雜度與環(huán)境中路標（特征）數(shù)量的平方成正比

●存在數(shù)據(jù)關聯(lián)問題無法處理虛警、漏檢和觀測的不確定性等問題。FastSLAM優(yōu)勢：●使用個獨立的EKF濾波器完成地圖特征的估計，其計算復雜度降低為

●適合大范圍場景中的應用缺陷：●必須解決新的特征觀測和地圖中已有特征之間的數(shù)據(jù)關聯(lián)問題2.2基于隨機有限集的SLAM算法

隨機有限集理論擴大了隨機變量統(tǒng)計理論的應用范圍，實現(xiàn)了隨機變量統(tǒng)計理論到隨機集統(tǒng)計理論的推廣。用RFS處理SLAM問題，也就是將地圖特征和傳感器觀測信息都表示成隨機有限集合的形式。優(yōu)點：●避免數(shù)據(jù)關聯(lián)●避免特征管理●誤差定量分析科學3.1貝葉斯濾波算法第一步為預測，通過基于狀態(tài)的置信度和控制來計算狀態(tài)的先驗置信度第二步為更新，用已經(jīng)觀測到的觀測的概率乘以預測置信度。利用歸一化常數(shù)對結果進行歸一化，最后推導出最后的后驗置信度基本的貝葉斯濾波算法先驗：全概率公式：后驗：貝葉斯公式：1：AlgorithmKalman_filter:2:3:4:5:6:7:return1：AlgorithmExtended_Kalman_filter:2:3:4:5:6:7:returnKF算法EKF算法3.2卡爾曼濾波算法3.3粒子濾波算法1：AlgorithmParticle_filter:2:3:fortodo4:sample5:6:7:endfor8:fortodo9:drawwithprobability10:addto11:endfor12:return用粒子集來表示置信度由粒子集遞歸地構建粒子集02基于矢量的SLAM經(jīng)典算法1.EKF-SLAM算法基本原理EKF-SLAM算法估計機器人位姿的同時，還對估計路徑中所遇到的所有地標點的坐標進行估計。將包含機器人位姿和地圖的狀態(tài)矢量稱為聯(lián)合狀態(tài)矢量，并將該矢量定義為。用均值和方差來描述機器人在時刻的聯(lián)合狀態(tài)，該聯(lián)合狀態(tài)矢量由下式給出：該狀態(tài)矢量是維的，式中、、為機器人在時刻的位姿坐標；和為第個地標的坐標，；為該地標的簽名。

EKF-SLAM算法流程圖（1）預測機器人從時刻到時刻的狀態(tài)變化可由非線性函數(shù)來描述：

和為狀態(tài)向量；為控制向量，包含平移速度和旋轉速度隨機變量是一個高斯隨機向量，表示狀態(tài)轉移的不確定性預測機器人運動時，狀態(tài)矢量根據(jù)標準帶噪聲速度模型的變化如下

為時刻到時刻的變化量，、、為機器人在時刻的位姿坐標；為控制誤差的協(xié)方差，表示服從均值為0，方差為

的高斯分布。其中將函數(shù)在機器人時刻的狀態(tài)后驗均值處一階泰勒展開可以得出在時刻機器人預測的聯(lián)合狀態(tài)的均值和協(xié)方差（2）觀測更新階段觀測模型：

為在時刻探測到的第個地標的坐標，為它的簽名

為地標相對機器人的距離；為地標相對機器人的角度；為地標簽名；為觀測誤差的協(xié)方差如果路標是第一次觀測到，可表示為之前被觀測到，那么時刻對路標的觀測數(shù)據(jù)可以表示為：

是機器人在時刻的預測位姿；、為路標的坐標，為的地標簽名。在預測的狀態(tài)處一階泰勒展開，得：相應的卡爾曼增益為：機器人位姿和地圖特征的聯(lián)合狀態(tài)矢量進行更新：2.FastSLAM算法基本原理粒子濾波器（ParticleFilter）用離散的隨機采樣點（粒子）來表征狀態(tài)向量的后驗概率密度函數(shù)，它對于非線性非高斯系統(tǒng)的估計問題具有普遍適用性。FastSLAM算法使用粒子濾波器估計路徑的后驗，使用EKF估計地圖特征的位置。因為因式分解，F(xiàn)astSLAM算法能為每個特征維持獨立的EKF。每個獨立的EKF是以機器人路徑為條件的，每個粒子擁有它自己的EKF集合，地圖里的一個特征對應一個EKF。FastSLAM算法中的粒子可以表示為：

為粒子的索引；為機器人的路徑估計；和為第個粒子的第個特征位置的均值和協(xié)方差。FastSLAM1.0算法流程圖（1）通過采樣新位姿擴展路徑后驗第個粒子對應的位姿為：（2）更新觀察到的特征估計特征更新后的值與新位姿一起被增加到臨時的粒子集合中對于沒有觀測到的特征，其后驗保持不變：對于觀測到的特征，使用貝葉斯準則更新：FastSLAM算法以EKF-SLAM同樣的方法線性化感知模型新的均值和協(xié)方差可以用EKF測量更新得到：重復步驟（1）、（2）次，產(chǎn)生個粒子的臨時集合。（3）重采樣臨時集合的粒子建議分布服從如下分布：重采樣過程導致了目標分布和建議分布的不同，重采樣重要性系數(shù)由目標分布和建議分布的商給出：進一步變換。在變換時，忽略與傳感器測量值不相關的變量：FastSLAM算法采用與步驟（2）使用的測量更新完全相同的線性近似，重要性系數(shù)如下所示：FastSLAM2.0在很大程度上與FastSLAM1.0類似，但在對位姿采樣時，建議分布考慮測量。03基于隨機有限集的SLAM算法隨機有限集最早起源于隨機集理論，主要是指有限集統(tǒng)計(finitesetstatistics，F(xiàn)ISST)理論。隨機集是指取值為集合的隨機元，是概率論中隨機變量(或隨機向量)概念的推廣，實際上就是元素及其個數(shù)都是隨機變量的集合。隨機變量處理的是隨機點值函數(shù)，而隨機集處理的是隨機集值函數(shù)。1.隨機有限集隨機集理論是點變量(向量)統(tǒng)計學向“集合變量”統(tǒng)計學的一種推廣。該理論能夠解決復雜環(huán)境下信息融合、多目標跟蹤的各種問題，是目前信息融合和多目標跟蹤研究領域最受關注的方向之一。利用隨機有限集理論，可以將多目標問題中的探測、跟蹤、屬性識別等問題統(tǒng)一起來，并能解決多目標狀態(tài)的后驗估計、多目標信息融合算法的性能評估等棘手問題。2.基于隨機有限集的SLAM（1）基于RFS的SLAM模型表示用RFS的形式來表示每一時刻的地圖特征狀態(tài)和觀測信息：

表示時刻地圖中特征的數(shù)量，表示地圖中的第個特征或路標把傳感器的觀測信息以RFS的形式進行表示：

表示機器人已知的位姿，表示傳感器對地圖特征進行觀測產(chǎn)生的RFS，表示虛警的RFS（2）基于RFS的SLAM問題描述在SLAM中都以RFS的形式表示，，，聯(lián)合估計的后驗概率密度可表示為在貝葉斯下的遞推過程如下所示：●根據(jù)之前的機器人的位姿和控制輸入量進行預測：●根據(jù)當前觀測的測量集合進行更新：

是一組集合（2.1）（2.2）3.PHD-SLAM算法基本原理處理基于集合估計的一個近似方法是利用概率假設密度（probabilityhypothesisdensity,PHD）。PHD濾波器跟蹤整體特征的建圖，并在進行新測量時嘗試檢測和跟蹤單個特征。利用標準條件概率，可將式(2.1)和式(2.2)分解：強度函數(shù)PHD的一步預測可表示為：PHD的更新可表示為：

，是在機器人位姿對特征檢測到的概率，