本發(fā)明涉及移動機器人路徑規(guī)劃技術(shù)領(lǐng)域，尤其涉及一種自適應(yīng)人工勢場與滾動窗口結(jié)合的移動機器人路徑規(guī)劃方法及裝置。

背景技術(shù)：

目前路徑規(guī)劃問題一直是機器人導(dǎo)航與避障的研究熱點。路徑規(guī)劃的目標(biāo)是尋找一條從起始位置點到目標(biāo)位置點的無碰撞運動軌跡。

移動機器人的路徑規(guī)劃可以分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃是建立在機器人的工作環(huán)境已知的基礎(chǔ)上的，局部路徑規(guī)劃適用于移動機器人工作環(huán)境未知或部分已知的應(yīng)用場景。

目前人工勢場法是一種常用的移動機器人局部路徑規(guī)劃方法。人工勢場法由目標(biāo)位置點周圍的引力場和障礙物周圍的斥力場組成。引力場產(chǎn)生引力使機器人朝向目標(biāo)前進，斥力場產(chǎn)生斥力使機器人在移動過程中遠離障礙物，移動機器人朝向引力和斥力的合力方向移動。

雖然人工勢場法表達形式簡單，使用方便高效，但同時也存在一些缺陷：1)易落入局部極小點；2)在狹窄區(qū)域無法通過；3)在障礙物附近震蕩；4)通過狹窄區(qū)域時震蕩；5)目標(biāo)點附近有障礙物時，無法到達目標(biāo)點。針對目標(biāo)點附近有障礙物無法到達目標(biāo)點的問題，目前也提出一種改進的斥力場函數(shù)來解決這個問題。但這種改進方法雖然解決了目標(biāo)點附近有障礙物無法到達目標(biāo)點的問題，但是在某些情況下使得路徑更容易陷入局部極小點，且使規(guī)劃的路徑更長。

技術(shù)實現(xiàn)要素：

本發(fā)明提供一種移動機器人路徑規(guī)劃方法及裝置，用于解決現(xiàn)有技術(shù)中移動機器人路徑規(guī)格不合理的問題。

第一方面，本發(fā)明提供一種移動機器人路徑規(guī)劃方法，包括：

s11、獲取移動機器人的當(dāng)前位置點、目標(biāo)位置點和障礙物區(qū)域信息；

s12、根據(jù)所述當(dāng)前位置點生成局部窗口，所述局部窗口為以所述當(dāng)前位置點為圓心、以預(yù)設(shè)長度為半徑的區(qū)域；

s13、判斷所述目標(biāo)位置點是否位于所述局部窗口內(nèi)，獲得第一判斷結(jié)果；

s14、若第一判斷結(jié)果為否，則根據(jù)所述當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式獲得局部子目標(biāo)位置點；

s15、根據(jù)所述當(dāng)前位置點、局部子目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式獲得所述當(dāng)前位置點到所述局部子目標(biāo)位置點的局部路徑并記錄；

s16、判斷所述局部子目標(biāo)位置點是否為目標(biāo)位置點，獲得第二判斷結(jié)果；

s17、若第二判斷結(jié)果為是，則將獲得的所述局部路徑進行平滑處理后顯示。

可選地，還包括：若第一判斷結(jié)果為是，則根據(jù)所述當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式獲得所述當(dāng)前位置點到目標(biāo)位置點的全局路徑并記錄，并將所述全局路徑進行平滑處理后顯示。

可選地，還包括：若第二判斷結(jié)果為否，則以當(dāng)前獲得的所述局部子目標(biāo)位置點為下一個當(dāng)前位置點，繼續(xù)執(zhí)行上述步驟s12-s16。

可選地，所述根據(jù)所述當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式獲得局部子目標(biāo)位置點，包括：

在所述局部窗口的圓周上等距離選取m個位置點；

從m個位置點中篩選出不與障礙物區(qū)域相交的位置點；

根據(jù)當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式獲得每個篩選出的位置點的對應(yīng)代價值，將代價值最小的位置點選為局部子目標(biāo)位置點；

其中，所述規(guī)劃公式為：

h(n)為選取的位置點的代價值，(xr,yr)為當(dāng)前位置點的坐標(biāo)，(xg,yg)為目標(biāo)位置點的坐標(biāo)，n為m個位置點中的第n個位置點，r為局部窗口的半徑。

可選地，所述根據(jù)所述當(dāng)前位置點、局部子目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式獲得所述當(dāng)前位置點到所述局部子目標(biāo)位置點的局部路徑，包括：

