一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃方法
【專利摘要】本發(fā)明涉及一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃方法，該方法在柵格地圖中使用，該方法包括：根據(jù)外部環(huán)境信息進(jìn)行二值分類并建立柵格地圖，根據(jù)基于速度的人工勢場法進(jìn)行外層路徑規(guī)劃，在路徑即將陷入局部極小和震蕩時，根據(jù)A星算法進(jìn)行基于內(nèi)層路徑規(guī)劃。本發(fā)明解決了傳統(tǒng)人工勢場法可能陷入局部極小和規(guī)劃路徑振蕩的問題，解決了A星算法不適用于實時路徑規(guī)劃的問題，保證了移動機(jī)器人的實時、安全和穩(wěn)定運行。
【專利說明】
-種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于移動機(jī)器人路徑規(guī)劃領(lǐng)域，設(shè)及一種基于柵格地圖的移動機(jī)器人實時 分層路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 移動機(jī)器人的路徑規(guī)劃是指在含有障礙物的工作空間中，按照一定的評價指標(biāo)， 捜索一條可W完成指定任務(wù)的移動路徑。移動機(jī)器人工作環(huán)境的描述方式主要包括柵格地 圖、拓?fù)涞貓D、幾何地圖與W上幾種地圖的混合地圖。柵格地圖一般指W大小相等或不等的 方形柵格將外部環(huán)境進(jìn)行劃分，根據(jù)柵格區(qū)域是否被障礙物占據(jù)將柵格進(jìn)行二值劃分。運 種建模方式便于計算機(jī)存儲與批量處理，因為是目前最為被廣泛應(yīng)用的地圖建立方式。
[0003] 移動機(jī)器人的路徑規(guī)劃主要可W分為單元分解法和勢場法等。
[0004] 單元分解法主要是將空間分解為一系列可W重新進(jìn)行分類的單元，并對不同單元 空間賦予特征信息W表征實際環(huán)境特征，通過對不同單元的信息的進(jìn)行分析得到符合規(guī)劃 需求的最終路徑。路徑規(guī)劃的需求跟所選取的具體方法不同而不同，例如A星算法、 Di Astra算法等捜索最優(yōu)或次優(yōu)路徑的方法，W及他們的改進(jìn)方法。但實際應(yīng)用中，由于A 星算法等單元分解法需要頻繁的對內(nèi)存進(jìn)行讀寫，在地圖較大時算法實時性差，常用于離 線路徑規(guī)劃。
[0005] 人工勢場法是另一種常用路徑規(guī)劃方法，它借物理學(xué)中的場論來解決機(jī)器人學(xué)中 的路徑規(guī)劃問題。該類方法最早由Khatib于1986年提出，現(xiàn)在已經(jīng)被廣泛應(yīng)用于移動機(jī)器 人的路徑規(guī)劃與實時避障。但傳統(tǒng)人工勢場法主要針對靜態(tài)情況，且規(guī)劃的路徑可能出現(xiàn) 局部極小或震蕩的情形導(dǎo)致路徑不適用于機(jī)器人運動或者規(guī)劃失敗。有很多學(xué)者針對W上 問題進(jìn)行了針對性的改進(jìn)，如添加新的代價函數(shù)，如速度、慣性和能量等等，但仍不能完全 解決問題。
[0006] 文獻(xiàn)《基于柵格-幾何混合地圖的移動機(jī)器人分層路徑規(guī)劃》針對機(jī)器人規(guī)劃問 題，建立了柵格-幾何混合地圖，在柵格地圖下進(jìn)行了改進(jìn)A星算法的上層路徑規(guī)劃，在幾何 地圖下進(jìn)行基于慣性人工勢場法的底層路徑規(guī)劃，通過慣性人工勢場法對改進(jìn)A星算法生 成的軌跡進(jìn)行修正。該方法針對靜態(tài)環(huán)境建立的混合地圖在實際機(jī)器人控制系統(tǒng)中實現(xiàn)較 為復(fù)雜，且每次規(guī)劃過程中改進(jìn)A星算法與慣性人工勢場二者都要運行一次后并進(jìn)行路徑 融合，無法保證移動機(jī)器人的實時運行，不能應(yīng)對動態(tài)變化場景。
[0007] 已授權(quán)專利《一種基于障礙物運動預(yù)測的移動機(jī)器人避障方法》闡述了一種基于 速度的人工勢場法的機(jī)器人避障方法，并使用IMM方法進(jìn)行障礙物運動預(yù)測。該方法重點研 究了移動機(jī)器人避障，但沒有解決用人工勢場法進(jìn)行路徑規(guī)劃帶來的局部極小或震蕩問 題，且該方法并不是全局路徑的規(guī)劃方法，不適用于移動機(jī)器人實時路徑規(guī)劃。

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

