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

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

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

環(huán)境原圖

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

移動(dòng)機(jī)器人構(gòu)建拓?fù)浣▓D

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

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

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

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

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

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

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

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

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

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

移動(dòng)機(jī)器人卡爾曼濾波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

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

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

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

；步驟4：計(jì)算新息

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

分為移動(dòng)機(jī)器人路徑估計(jì)和地圖估計(jì)

兩部分，然后再將地圖估計(jì)分解為N個(gè)相互獨(dú)立的特征估計(jì)

，具體形式如下式所示：6.6.5基于粒子濾波的SLAM算法移動(dòng)機(jī)器人路徑估計(jì)

的近似表達(dá)式為:對(duì)于假設(shè)的近似后驗(yàn)概率的提議分布

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

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

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

的粒子集

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