根據(jù)所述當(dāng)前位置點、局部子目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式計算獲得移動機器人的移動方向；

根據(jù)所述移動方向和預(yù)設(shè)步長及預(yù)設(shè)步長次數(shù)獲得局部路徑；

其中，所述路徑搜索公式為：

fatt(q)＝ηρg；

ftotal＝fatt(q)+frep(q)＝fatt(q)nrg+frep1(q)nor+frep2(q)nrg；

其中，

fatt(q)為引力，frep(q)為斥力，ftotal為合力，nor為沿著障礙物指向當(dāng)前位置點的方向，nrg為沿著機器人指向目標(biāo)位置點的方向，η為引力場增益系數(shù)，ρg為當(dāng)前位置點到目標(biāo)位置點的距離，ρb為當(dāng)前位置點到障礙物的距離，k為斥力場增益系數(shù)，a＞0，為預(yù)設(shè)系數(shù)，ρ0為預(yù)設(shè)閾值，frep1(q)為沿著障礙物指向當(dāng)前位置點的方向的斥力，frep2(q)為沿著機器人指向目標(biāo)位置點的方向的斥力。

第二方面，本發(fā)明提供一種移動機器人路徑規(guī)劃裝置，包括：

獲取模塊，用于獲取移動機器人的當(dāng)前位置點、目標(biāo)位置點和障礙物區(qū)域信息；

生成模塊，用于根據(jù)所述當(dāng)前位置點生成局部窗口，所述局部窗口為以所述當(dāng)前位置點為圓心、以預(yù)設(shè)長度為半徑的區(qū)域；

第一判斷模塊，用于判斷所述目標(biāo)位置點是否位于所述局部窗口內(nèi)，獲得第一判斷結(jié)果；

規(guī)劃模塊，用于當(dāng)?shù)谝慌袛嘟Y(jié)果為否，根據(jù)所述當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式獲得局部子目標(biāo)位置點；

搜索模塊，用于根據(jù)所述當(dāng)前位置點、局部子目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式獲得所述當(dāng)前位置點到所述局部子目標(biāo)位置點的局部路徑并記錄；

第二判斷模塊，用于判斷所述局部子目標(biāo)位置點是否為目標(biāo)位置點，獲得第二判斷結(jié)果；

執(zhí)行模塊，用于當(dāng)?shù)诙袛嘟Y(jié)果為是，將獲得的所述局部路徑進行平滑處理后顯示。

可選地，所述執(zhí)行模塊還用于：當(dāng)?shù)谝慌袛嘟Y(jié)果為是，根據(jù)所述當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式獲得所述當(dāng)前位置點到目標(biāo)位置點的全局路徑并記錄，并將所述全局路徑進行平滑處理后顯示。

可選地，所述執(zhí)行模塊還用于：當(dāng)?shù)诙袛嘟Y(jié)果為否，將當(dāng)前獲得的所述局部子目標(biāo)位置點為下一個當(dāng)前位置點，并下一個當(dāng)前位置點發(fā)送給所述生成模塊。

可選地，所述規(guī)劃模塊具體用于：

在所述局部窗口的圓周上等距離選取m個位置點；

從m個位置點中篩選出不與障礙物區(qū)域相交的位置點；

根據(jù)當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式獲得每個篩選出的位置點的對應(yīng)代價值，將代價值最小的位置點選為局部子目標(biāo)位置點；

其中，所述規(guī)劃公式為：

h(n)為選取的位置點的代價值，(xr,yr)為當(dāng)前位置點的坐標(biāo)，(xg,yg)為目標(biāo)位置點的坐標(biāo)，n為m個位置點中的第n個位置點，r為局部窗口的半徑。

可選地，所述搜索模塊具體用于：

根據(jù)所述當(dāng)前位置點、局部子目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式計算獲得移動機器人的移動方向；

根據(jù)所述移動方向和預(yù)設(shè)步長及預(yù)設(shè)步長次數(shù)獲得局部路徑；

其中，所述路徑搜索公式為：

fatt(q)＝ηρg；

ftotal＝fatt(q)+frep(q)＝fatt(q)nrg+frep1(q)nor+frep2(q)nrg；

其中，

fatt(q)為引力，frep(q)為斥力，ftotal為合力，nor為沿著障礙物指向當(dāng)前位置點的方向，nrg為沿著機器人指向目標(biāo)位置點的方向，η為引力場增益系數(shù)，ρg為當(dāng)前位置點到目標(biāo)位置點的距離，ρb為當(dāng)前位置點到障礙物的距離，k為斥力場增益系數(shù)，a＞0，為預(yù)設(shè)系數(shù)，ρ0為預(yù)設(shè)閾值，frep1(q)為沿著障礙物指向當(dāng)前位置點的方向的斥力，frep2(q)為沿著機器人指向目標(biāo)位置點的方向的斥力。

