移動機器人原理與技術(shù) 課件 第5、6章 移動機器人路徑規(guī)劃與避障、移動機器人同時定位與建圖

移動機器人技術(shù)原理與應(yīng)用第五章

移動機器人路徑規(guī)劃與避障移動機器人傳統(tǒng)路徑規(guī)劃移動機器人智能路徑規(guī)劃5.1移動機器人建圖5.25.3移動機器人常規(guī)避障方法5.4移動機器人依靠其配備的激光雷達、超聲波雷達、攝像頭等多種傳感器來收集外界環(huán)境的信息，通過對傳感器信息進行數(shù)據(jù)處理和融合，對其所工作的環(huán)境進行地圖構(gòu)建，并根據(jù)自身位置進行實時路徑規(guī)劃和動態(tài)避障，具備安全移動能力。移動機器人路徑規(guī)劃是指移動機器人按照距離、時間、能量等性能指標，在工作空間中搜索一條從起始狀態(tài)到目標狀態(tài)最優(yōu)且合理的運動策略，同時在運動過程中能完成障礙物躲避即避障，它是機器人執(zhí)行各種任務(wù)的基礎(chǔ)。移動機器人路徑規(guī)劃主要包含對地圖構(gòu)建和路徑搜索兩個層面。地圖構(gòu)建即應(yīng)用適當(dāng)?shù)慕▓D方法對移動機器人工作的環(huán)境空間進行描述；路徑搜索是指選用恰當(dāng)?shù)乃阉魉惴ǎ瓿蓮闹付ㄆ瘘c到終點的符合要求的最優(yōu)路徑。移動機器人避障是在行走過程中通過傳感器感知到妨礙其通行的靜態(tài)和動態(tài)物體時，按照一定的方法進行有效地躲避，從而能夠達到目標狀態(tài)。5.1移動機器人建圖移動機器人主要利用測距、視覺等傳感器獲取到環(huán)境信息（自然路標），按照某種方式對機器人的工作環(huán)境空間建立數(shù)學(xué)模型-環(huán)境地圖構(gòu)建簡稱建圖。根據(jù)建圖方式的不同，環(huán)境地圖可以分為兩大類：度量地圖、拓撲地圖。其中度量地圖側(cè)重于精確地表示地圖中物體的位置關(guān)系，最常見的建圖形式有特征地圖、柵格地圖等；而相比于度量地圖的精確性，拓撲地圖則更強調(diào)地圖元素之間的關(guān)系。5.1移動機器人建圖1．特征地圖特征地圖主要利用幾何特征如點、線、面等，來表達環(huán)境信息。近些年，特征地圖領(lǐng)域的技術(shù)應(yīng)用集中于視覺傳感器（一般使用相機）采集環(huán)境特征信息進行建圖。通過在采集的每一幀圖像里提取一系列的點、線等特征，并計算特征的空間位置信息，結(jié)合相機的位姿構(gòu)建特征地圖。5.1移動機器人建圖1．特征地圖

環(huán)境原圖

