基于改進A*和人工勢場算法的無軌膠輪車井下無人駕駛路徑規(guī)劃

鮑久圣，張牧野，葛世榮，劉 琴，袁曉明，王茂森，陰 妍，趙 亮

(1.中國礦業(yè)大學 機電工程學院，江蘇 徐州 221116；2.中國礦業(yè)大學(北京) 機電與信息工程學院 北京 100083；3.中國煤炭科工集團太原研究院有限公司，山西 太原 030006)

近年來，我國開始加快推進煤礦智能化建設(shè)，無軌膠輪車作為煤礦井下重要輔助運輸裝備，無人駕駛是其智能化發(fā)展的必然方向。無人駕駛技術(shù)主要包括感知、定位、規(guī)劃、決策等幾大部分，其中路徑規(guī)劃是無人駕駛執(zhí)行過程中尤為重要的環(huán)節(jié)。由于井下運輸任務存在不確定性，巷道工況也存在未知性，因此需針對無軌膠輪車井下行駛巷道運輸路線，研究科學高效的路徑規(guī)劃算法從而實現(xiàn)最優(yōu)路徑行駛。同時由于井下無軌膠輪車的工作環(huán)境中存在岔路口造成的盲區(qū)較多、未知突發(fā)情況較多、井下照度低、場景單一導致的定位依據(jù)少、網(wǎng)絡(luò)改造力度不夠等情況，且其研發(fā)需要考慮礦區(qū)的行業(yè)標準和安全生產(chǎn)指標，故其在工作中需要著重考慮路徑選擇和避障工作的先進性和準確性。

路徑規(guī)劃包括2種：一種是依據(jù)已知環(huán)境的前提下尋找從初始位置至目標位置的可通行路徑問題，即全局路徑規(guī)劃；另一種是依據(jù)部分或全部環(huán)境信息未知情況下的障礙物規(guī)避問題，即局部路徑規(guī)劃。目前，無人駕駛車輛的路徑規(guī)劃算法已有較多研究，全局路徑規(guī)劃主要有A*算法、蟻群算法、遺傳算法等；局部路徑規(guī)劃中常用的有人工勢場算法、神經(jīng)網(wǎng)絡(luò)算法、粒子群算法等。不同算法均存在各自的優(yōu)缺點，其中A*算法相比于其他全局路徑規(guī)劃算法，自身的可移植性和可塑性較強，簡單易實現(xiàn)，計算量小，復雜度低，只要存在起始點至目標點的路徑就一定能找到最優(yōu)路徑，在全局路徑規(guī)劃中應用較為廣泛，但是當障礙物密集、環(huán)境復雜時，A*算法效率較低，啟發(fā)性信息具有很大的主觀性選擇難度加大，難以搜索出最優(yōu)路徑。而且，在傳統(tǒng)A*算法中，沒有對路徑進行平滑處理，因此規(guī)劃路徑的轉(zhuǎn)折比較劇烈，不利于移動機器人的路徑跟蹤。人工勢場算法是一種最為常見的局部路徑規(guī)劃方法，利用該方法可以實時避開障礙物且能規(guī)劃出平滑的軌跡，實時性強、便于底層實時控制、規(guī)劃路徑較為平滑，有利于無人車實際控制。但是，人工勢場法僅僅利用局部信息，可能會使得機器人陷入局部極值點或者不能到達目標位置。當不同的勢場綜合作用的效果大小相同、方向相反時，還會出現(xiàn)往復運動的可能。

針對傳統(tǒng)A*算法和人工勢場法的不足，已有不少學者進行了改進研究。例如，汪首坤等提出了A*算法的變步長分段搜索法，解決了應用傳統(tǒng)A*算法搜索數(shù)據(jù)量大和搜索死循環(huán)等問題，提高了運算效率和避障效果；文獻[10]提出跳點搜索策略，減少了遍歷過程中所需訪問的節(jié)點數(shù)，運行速度很快，但路徑中仍存在轉(zhuǎn)折點；文獻[11]提出將混沌理論的搜索算法引入人工勢場法的勢場函數(shù)中，改變障礙物的斥力系數(shù)和目標點的引力系數(shù)，該方法解決了傳統(tǒng)人工勢場法的缺陷，如局部最優(yōu)問題等；丁家如等采用預規(guī)劃基于威脅分布的全局性信息的方法，彌補人工勢場法易陷入局部最小而無法找到可行路徑的不足。在井下機器人路徑規(guī)劃方面，針對傳統(tǒng)算法的不足，程新景基于A*算法和DWA算法對煤礦救援機器人進行了全局路徑與局部路徑的研究；劉停在機器視覺的基礎(chǔ)上，基于粒子群和蟻群算法的改進融合對井下救援探測機器人的路徑規(guī)劃問題進行了研究。但是，目前對于井下無軌膠輪車路徑規(guī)劃算法的研究尚鮮有涉及。

