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

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

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

ROS的基礎(chǔ)框架ROS的總體設(shè)計(jì)一個(gè)目標(biāo)?提高機(jī)器人研發(fā)中的軟件復(fù)用率五個(gè)特點(diǎn)?點(diǎn)對(duì)點(diǎn)設(shè)計(jì)

?多語言支持

?架構(gòu)精簡、集成度高

?組件化工具包豐富

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

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

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

（1）初始化階段

（2）轉(zhuǎn)移階段

（3）決策階段

（4）重采樣階段

（5）濾波

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

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

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

視覺與慣性導(dǎo)航融合SLAM和深度學(xué)習(xí)技術(shù)結(jié)合?；诔瑢拵В╱ltrawideband,UWB）的定位技術(shù)基于線/面特征的

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

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

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

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

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

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

和為狀態(tài)向量；為控制向量，包含平移速度和旋轉(zhuǎn)速度隨機(jī)變量是一個(gè)高斯隨機(jī)向量，表示狀態(tài)轉(zhuǎn)移的不確定性預(yù)測機(jī)器人運(yùn)動(dòng)時(shí)，狀態(tài)矢量根據(jù)標(biāo)準(zhǔn)帶噪聲速度模型的變化如下

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

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

為在時(shí)刻探測到的第個(gè)地標(biāo)的坐標(biāo)，為它的簽名

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

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

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

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

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

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

，是在機(jī)器人位姿對(duì)特征檢測到的概率，是對(duì)特征檢測的似然函數(shù)，是PHD隨機(jī)有限集的雜波強(qiáng)度。對(duì)進(jìn)行集合積分：在時(shí)刻，可用一組粒子集合來表示其密度，經(jīng)機(jī)器人位姿、特征PHD預(yù)測，特征PHD更新，機(jī)器人位姿更新等環(huán)節(jié)，實(shí)現(xiàn)PHD-SLAMGM-PHDSLAM方法步驟：（1）每個(gè)粒子的GM-PHD特征地圖1）預(yù)測通過時(shí)刻的觀測數(shù)據(jù)，使用逆空間測量模型計(jì)算新生高斯均值和方差，并平均分配權(quán)重為

，，分別表示新生高斯的權(quán)重，均值和協(xié)方差，為觀測的方差每個(gè)粒子的新生GM-PHD：

時(shí)刻的地圖PHD是高斯混合的形式：最終得：2）修正將機(jī)器人先前的位姿估計(jì)轉(zhuǎn)化為新位姿的預(yù)測樣本：在新的觀測中，權(quán)重會(huì)因漏檢率而減少，于是有：在PHD校正中引入計(jì)算因子：更新GM-PHD被檢測部分的權(quán)重：將漏檢和加權(quán)預(yù)測的高斯分量合并成高斯量，得到更新后的高斯PHD：選擇高斯權(quán)重較高的分量，刪除剩余的高斯分量。最后，可以得到基于高斯加權(quán)的GM-PHD地圖：（2）機(jī)器人軌跡更新根據(jù)機(jī)器人的控制輸入，預(yù)測時(shí)刻機(jī)器人的位姿：根據(jù)觀測信息計(jì)算新的權(quán)重，并歸一化：根據(jù)規(guī)劃權(quán)重對(duì)所有個(gè)粒子重采樣：最后得出粒子的后驗(yàn)集和對(duì)應(yīng)的地圖PHD：PHD-SLAM算法流程圖SLAM算法簡介與實(shí)現(xiàn)第二章目錄CONTENTS01SLAM算法簡介02基于矢量的SLAM經(jīng)典算法03基于隨機(jī)有限集的SLAM算法01SLAM算法簡介1.SLAM算法分類基于矢量的SLAM算法：應(yīng)用最早和最廣泛的算法，它將每一時(shí)刻機(jī)器人的位姿信息和地圖特征都表示成地圖、軌跡聯(lián)合狀態(tài)矢量(即向量序列)的形式，并通過遞歸公式對(duì)地圖、軌跡聯(lián)合狀態(tài)后驗(yàn)概率密度進(jìn)行遞歸估計(jì)?；跀U(kuò)展卡爾曼濾波（ExtendedKalmanFilter,EKF）的EKF-SLAM算法和基于序貫蒙特卡羅的FastSLAM算法都屬于比較典型的基于矢量的SLAM算法?；陔S機(jī)有限集的SLAM算法：這類算法與傳統(tǒng)的算法相比，最大的不同之處是以隨機(jī)有限集的形式來代替矢量表示。RFS是集合形式的隨機(jī)變量，其元素的數(shù)量和順序都是隨機(jī)的。利用RFS理論對(duì)機(jī)器人SLAM問題進(jìn)行建模，即用隨機(jī)有限集合而不是矢量形式來表示每一時(shí)刻的地圖特征狀態(tài)和觀測信息，這使得多特征－多觀測狀態(tài)通過這種方式得到更加有效的表達(dá)。2.1基于矢量的SLAM算法特點(diǎn)機(jī)器人采用條件概率表示置信度，置信度是以獲得數(shù)據(jù)為條件的關(guān)于狀態(tài)變量的后驗(yàn)概率，用

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

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

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

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

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

和為狀態(tài)向量；為控制向量，包含平移速度和旋轉(zhuǎn)速度隨機(jī)變量是一個(gè)高斯隨機(jī)向量，表示狀態(tài)轉(zhuǎn)移的不確定性預(yù)測機(jī)器人運(yùn)動(dòng)時(shí)，狀態(tài)矢量根據(jù)標(biāo)準(zhǔn)帶噪聲速度模型的變化如下

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

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

為在時(shí)刻探測到的第個(gè)地標(biāo)的坐標(biāo)，為它的簽名

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

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

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

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

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

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

