特征地圖的室內機器人路徑規(guī)劃融合算法

劉 朋，任工昌

陜西科技大學 機電工程學院，西安 710021

隨著自動化與人工智能技術的發(fā)展，在工農業(yè)生產和日常生活中出現了越來越多的移動機器人，它們能夠完成多種任務，減輕人們的勞動負擔。例如在工業(yè)領域，存在著大量物料抓取及搬運工作，但車間內的工作環(huán)境一般存在溫度高、濕度大、聲音嘈雜，甚至有毒等問題，而且各類設施密集，空間環(huán)境復雜，各項工作重復性高，因此特別適合移動機器人替代人來完成。

機器人在室內自主移動，先要進行路徑規(guī)劃，環(huán)境地圖作為先驗信息是路徑規(guī)劃中不可或缺的。機器人技術中環(huán)境地圖常使用特征地圖和柵格地圖。特征地圖[1]簡單直觀、模型數據量少、計算效率高，適用于結構化較好的室內環(huán)境，但是特征地圖中常用的角點或線段中點等點特征對環(huán)境細節(jié)表達不足，因此很難直接用于路徑規(guī)劃，一般常被用于機器人定位算法。柵格地圖因其簡單，易于創(chuàng)建和維護，不受地形限制等眾多優(yōu)點被大部分路徑規(guī)劃算法所采用，但是，隨著環(huán)境規(guī)模擴大，使用柵格地圖時消耗算力和存儲空間的增長速度遠高于環(huán)境規(guī)模的增長速度，因此使用柵格地圖的算法在環(huán)境規(guī)模較大時普遍存在計算效率較低的問題[2]。

在路徑規(guī)劃方面，根據機器人對地圖掌握的情況，可以分為全局路徑規(guī)劃與局部路徑規(guī)劃[3]。全局路徑規(guī)劃算法種類較多，以蟻群算法[4-5]、遺傳算法[6-7]、人工神經網絡[8-9]和粒子群算法[10-11]為代表的智能算法適用于復雜問題的求解和優(yōu)化，易于實現并行性，但是容易陷入局部最優(yōu)解或出現收斂速度慢的問題。以快速擴展隨機樹（rapidly-exploring random tree，RRT）算法[12-13]為代表的隨機采樣算法搜索能力強，在高維狀態(tài)空間應用廣泛，但是該類算法運算成本高、隨機性強。以A*算法[14-15]為代表的啟發(fā)式搜索算法是目前使用較廣泛的一種路徑規(guī)劃算法，但是其存在搜索效率低、拐點多、路徑不平滑等缺點。Petri網建模與整數線性規(guī)劃也被用來解決路徑規(guī)劃問題，但是這些方法必須事先建立好Petri 網模型，無法進行實時路徑規(guī)劃，一般應用于多任務、多約束的復雜場景[16]。Bug 系列算法[17]屬于應激式算法，在計算實時性方面有非常突出的表現，但由于其完全應激的基本特性，得到的路徑長度明顯較長。Dijkstra 算法也是一種較為常用路徑規(guī)劃算法，其優(yōu)點是算法簡單方便，可以獲取全局最優(yōu)路徑，但是同樣存在計算效率低、冗余拐點多、離障礙物近等問題[18]。文獻[19]利用幾何拓撲學方法，結合實際場景信息，對Dijkstra 算法規(guī)劃的路徑進行平滑處理，減少了路徑中的冗余節(jié)點和機器人的累計轉彎角度，但仍沒有改變算法計算效率低的問題。人工勢場法[20]和動態(tài)窗口法（dynamic window approach，DWA）[21]是普遍使用的局部路徑規(guī)劃算法。人工勢場法結構簡單，能夠實現實時避障，但其存在規(guī)劃路徑過長和局部最優(yōu)點的情況[22]。DWA通過當前時刻對周圍環(huán)境采樣，獲得下一時刻機器人的運動狀態(tài)，因此，規(guī)劃的路徑平滑性更好，但該算法缺少前瞻性，且高度依賴全局參數，容易陷入局部最優(yōu)解，出現路徑規(guī)劃失敗[23]。為了克服以上單一算法的不足，多采用算法融合的方法，文獻[24]提出一種融合改進A*蟻群算法與動態(tài)窗口法的平滑路徑規(guī)劃方法，解決前期蟻群效率低、易陷入局部最優(yōu)等問題，而且使用貝塞爾曲線對規(guī)劃的路徑進行平滑化處理，使路徑更接近實際移動路徑；文獻[25-26]分別采用關鍵點優(yōu)化策略和雙向搜索的方法對A*算法進行改進，通過改進的A*搜索出全局最短路徑的關鍵節(jié)點，再融合DWA算法規(guī)劃節(jié)點間的路徑，使整條路徑平滑；文獻[27]在算法融合時，對航向角的選擇進行了優(yōu)化，使規(guī)劃的路徑更平滑。但以上融合算法均使用柵格地圖，且關鍵節(jié)點的搜索使用A*算法，仍然無法避免出現算法計算效率低的問題。

