未知環(huán)境機(jī)器人路徑規(guī)劃環(huán)境建模

劉歡，劉迎輝，楊婷婷

（長(zhǎng)春汽車工業(yè)高等?？茖W(xué)校，吉林長(zhǎng)春，130000）

0 引言

柵格法的重要一步就是要找到一個(gè)可以適應(yīng)未知環(huán)境的柵格分布方法，所以需要確定柵格的大小[1]。如果柵格選擇過(guò)小，對(duì)環(huán)境分辨率過(guò)高，就會(huì)導(dǎo)致柵格的數(shù)量增加，計(jì)算機(jī)實(shí)時(shí)處理和存儲(chǔ)的數(shù)據(jù)也會(huì)相應(yīng)增加，同時(shí)規(guī)劃路徑所受到的干擾也會(huì)增多，最后會(huì)增大移動(dòng)機(jī)器人的決策工作難度，從而使整個(gè)規(guī)劃過(guò)程緩慢。如果柵格選取過(guò)大，雖然抗干擾的能力會(huì)有所提高，移動(dòng)機(jī)器人的決策速度會(huì)加快，但機(jī)器人身處復(fù)雜的工作環(huán)境時(shí)，可能不會(huì)得到比較合理有效的路徑。一般來(lái)說(shuō)，柵格尺寸與搜救環(huán)境中障礙物的復(fù)雜程度有關(guān)，通過(guò)計(jì)算障礙物區(qū)域面積與整個(gè)環(huán)境面積之比換算可得到比較準(zhǔn)確的柵格尺寸數(shù)值。

1 環(huán)境建模

本文在這個(gè)方法的基礎(chǔ)上進(jìn)行改進(jìn)，柵格法的邊長(zhǎng)確定主要分三步：（1）由局部范圍內(nèi)的所有障礙物的邊長(zhǎng)大小與機(jī)器人自身的直徑的2/3進(jìn)行比較，取最小值；（2）計(jì)算局部障礙物面積和局部環(huán)境面積的比；（3）將(1)中最小值與(2)中的面積比值相比較，取最小值。所以具體的改進(jìn)算法步驟如下：

（1）確定機(jī)器人所在局部范圍的環(huán)境的總體面積；

（2）確定障礙物的信息，具體位置以及最小障礙物的邊長(zhǎng)l1；

（3）判斷環(huán)境中的障礙物是否都已經(jīng)計(jì)算過(guò)，如果還有未計(jì)算的障礙物，則跳到步驟2；

（5）計(jì)算柵格的大小為：

其中：S表示為環(huán)境的總面積，lmax是障礙物最大邊長(zhǎng)，d 是移動(dòng)機(jī)器人的直徑，l為最終確定的柵格邊長(zhǎng)。

圖1(a)和(b)分別為改進(jìn)前障礙物柵格效果圖和改進(jìn)后障礙物柵格效果圖。圖1(a)中柵格的大小是按照障礙物總面積與整體環(huán)境總面積之比確定，可以看出將柵格的邊長(zhǎng)取得過(guò)于大，導(dǎo)致局部區(qū)域中幾乎無(wú)可通行的路徑，不僅影響機(jī)器人的工作效率而且會(huì)致使機(jī)器人無(wú)法到達(dá)目標(biāo)點(diǎn)位置。圖1(b)是通過(guò)公式(2)計(jì)算出的柵格邊長(zhǎng)l，可以將障礙物與障礙物區(qū)分開(kāi)，并且障礙物之間的距離也可以清晰地規(guī)劃出來(lái)，這樣一來(lái)機(jī)器人到目標(biāo)點(diǎn)的可通行路徑增多，到達(dá)終點(diǎn)的效率也不斷提高。

圖1 柵格對(duì)比法

