專利名稱：一種基于粒子群優(yōu)化算法的移動機(jī)器人路徑規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明計算機(jī)技術(shù)領(lǐng)域，具體是涉及一種基于粒子群優(yōu)化算法的移動機(jī)器人路徑規(guī)劃方法。
背景技術(shù)：
移動機(jī)器人是智能控制技術(shù)中的一個重要領(lǐng)域，已經(jīng)被廣泛應(yīng)用在軍事、工業(yè)、農(nóng)業(yè)和教育等領(lǐng)域。路徑規(guī)劃是移動機(jī)器人系統(tǒng)的一個重要內(nèi)容，
它直接影響機(jī)器人完成任務(wù)的質(zhì)量。粒子群優(yōu)化算法(Particle SwarmOptimization ， PS0)由Kennedy和Eberhart首先提出，是一種智能演化算法。把粒子群優(yōu)化算法(Particle Swarm Optimization ， PS0)用在路徑規(guī)劃中是一個新的嘗試。通過改進(jìn)的粒子群優(yōu)化算法可以使機(jī)器人的行走路徑變得光滑。在粒子群優(yōu)化算法(PARTICLE SWARM OPTIMIZATION ，PS0)的適應(yīng)值函數(shù)中加入了障礙物躲避參數(shù)，使得機(jī)器人可以有效地避開障礙物。這些都是粒子群優(yōu)化算法(Particle Swarm Optimization ，PS0)在全局路徑規(guī)劃中的應(yīng)用，改變適應(yīng)值函數(shù)使得機(jī)器人在局部路徑規(guī)劃中可以成功地躲避障礙物而到達(dá)目標(biāo)點(diǎn)。然而這個方法只能用在簡單的環(huán)境中，不能幫助機(jī)器人躲避環(huán)境陷阱，如無路徑必須返回。于是提出了一種基于極坐標(biāo)粒子群(Polar Particle Swarm Optimization ， PPS0)的路徑規(guī)劃方法。雖然這個方法可以幫助機(jī)器人逃避環(huán)境陷阱，但它在障礙物太大的時候就會失效。

