一種基于改進(jìn)型人工勢場法的機(jī)器人路徑規(guī)劃方法及系統(tǒng)的制作方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及機(jī)器人及智能車輛的局部導(dǎo)航領(lǐng)域，特別是一種基于改進(jìn)型人工勢場 法的機(jī)器人路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 移動機(jī)器人實時路徑規(guī)劃和導(dǎo)航是反映機(jī)器人自主能力的關(guān)鍵要素之一，也是較 難解決的問題之一。機(jī)器人路徑規(guī)劃主要分為環(huán)境信息已知的規(guī)劃和環(huán)境信息未知的規(guī) 劃。對于前者多采用離線規(guī)劃，得到的路徑較優(yōu)，后者多采用在線規(guī)劃，體現(xiàn)了路徑規(guī)劃的 實時性。
[0003] 近年來許多移動機(jī)器人路徑規(guī)劃的方法被人們所研究。主要的路徑規(guī)劃的方法可 W分為兩類:人工智能的方法(AI)和人工勢場法(APF)。前者主要運用的方法有遺傳算法 (GA)、模糊邏輯控制(FLC)和人工神經(jīng)網(wǎng)絡(luò)(ANN)，運些方法往往較為復(fù)雜運算速度也較為 緩慢。而后者由于其簡潔性和快速性在機(jī)器人路徑規(guī)劃中得到廣泛應(yīng)用，其基本思想是環(huán) 境中的目標(biāo)點對其的吸引力W及障礙物的對其的排斥力構(gòu)成一種勢場環(huán)境。然而人工勢場 法在避障應(yīng)用的過程中，常常會遇到局部最小的問題。
[0004] 如何避免人工勢場法在避障路徑規(guī)劃中出現(xiàn)局部最小的問題，是移動機(jī)器人路徑 規(guī)劃的關(guān)鍵。通過查閱專利和論文，主要有沿墻跟蹤方法、極限環(huán)法、虛擬水流法及引入內(nèi) 部主體狀態(tài)法等算法。
[0005] 運些方法雖然在一定程度上緩解了局部最小問題，但都有其各自的缺陷。其中沿 墻跟蹤法和極限環(huán)法存在規(guī)劃速度慢的問題。虛擬水流法在解決環(huán)境已經(jīng)情況下的局部極 小點問題有一定的效果，但算法效率不高。引入內(nèi)部主體狀態(tài)的方法成功的解決了復(fù)雜環(huán) 境中的局部最小值問題，但是不能解決通常情況下的靜態(tài)勢場問題，通用性不強(qiáng)。
[0006] 由于上述算法存在實時性不好、效率不高、會引入如環(huán)境適應(yīng)能力差等一些新的 問題等缺點。
[0007] 因此，急需一種既具有高效率，又能保證實時性且通用性強(qiáng)的改進(jìn)的人工勢場法。

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