特征地圖中的點（線段中點）特征無法直接用于路徑規(guī)劃，但是線段本身可以表達障礙物的輪廓，且其具有方向、長度等屬性，因此，包含完整屬性的線段特征不僅可以進行路徑規(guī)劃，還可以提高路徑搜索的速度，并且充分體現特征地圖計算效率高的優(yōu)點。本文提出一種基于特征地圖的路徑規(guī)劃融合算法。首先，給出了線段特征的表達方式和障礙物檢測方法；然后，結合Bug算法的基本原理，提出一種基于特征地圖的搜索優(yōu)化算法，通過先搜索再優(yōu)化的方式快速獲得最優(yōu)路徑的關鍵節(jié)點；接下來，分析了DWA算法中全局參數在不同階段對規(guī)劃路徑的影響程度，對目標函數進行改進，降低了DWA 算法對全局參數的敏感性；最后，將提出的搜索優(yōu)化算法與改進后的DWA進行融合，使算法的計算效率大幅度提高，同時規(guī)劃出的路徑更優(yōu)。

1 特征地圖及障礙物檢測

特征地圖是將原始觀測數據經過分割、合并后提取出的點、線、弧等幾何特征，并用這種離散特征來描述環(huán)境的地圖形式。路徑規(guī)劃時，特征地圖中的線段特征能夠表示環(huán)境中障礙物的范圍和方向，因此，地圖可表示為線段特征的集合QL：

其中，(xi,yi)為線段中點的坐標，θi為線段的傾角，li為線段的長度。

在柵格地圖中，進行障礙物檢測時，應計算機器人與地圖中障礙物（點）間的距離，而在特征地圖中則轉換為計算機器人到特征地圖中障礙物（線段）間的距離。機器人與障礙物線段間的距離有兩種情況：如果機器人中心點到障礙物線段的投影在該線段上，則該距離為投影點至機器人中心的距離，如圖1（a）所示；如果機器人中心到障礙物線段的投影點在該線段外，則該距離為機器人至線段端點（距離投影點較近的端點）的距離，如圖1（b）所示。如果di大于閾值，則檢測到障礙物。圖1 中，藍色圓圈表示機器人，pR為中心點，Li為障礙物線段，為點pR在線段Li或其延長線上的投影，pLi為線段Li的端點。

圖1 機器人與障礙物距離示意圖Fig.1 Diagram of distance between robot and obstacle

機器人與障礙物間距離的計算步驟為：

（1）根據機器人中心點pR的坐標(xR,yR)和障礙物線段Li的特征QLi，計算投影pR′的坐標(,)；

（2）計算pR′到障礙物線段中點(xi,yi) 的距離；

（3）如果≤li/2，則在線段Li上，di為pR與兩點之間的距離，dp=0；

（4）否則，在線段Li的延長線上，分別計算其與線段Li兩端點pi1和pi2之間的距離dp1和dp2，dp=min(dp1,dp2)，為pR與兩點之間的距離，則：

2 全局路徑搜索及優(yōu)化

全局路徑規(guī)劃時，A*的計算效率較低，規(guī)劃的路徑拐點較多，也影響了融合算法的結果。因此，結合特征地圖中障礙物具有方向性的特點，使用Bug算法的原理，快速搜索可行路徑節(jié)點，再經過節(jié)點優(yōu)化得到最優(yōu)路徑，可以大幅提高算法的計算效率。

2.1 Bug算法簡介