點線特征地圖5.1移動機器人建圖1．特征地圖特征地圖存在兩個關(guān)鍵問題，一個是，當(dāng)移動機器人對環(huán)境中的特征進行檢測時，可能會出現(xiàn)錯誤檢測或者漏檢測，特征要與前一時刻的檢測結(jié)果相關(guān)聯(lián)，以便于判斷提取的特征是否屬于同一個物體，這也稱為數(shù)據(jù)關(guān)聯(lián)問題。一般采用特征中的顯著特征進行檢測，顯著特征能確保移動機器人在地圖中的實體附近進行定位時，地圖中的實體很容易地再次被檢測出來。5.1移動機器人建圖1．特征地圖基于相機的特征地圖構(gòu)建的另一個關(guān)鍵問題是，環(huán)境特征的空間位置信息并不是容易獲得的變量，或者說，并不能直觀的觀測到一個特征的全部維度。這意味著需要通過對不同圖像幀的數(shù)據(jù)信息進行融合處理，運用概率學(xué)方法（也包括一些非概率學(xué)的方法）得到更多信息，包括深度信息。特征地圖的優(yōu)點：即在通過相機對環(huán)境進行特征提取時，不需要包含環(huán)境中物體的全部細節(jié)。另外特征地圖通常比柵格地圖減少對存儲空間的需求，節(jié)省計算成本。5.1移動機器人建圖2．柵格地圖柵格地圖是指將整個地圖分為若干相同大小的柵格，對于每一個柵格，它要么處于被障礙物占據(jù)的狀態(tài)，要么處于沒有被障礙物占據(jù)的空閑狀態(tài)，若用p(s)來表示每個柵格被占據(jù)的概率，p(s=1)表示此柵格處于占據(jù)狀態(tài)，p(s=0)表示此柵格處于空閑狀態(tài)，兩者概率之和為1，且每個柵格被占據(jù)的概率相互獨立。一個柵格狀態(tài)的數(shù)值越大，就越表示該柵格為占據(jù)狀態(tài)，相反數(shù)值越小，就表示該柵格為空閑狀態(tài)。5.1移動機器人建圖2．柵格地圖引入兩者的比值來表示柵格的狀態(tài)：當(dāng)傳感器獲得新的環(huán)境測量值{Measurement,z{0,1}}時，相關(guān)的柵格就要更新柵格狀態(tài)Odd(s)。假設(shè)新的測量值到來之前，該柵格的狀態(tài)為Odd(s)，則到來之后，更新柵格的狀態(tài)為：5.1移動機器人建圖2．柵格地圖根據(jù)貝葉斯公式得出以下兩個式子：上式同時取對數(shù)，用logOdd(s)表示一個柵格的狀態(tài)：5.1移動機器人建圖2．柵格地圖此時，該測量值的模型只有占據(jù)looccu和空閑lofree兩個定值，分別為：此時，用logOdd(s)來表示柵格s的狀態(tài)S，更新規(guī)則就簡化為：S+和S-分別表示測量值之后和之前柵格s的狀態(tài)。5.1移動機器人建圖2．柵格地圖經(jīng)過這樣的建模，更新一個柵格的狀態(tài)只需要做簡單的加法即可：在已知上一時刻的柵格占據(jù)狀態(tài)情況下，由移動機器人的位姿和當(dāng)前時刻傳感器觀測獲得的新測量值，就可以計算得到現(xiàn)在時刻柵格的占據(jù)概率，再根據(jù)移動機器人的位姿將柵格占據(jù)狀態(tài)映射到全局地圖中。5.1移動機器人建圖2．柵格地圖右圖描述了采用激光傳感器如何構(gòu)建柵格地圖，假設(shè)looccu=0.9，lofree=-0.7。激光雷達構(gòu)建柵格地圖5.1移動機器人建圖2．柵格地圖與強調(diào)幾何形狀的特征地圖不同，柵格地圖更關(guān)心的是每個柵格被占據(jù)的概率。如果提高柵格地圖的分辨率，它能夠精確的描述環(huán)境細節(jié)。然而，當(dāng)機器人通過較為復(fù)雜的環(huán)境時，若地圖分辨率設(shè)置過高，將會增加環(huán)境信息存儲的計算復(fù)雜性；嚴重時可能導(dǎo)致計算機存儲空間不足，地圖處理速度減慢等問題。根據(jù)柵格地圖所描述的工作環(huán)境的維度，可以將柵格地圖分為二維柵格地圖和三維柵格地圖。5.1移動機器人建圖2．柵格地圖二維柵格地圖三維柵格地圖5.1移動機器人建圖3．拓撲地圖拓撲地圖通常用圖結(jié)構(gòu)表示，它通過將環(huán)境抽象為節(jié)點和線，進而將環(huán)境表示成一個具有多連通區(qū)域的地圖，其中節(jié)點用于表示環(huán)境中的一個特征狀態(tài)或地點，線用于表示兩個對應(yīng)地點之間的連通狀態(tài)。拓撲地圖可以直接表達環(huán)境中各節(jié)點間的連通性，將其用于機器人的路徑規(guī)劃等任務(wù)時，它表現(xiàn)的更加高效和快速。然而，由于地圖中缺少度量信息，因此難以確保不同地點之間的可靠導(dǎo)航。5.1移動機器人建圖3．拓撲地圖

移動機器人構(gòu)建拓撲建圖