煤礦井下巷道空間狹小，在長、寬、高三維空間方向上均受到限制，多數(shù)情況下無軌膠輪車井下行駛路徑相對固定，但在行駛途中經(jīng)常會遭遇往來行人、車輛等動態(tài)障礙物，因此可以考慮選擇簡單易實現(xiàn)且計算量小的A*算法用于無軌膠輪車的全局路徑規(guī)劃，選擇計算量小、適合底層實時控制、動態(tài)避障效果好的的人工勢場算法用于無人駕駛無軌膠輪車局部路徑規(guī)劃。筆者將在傳統(tǒng)A*算法和人工勢場算法的基礎(chǔ)上，針對無軌膠輪車井下無人駕駛的路徑規(guī)劃需求對其進行算法改進研究。

1 全局路徑規(guī)劃A*算法改進

筆者基于對常用路徑規(guī)劃算法的對比分析，選擇簡單易實現(xiàn)的A*算法用于無人駕駛無軌膠輪車全局路徑規(guī)劃的基本算法，并針對井下行駛工況對其進行改進處理。

1.1 傳統(tǒng)A*算法井下應用局限性分析

A*算法建立在Dijkstra算法和最佳優(yōu)先搜索(BFS)基礎(chǔ)之上，是一種啟發(fā)式搜索算法。其中，Dijkstra算法由起始點對圖中全部可行點進行遍歷，搜索一條最優(yōu)路徑，BFS則是從起點向外搜索至目標點。A*算法在BFS算法的基礎(chǔ)上增加目標導引，既保證圖搜索概率的完備性，也提高路徑規(guī)劃效率。

..傳統(tǒng)A*算法原理

A*算法是經(jīng)典的啟發(fā)式搜索算法，它是在經(jīng)典單源路徑算法Dijkstra算法的基礎(chǔ)上改進而來。其最顯著的特點就是，通過在搜索過程中給定啟發(fā)函數(shù)來減少搜索節(jié)點，從而提高路徑搜索效率。A*算法的估價函數(shù)()表示為

()=()+()

(1)

式中，()為起始點至目標點的預估消耗；()為起始點至當前節(jié)點的實際消耗；()為當前節(jié)點至目標點的估計消耗，也稱為啟發(fā)函數(shù)。

A*算法路徑規(guī)劃過程如下：定義初始位置和目標位置后，A*算法有方向有目的的向目標位置搜索，尋找離當前點()最小的節(jié)點，尋找到該點后，將該點作為下一個基礎(chǔ)點，繼續(xù)尋找距該基礎(chǔ)點()值最小的點，循環(huán)上述過程直到尋得目標位置所在點。

..傳統(tǒng)A*算法井下應用存在的問題

圖1 傳統(tǒng)A*算法規(guī)劃的路徑Fig.1 Path planning with traditional A* algorithm

A*算法是一種易實現(xiàn)的算法，但將其直接應用于無人駕駛無軌膠輪車的全局路徑規(guī)劃是不合適的，原因在于其優(yōu)化后得到的路徑冗余點較多，且該方法得到的運動路線折線多、轉(zhuǎn)折次數(shù)多、轉(zhuǎn)折角大，這些缺陷嚴重影響了路徑規(guī)劃的效果。這對于無軌膠輪車的控制非常不利。此外，A*算法是一種搜索式算法，搜索過程需要耗費大量時間，如圖1所示，對于無軌膠輪車來說，大部分時間運行在無障礙區(qū)域，如果能夠加快A*算法在無障礙物區(qū)域的搜索效率，減少在無障礙物區(qū)域的搜索量，這無疑將提升路徑規(guī)劃的效率。

1.2 基于指數(shù)加權(quán)和樣條插值的A*算法改進

針對傳統(tǒng)A*算法存在的搜索速度慢、搜索節(jié)點多、路徑不平滑的缺點，筆者從以下2個方面進行A*算法改進。

..估價函數(shù)優(yōu)化

A*算法在搜索路徑時，啟發(fā)函數(shù)值決定了其搜索效率，由于當前節(jié)點到目標點所消耗的()總是小于當前點至目標點的實際消耗，所以在搜索過程中需要搜索過多的節(jié)點。如果當前節(jié)點到目標點的估計消耗等于實際消耗，那只需1次搜索即可到達目標點。如果能夠提升估計消耗()的權(quán)重，搜索的節(jié)點數(shù)將會減少，算法效率將得到提升，基于此方法，趙真明等對啟發(fā)函數(shù)進行了加權(quán)：

()=()+()

(2)

