一種基于距離網(wǎng)格地圖的移動(dòng)機(jī)器人路徑規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及移動(dòng)機(jī)器人路徑規(guī)劃領(lǐng)域，具體是一種基于距離網(wǎng)格地圖的移動(dòng)機(jī)器 人路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 在移動(dòng)機(jī)器人短短幾十年的發(fā)展過程中，避障及路徑規(guī)劃算法一直是學(xué)者們研究 的熱口問題，該問題也是智能移動(dòng)移動(dòng)機(jī)器人自主能力的關(guān)鍵問題之一。如何在未知不確 定環(huán)境中無碰撞的進(jìn)行路徑規(guī)劃是高度自主化的行為，每年在該方向的論文與算法都非常 多。目前一般采取全局路徑規(guī)劃和局部避障規(guī)避相結(jié)合的方法實(shí)現(xiàn)移動(dòng)移動(dòng)機(jī)器人的自 主導(dǎo)航。其中比較流行的避障算法有基于邊緣檢測法巧dge-Detection Method)、勢場法 (Potential Field Methods)、虛擬力場法（Virtual Force Field Method)、矢量場直方圖 法（Vector Field Histogram Method) W及后續(xù)改進(jìn)的VFH+、V即等方法，該些方法都是局 部避障算法，除了 VFH*算法在對(duì)后續(xù)路徑的預(yù)測加入A*捜索來進(jìn)行路徑選擇優(yōu)化外，其余 算法都不對(duì)全局的路徑進(jìn)行規(guī)劃。
[0003] 距離網(wǎng)格地圖表示的是單元格與障礙距離的最小值，通常用于移動(dòng)移動(dòng)機(jī)器人的 避障預(yù)警、非完備移動(dòng)移動(dòng)機(jī)器人的路徑規(guī)劃W及定位等問題。在靜止的環(huán)境中，距離網(wǎng)格 地圖變動(dòng)不大，所需要更新維護(hù)的計(jì)算成本幾乎為零。然而，移動(dòng)移動(dòng)機(jī)器人所處的環(huán)境往 往是一個(gè)動(dòng)態(tài)的環(huán)境，障礙物的變動(dòng)，存在其他移動(dòng)物體，甚至是傳感器噪聲，都需要在改 變概率網(wǎng)格地圖的基礎(chǔ)上來改變距離網(wǎng)格地圖。
[0004] 傳統(tǒng)的基于距離網(wǎng)格地圖的路徑規(guī)劃方法存在的缺陷：
[000引 （1)傳統(tǒng)距離網(wǎng)格地圖更新耗費(fèi)計(jì)算量大，地圖維護(hù)占用時(shí)間長，難W動(dòng)態(tài)更新；
[0006] (2)路徑規(guī)劃時(shí)使用啟發(fā)式捜索算法，在捜索時(shí)由于不考慮移動(dòng)移動(dòng)機(jī)器人的運(yùn) 動(dòng)限制和移動(dòng)機(jī)器人自身的尺寸，使得所規(guī)劃路徑在局部與障礙物距離過小、在部分轉(zhuǎn)向 之處（路徑拐點(diǎn)）為銳角，無法直接使用規(guī)劃出的路徑來控制移動(dòng)移動(dòng)機(jī)器人的運(yùn)動(dòng)。

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