拓撲地圖5.2移動機器人傳統(tǒng)路徑規(guī)劃從獲取障礙物信息是靜態(tài)或是動態(tài)的角度看，全局路徑規(guī)劃屬于靜態(tài)規(guī)劃（又稱離線規(guī)劃）。全局路徑規(guī)劃需要掌握所有的環(huán)境信息，根據(jù)環(huán)境地圖的所有信息進行路徑規(guī)劃；局部路徑規(guī)劃只需要有傳感器實時采集環(huán)境信息，了解環(huán)境地圖信息，然后確定出所在地圖的位置及其障礙物分布情況，從而可以選出從當(dāng)前節(jié)點到某一子目標的最優(yōu)路徑。5.2移動機器人傳統(tǒng)路徑規(guī)劃5.2.1人工勢場法路徑規(guī)劃1.人工勢場法原理人工勢場法的基本原理就是將移動機器人假設(shè)成一個點，該點在一個虛擬力場中運動，虛擬力場是由目標點對移動機器人的引力場和障礙物對移動機器人的斥力場組成。移動機器人受到虛擬力，該虛擬力包括障礙物對移動機器人產(chǎn)生的斥力，目標點對移動機器人產(chǎn)生的引力，引力和斥力的合力作為移動機器人的加速力，該加速力“推動”移動機器人向著目標做無碰運動。5.2.1人工勢場法路徑規(guī)劃1.人工勢場法原理將引力場與斥力場的和定義為人工勢場法的勢場函數(shù)，移動機器人移動的方向為勢場函數(shù)梯度下降的方向。在移動機器人作業(yè)空間中，移動機器人的空間位姿可以用q表示，其勢場可以用U(q)表示，移動機器人目標狀態(tài)位姿可用qg來表示，并定義和目標位姿qg相關(guān)聯(lián)的吸引勢Ua(q)，以及和障礙物Uobs相關(guān)聯(lián)的排斥勢Urep(q)。那么，位姿空間中某一位姿的場可以用下式表示：5.2.1人工勢場法路徑規(guī)劃1.人工勢場法原理移動機器人所受到的虛擬力為目標位姿的吸引力和障礙物的斥力的合力，如下式所示：

表示U在q處的梯度，它是一個向量，其方向是位姿q所處勢場變化率最大的方向。那么，對于二維空間中的移動機器人位姿q(x,y)來說，有：5.2.1人工勢場法路徑規(guī)劃1.人工勢場法原理對于吸引勢Ua(q)和排斥勢Urep(q)，最常用的定義為靜電場勢場模型，如下式：ρ(q)=∥q-qg∥為從q到qg的歐氏距離；ξ、η為正比例系數(shù)；ρ0為正常數(shù)表示障礙物區(qū)域可對移動機器人的運動產(chǎn)生影響的最大距離；ρ(q)為障礙物區(qū)域Cobs到位姿q的最小距離5.2.1人工勢場法路徑規(guī)劃1.人工勢場法原理當(dāng)q無限接近于Cobs時，Urep(q)趨近于無窮大。結(jié)合前面的公式，可得到移動機器人所受吸引力和排斥力為：用qc表示障礙物區(qū)域Cobs上距離最近的位姿點，也就是ρ(q)=∥q-qg∥。則