其中，為權(quán)重系數(shù)。經(jīng)過加權(quán)改進后，算法的搜索效率得到了提升。但在實際搜索過程中，隨著當前節(jié)點逐漸接近目標點，預估消耗()會逐漸接近真實消耗，此時權(quán)值應逐漸接近于1，使用固定的權(quán)重系數(shù)顯然無法在接近目標點使預估消耗接近真實消耗。因此，筆者在文獻[20]的方法基礎(chǔ)上進行改進，使用預估消耗的指數(shù)函數(shù)作為啟發(fā)函數(shù)的加權(quán)系數(shù)，優(yōu)化后的估價函數(shù)為

()=()+e()()

(3)

圖2 加權(quán)A*算法規(guī)劃的路徑Fig.2 Path planning with weighted A* algorithm

在式(3)中，隨著當前節(jié)點逐漸接近目標點，預估消耗()逐漸趨近于0，加權(quán)系數(shù)逐漸趨近于1。圖2為加權(quán)后的A*算法搜索節(jié)點數(shù)，相比圖1規(guī)劃的結(jié)果大大減少。傳統(tǒng)A*算法搜索節(jié)點數(shù)為123個，搜索時間為0.875 4 s，改進后算法搜索節(jié)點數(shù)為66個，搜索時間為0.182 6 s，計算可得加權(quán)后的A*算法搜索時間僅為傳統(tǒng)A*算法的20%左右。

..路徑平滑處理

啟發(fā)函數(shù)加權(quán)改進后的A*路徑算法能夠快速搜索出1條從起點至目標點的路徑，不過路徑上存在拐點的情況依然沒有解決，無軌膠輪車跟蹤這樣的路徑時會出現(xiàn)較大且不平穩(wěn)的轉(zhuǎn)向，不利于無軌膠輪車的實際控制，需對這樣的路徑進行平滑處理。常用的路徑平滑方式有高階貝塞爾曲線、圓弧處理和三次樣條插值法等。對于三次樣條曲線，只需給定路徑坐標信息和邊界條件即可擬合出1條光滑的曲線，筆者選擇三次樣條插值對改進后A*算法規(guī)劃的路徑進行平滑處理。

三次樣條插值定義如下：如果存在這樣的分段函數(shù)()，在+1個離散點構(gòu)成的個不同的任一區(qū)間[，+1](=0，1，2，…，-1，遞增)中，()=()都是三次多項式，且在任意點處都滿足()=(=0，1，2，…，)，那么可將這樣的函數(shù)稱作三次樣條插值函數(shù)。()具有二階導數(shù)，且其二階導數(shù)在區(qū)間[，]內(nèi)是連續(xù)的，也可以理解為()曲線是一條光滑曲線，因此使用該函數(shù)擬合出的路徑也必然是光滑的。

對于使用指數(shù)函數(shù)加權(quán)后A*算法規(guī)劃出的路徑，不需要對全部路徑點進行三次樣條插值，只需對轉(zhuǎn)折處進行插值擬合即可，使用三次樣條差值平滑處理前后的路徑對比如圖3所示。圖3(a)為使用指數(shù)函數(shù)加權(quán)后A*算法規(guī)劃出的路徑，轉(zhuǎn)折點較多，路徑不平滑，圖3(b)為使用三次樣條插值法處理后的路徑，雖然還有一些微小的轉(zhuǎn)折，但路線基本平滑。

1.3 基于改進A*算法的全局規(guī)劃仿真

無軌膠輪車在井下行駛時經(jīng)常通過聯(lián)絡(luò)巷變換到其他巷道，如圖4所示。本節(jié)使用指數(shù)函數(shù)加權(quán)后A*算法模擬無障礙物情況下無軌膠輪車從當前巷道(圖4中巷道1)通過聯(lián)絡(luò)巷進入另一條巷道(圖4中巷道2)時的路徑規(guī)劃，對規(guī)劃出的路徑使用三次樣條插值法進行平滑處理，仿真試驗相關(guān)參數(shù)設(shè)置見表1。

指數(shù)加權(quán)后A*算法規(guī)劃的路徑如圖5(a)所示，使用三次樣條插值處理后的路徑如圖5(b)所示。

由圖5可看出，路徑規(guī)劃過程中搜索節(jié)點數(shù)較少，都在其規(guī)劃的路徑上，具體搜索節(jié)點數(shù)為37個，所用時間為0.075 74 s。同時，也應該看到使用指數(shù)加權(quán)后的A*算法不能消除路徑上的拐點，因此需要使用三次樣條插值法對指數(shù)函數(shù)加權(quán)后A*算法規(guī)

圖3 平滑處理前后路徑對比Fig.3 Path comparison before and after smoothing