[0007] 本發(fā)明的目的在于提供一種基于距離網(wǎng)格地圖的移動(dòng)機(jī)器人路徑規(guī)劃方法，解決 了基于距離網(wǎng)格地圖的地圖動(dòng)態(tài)更新管理問題和未知地圖下的地圖構(gòu)建及路徑規(guī)劃問題。 [000引為實(shí)現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：
[0009] 一種基于距離網(wǎng)格地圖的移動(dòng)機(jī)器人路徑規(guī)劃方法，包含W下步驟：
[0010] 步驟1;設(shè)定初始概率網(wǎng)格地圖和距離網(wǎng)格地圖為空地圖；控制移動(dòng)機(jī)器人在目 標(biāo)區(qū)域內(nèi)移動(dòng)，并通過傳感器采集目標(biāo)區(qū)域的地形信息，構(gòu)建W移動(dòng)機(jī)器人為中屯、的活動(dòng) 窗口，所述的活動(dòng)窗口對(duì)應(yīng)存儲(chǔ)在移動(dòng)機(jī)器人內(nèi)的初始距離網(wǎng)格地圖中的一個(gè)子區(qū)域，選 取與活動(dòng)窗口對(duì)應(yīng)的目標(biāo)區(qū)域中的部分地形信息對(duì)所述的子區(qū)域進(jìn)行更新，直至活動(dòng)窗口 遍歷整個(gè)初始距離網(wǎng)格地圖，得到更新后的距離網(wǎng)格地圖；
[001U 步驟2 ;在更新后的距離網(wǎng)格地圖上，確定移動(dòng)機(jī)器人的起點(diǎn)和終點(diǎn)，將每個(gè)網(wǎng)格 的中屯、作為節(jié)點(diǎn)，采用A*捜索算法并根據(jù)移動(dòng)機(jī)器人在每個(gè)節(jié)點(diǎn)的姿態(tài)角進(jìn)行節(jié)點(diǎn)展開， 確定移動(dòng)機(jī)器人由起點(diǎn)運(yùn)動(dòng)至終點(diǎn)所有經(jīng)過的節(jié)點(diǎn)，連線起點(diǎn)、所有經(jīng)過的節(jié)點(diǎn)和終點(diǎn)，規(guī) 劃出移動(dòng)機(jī)器人的路徑。
[0012] 作為本發(fā)明進(jìn)一步的方案；步驟1中，距離網(wǎng)格地圖更新過程中的活動(dòng)窗口和前 沿網(wǎng)格的選擇方法；
[0013] 為了分散距離網(wǎng)格地圖的更新計(jì)算量，對(duì)距離網(wǎng)格地圖的更新計(jì)算被限制在了活 動(dòng)窗口之內(nèi)，活動(dòng)窗口過大，會(huì)導(dǎo)致重復(fù)的、不必要的更新隊(duì)列插入操作，而活動(dòng)窗口選擇 過小，更新向量生成器所產(chǎn)生的某個(gè)更新向量（X，y，d)的更新坐標(biāo)x，y超出活動(dòng)窗口的范 圍，由此將會(huì)使得對(duì)應(yīng)距離網(wǎng)格地圖上沒有體現(xiàn)數(shù)據(jù)的最近更新，導(dǎo)致距離網(wǎng)格地圖與概 率網(wǎng)格地圖的不一致。
[0014] 一般來講在使用聲納等不精確數(shù)據(jù)時(shí)，數(shù)據(jù)值越大，即意味著離機(jī)器人越遠(yuǎn)，其可 靠度越差，所W更新向量生成器在使用聲納數(shù)據(jù)時(shí)，將會(huì)接受一定范圍內(nèi)的數(shù)據(jù)，超過最遠(yuǎn) 距離Sm"的數(shù)據(jù)將會(huì)被丟棄，W保證數(shù)據(jù)的可靠性。所W在活動(dòng)窗口的高和寬應(yīng)該滿足下 式：
[0015]
[0016] 式中；馬為活動(dòng)窗口的高，W,為活動(dòng)窗口的寬，8。"為保持聲響數(shù)據(jù)可靠的最遠(yuǎn)距 離，INTO為取整操作。
[0017] 由于移動(dòng)機(jī)器人的數(shù)據(jù)獲取與控制并非連續(xù)的過程，而是一系列離散過程所組成 的。傳感器數(shù)據(jù)更新間隔一個(gè)固定的時(shí)間At,，在一個(gè)或者幾個(gè)At,內(nèi)完成移動(dòng)機(jī)器人的 地圖構(gòu)建、導(dǎo)航計(jì)算等工作，所W雖然在現(xiàn)實(shí)中，宏觀上移動(dòng)機(jī)器人的位置是連續(xù)變化的過 程，但是對(duì)于導(dǎo)航計(jì)算的算法程序而言，改變是離散的、跳變的，無論移動(dòng)機(jī)器人沿著哪一 條路徑由一點(diǎn)到達(dá)另一點(diǎn)，所引起的變化對(duì)局部的距離網(wǎng)格地圖而言，首先需要根據(jù)活動(dòng) 窗口的移動(dòng)情況，計(jì)算出需要進(jìn)行距離網(wǎng)格地圖更新計(jì)算的"前沿"（待更新距離網(wǎng)格），然 后再采用隊(duì)列方法進(jìn)行地圖更新。
[0018] 作為本發(fā)明進(jìn)一步的方案；步驟1中，將時(shí)間為t-1時(shí)移動(dòng)機(jī)器人的中屯、坐標(biāo)記為 (V yp)，當(dāng)前計(jì)算更新時(shí)間為t時(shí)的移動(dòng)機(jī)器人中屯、坐標(biāo)記為（Xr，y,)，移動(dòng)機(jī)器人坐標(biāo)變 化量為（A X，A y)，A i和A j分別為活動(dòng)窗口中屯、坐標(biāo)行列序號(hào)的變化量，為整數(shù)，距離網(wǎng) 格單元格的邊長為S。，W單元格數(shù)目來表示的活動(dòng)窗口的寬和高分別為W。、H。。
[0019] A X和A y按照下式計(jì)算；
[0020]
[0021] Ai和A j按照下式計(jì)算；
[0022]
[0023] 分為W下四種情況計(jì)算"前沿"網(wǎng)格的行列序號(hào)范圍：
[0024] (1)如果Ai、A j全為0,則不進(jìn)行活動(dòng)窗口刷新計(jì)算；
[0025] (2)如果Ai = 0, A j聲0,則不存在縱向"前沿"網(wǎng)格，僅需計(jì)算出橫向"前沿" 網(wǎng)格的序號(hào)范圍；
[0026] (3)如果Ai聲0, A j = 0,則不存在橫向"前沿"網(wǎng)格，僅需計(jì)算出縱向"前沿" 網(wǎng)格的序號(hào)范圍；
[0027] (4)如果A i、A j均非零值，則需要分別計(jì)算出橫向和縱向兩個(gè)方向上的"前沿" 網(wǎng)格的序號(hào)范圍。
[002引作為本發(fā)明進(jìn)一步的方案；步驟1中，距離網(wǎng)格地圖障礙物增加及刪除時(shí)的距離 網(wǎng)格地圖動(dòng)態(tài)更新方法；
[0029] 在障礙增加時(shí)對(duì)距離網(wǎng)格地圖的更新需要設(shè)定一個(gè)障礙增加更新隊(duì)列如，其中存 放的是距離網(wǎng)格地圖更新單元，更新單元包含單元格的行列序號(hào)（i，j)。當(dāng)距離網(wǎng)格地圖中 的某個(gè)單元格被更新時(shí)，根據(jù)該單元格的信息生成對(duì)應(yīng)的增加更新單元，并將其加入障礙 增加更新隊(duì)列如。障礙增加更新算法從障礙增加更新隊(duì)列如取出一個(gè)更新單元，并將其 從障礙增加更新隊(duì)列如中刪除，根據(jù)增加更新單元提供的行列序號(hào)（i，j)，嘗試更新（i，j) 周圍的八個(gè)單元格，并將新生成的更新單元加入障礙增加更新隊(duì)列如。只要障礙增加更新 隊(duì)列如不為空，障礙增加更新算法將一直繼續(xù)下去；
[0030] 在進(jìn)行清除時(shí)同樣需要使用一個(gè)障礙清除更新隊(duì)列Qe，根據(jù)被清除的單元格信息 生成清除更新單元，并將其加入障礙清除更新隊(duì)列Qe，障礙清除更新算法從障礙清除更新 隊(duì)列Qe中取出一個(gè)清除更新單元，按照與障礙增加更新算法類似的迭代方式，對(duì)所有與清 除更新單元的單元格處相關(guān)聯(lián)的單元格進(jìn)行清除操作，直到到達(dá)與之不相關(guān)的單元格，之 后按照"邊緣"的信息生成更新單元，并將其加入障礙增加更新隊(duì)列如，并調(diào)用障礙增加更 新算法對(duì)距離網(wǎng)格地圖進(jìn)行更新。
[0031] 作為本發(fā)明進(jìn)一步的方案；步驟1中，距離網(wǎng)格地圖更新隊(duì)列的優(yōu)化方法。
[0032] 在實(shí)際應(yīng)用中，距離傳感器數(shù)據(jù)的更新往往是一組多個(gè)，會(huì)導(dǎo)致概率網(wǎng)格地圖Me 中數(shù)個(gè)單元產(chǎn)生超過或低于闊值Tc的變化，該種變化使得對(duì)應(yīng)的距離網(wǎng)格地圖進(jìn)行更新 操作，體現(xiàn)在障礙增加更新隊(duì)列如上，就是隊(duì)列中同時(shí)加入了數(shù)個(gè)元素，隨著更新操作的 進(jìn)行，一些重復(fù)的元素將被添加進(jìn)來。因此，在設(shè)計(jì)更新隊(duì)列排序函數(shù)的時(shí)候需要考慮位置 參數(shù)，同時(shí)需要納入考慮的是更新的層級(jí)Lu，所謂的Lu反映的是網(wǎng)格單元展開的層次，LU越 大的單元距離障礙的距離必然比較大，應(yīng)該放到隊(duì)列的尾部，越晚被更新。因此需要設(shè)計(jì)一 個(gè)考慮層級(jí)Lu和位置的排序估值函數(shù)，對(duì)該函數(shù)的表述如下式所示：
[003引 P (i，j) = Lu ? Wg ? Hg+Wg ? j+i
[0034] 在上式中Wg和H g為距離網(wǎng)格地圖的高度和寬度，i和j是待更新單元的行序號(hào)和 列序號(hào)，該函數(shù)將不同的待更新網(wǎng)格單元進(jìn)行了區(qū)分。
[0035] 作為本發(fā)明進(jìn)一步的方案；步驟2中，對(duì)考慮機(jī)器人姿態(tài)方位的狀態(tài)進(jìn)行離散化 處理后捜索節(jié)點(diǎn)展開方法；
[0036] 本發(fā)明中，移動(dòng)機(jī)器人的狀態(tài)向量除了其坐標(biāo)值之外，還加上了移動(dòng)機(jī)器人姿態(tài) 方位（即移動(dòng)機(jī)器人出發(fā)時(shí)朝向的方位），在對(duì)節(jié)點(diǎn)進(jìn)行展開時(shí)，排除了那些不可遷移的狀 態(tài)，使得所規(guī)劃的路徑對(duì)于該些有運(yùn)動(dòng)限制的移動(dòng)機(jī)器人可行，同時(shí)使得生成的路徑更容 易進(jìn)行平滑處理。
[0037] 本發(fā)明中，移動(dòng)機(jī)器人的狀態(tài)向量使用Si