是由qc指向q的單位向量，即5.2.1人工勢場法路徑規(guī)劃2.移動機器人軌跡生成在人工勢場中，移動機器人受到引力和斥力的合力作用，產(chǎn)生加速度ak：且加速度方向與F(qk)合力的方向一致，式中a0為加速度常數(shù)。設(shè)移動機器人系統(tǒng)對環(huán)境的采樣周期為T0，移動機器人的實際位姿為：5.2.1人工勢場法路徑規(guī)劃2.移動機器人軌跡生成移動機器人速度Vk的方向可用移動機器人姿態(tài)角θk表示：經(jīng)過一個系統(tǒng)采樣周期T0后，系統(tǒng)位姿變成：使用以上的公式計算環(huán)境中每一點的勢場，移動機器人作為一個質(zhì)點，在勢場力的引導(dǎo)下從起點開始移動，直到終點結(jié)束，其移動的軌跡即為規(guī)劃路徑。5.2.1人工勢場法路徑規(guī)劃2.移動機器人軌跡生成計算Urep時應(yīng)該注意，對各個障礙物可以選擇不同的η、ρ。如果某個障礙物離目標點較近，則應(yīng)該選擇較小的ρ0，以盡量使目標點在障礙物的影響范圍之外，否則排斥力可能會使移動機器人永遠無法到達目標點。如果目標點在某個障礙物的影響范圍之內(nèi)，Urep(qg)不為零，則U(q)的全局最小點不是目標點qg。此時，可以通過在目標點附近建立新的勢場函數(shù)，使得Urep(qg)為零。5.2.1人工勢場法路徑規(guī)劃3.人工勢場法路徑規(guī)劃人工勢場算法可以描述如下:（1）輸入。初始位姿qi，目標位姿qg障礙物信息。（2）輸出。一條連接qi和qg的位姿序列或者指出該序列不存在。5.2.1人工勢場法路徑規(guī)劃3.人工勢場法路徑規(guī)劃（3）過程。從qi開始每次計算當(dāng)前位姿qk的勢場力F(qk)并沿其方向前進一個小的步長δk，δk根據(jù)當(dāng)前位姿設(shè)置不同的值。重復(fù)計算一直到找到qg或者無路可走時結(jié)束。每一步選擇的δk必須足夠小，以保證從qk到qk+1的路徑不會和障礙物相碰撞。5.2.2A*算法路徑規(guī)劃1.A*(A-Star)算法原理A*算法是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，整體算法外部框架采用基本的搜索遍歷方法。A*算法的思想是以當(dāng)前節(jié)點n為中心，向周圍擴展待選節(jié)點node=[n1，n2，n3……]，傳統(tǒng)A*算法在擴展節(jié)點時與柵格法相結(jié)合，可以擴展點n所在方格周圍8個方格的中心點為下一步待選節(jié)點，但需要同時考慮該節(jié)點是否在威脅區(qū)范圍內(nèi)。再通過評價函數(shù)計算各個待選節(jié)點的代價值，選擇代價最小的節(jié)點作為下一步節(jié)點n+1。5.2.2A*算法路徑規(guī)劃1.A*(A-Star)算法原理A*算法的評價函數(shù)是：其中，f(n)是待搜索點總的路徑估價函數(shù)，g(n)表示從起始點到當(dāng)前點的最短路徑，h(n)表示當(dāng)前點到終止點的最短路徑。5.2.2A*算法路徑規(guī)劃2.A*算法路徑規(guī)劃路徑上的節(jié)點被存儲在稱為OpenList和CloseList的兩個列表中。OpenList中存放候選檢查的節(jié)點，CloseList存放已經(jīng)檢查過的節(jié)點。在算法的每個循環(huán)中，將從OpenList中選擇具有最小成本評估值的最佳節(jié)點。如果節(jié)點是目標節(jié)點，那么表示已經(jīng)搜索到了最佳路徑。否則，從OpenList中移除節(jié)點，并附加到CloseList。同時，不在CloseList和OpenList中的節(jié)點的鄰居節(jié)點需要添加到OpenList中，并將其父節(jié)點設(shè)置為節(jié)點。5.3移動機器人智能路徑規(guī)劃5.3.1基于遺傳算法的路徑規(guī)劃1.遺傳算法基本原理遺傳算法最初是從生物學(xué)中提煉、總結(jié)出來的，后來逐漸用于解決實際問題。與生物學(xué)中的染色體行為類似，在實際問題中通過合適的方式有選擇的保留“父代”中“優(yōu)秀”(符合某些研究特性)的染色體來遺傳給子代，來模擬自然界中的自然選擇；其次，選擇一定數(shù)量的父代染色體進行“交叉”操作來提供所求解問題的“新解”，當(dāng)然在“交叉”過程中還應(yīng)當(dāng)適當(dāng)保留一定數(shù)量的優(yōu)秀父代染色體5.3.1基于遺傳算法的路徑規(guī)劃1.遺傳算法基本原理不進行交叉操作，防止錯誤地將較優(yōu)解進行交叉操作；最后，在遺傳算法中還應(yīng)當(dāng)使得極少數(shù)的父代發(fā)生變異操作，因為這樣能保證染色體的多樣性，可以讓遺傳算法求得的最優(yōu)解較難陷入局部最優(yōu)。遺傳算法有如下3個重要概念:（1）編碼遺傳算法中首先要解決的就是如何將求解問題映射成數(shù)學(xué)問題，也就是數(shù)學(xué)建模，這就需要編碼，實現(xiàn)表現(xiàn)型5.3.1基于遺傳算法的路徑規(guī)劃1.遺傳算法基本原理和基因型的映射。一個可行解即被稱為一條“染色體”，一個可行解一般由多個變量構(gòu)成，這每一個變量就被稱為染色體上的一個“基因”。一個可行解即為一個個體，許許多多的個體就構(gòu)成了種群。（2）適應(yīng)度評估采用適應(yīng)度函數(shù)進行適應(yīng)度評估，通過計算適應(yīng)度函數(shù)的值表示個體的好壞。適應(yīng)度評估是遺傳算法進化的驅(qū)動力，也是進行自然選擇的唯一標準。5.3.1基于遺傳算法的路徑規(guī)劃1.遺傳算法基本原理（3）選擇交叉變異對于給定的初始種群，賦予它進化的能力，在進化中盡可能的保留種群中優(yōu)秀的個體，淘汰較差的個體，每次進化都會生成一個最優(yōu)的個體，無止境的進化下去總會找到最優(yōu)的解。遺傳算法進化能力的實現(xiàn)就是遺傳算子的作用，通過選擇算子、交叉算子、變異算子的操作，實現(xiàn)種群的不斷迭代進化。5.3.1基于遺傳算法的路徑規(guī)劃1.遺傳算法基本原理遺傳算法路徑規(guī)劃流程圖5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃移動機器人進行路徑規(guī)劃之前首先要建立地圖，采用柵格法建立移動機器人地圖。本例中不考慮障礙物高度問題，移動機器人行走空間為二維平面空間；且障礙物的大小、位置已知，并且不存在動態(tài)障礙物；同時，在規(guī)劃時把移動機器人看作質(zhì)點。柵格地圖中，白色部分為自由柵格，黑色部分為障礙物柵格。在構(gòu)建柵格地圖時，以地圖左下角第一個柵格為坐標原點建立直角坐標系，并且從左下角開始每一個柵格進行編號N，編號從0開5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃始。每一個柵格編好的號碼在遺傳算法中即為十進制編碼。因此可用此編號來表示一條路徑，比如起點為0終點為24的路徑可以表示為（0，6，7，8，13，19，24）。柵格地圖5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃移動機器人路徑規(guī)劃的具體過程如下：(1)編碼和種群初始化移動機器人的起始位置為柵格0，目標位置為柵格99。初始化種群要求隨機產(chǎn)生多條可行路徑，可行路徑表示不與障礙物柵格相碰撞的路徑。可行路徑的產(chǎn)生分為兩個主要步驟：第一步首先產(chǎn)生一條間斷路徑。移動機器人每次行走一個柵格，因此每一行至少有一個柵格在可行路徑中。所5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃以初始化時先按順序在每一行隨機取出一個無障礙柵格，形成一條間斷的路徑。為了減短路徑長度，路徑的第一個和最后一個柵格分別為移動機器人的起始位置和目標位置。第二步是將間斷的路徑連接為連續(xù)路徑。從第一個柵格開始，應(yīng)用兩個柵格的位置差絕對值函數(shù)開始判斷相鄰的兩個柵格是否為連續(xù)柵格。若新柵格為障礙物柵格則以上下左右順序取新柵格的相鄰柵格，并判斷此柵格是5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃否已經(jīng)在路徑中（防止陷入死循環(huán)），如果此柵格為無障礙柵格且不在路徑中則插入路徑中，如果遍歷上下左右四個柵格后沒有滿足條件的柵格則刪除這條路徑；若新柵格為無障礙物柵格，則插入兩個不連續(xù)柵格中間。繼續(xù)判斷新插入的柵格和新插入的柵格的前一個柵格是否連續(xù)，若不連續(xù)則循環(huán)以上步驟，直到兩個柵格連續(xù)。當(dāng)兩個柵格連續(xù)后取下一個柵格，循環(huán)以上步驟，直到整條路徑連續(xù)。5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃初始化路徑流程圖5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃（2）適應(yīng)度函數(shù)計算分別設(shè)置判斷每一個路徑長短和平滑程度的函數(shù)，此兩個函數(shù)按比例系數(shù)構(gòu)成統(tǒng)一的適應(yīng)度函數(shù)。全部路徑長度的計算采用每兩點間歐氏距離的總和表示，選用輪盤賭方式進行路徑選擇，滿足路徑最短的要求，取全部路徑長度的倒數(shù)作為適應(yīng)度函數(shù)的第一部分：5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃移動機器人由于運動學(xué)和動力學(xué)的約束，行進時拐彎不宜過大，因此產(chǎn)生的路徑有平滑度的要求。路徑越平滑，每一組相鄰的第一點和第三點（簡稱相鄰三點）形成的夾角越大，角度越大相鄰三點之間的距離越大，因此計算路徑中所有相鄰三點的距離作為適應(yīng)度函數(shù)的第二部分，計算公式如下：5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃使用時，適應(yīng)度函數(shù)的兩部分需分別取權(quán)重系數(shù)α和β：(3)選擇選擇方法采用簡單的基于概率的輪盤賭方法。首先計算出所有路徑個體的適應(yīng)度函數(shù)的和，再計算每一個個體所占的比率，根據(jù)每個個體的概率，以輪盤賭的方式選擇出下一代個體：5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃（4）交叉設(shè)定一個交叉概率Pc閾值，同時，產(chǎn)生0-1之間的一個隨機數(shù)，將此隨機數(shù)的值和交叉概率閾值比較，若隨機數(shù)的值小于Pc則進行交叉操作。具體的交叉操作是找出兩條路徑中所有相同的點，然后隨機選擇其中的一個點，將之后的路徑進行交叉操作。5.3.1基于遺傳算法的路徑規(guī)劃2.移動機器人路徑規(guī)劃（5）變異確定變異概率Pm閾值，產(chǎn)生0-1之間的一個隨機數(shù)，并和變異概率閾值比較，若小于變異概率值則進行變異操作。變異方法是隨機選取路徑中除起點和終點以外的兩個柵格，去除這兩個柵格之間的路徑，然后以這兩個柵格為相鄰點，使用初始化路徑中的第二步將這兩個點進行連續(xù)操作。此時有可能無法產(chǎn)生連續(xù)的路徑，則需要重新選擇兩個柵格執(zhí)行以上操作，直到完成變異操作。5.3.1基于遺傳算法的路徑規(guī)劃交叉和變異流程圖5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理蟻群算法的原理可以描述如下：在螞蟻運動過程中，螞蟻的移動方向是由各條路徑上的信息量所決定的，路徑上的殘留的信息素濃度和路徑上的距離啟發(fā)式信息決定了螞蟻選擇該路徑的幾率大小：5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理τij表示路徑(i,j)上殘留的信息素濃度，α表示信息啟發(fā)因子，反映螞蟻在搜索過程中路徑上的信息素濃度對選擇該路徑的相對重要程度。若α過小，則算法不僅收斂速度過慢，而且易陷入局部最優(yōu);若過α大，則信息素的正反饋作用過強，算法會出現(xiàn)過全局搜索能力低，過早收斂，陷入局部最優(yōu)情況。ηij表示螞蟻從i轉(zhuǎn)移到j(luò)的期望程度，β是期望啟發(fā)式因子，反映啟發(fā)式信息在指導(dǎo)蟻群搜索過程中的相對重要程度。若β過小，則蟻群會陷入純粹的隨5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理機搜索之中，找到最優(yōu)解的難度增大，收斂過慢;若β過大，則算法的收斂速度提高，但收斂性能可能會變差。為避免信息素濃度過大淹沒啟發(fā)式信息，需在所有螞蟻走過一個循環(huán)后，對路徑上殘留的信息素進行更新處理。t+n時刻在路徑(i,j)上的信息素濃度可以按下式調(diào)整：5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理ρ表示信息素揮發(fā)因子，路徑上的信息素不會持久存在，而是會隨著時間揮發(fā)掉一部分，因此信息素揮發(fā)因子的大小對蟻群算法的全局搜索能力和收斂速率有著直接的影響，當(dāng)ρ過小時，算法具有較強的全局搜索能力和隨機性，但收斂速度慢；當(dāng)ρ過大時，螞蟻以前搜索過的路徑再次被選擇的概率增大，全局搜索能力較弱。是一個比例，因此它的取值范圍在0,1之間。5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理根據(jù)信息素更新問題，存在三種不同的模型，分別是蟻密系統(tǒng)(Ant-DensitySystem)模型、蟻量系統(tǒng)(Ant-QuantitySystem)模型和蟻周系統(tǒng)(Ant-CycleSystem)模型，其主要區(qū)別在于的計算方式不同。蟻密系統(tǒng)模型：5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理蟻量系統(tǒng)模型：蟻周系統(tǒng)模型：5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理2.蟻群算法的基本步驟（1）設(shè)置相關(guān)參數(shù)并初始化。在利用蟻群算法求解問題之前，要先定義算法中所包含的所有參數(shù)，并指出每一個參數(shù)的含義，并結(jié)合實際案例逐一進行賦值操作。在基本蟻群算法中，需要進行初始化的參數(shù)有總迭代次數(shù)、種群規(guī)模、信息素揮發(fā)系數(shù)等。（2）生成初始可行路徑。蟻群算法只能對給定的路徑逐步進行優(yōu)化操作，但是無法直接生成可行初始路徑，所5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理以需要按照一定的規(guī)則生成一條可行的初始路徑。（3）更新信息素濃度。每當(dāng)蟻群更新了一次路徑之后，就要在更新后的路徑上根據(jù)一定的規(guī)則來更新信息素濃度，螞蟻根據(jù)信息素濃度的大小來確定下步的路徑。（4）更新階段性最優(yōu)路徑。在完成信息素濃度的更新之后，獲取所有路徑節(jié)點之間的信息素濃度，再根據(jù)一定的方式來確定螞蟻走每一條支路的概率，最終生成階段性最優(yōu)路徑。5.3.2基于蟻群算法的路徑規(guī)劃1.蟻群算法原理（5）重復(fù)步驟（3）和在步驟（4），完成設(shè)定的迭代次數(shù)或達到提前設(shè)定的閾值之后，輸出最終的最優(yōu)解。蟻群算法路徑規(guī)劃5.3.3移動機器人路徑規(guī)劃的發(fā)展趨勢1.局部路徑規(guī)劃與全局路徑規(guī)劃相結(jié)合全局路徑規(guī)劃一般是建立在已知環(huán)境信息的基礎(chǔ)上，適應(yīng)范圍相對有限。局部路徑規(guī)劃能適應(yīng)未知環(huán)境，但有時反應(yīng)速度不快，對局部路徑規(guī)劃系統(tǒng)品質(zhì)要求較高，因此，如果把兩者結(jié)合即可達到更好的規(guī)劃效果。2.傳統(tǒng)路徑規(guī)劃方法與新的智能方法相結(jié)合近年來，一些新的智能技術(shù)逐漸被引入到自主路徑規(guī)劃中來，也促使了各種方法的融合發(fā)展，例如：人工勢場法與神經(jīng)網(wǎng)絡(luò)、模糊控制的結(jié)合，以及模糊控制與人工5.3.3移動機器人路徑規(guī)劃的發(fā)展趨勢神經(jīng)網(wǎng)絡(luò)、遺傳算法及行為控制之間的結(jié)合等。3.多傳感器信息融合用于局部路徑規(guī)劃移動機器人在動態(tài)環(huán)境中進行路徑規(guī)劃所需的信息都是從傳感器獲得，單一傳感器難以保證輸入信息的準確性與可靠性，多傳感器所獲得的信息具有冗余性、互補性、實時性，且可快速并行分析現(xiàn)場環(huán)境。目前的方法有：采用概率方法表示信息的加權(quán)平均法、貝葉斯估計法、卡爾曼濾波法、統(tǒng)計決策理論法、仿效生物神經(jīng)網(wǎng)絡(luò)的信息處理方法、人工神經(jīng)網(wǎng)絡(luò)法等。5.3.3移動機器人路徑規(guī)劃的發(fā)展趨勢4.局部路徑規(guī)劃與動態(tài)環(huán)境路徑規(guī)劃相結(jié)合類似足球機器人比賽，需考慮目標點情況。這類規(guī)劃由于要考慮機器人及目標點狀態(tài)，使得規(guī)劃問題更為復(fù)雜，同時也賦予移動機器人更高的自主性以及智能水平。5.多智能移動機器人協(xié)調(diào)規(guī)劃該智能技術(shù)正在逐漸成為新的研究熱點，受到廣泛關(guān)注。由于障礙物與移動機器人數(shù)目的增加，極大地提高了自主路徑規(guī)劃的難度，這是移動機器人技術(shù)研究亟需拓展的領(lǐng)域。5.4移動機器人常規(guī)避障方法避障是指移動機器人根據(jù)采集的障礙物的狀態(tài)信息，在行走過程中通過傳感器感知到妨礙其通行的靜態(tài)和動態(tài)物體時，按照一定的方法進行有效地避障，最后達到目標點。實現(xiàn)避障與導(dǎo)航的必要條件是環(huán)境感知，在未知或者是部分未知的環(huán)境下避障需要通過傳感器獲取周圍環(huán)境信息，包括障礙物的尺寸、形狀和位置等信息。避障使用的傳感器主要有超聲傳感器、視覺傳感器、紅外傳感器、激光傳感器等。常用的避障方法有BUG算法、動態(tài)窗口法等。5.4.1BUG算法BUG算法（BugAlgorithms）是一種最簡單的避障算法。其算法原理類似昆蟲爬行的運動決策策略。在未遇到障礙物時，沿直線向目標運動；在遇到障礙物后，沿著障礙物邊界繞行，并利用一定的判斷準則離開障礙物繼續(xù)直行。這種應(yīng)激式的算法計算簡便，不需要獲知全局地圖和障礙物形狀，具備完備性。但是其生成的路徑平滑性不夠好，對機器人的各種微分約束適應(yīng)性比較差。BUG算法又分為BUG1算法和BUG2算法。5.4.1BUG算法1.BUG1算法BUG1算法的基本思想是在沒有障礙物時，移動機器人沿著直線向目標運動可以得到最短的路線。當(dāng)傳感器檢測到障礙物時，移動機器人繞行障礙物直到能夠繼續(xù)沿直線向目標運動。BUG1算法只有兩個行為：向目標直行和繞著障礙物的邊界走。假設(shè)移動機器人能夠計算兩點之間的距離，并且不考慮移動機器人的定位誤差。起始點和目標點分別為qstart和qgoal表示。初始時刻i=0，令qL0=qstart，并稱連接qLi和qgoal5.4.1BUG算法1.BUG1算法的線段為m-line。在沒有遇到障礙時，移動機器人沿著m-line朝目標qgoal直線移動。如果遇到障礙，則稱qH1點為第一次遇到障礙時的撞擊點（hitpoint）。接著，移動機器人環(huán)繞障礙物移動直至返回qH1點。然后判斷出障礙物周邊上離目標最近的點，并移到這個點上，該點稱為離開點（leavepoint），由qL1表示。從qL1開始移動機器人再次沿直線駛向目標，如果這條線與當(dāng)前障礙物相交，則不存在到達目標的路徑。5.4.1BUG算法1.BUG1算法

