移動機器人原理與技術 課件 第六章 移動機器人同時定位與建圖

移動機器人技術原理與應用第六章

移動機器人同時定位與建圖基于濾波的移動機器人SLAM原理基于濾波的移動機器人SLAM濾波算法框架6.1基于濾波的移動機器人SLAM簡介6.26.3基于濾波的移動機器人及環(huán)境的模型6.4移動機器人卡爾曼濾波SLAM算法移動機器人粒子濾波SLAM算法6.56.6基于圖優(yōu)化的移動機器人SLAM算法6.76.1基于濾波的移動機器人SLAM簡介SLAM主要研究在對機器人位姿和其環(huán)境信息都不具備先驗知識的情況下，如何應用合理的表征方法對環(huán)境建模（也就是構建地圖）并同時確定移動機器人自身位姿。在應用隨機概率方法解決同時定位與地圖創(chuàng)建問題時，首先要建立概率表示的移動機器人運動模型和觀測模型，用狀態(tài)向量表示移動機器人自身的位姿和環(huán)境特征的位置，運用濾波技術對移動機器人的位姿和環(huán)境特征進行概率估計。6.1基于濾波的移動機器人SLAM簡介機器人所有的可能位姿和環(huán)境特征保持概率分布，隨著機器人的運動，觀測到新的環(huán)境數據，概率分布被更新，從而減少機器人位姿估計和環(huán)境特征估計的不確定性。高斯分布是最常用的概率分布，在假設機器人位姿和環(huán)境特征服從高斯分布的情況下，SLAM問題轉化為狀態(tài)向量的高斯均值和方差估計問題。實際的移動機器人系統(tǒng)是非線性非高斯的，對于非線性非高斯的狀態(tài)估計問題，粒子濾波器（ParticlFilter）更具有普遍適用性。6.2基于濾波的移動機器人SLAM原理6.2.1移動機器人SLAM系統(tǒng)狀態(tài)在移動機器人實際運動過程中，控制量uk在k-1時刻作用于機器人，使機器人在k時刻到達狀態(tài)Xk，Xk表示k時刻移動機器人位姿(位置和姿態(tài))。在移動機器人運動起始的0時刻和k-1時刻間的移動機器人狀態(tài)序列記為X0:k-1=(X0,X1,....,Xk-1)，在0時刻和k-1時刻間的控制量序列可表示為u0:k-1=(u0,u1,....,uk-1)。環(huán)境中景物的外觀特征可作為環(huán)境狀態(tài)信息，考慮到存儲空間及運算能力，實際問題中只提取景物的關鍵特征作為環(huán)境狀態(tài)信息，某時刻對應的6.2.1移動機器人SLAM系統(tǒng)狀態(tài)環(huán)境狀態(tài)記為M={m1,m2,...,mn}，其在與地圖中已有的特征匹配后加入地圖，系統(tǒng)會逐漸建立起增量地圖。移動機器人與環(huán)境構成了移動機器人系統(tǒng)，系統(tǒng)狀態(tài)用[XkM]T表示，即系統(tǒng)狀態(tài)包含移動機器人自身的狀態(tài)和移動機器人環(huán)境中景物的特征。SLAM問題描述為：在環(huán)境信息M未知，移動機器人初始位姿X0已知，輸入控制量uk給定的前提下，創(chuàng)建地圖(由環(huán)境信息構成)的同時確定機器人的狀態(tài)(即位姿)Xk。6.2.1移動機器人SLAM系統(tǒng)狀態(tài)環(huán)境狀態(tài)記為M={m1,m2,...,mn}，其在與地圖中已有的特征匹配后加入地圖，系統(tǒng)會逐漸建立起增量地圖。移動機器人與環(huán)境構成了移動機器人系統(tǒng)，系統(tǒng)狀態(tài)用[XkM]T表示，即系統(tǒng)狀態(tài)包含移動機器人自身的狀態(tài)和移動機器人環(huán)境中景物的特征。SLAM問題描述為：在環(huán)境信息M未知，移動機器人初始位姿X0已知，輸入控制量uk給定的前提下，創(chuàng)建地圖(由環(huán)境信息構成)的同時確定機器人的狀態(tài)(即位姿)Xk。6.2.1移動機器人SLAM系統(tǒng)狀態(tài)對于運動在平面上的移動機器人，其在根據uk而進行的運動中，應用內部傳感器即本體感受器，根據里程計進行位置預測，也稱為估計；同時根據系統(tǒng)觀測模型進行該位置上的觀測特征預測；應用外部傳感器對環(huán)境進行測量，獲得觀測特征，將該觀測特征與預測觀測特征進行匹配，匹配成功的觀測特征對移動機器人的位置估計進行修正，得到Xk；同時，匹配成功的特征信息構建地圖，即得到M。6.2.1移動機器人SLAM系統(tǒng)狀態(tài)SLAM通用架構6.2.2移動機器人狀態(tài)的概率描述移動機器人本身狀態(tài)Xk與移動機器人系統(tǒng)在k時刻之前發(fā)生的所有事件是概率相關的。k時刻的移動機器人狀態(tài)Xk的出現是基于所有先前時刻的移動機器人狀態(tài)X0:k-1、控制量u0:k-1和觀測量Z1:k-1。因此，移動機器人Xk可表示為如下概率形式：由馬爾可夫法則，Xk-1可完整地表征Z1:k-1和u0:k-1，若Xk-1已經計算得到，則Xk只與uk和Xk-1有關。根據概率學的條件獨立原理，下式成立：6.2.2移動機器人狀態(tài)的概率描述