總而言之，優(yōu)化后的柵格法細(xì)化了柵格分布。在未知環(huán)境中[2]，障礙物信息非常復(fù)雜，障礙物的位置也雜亂無(wú)章，若柵格大小選取不當(dāng)就會(huì)導(dǎo)致兩個(gè)障礙物之間的距離忽略掉，或者將兩個(gè)障礙物的可通行距離隱藏掉。本文提出的算法，可以根據(jù)現(xiàn)場(chǎng)環(huán)境信息不斷更新柵格大小，適應(yīng)環(huán)境進(jìn)行建模。

2 粒子群優(yōu)化滾動(dòng)窗口法

滾動(dòng)窗口算法主要是應(yīng)用啟發(fā)式算法來(lái)尋找局部子目標(biāo)，由于上面介紹的基本滾動(dòng)窗口法有很大問(wèn)題：尋找路徑誤差過(guò)大導(dǎo)致邊界子目標(biāo)的選擇會(huì)有偏差，導(dǎo)致尋找路徑不是最短的路徑，浪費(fèi)過(guò)多時(shí)間。所以本文應(yīng)用GLPSO粒子群算法[3]進(jìn)行路徑規(guī)劃與制定局部范圍子目標(biāo)。

■2.1 GLPSO算法

粒子群算法采用了三種規(guī)則對(duì)粒子個(gè)體進(jìn)行規(guī)劃：(1)飛向距離下一目標(biāo)最近的個(gè)體；(2)飛向最終目標(biāo)；(3)飛向群體最優(yōu)位置。粒子群算法的原理是首先確定尋優(yōu)空間中的粒子數(shù)量，并隨機(jī)初始化此空間的粒子的速度和位置。粒子群算法的速度與位置公式更新為式(3)、(4)。

其中：wmax,wmin分別為最大慣性權(quán)重和最小慣性權(quán)重，t和tmax為當(dāng)前的迭代次數(shù)和最高迭代次數(shù)。此時(shí)ω值會(huì)隨當(dāng)前迭代次數(shù)的增加而變小，在迭代初期時(shí)ω值最大擁有較好的全局搜索能力，迭代后期ω值逐漸變小，有助于收斂到全局最優(yōu)。

在標(biāo)準(zhǔn)的PSO算法中個(gè)體的當(dāng)前速度的方向只根據(jù)個(gè)體最優(yōu)值和群體最優(yōu)值的方向決定，所以方向單一并且容易陷入局部最優(yōu)，機(jī)器人就會(huì)在自認(rèn)為的某一點(diǎn)停止。即運(yùn)用遺傳變異算法遺傳變異的方法把vi(t)變?yōu)?個(gè)變異函數(shù)，來(lái)改變當(dāng)前的固定值：

其中v'為遺傳變異后的當(dāng)前速度，(x,y)為當(dāng)前的坐標(biāo)，為常數(shù)，值不同時(shí)會(huì)有不同的曲線變化率。

■2.2 問(wèn)題描述

應(yīng)用改進(jìn)粒子群算法（GLPSO）為滾動(dòng)窗口法中的啟發(fā)式算法，每一次進(jìn)行推進(jìn)后，窗口中產(chǎn)生新環(huán)境，經(jīng)建模后應(yīng)用GLPSO算法，并進(jìn)入循環(huán)模式。以機(jī)器人位置為圓心，以傳感器可達(dá)視角邊界為半徑，構(gòu)成的范圍為滾動(dòng)窗口。該窗口內(nèi)的預(yù)測(cè)模型一方面是模擬傳感器系統(tǒng)獲取當(dāng)前位置的局部環(huán)境地圖，另一方面呈現(xiàn)出整個(gè)環(huán)境空間信息向該窗口范圍的縮影。機(jī)器人的初始位置為起點(diǎn)，通過(guò)獲取到的環(huán)境信息進(jìn)行建模，之后采用GLPSO算法計(jì)算出子目標(biāo)位置，之后執(zhí)行局部環(huán)境的規(guī)劃，可控制機(jī)器人到達(dá)終點(diǎn)目標(biāo)位置。

■2.3 局部子目標(biāo)獲取