Bug算法原理類似昆蟲爬行的運動決策策略，其包括兩種基本運動模式：模式1，在未遇到障礙物時，沿直線向目標運動；模式2，在遇到障礙物后，沿著障礙物邊界繞行。通過兩個模式互相轉換，Bug算法可實時調整其路徑規(guī)劃策略直到抵達終點。不同的Bug 算法主要區(qū)別在于其障礙物繞行策略及模式切換策略不同。如圖2 所示為Bug1 算法原理示意圖，灰色矩形表示障礙物，綠色圓圈為起點，紅色三角為目標點，虛線箭頭表示搜索的路徑。爬蟲先以起點和目標點連線方向作為爬行方向，當遇到障礙物時，沿障礙物外圍輪廓爬行，當再回到目標連線上后，繼續(xù)沿目標方向爬行，直至到達目標點。

圖2 Bug1算法原理示意圖Fig.2 Schematic diagram of Bug1 algorithm principle

Bug 算法能夠在未知環(huán)境的情況下，根據自身位置、目標及傳感器測量信息，實時地規(guī)劃出到達終點的路徑。但該算法應用于全局路徑規(guī)劃時，由于其基于有限的局部路徑信息做出的決策缺乏對路徑長度的考慮，通常規(guī)劃的路徑會遠超過最優(yōu)路徑長度，而且平滑性較差，不能很好適應機器人的運動特性。

2.2 搜索優(yōu)化算法原理

在進行路徑搜索時，機器人從起點沿終點方向以單位步長向前搜索，檢測到障礙物時，即圖1 中di小于安全距離dsafe時，該位置為轉折點。如果轉折點處障礙物Li傾角為θi，且該轉折點為首次出現，如圖3（a）中點，則在此處機器人轉向θi方向搜索；如果該轉折點已在其他路徑中出現過，如圖3（b）中點，則機器人在此處轉向θi+π 方向搜索，直至遠離障礙物Li，機器人重新沿終點方向搜索。當機器人與終點連線和所有障礙物線段均不相交，而且與障礙物間距離大于安全距離時，該條路徑搜索結束。圖3中，Li為障礙物線段，綠色△表示目標位置，綠色〇表示機器人起點位置，藍色虛線〇表示路徑節(jié)點位置，、、分別表示轉折點、角點和障礙物端點，桔色箭頭表示路徑搜索方向。

一條路徑搜索完成后，將起點至終點間的所有節(jié)點pi作為該條規(guī)劃路徑。如果搜索時某個節(jié)點在同一條搜索路徑中第二次出現，則該條路徑不可行，進入下一條路徑搜索。為了避免出現搜索路徑重復或遺漏，應依次從所有已出現的轉折點中的最后一個開始判斷。搜索出所有可行路徑后，將路程最短的一條進行節(jié)點優(yōu)化，獲得最優(yōu)路徑的關鍵節(jié)點。圖3中共搜索出3條可行路徑，路徑3的路程最短，但其中是多余節(jié)點，經過優(yōu)化后的最優(yōu)路徑如圖3（d）所示[28]。

搜索優(yōu)化算法利用Bug算法搜索效率高的優(yōu)點，使用特征地圖又進一步降低障礙物碰撞檢測和沿障礙物邊界搜索時的計算難度，提高了計算效率；同時，通過節(jié)點優(yōu)化可以使得最終規(guī)劃路徑為最優(yōu)路徑，避免出現Bug算法搜索路徑過長的問題。

2.3 角點問題

室內環(huán)境中存在著許多拐角，對應于特征地圖中的角點，也代表著兩條線段特征相交于此點。根據機器人與角點之間的位置關系，可將角點分為內角點與外角點，如圖4 所示。La、Lb表示障礙物線段，藍色圓圈表示機器人，黑色箭頭表示機器人當前位置的航向角，綠色箭頭表示機器人移動路徑。

圖4 角點位置示意圖Fig.4 Diagram of corner point position

如果機器人處于內角點位置，則其搜索路徑的方向是確定的，如圖4（a）所示，機器人應轉向與線段Lb平行且豎直向下的方向搜索；如果機器人處于如圖4（b）所示外角點位置，則其應以線段La為障礙物，繼續(xù)沿當前方向搜索，直至遠離線段La后，再沿目標位置方向搜索；如果機器人處于如圖4（c）所示外角點位置，則選擇與當前搜索方向相交的線段Lb作為障礙物，按前述方法繼續(xù)搜索。