[0008] 本發(fā)明的主要目的是利用柵格地圖便于計算機(jī)建立、儲存和分析的優(yōu)點，使用基 于速度的人工勢場法進(jìn)行外層路徑規(guī)劃，保留勢場法實時性和簡潔性，當(dāng)勢場法的規(guī)劃路 徑發(fā)生震蕩或遇到局部極小問題時，切換到A星算法進(jìn)行內(nèi)層路徑規(guī)劃，并及時切回，最大 限度避免移動機(jī)器人路徑規(guī)劃失敗，從而保證機(jī)器人的實時、安全和穩(wěn)定運行。
[0009] -種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃方法，該方法包括W下步驟： 步驟一：明確機(jī)器人路徑規(guī)劃的目標(biāo)，并進(jìn)行初始化:機(jī)器人搭載的計算機(jī)通過采集設(shè) 備建立并實時更新包含外部環(huán)境信息的柵格地圖，確定規(guī)劃的起點和目標(biāo)點，應(yīng)用基于速 度的人工勢場法和A星算法進(jìn)行分層路徑規(guī)劃，保證機(jī)器人實時、安全和穩(wěn)定地運行到目標(biāo) 占. 步驟二:進(jìn)行基于速度的人工勢場法外層路徑規(guī)劃:計算出基于速度的人工勢場下機(jī) 器人所受的合力矢量，該合力矢量在機(jī)器人規(guī)劃的路徑不處于震蕩和局部極小時，直接指 導(dǎo)機(jī)器人運動，并儲存在該矢量指導(dǎo)下生成規(guī)劃路徑。合力矢量方程為：
其中巧為引力增益系數(shù);與1楚，:穿g)表示一個矢量，矢量的大小為當(dāng)前位置f與目標(biāo)點 位置鮮之間的歐幾里得距離矢量的方向為皆與％兩個位置連線上從機(jī)器人位置 指向目標(biāo)點的位置;其中I為斥力增益函數(shù)；錢為正常數(shù)，表示障礙物對移動機(jī)器人產(chǎn)生影 響的最大距離；滬沾9技-'>表示一個矢量，矢量大小為當(dāng)前位置點密與障礙物點喪&的歐幾里 得距離松-狼，矢量方向為障礙物位置指向機(jī)器人位置；其中ifgf:為可調(diào)的速度常量， C為機(jī)器人與障礙物的相對運動速度，揖為節(jié)。與品的夾角，京為機(jī)器人所在點指向障 礙物點的向量； 步驟=:進(jìn)行基于A星算法的內(nèi)層路徑規(guī)劃:在機(jī)器人規(guī)劃的路徑即將處于震蕩和局部 極小時，路徑規(guī)劃方法切換到A星算法，W當(dāng)前點為起點，選取臨時的目標(biāo)點，規(guī)劃出到臨時 目標(biāo)點的路徑，轉(zhuǎn)到步驟四；如果臨時目標(biāo)點同時是整個規(guī)劃周期的目標(biāo)點，則到達(dá)目標(biāo)點 后，轉(zhuǎn)到步驟五； 步驟四：內(nèi)層和外層的規(guī)劃路徑合并后，從A星算法的臨時目標(biāo)點開始進(jìn)行基于速度的 人工勢場法外層路徑規(guī)劃，即回到步驟二； 步驟五:規(guī)劃周期結(jié)束，將所有路徑合并后，命令機(jī)器人按照規(guī)劃路徑運行，同時如果 需要重新開始規(guī)劃周期就回到步驟一，重新啟動路徑規(guī)劃流程，直到機(jī)器人停止運動。
【附圖說明】
[0010] 圖1為一種只用基于速度的人工勢場法由于局部極小而規(guī)劃失敗的情況。
[0011] 圖2為基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃法的概要流程圖。
[0012] 圖3外層基于速度的人工勢場法路徑規(guī)劃流程圖。
[OOU]圖4內(nèi)層基于的A星路徑規(guī)劃流程圖。
[0014] 圖5為機(jī)器人在基于速度的人工勢場法下受力分析圖。
[0015] 圖6為邊長10個單位長的柵格地圖內(nèi)A星路徑規(guī)劃方法的路徑。
[0016] 圖7為邊長10個單位長的柵格地圖內(nèi)分層路徑規(guī)劃方法的路徑。
[0017] 圖8為邊長25個單位長的柵格地圖內(nèi)A星路徑規(guī)劃方法的路徑。
[0018] 圖9為邊長25個單位長的柵格地圖內(nèi)分層路徑規(guī)劃方法的路徑。
[0019] 圖10為邊長50個單位長的柵格地圖內(nèi)A星路徑規(guī)劃方法的路徑。
[0020] 圖11為邊長50個單位長的柵格地圖內(nèi)分層路徑規(guī)劃方法的路徑。
[0021] 圖12W上=種情況分層路徑規(guī)劃方法與A星規(guī)劃方法的規(guī)劃時間對比圖。
[0022] 其中，在方形網(wǎng)狀柵格地圖中起始點在左下角用圓圈表示，目標(biāo)點在右上角用五 角星表示，障礙物所在柵格用黑色填充，障礙物膨脹柵格用斑點填充，最終規(guī)劃得到的路徑 柵格用斜網(wǎng)格線填充。
【具體實施方式】
【具體實施方式】 [0023] 一：明確機(jī)器人路徑規(guī)劃的目標(biāo)，并進(jìn)行初始化:移動機(jī)器人搭載的 計算機(jī)通過采集設(shè)備建立并實時更新包含外部環(huán)境信息的柵格地圖，確定規(guī)劃的起點和目 標(biāo)點，應(yīng)用基于速度的人工勢場法和A星算法進(jìn)行分層路徑規(guī)劃，保證機(jī)器人實時、安全和 穩(wěn)定地運行到目標(biāo)點。
[0024]
【具體實施方式】二:本實施是對一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃 方法作進(jìn)一步說明，當(dāng)柵格地圖規(guī)模等于資*滿時，A*路徑規(guī)劃方法的計算復(fù)雜度可達(dá) 海輯戶^。如果將該環(huán)境模型中的柵格粒度變?yōu)樵瓉淼亩种唬瑒t柵格規(guī)模將變?yōu)?2孤巧萊，而計算量則會增至巧。因此盡管A*可W保證找到一條最優(yōu)路徑，但是并不能 保證移動機(jī)器人實時運動。
[0025] 盡管基于速度的人工勢場法由于其算法的簡潔性更適用于實時性的路徑規(guī)劃，但 局部極小現(xiàn)象和可能出現(xiàn)的震蕩都會引發(fā)機(jī)器人運行不穩(wěn)定。由于勢場的本質(zhì)，如果參數(shù) 選擇不當(dāng)，機(jī)器人有忽視物理上的接觸，僅視作場的相互作用，機(jī)器人有過于接近障礙并引 發(fā)危險的可能，如圖1為一種只用基于速度的人工勢場法或常規(guī)人工勢場法由于局部極小 而規(guī)劃失敗的情況。
[0026] 因此本發(fā)明設(shè)計了實時切換的分層軌跡規(guī)劃策略，圖2為其概要流程說明。在地圖 信息柵格化完成之后，會設(shè)置好當(dāng)前規(guī)劃起點與目標(biāo)點，進(jìn)行外層基于速度的人工勢場運 動規(guī)劃，圖3為外層路徑規(guī)劃流程，當(dāng)判斷到規(guī)劃路徑機(jī)器人將處于局部極小、震蕩等狀況 的時候，切換到內(nèi)層A星算法路徑規(guī)劃，圖4為外層路徑規(guī)劃流程。
[0027] 【具體實施方式】本實施是對一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃 方法作進(jìn)一步說明，本實施方式中，在外層基于速度的人工市場法路徑規(guī)劃過程中，移動機(jī) 器人受到的引力勢能與目標(biāo)點的歐幾里得距離呈反比關(guān)系，當(dāng)引力勢能為零時，移動機(jī)器 人到達(dá)目標(biāo)點。引力勢能函數(shù)表示為：
其中巧為引力增益系數(shù)；：巧細(xì)讀P表示一個矢量，矢量的大小為當(dāng)前位置t與目標(biāo) 點位置起之間的歐幾里得距離矢量的方向為3與fs兩個位置連線上從機(jī)器人位 置指向目標(biāo)點的位置。
[00%]則由引力勢場產(chǎn)生的引力為：
弓巧方向與的黎《f)的方向相同。
[0029] 移動機(jī)器人收到的斥力勢能與引力勢能與目標(biāo)點的歐幾里得距離也呈反比關(guān)系。 當(dāng)斥力勢能為零時，機(jī)器人已經(jīng)脫離障礙物的影響范圍，本發(fā)明設(shè)定為錢:。斥力勢能函數(shù) 表示為：
其中裘為斥力增益函數(shù)；爲(wèi)為正常數(shù)，表示障礙物對移動機(jī)器人產(chǎn)生影響的最大距離； 滅祭叛表示一個矢量，矢量大小為當(dāng)前位置點察與障礙物點截:&的歐幾里得距離|g- ，矢量方向為障礙物位置指向機(jī)器人位置。
[0030] 相應(yīng)的斥力方程為：
[0031] 除了可能遇到的震蕩、局部極小問題，傳統(tǒng)人工勢場適用于靜態(tài)環(huán)境的路徑規(guī)劃。 而在動態(tài)和不確定性的環(huán)境中，機(jī)器人攜帶空間測繪設(shè)備由于自身所處位置受限，無法預(yù) 先知道下一周規(guī)劃周期尚未測繪到障礙，從而降低機(jī)器人避障的能力。因此本發(fā)明采用了 基于速度的人工勢場法，提高機(jī)器人在在實時環(huán)境中對動態(tài)環(huán)境變化適應(yīng)性。
[0032] 動態(tài)變化主要來自于空間障礙，需要對斥力勢能￡^蘇|進(jìn)行改進(jìn)。
[0033] 機(jī)器人與障礙物之間的相對速度勢場：
* 其中:胃為可調(diào)的速度常量，I';。.為機(jī)器人與障礙物的相對運動速度，聲為:?。與 苗的夾角，；馬為機(jī)器人所在點指向障礙物點的向量，當(dāng)琢封r|讓,即障礙物不向機(jī)器人 運動，或機(jī)器人不在障礙影響范圍內(nèi)時，則速度斥力勢能可忽略。
[0034] 速度斥力為：
[0035] 加入速度因素機(jī)器人收到的總斥力勢能為：
[00;
[0037] 根據(jù)上述的引力勢能、斥力勢能，可W得到機(jī)器人在某一位置所受到的全局勢能。 全局勢能函數(shù)為：
[0038] 在考慮速度因素后，機(jī)器人最終所受的合力為：
[0039] 圖5為對移動機(jī)器人的受力分析。
[0040] 【具體實施方式】四：本實施是對一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃 方法作進(jìn)一步說明，本實施方式中，內(nèi)層規(guī)劃所采用的A星算法路徑規(guī)劃方法啟發(fā)式捜索算 法，它根據(jù)局部信息不斷地逼近目標(biāo)位置，最終得到有效路徑，其評估公式如公式：
其中讀據(jù)表示從初始結(jié)點到任意結(jié)點心的代價，楚據(jù)表示從任意結(jié)點孩到目標(biāo)點的 啟發(fā)式評估代價。
[0041] 恰當(dāng)?shù)膯l(fā)函數(shù)會提高A星算法效率，且路徑更優(yōu)。本發(fā)明中的A星算法認(rèn)為機(jī)器 人可W沿著柵格的對角線移動，考慮到機(jī)器人對角線移動的需要轉(zhuǎn)彎等操作，那么假設(shè)一 個位置水平或垂直移動到鄰近位置的最小代價致，對角線移動的代價設(shè)為衣帶毋。啟發(fā)函 數(shù)中移動步數(shù)表示為：
其中敎為沿著斜線可W移動的步數(shù)，其中旅代表當(dāng)前節(jié)點；演^杳！為曼哈頓距 離，即X，Y方向上所有沿直線移動的步數(shù)之和。
[0042] 最終的啟發(fā)函數(shù)為：

