基于電子地圖的D*補償算法AGV路徑規(guī)劃*

程 滿 楊光永 徐天奇 黃卓群 戈一航

（云南民族大學(xué)電氣信息工程學(xué)院 昆明 650500）

1 引言

自動導(dǎo)引車（Automated Guided Vehicle，AGV）隨著智能化的發(fā)展，在智能倉儲和室內(nèi)搬運中發(fā)揮越來越大的作用［1］。路徑規(guī)劃作為AGV的核心之一，合理的路徑規(guī)劃是智能車輛不可或缺的一部分，決定了智能車輛的安全性和穩(wěn)定。傳統(tǒng)幾何方法中的幾何法，例如可視圖法［2］、沃羅諾伊圖法、自由空間法等計算量大、路徑不是最優(yōu)；單元劃分法，缺點是單元分解與計算單元之間的鄰接關(guān)系的開銷較大；人工勢場法，實際上是一種擬物方法，模擬自然界中的靜電場、流體等，缺點是容易陷入局部最優(yōu)而無法完成任務(wù)［3～5］。智能算法主要包括模糊邏輯算法［6］、神經(jīng)網(wǎng)絡(luò)算法、遺傳算法、蟻群算法、模擬退火等，智能算法［7］普遍存在著對計算機要求高，收斂速度慢、控制變量多之類的缺陷［7］。D*算法是在圖搜索的思維下發(fā)展的智能啟發(fā)式算法［8］，原理簡單，容易實現(xiàn)，D*算法是A*算法的擴展，與A*算法中有向靜態(tài)弧長（結(jié)點與結(jié)點之間的代價）的定義不同，D*算法結(jié)點之間的弧長代價可以隨算法的實施進程而動態(tài)變化，即動態(tài)A*算法（Dynamic A*），D*算法由此得名，不同在于D*是從目標點到起點的反向搜索。文獻［9］對于地圖進行分區(qū)以及子節(jié)點的改進，實現(xiàn)在不同區(qū)域不同搜索，并用Floyd算法平滑處理，保證了效率的同也實現(xiàn)了很好的避障效果。文獻［10］考慮到了AGV的尺寸大小和位姿對行駛的影響，采用改進的遺傳算法對A*算法產(chǎn)生的初始路徑進行優(yōu)化。文獻中所提出的方法，在一定程度上實現(xiàn)了很好的效果，但是有一些缺陷。比如障礙物的突然出現(xiàn)，這些算法就不能實現(xiàn)很好的預(yù)期效果；以及改進后的算法相較于之前的算法而言，有時損失了例如路徑最優(yōu)、規(guī)劃時間短等方面來滿足某些方面的優(yōu)勢。對于以上的問題，本文提出了D*補償算法，在不損失最優(yōu)路徑的同時也能很好的實現(xiàn)避障功能，以及考慮到了實際行駛情況，特別注意轉(zhuǎn)彎次數(shù)多的問題。

2 補償D*算法

2.1 距離表達

D*算法原始的距離計算公式是歐氏距離（Euclidean Distance）表達式如下：

由于原始距離要進行平方項和開方處理，速度慢?，F(xiàn)在換成曼哈頓距離（Manhattan Distance）表達式如下：

2.2 啟發(fā)函數(shù)

D*是一種啟發(fā)式搜索算法，D*估價函數(shù)如下：

圖1 節(jié)點的轉(zhuǎn)彎判斷

對g(n)累計代價做改進，將轉(zhuǎn)彎次數(shù)這個要素也參考進去。提出g'(n)，λ和c(n)，c(n)與AGV行駛過程中轉(zhuǎn)過彎的次數(shù)成正比，λ代表轉(zhuǎn)過的彎次數(shù)，g'()n表示新的累計代價，本文中正比的這個比例設(shè)置為10。

2.3 平滑行駛