BUG1算法成功找到路徑示意圖BUG1算法無法成功找到路徑示意圖5.4.1BUG算法2.BUG2算法BUG2算法有朝向目標的直行和沿邊界繞行兩種運動。BUG2算法中的直線m-line是連接初始點和目標點的直線，計算過程中保持不變。遇到障礙物時移動機器人繞行障礙物，繞行過程中，在距離目標更近的點再次遇到直線m-line就停止繞行而沿著直線m-line向目標直行。如此循環(huán)，直到移動機器人到達目標點qgoal。在繞行過程中，若未遇到直線m-line上與目標更近的點qLi，則回到qHi點，那么移動機器人不能到達目標。5.4.1BUG算法2.BUG2算法

BUG2算法成功找到路徑示意圖BUG2算法無法成功找到路徑示意圖5.4.2向量場直方圖法向量場直方圖極坐標5.4.2向量場直方圖法向量場直方圖(vectorfieldhistogram,VFH)是一種由人工勢場法改進而來的移動機器人導(dǎo)航算法。該方法可以用極坐標圖描述，x軸代表移動機器人行走的方向與障礙物之間的角度，y軸是根據(jù)占有柵格的多少來計算障礙物在移動機器人運動方向的概率。引入代價函數(shù)L，為所有可以通過的方向賦值。5.4.2向量場直方圖法代價函數(shù)L可以表示為:其中θtar表示目標方向角度，θwheel表示輪子轉(zhuǎn)動角度，θbef表示原來運動方向角度，k、u、w為比例系數(shù)，選擇具有最小代價函數(shù)值L的方向作為移動機器人的行走方向，進而形成最優(yōu)路徑，調(diào)整這些系數(shù)可以改變移動機器人的避障效果。移動機器人技術(shù)原理與應(yīng)用第六章

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

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

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

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

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