由上述技術(shù)方案可知，本發(fā)明實施例提出一種移動機器人路徑規(guī)劃方法及裝置，通過將人工勢場路徑規(guī)劃分解為多個局部窗口中尋找局部目標(biāo)的過程，以及在移動機器人與目標(biāo)的距離很近時，利用路徑搜索公式可以減弱斥力的作用，使移動機器人到達目標(biāo)點，同時在遠離目標(biāo)時，使移動機器人不易陷入局部極小點，完成合理的路徑規(guī)劃。

附圖說明

圖1為本發(fā)明實施例1提供的移動機器人路徑規(guī)劃方法的流程示意圖；

圖2為本發(fā)明實施例提供的局部子目標(biāo)位置點的獲取步驟的流程示意圖；

圖3為本發(fā)明實施例提供的局部子目標(biāo)位置點的獲取步驟的情景示意圖；

圖4為本發(fā)明實施例提供的路徑平滑處理的情景示意圖；

圖5為本發(fā)明實施例提供的路徑平滑處理的流程示意圖；

圖6為本發(fā)明實施例1提供的移動機器人路徑規(guī)劃方法的完整流程示意圖；

圖7為本發(fā)明實施例2提供的移動機器人路徑規(guī)劃裝置的結(jié)構(gòu)示意圖。

具體實施方式

下面結(jié)合附圖和實施例，對本發(fā)明的具體實施方式作進一步詳細描述。以下實施例用于說明本發(fā)明，但不用來限制本發(fā)明的范圍。

圖1和圖6示出了本發(fā)明實施例1提供一種移動機器人路徑規(guī)劃方法，包括：

s11、獲取移動機器人的當(dāng)前位置點、目標(biāo)位置點和障礙物區(qū)域信息。

在本步驟中，需要說明的是，該當(dāng)前位置點既可指移動機器人開啟移動時的位置點，也可指在路徑規(guī)劃過程中的出現(xiàn)的其他位置點。該目標(biāo)位置點為移動機器人接收到移動指令后要到達的位置點。該障礙物區(qū)域信息可指移動機器人在路徑規(guī)劃過程中遇到的障礙物所占據(jù)區(qū)域。

s12、根據(jù)所述當(dāng)前位置點生成局部窗口，所述局部窗口為以所述當(dāng)前位置點為圓心、以預(yù)設(shè)長度為半徑的區(qū)域。

s13、判斷所述目標(biāo)位置點是否位于所述局部窗口內(nèi)，獲得第一判斷結(jié)果。

s14、若第一判斷結(jié)果為否，則根據(jù)所述當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式獲得局部子目標(biāo)位置點。

在本步驟中，需要說明的是，當(dāng)目標(biāo)位置點不位于局部窗口內(nèi)，可說明移動機器人無法在有限區(qū)域內(nèi)到達目標(biāo)位置點。故系統(tǒng)需根據(jù)當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式得到局部子目標(biāo)位置點。該局部子目標(biāo)位置點為移動機器人在到達目標(biāo)位置點的路徑中經(jīng)過的位置點。

如圖2所示，下面對獲取局部子目標(biāo)位置點的步驟進行解釋說明：

s141、在所述局部窗口的圓周上等距離選取m個位置點；

s142、從m個位置點中篩選出不與障礙物區(qū)域相交的位置點；

s143、根據(jù)當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式獲得每個篩選出的位置點的對應(yīng)代價值，將代價值最小的位置點選為局部子目標(biāo)位置點；

其中，所述規(guī)劃公式為：

h(n)為選取的位置點的代價值，(xr,yr)為當(dāng)前位置點的坐標(biāo)，(xg,yg)為目標(biāo)位置點的坐標(biāo)，n為m個位置點中的第n個位置點，r為局部窗口的半徑。

以具體實例對上述步驟進行解釋說明：

如圖3所示，以當(dāng)前位置點a為圓心，以長度r為半徑生成區(qū)域窗口。在該區(qū)域窗口的圓周上等距離選取8個位置點，分別為1、2、3、4、5、6、7、8。從圖中可以看出，位置點3和4與障礙物區(qū)域相交，故僅根據(jù)當(dāng)前位置點a、目標(biāo)位置點b和預(yù)設(shè)的規(guī)劃公式獲得1、2、5、6、7、8位置點的對應(yīng)代價值。從這些代價值中選取最小代價值所對應(yīng)的位置點為局部子目標(biāo)位置點。