，是在機(jī)器人位姿對(duì)特征檢測到的概率，是對(duì)特征檢測的似然函數(shù)，是PHD隨機(jī)有限集的雜波強(qiáng)度。對(duì)進(jìn)行集合積分：在時(shí)刻，可用一組粒子集合來表示其密度，經(jīng)機(jī)器人位姿、特征PHD預(yù)測，特征PHD更新，機(jī)器人位姿更新等環(huán)節(jié)，實(shí)現(xiàn)PHD-SLAMGM-PHDSLAM方法步驟：（1）每個(gè)粒子的GM-PHD特征地圖1）預(yù)測通過時(shí)刻的觀測數(shù)據(jù)，使用逆空間測量模型計(jì)算新生高斯均值和方差，并平均分配權(quán)重為

，，分別表示新生高斯的權(quán)重，均值和協(xié)方差，為觀測的方差每個(gè)粒子的新生GM-PHD：

時(shí)刻的地圖PHD是高斯混合的形式：最終得：2）修正將機(jī)器人先前的位姿估計(jì)轉(zhuǎn)化為新位姿的預(yù)測樣本：在新的觀測中，權(quán)重會(huì)因漏檢率而減少，于是有：在PHD校正中引入計(jì)算因子：更新GM-PHD被檢測部分的權(quán)重：將漏檢和加權(quán)預(yù)測的高斯分量合并成高斯量，得到更新后的高斯PHD：選擇高斯權(quán)重較高的分量，刪除剩余的高斯分量。最后，可以得到基于高斯加權(quán)的GM-PHD地圖：（2）機(jī)器人軌跡更新根據(jù)機(jī)器人的控制輸入，預(yù)測時(shí)刻機(jī)器人的位姿：根據(jù)觀測信息計(jì)算新的權(quán)重，并歸一化：根據(jù)規(guī)劃權(quán)重對(duì)所有個(gè)粒子重采樣：最后得出粒子的后驗(yàn)集和對(duì)應(yīng)的地圖PHD：PHD-SLAM算法流程圖基于ROS系統(tǒng)的SLAM技術(shù)第三章目錄CONTENTS01ROS系統(tǒng)02基于ROS系統(tǒng)的機(jī)器人實(shí)踐03基于ROS系統(tǒng)的SLAM開源方案01ROS系統(tǒng)ROS從某種意義上說是一個(gè)操作系統(tǒng)（OS），它能夠提供類似操作系統(tǒng)所能提供的功能，包括硬件抽象描述、底層啟動(dòng)管理、公共功能的執(zhí)行、程序間消息的傳遞以及程序發(fā)行包管理，它也能提供工具程序和庫，用于獲取、建立、編寫和運(yùn)行多機(jī)整合的程序。ROS的首要設(shè)計(jì)目標(biāo)是在機(jī)器人研發(fā)領(lǐng)域提高代碼的復(fù)用率。ROS系統(tǒng)的應(yīng)用1.ROS的起源2.ROS的五個(gè)特點(diǎn)（1）點(diǎn)對(duì)點(diǎn)設(shè)計(jì)節(jié)點(diǎn)單元分布式網(wǎng)絡(luò)RPC+TCP/UDP通信系統(tǒng)（2）多語言支持支持Python、C++、Java等編程語言語言無關(guān)的接口定義（3）精簡與集成每個(gè)功能節(jié)點(diǎn)可以單獨(dú)編譯集成眾多開源項(xiàng)目接口統(tǒng)一、提高軟件復(fù)用性（4）工具包豐富3D可視化工具—rviz物理仿真環(huán)境—gazebo數(shù)據(jù)記錄工具—rosbagQT工具箱—rqt_*rvizrqt_plot（5）免費(fèi)且開源BSD許可，可修改、可復(fù)用、可商用軟件包數(shù)量指數(shù)級(jí)增加，良好的生態(tài)系統(tǒng)3.ROS文件系統(tǒng)級(jí)文件系統(tǒng)級(jí)是用于描述可以在硬盤上查找到的代碼及可執(zhí)行文件程序，在這一級(jí)中將使用一組概念來解釋ROS的內(nèi)部組成、文件架構(gòu)以及工作所需的核心文件。ROS文件系統(tǒng)主要概念（1）功能包（package）：功能包是ROS軟件組織的基本形式，是構(gòu)成ROS的基本單元。一個(gè)功能包可以包含多個(gè)可執(zhí)行文件（節(jié)點(diǎn)）。（2）功能包清單（packagemanifest）：記錄功能包的基本信息。（3）綜合功能包（Metapackage）：也稱元功能包，將幾個(gè)具有某些功能的功能包組織在一起。（4）綜合功能包清單（Metapackagemanifest）：會(huì)包含運(yùn)行時(shí)所需要依賴的功能包或者聲明一些引用的標(biāo)簽。（5）消息類型（.msg）：消息是ROS節(jié)點(diǎn)之間發(fā)布/訂閱的通信消息。（6）服務(wù)類型（.srv）：服務(wù)類型定義了ROS服務(wù)器/客戶端通信模型下的請求與應(yīng)答數(shù)據(jù)類型。文件系統(tǒng)級(jí)4.ROS計(jì)算圖級(jí)計(jì)算圖級(jí)體現(xiàn)的是進(jìn)程與系統(tǒng)之間的通信，描述程序是如何運(yùn)行的。ROS會(huì)創(chuàng)建一個(gè)連接所有進(jìn)程的網(wǎng)絡(luò)，子系統(tǒng)中的任何節(jié)點(diǎn)都可以訪問此網(wǎng)絡(luò)，并通過該網(wǎng)絡(luò)與其他節(jié)點(diǎn)交互，獲取其他節(jié)點(diǎn)發(fā)布的消息，并將自身的數(shù)據(jù)發(fā)布到網(wǎng)絡(luò)上。計(jì)算圖級(jí)（1）節(jié)點(diǎn)（Node）:執(zhí)行運(yùn)算任務(wù)的進(jìn)程。在ROS系統(tǒng)中，節(jié)點(diǎn)是最小的進(jìn)程單元。一個(gè)軟件包里面可以有多個(gè)可執(zhí)行文件，可執(zhí)行文件在運(yùn)行之后就成了一個(gè)進(jìn)程（這個(gè)進(jìn)程便是一個(gè)節(jié)點(diǎn)）。（2）節(jié)點(diǎn)管理器(NodeMaster):master在整個(gè)網(wǎng)絡(luò)通信架構(gòu)里相當(dāng)于管理中心，管理著各個(gè)node。node首先在master處進(jìn)行注冊，之后master會(huì)將該node納入整個(gè)ROS程序中。（3）消息（Message）:節(jié)點(diǎn)之間最重要的通信機(jī)制就是基于發(fā)布/訂閱模型的消息通信。消息包含一個(gè)節(jié)點(diǎn)發(fā)送到其他節(jié)點(diǎn)的數(shù)據(jù)信息，節(jié)點(diǎn)是通過消息實(shí)現(xiàn)彼此的邏輯聯(lián)系與數(shù)據(jù)交換。（4）主題（Topic）:單向異步通信機(jī)制，傳輸消息(Message)。在這種機(jī)制中，Message以一種發(fā)布/訂閱的方式進(jìn)行傳遞(Publish-Subscribe).每個(gè)Topic的消息類型都是強(qiáng)類型，發(fā)布到其上的Message都必須與Topic的ROS消息類型匹配，而且節(jié)點(diǎn)只能接收類型匹配的消息。單向異步通信機(jī)制（5）服務(wù)（Service）:雙向同步通信機(jī)制，ROS中稱其為“服務(wù)”，傳輸請求/應(yīng)答數(shù)據(jù)，是一個(gè)request-reply模型.與主題不同的是，ROS中只允許有一個(gè)節(jié)點(diǎn)提供指定命名的服務(wù)。這種機(jī)制不僅可以發(fā)送消息，還存在反饋。雙向同步通信機(jī)制（6）參數(shù)服務(wù)器（ParameterServer）:參數(shù)服務(wù)器能夠是數(shù)據(jù)通過關(guān)鍵詞存儲(chǔ)在一個(gè)系統(tǒng)的核心位置（節(jié)點(diǎn)管理器）。通過使用參數(shù)就能夠在運(yùn)行時(shí)配置節(jié)點(diǎn)或改變節(jié)點(diǎn)的工作任務(wù)。（7）消息記錄包（Bag）:用于記錄和回放ROS消息數(shù)據(jù)的文件格式，保存在.bag文件中。Bag是一種用于存儲(chǔ)數(shù)據(jù)的重要機(jī)制，它可以獲取并且記錄各種難以收集的傳感器數(shù)據(jù)，程序可以通過Bag反復(fù)獲取實(shí)驗(yàn)數(shù)據(jù)，常用于調(diào)試算法。rosparam支持參數(shù)6.ROS開源社區(qū)級(jí)（1）發(fā)行版（Distribution）：ROS發(fā)行版包括一系列帶有版本號(hào)、可以直接安裝的功能包。（2）軟件源（Repository）：ROS依賴于共享網(wǎng)絡(luò)上的開源代碼，不同的組織機(jī)構(gòu)可以開發(fā)或者共享自己的機(jī)器人軟件。（3）ROS維基（ROSwiki）：記錄ROS信息文檔的主要論壇。（4）郵件列表（Mailinglist）：交流ROS更新的主要渠道，同時(shí)也可以交流ROS開發(fā)的各種疑問。（5）ROS問答（ROSAnswer）：咨詢ROS相關(guān)問題的網(wǎng)站。（6）博客（Blog）：發(fā)布ROS社區(qū)中的新聞、圖片、視頻等，網(wǎng)址是/news。ROS開源社區(qū)ROS系統(tǒng)實(shí)現(xiàn)三個(gè)層次02基于ROS系統(tǒng)的機(jī)器人實(shí)踐

