移動機(jī)器人原理與技術(shù) 課件 第五章 移動機(jī)器人路徑規(guī)劃與避障

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

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

環(huán)境原圖

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

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

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

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

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