s15、根據(jù)所述當(dāng)前位置點、局部子目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式獲得所述當(dāng)前位置點到所述局部子目標(biāo)位置點的局部路徑并記錄。

在本步驟中，需要說明的是，由于上述步驟獲得局部子目標(biāo)位置點，故首先要計算當(dāng)前位置點到局部子目標(biāo)位置點的路徑。

在本發(fā)明實施例中，首先根據(jù)所述當(dāng)前位置點、局部子目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式計算獲得移動機器人的移動方向；再根據(jù)所述移動方向和預(yù)設(shè)步長及預(yù)設(shè)步長次數(shù)獲得局部路徑。

所述路徑搜索公式為：

fatt(q)＝ηρg；

ftotal＝fatt(q)+frep(q)＝fatt(q)nrg+frep1(q)nor+frep2(q)nrg；

其中，

fatt(q)為引力，frep(q)為斥力，ftotal為合力，nor為沿著障礙物指向當(dāng)前位置點的方向，nrg為沿著機器人指向目標(biāo)位置點的方向，η為引力場增益系數(shù)，ρg為當(dāng)前位置點到目標(biāo)位置點的距離，ρb為當(dāng)前位置點到障礙物的距離，k為斥力場增益系數(shù)，a＞0，為預(yù)設(shè)系數(shù)，ρ0為預(yù)設(shè)閾值，frep1(q)為沿著障礙物指向當(dāng)前位置點的方向的斥力，frep2(q)為沿著機器人指向目標(biāo)位置點的方向的斥力。

由上述函數(shù)公式可以看出，a>0，當(dāng)移動機器人遠離目標(biāo)時，ρg→∞，因此此時可使得在遠處規(guī)劃的路徑短且不易陷入局部極小點。當(dāng)移動機器人靠近目標(biāo)時，因此此時保證移動機器人可以到達目標(biāo)點。因此，上述公式既保證了在遠離目標(biāo)時不改變?nèi)斯輬鏊惴ǖ奶匦?，又可以解決當(dāng)目標(biāo)附近存在障礙物時，目標(biāo)不可達的問題。

s16、判斷所述局部子目標(biāo)位置點是否為目標(biāo)位置點，獲得第二判斷結(jié)果。

s17、若第二判斷結(jié)果為是，則將獲得的所述局部路徑進行平滑處理后顯示。

在本步驟中，需要說明的是，當(dāng)局部子目標(biāo)位置點是目標(biāo)位置點，系統(tǒng)將獲得的局部路徑進行平滑處理后顯示。

當(dāng)局部子目標(biāo)位置點不是目標(biāo)位置點時，系統(tǒng)會繼續(xù)以當(dāng)前獲得的所述局部子目標(biāo)位置點為下一個當(dāng)前位置點，繼續(xù)執(zhí)行上述步驟s12-s16，直到得到的局部位置點為目標(biāo)位置點為止，此時會獲取到多個局部路徑，將多個局部路徑結(jié)合進行平滑處理后顯示，以使移動機器人按照平滑處理后的路徑進行移動，達到避開障礙物的目的。

還需要說明的是，當(dāng)目標(biāo)位置點位于區(qū)域窗口內(nèi)，此時系統(tǒng)根據(jù)所述當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式獲得所述當(dāng)前位置點到目標(biāo)位置點的全局路徑并記錄，并將所述全局路徑進行平滑處理后顯示，使移動機器人按照平滑處理后的路徑進行移動，達到避開障礙物的目的。

下面對路徑的平滑處理進行解釋說明：

如圖4和圖5所示，獲得的路徑由一系列的路徑點組成，p0是路徑的起點(start)，pn是路徑的終點(goal)，為了使路徑平滑且盡可能短，以p0為起點，與其他的路徑點相連，直到與連線與障礙物(obstacle)相交，如p0pt+1，然后取pt為路徑的一個轉(zhuǎn)折點和新的起點，向后繼續(xù)連線，直到到達目標(biāo)點為止，然后將將得到的路徑點連接并進行曲線擬合得到一條光滑的路徑。為了保證移動機器人沿平滑處理后的路徑前進時不與障礙物碰撞，在路徑平滑處理之前先對障礙物進行膨脹處理，保證了路徑的安全性，圖中d為膨脹半徑。