圖4 巷道變換示意Fig.4 Schematic diagram of roadway change

表1 A*算法路徑規(guī)劃仿真參數(shù)設(shè)置

圖5 改進后A*算法規(guī)劃的路徑Fig.5 Path planned by the improved A* algorithm

劃的路徑進行平滑處理，平滑處理后的路徑無明顯轉(zhuǎn)折點，且處理后的路徑與處理前差距在一個柵格內(nèi)，可以保證平滑處理的安全性，比處理前的路徑更符合無軌膠輪車運動學約束。

2 局部路徑規(guī)劃人工勢場算法改進

選擇實時性強的人工勢場算法用于無軌膠輪車井下無人駕駛局部路徑規(guī)劃的基本算法，通過改進處理使其滿足需求。

2.1 人工勢場算法井下應用局限性分析

人工勢場算法是一種模擬物理力場的算法，它的基本思想是將機器人在周圍環(huán)境中的運動，設(shè)計成一種抽象的人造引力場中的運動，目標點對移動機器人產(chǎn)生“引力”，障礙物對移動機器人產(chǎn)生“斥力”，最后通過求合力來控制移動機器人的運動。

..傳統(tǒng)人工勢場算法原理

無人車在行駛途中最大的人工勢場引力源自目標點，當勢力場為0時，2者之間距離為0，表示車輛已到達目標點。無人車只有在障礙物勢場范圍內(nèi)才會受到斥力勢場的影響，在障礙物勢場范圍內(nèi)，無人車受到的勢場斥力與無人車和障礙物之間的距離成反比。勢力場函數(shù)用于描述運行過程中目標點和障礙物對無人車產(chǎn)生的影響，需要考慮無人車向目標點移動的效率問題來構(gòu)建專門的函數(shù)衡量勢場的大小。

在構(gòu)建的虛擬人工勢場中，無人車受到的力是障礙物的斥力和目標點的引力的合力，該合力為矢量，合力的方向決定了無人車的行駛方向。障礙物的數(shù)量可能不止一個，因此無人車可能會受到多個斥力作用，在使用人工勢場算法做路徑規(guī)劃時，要將無人車受到的所有斥力和引力按矢量運算原則相加，在合力的作用下向目標點靠近，如圖6所示。

圖6 人工勢場中車輛受力示意Fig.6 Schematic diagram of vehicle force in artificial potential field

..傳統(tǒng)人工勢場算法井下應用存在的問題

人工勢場算法起初主要應用于機器人局部路徑規(guī)劃，隨著無人駕駛技術(shù)的發(fā)展才逐漸應用于地面車輛的路徑規(guī)劃中。機器人在使用人工勢場法進行路徑規(guī)劃時很少考慮道路的約束，但是無人車的運動狀態(tài)是時變的，障礙物數(shù)量、形狀和位置也具有不可預知性，導致無人車在構(gòu)建的虛擬勢場中會出現(xiàn)局部最優(yōu)解和目標不可達問題。當無人車在某個引力與斥力大小相等，方向相反的位置時，無人車受力為0，陷入局部最優(yōu)解。當目標點與障礙物之間的距離較近時，斥力非常大，引力相對較小，導致無人車出現(xiàn)目標不可達。

在地面無人駕駛車輛的路徑規(guī)劃過程中，經(jīng)典的做法是建立車道線勢場，此前也有研究人員將人工勢場算法用于煤礦機器人的路徑規(guī)劃，但是沒有考慮巷道的約束，故難以直接用于無軌膠輪車在井下巷道內(nèi)的無人駕駛。

2.2 基于巷道斥力勢場的人工勢場算法改進

針對傳統(tǒng)人工勢場算法存在的局部最優(yōu)解和目標不可達問題，筆者將從2個方面進行改進，并建立移動障礙物相對速度斥力勢場。

..斥力勢場修正因子

為了解決傳統(tǒng)人工勢場算法目標不可達問題，筆者針對巷道壁和障礙物的斥力分別進行修正。根據(jù)《煤礦安全規(guī)程》相關(guān)規(guī)定，無軌膠輪車與巷道壁之間的最小距離應≥0.3 m，人行側(cè)應保留至少0.8 m，因此將距離巷道壁0.8 m以內(nèi)的區(qū)域設(shè)置為高危區(qū)域，其余位置為低危區(qū)域。根據(jù)不同危險程度，建立分段函數(shù)來表示巷道空間約束勢場，在離巷道壁距離小于0.8 m的區(qū)域使用變化較快的指數(shù)函數(shù)建立巷道空間約束勢場，在離巷道壁距離大于0.8 m的區(qū)域低危險區(qū)域使用三角函數(shù)來建立巷道空間約束勢場，建立巷道空間約束勢場函數(shù)為