TurtleBot第一代發(fā)布于2010年，兩年后發(fā)布了第二代產(chǎn)品。前兩代TurtleBot都使用iRobot的機(jī)器人作為底盤，在底盤上可以裝載激光雷達(dá)、Kinect等傳感器，使用PC搭載基于ROS的控制系統(tǒng)。

TurtleBot3徹底顛覆了原有TurtleBot的外形設(shè)計(jì)，成本進(jìn)一步降低，而且模塊化更強(qiáng)，可以根據(jù)開發(fā)者的需求自由改裝。TurtleBot2主要結(jié)構(gòu)示意圖在本書中，主要介紹的版本是TurtleBot2，TurtleBot2主要構(gòu)成如圖所示，其主要包含三個(gè)部分，Kobuki底盤、主機(jī)以及傳感器Kinect。Kobuki底盤：Kobuki是Yujin公司開發(fā)的移動(dòng)機(jī)器人底盤，機(jī)器人接口控制板有32個(gè)內(nèi)置傳感器，兩個(gè)驅(qū)動(dòng)輪，2個(gè)從動(dòng)輪共四個(gè)可移動(dòng)小輪，110度/秒單軸陀螺儀，一對(duì)編碼器，一個(gè)可以擴(kuò)大的輸入輸出端口和一個(gè)后擋板。開放式接口可以直接實(shí)現(xiàn)對(duì)機(jī)器人的移動(dòng)、聲音、顯示以及輸入傳感器的操作。Kobuki底盤

最初TurtleBot2所帶的傳感器是微軟在2009年發(fā)布的kinectV1深度傳感器在2014年微軟公司發(fā)布了Kinect的二代產(chǎn)品?，F(xiàn)在的TurtleBot2所配備的傳感器Kinect2.0。

