機(jī)器人靜態(tài)路徑規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種機(jī)器人靜態(tài)路徑規(guī)劃方法，屬于機(jī)器人路徑規(guī)劃技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 自移動(dòng)機(jī)器人的路徑規(guī)劃問題是機(jī)器人領(lǐng)域的核心問題之一，根據(jù)機(jī)器人對(duì)空間 環(huán)境的已知程度，路徑規(guī)劃問題可以被分為靜態(tài)路徑規(guī)劃和動(dòng)態(tài)路徑規(guī)劃。針對(duì)路徑規(guī)劃 問題，曾經(jīng)提出了許多算法，主要包括勢(shì)場(chǎng)法、柵格法和粒子群法。以傳統(tǒng)的人工勢(shì)場(chǎng)法為 例，它采用的是基于矢量合成的方法，通過直觀的規(guī)則即在源于障礙物的排斥力與源于目 標(biāo)點(diǎn)的吸引力的合力作用下規(guī)劃?rùn)C(jī)器人的運(yùn)動(dòng)路徑。在只有局部信息的情況下，它的避障 策略是十分有效的。但由于機(jī)器人運(yùn)動(dòng)的速度以及方向取決于力矢量和的大小和方向，當(dāng) 合力為零時(shí)，機(jī)器人就無法運(yùn)動(dòng)了，從而陷入局部極小點(diǎn)的困境。另外，傳統(tǒng)的人工勢(shì)場(chǎng)法 是根據(jù)機(jī)器人周圍障礙物以及目標(biāo)點(diǎn)對(duì)機(jī)器人的作用求出對(duì)機(jī)器人當(dāng)前位置的合力，然后 根據(jù)合力給出機(jī)器人的運(yùn)動(dòng)方向，調(diào)整機(jī)器人位置。根據(jù)行進(jìn)過程中傳感器不斷返回來的 數(shù)據(jù)，求出地圖中每個(gè)節(jié)點(diǎn)相對(duì)于起點(diǎn)的勢(shì)場(chǎng)值。勢(shì)場(chǎng)值由起始點(diǎn)依相鄰節(jié)點(diǎn)的代價(jià)值依 次遞減，直到目標(biāo)點(diǎn)。障礙物處的勢(shì)場(chǎng)值為無窮大，表明障礙物不可達(dá)。離散勢(shì)場(chǎng)構(gòu)造完畢 后，由勢(shì)場(chǎng)目標(biāo)點(diǎn)開始沿勢(shì)場(chǎng)下降最快方向得到預(yù)規(guī)劃路徑。機(jī)器人沿預(yù)規(guī)劃路徑運(yùn)動(dòng)，直 到環(huán)境信息改變或到達(dá)目標(biāo)點(diǎn)。由于傳統(tǒng)的人工勢(shì)場(chǎng)法采用的是基于矢量合成的方法，它 通過直觀的規(guī)則即在源于障礙物的排斥力與源于目標(biāo)點(diǎn)的吸引力的合力作用下規(guī)劃?rùn)C(jī)器 人的運(yùn)動(dòng)。在只有局部信息的情況下，它的避障策略是十分有效的。但由于機(jī)器人運(yùn)動(dòng)的 速度以及方向取決于力矢量和的大小和方向，當(dāng)合力為零時(shí)，機(jī)器人就無法運(yùn)動(dòng)了，這就是 采用人工勢(shì)場(chǎng)法易陷入局部極小點(diǎn)的原因。對(duì)于粒子群算法，在初始階段，粒子的行走路徑 較分散，路徑的收斂速度較慢，影響路徑規(guī)劃的效率。因此，不同的算法都具有各自的優(yōu)勢(shì)， 但也有其方法上的局限性。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明所要解決的技術(shù)問題在于針對(duì)現(xiàn)有技術(shù)的不足，提供一種機(jī)器人靜態(tài)路徑 規(guī)劃方法，將勢(shì)場(chǎng)法、柵格法和粒子群法結(jié)合起來，直接求取柵格地圖上的勢(shì)場(chǎng)分布，勢(shì)場(chǎng) 值由起始點(diǎn)依相鄰節(jié)點(diǎn)的代價(jià)值依次遞增，直到目標(biāo)點(diǎn)，由勢(shì)場(chǎng)目標(biāo)點(diǎn)開始沿勢(shì)場(chǎng)下降最 快方向得到預(yù)規(guī)劃路徑，安全有效且路徑規(guī)劃準(zhǔn)確可靠。
[0004] 本發(fā)明所要解決的技術(shù)問題是通過如下技術(shù)方案實(shí)現(xiàn)的：
[0005] -種機(jī)器人靜態(tài)路徑規(guī)劃方法，該方法包括如下步驟：
[0006] 步驟100 :設(shè)定目標(biāo)點(diǎn)，以目標(biāo)點(diǎn)為終點(diǎn)，在地圖范圍內(nèi)建立人工勢(shì)場(chǎng)；
[0007] 步驟200 :引入粒子群算法，在機(jī)器人的起點(diǎn)設(shè)有數(shù)量為m的粒子群，第i個(gè)粒子 在第t步的飛行速度為?·, (/)，按照人工勢(shì)場(chǎng)并結(jié)合粒子群算法對(duì)每個(gè)粒子從起點(diǎn)到終點(diǎn)的 路徑進(jìn)行模擬行走，在模擬行走的過程中，每個(gè)粒子形成各自的運(yùn)動(dòng)軌跡；
[0008] 步驟300 :大部分粒子逐漸向多條軌跡中的一條軌跡聚攏收斂，進(jìn)而在地圖范圍 內(nèi)得到從起點(diǎn)到終點(diǎn)的最優(yōu)行走路徑；
[0009] 步驟400 :機(jī)器人最終按照最優(yōu)行走路徑，完成從起點(diǎn)到終點(diǎn)的運(yùn)動(dòng)過程。
[0010] 更具體地，所述步驟200中人工勢(shì)場(chǎng)并結(jié)合粒子群算法對(duì)每個(gè)粒子從起點(diǎn)到終點(diǎn) 的路徑進(jìn)行模擬行走，具體包括：
[0011] 當(dāng)?shù)趇個(gè)粒子行走到第t步時(shí)，根據(jù)人工勢(shì)場(chǎng)法，由該勢(shì)場(chǎng)生成的對(duì)粒子i的作用 力匕⑴：取匕⑴的單位方向向量為
[0012] 根據(jù)粒子群算法得到此時(shí)粒子i的飛行速度Γν?小取1@；)的單位方向向量為 Vll (0 ；
[0013] 結(jié)合人工勢(shì)場(chǎng)和粒子群算法，對(duì)粒子i的飛行速度進(jìn)行調(diào)整，定義調(diào)整后飛行速 度⑴=(?) + ι#2?· (?)，其中m和η為常數(shù)系數(shù)；
[0014] 在粒子群算法中以調(diào)整后的飛行速度R⑴替代飛行速度。
[0015] 所述地圖為柵格地圖，所述步驟200中人工勢(shì)場(chǎng)并結(jié)合粒子群算法對(duì)每個(gè)粒子從 起點(diǎn)到終點(diǎn)的路徑進(jìn)行模擬行走，還包括：將調(diào)整后的飛行速度&(0的方向修正為朝向與 其最接近的柵格中心的方向，定義修正后飛行速度為Ρ',.?/；)，在粒子群算法中以修正后飛 行速度Ρ^)替代調(diào)整后的飛行速度&
[0016] 所述步驟200中人工勢(shì)場(chǎng)并結(jié)合粒子群算法對(duì)每個(gè)粒子從起點(diǎn)到終點(diǎn)的路徑進(jìn) 行模擬行走，具體包括：
[0017] 當(dāng)?shù)趇個(gè)粒子行走到第t步時(shí)，根據(jù)人工勢(shì)場(chǎng)法，由該勢(shì)場(chǎng)生成的對(duì)粒子i的作用 力的單位方向向量為瓦(>);
[0018] 根據(jù)粒子群算法得到此時(shí)粒子i的飛行速度取的單位方向向量為 fait).;
[0019] 結(jié)合人工勢(shì)場(chǎng)和粒子群算法，對(duì)粒子i的飛行速度進(jìn)行調(diào)整，定義調(diào)整后飛行速 度?7, = 丨十/?r:,_(V)，其中m和η為常數(shù)系數(shù)；
[0020] 所述地圖為柵格地圖，所述步驟200中人工勢(shì)場(chǎng)并結(jié)合粒子群算法對(duì)每個(gè)粒子從 起點(diǎn)到終點(diǎn)的路徑進(jìn)行模擬行走，還包括：將調(diào)整后的飛行速度巧(>)的方向修正為朝向與 其最接近的柵格中心的方向，定義修正后飛行速度為在粒子群算法中以修正后飛 行速度P⑷替代飛行速度:^⑴。
[0021 ] 所述柵格地圖的創(chuàng)建方法，具體包括：
[0022] 步驟A :將地圖劃分為許多單元格形成初始柵格地圖，把含有障礙物的單元格定 義為在先障礙物區(qū)域X;
[0023] 步驟B :以初始柵格地圖中柵格的頂點(diǎn)為中心輻射至單元格大小，形成新的柵格 地圖，其中所述頂點(diǎn)成為新的柵格地圖單元格的中心點(diǎn)；
[0024] 步驟C :在新的柵格地圖中，把含有在先障礙物區(qū)域X的單元格定義為最終障礙物 區(qū)域Z。
[0025] 所述步驟A具體為：
[0026] 所述的柵格化地圖具體包括，將地圖劃分為許多單元格形成初始柵格地圖，并定 義自移動(dòng)機(jī)器人在初始柵格地圖中的起始位置，自移動(dòng)機(jī)器人在行走的同時(shí)檢測(cè)行走區(qū)域 內(nèi)是否存在障礙物，若判斷存在障礙物，則在初始柵格地圖中把障礙物所對(duì)應(yīng)的單元格定 義為在先障礙物區(qū)域X。
[0027] 所述步驟100中的人工勢(shì)場(chǎng)具體包括：
[0028] 引力勢(shì)函數(shù)為：
[0029]
[0030] 其中，kg為位置增益系數(shù)，X_Xg為粒子X與目標(biāo)點(diǎn)Xg之間的距離；
[0031] 斥力勢(shì)函數(shù)為：
[0032]
[0033] 其中，k。為位置增益系數(shù)，P為粒子X與障礙物之間的最短距離，P。為一常數(shù)，代 表障礙物的影響距離；
[0034] 總勢(shì)場(chǎng)函數(shù)為：
[0035]
[0036] 當(dāng)粒子距離障礙物越遠(yuǎn)時(shí)，Urap(X)越小，當(dāng)距離目標(biāo)點(diǎn)越近時(shí)，Ug(X)越小，因此， 在啟發(fā)函數(shù)下，粒子尋找U (X)下降最快的方向，逐漸的遠(yuǎn)離障礙物而趨近目標(biāo)點(diǎn)。
[0037] 所述步驟200中的粒子群算法具體包括：
[0038] 假設(shè)在一個(gè)D維的目標(biāo)搜索空間中，有m個(gè)粒子組成一個(gè)群落，其中第i個(gè)粒子表 示為一個(gè)D維的向量無…··b>W = L2···"2，g卩：第i個(gè)粒子在〇維的搜索空間中的位 置是\
[0039] 將:馬帶入目標(biāo)函數(shù)計(jì)算出其適應(yīng)值，根據(jù)適應(yīng)值的大小衡量?,的優(yōu)劣；
[0040] 第i個(gè)粒子的飛行速度也是一個(gè)D維的向量，記為％ 記第i個(gè)粒 子迄今為止搜索到的最優(yōu)位置為尼M/V/h，…，Avd ;由此可以得出整個(gè)粒子群迄今為止 搜索到的最優(yōu)位置為艮；
[0041] 到第t步時(shí)，粒子根據(jù)下面的公式更新自己的速度和位置：
[0042]
[0043]
[0044] 其中，Cl和c2為學(xué)習(xí)因子，也稱加速常數(shù)；
[0045] A和r2為（0, 1)范圍內(nèi)的均勻隨機(jī)數(shù)；
[0046]