【具體實施方式】 [0043] 四：本實施方式是對一種基于柵格地圖的移動機(jī)器人實時分層路徑 規(guī)劃方法作進(jìn)一步說明，在進(jìn)行一個完整的路徑規(guī)劃周期結(jié)束之后，機(jī)器人會按照規(guī)劃好 的路徑進(jìn)行運動，同時更新外部環(huán)境的柵格地圖，直到新的路徑規(guī)劃周期結(jié)束后機(jī)器人獲 得新的規(guī)劃路徑。其每個規(guī)劃周期內(nèi)完整的規(guī)劃流程如下。
[0044] 1、地圖預(yù)處理過程： 首先進(jìn)行當(dāng)前規(guī)劃周期中的地圖數(shù)據(jù)更新，建立存儲空間所有柵格運行代價的數(shù)組 設(shè)定障礙點的運行代價為4,非障礙點運行代價為1。然后進(jìn)行膨脹運算， 設(shè)定包圍障礙物的柵格區(qū)域運行代價也為4。地圖預(yù)處理之后，建立用于存儲當(dāng)前周期全局 規(guī)劃路徑的數(shù)組爹，開始進(jìn)行路徑規(guī)劃。
[0045] 2、外層路徑規(guī)劃流程： 外層規(guī)劃流程選用實時性好的基于速度的人工勢場法： Step 1設(shè)定引力增益系數(shù)幫、斥力增益系數(shù)愛，障礙物影響半徑和單步步長志，運 四個參數(shù)要根據(jù)實際情況變化； Step 2判斷當(dāng)前位置是否到達(dá)當(dāng)前周期內(nèi)規(guī)劃路徑的目標(biāo)點免,如果到達(dá)就 跳轉(zhuǎn)到Step 9,如果未到達(dá)則執(zhí)行下一步； Step 3判斷當(dāng)前規(guī)劃路徑是否有局部極小、震蕩的問題，如果沒有則繼續(xù)執(zhí)行下一 步，否則切換到內(nèi)層A星算法路徑規(guī)劃； Step 4計算機(jī)器人當(dāng)前位置與目標(biāo)之間的角度％，進(jìn)而計算出引力 對機(jī)器人在X，Y方向上的分力稽遜和％ ; Step 5計算機(jī)器人當(dāng)前位置姨,每與各障礙點|；考_藏菜論；!之間的每個夾角％懲， 計算出斥力對機(jī)器人在X，Y方向上的分力和^ ; Step 6計算機(jī)器人當(dāng)前與各障礙點相對速度馬。，計算速度斥力對機(jī)器人在X，Y方向 上的分力戶:-卿，卿'; step 7計算機(jī)器人在X，Y方向上受到的合力.巧胃和巧癡?，并計算合力與X軸的夾角 沒S邸; step 8根據(jù)步長王、馬^和當(dāng)前機(jī)器人位置，計算并取整得出下一步的位置，聚）， 將坐標(biāo)存儲到F，并回到Step2; Step 9輸出最終全局路徑夢，結(jié)束當(dāng)前規(guī)劃周期。
[0046] 3、內(nèi)層路徑規(guī)劃流程： 本發(fā)明的內(nèi)層A星算法中最為重要的部分是對臨時目標(biāo)點的選取。本發(fā)明假設(shè)在規(guī)劃 周期開始時，移動機(jī)器人通過采集可用于地圖構(gòu)建的傳感器的信息，如激光雷達(dá)、攝像頭 等，可W實時更新所處環(huán)境的柵格化地圖，使機(jī)器人有足夠的依據(jù)進(jìn)行臨時目標(biāo)點的選?。?Step 1建立數(shù)組輯趣.和擔(dān)聳，保存代價待考察的節(jié)點，Gd保存已經(jīng)考察過其代價 的節(jié)點，建立數(shù)組馬姆i保存節(jié)點的代價，建立保存局部規(guī)劃路徑，將切換到內(nèi)層規(guī)劃時 的規(guī)劃位置(不存入錢3，作為內(nèi)層A星規(guī)劃的起點； Step 2建立局部柵格地圖并選取臨時目標(biāo)點。在當(dāng)前的規(guī)劃周期內(nèi)，在當(dāng)前位置 揖貧加 ，1|誠)與目標(biāo)操gj貧所連的直線上選取一臨時目標(biāo)點（馬_放^;_放），當(dāng)前位置與局 部目標(biāo)點構(gòu)成了新的正方形局部柵格地圖，其邊長每^根據(jù)障礙物分布密度進(jìn)行選取，且 馬足W下公式：
判斷全局目標(biāo){菜^=?)是否處于局部地圖內(nèi)，如果存在則將全局目標(biāo)點坐標(biāo)設(shè)為臨時 目標(biāo)點，如果選取的臨時目標(biāo)點不可達(dá)，如選取在障礙柵格或膨脹柵格上，需要同時將臨時 目標(biāo)點坐標(biāo)和增加一個步長，再判斷臨時目標(biāo)點是否可達(dá)，如果不可達(dá)則繼續(xù)增加，直 到其可達(dá)或者全局目標(biāo)點被包絡(luò)進(jìn)來； Step 3根據(jù)當(dāng)前起始點和臨時目標(biāo)點，進(jìn)行A星路徑規(guī)劃，并得到內(nèi)層規(guī)劃路徑1?; Step 4將內(nèi)部路徑?!、:添加到全局路徑P，結(jié)束內(nèi)層A星路徑規(guī)劃，切換回到外層規(guī)劃 流程。
[0047] 【具體實施方式】五:本實施方式是對一種基于柵格地圖的移動機(jī)器人實時分層路徑 規(guī)劃方法作進(jìn)一步說明，本實施方式中為了測試本方法的實時性，將其與只使用A星路徑規(guī) 劃方法的情況進(jìn)行了對比。在障礙物為5個且對移動機(jī)器人產(chǎn)生影響的時候，在尺寸為10， 25和50個單位長的地圖中進(jìn)行了 MATLAB仿真實驗，其中柵格地圖尺寸為10個單位長時不進(jìn) 行膨脹運算，分層規(guī)劃有關(guān)參數(shù)為濟(jì)=0.5，:繁=200, A=3單位長，Z=I單位長，￡^=3單位 長，誇=1單位長/s,圖6~圖11為W上路徑規(guī)劃的結(jié)果圖，圖12為W上路徑規(guī)劃時間統(tǒng)計結(jié) 果對比圖。
【主權(quán)項】
1. 一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃方法，其特征在于，該方法包括 以下步驟： 步驟一:地圖預(yù)處理過程:開始新的規(guī)劃周期，進(jìn)行當(dāng)前規(guī)劃周期中的柵格地圖數(shù)據(jù)更 新，給定所有柵格的運行代價，進(jìn)行膨脹運算，設(shè)定當(dāng)前規(guī)劃周期的起點和目標(biāo)點； 步驟二:基于速度的人工勢場法外層路徑規(guī)劃過程:設(shè)定基于速度的人工勢場法參數(shù)， 根據(jù)引力勢能、斥力勢能所產(chǎn)生合力矢量的方程計算移動機(jī)器人所受到的合力矢量，合力 矢量方程為：該合力矢量直接指導(dǎo)移動機(jī)器人運行，并對所規(guī)劃路徑的狀態(tài)進(jìn)行循環(huán)檢測，當(dāng)規(guī)劃 路徑即將處于局部極小和震蕩狀態(tài)時且未達(dá)到全局目標(biāo)點時，轉(zhuǎn)到步驟三；當(dāng)機(jī)器人到達(dá) 目標(biāo)點時，轉(zhuǎn)到步驟四； 步驟三:基于A星算法的內(nèi)層路徑規(guī)劃過程:暫停外層基于速度的人工勢場法路徑規(guī)劃 過程，選取臨時目標(biāo)點并切換到A星算法進(jìn)行機(jī)器人路徑規(guī)劃；當(dāng)規(guī)劃路徑不再處于局部極 小和震蕩狀態(tài)且未到達(dá)臨時目標(biāo)點時，轉(zhuǎn)到步驟二；當(dāng)機(jī)器人到達(dá)全局目標(biāo)點時，轉(zhuǎn)到步驟 四； 步驟四：路徑規(guī)劃結(jié)果輸出：合并內(nèi)外層規(guī)劃路徑，將此規(guī)劃周期生產(chǎn)的路徑保存并讓 機(jī)器人按照規(guī)劃路徑移動；如果不滿足停止路徑規(guī)劃條件，回到步驟一，否則停止路徑規(guī) 劃，機(jī)器人也停止運動。2. 根據(jù)權(quán)利要求1所述的一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃方法，其 特征在于步驟一所述的停止路徑規(guī)劃條件為機(jī)器人到達(dá)了全局的目標(biāo)或人為干預(yù)、程序要 求或機(jī)器人處于危險狀態(tài)，使得機(jī)器人停止運動； 步驟一所述的柵格地圖為正方形，由許多單元柵格構(gòu)成，可分為障礙柵格，非障礙柵格 和膨脹柵格;其中障礙柵格的運行代價為較大值，非障礙柵格運行代價為較低值，膨脹運算 使圍繞障礙物一圈的非障礙柵格為膨脹柵格，其運行代價與障礙柵格相同，但不對機(jī)器人 產(chǎn)生勢場。3. 根據(jù)權(quán)利要求1所述的一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃方法，其 特征在于步驟二所述的引力勢能方程為：其中-為引力增益系數(shù);#(1?)表示一個矢量，矢量的大小為當(dāng)前位置f與目標(biāo)點位 置^之間的歐幾里得距離|risi，矢量的方向為f與％兩個位置連線上從機(jī)器人位置指 向目標(biāo)點的位置； 引力勢能產(chǎn)生的引力方程為：步驟二所述的總斥力勢能由障礙物產(chǎn)生的斥力勢能和機(jī)器人與障礙物相對速度產(chǎn)生 的斥力勢能共同構(gòu)成，總斥力勢能中障礙物產(chǎn)生的斥力勢能方程為：其中I為斥力增益函數(shù)；島為正常數(shù)，表示障礙物對移動機(jī)器人產(chǎn)生影響的最大距離； 表示一個矢量，矢量大小為當(dāng)前位置點(6:與障礙物點的歐幾里得距離 ，矢量方向為障礙物位置指向機(jī)器人位置； 總斥力勢能中機(jī)器人與障礙物相對速度產(chǎn)生的斥力勢能方程為：其中養(yǎng)_:為可調(diào)的速度常量，尾為機(jī)器人與障礙物的相對運動速度，$為％與的 夾角，為機(jī)器人所在點指向障礙物點的向量，當(dāng)〃_-||f即障礙物不向機(jī)器人運動， 或機(jī)器人不在障礙影響范圍內(nèi)時，則速度斥力勢能可忽略； 總斥力勢能方程為：總斥力勢能產(chǎn)生的總斥力方程為：除了可能遇到的路徑局部極小和震蕩問題，傳統(tǒng)人工勢場適用于靜態(tài)環(huán)境的路徑規(guī) 劃;而在動態(tài)和不確定性的環(huán)境中，機(jī)器人攜帶空間測繪設(shè)備由于自身所處位置受限，無法 預(yù)先知道下一周規(guī)劃周期尚未測繪到障礙，從而降低了機(jī)器人動態(tài)避障的能力；因此本文 采用了基于速度的人工勢場法，提高機(jī)器人在在實時環(huán)境中對動態(tài)環(huán)境變化的適應(yīng)性。4.根據(jù)權(quán)利要求1所述的一種基于柵格地圖的移動機(jī)器人實時分層路徑規(guī)劃方法，步 驟三中臨時目標(biāo)點選取的方法為： 在當(dāng)前的規(guī)劃周期內(nèi)，在切換到A星規(guī)劃時移動機(jī)器人的當(dāng)前位置與目標(biāo)所連的直線 上選取臨時目標(biāo)點，當(dāng)前位置與臨時目標(biāo)點包含在局部新的正方形柵格地圖內(nèi)，其邊長根 據(jù)障礙物分布密度進(jìn)行選取，且邊長1滿足以下公式：判斷臨時目標(biāo)點是否處于局部地圖內(nèi)，如果存在則將全局目標(biāo)點坐標(biāo)設(shè)為臨時目標(biāo) 點，如果選取的臨時目標(biāo)點不可達(dá)，在則同時將臨時目標(biāo)點坐標(biāo)和增加一個步長，判斷 臨時目標(biāo)點是否可達(dá)，如果不可達(dá)則￡&繼續(xù)增加一個步長并重新判斷，直到可達(dá)或者全局 目標(biāo)點被包絡(luò)進(jìn)來，如果全局目標(biāo)被包絡(luò)進(jìn)來，則直接將其設(shè)為臨時目標(biāo)點，隨后按照A星 算法對機(jī)器人進(jìn)行路徑規(guī)劃。
【文檔編號】G05D1/02GK105955262SQ201610299576
【公開日】2016年9月21日
【申請日】2016年5月9日
【發(fā)明人】尤波, 李智, 丁亮, 高海波, 許家忠, 李東潔, 王明睿, 張樂超
【申請人】哈爾濱理工大學(xué)