發(fā)明內(nèi)容
本發(fā)明的目的在于針對現(xiàn)有技術(shù)的不足，提供一種基于粒子群優(yōu)化算法的移動機(jī)器人路徑規(guī)劃方法。本發(fā)明方法的具體步驟是
步驟(l)設(shè)機(jī)器人的當(dāng)前位置為Si ，機(jī)器人傳感器感應(yīng)圓的半徑為R， S,與目標(biāo)點(diǎn)之間的距離為D;以S,為圓心做圓C，如果R^D，則圓C的半徑為R，如果R〉D，則圓C的半徑為D;以S,為射線出發(fā)點(diǎn)，做平行于x軸的射線，射線與圓C相交于Q點(diǎn)，并將此射線作為極坐標(biāo)軸；步驟(2)將《點(diǎn)與Q點(diǎn)間線段進(jìn)行K等分，得到長度為I I觀I I /K的K段線段，以各個等分點(diǎn)到S點(diǎn)的距離為半徑，以S,為圓心做圓，得到K個同心圓；
步驟(3)可行路徑是一條從起點(diǎn)&到圓C上點(diǎn)的若干線段組成的折線，用這些折線的端點(diǎn)即路徑關(guān)鍵點(diǎn)的序列表示，機(jī)器人移動過的路徑就是這些點(diǎn)的連線，定義為尸={&,《,.."巧"."^}，1^7、附，附<〖；其中《,…,尸),…,^都是非障礙物點(diǎn)，即相鄰的兩點(diǎn)之間的連線上沒有障礙物；
步驟(4)在當(dāng)前非障礙物點(diǎn)上將傳感器感應(yīng)到的半徑為R的圓等分為n個扇形區(qū)域械,(12,...，(^,..,(111}，每個扇形的角度為360/n度；
步驟(5)每次選擇一個扇形區(qū)域作為探索區(qū)域扇形區(qū)域dj被選擇，用粒子群優(yōu)化算法探索c^，如果在扇形區(qū)域dj中找到可行路徑，移動機(jī)器人至
下一個非障礙物點(diǎn)；如果找不到路徑，探索下一個扇形區(qū)域、1;對每一個
扇形區(qū)域探索利用PSO算法對機(jī)器人路徑進(jìn)行規(guī)劃，最終找到無障礙物的路徑；
步驟(6)如果在傳感器感應(yīng)到的半徑為R的圓的范圍內(nèi)找不到可行路徑，則返回到前一點(diǎn)，探索在前一點(diǎn)中沒有探索過的扇形區(qū)域；把機(jī)器人每次走過的路徑存儲在一個鏈表中，以方便機(jī)器人搜索；
步驟(7)當(dāng)圓C的半徑為D時，目標(biāo)點(diǎn)即在圓周C上，機(jī)器人利用PSO算法所做的移動即可到達(dá)目標(biāo)點(diǎn)。當(dāng)圓C的半徑小于D時，機(jī)器人移動Pm到后，將Pm的極坐標(biāo)值賦值給Si，重復(fù)步驟(1)到(7)，直到找到一條無碰撞路徑，到達(dá)目標(biāo)點(diǎn)。
經(jīng)過基于極坐標(biāo)粒子群的深度優(yōu)先搜索找到一個無碰撞的路徑，最終到達(dá)目標(biāo)點(diǎn)。
本發(fā)明在極坐標(biāo)粒子群中加入了深度優(yōu)先搜索。用這種方法，機(jī)器人可以在大多數(shù)復(fù)雜環(huán)境中有效地找到一個無碰撞的路徑，最終到達(dá)目標(biāo)點(diǎn)。
具體實(shí)施例方式
采用深度優(yōu)先搜索算法和粒子群優(yōu)化算法相結(jié)合的方法，對移動機(jī)器人在未知環(huán)境信息下進(jìn)行路徑規(guī)劃。旨在移動機(jī)器人能用粒子群優(yōu)化算法在未知的復(fù)雜環(huán)境中找到有效路徑。
該基于粒子群優(yōu)化算法的移動機(jī)器人路徑規(guī)劃方法的具體步驟是步驟l:對環(huán)境進(jìn)行建模。
用多邊形表示障礙物，用點(diǎn)表示機(jī)器人，用小方框表示機(jī)器人的起始位置，用十字表示目標(biāo)點(diǎn)。步驟2:對機(jī)器人路徑進(jìn)行規(guī)劃。
(1) 假設(shè)機(jī)器人的當(dāng)前位置為S，，以S,為圓點(diǎn)做圓C，如果R^D,則C的
半徑為R，如果R〉D，則C的半徑為D。其中R表示機(jī)器人傳感器感應(yīng)圓的半徑，D表示Sj與目標(biāo)點(diǎn)的距離。以S，為射線出發(fā)點(diǎn)，做平行于x軸的射線，射線與圓C相交于Q點(diǎn)，并將此射線作為極坐標(biāo)軸。
(2) 將《2點(diǎn)進(jìn)行K等分，得到間隔長度為l 1/K的K段線段，以各個等分點(diǎn)到原點(diǎn)的連線為半徑，以S,為圓心做圓，得到一系列的同心圓。
(3) 可行路徑是一條從起點(diǎn)Si，到圓C上點(diǎn)的若干線段組成的折線，用這些折線的端點(diǎn)即路徑關(guān)鍵點(diǎn)的序列表示。機(jī)器人移動過的路徑就是這些點(diǎn)的連線，定義為戶-W,S,…，^,…,尸J，l《7^m。其中/^是非障礙物點(diǎn)，和5相連的點(diǎn)的連線上沒有障礙物。
(4) 當(dāng)圓C的半徑為D時，目標(biāo)點(diǎn)即在圓周C上，機(jī)器人利用PS0算法所做的移動即可到達(dá)目標(biāo)點(diǎn)。當(dāng)圓C的半徑小于D時，機(jī)器人移動Pm到后，將Pm的極坐標(biāo)值賦值給Si，重復(fù)步驟1)到4)，直到找到一條無碰撞路徑，到達(dá)目標(biāo)點(diǎn)。
歩驟3:基于極坐標(biāo)粒子群的深度優(yōu)先搜索。
(1) 記錄機(jī)器人移動過的點(diǎn)記為^，s,，S2,…sJ 。假設(shè)機(jī)器人的當(dāng)前位置為S,，傳感器感應(yīng)到的范圍為圓q ， 將圓C,平分為n個扇形{^dndjdj+^^dj ，每個扇形的角度為360/n度。
(2) 每次選擇一個扇形作為探索區(qū)域。假設(shè)dj被選擇，用粒子群優(yōu)化算法PSO探索dj，如果在dj中找到可行路徑，移動機(jī)器人；如果找不到路徑，探索c^部分。對每一個扇形區(qū)域探索利用PSO算法對機(jī)器人路徑進(jìn)行規(guī)劃，最終找到無障礙物的路徑。
(3) 如果在圓C,范圍內(nèi)找不到可行路徑，則返回、點(diǎn)，探索在Cw中沒有探索過的區(qū)域(扇形)。把機(jī)器人每次走過的路徑存儲在一個鏈表中，以方便其搜索。
經(jīng)過基于極坐標(biāo)粒子群的深度優(yōu)先搜索找到一個無碰撞的路徑，最終到達(dá)目標(biāo)點(diǎn)。
權(quán)利要求
1、一種基于粒子群優(yōu)化算法的移動機(jī)器人路徑規(guī)劃方法，其特征在于該方法的步驟包括步驟(1)設(shè)機(jī)器人的當(dāng)前位置為Si，機(jī)器人傳感器感應(yīng)圓的半徑為R，Si與目標(biāo)點(diǎn)之間的距離為D；以Si為圓心做圓C，如果R≤D，則圓C的半徑為R，如果R＞D，則圓C的半徑為D；以Si為射線出發(fā)點(diǎn)，做平行于x軸的射線，射線與圓C相交于Q點(diǎn)，并將此射線作為極坐標(biāo)軸；步驟(2)將Si點(diǎn)與Q點(diǎn)間線段進(jìn)行K等分，得到長度為||SiQ||/K的K段線段，以各個等分點(diǎn)到Si點(diǎn)的距離為半徑，以Si為圓心做圓，得到K個同心圓；步驟(3)可行路徑是一條從起點(diǎn)Si到圓C上點(diǎn)的若干線段組成的折線，用這些折線的端點(diǎn)即路徑關(guān)鍵點(diǎn)的序列表示，機(jī)器人移動過的路徑就是這些點(diǎn)的連線，定義為P＝{Si，P1，...，Pj，...，Pm}，1≤j≤m，m＜K；其中P1，...，Pj，...，Pm都是非障礙物點(diǎn)，即相鄰的兩點(diǎn)之間的連線上沒有障礙物；步驟(4)在當(dāng)前非障礙物點(diǎn)上將傳感器感應(yīng)到的半徑為R的圓等分為n個扇形區(qū)域{d1，d2，...，dj，dj+1，...，dn}，每個扇形的角度為360/n度；步驟(5)每次選擇一個扇形區(qū)域作為探索區(qū)域扇形區(qū)域dj被選擇，用粒子群優(yōu)化算法探索dj，如果在扇形區(qū)域dj中找到可行路徑，移動機(jī)器人至下一個非障礙物點(diǎn)；如果找不到路徑，探索下一個扇形區(qū)域dj+1；對每一個扇形區(qū)域探索利用PSO算法對機(jī)器人路徑進(jìn)行規(guī)劃，最終找到無障礙物的路徑；步驟(6)如果在傳感器感應(yīng)到的半徑為R的圓的范圍內(nèi)找不到可行路徑，則返回到前一點(diǎn)，探索在前一點(diǎn)中沒有探索過的扇形區(qū)域；把機(jī)器人每次走過的路徑存儲在一個鏈表中；步驟(7)當(dāng)圓C的半徑為D時，目標(biāo)點(diǎn)即在圓周C上，機(jī)器人利用PSO算法所做的移動即可到達(dá)目標(biāo)點(diǎn)；當(dāng)圓C的半徑小于D時，機(jī)器人移動pm到后，將pm的極坐標(biāo)值賦值給Si，重復(fù)步驟(1)到(7)，直到找到一條無碰撞路徑，到達(dá)目標(biāo)點(diǎn)。
全文摘要
本發(fā)明涉及一種基于粒子群優(yōu)化算法的移動機(jī)器人路徑規(guī)劃方法。目前移動機(jī)器人路徑規(guī)劃方法對于路徑的選擇比較單一、容易走入死胡同。本發(fā)明的具體步驟是首先對環(huán)境進(jìn)行建模；用多邊形表示障礙物，用點(diǎn)表示機(jī)器人，用小方框表示機(jī)器人的起始位置，用十字表示目標(biāo)點(diǎn)。機(jī)器人開始點(diǎn)標(biāo)記為S，目標(biāo)點(diǎn)標(biāo)記為G；然后對機(jī)器人路徑利用粒子群優(yōu)化算法進(jìn)行規(guī)劃；最后對規(guī)劃的路徑進(jìn)行深度優(yōu)先搜索。本發(fā)明在極坐標(biāo)粒子群中加入了深度優(yōu)先搜索。用這種方法，機(jī)器人可以在大多數(shù)復(fù)雜環(huán)境中有效地找到一個無碰撞的路徑，最終到達(dá)目標(biāo)點(diǎn)。
文檔編號G05D1/00GK101604166SQ20091010061
公開日2009年12月16日 申請日期2009年7月10日 優(yōu)先權(quán)日2009年7月10日
發(fā)明者吳國華, 禎 張, 方美娥, 王玉娟 申請人:杭州電子科技大學(xué)