2.4 障礙物端點繞行問題

當機器人沿障礙物線段搜索并遠離該障礙物后，如果目標位置與機器人當前搜索方向相反，則可能出現機器人與目標位置的連線始終和障礙物相交的情況，如圖5中桔色虛線箭頭所示。在障礙物端點處機器人應采取繞行的方式，即先沿垂直于障礙物的方向移動一段距離，離開障礙物后再繼續(xù)沿目標位置搜索。圖5 中，機器人在藍色虛線〇位置時，dpi大于安全距離dsafe，表示機器人已離開當前障礙物線段Li，然后搜索轉向沿與障礙物線段垂直的方向，在藍色實線〇位置，大于安全距離dsafe，則可轉向目標位置方向搜索，其中虛線與實線桔色箭頭分別表示機器人在虛線與實線位置與目標位置的方向。

圖5 線段端點繞行搜索示意圖Fig.5 Schematic diagram of searching for going around endpoint of line segment

2.5 路徑優(yōu)化

由于搜索時，大部分路徑是沿著與障礙物線段平行方向搜索，搜索后路程最短的一條也可能不是最優(yōu)路徑，還需要對其進行優(yōu)化，刪除冗余節(jié)點，如圖3（c）中pt1。在待優(yōu)化的路徑中，假設連續(xù)相鄰的3個節(jié)點分別是pi-1、pi、pi+1，如果由節(jié)點pi-1和pi+1作為端點構成的線段Lpi與地圖內任意障礙物線段Li均不相交，且與Li之間距離大于安全距離dsafe，則可將節(jié)點pi從路徑中刪除，否則，節(jié)點pi不能刪除。

相鄰兩節(jié)點間路徑如果過長，則可能導致無法合并的情況，同時為了提高優(yōu)化算法的計算效率，使用了一種變分割參數的方法。算法流程圖如圖6 所示，disdiv為分割參數，插入的新節(jié)點方向應與該段路徑移動方向一致，優(yōu)化結束后，若某節(jié)點與其前一節(jié)點的方向相同，則該節(jié)點可作為多余節(jié)點刪除。

圖6 路徑優(yōu)化算法流程圖Fig.6 Flow chart of path optimization algorithm

3 DWA算法及改進

DWA 是目前較為成熟，且在柵格地圖中得到廣泛使用的一種局部路徑規(guī)劃算法。該算法在機器人運動過程中，通過機器人自身的運動特性和給定的預估周期，計算出下一采樣周期內機器人所能達到的速度空間(vt,ωt)。然后，通過目標函數計算出在該范圍內機器人下一時刻的最優(yōu)線速度與角速度。由于目標函數同時考慮了目標方向、機器人速度及其與障礙物柵格間的距離，可以較好地實現機器人的避障功能。在特征地圖中，應使用式（2）進行障礙物檢測。

3.1 傳統(tǒng)DWA算法

DWA 實現的是一個速度空間約束的優(yōu)化問題，根據機器人自身硬件條件的限制，其應滿足速度、動力學和安全三項約束，以此來確定機器人運動時的速度范圍V，然后通過式（3）所示的目標函數對上述速度范圍V內一系列可行的軌跡進行評價，將目標函數G(v,ω)值最大時對應的軌跡作為最優(yōu)軌跡，則該軌跡對應的線速度vt和角速度ωt即為下一周期機器人運動的速度。

式（3）中各具體函數如下：

式中，(xt,yt)表示機器人在預測軌跡末端時的位置，θt表示機器人在(xt,yt)時的航向角與目標方向的夾角，dist則表示在(xt,yt)時機器人與所有障礙物線段間的最小距離，其中di由式（2）計算，vt表示機器人在(xt,yt)時的運動線速度；α、β、γ為權重系數，代表該項在目標函數中的影響程度。機器人在每一個采樣周期內均按照規(guī)劃的線速度和角速度前進，當機器人與目標位置之間的距離小于閾值disfth，則路徑規(guī)劃結束。為了避免權重系數差異過大，一般需要將目標函數中的三項數據進行歸一化處理。

3.2 DWA算法的改進