kinect1Kinect2Kinect1，彩色攝像頭為640*480，深度攝像頭為320*240，可以同時(shí)檢測到2名用戶的姿勢可以檢測到人體20個(gè)關(guān)節(jié)點(diǎn)檢測范圍為0.8m-4.0m角度（水平57°垂直43°）Kinect2，彩色攝像頭為1920*1080，深度攝像頭為512*424，可以同時(shí)檢測到6名用戶的姿勢可以檢測到人體25個(gè)關(guān)節(jié)點(diǎn)檢測范圍為0.5m-4.5m角度（水平70°垂直60°）03基于ROS系統(tǒng)的SLAM開源方案基于ROS的開源SLAM方案就非常多，總共可以分為兩個(gè)大類，分別是基于激光雷達(dá)的開源方案以及基于視覺的開源方案。1.基于激光雷達(dá)的SLAM算法（1）HectorSLAMHectorSLAM是一種結(jié)合了魯棒性較好的掃描匹配（scanmatching）算法的2D激光SLAM方法，同時(shí)在該方案中使用了慣性傳感系統(tǒng)的導(dǎo)航技術(shù)。要求：高更新頻率、低測量噪聲的激光掃描儀，不需要里程計(jì)特點(diǎn)：內(nèi)存中有不同的地圖，同時(shí)使用之前估計(jì)的姿勢來更新它們，計(jì)算成本很低，且地圖總是保持一致。HectorSLAM多分辨率地圖（2）Gmapping這是一種基于Rao-Blackwellized的粒子濾波SLAM方法?；诹Ｗ訛V波的算法用許多加權(quán)粒子表示路徑的后驗(yàn)概率，每個(gè)粒子都給出一個(gè)重要性因子。特點(diǎn)：●通常需要大量的粒子才能獲得比較好的的結(jié)果，從而增加該算法的計(jì)算復(fù)雜性。

●與PF重采樣過程相關(guān)的粒子退化耗盡問題降低了算法的準(zhǔn)確性。30粒子80粒子Gmapping基于不同粒子數(shù)的地圖（3）CartographerCartographer是谷歌推出的一套基于圖優(yōu)化的SLAM算法，該算法以柵格的形式來建立地圖，并且將局部匹配直接表示成一個(gè)非線性優(yōu)化問題，后端用圖來優(yōu)化。特點(diǎn)：

●目標(biāo)是降低計(jì)算資源消耗、實(shí)時(shí)優(yōu)化并且不追求高精度。●整個(gè)算法的核心代碼依賴很少Cartographer地圖2.基于視覺的SLAM算法MonoSLAM以擴(kuò)展卡爾曼濾波為后端，追蹤前端非常稀疏的特征點(diǎn)。

●第一個(gè)實(shí)時(shí)的視覺SLAM系統(tǒng)

●應(yīng)用場景窄，路標(biāo)數(shù)量有限，稀疏特征點(diǎn)非常容易丟失PTMA實(shí)現(xiàn)了跟蹤與建圖過程的并行化，跟蹤部分需要實(shí)時(shí)響應(yīng)圖像數(shù)據(jù)，而對(duì)地圖的優(yōu)化則沒必要實(shí)時(shí)的計(jì)算。也就意味著后端優(yōu)化可以在后端進(jìn)行，然后在必要的時(shí)候進(jìn)行線程同步即可。 ●首次將前后端的概念區(qū)分出來；引入了關(guān)鍵幀機(jī)制 ●應(yīng)用場景小，跟蹤容易丟失ORB-SLAM代表著特征點(diǎn)SLAM的一個(gè)高峰，支持單目、雙目、RGB-D三種模式，整個(gè)系統(tǒng)圍繞ORB特征進(jìn)行計(jì)算，包括視覺里程計(jì)與回環(huán)檢測的ORB字典?！馩RB回環(huán)檢測可以有效的降低算法運(yùn)行過程中的累積誤差，并且在丟失后可以迅速找回●創(chuàng)新式的使用了三個(gè)線程完成SLAM

基于特征估計(jì)的激光SLAM技術(shù)第四章目錄CONTENTS01rviz和Gazebo工具02激光雷達(dá)傳感器03基于激光的Gmapping算法04基于激光的HectorSLAM算法05基于激光的Cartographer算法01rviz和Gazebo工具

機(jī)器人系統(tǒng)中存在大量數(shù)據(jù)，這些數(shù)據(jù)在計(jì)算過程中往往都處于數(shù)據(jù)形態(tài)，比如圖像數(shù)據(jù)中0~255的RGB值。但是這種數(shù)據(jù)形態(tài)的值往往不利于開發(fā)者去感受數(shù)據(jù)所描述的內(nèi)容，所以常常需要將數(shù)據(jù)可視化顯示，例如機(jī)器人模型的可視化、圖像數(shù)據(jù)的可視化、地圖數(shù)據(jù)的可視化等。rviz是ROS可視化（ROSvisualization）的縮寫，是ROS中強(qiáng)有力的3D可視化工具。它使得用戶能夠查看模擬機(jī)器人模型、來自機(jī)器人傳感器的傳感器日志信息，并且重放已記錄的傳感器信息。用戶能夠?qū)C(jī)器人應(yīng)用程序進(jìn)行調(diào)試，調(diào)試內(nèi)容涵蓋從傳感器輸入到計(jì)劃動(dòng)作(或計(jì)劃外動(dòng)作)的整個(gè)過程。rvizrviz主界面劃分為以下幾個(gè)主要的顯示區(qū)域:1.中心窗口2.