本發(fā)明實施例1提出一種移動機器人路徑規(guī)劃方法，通過將人工勢場路徑規(guī)劃分解為多個局部窗口中尋找局部目標(biāo)的過程，以及在移動機器人與目標(biāo)的距離很近時，利用路徑搜索公式可以減弱斥力的作用，使移動機器人到達目標(biāo)點，同時在遠離目標(biāo)時，使移動機器人不易陷入局部極小點，完成合理的路徑規(guī)劃。

圖7示出了本發(fā)明實施例2提供的一種移動機器人路徑規(guī)劃裝置，包括獲取模塊21、生成模塊22、第一判斷模塊23、規(guī)劃模塊24、搜索模塊25、第二判斷模塊26和執(zhí)行模塊27，其中：

獲取模塊，用于獲取移動機器人的當(dāng)前位置點、目標(biāo)位置點和障礙物區(qū)域信息；

生成模塊，用于根據(jù)所述當(dāng)前位置點生成局部窗口，所述局部窗口為以所述當(dāng)前位置點為圓心、以預(yù)設(shè)長度為半徑的區(qū)域；

第一判斷模塊，用于判斷所述目標(biāo)位置點是否位于所述局部窗口內(nèi)，獲得第一判斷結(jié)果；

規(guī)劃模塊，用于當(dāng)?shù)谝慌袛嘟Y(jié)果為否，根據(jù)所述當(dāng)前位置點、目標(biāo)位置點和預(yù)設(shè)的規(guī)劃公式獲得局部子目標(biāo)位置點；

搜索模塊，用于根據(jù)所述當(dāng)前位置點、局部子目標(biāo)位置點和預(yù)設(shè)的路徑搜索公式獲得所述當(dāng)前位置點到所述局部子目標(biāo)位置點的局部路徑并記錄；

第二判斷模塊，用于判斷所述局部子目標(biāo)位置點是否為目標(biāo)位置點，獲得第二判斷結(jié)果；

執(zhí)行模塊，用于當(dāng)?shù)诙袛嘟Y(jié)果為是，將獲得的所述局部路徑進行平滑處理后顯示。

由于本發(fā)明實施例2所述裝置與上述實施例所述方法的原理相同，對于更加詳細的解釋內(nèi)容在此不再贅述。

需要說明的是，本發(fā)明實施例中可以通過硬件處理器(hardwareprocessor)來實現(xiàn)相關(guān)功能模塊。

本發(fā)明實施例2提出一種移動機器人路徑規(guī)劃裝置，通過將人工勢場路徑規(guī)劃分解為多個局部窗口中尋找局部目標(biāo)的過程，以及在移動機器人與目標(biāo)的距離很近時，利用路徑搜索公式可以減弱斥力的作用，使移動機器人到達目標(biāo)點，同時在遠離目標(biāo)時，使移動機器人不易陷入局部極小點，完成合理的路徑規(guī)劃。

此外，本領(lǐng)域的技術(shù)人員能夠理解，盡管在此所述的一些實施例包括其它實施例中所包括的某些特征而不是其它特征，但是不同實施例的特征的組合意味著處于本發(fā)明的范圍之內(nèi)并且形成不同的實施例。例如，在下面的權(quán)利要求書中，所要求保護的實施例的任意之一都可以以任意的組合方式來使用。

應(yīng)該注意的是上述實施例對本發(fā)明進行說明而不是對本發(fā)明進行限制，并且本領(lǐng)域技術(shù)人員在不脫離所附權(quán)利要求的范圍的情況下可設(shè)計出替換實施例。在權(quán)利要求中，不應(yīng)將位于括號之間的任何參考符號構(gòu)造成對權(quán)利要求的限制。單詞“包含”不排除存在未列在權(quán)利要求中的元件或步驟。位于元件之前的單詞“一”或“一個”不排除存在多個這樣的元件。本發(fā)明可以借助于包括有若干不同元件的硬件以及借助于適當(dāng)編程的計算機來實現(xiàn)。在列舉了若干裝置的單元權(quán)利要求中，這些裝置中的若干個可以是通過同一個硬件項來具體體現(xiàn)。單詞第一、第二、以及第三等的使用不表示任何順序。可將這些單詞解釋為名稱。

本領(lǐng)域普通技術(shù)人員可以理解：以上各實施例僅用以說明本發(fā)明的技術(shù)方案，而非對其限制；盡管參照前述各實施例對本發(fā)明進行了詳細的說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解：其依然可以對前述各實施例所記載的技術(shù)方案進行修改，或者對其中部分或者全部技術(shù)特征進行等同替換；而這些修改或者替換，并不使相應(yīng)技術(shù)方案的本質(zhì)脫離本發(fā)明權(quán)利要求所限定的范圍。