(4)

式中，為高危區(qū)域斥力勢場強度系數(shù)；為低危區(qū)域斥力勢場強度系數(shù)；為巷道寬度；為無軌膠輪車幾何中心距巷道壁距離；，，為常量系數(shù)，由巷道寬度決定。

取=10，=20,=8，=1，=5/22，=9/11，根據(jù)式(4)建立的巷道空間約束勢場圖，如圖7所示。

圖7 巷道空間約束勢場Fig.7 Roadway space constrained potential field

由式(4)和圖7可知，在靠近巷道壁0.8 m內(nèi)(高危區(qū)域)斥力勢場較大，且變化趨勢很快，靠近巷道中間位置的區(qū)域斥力勢場較小，變化趨勢也相對緩和，在巷道正中間巷道空間約束勢場強度為0。

引入斥力勢場修正因子對障礙物斥力勢場進行修正，修正后的斥力勢場函數(shù)為

(5)

式中，為斥力勢場強度(正比例增益因子)；為障礙物所在位置(,)；為目標點所在位置(，)；為斥力勢場的最大作用距離，為常數(shù)；-為無人車與障礙物之間的距離；-為無人車與目標點之間的距離；為修正因子系數(shù)，>0。

由式(5)可知，無人車到達目標地點時，=，無人車不受斥力勢場影響，因此引入修正因子可使無人車到達終點時處于0勢能點。

與原斥力勢場相比，引入修正因子后的斥力勢場隨著無人車和目標點之間距離的減小呈衰減趨勢，取值不同，衰減趨勢也不同。將起點設(shè)置為(0,0)目標點坐標設(shè)置為(1,1)，可得勢場衰減趨勢如圖8所示，越大，斥力勢場衰減越快，同時，如果取值過大，斥力勢場衰減過于迅速，可能導致無人車避障失敗，因此的取值應當兼顧衰減趨勢和避障效果。

圖8 不同修正因子下的斥力勢場衰減趨勢Fig.8 Attenuation trend of repulsion potential field under different correction factors

當≠時，修正后的障礙物斥力函數(shù)為

(6)

(7)

(8)

式中，與為的2個分力，這2個分力指向不同方向，的方向為障礙物到無人車，的方向與相反。

修正因子系數(shù)不但影響斥力勢場衰減趨勢，還影響無人車受力情況，具體情況為：

(1) 0<<1時

(9)

(10)

無人車接近目標位置時，→0，→∞，無人車在和的合力作用下駛向目標位置。

(2)=1時

(11)

(12)

無人車接近目標位置時，趨近于0，趨近于一個常量，無人車在和的合力作用下駛向目標位置。

(3)>1時

(13)

(14)

無人車接近目標位置時，趨近于0，也趨近于0，無人車在的作用下駛向目標位置。

取=1，斥力系數(shù)設(shè)置為100，引力系數(shù)設(shè)置為10，起點設(shè)置為(0,0)，終點設(shè)置為(80,80)，改進后的人工勢場算法仿真實例如圖9所示。由圖9可知，改進后的人工勢場算法可解決目標不可達問題。

圖9 解決目標不可達仿真實例Fig.9 Simulation example of solving target unreachable

..出逃力引入

對斥力函數(shù)進行修正后，無人車的目標不可達問題得到解決，但仍無法解決局部極小值問題。為此，筆者引入了出逃力。在無人車陷入局部極小點后，判斷是否到達目標點，如若沒有，由出逃力幫助無人車逃出局部極小值點，出逃力的大小隨機賦值，但其方向需與引力方向或斥力方向垂直。然而，巷道比較特殊，如若在單車道巷道中陷入局部極小值點，代表前方有障礙物不可通行，如圖10所示，此種情況下不用引入出逃力，應立即停車,引入出逃力后程序運行流程如圖11所示。

圖10 單車道巷道內(nèi)陷入局部極小值示意Fig.10 Schematic diagram of falling into local minimum in single lane roadway

圖11 引入出逃力后路徑規(guī)劃流程Fig.11 Path planning process after introducing escape force

斥力系數(shù)設(shè)置為100，引力系數(shù)設(shè)置為10，起點設(shè)置為(0,0)，終點設(shè)置為(90,90)，出逃力以隨機數(shù)的方式給出，仿真實例如圖12所示，在陷入局部極小值后，出逃力將無人車帶離局部極小值點，繼續(xù)向前行駛，到達目標點。

圖12 解決局部極小值仿真實例Fig.12 Solve local minimum simulation example

..相對速度勢場建立