Displays控制面板3.工具欄4.Views控制面板5.Time控制面板6.主窗口菜單欄。常見的display類型類型描述消息類型Axes顯示坐標(biāo)系－Gamera從相機(jī)視角顯示圖像sensor_msgs/Imagesensor_msgs/CameraInfoGrid顯示網(wǎng)格－Image顯示圖像sensor_msgs/ImageLaserScan顯示激光雷達(dá)數(shù)據(jù)sensor_msgs/LaserScanImage顯示圖像sensor_msgs/ImagePointClode2顯示點(diǎn)云數(shù)據(jù)sensor_msgs/PointCloud2Odomerty顯示里程計(jì)數(shù)據(jù)nav_msgs/OdometryPointClode2顯示點(diǎn)云數(shù)據(jù)sensor_msgs/PointCloud2RobotModel顯示機(jī)器人模型－PointClode2顯示TF樹－rviz的視角1.Orbital軌道攝像機(jī)視角，圍繞一個(gè)焦點(diǎn)旋轉(zhuǎn)，Ｌ:單擊并拖動(dòng)圍繞焦點(diǎn)旋轉(zhuǎn)Ｍ:在相機(jī)向上和向右向量形成平面，按下中鍵并拖動(dòng)焦點(diǎn)R:單擊拖動(dòng)鼠標(biāo)放大/縮小焦點(diǎn)向上拖動(dòng)放大，向下拖動(dòng)縮小滾輪:放大縮小焦點(diǎn)2.FPS第一視角FPVＬ:單擊選中對(duì)象，并拖動(dòng)旋轉(zhuǎn)Ｍ:移動(dòng)沿著相機(jī)向上、向右向量形成的平面移動(dòng)R:右擊并拖動(dòng)，前后移動(dòng)，上拖向前移動(dòng)，下拖向后移動(dòng)滾輪:前后移動(dòng)3.top-downOrthographic自上而下的正視圖Ｌ:單擊并拖動(dòng)繞Ｚ軸旋轉(zhuǎn)Ｍ:在ＸＹ平面移動(dòng)R:右擊并拖動(dòng)，縮放圖像滾輪:縮放圖像4.ＸＹOrthographic限制在XY平面GazeboGazebo是一款3D動(dòng)態(tài)模擬器，能夠在復(fù)雜的室內(nèi)和室外環(huán)境中準(zhǔn)確有效地模擬機(jī)器人群。與游戲引擎提供高保真度的視覺模擬類似，Gazebo提供高保真度的物理模擬，其提供一整套傳感器模型，以及對(duì)用戶和程序非常友好的交互方式。1.Gazebo的典型用途●測試機(jī)器人算法●設(shè)計(jì)機(jī)器人●用現(xiàn)實(shí)場景進(jìn)行回歸測試2.Gazebo的一些主要特點(diǎn)●包含多個(gè)物理引擎●包含豐富的機(jī)器人模型和環(huán)境庫●包含各種各樣的傳感器●程序設(shè)計(jì)方便和具有簡單的圖形界面3.系統(tǒng)要求Gazebo官方建議，Gazebo目前最好在Ubuntu或者其他的Linux發(fā)行版上運(yùn)行。同時(shí)您的計(jì)算機(jī)需要具有以下功能：●專用GPU：Nvidia卡往往在Ubuntu中運(yùn)行良好●至少是IntelI5或同等產(chǎn)品的CPU●至少500MB的可用磁盤空間●安裝盡可能高版本的UbuntuTrusty02激光雷達(dá)傳感器激光雷達(dá)探測原理激光雷達(dá)（Lidar）類似于雷達(dá)（Radar），但是分辨率更高，因?yàn)榧す獾牟ㄩL大約比無線電的波長小10萬倍?？梢杂脕韰^(qū)分真實(shí)移動(dòng)中的行人和人物海報(bào)、在三維立體空間中建模、檢測靜態(tài)物體、精確測距。激光雷達(dá)是通過發(fā)射激光束來探測目標(biāo)位置、速度等特征量的雷達(dá)系統(tǒng)，具有測量精度高、方向性好等優(yōu)點(diǎn)。隼鳥2號(hào)LiDAR結(jié)合光學(xué)相機(jī)影像獲取龍宮全球三維地形信息iPadPro探測點(diǎn)云示意圖激光雷達(dá)優(yōu)點(diǎn)●具有極高的分辨率●抗干擾能力強(qiáng)●獲取的信息量豐富●可全天時(shí)工作激光雷達(dá)缺點(diǎn)●工作時(shí)受天氣和大氣影響大。激光一般在晴朗的天氣里衰減較小，傳播距離較遠(yuǎn)。而在大雨、濃煙、濃霧等壞天氣里，衰減急劇加大，傳播距離大受影響?！窦す饫走_(dá)的波束極窄,在空間搜索目標(biāo)非常困難，直接影響對(duì)非合作目標(biāo)的截獲概率和探測效率,只能在較小的范圍內(nèi)搜索、捕獲目標(biāo)，激光雷達(dá)RPLIDARA1高度60毫米直徑98.5毫米重量170克測量半徑范圍0.15-12米掃描測距角度360度測量頻率最大可達(dá)每秒8000次掃描頻率5.5赫茲。激光雷達(dá)RPLIDARA2高度41毫米直徑190毫米重量76克測量半徑范圍0.15-18米掃描測距角度360度測量頻率最大可達(dá)每秒8000次掃描頻率15赫茲。激光雷達(dá)RPLIDARA1/2是思嵐科技（SLAMTEC）有限公司的產(chǎn)品,能夠準(zhǔn)確測量試場中物體輪廓邊沿與設(shè)備間的相對(duì)距離，可快速獲得環(huán)境輪廓信息，這些輪廓信息組成所謂的點(diǎn)云并繪制出3D環(huán)境地圖，可以幫助機(jī)器人實(shí)現(xiàn)自主構(gòu)建地圖、實(shí)時(shí)路徑規(guī)劃與自動(dòng)避開障礙物。03基于激光的Gmapping算法Gmapping是一種基于Rao-Blackwellized粒子濾波器并適用于網(wǎng)格地圖的SLAM算法，也是一種有效解決同時(shí)定位和建圖的算法，它將定位和建圖分離，并且每一個(gè)粒子都攜帶一幅地圖。

Gmapping算法流程圖重要性重采樣（SamplingImportanceResampling,SIR）濾波器