DWA算法在使用時，目標函數式（3）中3個分項函數的權重系數均為常數，但是機器人移動過程是動態(tài)變化的，3個函數對于規(guī)劃路徑的影響程度也因機器人所處位置的不同而不斷變化。在某些特殊位置，固定值的權重系數很容易使算法陷入局部最優(yōu)解。

圖7 所示為當機器人起點位置與目標位置之間存在障礙物，α、β和γ不同取值時規(guī)劃路徑的結果，其中○代表起點位置，*表示目標位置，黑色粗實線表示障礙物，藍色細實線表示規(guī)劃路徑。

圖7 不同權重系數時規(guī)劃路徑的情況Fig.7 Path planning with different weight coefficients

由于dist函數主要影響機器人與障礙物之間的距離，主要改變了α和γ兩個參數。在圖7（a）中，當α=1，β=1，γ=1 時，初始階段heading函數項影響較大，機器人的移動方向即為最優(yōu)方向。為了避免碰撞，dist函數使機器人減速，直至停止移動而無法繼續(xù)規(guī)劃路徑；增大velocity函數的權重，可以使機器人提前繞開障礙物而改變速度方向，但在接近目標位置時由于速度影響過大而出現發(fā)散，始終無法到達目標位置；只有當α和γ取值比例合適時，機器人才能順利到達目標位置，如圖7（e）所示。

通過分析得出，機器人與目標位置之間的距離distgoal、與障礙物之間的最小距離distobt和α、β、γ有密切關系，α與distgoal成反比，與distobt成正比，β與distobt成反比，γ與distgoal成正比。因此，本文對DWA 算法中的目標函數進行了改進，得到新的目標函數為：

另一方面教師在教學過程中大量采用項目化與“基于工作過程”的項目化教學改革，通過教學方式的改革將課堂教學模式變得活躍，同時讓學生參與度提高，也能讓學生體驗到將來工作崗位所需要完成的工作任務，對比傳統(tǒng)教學在教學效果上有較大提高。但在實施過程中仍然會出現部分學生學習積極性較差以及各課程課外任務繁重，最后學生失去完成課程項目任務耐性的現象。

改進后的DWA 算法降低了對目標函數中權重系數的敏感性，減小了陷入局部最優(yōu)的可能，從而提高了路徑規(guī)劃的成功率。

4 融合算法

由搜索優(yōu)化算法得到的全局最優(yōu)路徑節(jié)點集合Pathnode為：

其中，Ps為起點，Pe為終點，Pi為中間節(jié)點。

路徑中的n+2 個節(jié)點將全局路徑分為n+1 段局部路徑，則局部路徑集合Path為：

在(Ps,P1)段內，使用改進的DWA算法規(guī)劃由Ps到P1的路徑，當機器人與P1點間的距離小于閾值disfth時，該段局部路徑規(guī)劃結束，將機器人當前位置、航向角及速度作為初始參數，繼續(xù)使用改進的DWA 算法完成下一段(P1,P2)內的局部路徑規(guī)劃，直至機器人到達終點Pe附近，各段局部路徑規(guī)劃的結果合并即為最終規(guī)劃的完整路徑。

使用融合算法時，為了保證局部路徑規(guī)劃與全局路徑規(guī)劃的結果相符，在目標函數式（5）的基礎上增加了distpath(v,ω)項，即：

distpath(v,ω)函數為：

其中，dpl(v,ω)表示機器人在預測軌跡末端時，其與該段局部路徑內始末點連線間的距離，δ為distpath(v,ω)函數的權重系數，為了避免該項函數值為負，distpath值最小為0。

機器人在局部路徑規(guī)劃時的終點Pi處（全局路徑的中間節(jié)點）會出現不必要的減速，因此，判斷算法停止的閾值disfth應不小于安全距離dsafe；同時，根據DWA 算法原理，在Pi點附近時，如果預測軌跡末端越過Pi點，夾角θ將急劇增大，heading項函數值迅速減小，也會使機器人出現明顯減速。如圖8（a）所示，藍色圓圈表示機器人，細實線表示可行軌跡，*表示局部路徑的終點Pi，淺藍色虛線圓圈、綠色箭頭和紅色箭頭分別表示機器人在預測軌跡末端時的位置、航向角和其與Pi點之間的方位角。針對該情況，在以中間節(jié)點Pi為局部路徑規(guī)劃終點計算heading函數時，使用夾角θ′，即heading=π-θ′，如圖8（b）所示，紅色細箭頭表示機器人在當前位置與Pi點之間的方位角。