基于實際行駛，AGV遇到大轉(zhuǎn)彎時候不可能停在原地，然后轉(zhuǎn)大彎操作，這樣操作性不強，對本身運行非常危險。因此在行駛中我們更希望AGV行駛的路徑是一個圓滑的路徑，平緩的從起點走到終點。假設(shè)平滑之前的點為［x1,x2,x3,…,xn-1,xn]，平滑之后的點為[y1,y,y3,…,yn-1,yn]。本文中α表示偏離度，β表示平滑度，為了得到最佳的效果本文的α和β都設(shè)置為0.5，迭代次數(shù)為9次，可以得到最優(yōu)的平滑行駛路徑。

圖2 路徑平滑對照圖

路徑平滑的實質(zhì)是求解y(i)滿足兩種距離的最?。?/p>

最小化目標為

求解采用梯度下降法（gradient descent），多次迭代，調(diào)整y(i)使得目標函數(shù)取得最小值，初始化y(i)=x(i)，迭代：

2.4 安全行駛

長度小的障礙物，直接采用圓形進行包圍，損失的可行路徑比較小。外包圓直徑取障礙物的頂點連線的最大值，圓心為最大連接線的中點。

障礙物在極坐標下形成的曲線表示為

圖3 障礙物圓形包圍膨脹

加入的電子地圖障礙物是矩形，建模時膨脹矩形即可，可以節(jié)約大量的可行路徑，在兩障礙物中間不會判斷為不可行區(qū)域。參照Costmap的柵格空間覆蓋枚舉的改進型方法的思想，簡單的劃分為Freespace（自由層）和Lethal（致命層）。膨脹的度設(shè)置為0.5個單元柵格大小。

3 總的結(jié)構(gòu)流程圖

圖4流程圖節(jié)點nij，鄰近點nrs，優(yōu)先隊列open和closed，每一個節(jié)點nij分配的狀態(tài)標志是（tnij）累計代價g（n），提出新的累計代價g'(n)。

圖4 矩形障礙物矩形膨脹

4 改進算法的仿真結(jié)果分析

對比仿真實驗的起點位置都設(shè)置為start=（7，3），終點位置為goal=（23，10），橫坐標為x軸，縱坐標為y軸。

4.1 算法的比較

圖9算法在迭代次數(shù)上面相較于圖8算法來說并沒有優(yōu)勢，但在轉(zhuǎn)彎次數(shù)的減少，以及路徑的減少有較明顯的區(qū)別，實現(xiàn)了較優(yōu)的改進。實際生活中的AGV的行駛，走曲線可以維持速度在一個更穩(wěn)定的范圍之內(nèi)移動，到達目標點的耗時反而短一些。圖9算法還有一個顯而易見的優(yōu)點就是與障礙物有一定的安全距離，滿足安全行駛的要素。按照設(shè)置的預(yù)計代價函數(shù)，計算新的路徑，主要考慮了轉(zhuǎn)彎次數(shù)的問題，圖9算法相對于圖6-8算法，行駛的路徑長度減少，圖9算法路徑長度相較于圖8算法減少了25.5%，轉(zhuǎn)彎次數(shù)減少了50%。

圖5 結(jié)構(gòu)流程

圖6 Dijkstra算法規(guī)劃的路徑

圖7 A*算法規(guī)劃的路徑

圖8 D*算法規(guī)劃的路徑

圖9 D*補償算法規(guī)劃的路徑

4.2 臨時障礙物

在遭遇臨時障礙物的情況下，圖11中的算法依然可以很好地避開障礙物，同時與障礙物保持一定的安全距離，相較于圖10中的算法，轉(zhuǎn)彎的次數(shù)減少了，行駛更短的路徑。

圖10 D*算法遭遇障礙物的路徑

圖11 補償后的D*遭遇障礙物的路徑

表1 圖5～8的參數(shù)比較

5 結(jié)語

補償后的D*算法在路徑規(guī)劃問題中考慮了轉(zhuǎn)彎因素的影響，補償后的算法有效地避免了行駛路徑與障礙物的距離過于接近，保證了AGV的安全行駛，更好地滿足了實際的行駛需求。在Matlab上完成了補償后D*算法的仿真，并和Dijkstra、A*、D*算法進行了比較，驗證了安全性和行駛路徑減小。在后續(xù)的工作中將探索算法在硬件上的實現(xiàn)，如何更快速度地完成規(guī)劃過程。