稱為移動機器人的狀態(tài)傳遞概率，表示k時刻移動機器人的起始狀態(tài)為Xk-1，在輸入控制量uk后到達狀態(tài)Xk的概率。實際應用系統(tǒng)中，一般用下式函數來表征該狀態(tài)傳遞概率：上式稱為機器人運動模型或狀態(tài)傳遞模型，其中，f(·)稱為狀態(tài)傳遞函數，一般為非線性函數；ωk為用來表示建模誤差或控制量噪聲的加性噪聲。6.2.3移動機器人傳感器觀測的概率表示移動機器人通過自身傳感器得到的位置狀態(tài)是不準確的，需要借助如攝像頭、激光、雷達、超聲波等外部傳感器的觀測值Zk來修正移動機器人系統(tǒng)的狀態(tài)估計值，從而使移動機器人系統(tǒng)的狀態(tài)盡量接近實際值。對于從1時刻到k時刻時間段內的傳感器觀測表示為Z1:k{Z1,Z2,...,Zk},觀測Zk表述為如下概率形式：k時刻觀測的Zk完全可由Xk和M推測。

稱為觀測概率，表示k時刻在系統(tǒng)狀態(tài)Xk時所能觀測到的Zk的似然概率。6.2.3移動機器人傳感器觀測的概率表示實際應用系統(tǒng)中，一般下式函數來表示：h(·)稱為觀測模型，vk為傳感器本身存在或測量方式引起的噪聲。移動機器人系統(tǒng)是一個動態(tài)隨機系統(tǒng)，由移動機器人狀態(tài)傳遞概率和傳感器觀測概率共同描述，對應的時間生成模型又稱為隱馬爾可夫模型或者動態(tài)貝葉斯網絡。6.3基于濾波的移動機器人SLAM濾波算法框架6.3.1貝葉斯濾波的時間更新貝葉斯法則是貝葉斯濾波推導的基礎，根據概率問題中的邊緣概率法則，時刻系統(tǒng)的后驗概率可分解為：由條件概率的鏈式法則，有下式成立：由于移動機器人的運動是馬爾可夫的，Xk僅與Xk-1和uk有關，所以有：6.3.1貝葉斯濾波的時間更新又因為Xk-1與k時刻的控制量uk無關，則由此當已知k-1時刻的聯合后驗概率