移動機器人卡爾曼濾波SLAM算法是線性離散的假設(shè)條件下,Fk是系統(tǒng)n×n維的狀態(tài)轉(zhuǎn)移矩陣，Bk是系統(tǒng)的輸入控制矩陣，Hk表示m×n維的觀測矩陣，uk用來表示系統(tǒng)的輸入控制向量，此系統(tǒng)的狀態(tài)方程和觀測方程可以表示如下：過程噪聲ωk和觀測噪聲vk滿足高斯白噪聲假設(shè)，即：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)轉(zhuǎn)移函數(shù)和觀測函數(shù)圍繞移動機器人當(dāng)前狀態(tài)估計值擴展成泰勒級數(shù)形式，并略去其二次冪及其高階小項，得到近似線性化模型，實現(xiàn)非線性系統(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：應(yīng)用傳感器獲得k時刻的對環(huán)境特征的實際觀測值Yk；步驟3：根據(jù)移動機器人位姿預(yù)測與實際觀測計算得到觀測預(yù)測

；步驟4：計算新息

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

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

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

移動機器人卡爾曼濾波SLAM算法“CarParkDataset”數(shù)據(jù)集的EKF-SLAM結(jié)果6.6

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

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

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

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

：6.6.2重要性采樣具體推導(dǎo)如下：6.6.2重要性采樣

稱為未歸一化的重要性權(quán)值。另外，

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

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

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

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

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

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

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

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

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

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

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

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

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

的粒子集

。5.更新地圖應(yīng)用EKF根據(jù)粒子的關(guān)聯(lián)觀測特征更新各個粒子時刻