無軌膠輪車有時也運行在其他環(huán)境，這時障礙物為動態(tài)障礙物。傳統(tǒng)人工勢場法在引入斥力衰減系數(shù)和出逃力后，可解決目標不可達和局部最優(yōu)。為了提升人工勢場算法在動態(tài)避障中的可靠性，筆者進一步引入無人車與障礙物的相對速度勢場函數(shù)：

(15)

無軌膠輪車與動態(tài)障礙物相對速度勢場斥力為

(16)

式中，為無軌膠輪車當前速度矢量；為動態(tài)障礙物當前速度矢量；為相對速度勢場系數(shù)。

2.3 基于改進人工勢場算法的局部規(guī)劃仿真

無軌膠輪車在巷道內(nèi)的局部路徑規(guī)劃場景需求主要有：單車道遇對向來車、雙車道遇對向來車以及交叉巷道中2個及以上方向有來車時的會車避讓，下面將對上述3種場景進行局部路徑規(guī)劃仿真試驗。

..單車道會車路徑規(guī)劃仿真試驗

單車道會車時避讓過程如圖13所示，假定A車為運人車輛，B車為運料車輛，根據(jù)井下行車規(guī)則：A車應減速行駛，B車應進入躲避硐室，待A車通過會車區(qū)域后，B車再駛出躲避硐室繼續(xù)向前行駛。仿真結(jié)果如圖14所示，分析可知，在單車道會車場景下當前車輛B車能規(guī)劃出合理安全的行車路徑。

圖13 單車道巷道會車過程示意Fig.13 Schematic diagram of the meeting process of single-lane roadway

圖14 單車道巷道會車路徑規(guī)劃仿真Fig.14 Single-lane roadway meeting path planning simulation

..雙車道巷道會車路徑規(guī)劃仿真

雙車道會車時避讓過程如圖15所示，假定2輛車在水平巷運行，A車為運人車輛，B車為運料車輛，根據(jù)井下行車規(guī)則：A車應減速行駛通過會車區(qū)域，B車應靠邊停車，待A車通過會車區(qū)域后B車再繼續(xù)向前行駛。仿真結(jié)果如圖16所示，分析可知，在雙車道巷道會車場景下當前車輛A車能規(guī)劃出合理安全的行車路徑。

圖15 雙車道巷道會車過程示意Fig.15 Schematic diagram of the two-lane roadway meeting process

圖16 雙車道巷道會車路徑規(guī)劃仿真Fig.16 Two-lane roadway meeting path planning simulation

..交叉巷道會車仿真

圖17 交叉巷道會車過程示意Fig.17 Schematic diagram of the crossing roadway meeting process

交叉巷道會車流程復雜，當前《煤礦安全規(guī)程》和各大煤礦都沒有針對交叉巷道制定統(tǒng)一的會車規(guī)則，筆者參考單、雙車道巷道會車策略和交叉巷道特征，規(guī)定交叉巷道發(fā)生會車時，只允許一輛車通行，制定如下會車策略：首先根據(jù)岔道車輛先行原則進行會車，即只要岔道中有來車，則其他方向車輛進入躲避硐室(單車道巷道)或靠邊停車(雙車道巷道)，等待岔道車輛先行通過會車區(qū)域，待岔道車輛通過會車區(qū)域后，如只剩一輛車即按原規(guī)劃路徑繼續(xù)行駛即可；若岔道車輛通過會車區(qū)域后還剩2輛無軌膠輪車，則判斷兩車后續(xù)路徑是否相同，若剩余兩車后續(xù)路徑相同，意味著兩車后續(xù)都將進入岔道，此時遵循就近原則進行會車，即離岔道口近的車輛先行；若路徑不相同但其中一輛車后續(xù)進入岔道，則根據(jù)“岔道先行”原則進行會車，待進入岔道的車輛駛離會車區(qū)域后，另一輛車才按原路徑繼續(xù)行駛；若后續(xù)兩車相向行駛，則表示剩余兩車均不進入岔道，此時根據(jù)直巷類型分別根據(jù)單車道巷道會車策略或雙車道巷道會車策略進行會車。假設(shè)為3個巷道均有來車，如圖17所示，當前無軌膠輪車為A車，將另外兩輛車視為靜態(tài)障礙物，仿真分析A車的路徑規(guī)劃問題。A車路徑規(guī)劃問題又可分為2種場景，場景1為出叉巷后左轉(zhuǎn)，如圖18(a) 所示，場景2為出叉巷后右轉(zhuǎn)，如圖18(b)所示。交叉巷道路徑規(guī)劃仿真結(jié)果如圖18所示，分析可知，當前車輛A在出交叉巷后，無論是左轉(zhuǎn)還是右轉(zhuǎn)，都能在有車輛的情況下規(guī)劃出安全可通行的路徑。

圖18 交叉巷道會車路徑規(guī)劃仿真Fig.18 Path planning simulation of crossing lanes