Gmapping算法的實(shí)現(xiàn)步驟1）第i個(gè)粒子在k時(shí)刻的預(yù)測位姿可以由k-1

時(shí)刻的位姿和控制輸入通過運(yùn)動(dòng)模型得出。2）機(jī)器人基于地圖

，從預(yù)測的位姿處執(zhí)行掃描匹配算法。掃描匹配執(zhí)行的搜索區(qū)域被限定在附近的一個(gè)有限區(qū)域內(nèi)。如果掃描匹配失敗，則根據(jù)運(yùn)動(dòng)模型計(jì)算位姿和權(quán)重（忽略步驟3和4）。3）通過掃描匹配器在位姿周圍選取一組采樣點(diǎn)，其中是掃描匹配器通過將當(dāng)前觀測值與已構(gòu)建的地圖進(jìn)行匹配，找到最有可能的位姿?；谶@些點(diǎn)，通過逐點(diǎn)評(píng)估采樣位姿中的目標(biāo)分布來計(jì)算建議的平均值和協(xié)方差矩陣。在此階段計(jì)算加權(quán)系數(shù)。4）根據(jù)改進(jìn)的建議分布得出粒子i

的新位姿，位姿服從高斯分布的形式。5）更新粒子權(quán)重。6）根據(jù)機(jī)器人位姿和最新觀測，更新粒子i的地圖。7）得到新粒子樣本后，根據(jù)閾值執(zhí)行重采樣步驟。04基于激光的HectorSLAM算法HectorSLAM使用一種快速在線學(xué)習(xí)的占用柵格地圖系統(tǒng)，它是結(jié)合了激光雷達(dá)系統(tǒng)和基于慣性傳感的三維姿態(tài)估計(jì)系統(tǒng)的魯棒掃描匹配方法。通過使用快速近似的地圖梯度和多分辨率柵格，實(shí)現(xiàn)了可靠的定位和建圖。HectorSLAM功能包使用高斯牛頓的方法，不需要里程計(jì)數(shù)據(jù)，只根據(jù)激光信息便可構(gòu)建地圖。

SLAM一般分為前端和后端系統(tǒng)。SLAM前端用于實(shí)時(shí)在線估計(jì)機(jī)器人運(yùn)動(dòng)，后端用于對(duì)位姿圖進(jìn)行優(yōu)化。前端快速掃描匹配步驟用于位姿估計(jì)，而較慢的后端建圖步驟在后臺(tái)或遠(yuǎn)程計(jì)算機(jī)上運(yùn)行。HectorSLAM方法主要關(guān)注前端系統(tǒng)，沒有提供相應(yīng)的位姿圖優(yōu)化，主要是由于這種優(yōu)化在真實(shí)環(huán)境下建圖是不需要的，因?yàn)镠ectorSLAM方法對(duì)于機(jī)器人執(zhí)行任務(wù)來說已經(jīng)足夠精確。HectorSLAM算法原理HectorSLAM系統(tǒng)需要用到6自由度運(yùn)動(dòng)的平臺(tái)，這不同于其它2D網(wǎng)格SLAM算法所假設(shè)的3自由度運(yùn)動(dòng)，因此系統(tǒng)必須估計(jì)由平臺(tái)的平移和旋轉(zhuǎn)組成的全6自由度狀態(tài)。為此，該系統(tǒng)由兩個(gè)主要組件組成，導(dǎo)航濾波器子系統(tǒng)融合來自慣性測量單元和其他可用傳感器的信息，形成一致的3D解決方案，而2DSLAM子系統(tǒng)用于提供地平面內(nèi)的位姿和航向信息。這兩種估計(jì)都是單獨(dú)更新的，并且只是松散耦合的，因此它們隨著時(shí)間的推移保持同步。Hectorslam算法的實(shí)現(xiàn)步驟（1）在2DSLAM中，為了能夠表示任意環(huán)境，HectorSLAM使用占用柵格地圖，占用柵格地圖的離散特性限制了地圖表達(dá)精度，也不允許直接計(jì)算插值或?qū)?shù)。給定一個(gè)連續(xù)的地圖坐標(biāo)，占用值以及梯度可以如圖用四個(gè)近鄰點(diǎn)來近似，沿著x軸和y軸進(jìn)行線性插值，然后得到:導(dǎo)數(shù)可以近似為（2）掃描匹配是將激光掃描相互對(duì)齊或與現(xiàn)有地圖對(duì)齊的過程，HectorSLAM實(shí)質(zhì)上是一個(gè)將掃描末端點(diǎn)與已知地圖對(duì)準(zhǔn)優(yōu)化方法,其基本思路是使用高斯-牛頓法。通過解最小二乘函數(shù)，使得激光掃描與地圖有最佳的對(duì)齊：代表機(jī)器人的位姿，代表i號(hào)激光束在機(jī)器人姿態(tài)下掃描點(diǎn)的世界坐標(biāo)，其坐標(biāo)變化為：根據(jù)優(yōu)化測量誤差來實(shí)現(xiàn)對(duì)的估計(jì)：對(duì)做一階泰勒展開得：對(duì)上式求偏導(dǎo)并設(shè)置為，可得上述最小化問題的高斯牛頓方程：其中（3）HectorSLAM使用擴(kuò)展卡爾曼濾波器(EKF)。在掃描時(shí)的卡爾曼估計(jì)可以用均值和協(xié)方差來表示，位姿估計(jì)

直接得出，融合結(jié)果如下：觀測器矩陣C將整個(gè)狀態(tài)空間投影到SLAM系統(tǒng)的三維子空間中，參數(shù)用來調(diào)整SLAM更新的效果。于是，進(jìn)一步有：05基于激光的Cartographer算法基于激光的SLAM方法中，掃描--掃描匹配（Scan-to-scanmatching）經(jīng)常用于計(jì)算相對(duì)姿態(tài)變化。然而，掃描--掃描匹配本身會(huì)很快積累誤差。掃描--地圖匹配（Scan-to-mapmatching）有助于限制這種誤差積累，可使用高斯-牛頓方法在線性插值圖上尋找局部最優(yōu)解。當(dāng)高精度頻率激光雷達(dá)提供良好的位姿初始估計(jì)時(shí)，局部優(yōu)化的掃描--地圖匹配是有效和魯棒的。