首先局部子目標(biāo)一定是全局最終目標(biāo)的映射，具有最終目標(biāo)的相應(yīng)特性。即在局部范圍的邊界會(huì)有多個(gè)最終目標(biāo)映射出來(lái)的子目標(biāo)，通過(guò)對(duì)局部信息的獲取，利用GLPSO算法中xij(t+1)=xij(t)+vij(t+1)''對(duì)最優(yōu)值的求解方法作為啟發(fā)式算法。此時(shí)需要確定以機(jī)器人為圓心，以子目標(biāo)點(diǎn)為終點(diǎn)，并確定兩點(diǎn)之間的距離。很明顯在此環(huán)境中兩點(diǎn)間距離不可能是其直線距離，因?yàn)橹虚g會(huì)有好多障礙物。所以應(yīng)用上文改進(jìn)后的柵格法把環(huán)境進(jìn)行分割，每個(gè)格內(nèi)的中心點(diǎn)坐標(biāo)作為該表格的位置。此時(shí)應(yīng)用歐氏距離計(jì)算兩個(gè)目標(biāo)點(diǎn)的距離如公式(8)所示，到目標(biāo)點(diǎn)的最短距離如(9)所示。

其中f(i)表示機(jī)器人中心到局部子目標(biāo)的距離，(xi,yi)表示粒子當(dāng)前的坐標(biāo)，(xi+1,yi+1)表示粒子的下一個(gè)位置的坐標(biāo)，n代表從當(dāng)前點(diǎn)到局部子目標(biāo)的個(gè)數(shù)。通過(guò)確定fmin選擇子目標(biāo)，能夠?qū)崿F(xiàn)在無(wú)法獲知的環(huán)境中快速準(zhǔn)確地為機(jī)器人進(jìn)行路徑規(guī)劃，通過(guò)不斷地局部環(huán)境規(guī)劃，來(lái)獲取全局路徑的優(yōu)化。

機(jī)器人通過(guò)滾動(dòng)窗口法來(lái)規(guī)劃路徑的方法如圖2所示。

圖2 改進(jìn)后機(jī)器人滾動(dòng)路徑執(zhí)行圖

圖3 建模流程圖

3 適應(yīng)度函數(shù)(fitness)加入安全因子

自然界中的生物個(gè)體都會(huì)對(duì)所在環(huán)境有一個(gè)適應(yīng)度（fitness），這非常符合適應(yīng)法則，那么適應(yīng)度函數(shù)也可以作為個(gè)體對(duì)象的適應(yīng)準(zhǔn)則。在本文的應(yīng)用中，適應(yīng)度函數(shù)fmin等于機(jī)器人到目標(biāo)粒子之間的最短距離，所以機(jī)器人路徑規(guī)劃中適應(yīng)度函數(shù)是極其重要的，它間接決定了搜救算法是否能夠精準(zhǔn)地計(jì)算出最優(yōu)路徑[4]。若適應(yīng)度函數(shù)與當(dāng)前環(huán)境不符或者誤差過(guò)大，那么將會(huì)致使搜救算法陷入局部最優(yōu)或者永遠(yuǎn)找不到最終目標(biāo)點(diǎn)，且無(wú)法把符合標(biāo)準(zhǔn)的點(diǎn)選擇出來(lái)；若適應(yīng)度函數(shù)在規(guī)劃路徑中僅僅只是由點(diǎn)與點(diǎn)之間的距離組成，那么通過(guò)搜救算法規(guī)劃出來(lái)的路徑不會(huì)考慮機(jī)器人本身因素。

為解決上述問(wèn)題，本文的適應(yīng)度函數(shù)主要由安全性因子以及路徑的最短距離作為規(guī)劃路徑的可行性評(píng)價(jià)因子，從而得到個(gè)體適應(yīng)度評(píng)價(jià)函數(shù)。

