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

劉 朋，任工昌

陜西科技大學(xué) 機(jī)電工程學(xué)院，西安 710021

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

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

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

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

1 特征地圖及障礙物檢測(cè)

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

其中，(xi,yi)為線段中點(diǎn)的坐標(biāo)，θi為線段的傾角，li為線段的長(zhǎng)度。

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

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

機(jī)器人與障礙物間距離的計(jì)算步驟為：

（1）根據(jù)機(jī)器人中心點(diǎn)pR的坐標(biāo)(xR,yR)和障礙物線段Li的特征QLi，計(jì)算投影pR′的坐標(biāo)(,)；

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

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

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

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

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

2.1 Bug算法簡(jiǎn)介

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

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

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

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

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

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

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

2.3 角點(diǎn)問(wèn)題

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

圖4 角點(diǎn)位置示意圖Fig.4 Diagram of corner point position

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

2.4 障礙物端點(diǎn)繞行問(wèn)題

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

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

2.5 路徑優(yōu)化

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

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

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

3 DWA算法及改進(jìn)

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

3.1 傳統(tǒng)DWA算法

DWA 實(shí)現(xiàn)的是一個(gè)速度空間約束的優(yōu)化問(wèn)題，根據(jù)機(jī)器人自身硬件條件的限制，其應(yīng)滿足速度、動(dòng)力學(xué)和安全三項(xiàng)約束，以此來(lái)確定機(jī)器人運(yùn)動(dòng)時(shí)的速度范圍V，然后通過(guò)式（3）所示的目標(biāo)函數(shù)對(duì)上述速度范圍V內(nèi)一系列可行的軌跡進(jìn)行評(píng)價(jià)，將目標(biāo)函數(shù)G(v,ω)值最大時(shí)對(duì)應(yīng)的軌跡作為最優(yōu)軌跡，則該軌跡對(duì)應(yīng)的線速度vt和角速度ωt即為下一周期機(jī)器人運(yùn)動(dòng)的速度。

式（3）中各具體函數(shù)如下：

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

3.2 DWA算法的改進(jìn)

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

圖7 所示為當(dāng)機(jī)器人起點(diǎn)位置與目標(biāo)位置之間存在障礙物，α、β和γ不同取值時(shí)規(guī)劃路徑的結(jié)果，其中○代表起點(diǎn)位置，*表示目標(biāo)位置，黑色粗實(shí)線表示障礙物，藍(lán)色細(xì)實(shí)線表示規(guī)劃路徑。

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

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

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

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

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

4 融合算法

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

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

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

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

使用融合算法時(shí)，為了保證局部路徑規(guī)劃與全局路徑規(guī)劃的結(jié)果相符，在目標(biāo)函數(shù)式（5）的基礎(chǔ)上增加了distpath(v,ω)項(xiàng)，即：

distpath(v,ω)函數(shù)為：

其中，dpl(v,ω)表示機(jī)器人在預(yù)測(cè)軌跡末端時(shí)，其與該段局部路徑內(nèi)始末點(diǎn)連線間的距離，δ為distpath(v,ω)函數(shù)的權(quán)重系數(shù)，為了避免該項(xiàng)函數(shù)值為負(fù)，distpath值最小為0。

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

圖8 heading 函數(shù)中θ 角改進(jìn)前后示意圖Fig.8 Schematic diagram of θ before and after improvement in heading function

5 仿真實(shí)驗(yàn)與分析

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

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

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

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

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

5.2 改進(jìn)DWA算法仿真

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

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

圖10 改進(jìn)目標(biāo)函數(shù)后規(guī)劃的路徑Fig.10 Path after improving objective function

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

表1 中，使用改進(jìn)后的目標(biāo)函數(shù)，除第7 組無(wú)法完成，其余組均完成路徑規(guī)劃，而且路徑的性能均優(yōu)于目標(biāo)函數(shù)改進(jìn)前，說(shuō)明改進(jìn)后的算法對(duì)權(quán)重系數(shù)的敏感性較低。算法中使用的其他參數(shù)：采樣周期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 融合算法仿真及對(duì)比分析

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

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

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

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

6 結(jié)論

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