像素精度的掃描匹配方法，可以進(jìn)一步減少了局部誤差積累。解決剩余局部誤差累積的兩種常見方法是粒子濾波和基于圖形的SLAM。Cartographer是谷歌推出的一套基于圖優(yōu)化的SLAM算法。Cartographer的設(shè)計(jì)目的是在計(jì)算資源有限的情況下，實(shí)時(shí)獲取相對(duì)較高精度的2D地圖?？紤]到基于模擬策略的粒子濾波方法在較大環(huán)境下對(duì)內(nèi)存和計(jì)算資源的需求較高，Cartographer采用基于圖優(yōu)化方法。Cartographer是一個(gè)實(shí)時(shí)的室內(nèi)建圖算法，能生成分辨率的柵格地圖。在前端將最新的激光雷達(dá)掃描數(shù)據(jù)在相鄰的子圖上(整個(gè)地圖的一小塊)完成掃描匹配，得到一個(gè)在短時(shí)間內(nèi)準(zhǔn)確的最佳插入位置(位姿)后，將掃描插入到子圖中。掃描匹配中，位姿估計(jì)的誤差會(huì)在整個(gè)地圖中隨時(shí)間逐漸累積，在后端中,通過回環(huán)檢測加約束進(jìn)行優(yōu)化消除誤差。Cartographer算法框架圖Cartographer算法原理Cartographer系統(tǒng)將獨(dú)立的局部和全局方法綜合來實(shí)現(xiàn)二維的定位與地圖構(gòu)建。在局部方法中，每個(gè)連續(xù)的掃描都與世界坐標(biāo)系上的一小塊區(qū)域（稱為子圖）進(jìn)行匹配，使用非線性優(yōu)化將掃描與子圖對(duì)齊，此過程被稱為掃描匹配。子圖的構(gòu)建就是將掃描結(jié)果不斷與子圖的坐標(biāo)系對(duì)齊的迭代過程。將掃描原點(diǎn)設(shè)為，雷達(dá)掃描點(diǎn)記為，。變換表示的是位姿位于掃描幀中的位置轉(zhuǎn)換到子圖幀中的位置，于是，對(duì)于一個(gè)雷達(dá)掃描點(diǎn)，轉(zhuǎn)換到子圖幀中可以用下式表示：連續(xù)幀掃描可以用來構(gòu)建一個(gè)子圖，這些子圖采用概率網(wǎng)格

:的形式，r是網(wǎng)格地圖的分辨率網(wǎng)格點(diǎn)和相關(guān)像素

網(wǎng)格點(diǎn)所對(duì)應(yīng)計(jì)算的值代表存在障礙物的概率。對(duì)于每個(gè)網(wǎng)格點(diǎn)定義相應(yīng)的像素，它是由離網(wǎng)格點(diǎn)最近的所有點(diǎn)組成的。每一個(gè)像素是尺寸為的方格，它代表對(duì)應(yīng)范圍內(nèi)的所有點(diǎn)，掃描數(shù)據(jù)點(diǎn)是實(shí)數(shù)，概率網(wǎng)格的尺寸是整數(shù)。每當(dāng)掃描要插入到概率網(wǎng)格地圖時(shí)，都要確定命中（hits）的點(diǎn)和未命中（misses）的點(diǎn)集。命中就是網(wǎng)格里有雷達(dá)數(shù)據(jù)點(diǎn)，未命中就是沒雷達(dá)數(shù)據(jù)點(diǎn)。每一個(gè)Hit的點(diǎn)（陰影且畫的），對(duì)應(yīng)的最近鄰格子點(diǎn)加入到命中點(diǎn)集合；每一個(gè)Miss的點(diǎn)（陰影），雷達(dá)原點(diǎn)到掃描點(diǎn)中間的格子點(diǎn)全部加到未命中點(diǎn)集合，如果這個(gè)點(diǎn)已經(jīng)在命中點(diǎn)集中則排除。與hit和miss相關(guān)的掃描和像素每個(gè)還未觀察到的網(wǎng)格，可以自定義賦值為一個(gè)或者值（一般用0.5，即不知道這個(gè)網(wǎng)格有沒有被占用）。已經(jīng)觀察到的網(wǎng)格，它的概率更新為：每個(gè)網(wǎng)格的命中概率更新公式為：一幀掃描插入子圖之前，首先要對(duì)這幀掃描的位姿進(jìn)行優(yōu)化。Cartographer使用的是基于Ceres庫的掃描匹配。該優(yōu)化問題可以描述成一個(gè)非線性最小二乘問題：與掃描匹配一樣，環(huán)路閉合優(yōu)化也可描述為一個(gè)非線性最小二乘問題，每間隔幾秒，就用Ceres庫來計(jì)算出一個(gè)解：殘差的計(jì)算：當(dāng)掃描匹配將不正確的約束添加到優(yōu)化問題時(shí)，損失函數(shù)（例如Huberloss）可以用于減少異常值的影響。使用基于分支定界（Branch-and-bound）掃描匹配方法，得到精確到像素級(jí)的最優(yōu)位姿估計(jì)，核心公式如下：分支定界算法的基本步驟分支定界法是求解整數(shù)規(guī)劃問題的最常用算法，這種方法不但可以求解純整數(shù)規(guī)劃，還可以求解混合整數(shù)規(guī)劃問題。分支定界法采用搜索與迭代的方法，選擇不同的分支變量和子問題進(jìn)行分支?；谝曈X的SLAM技術(shù)第五章目錄CONTENTS01經(jīng)典視覺SLAM框架02MonoSLAM算法03ORB-SLAM204多機(jī)器人視覺SLAM技術(shù)01經(jīng)典視覺SLAM框架視覺SLAM框架前端：VO后端：Optimization回環(huán)檢測Loop

