一種基于改進型人工勢場法的機器人路徑規劃方法及系統的制作方法
【技術領域】
[0001] 本發明設及機器人及智能車輛的局部導航領域，特別是一種基于改進型人工勢場 法的機器人路徑規劃方法。
【背景技術】
[0002] 移動機器人實時路徑規劃和導航是反映機器人自主能力的關鍵要素之一，也是較 難解決的問題之一。機器人路徑規劃主要分為環境信息已知的規劃和環境信息未知的規 劃。對于前者多采用離線規劃，得到的路徑較優，后者多采用在線規劃，體現了路徑規劃的 實時性。
[0003] 近年來許多移動機器人路徑規劃的方法被人們所研究。主要的路徑規劃的方法可 W分為兩類:人工智能的方法(AI)和人工勢場法(APF)。前者主要運用的方法有遺傳算法 (GA)、模糊邏輯控制(FLC)和人工神經網絡(ANN)，運些方法往往較為復雜運算速度也較為 緩慢。而后者由于其簡潔性和快速性在機器人路徑規劃中得到廣泛應用，其基本思想是環 境中的目標點對其的吸引力W及障礙物的對其的排斥力構成一種勢場環境。然而人工勢場 法在避障應用的過程中，常常會遇到局部最小的問題。
[0004] 如何避免人工勢場法在避障路徑規劃中出現局部最小的問題，是移動機器人路徑 規劃的關鍵。通過查閱專利和論文，主要有沿墻跟蹤方法、極限環法、虛擬水流法及引入內 部主體狀態法等算法。
[0005] 運些方法雖然在一定程度上緩解了局部最小問題，但都有其各自的缺陷。其中沿 墻跟蹤法和極限環法存在規劃速度慢的問題。虛擬水流法在解決環境已經情況下的局部極 小點問題有一定的效果，但算法效率不高。引入內部主體狀態的方法成功的解決了復雜環 境中的局部最小值問題，但是不能解決通常情況下的靜態勢場問題，通用性不強。
[0006] 由于上述算法存在實時性不好、效率不高、會引入如環境適應能力差等一些新的 問題等缺點。
[0007] 因此，急需一種既具有高效率，又能保證實時性且通用性強的改進的人工勢場法。
【發明內容】
[000引本發明的目的就是提供一種基于改進型人工勢場法的機器人路徑規劃方法及系 統，適用于局部導航中，采用基于勢場法的運動規劃及規避障礙物。
[0009] 本發明的目的是通過運樣的技術方案實現的：
[0010] 本發明提供了一種基于改進型人工勢場法的機器人路徑規劃方法，包括W下步 驟：
[0011] S1:獲取機器人的初始化狀態參數、環境信息和最終目標點；
[0012] S2:獲取機器人當前坐標位置和局部目標點；
[0013] S3:建立基于時間虛擬驅動力的人工勢場法生成機器人當前坐標位置和局部目標 點之間的可達路徑；
[0014] S4:控制機器人沿可達路徑行進；
[0015] S5:在激光雷達可視范圍內檢測機器人當前坐標位置是否達到局部目標點，如果 沒有達到，則返回步驟S4繼續控制驅動機器人行進；
[0016] S6:如果達到局部目標點，則檢測機器人是否達到最終目標點，如果沒有達到，貝U 返回步驟S2;
[0017] S7:如果達到最終目標點，則結束機器人的行進。
[0018] 進一步，所述局部目標點的確定，具體包括W下步驟：
[0019] S21:檢測最終目標點和當前坐標位置之間可通過的直線路徑下有無障礙物，如果 沒有障礙物，則設置最終目標點作為局部目標點；
[0020] S22:如果有障礙物，則判斷障礙物的個數是否小于兩個，如果否，則設置離此障礙 物充分安全距離的任一點最為新坐標點作為局部目標點；
[0021 ] S23:如果是，則建立離最終目標點最近的兩個障礙物呈現的斥力勢場，并構建試 探點尋找斥力場合力零的坐標作為局部目標點。
[0022] 進一步，所述試探點尋找斥力場合力零的坐標具體按照W下步驟來實現：
[0023] S231:按照W下斥力勢函數建立障礙物對機器人造成的人工斥力勢場模型；
[0024]
[0025] 式中；
[0026] δ :相應的正比例位置增益系數；
[0027] Ρ0:正常數，表示障礙物區域可對機器人的運動產生影響的最大距離；
[002引 p(q):某一障礙物區域Cobs到位置q的最小距離，對于所有的q/ eC0bs，P(q)=min| q-q' Μ;
[0029] S232:根據斥力勢函數按照W下公式確定機器人所受的排斥力：
[0030]
[00川式中，用qc表示障礙物區域Cobs上距離q最近的位置點；
[003^ p(q)= I |q-qc| I ;
[0033] ^是由qc指向q的單位向量，
[0034] S233:選取距離全局目標點距離最近的障礙物，并獲取障礙物在平面上呈現的斥 力勢場，
[0035] S234:根據斥力勢場計算排斥力，選取排斥力合力為零的點作為局部目標點。
[0036] 進一步，所述機器人在達到局部目標點之前還包括W下步驟來實現：
[0037] S31:建立機器人與障礙物之間的斥力勢函數并計算斥力：
[003引S32:建立機器人與局部目標點之間的引力勢函數；
[0039]
[0040] 式中；
[0041] ε:人工勢場法引力勢場增益參數；
[0042] 化(q):機器人當前位置距離局部目標點的歐氏距離；
[0043] S33:按照W下公式計算局部目標點對機器人的引力：
[0044]
[0045] S34:通過W下公式來計算機器人的引力和斥力的合力為零時確定局部極小點；
[0046]
[0047] S35:按照W下公式計算時間虛擬驅動力的大小：
[004引
[0049] 式中；
[0050] 丫 ：調節Ftime的常數。
[0051] 本發明提供了一種基于改進型人工勢場法的機器人路徑規劃系統，包括機器人參 數信息采集單元、機器人位置采集單元、局部目標點生成單元、可達路徑生成單元、進行控 制單元、局部目標點判斷單元和最終目標點判斷單元；
[0052] 所述機器人參數信息采集單元，用于獲取機器人的初始化狀態參數、環境信息和 最終目標點；
[0053] 所述機器人位置采集單元，用于獲取機器人當前坐標位置；
[0054] 所述局部目標點生成單元，用于確定機器人行進中的局部目標點；
[0055] 所述可達路徑生成單元，用于計算機器人當前位置和局部目標點之間的可達路 徑；
[0056] 所述進行控制單元，用于控制、驅動機器人沿可達路徑行進；
[0057] 所述局部目標點判斷單元，用于計算并判斷機器人當前坐標位置是否達到局部目 標點；
[0058] 所述最終目標點判斷單元，用于計算并判斷機器人是否達到最終目標點；
[0059] 所述機器人參數信息采集單元和機器人位置采集單元分別與局部目標點生成單 元連接;所述機器人位置采集單元與可達路徑生成單元連接;所述可達路徑生成單元與進 行控制單元連接;所述局部目標點判斷單元和最終目標點判斷單元分別與行進控制單元連 接。
[0060] 進一步，所述局部目標點生成單元是按照W下步驟來進行的：
[0061] S11:檢測最終目標點和當前坐標位置之間可通過的直線路徑下有無障礙物，如果 沒有障礙物，則設置最終目標點作為局部目標點；
[0062] S12:如果有障礙物，則判斷障礙物的個數是否小于兩個，如果否，則設置離此障礙 物充分安全距離的任一點最為新坐標點作為局部目標點；
[0063] S13:如果是，則建立離最終目標點最近的兩個障礙物呈現的斥力勢場，并構建試 探點尋找斥力場合力零的坐標作為局部目標點；
[0064] 所述試探點尋找斥力場合力零的坐標具體按照W下步驟來實現：
[0065] S14:按照W下斥力勢函數建立障礙物對機器人造成的人工斥力勢場模型；
[0066]
[0067]式中；
[006引δ :相應的正比例位置增益系數；
[0069] Ρ0:正常數，表示障礙物區域可對機器人的運動產生影響的最大距離；
[0070] P(q):某一障礙物區域Cobs到位置q的最小距離，對于所有的q/ eC〇bs，P(q)=min| q-q' Μ;
[0071] S15:根據斥力勢函數按照w下公式確定機器人所受的排斥力：
[0072]
[007引式中，用qc表示障礙物區域Cobs上距離q最近的位置點；
[0074] p(q)=||q-qc||;
[0075] 贏是由qc指向q的單位向量，
[0076] S16:選取距離全局目標點距離最近的障礙物，并獲取障礙物在平面上呈現的斥力 勢場，
[0077] S17:根據斥力勢場計算排斥力，選取排斥力合力為零的點作為局部目標點。
[0078] 進一步，還包括與進行控制單元連接的時間虛擬驅動力單元;所述時間虛擬驅動 力單元機是通過W下步驟來實現：
[0079] S41:建立機器人與障礙物之間的斥力勢函數并計算斥力：
[0080] S42:建立機器人與局部目標點之間的引力勢函數；
[0081]
[0082] 式中；
[0083] ε:人工勢場法引力勢場增益參數；
[0084] 化(q):機器人當前位置距離局部目標點的歐氏距離；
[0085] S43:按照W下公式計算局部目標點對機器人的引力：
[0086]
[0087] S44:通過W下公式來計算機器人的引力和斥力的合力為零時確定局部極小點；
[00則玲￡￡ (的=-馬巧(機
[0089] S45:按照W下公式計算時間虛擬驅動力的大小：
[0090] 、
/
[0091] 式中：
[0092] 丫 ：調節Ftime的常數。
[0093] 由于采用了上述技術方案，本發明具有如下的優點：
[0094] 本發明是基于人工勢場法來規劃機器人路徑規劃，解決了傳統勢場法對機器人進 行路徑規劃出現的局部極小點問題，提高了路徑規劃的實時性、環境適應性效率。本發明對 傳統的人工勢場法進行了改進，即改進引力勢函數，同時將整個任務劃分為許多局部目標 點，從而達到最優的路徑。
[0095] 引力勢函數把機器人到達目標的預計時間考慮進去，并將該時間轉化為虛擬的引 力，其一，保證目標點為整個勢場的全局