本文不會(huì)將機(jī)器人視為一個(gè)質(zhì)點(diǎn)，路徑安全性是作為路徑一個(gè)重要的指標(biāo)，即如果將機(jī)器人與障礙物之間的寬度的小于50%則認(rèn)為不可通行，若寬度大于50%即通行[5]。但本文認(rèn)為在未知環(huán)境中障礙物與障礙物之間的距離不會(huì)過(guò)大，所以本文淘汰不適合路徑的規(guī)則定義為障礙物與障礙物之間大于等于移動(dòng)機(jī)器人的3/4的距離就可以通行，若小于移動(dòng)機(jī)器人的3/4則不可通行。

首先確定機(jī)器人的安全距離比，路徑中每個(gè)可行的節(jié)點(diǎn)都作為基準(zhǔn)點(diǎn)，安全距離大于基準(zhǔn)點(diǎn)與障礙物之間距離的1/3。安全距離越大局部安全距離比越大，如公式(10)所示，表示機(jī)器人按照路徑行駛時(shí)兩側(cè)的最小距離，可行的安全距離比如(11)所示。

在上述式子中：lir為i節(jié)點(diǎn)處行駛時(shí)兩側(cè)的最小距離；為基準(zhǔn)點(diǎn)與障礙物之間；di為i節(jié)點(diǎn)處自由空間或道路的寬度；n為路徑點(diǎn)數(shù)目；Lir為可行的安全距離比。

路徑通行度即自由空間面積的最小值與該點(diǎn)的自由空間總面積的百分比，如公式(12)所示。

式中：p為全局通行度；wmin為全局地圖可通行的自由空間的最小水平截面寬度；wrobot為機(jī)器人的寬度。

然后定義路徑安全度為最小局部安全距離比全局通行度的百分比如公式(13)所示：

要加強(qiáng)社會(huì)整體的信用體系建設(shè)，包括個(gè)人征信體系和企業(yè)信用評(píng)價(jià)體系等，還要加快互聯(lián)網(wǎng)金融平臺(tái)與中國(guó)人民銀行信用體系建設(shè)的對(duì)接。除此之外，應(yīng)當(dāng)加快第三方信用評(píng)級(jí)機(jī)制的建設(shè)，構(gòu)建第三方動(dòng)態(tài)監(jiān)管體系。

其中：s為路徑的安全性因子。

適應(yīng)度函數(shù)不僅是粒子群優(yōu)化算法中判斷參數(shù)是否合適的重要因素，而且也是路徑規(guī)劃中建模的標(biāo)準(zhǔn)，可以用作判定路徑距離和路徑是否安全的重要函數(shù)。本文以最短路徑函數(shù)加上路徑通行的安全性因子構(gòu)成新的適應(yīng)度函數(shù)，并在此基礎(chǔ)上對(duì)所有參數(shù)進(jìn)行加權(quán)平均處理，以達(dá)到滿足復(fù)雜環(huán)境機(jī)器人的路徑規(guī)劃要求，如公式(14)(15)(16)所示。

公式中α、β為各自函數(shù)的加權(quán)因子，其范圍為[0,1]。通過(guò)調(diào)整α和β可以調(diào)節(jié)f1、f2在適應(yīng)度函數(shù)中對(duì)F的影響，當(dāng)α=1,β=0轉(zhuǎn)化為常規(guī)的僅由路長(zhǎng)作為適應(yīng)度函數(shù)的數(shù)學(xué)模型，f2作為路徑的平滑度函數(shù)及安全系數(shù)。

4 總體建模流程

Step1：通過(guò)滾動(dòng)窗口法進(jìn)行環(huán)境掃描，標(biāo)記出障礙物位置，記錄最大及最小障礙物邊長(zhǎng)；

Step2：計(jì)算局部環(huán)境的總面積sods，障礙物的總面積l temp，根據(jù)公式(1)計(jì)算出柵格ltemp大小；

Step4：利用粒子群是算法為啟發(fā)式算法，確定局部子目標(biāo)；

Step5：根據(jù)公式(3)求出機(jī)器人到目標(biāo)點(diǎn)的距離之和f(i);

Step6：根據(jù)公式(13)求出路徑的安全系數(shù)；

Step7：根據(jù)公式(16)求出環(huán)境建模的適應(yīng)度函數(shù)F(x);