圖8 heading 函數中θ 角改進前后示意圖Fig.8 Schematic diagram of θ before and after improvement in heading function

5 仿真實驗與分析

為驗證本文算法的性能及有效性，在Matlab2016b中對算法進行了仿真驗證和對比分析，計算機平臺CPU 為Intel Core i5-6300U，內存8 GB，操作系統(tǒng)為64位Windows 10。

5.1 搜索優(yōu)化算法仿真

模擬室內的復雜環(huán)境，以某樓梯形平臺為基礎，使用7 個不同的立方體作為障礙物，建立如圖9所示仿真環(huán)境的特征地圖，任意選取兩點作為起點和終點，分別使用搜索優(yōu)化算法和A*算法進行路徑規(guī)劃。

圖9 搜索路徑與A*算法規(guī)劃路徑對比Fig.9 Path planning comparison of searching path with A*algorithm

搜索算法中參數dsafe=0.3 m，step=0.08 m，經過10 次搜索得到2 條可行路徑，優(yōu)化后計算總耗時為1.1 s。A*算法中柵格的分辨率為0.1 m，計算耗時共12.6 s。圖9 中粗實線表示障礙物，藍色圓圈表示起點（1.5,-1），桔色三角表示終點（7,-2.5），藍色虛線與綠色點線分別表示搜索出的兩條可行路徑，其中綠色點線為路程最短，經過優(yōu)化后的最優(yōu)路徑為紅色細實線，紫色點劃線為A*算法規(guī)劃的路徑。由結果可以發(fā)現，搜索得到的路徑優(yōu)化后明顯更短，與A*算法規(guī)劃的路徑相似，但在計算耗時方面要遠優(yōu)于A*算法。

5.2 改進DWA算法仿真

為驗證改進后DWA 算法規(guī)劃路徑的性能及其對目標函數中全局參數的敏感性，在圖7所示的仿真環(huán)境中，使用不同的權重系數分別進行路徑規(guī)劃，結果為圖10和表1所示。

表1 目標函數改進前后規(guī)劃路徑性能參數對比Table 1 Comparison of planning path performance parameters before and after objective function improvement

圖10 改進目標函數后規(guī)劃的路徑Fig.10 Path after improving objective function

圖10 為α1=1，α2=1，β=1，γ=2 時規(guī)劃的路徑及各項函數權重變化的趨勢。表1 為不同權重系數下規(guī)劃路徑的性能參數對比。其中，移動耗時為機器人按規(guī)劃的路徑點移動時耗費的全部時間，路徑平滑度的定義為：規(guī)劃的路徑點中，曲率半徑大于0.5 m的點在全部路徑點中的占比。由圖10（b）可以發(fā)現，目標函數中的3個分項函數權重的變化趨勢與前文分析一致，符合機器人在不同位置時的運動規(guī)律。

表1 中，使用改進后的目標函數，除第7 組無法完成，其余組均完成路徑規(guī)劃，而且路徑的性能均優(yōu)于目標函數改進前，說明改進后的算法對權重系數的敏感性較低。算法中使用的其他參數：采樣周期2 s，最大線速度1 m/s，最大角速度30(°)/s，速度和角速度分辨率分別為0.2 m/s 和1(°)/s，Δt=0.1 s，disfth=0.06 m，dsafe=0.3 m。

5.3 融合算法仿真及對比分析

進一步驗證提出的融合算法有效性及計算效率，在圖9 中選取6 組起點與終點，分別采用本文融合算法、文獻[25]中的融合算法以及文獻[19]中的Dijkstra與DWA融合的算法進行路徑規(guī)劃，以下簡稱算法A、B 和C，結果如圖11 和表2 所示。紅色細實線、藍色虛線和綠色點劃線分別表示A、B、C 算法規(guī)劃的路徑，*、+和△分別表示由搜索優(yōu)化算法、改進A*算法和Dijkstra算法規(guī)劃出的路徑節(jié)點。表2中對三種算法規(guī)劃路徑性能進行對比，最小距離為整條路徑中機器人與障礙物間的最小距離。算法B 和C由于規(guī)劃路徑的最小距離均遠小于安全距離，在進行局部路徑規(guī)劃時增加了0.3 m（安全距離）的膨脹距離。算法A中α1=1，α2=1，β=1，γ=3，δ=5。算法B 和C 中局部路徑規(guī)劃的目標函數使用式（10），α=2，β=1，γ=3，δ=5，其他參數與前文相同。