3 模擬巷道內(nèi)無人駕駛路徑規(guī)劃試驗

根據(jù)《煤礦安全規(guī)程》和國家安標中心相關(guān)政策，若要在井下開展無軌膠輪車無人駕駛試驗，需要對無人駕駛系統(tǒng)所有電氣設(shè)備進行防爆處理并取得國家安標證書，試驗成本高且周期很長。因此，筆者采取模擬試驗的方法對前文建立的改進算法進行驗證，按8∶1縮比搭建交叉巷道模型用做試驗場地，在搭建好的交叉巷道模型中使用基于ROS的微型無人車開展路徑規(guī)劃試驗。

3.1 試驗場景搭建

煤礦井下交叉巷道處多為單車道巷道與雙車道巷道相交叉，因此建立一個交叉巷道模型即可模擬交叉巷道會車和雙車道巷道會車等多種行車場景?？紤]實驗室空間布置和試驗成本問題，按大約8∶1的比例縮放搭建交叉巷道模型。雙車道巷道長度設(shè)置為4 m，寬度設(shè)置為0.6 m，單車道巷道長度設(shè)置為2 m，寬度設(shè)置為0.4 m，巷道高度設(shè)置為0.6 m，交叉巷道模型設(shè)計如圖19所示。

圖19 巷道模型設(shè)計Fig.19 Roadway model design

巷道模型搭建完畢后，使用激光雷達掃描巷道模型，基于SLAM構(gòu)建巷道模型地圖，這是微型無人車進行路徑規(guī)劃的前提，基于SLAM構(gòu)建的巷道模型地圖如圖20所示。

圖20 巷道模型地圖Fig.20 Roadway model map

圖21 基于ROS的微型無人車Fig.21 ROS-based mini driverless vehicle

本文所使用的微型無人車如圖21所示，微型無人車主要包括感知系統(tǒng)、決策系統(tǒng)、執(zhí)行系統(tǒng)和通信系統(tǒng)等。感知系統(tǒng)中激光雷達將感知到的環(huán)境信息通過USB接口傳送到?jīng)Q策系統(tǒng)，九軸陀螺儀感知到的加速度等信息首先發(fā)送給底層控制板，驅(qū)動板處理后通過串口發(fā)送給決策系統(tǒng)，主控制器處理環(huán)境、速度等信息后，根據(jù)處理結(jié)果進行下一步?jīng)Q策，將決策結(jié)果以指令的方式通過串口發(fā)送給底層控制板，控制板最后將執(zhí)行指令發(fā)送給電機驅(qū)動器和舵機驅(qū)動器，驅(qū)動電機和舵機完成相應動作。上位機通過局域網(wǎng)和無人車決策系統(tǒng)連接，對無人車實施控制。

筆者搭建微型無人車的決策系統(tǒng)使用英偉達Jetson Nano B01開發(fā)板作為主控制器，Jetson Nano B01開發(fā)板體積小、功耗低、算力強大，可支持多個高精度傳感器同時使用，廣泛應用于圖像處理、人工智能領(lǐng)域，其主要性能參數(shù)見表2。

3.2 路徑規(guī)劃模擬試驗

由于局部路徑規(guī)劃是在全局路徑規(guī)劃的基礎(chǔ)上進行的，所以首先對改進A*算法進行全局路徑規(guī)劃試驗，而后在改進A*算法規(guī)劃的路徑上進行改進人工勢場算法的局部路徑規(guī)劃試驗。

表2 Jetson Nano B01開發(fā)板參數(shù)

..全局路徑規(guī)劃模擬試驗

將路徑起點設(shè)置于巷道模型雙車道巷道入口處，終點設(shè)置在巷道模型單車道巷道出口處，如圖22所示，分別使用傳統(tǒng)A*算法和本文改進后A*算法進行路徑規(guī)劃，路徑規(guī)劃結(jié)果如圖23所示。

圖22 A*算法路徑規(guī)劃試驗起點終點設(shè)置Fig.22 Setting of starting point and ending point of A* path planning test

圖23 A*算法改進前后路徑規(guī)劃試驗結(jié)果Fig.23 Experimental results of path planning before and after A* algorithm improvement

改進前后A*算法規(guī)劃路徑對比結(jié)果如圖24所示，對比分析2條路徑可知，在雙車道直巷內(nèi)軸與軸偏差較小，軸偏差控制在±0.02 m之內(nèi)，最大偏差出現(xiàn)在模型試驗車轉(zhuǎn)彎時，為-0.031 2 m。該試驗證明，改進前A*算法規(guī)劃的路徑拐點多，改進后A*算法規(guī)劃的更加平滑，路徑更加合理，更有利于微型無人車的跟蹤。