[000引本發(fā)明的目的就是提供一種基于改進(jìn)型人工勢場法的機(jī)器人路徑規(guī)劃方法及系 統(tǒng)，適用于局部導(dǎo)航中，采用基于勢場法的運動規(guī)劃及規(guī)避障礙物。
[0009] 本發(fā)明的目的是通過運樣的技術(shù)方案實現(xiàn)的：
[0010] 本發(fā)明提供了一種基于改進(jìn)型人工勢場法的機(jī)器人路徑規(guī)劃方法，包括W下步 驟：
[0011] S1:獲取機(jī)器人的初始化狀態(tài)參數(shù)、環(huán)境信息和最終目標(biāo)點；
[0012] S2:獲取機(jī)器人當(dāng)前坐標(biāo)位置和局部目標(biāo)點；
[0013] S3:建立基于時間虛擬驅(qū)動力的人工勢場法生成機(jī)器人當(dāng)前坐標(biāo)位置和局部目標(biāo) 點之間的可達(dá)路徑；
[0014] S4:控制機(jī)器人沿可達(dá)路徑行進(jìn)；
[0015] S5:在激光雷達(dá)可視范圍內(nèi)檢測機(jī)器人當(dāng)前坐標(biāo)位置是否達(dá)到局部目標(biāo)點，如果 沒有達(dá)到，則返回步驟S4繼續(xù)控制驅(qū)動機(jī)器人行進(jìn)；
[0016] S6:如果達(dá)到局部目標(biāo)點，則檢測機(jī)器人是否達(dá)到最終目標(biāo)點，如果沒有達(dá)到，貝U 返回步驟S2;
[0017] S7:如果達(dá)到最終目標(biāo)點，則結(jié)束機(jī)器人的行進(jìn)。
[0018] 進(jìn)一步，所述局部目標(biāo)點的確定，具體包括W下步驟：
[0019] S21:檢測最終目標(biāo)點和當(dāng)前坐標(biāo)位置之間可通過的直線路徑下有無障礙物，如果 沒有障礙物，則設(shè)置最終目標(biāo)點作為局部目標(biāo)點；
[0020] S22:如果有障礙物，則判斷障礙物的個數(shù)是否小于兩個，如果否，則設(shè)置離此障礙 物充分安全距離的任一點最為新坐標(biāo)點作為局部目標(biāo)點；
[0021 ] S23:如果是，則建立離最終目標(biāo)點最近的兩個障礙物呈現(xiàn)的斥力勢場，并構(gòu)建試 探點尋找斥力場合力零的坐標(biāo)作為局部目標(biāo)點。
[0022] 進(jìn)一步，所述試探點尋找斥力場合力零的坐標(biāo)具體按照W下步驟來實現(xiàn)：
[0023] S231:按照W下斥力勢函數(shù)建立障礙物對機(jī)器人造成的人工斥力勢場模型；
[0024]
[0025] 式中；
[0026] δ :相應(yīng)的正比例位置增益系數(shù)；
[0027] Ρ0:正常數(shù)，表示障礙物區(qū)域可對機(jī)器人的運動產(chǎn)生影響的最大距離；
[002引 p(q):某一障礙物區(qū)域Cobs到位置q的最小距離，對于所有的q/ eC0bs，P(q)=min| q-q' Μ;
[0029] S232:根據(jù)斥力勢函數(shù)按照W下公式確定機(jī)器人所受的排斥力：
[0030]
[00川式中，用qc表示障礙物區(qū)域Cobs上距離q最近的位置點；
[003^ p(q)= I |q-qc| I ;
[0033] ^是由qc指向q的單位向量，
[0034] S233:選取距離全局目標(biāo)點距離最近的障礙物，并獲取障礙物在平面上呈現(xiàn)的斥 力勢場，
[0035] S234:根據(jù)斥力勢場計算排斥力，選取排斥力合力為零的點作為局部目標(biāo)點。
[0036] 進(jìn)一步，所述機(jī)器人在達(dá)到局部目標(biāo)點之前還包括W下步驟來實現(xiàn)：
[0037] S31:建立機(jī)器人與障礙物之間的斥力勢函數(shù)并計算斥力：
[003引S32:建立機(jī)器人與局部目標(biāo)點之間的引力勢函數(shù)；
[0039]
[0040] 式中；
[0041] ε:人工勢場法引力勢場增益參數(shù)；
[0042] 化(q):機(jī)器人當(dāng)前位置距離局部目標(biāo)點的歐氏距離；
[0043] S33:按照W下公式計算局部目標(biāo)點對機(jī)器人的引力：
[0044]
[0045] S34:通過W下公式來計算機(jī)器人的引力和斥力的合力為零時確定局部極小點；
[0046]
[0047] S35:按照W下公式計算時間虛擬驅(qū)動力的大?。?br>[004引
[0049] 式中；
[0050] 丫 ：調(diào)節(jié)Ftime的常數(shù)。
[0051] 本發(fā)明提供了一種基于改進(jìn)型人工勢場法的機(jī)器人路徑規(guī)劃系統(tǒng)，包括機(jī)器人參 數(shù)信息采集單元、機(jī)器人位置采集單元、局部目標(biāo)點生成單元、可達(dá)路徑生成單元、進(jìn)行控 制單元、局部目標(biāo)點判斷單元和最終目標(biāo)點判斷單元；
[0052] 所述機(jī)器人參數(shù)信息采集單元，用于獲取機(jī)器人的初始化狀態(tài)參數(shù)、環(huán)境信息和 最終目標(biāo)點；
[0053] 所述機(jī)器人位置采集單元，用于獲取機(jī)器人當(dāng)前坐標(biāo)位置；
[0054] 所述局部目標(biāo)點生成單元，用于確定機(jī)器人行進(jìn)中的局部目標(biāo)點；
[0055] 所述可達(dá)路徑生成單元，用于計算機(jī)器人當(dāng)前位置和局部目標(biāo)點之間的可達(dá)路 徑；
[0056] 所述進(jìn)行控制單元，用于控制、驅(qū)動機(jī)器人沿可達(dá)路徑行進(jìn)；
[0057] 所述局部目標(biāo)點判斷單元，用于計算并判斷機(jī)器人當(dāng)前坐標(biāo)位置是否達(dá)到局部目 標(biāo)點；
[0058] 所述最終目標(biāo)點判斷單元，用于計算并判斷機(jī)器人是否達(dá)到最終目標(biāo)點；
[0059] 所述機(jī)器人參數(shù)信息采集單元和機(jī)器人位置采集單元分別與局部目標(biāo)點生成單 元連接;所述機(jī)器人位置采集單元與可達(dá)路徑生成單元連接;所述可達(dá)路徑生成單元與進(jìn) 行控制單元連接;所述局部目標(biāo)點判斷單元和最終目標(biāo)點判斷單元分別與行進(jìn)控制單元連 接。
[0060] 進(jìn)一步，所述局部目標(biāo)點生成單元是按照W下步驟來進(jìn)行的：
[0061] S11:檢測最終目標(biāo)點和當(dāng)前坐標(biāo)位置之間可通過的直線路徑下有無障礙物，如果 沒有障礙物，則設(shè)置最終目標(biāo)點作為局部目標(biāo)點；
[0062] S12:如果有障礙物，則判斷障礙物的個數(shù)是否小于兩個，如果否，則設(shè)置離此障礙 物充分安全距離的任一點最為新坐標(biāo)點作為局部目標(biāo)點；
[0063] S13:如果是，則建立離最終目標(biāo)點最近的兩個障礙物呈現(xiàn)的斥力勢場，并構(gòu)建試 探點尋找斥力場合力零的坐標(biāo)作為局部目標(biāo)點；
[0064] 所述試探點尋找斥力場合力零的坐標(biāo)具體按照W下步驟來實現(xiàn)：
[0065] S14:按照W下斥力勢函數(shù)建立障礙物對機(jī)器人造成的人工斥力勢場模型；
[0066]
[0067]式中；
[006引δ :相應(yīng)的正比例位置增益系數(shù)；
[0069] Ρ0:正常數(shù)，表示障礙物區(qū)域可對機(jī)器人的運動產(chǎn)生影響的最大距離；
[0070] P(q):某一障礙物區(qū)域Cobs到位置q的最小距離，對于所有的q/ eC〇bs，P(q)=min| q-q' Μ;
[0071] S15:根據(jù)斥力勢函數(shù)按照w下公式確定機(jī)器人所受的排斥力：
[0072]
[007引式中，用qc表示障礙物區(qū)域Cobs上距離q最近的位置點；
[0074] p(q)=||q-qc||;
[0075] 贏是由qc指向q的單位向量，
[0076] S16:選取距離全局目標(biāo)點距離最近的障礙物，并獲取障礙物在平面上呈現(xiàn)的斥力 勢場，
[0077] S17:根據(jù)斥力勢場計算排斥力，選取排斥力合力為零的點作為局部目標(biāo)點。
[0078] 進(jìn)一步，還包括與進(jìn)行控制單元連接的時間虛擬驅(qū)動力單元;所述時間虛擬驅(qū)動 力單元機(jī)是通過W下步驟來實現(xiàn)：
[0079] S41:建立機(jī)器人與障礙物之間的斥力勢函數(shù)并計算斥力：
[0080] S42:建立機(jī)器人與局部目標(biāo)點之間的引力勢函數(shù)；
[0081]
[0082] 式中；
[0083] ε:人工勢場法引力勢場增益參數(shù)；
[0084] 化(q):機(jī)器人當(dāng)前位置距離局部目標(biāo)點的歐氏距離；
[0085] S43:按照W下公式計算局部目標(biāo)點對機(jī)器人的引力：
[0086]
[0087] S44:通過W下公式來計算機(jī)器人的引力和斥力的合力為零時確定局部極小點；
[00則玲￡￡ (的=-馬巧(機(jī)
[0089] S45:按照W下公式計算時間虛擬驅(qū)動力的大?。?br>[0090] 、
/
[0091] 式中：
[0092] 丫 ：調(diào)節(jié)Ftime的常數(shù)。
[0093] 由于采用了上述技術(shù)方案，本發(fā)明具有如下的優(yōu)點：
[0094] 本發(fā)明是基于人工勢場法來規(guī)劃機(jī)器人路徑規(guī)劃，解決了傳統(tǒng)勢場法對機(jī)器人進(jìn) 行路徑規(guī)劃出現(xiàn)的局部極小點問題，提高了路徑規(guī)劃的實時性、環(huán)境適應(yīng)性效率。本發(fā)明對 傳統(tǒng)的人工勢場法進(jìn)行了改進(jìn)，即改進(jìn)引力勢函數(shù)，同時將整個任務(wù)劃分為許多局部目標(biāo) 點，從而達(dá)到最優(yōu)的路徑。
[0095] 引力勢函數(shù)把機(jī)器人到達(dá)目標(biāo)的預(yù)計時間考慮進(jìn)去，并將該時間轉(zhuǎn)化為虛擬的引 力，其一，保證目標(biāo)點為整個勢場的全局