本發(fā)明設(shè)計(jì)一種移動(dòng)機(jī)器人完全遍歷路徑規(guī)劃方法,特別是涉及一種在動(dòng)態(tài)未知復(fù)雜環(huán)境下的移動(dòng)機(jī)器人完全遍歷路徑規(guī)劃方法。
背景技術(shù):
未知環(huán)境下的完全遍歷路徑規(guī)劃是移動(dòng)機(jī)器人自主導(dǎo)航中的一個(gè)重要問題,該問題常存在于自動(dòng)掃地機(jī)器人、排雷機(jī)器人、海底測(cè)繪機(jī)器人等應(yīng)用中。完全遍歷路徑規(guī)劃要求機(jī)器人的足跡或者傳感器探測(cè)覆蓋整個(gè)工作空間。當(dāng)機(jī)器人工作在未知環(huán)境下,為了完全遍歷工作空間,需要實(shí)時(shí)構(gòu)建地圖和動(dòng)態(tài)規(guī)劃路徑。
常見的完全遍歷路徑規(guī)劃方法有人工勢(shì)場(chǎng)法、模板模型法、A*算法等。人工勢(shì)場(chǎng)法在實(shí)時(shí)避障和平滑軌跡控制方面得到廣泛應(yīng)用,但該方法存在局部極小值問題,在處理局部最優(yōu)解時(shí),容易產(chǎn)生死鎖(即機(jī)器人的四周為障礙或已遍歷區(qū)域);在靠近障礙物時(shí)容易震蕩;在狹窄通道中擺動(dòng);存在陷阱區(qū)域等。模板模型法需要事先定義好典型模板,因此該方法不適于處理動(dòng)態(tài)變化的環(huán)境。A*算法需要知道環(huán)境的先驗(yàn)知識(shí),當(dāng)環(huán)境信息改變時(shí),需要重新規(guī)劃,計(jì)算量大。Simon X.Yang(Chaomin Luo,Simon X.Yang.A Bioinspired Neural Network for Real-Time Concurrent Map Building and Complete Coverage Rotot Navigation in Unknown Environment.IEEE TRANSACTIONS ON NEURAL NETWORKS,VOL.19,NO.7,JULY2008.)提出了一種基于生物激勵(lì)的神經(jīng)網(wǎng)絡(luò)算法,機(jī)器人在利用有限的傳感器信息進(jìn)行路徑規(guī)劃時(shí),通過神經(jīng)動(dòng)力學(xué)建立了由正方形或矩形單元組成的環(huán)境地圖,能夠有效解決點(diǎn)到點(diǎn)路徑規(guī)劃問題,但該方法也存在一些不足,如分離區(qū)域之間的路徑重疊率高,路徑非最優(yōu),逃離死區(qū)時(shí)需等待較長(zhǎng)時(shí)間等問題。
技術(shù)實(shí)現(xiàn)要素:
針對(duì)上述問題,本發(fā)明提出了一種基于回溯搜索的生物激勵(lì)完全遍歷路徑規(guī)劃方法,該方法綜合了生物激勵(lì)神經(jīng)網(wǎng)絡(luò)算法、回溯算法(Ginsberg M L.Dynamic backtracking[J].Journal of Artificial Intelligence Research,1993:25-46.)、D*(D Star)(Stentz A.The focussed D*algorithm for real-time replanning[C]//International Joint Conference on Artificial Intelligence.1995,95:1652-1659.)算法的優(yōu)點(diǎn),不存在局部極小值問題,計(jì)算量小,實(shí)現(xiàn)簡(jiǎn)單,能實(shí)現(xiàn)機(jī)器人在復(fù)雜環(huán)境下的完全遍歷路徑規(guī)劃。
一種基于回溯搜索的生物激勵(lì)完全遍歷路徑規(guī)劃方法,包括以下幾個(gè)步驟:
1)以機(jī)器人執(zhí)行器的工作直徑構(gòu)建柵格地圖;
2)將鄰域內(nèi)有未遍歷區(qū)域的柵格作為節(jié)點(diǎn)構(gòu)建節(jié)點(diǎn)隊(duì)列;
3)將柵格地圖中的每一個(gè)坐標(biāo)點(diǎn)作為一個(gè)神經(jīng)元,并初始化每個(gè)神經(jīng)元的活性值為0;
4)判斷節(jié)點(diǎn)隊(duì)列的狀態(tài),若為空,則結(jié)束遍歷;否則繼續(xù)遍歷;
5)按一定的時(shí)間間隔讀取激光傳感器采集的障礙物的測(cè)距數(shù)據(jù),將該測(cè)距數(shù)據(jù)轉(zhuǎn)化成柵格坐標(biāo),并更新每個(gè)神經(jīng)元的活性值;
6)依據(jù)機(jī)器人當(dāng)前狀態(tài),選擇采用基于生物激勵(lì)神經(jīng)網(wǎng)絡(luò)模型的路徑規(guī)劃方法或者利用回溯搜索算法與D*算法相結(jié)合的規(guī)劃路徑方法決策出下一個(gè)目標(biāo)位置;
7)移動(dòng)機(jī)器人到目標(biāo)位置,返回步驟4)。
下面具體說明本發(fā)明的各個(gè)步驟。
步驟1:構(gòu)建柵格地圖
以移動(dòng)機(jī)器人的幾何中心為原點(diǎn),機(jī)器人的運(yùn)動(dòng)方向?yàn)閅軸的正方向,順時(shí)針方向旋轉(zhuǎn)90度為X軸的正方向,建立移動(dòng)坐標(biāo)系L;以移動(dòng)機(jī)器人開始移動(dòng)前的幾何中心所在位置為原點(diǎn),以機(jī)器人開始移動(dòng)前的指向?yàn)閅軸的正方向,順時(shí)針方向旋轉(zhuǎn)90度為X軸的正方向,建立全局坐標(biāo)系G。
以精度p將全局坐標(biāo)系轉(zhuǎn)化成柵格地圖,每個(gè)柵格的長(zhǎng)和寬分別為Δx=p和Δy=p,所述p為機(jī)器人執(zhí)行器的工作直徑;對(duì)掃地機(jī)器人來(lái)說,p為機(jī)器人底盤的直徑,取值范圍為0.2m到0.4m。
將環(huán)境中已知的靜態(tài)障礙物以及移動(dòng)機(jī)器人的所在位置對(duì)應(yīng)的柵格坐標(biāo)設(shè)置為:和其中,(xg,yg)表示物體與機(jī)器人在全局坐標(biāo)系G中的坐標(biāo),(xs,ys)表示對(duì)應(yīng)的柵格;移動(dòng)機(jī)器人在全局坐標(biāo)系下的坐標(biāo)為(xr,yr),激光傳感器安裝于機(jī)器人的幾何中心,探測(cè)到前方d處有障礙物,與機(jī)器人X軸方向的夾角為α,移動(dòng)坐標(biāo)系與全局坐標(biāo)系的夾角為θ,則該障礙物在全局坐標(biāo)系下的柵格坐標(biāo)為:對(duì)每個(gè)柵格賦值,-1表示無(wú)障礙柵格,0表示未知柵格,1表示該柵格被障礙物占據(jù);規(guī)定,即使某個(gè)柵格只有部分區(qū)域被障礙物占據(jù),仍賦值為“1”。
步驟2:構(gòu)建節(jié)點(diǎn)隊(duì)列
節(jié)點(diǎn)定義為鄰域內(nèi)有未遍歷的柵格,當(dāng)前柵格鄰域內(nèi)的未遍歷柵格數(shù)量定義為:n為與當(dāng)前柵格相鄰的柵格個(gè)數(shù),g表示柵格狀態(tài),k,l為柵格索引;若Ng>0,則將當(dāng)前柵格定義為節(jié)點(diǎn),并加入到節(jié)點(diǎn)隊(duì)列。當(dāng)節(jié)點(diǎn)隊(duì)列的節(jié)點(diǎn)數(shù)為0時(shí),說明已經(jīng)沒有未遍歷節(jié)點(diǎn),結(jié)束遍歷工作。
步驟3:將柵格地圖中的每一個(gè)坐標(biāo)點(diǎn)作為一個(gè)神經(jīng)元,并初始化每個(gè)神經(jīng)元的活性值為0。
步驟4:判斷節(jié)點(diǎn)隊(duì)列是否為空。若節(jié)點(diǎn)隊(duì)列為空,說明已經(jīng)沒有未遍歷節(jié)點(diǎn),結(jié)束遍歷工作;否則需要繼續(xù)遍歷。
步驟5:按時(shí)間間隔T讀取激光傳感器采集的障礙物的測(cè)距數(shù)據(jù),并將數(shù)據(jù)轉(zhuǎn)化成柵格坐標(biāo),并將全局坐標(biāo)系的每個(gè)神經(jīng)元(每個(gè)神經(jīng)元對(duì)應(yīng)一個(gè)柵格)的活性值按如下公式更新:
其中,xi表示第i個(gè)神經(jīng)元的狀態(tài),即神經(jīng)元活性;A,B和D是非負(fù)常數(shù),A代表衰減率,B和D分別代表神經(jīng)元狀態(tài)xi的上下限,即xi∈[-D,B];k是與第i個(gè)神經(jīng)元相鄰近的神經(jīng)元個(gè)數(shù);Ii為外部輸入,當(dāng)外部為未清掃區(qū)域時(shí)Ii=E;當(dāng)外部為障礙時(shí)Ii=-E;否則Ii=0。E是一個(gè)很大的正常數(shù)(E>>B),確保了目標(biāo)(未清掃區(qū)域)總是處于神經(jīng)網(wǎng)絡(luò)的波峰,障礙總是處于波谷。和[Ii]-分別表示刺激性和抑制性輸入。第i個(gè)神經(jīng)元和第j個(gè)神經(jīng)元的連接權(quán)值可以定義為ωij=f(|qi-qj|),|qi-qj|代表向量qi和qj之間的歐氏距離。f(a)可以是任意單調(diào)遞減的函數(shù),例如可以定義為:
其中μ和r0都是正常數(shù),r0表示感受域,即機(jī)器人傳感器的探測(cè)半徑。因此,每個(gè)神經(jīng)元只與小范圍區(qū)域[0,r0]內(nèi)的神經(jīng)元有直接聯(lián)系。優(yōu)選地,本發(fā)明只考慮與當(dāng)前柵格緊鄰的8個(gè)柵格,因此取r0=2,神經(jīng)元連接示意圖如圖3所示,第i個(gè)神經(jīng)元的感受域?yàn)榘霃綖閞0的圓區(qū)域,第i個(gè)神經(jīng)元
步驟6:路徑規(guī)劃算法
在本發(fā)明中,機(jī)器人采用“弓”字型路徑。當(dāng)機(jī)器人未陷入死鎖時(shí),采用基于生物激勵(lì)神經(jīng)網(wǎng)絡(luò)模型進(jìn)行路徑規(guī)劃;當(dāng)陷入死鎖時(shí),采用回溯搜索算法與D*算法規(guī)劃路徑。下面分別進(jìn)行說明:
1)基于生物激勵(lì)神經(jīng)網(wǎng)絡(luò)模型的路徑規(guī)劃方法
在基于生物激勵(lì)的神經(jīng)網(wǎng)絡(luò)模型中,機(jī)器人的路徑由狀態(tài)方程及上一時(shí)刻所處的位置決定。給定機(jī)器人的當(dāng)前位置pc,假設(shè)下個(gè)時(shí)刻的位置為pn,則pn按下式確定:
其中,c是一個(gè)正常數(shù),表示轉(zhuǎn)角所占的權(quán)重,k是鄰域的神經(jīng)元的個(gè)數(shù),xj表示第j個(gè)神經(jīng)元的活性值,yj是關(guān)于角度變化的單調(diào)遞增函數(shù),定義前一時(shí)刻的位置pp,當(dāng)前位置pc,下一位置pj,則yj定義為:
其中Δθj∈[0,π]是指角度變化值,當(dāng)機(jī)器人走直線時(shí)Δθj=0,往回走時(shí)Δθj=π,因此Δθj可以定義為:
其中,θj表示機(jī)器人在下一位置的朝向,θc表示機(jī)器人在當(dāng)前位置的朝向,atan表示反正切運(yùn)算。需要注意的是,當(dāng)機(jī)器人遇到障礙物且左右轉(zhuǎn)的角度變化值一樣,即存在多個(gè)解時(shí),優(yōu)先選取使機(jī)器人右轉(zhuǎn)的解。
當(dāng)機(jī)器人從當(dāng)前位置到達(dá)下一個(gè)位置,下一個(gè)位置就變成新的當(dāng)前位置(如果找到下一位置與當(dāng)前位置一樣,機(jī)器人將不會(huì)移動(dòng))。機(jī)器人下一時(shí)刻的位置根據(jù)變化著的環(huán)境作適應(yīng)性地變化。
該算法是的機(jī)器人在未知的環(huán)境下,朝著活性值最高且轉(zhuǎn)向角最小的未清掃區(qū)域移動(dòng),并通過激光掃描儀實(shí)時(shí)建立環(huán)境地圖,直到完全覆蓋整個(gè)環(huán)境空間。
2)回溯搜索算法與D*算法相結(jié)合的規(guī)劃路徑方法
該算法涉及到所述節(jié)點(diǎn)隊(duì)列的創(chuàng)建于維護(hù)。首先利用生物激勵(lì)神經(jīng)網(wǎng)絡(luò)使機(jī)器人覆蓋工作空間,直到陷入死鎖。機(jī)器人在行走過程中構(gòu)建空間地圖,將檢測(cè)到的鄰域內(nèi)有未遍歷的點(diǎn)作為節(jié)點(diǎn)記錄下來(lái),并按時(shí)間先后順序保存在節(jié)點(diǎn)隊(duì)列表中,同時(shí)剔除已遍歷節(jié)點(diǎn)。當(dāng)機(jī)器人陷入死鎖時(shí),說明局部區(qū)域已經(jīng)遍歷完成。這時(shí),將當(dāng)前點(diǎn)設(shè)置為起始點(diǎn),從節(jié)點(diǎn)隊(duì)列中找到距當(dāng)前時(shí)間最近的節(jié)點(diǎn)作為下一個(gè)目標(biāo)點(diǎn)。再根據(jù)已經(jīng)構(gòu)建的環(huán)境地圖,由D*算法規(guī)劃出一條點(diǎn)到點(diǎn)的最短避障路徑,當(dāng)?shù)竭_(dá)目標(biāo)后,由于目標(biāo)點(diǎn)鄰域內(nèi)有未遍歷區(qū)域,機(jī)器人退出死鎖,此時(shí)算法首先更新節(jié)點(diǎn)隊(duì)列,剔除已遍歷的節(jié)點(diǎn),加入新的節(jié)點(diǎn),然后按照生物激勵(lì)神經(jīng)網(wǎng)絡(luò)算法遍歷該區(qū)域。由于D*算法能實(shí)時(shí)更新啟發(fā)函數(shù),因此即使在點(diǎn)到點(diǎn)規(guī)劃過程中遇到未知障礙,也能輕松避障,并以最短路徑到達(dá)目標(biāo)點(diǎn)。當(dāng)節(jié)點(diǎn)隊(duì)列的節(jié)點(diǎn)數(shù)為0時(shí),說明已經(jīng)沒有未遍歷節(jié)點(diǎn),結(jié)束遍歷工作。
本發(fā)明的有益效果是:
本發(fā)明綜合了生物激勵(lì)神經(jīng)網(wǎng)絡(luò)算法、回溯算法、D*(D Star)算法的優(yōu)點(diǎn),不存在局部極小值問題,計(jì)算量小,實(shí)現(xiàn)簡(jiǎn)單,能實(shí)現(xiàn)機(jī)器人在復(fù)雜環(huán)境下的完全遍歷路徑規(guī)劃。
附圖說明
圖1是本發(fā)明方法的步驟流程圖;
圖2是全局坐標(biāo)系與局部坐標(biāo)系示意圖;
圖3是神經(jīng)元連接示意圖;
圖4是基于生物激勵(lì)神經(jīng)網(wǎng)絡(luò)模型的路徑規(guī)劃方法的局部路徑圖;
圖5是圖3對(duì)應(yīng)的活性直方圖;
圖6是本發(fā)明在雙U型障礙物中的生成路徑圖。
具體實(shí)施方式
下面結(jié)合符合和實(shí)例對(duì)本發(fā)明作進(jìn)一步說明。
如圖1為本發(fā)明的基于回溯搜索的生物激勵(lì)完全遍歷路徑規(guī)劃方法的流程圖,包括以下幾個(gè)步驟:
步驟1:構(gòu)建柵格地圖
如圖2所示,以機(jī)器人開始移動(dòng)前的幾何中心所在位置為原點(diǎn),以機(jī)器人開始移動(dòng)前的指向?yàn)閅軸的正方向,順時(shí)針方向旋轉(zhuǎn)90度為X軸的正方向,建立全局坐標(biāo)系G(XGOGYG);以移動(dòng)機(jī)器人的幾何中心為原點(diǎn),機(jī)器人的運(yùn)動(dòng)方向?yàn)閅軸的正方向,順時(shí)針方向旋轉(zhuǎn)90度為X軸的正方向,建立移動(dòng)坐標(biāo)系L(XLOLYL)。
以精度p將全局坐標(biāo)系轉(zhuǎn)化成柵格地圖,每個(gè)柵格的長(zhǎng)和寬分別為Δx=p和Δy=p,所述p為機(jī)器人執(zhí)行器的工作直徑;對(duì)掃地機(jī)器人來(lái)說,p為機(jī)器人底盤的直徑,取值范圍為0.2m到0.4m。
上述柵格坐標(biāo)系的原點(diǎn)與全局坐標(biāo)系的原點(diǎn)重合,將環(huán)境中已知的靜態(tài)障礙物以及移動(dòng)機(jī)器人的當(dāng)前位置對(duì)應(yīng)的柵格坐標(biāo)設(shè)置為:和其中,(xg,yg)表示物體與機(jī)器人在全局坐標(biāo)系G中的坐標(biāo),(xs,ys)表示對(duì)應(yīng)的柵格;移動(dòng)機(jī)器人在全局坐標(biāo)系下的坐標(biāo)為(xr,yr),激光傳感器安裝于機(jī)器人的幾何中心,探測(cè)到前方d處有障礙物,與機(jī)器人X軸方向的夾角為α,移動(dòng)坐標(biāo)系與全局坐標(biāo)系的夾角為θ,則該障礙物在全局坐標(biāo)系下的柵格坐標(biāo)為:對(duì)每個(gè)柵格賦值,-1表示無(wú)障礙柵格,0表示未知柵格,1表示該柵格被障礙物占據(jù);規(guī)定,即使某個(gè)柵格只有部分區(qū)域被障礙物占據(jù),仍賦值為“1”。
步驟2:構(gòu)建節(jié)點(diǎn)隊(duì)列
節(jié)點(diǎn)定義為鄰域內(nèi)有未遍歷的柵格,當(dāng)前柵格鄰域內(nèi)的未遍歷柵格數(shù)量定義為:n為與當(dāng)前柵格相鄰的柵格個(gè)數(shù),g表示柵格狀態(tài);若Ng>0,則將當(dāng)前柵格定義為節(jié)點(diǎn),并加入到節(jié)點(diǎn)隊(duì)列,節(jié)點(diǎn)隊(duì)列保存該節(jié)點(diǎn)的柵格坐標(biāo)以及機(jī)器人經(jīng)過該節(jié)點(diǎn)的時(shí)間。
步驟3:將柵格地圖中的每一個(gè)柵格作為一個(gè)神經(jīng)元,并初始化每個(gè)神經(jīng)元的活性值為0。
步驟4:判斷節(jié)點(diǎn)隊(duì)列是否為空。若節(jié)點(diǎn)隊(duì)列為空,說明已經(jīng)沒有未遍歷節(jié)點(diǎn),結(jié)束遍歷工作;否則需要繼續(xù)遍歷,首先更新節(jié)點(diǎn)隊(duì)列,剔除隊(duì)列中已遍歷的節(jié)點(diǎn),添加新的節(jié)點(diǎn),然后執(zhí)行步驟5。
步驟5:按時(shí)間間隔T讀取激光傳感器采集的障礙物的測(cè)距數(shù)據(jù),并將數(shù)據(jù)轉(zhuǎn)化成柵格坐標(biāo),并將全局坐標(biāo)系的每個(gè)神經(jīng)元(每個(gè)神經(jīng)元對(duì)應(yīng)一個(gè)柵格)的活性值按如下公式更新:
其中,xi表示第i個(gè)神經(jīng)元的狀態(tài),即神經(jīng)元活性;A,B和D是非負(fù)常數(shù),A代表衰減率,B和D分別代表神經(jīng)元狀態(tài)xi的上下限,即xi∈[-D,B];k是與第i個(gè)神經(jīng)元相鄰近的神經(jīng)元個(gè)數(shù);Ii為外部輸入,當(dāng)外部為未清掃區(qū)域時(shí)Ii=E;當(dāng)外部為障礙時(shí)Ii=-E;否則Ii=0。E是一個(gè)很大的正常數(shù)(E>>B),確保了目標(biāo)(未清掃區(qū)域)總是處于神經(jīng)網(wǎng)絡(luò)的波峰,障礙總是處于波谷。和[Ii]-分別表示刺激性和抑制性輸入。第i個(gè)神經(jīng)元和第j個(gè)神經(jīng)元的連接權(quán)值可以定義為ωij=f(|qi-qj|),|qi-qj|代表向量qi和qj之間的歐氏距離.f(a)可以是任意單調(diào)遞減的函數(shù),例如可以定義為:
其中μ和r0都是正常數(shù),r0表示感受域,即機(jī)器人傳感器的探測(cè)半徑。因此,每個(gè)神經(jīng)元只與小范圍區(qū)域[0,r0]內(nèi)的神經(jīng)元有直接聯(lián)系。這里只考慮與當(dāng)前柵格緊鄰的8個(gè)柵格,因此取r0=2,神經(jīng)元連接示意圖如圖3所示,第i個(gè)神經(jīng)元的感受域?yàn)榘霃綖閞0的圓區(qū)域,第i個(gè)神經(jīng)元。
步驟6:路徑規(guī)劃
該步驟首先判斷機(jī)器人在當(dāng)前位置是否陷入死鎖。死鎖定義為當(dāng)前位置的相隔為1的鄰域內(nèi)都為障礙或者已遍歷。若機(jī)器人未陷入死鎖,則采用基于生物激勵(lì)神經(jīng)網(wǎng)絡(luò)模型的路徑規(guī)劃方法決策出下一個(gè)目標(biāo)位置;否則,利用回溯搜索算法與D*算法相結(jié)合的規(guī)劃路徑方法首先從節(jié)點(diǎn)隊(duì)列中找出距離當(dāng)前時(shí)間最近的節(jié)點(diǎn)作為目標(biāo)位置,然后用D*算法規(guī)劃出當(dāng)前位置到目標(biāo)位置的最短路徑,最后控制機(jī)器人移動(dòng)到目標(biāo)位置。返回步驟4。
圖4是由本方法生成的路徑,該地圖由30*23個(gè)柵格組成.機(jī)器人默認(rèn)按照從左到右,從上到下做“弓”字型運(yùn)動(dòng)。機(jī)器人從S(3,2)出發(fā),將經(jīng)過點(diǎn)D(14,2),最終到達(dá)終點(diǎn)E(29,8)。
圖5表示機(jī)器人在D的活性直方圖,活性值最高的區(qū)域表示檢測(cè)到的尚未遍歷的區(qū)域,左邊活性值衰減到接近0的區(qū)域表示已覆蓋的區(qū)域,未知區(qū)域的活性值保持在0。從圖中可以看出該算法在遍歷過程中可將障礙物輪廓構(gòu)建出來(lái)。
圖6是本發(fā)明在雙U型障礙物問題下生成的路徑,圖中,除左下角處的黑色點(diǎn)以外其它的黑色點(diǎn)表示機(jī)器人陷入死鎖,灰色點(diǎn)表示由回溯搜索算法與D*算法生成的路徑。從圖中可以看出,機(jī)器人的行走路徑平滑,轉(zhuǎn)彎少,在陷入死鎖時(shí)能快速找到一條最短路徑逃離到未遍歷區(qū)域。
以上實(shí)施例僅用以說明本發(fā)明的技術(shù)方案而非對(duì)其進(jìn)行限制,本領(lǐng)域的普通技術(shù)人員可以對(duì)本發(fā)明的技術(shù)方案進(jìn)行修改或者等同替換,而不脫離本發(fā)明的精神和范圍,本發(fā)明的保護(hù)范圍應(yīng)以權(quán)利要求書所述為準(zhǔn)。