表2 三種融合算法規(guī)劃路徑性能對比Table 2 Comparison of path performance of three fusion algorithms

圖11 三種融合算法規(guī)劃路徑對比Fig.11 Comparison of three fusion algorithms for planned path

由表2 中對比結果發(fā)現，算法A、B、C 均能順利規(guī)劃出全部路徑。在路徑總里程方面，算法A的路徑最短，與算法B 相比最多短12.32%（第1 組），與算法C相比最多短14.15%（第3組）。在最小距離方面，算法A略小于另兩種算法，但是算法B和C的最小距離是在增加膨脹距離后得到的。算法A中的最小距離主要由搜索優(yōu)化時的dsafe決定，并且可以改變dsafe的值來改變最小距離，而A*和Dijkstra算法規(guī)劃的路徑節(jié)點離障礙物更近，且無法改變，只能通過局部路徑規(guī)劃時增加膨脹距離來避免機器人與障礙物發(fā)生碰撞。機器人移動耗時方面，算法A 均優(yōu)于算法B 和C，移動耗時最多分別減少28.66%（第1組）和45.96%（第3 組），最少分別減少17.30%（第5 組）和19.90%（第5 組），排除路徑總里程的影響因素（第4 組），說明算法A規(guī)劃的路徑可以獲得更高的平均線速度，同時算法A消除了機器人在中間節(jié)點出現的不必要減速，使機器人移動過程更平順。在平滑度方面，三種算法規(guī)劃的路徑均較好，但算法A 和B 優(yōu)于算法C。在計算耗時方面，算法A 明顯優(yōu)于算法B 和C，計算耗時最多分別減小79.27%（第6 組）和68.33%（第2組），最少分別減小43.16%（第5 組）和62.67%（第1組），主要由于算法A中基于特征地圖的搜索優(yōu)化算法相較于A*和Dijkstra算法計算效率較高，隨著規(guī)劃路徑長度的增加和地圖復雜程度的提高，這種優(yōu)勢將越加明顯。另外，算法A在中間節(jié)點較少的情況下仍能得到質量更高的路徑，說明改進后的DWA算法的適應性更好。因此，經過綜合對比，在復雜環(huán)境下，本文融合算法能夠實現規(guī)劃的路徑更優(yōu)，同時計算效率更高。

6 結論

利用特征地圖模型數量少、計算效率高的優(yōu)點，提出一種基于特征地圖且融合了搜索優(yōu)化與改進DWA 的路徑規(guī)劃算法，該算法適用于室內復雜環(huán)境中機器人的路徑規(guī)劃。首先，給出了能夠用于路徑規(guī)劃的特征地圖表達方式，并針對特征地圖，改進了障礙物檢測方法。然后，結合Bug算法的基本原理和線段特征具有方向性等特點，提出基于特征地圖的搜索優(yōu)化算法，實現全局最優(yōu)路徑的快速搜索，并針對搜索過程中內外角點搜索方向選擇、障礙物端點繞行等問題進行了分析并提出解決方法。接下來，針對DWA算法對全局參數敏感、容易陷入局部最優(yōu)的問題，分析了目標函數中各分項函數在路徑規(guī)劃過程中的影響權重，提出了新的目標函數。最后，在算法融合時，為避免機器人在中間節(jié)點出現明顯減速，對目標函數中heading項的計算方法進行了改進。在同等仿真條件下進行對比驗證，結果表明搜索優(yōu)化算法具有較高的計算效率，改進后的DWA能夠降低其對全局參數的敏感性而且規(guī)劃的路徑質量更高，提出的融合算法與其他融合算法相比，規(guī)劃的路徑總里程最多減小14.15%，移動耗時最多減小45.96%，計算耗時最多減小79.27%，而且機器人移動更平滑，特別適合于室內復雜場景中的路徑規(guī)劃。