未知環境機器人路徑規劃環境建模

劉歡，劉迎輝，楊婷婷

（長春汽車工業高等專科學校，吉林長春，130000）

0 引言

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

1 環境建模

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

（1）確定機器人所在局部范圍的環境的總體面積；

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

（3）判斷環境中的障礙物是否都已經計算過，如果還有未計算的障礙物，則跳到步驟2；

（5）計算柵格的大小為：

其中：S表示為環境的總面積，lmax是障礙物最大邊長，d 是移動機器人的直徑，l為最終確定的柵格邊長。

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

圖1 柵格對比法

總而言之，優化后的柵格法細化了柵格分布。在未知環境中[2]，障礙物信息非常復雜，障礙物的位置也雜亂無章，若柵格大小選取不當就會導致兩個障礙物之間的距離忽略掉，或者將兩個障礙物的可通行距離隱藏掉。本文提出的算法，可以根據現場環境信息不斷更新柵格大小，適應環境進行建模。

2 粒子群優化滾動窗口法

滾動窗口算法主要是應用啟發式算法來尋找局部子目標，由于上面介紹的基本滾動窗口法有很大問題：尋找路徑誤差過大導致邊界子目標的選擇會有偏差，導致尋找路徑不是最短的路徑，浪費過多時間。所以本文應用GLPSO粒子群算法[3]進行路徑規劃與制定局部范圍子目標。

■2.1 GLPSO算法

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

其中：wmax,wmin分別為最大慣性權重和最小慣性權重，t和tmax為當前的迭代次數和最高迭代次數。此時ω值會隨當前迭代次數的增加而變小，在迭代初期時ω值最大擁有較好的全局搜索能力，迭代后期ω值逐漸變小，有助于收斂到全局最優。

在標準的PSO算法中個體的當前速度的方向只根據個體最優值和群體最優值的方向決定，所以方向單一并且容易陷入局部最優，機器人就會在自認為的某一點停止。即運用遺傳變異算法遺傳變異的方法把vi(t)變為8個變異函數，來改變當前的固定值：

其中v'為遺傳變異后的當前速度，(x,y)為當前的坐標，為常數，值不同時會有不同的曲線變化率。

■2.2 問題描述

應用改進粒子群算法（GLPSO）為滾動窗口法中的啟發式算法，每一次進行推進后，窗口中產生新環境，經建模后應用GLPSO算法，并進入循環模式。以機器人位置為圓心，以傳感器可達視角邊界為半徑，構成的范圍為滾動窗口。該窗口內的預測模型一方面是模擬傳感器系統獲取當前位置的局部環境地圖，另一方面呈現出整個環境空間信息向該窗口范圍的縮影。機器人的初始位置為起點，通過獲取到的環境信息進行建模，之后采用GLPSO算法計算出子目標位置，之后執行局部環境的規劃，可控制機器人到達終點目標位置。

■2.3 局部子目標獲取

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

其中f(i)表示機器人中心到局部子目標的距離，(xi,yi)表示粒子當前的坐標，(xi+1,yi+1)表示粒子的下一個位置的坐標，n代表從當前點到局部子目標的個數。通過確定fmin選擇子目標，能夠實現在無法獲知的環境中快速準確地為機器人進行路徑規劃，通過不斷地局部環境規劃，來獲取全局路徑的優化。

機器人通過滾動窗口法來規劃路徑的方法如圖2所示。

圖2 改進后機器人滾動路徑執行圖

圖3 建模流程圖

3 適應度函數(fitness)加入安全因子

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

為解決上述問題，本文的適應度函數主要由安全性因子以及路徑的最短距離作為規劃路徑的可行性評價因子，從而得到個體適應度評價函數。

本文不會將機器人視為一個質點，路徑安全性是作為路徑一個重要的指標，即如果將機器人與障礙物之間的寬度的小于50%則認為不可通行，若寬度大于50%即通行[5]。但本文認為在未知環境中障礙物與障礙物之間的距離不會過大，所以本文淘汰不適合路徑的規則定義為障礙物與障礙物之間大于等于移動機器人的3/4的距離就可以通行，若小于移動機器人的3/4則不可通行。

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

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

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

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

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

要加強社會整體的信用體系建設，包括個人征信體系和企業信用評價體系等，還要加快互聯網金融平臺與中國人民銀行信用體系建設的對接。除此之外，應當加快第三方信用評級機制的建設，構建第三方動態監管體系。

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

適應度函數不僅是粒子群優化算法中判斷參數是否合適的重要因素，而且也是路徑規劃中建模的標準，可以用作判定路徑距離和路徑是否安全的重要函數。本文以最短路徑函數加上路徑通行的安全性因子構成新的適應度函數，并在此基礎上對所有參數進行加權平均處理，以達到滿足復雜環境機器人的路徑規劃要求，如公式(14)(15)(16)所示。

公式中α、β為各自函數的加權因子，其范圍為[0,1]。通過調整α和β可以調節f1、f2在適應度函數中對F的影響，當α=1,β=0轉化為常規的僅由路長作為適應度函數的數學模型，f2作為路徑的平滑度函數及安全系數。

4 總體建模流程

Step1：通過滾動窗口法進行環境掃描，標記出障礙物位置，記錄最大及最小障礙物邊長；

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

Step4：利用粒子群是算法為啟發式算法，確定局部子目標；

Step5：根據公式(3)求出機器人到目標點的距離之和f(i);

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

Step7：根據公式(16)求出環境建模的適應度函數F(x);

Step8：最后根據改進后粒子群函數（GLPSO）來規劃路徑。

Step9：機器人根據路徑走到局部子目標，之后轉到Step1。

根據以上的操作，首先把環境進行細化運用格柵法物位置及大小測量出來，其次，運用改進的粒子群算法GLPSO來進行路徑的規劃設計，找出一條路徑距離較短、安全度較高的路徑，流程圖如3所示。

5 仿真實驗及結果分析

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

圖4 環境建模

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

表1 規劃算法參數對比

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