和k時刻的控制量uk，得到：上式即為貝葉斯濾波的時間更新過程。6.3.2貝葉斯濾波的測量更新若傳感器在k時刻的觀測量為Zk，由貝葉斯法則可知下式成立：按馬爾可夫假設，觀測量條件Zk獨立于過去的測量Z1:k-1和控制量uk，有下式成立：6.3.2貝葉斯濾波的測量更新

與X無關，用η表示，稱為正則化參數上式(描述了貝葉斯濾波的測量更新過程。6.4基于濾波的移動機器人及環(huán)境的模型6.4.1移動機器人運動模型1．基于里程計的運動模型移動機器人位姿變化的大小通過安裝在其驅動車輪上的光電編碼器來推算，根據編碼器碼盤輸出的脈沖個數、碼盤轉數和車輪半徑，計算出驅動車輪在給定時間內轉過的弧度，進而計算出驅動車輪運動的距離。設Δt時間內光碼盤輸出的脈沖數為N，光碼盤為p線/轉，驅動車輪半徑為r，則該驅動車輪運動的距離Δd為：6.4.1移動機器人運動模型1．基于里程計的運動模型對于具有兩個驅動輪的輪式移動機器人，假設按照上式計算出的左右兩個驅動車輪的移動距離分別為ΔdL和ΔdR，移動機器人從狀態(tài)移動到，則移動機器人的移動距離表示為，移動機器人轉動的角度為，L為移動機器人車輪間距。6.4.1移動機器人運動模型1．基于里程計的運動模型移動機器人運動模型若由移動機器人移動距離和轉動的角度表示移動機器人的輸入控制變量移動機器人的運動半徑可表述為：6.4.1移動機器人運動模型1．基于里程計的運動模型在移動機器人運動中，基于里程計的運動模型同時考慮了移動機器人移動的距離變化和轉動的方向角變化，更有效地逼近移動機器人運動的實際軌跡?；诶锍逃嫷囊苿訖C器人運動模型可描述為：在時間Δt內，機器人路徑為弧長ΔDk，偏轉角度為Δφk。6.4.1移動機器人運動模型2．基于控制命令的運動模型當可以得到移動機器人運動的線速度和偏轉角時，由線速度和偏轉角組成輸入控制變量的移動機器人運動模型表示為:6.4.2傳感器觀測模型傳感器觀測模型6.4.2傳感器觀測模型1．基于移動機器人坐標系的極坐標表示法觀測模型通過移動機器人攜帶的傳感器觀測得到坐標為的特征點，在移動機器人坐標系下的模型可寫為：vk為傳感器本身存在或測量方式引起的噪聲。6.4.2傳感器觀測模型2．基于全局坐標系的觀測模型把在移動機器人坐標系中某個環(huán)境特征點的坐標轉換到世界坐標系下，就得到觀測量在世界坐標系中的坐標的觀測方程，即移動機器人系統(tǒng)的觀測模型：vk為傳感器本身存在或測量方式引起的噪聲。移動機器人系統(tǒng)含有n維的狀態(tài)向量Xk、m維的觀測序列Zk、m維的觀測噪聲vk和n維的過程噪聲序列ωk，在系統(tǒng)6.5

移動機器人卡爾曼濾波SLAM算法是線性離散的假設條件下,Fk是系統(tǒng)n×n維的狀態(tài)轉移矩陣，Bk是系統(tǒng)的輸入控制矩陣，Hk表示m×n維的觀測矩陣，uk用來表示系統(tǒng)的輸入控制向量，此系統(tǒng)的狀態(tài)方程和觀測方程可以表示如下：過程噪聲ωk和觀測噪聲vk滿足高斯白噪聲假設，即：Qk為ωk的協(xié)方差矩陣，Rk為vk的協(xié)方差矩陣。6.5

移動機器人卡爾曼濾波SLAM算法系統(tǒng)通過時刻的狀態(tài)值獲得時刻的狀態(tài)和協(xié)方差的先驗估計的過程稱為系統(tǒng)的時間更新。利用時刻的觀測對上述先驗估計進行校正的過程則稱為測量更新，校正后的系統(tǒng)狀態(tài)和協(xié)方差矩陣就成為時刻的先驗估計。將非線性狀態(tài)轉移函數和觀測函數圍繞移動機器人當前狀態(tài)估計值擴展成泰勒級數形式，并略去其二次冪及其高階小項，得到近似線性化模型，實現非線性系統(tǒng)的線性化，并運用卡爾曼濾波算法框架遞推計算移動機器人系統(tǒng)狀態(tài)，即擴展卡爾曼濾波（extendedkalmanfilter，EKF）SLAM算法。6.5

移動機器人卡爾曼濾波SLAM算法基于擴展卡爾曼濾波的SLAM算法的5個步驟如下：步驟1：由k-1時刻的移動機器人狀態(tài)和控制命令uk計算k時刻的狀態(tài)及誤差協(xié)方差；式中r代表機器人，l表示環(huán)境。6.5

移動機器人卡爾曼濾波SLAM算法步驟2：應用傳感器獲得k時刻的對環(huán)境特征的實際觀測值Yk；步驟3：根據移動機器人位姿預測與實際觀測計算得到觀測預測

；步驟4：計算新息

和增益Kk；步驟5：狀態(tài)更新；6.5

移動機器人卡爾曼濾波SLAM算法澳大利亞機器人中心提供了“CarParkDataset”標準數據集，用以進行EKF-SLAM算法實驗。該數據集可以在其網站.au下載得到。實驗用的智能車上安裝了GPS、激光雷達和慣導三種傳感器，慣導測量車輛左后輪線速度及車輛舵角，激光雷達則測量路標的距離和方向，智能車在運動過程中的經緯度通過GPS記錄?！癈arParkDataset”數據集紀錄了智能車在實驗場地東北部45m*30m的區(qū)域中行駛約2分鐘的過程中各傳感器的測量值。數據由三部分組成，一部分是智能車在運動過程中6.5

移動機器人卡爾曼濾波SLAM算法的GPS信息，記錄智能車的運動路徑，雖然有一定的誤差，一般仍被作為從總體上對SLAM算法的性能進行評估的參考標準。第二部分是慣導傳感器在各個時刻的測量值，代入智能車的運動方程則可預測智能車在下一個時刻的位姿，第三部分是激光雷達在各個時刻對人工路標的觀測數據，通過提取觀測特征并進行數據關聯，最終根據觀測數據值和數據關聯結果修正地圖估計和智能車的位姿估計。6.5

移動機器人卡爾曼濾波SLAM算法“CarParkDataset”數據集的EKF-SLAM結果6.6

移動機器人粒子濾波SLAM算法6.6.1蒙特卡羅采樣對于要解決的問題，蒙特卡羅方法首先建立一個概率模型，使概率模型的參數等于問題的解，所求參數的統(tǒng)計特征通過對模型的觀察或抽樣試驗來計算，最后給出所求解的近似值。蒙特卡羅方法通過隨機試驗求解積分問題，將服從分布密度函數p(r)的隨機變量g(r)的數學期望E(g(r))作為所要求解的積分：6.6.1

蒙特卡羅采樣從分布密度函數p(r)中通過某種試驗獲得采樣N個觀測值和N個相應的隨機變量值g(ri)i=1,...,N的算術平均值作為積分的估計值:當采樣點N足夠大時，根據大數定律，趨于數學期望E(g)。6.6.2重要性采樣蒙特卡羅采樣需要從后驗概率分布p(r)中采樣N個點，對于移動機器人系統(tǒng)，移動機器人位置估計E(g(X0:k))可以表示為：在移動機器人系統(tǒng)中，系統(tǒng)的后驗分布密度函數

無法直接獲得，對系統(tǒng)狀態(tài)的估計無法通過蒙特卡羅方法采樣后驗分布密度函數而進行。但可以對系統(tǒng)中一個已知的并且容易采樣的概率分布函數

進行采樣，該概率分布函數被稱為重要性采樣分布或者重要性函數。通過對由重要性函數的采樣點進行加權來逼近

：6.6.2重要性采樣具體推導如下：6.6.2重要性采樣

稱為未歸一化的重要性權值。另外，

可以表示為：6.6.2重要性采樣如果能夠從重要性函數式

采樣到樣本點，則通過蒙特卡羅采樣后的表示為：稱為歸一化權值。6.6.3序列重要性采樣重要性函數

可以分解為：實際動態(tài)系統(tǒng)服從一階馬爾可夫過程并且滿足系統(tǒng)觀測獨立，其數學表達式可以分別描述為：p(X0)表示該動態(tài)系統(tǒng)的初始先驗密度。6.6.3序列重要性采樣如果重要性分布函數只依賴于前一狀態(tài)Xk-1和當前的觀測值Zk，即上式的重要性采樣函數采樣得到的樣本

，樣本的最優(yōu)的選擇方法是：當前時刻的粒子由前一時刻粒子Xk-1和當前的觀測值Zk采樣得到，這種采樣方法稱為序列重要性采樣。將一定的權值wik賦予給每一個樣本Xik，則帶權重的粒子集表示Xk的概率分布，形式如下：6.6.3序列重要性采樣可以近似地表達為：式中，

是狄拉克函數。6.6.4退化問題與重采樣序列重要性采樣算法中，當前時刻的粒子權值是由上一時刻的粒子權值遞推得到，存在誤差的權值的誤差會隨著時間的傳播而進一步的積累，導致只有少數粒子的權值比較大，而大多數的粒子因權值太小以至于最終被忽略不計，即序列重要性采樣存在“粒子退化”現象。6.6.4

退化問題與重采樣“粒子退化”用指標Neff來衡量，Neff值小于閾值則表明“粒子退化”現象嚴重，需要采取措施改善退化現象。Neff由下式近似計算：通過兩種方案來改善退化現象：選擇好的建議分布或進行重采樣。在通過序列重要性函數采樣得到個樣本的基礎上，再對序列重要性函數進行次采樣，剔除權值較小的樣本，保留權值較大的樣本。也就是權值較小的樣本被復制的權值較大的樣本所代替，得到一組新的樣本集合。6.6.5基于粒子濾波的SLAM算法標準PF是在SIS方法上加入重采樣策略，也稱為SIR粒子濾波，從序列重要性函數采樣得到的樣本就是粒子濾波中的粒子。Montemerlo首先將粒子濾波器用于SLAM領域，提出了FastSLAM算法。其核心思想是用Rao-Blackwellise分解將SLAM問題分離為線性狀態(tài)的地圖特征估計與非線性狀態(tài)的路徑估計，應用SIR粒子濾波器估移動計機器人的路徑，地圖則采用擴展卡爾曼濾波器更新，FastSLAM算法也稱為RBPF-SLAM算法。6.6.5基于粒子濾波的SLAM算法根據貝葉斯公式以及環(huán)境特征估計間的獨立性假設，FastSLAM對移動機器人位姿和環(huán)境地圖的聯合后驗概率分布

分為移動機器人路徑估計和地圖估計

兩部分，然后再將地圖估計分解為N個相互獨立的特征估計

，具體形式如下式所示：6.6.5基于粒子濾波的SLAM算法移動機器人路徑估計

的近似表達式為:對于假設的近似后驗概率的提議分布

，滿足：從這個提議分布產生粒子，并賦權值:已知k-1時刻的粒子集

，從提議分布的狀態(tài)空間中采樣得到第i個粒子k時刻的位姿估計Xik，根據Xi1:k-16.6.5基于粒子濾波的SLAM算法和Xik得到第i個粒子k時刻的路徑移動機估計Xi1:k，同時權值遞推表達式為:FastsLAM2.0算法步驟具體如下所示：1.預測預測機器人k時刻的位姿分布，根據給定的系統(tǒng)控制向量uk和機器人的運動模型實現，同時計算在k時刻的位姿向量的預測均值和方差。6.6.5基于粒子濾波的SLAM算法2.數據關聯將k時刻觀測信息Zk和各個粒子在k-1時刻的地圖估計應用極大化觀測概率函數方法依次進行數據關聯。3.獲取提議分布采用EKF根據粒子的關聯觀測特征對粒子的先驗分布

進行觀測更新，計算各個粒子位姿向量在k時刻的濾波均值和方差，得各個粒子的提議分布4.移動機器人路徑估計采用SIR粒子濾波器估計移動機器人的路徑，獲取k時刻6.6.5基于粒子濾波的SLAM算法表示移動機器人路徑后驗概率分布

的粒子集

。5.更新地圖應用EKF根據粒子的關聯觀測特征更新各個粒子時刻的特征估計。對于沒有關聯上的觀測特征，則將對應的觀測特征作為新特征加入地圖，并刪除偽特征。6.7基于圖優(yōu)化的移動機器人SLAM算法圖優(yōu)化是把非線性系統(tǒng)用圖(Graph)表達，根據圖論知識對非線性系統(tǒng)狀態(tài)進行估計的方式。圖論中，圖由頂點（Vertex）和連接頂點之間邊（Edge）組成，圖通常表示為G={V,E}，其中，V表示頂點集合，E表示邊集合。圖中的頂點表示非線性系統(tǒng)中狀態(tài)值，頂點之間的邊表示非線性系統(tǒng)中狀態(tài)之間的約束關系。6.7基于圖優(yōu)化的移動機器人SLAM算法位姿Xk之間的約束關系滿足移動機器人運動模型，位姿與觀測的路標之間的約束關系滿足移動機器人觀測模型。位姿x1到x4以及路標p1、p2構成圖G的頂點，各位姿之間以及位姿與路標之間都存在的約束關系構成圖G的邊。圖優(yōu)化SLAM是指圖論知識求解SLAM中移動機器人位姿問題。6.7基于圖優(yōu)化的移動機器人SLAM算法圖優(yōu)化SLAM過程分為以下幾個步驟：1.傳感器數據獲得。移動機器人通過攜帶的傳感器采集外界環(huán)境信息，移動機器人通過碼盤、慣性傳感器等進行本體移動數據的讀取。圖優(yōu)化SLAM框架圖6.7基于圖優(yōu)化的移動機器人SLAM算法2.前端視覺里程計構成。視覺里程計指對傳感器采集的相鄰圖像特征點進行提取，同時，根據機器視覺以及相機成像原理計算出相機位姿，該相機安裝于移動機器人上，因為，可以確定出移動機器人位姿，同時，部分圖像特征點形成局部地圖。VO又稱為前端。3.后端非線性優(yōu)化。由于前端對移動機器人的位姿估計不可避免地存在誤差，移動機器人需要將位姿估計通過回環(huán)檢測等措施進行優(yōu)化，得到全局一致的移動機器人位姿軌跡和地圖。由于該過程在VO之后，又稱為后端。6.7基于圖優(yōu)化的移動機器人SLAM算法4.回環(huán)檢測，又稱閉環(huán)檢測。閉環(huán)檢測主要解決移動機器人的位姿估計誤差會隨時間累積的問題，目的是通過前后幀圖像間的關系對比來判斷移動機器人走過的位置是否重合即回環(huán)。通過充分而且正確的回環(huán)檢測，就可以消除累積誤差，得到正確的移動機器人軌跡，構建出高精度地圖。5.建圖。根據移動機器人位姿與觀測的關系，采用一定的表達方式，建立出環(huán)境的地圖。6.7.1ORB-SLAM算法ORB-SLAM是一個完整的視覺SLAM系統(tǒng)，包括視覺里程計、跟蹤、回環(huán)檢測等步驟，是一種完全基于稀疏特征點的單目SLAM系統(tǒng)，同時還有單目、雙目、RGBD相機的接口。其核心是使用ORB(OrintedFASTandBRIEF)作為圖像的核心特征。ORB-SLAM是三個流程同時運行的，結構圖如圖6-7所示。第一個是跟蹤，第二個是建圖，第三個是閉環(huán)檢測。跟蹤是從圖像中提取ORB