Step8：最后根據(jù)改進(jìn)后粒子群函數(shù)（GLPSO）來(lái)規(guī)劃路徑。

Step9：機(jī)器人根據(jù)路徑走到局部子目標(biāo)，之后轉(zhuǎn)到Step1。

根據(jù)以上的操作，首先把環(huán)境進(jìn)行細(xì)化運(yùn)用格柵法物位置及大小測(cè)量出來(lái)，其次，運(yùn)用改進(jìn)的粒子群算法GLPSO來(lái)進(jìn)行路徑的規(guī)劃設(shè)計(jì)，找出一條路徑距離較短、安全度較高的路徑，流程圖如3所示。

5 仿真實(shí)驗(yàn)及結(jié)果分析

為了驗(yàn)證基于改進(jìn)后滾動(dòng)窗口與柵格法結(jié)合的有效性，進(jìn)行算法仿真。在統(tǒng)一的柵格中建模，障礙物密度為1/3。圖4所示為20x20柵格地圖，其中綠色表示起點(diǎn)，紅色表示目標(biāo)節(jié)點(diǎn)。分別采用兩種適應(yīng)度函數(shù)：（1）無(wú)安全因子適應(yīng)度函數(shù)（2）本文提出F(x)=α×f1+β×f2的適應(yīng)度函數(shù)和兩種路徑搜索算法分別是A＊算法[6]與改進(jìn)后的GLPSO算法進(jìn)行路徑規(guī)劃。

圖4 環(huán)境建模

圖4(a)中所示為基本建模算法，主要運(yùn)用原有的滾動(dòng)窗口法即啟發(fā)函數(shù)A＊算法求解局部子目標(biāo)值，從上圖可清晰的看出機(jī)器人行走的路線非常曲折，每個(gè)轉(zhuǎn)折點(diǎn)幾乎都是碰到障礙物之后再躲避，智能尋路效果差，安全性能低；導(dǎo)致時(shí)間過(guò)長(zhǎng)，浪費(fèi)時(shí)間。圖4(b)所示應(yīng)用滾動(dòng)窗口算法加入GLPSO算法，適應(yīng)度函數(shù)再加入安全性因子，考慮了路面過(guò)于狹窄的情形，增強(qiáng)了算法跳出局部最優(yōu)的能力，提高了路徑中的安全性和實(shí)時(shí)性。從上圖可以清晰地看出機(jī)器人在遇到障礙物之前就障礙物位置確定好，并且明顯看出每一階段的局部目標(biāo)點(diǎn)的選取都貼合最終點(diǎn)，規(guī)劃出的路徑也沒(méi)有陷入局部最優(yōu)的情況。

表1 規(guī)劃算法參數(shù)對(duì)比

由表格1中的數(shù)據(jù)對(duì)比得出，改進(jìn)后的GLPSO算法在尋找局部節(jié)點(diǎn)的個(gè)數(shù)比A＊算法的找出的節(jié)點(diǎn)個(gè)數(shù)減少了48%，節(jié)省數(shù)據(jù)計(jì)算量，提高算法運(yùn)行效率。lir是障礙物與機(jī)器人的距離，d是兩個(gè)障礙物之間的距離，機(jī)器人與障礙物之間距離小于1/3＊d是比較危險(xiǎn)的行為，在上圖4(a)中機(jī)器人與障礙物之間距離小于1/3＊d的點(diǎn)有11處，而圖4(b)中機(jī)器人與障礙物之間距離小于1/3＊d的點(diǎn)只有5處，在安全性上改進(jìn)后的算法優(yōu)越于基本規(guī)劃算法，安全性提高了54%。在搜索時(shí)間上改進(jìn)后的算法比傳統(tǒng)A＊算法時(shí)間節(jié)約了46%。在路徑規(guī)劃長(zhǎng)度上GLPSO算法比A＊算法的規(guī)劃的長(zhǎng)度縮短了17%。所以，運(yùn)用改進(jìn)后的粒子群算法來(lái)規(guī)劃路徑在性能上都有所提高。