圖24 改進前后A*算法規(guī)劃的路徑對比Fig.24 Comparison of path planning of A* algorithm before and after improvement

..局部路徑規(guī)劃模擬試驗

在模擬巷道地圖已知的情況下，指定起點與終點，可使用A*算法規(guī)劃出一條由起點至終點的全局路徑，然而當規(guī)劃好的全局路徑被未知障礙物干涉或者遇到對向來車時，無人車將無法繼續(xù)按照規(guī)劃好的全局路徑向前行駛，這時就需要使用人工勢場算法進行局部路徑規(guī)劃，待避障或會車結(jié)束后再回到全局路徑繼續(xù)向前行駛，A*-人工勢場算法聯(lián)合路徑規(guī)劃具體流程如圖25所示。

圖25 A*-人工勢場算法聯(lián)合路徑規(guī)劃流程Fig.25 A*-APF combined path planning flowchart

首先，根據(jù)建立的地圖以及指定的起點和終點規(guī)劃全局路徑，微型無人車首先按照全局路徑行駛，行駛途中遇未知障礙物時使用人工勢場算法進行局部路徑規(guī)劃，待避障結(jié)束后，再回到全局路徑，重復上述過程直到微型無人車到達終點。

本組試驗使用微型無人車和交叉巷道模型模擬無人駕駛無軌膠輪車巷道內(nèi)會車和遇未知障礙物時的避障，起點與終點設(shè)置如圖26所示，在無障礙物情況下首先使用改進A*算法規(guī)劃出全局路徑，規(guī)劃的全局路徑如圖27所示。然后在長直雙車道巷道中設(shè)置2個障礙物，障礙物1用于模擬對向來車，假定微型無人車為讓車車輛且對向來車車體較大，需借助躲避硐室進行會車；障礙物2為未知障礙物。

圖26 人工勢場法試驗起點與終點設(shè)置Fig.26 Setting of test starting point and end point of APF

有障礙物的情況下微型無人車規(guī)劃的實際路徑如圖28所示，無障礙物情況下規(guī)劃的全局路徑與有障礙物情況下的實際路徑對比如圖29所示。

圖27 無障礙物情況下改進A*算法規(guī)劃的全局路徑Fig.27 Global path planning by improved A* algorithm without obstacles

圖28 微型無人車避障過程Fig.28 Mini driverless vehicle obstacle avoidance process

圖29 全局路徑與實際路徑對比Fig.29 Comparison between global path and actual path

微型無人車首先按照規(guī)劃好的全局路徑行駛，行駛一段距離后遇到對向來車需進行避讓，此時路徑發(fā)生變化，待繞過對向來車后回到全局路徑繼續(xù)向前行駛，在遇到未知障礙物時為了安全避開障礙物路徑發(fā)生了微小的偏移，隨后回到全局路徑并按全局路徑行駛至終點。對比分析兩條路徑可知，在無障礙物的模擬巷道區(qū)域內(nèi)軸與軸偏差較小，軸偏差控制在±0.01 m之間，最大偏差出現(xiàn)在會車時，為0.035 3 m，誤差較無障礙物區(qū)域偏大，這是由于避障時車輛需要轉(zhuǎn)向所致，避讓障礙物時軸偏差在±0.02 m之間。試驗結(jié)果證明了本文所用A*-人工勢場算法規(guī)劃的全局路徑和局部路徑是合理的。

4 結(jié) 論

(1)通過指數(shù)函數(shù)優(yōu)化可減少傳統(tǒng)A*算法的計算量，提高無人駕駛無軌膠輪車的尋路效率；三次樣條插值處理使得處理后的路徑更加平滑，更適用井下巷道環(huán)境；改進A*算法的路徑搜索時間縮短為傳統(tǒng)A*算法的20%左右，且更符合井下無軌膠輪車的運動學約束。

(2建立相對速度勢場，引入斥力勢場修正因子和出逃力對傳統(tǒng)人工勢場算法進行改進，可解決其局部最優(yōu)解和目標不可達問題；改進的人工勢場算法在不同工況下均能規(guī)劃出可供無人駕駛無軌膠輪車會車或避障行駛的合理路徑。

(3)通過開展無人駕駛路徑規(guī)劃模擬試驗，發(fā)現(xiàn)改進后的A*算法規(guī)劃的全局路徑誤差在±0.02 m，改進人工勢場算法能夠在有障礙物的工況下優(yōu)化A*算法規(guī)劃出來的全局路徑，控制小車安全行駛避開障礙物，表明本文建立的改進A*-人工勢場聯(lián)合算法可滿足無軌膠輪車在煤礦井下巷道內(nèi)的路徑規(guī)劃要求。