Closing建圖Mapping視覺傳感器1.單目相機(jī)模型某一相機(jī)拍攝時(shí)，若外部所有可視景物光線只能通過相機(jī)的光心到達(dá)相機(jī)的成像平面并形成倒立縮小的成像平面點(diǎn)，這種相機(jī)就屬于針孔模型相機(jī)幾何關(guān)系:2.雙目相機(jī)模型雙目相機(jī)一般是由左右兩個(gè)單目相機(jī)組成，兩個(gè)相機(jī)水平放置，當(dāng)然也還有其他放置方式，比如上下兩目，本質(zhì)上沒有什么區(qū)別。雙目相機(jī)雙目相機(jī)成像原理幾何關(guān)系:3.深度相機(jī)模型深度相機(jī)能夠主動(dòng)測量每個(gè)像素的深度信息紅外結(jié)構(gòu)光來測量像素距離（Kinect1代相機(jī)）通過飛行時(shí)間法原理來測量像素距離（Kinect2代相機(jī)）●在基于紅外結(jié)構(gòu)光的原理中，相機(jī)主要是根據(jù)返回的結(jié)構(gòu)光的圖案來計(jì)算物體與相機(jī)之間的距離?！窕陲w行時(shí)間法原理的相機(jī)主要是通過向目標(biāo)發(fā)射脈沖光，然后根據(jù)發(fā)送到返回之間光束的飛行時(shí)間來確定物體和相機(jī)之間的距離的。視覺里程計(jì)VisualOdometry相鄰圖像估計(jì)相機(jī)運(yùn)動(dòng)基本形式：通過兩張圖像計(jì)算運(yùn)動(dòng)和結(jié)構(gòu)不可避免地有漂移方法●特征點(diǎn)法

●直接法后端優(yōu)化?濾波?BA圖優(yōu)化（視覺主流方法）1.BA代價(jià)函數(shù)與圖優(yōu)化（1）1.投影模型第一步，把世界坐標(biāo)轉(zhuǎn)換到相機(jī)坐標(biāo)系：第二步，將投至歸一化平面，得到歸一化坐標(biāo)：第三步，考慮歸一化坐標(biāo)的徑向畸變情況，可以用一個(gè)多項(xiàng)式函數(shù)來描述畸變前后的坐標(biāo)變化：第四步，根據(jù)內(nèi)參模型，計(jì)算像素坐標(biāo)：（2）BA優(yōu)化函數(shù)投影的整個(gè)過程看似有些復(fù)雜，可以將整個(gè)過程概括為一個(gè)觀測方程，可以把它抽象成：關(guān)于觀測的誤差：整體的代價(jià)函數(shù)可以定義為：2.BA求解把自變量定義成所有待優(yōu)化的變量：給自變量一個(gè)增量時(shí)，目標(biāo)函數(shù)變?yōu)椋喊芽臻g點(diǎn)的變量也放在一起，把相機(jī)位姿變量放到一起：公式可以簡化表達(dá)：3.位姿圖優(yōu)化相機(jī)的位姿以及位姿節(jié)點(diǎn)之間相對(duì)運(yùn)動(dòng)的估計(jì):按照李群的寫法來表示:構(gòu)建誤差:將擾動(dòng)項(xiàng)移至兩側(cè)，根據(jù)伴隨性質(zhì)，可以得到：稍加整理可以得到：求出誤差關(guān)于兩個(gè)位姿的雅可比矩陣：取近似：總體目標(biāo)函數(shù)為：將擾動(dòng)項(xiàng)移至最后，導(dǎo)出右乘形式的雅可比矩陣：回環(huán)檢測檢測機(jī)器人是否回到早先位置識(shí)別到達(dá)過的場景計(jì)算圖像間的相似性方法：詞袋模型建圖用于導(dǎo)航、規(guī)劃、通訊、可視化、交互等度量地圖vs拓?fù)涞貓D稀疏地圖vs稠密地圖02MonoSLAM算法MonoSLAM是由AndrewJ.Davison等人在2007年提出的一種在未知場景中恢復(fù)單目相機(jī)3D移動(dòng)軌跡的實(shí)時(shí)算法。該方法的核心是在概率框架內(nèi)在線創(chuàng)建一個(gè)稀疏持久的地圖路標(biāo)。?采用一種主動(dòng)的地圖構(gòu)建與測量方法?使用一般的平滑相機(jī)運(yùn)動(dòng)模型來捕捉視頻流中固有的動(dòng)力學(xué)先驗(yàn)信息?徹底解決單目特征初始化問題。MonoSLAM系統(tǒng)圖MonoSLAM算法1、概率三維地圖MonoSLAM是基于概率特征的建圖，地圖中的信息代表相機(jī)狀態(tài)和所有感興趣的特征的當(dāng)前估計(jì)，還代表這些估計(jì)中的不確定性。地圖可以狀態(tài)向量和協(xié)方差矩陣表示：相機(jī)的狀態(tài)向量：2、自然視覺路標(biāo)●MonoSLAM的目標(biāo)是在相機(jī)可能劇烈運(yùn)動(dòng)下能夠重復(fù)識(shí)別一樣的視覺地標(biāo)●假定每一個(gè)特征都在一個(gè)局部平面上●在初始化階段，認(rèn)為局部平面的法向與特征至相機(jī)矢量平行3、系統(tǒng)初始化在單目相機(jī)SLAM算法中，初始化時(shí)通常在相機(jī)前面放置一個(gè)已知物體作為先驗(yàn)信息●在單目相機(jī)SLAM中，沒有直接的方法來測量特征深度或里程信息●初始化時(shí)，已知特征信息可有助于直接進(jìn)入預(yù)測、測量、更新的模式4