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

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

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

1 引言

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

2 補償D*算法

2.1 距離表達

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

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

2.2 啟發函數

D*是一種啟發式搜索算法，D*估價函數如下：

圖1 節點的轉彎判斷

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

2.3 平滑行駛

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

圖2 路徑平滑對照圖

路徑平滑的實質是求解y(i)滿足兩種距離的最小：

最小化目標為

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

2.4 安全行駛

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

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

圖3 障礙物圓形包圍膨脹

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

3 總的結構流程圖

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

圖4 矩形障礙物矩形膨脹

4 改進算法的仿真結果分析

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

4.1 算法的比較

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

圖5 結構流程

圖6 Dijkstra算法規劃的路徑

圖7 A*算法規劃的路徑

圖8 D*算法規劃的路徑

圖9 D*補償算法規劃的路徑

4.2 臨時障礙物

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

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

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

表1 圖5～8的參數比較

5 結語

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