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

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

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

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

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

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

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

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

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

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

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

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

()=()+()

(1)

式中，()為起始點(diǎn)至目標(biāo)點(diǎn)的預(yù)估消耗；()為起始點(diǎn)至當(dāng)前節(jié)點(diǎn)的實(shí)際消耗；()為當(dāng)前節(jié)點(diǎn)至目標(biāo)點(diǎn)的估計(jì)消耗，也稱為啟發(fā)函數(shù)。

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

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

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

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

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

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

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

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

()=()+()

(2)

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

()=()+e()()

(3)

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

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

..路徑平滑處理

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

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

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

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

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

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

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

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

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

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

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

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

2 局部路徑規(guī)劃人工勢(shì)場(chǎng)算法改進(jìn)

選擇實(shí)時(shí)性強(qiáng)的人工勢(shì)場(chǎng)算法用于無(wú)軌膠輪車井下無(wú)人駕駛局部路徑規(guī)劃的基本算法，通過(guò)改進(jìn)處理使其滿足需求。

2.1 人工勢(shì)場(chǎng)算法井下應(yīng)用局限性分析

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

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

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

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

圖6 人工勢(shì)場(chǎng)中車輛受力示意Fig.6 Schematic diagram of vehicle force in artificial potential field

..傳統(tǒng)人工勢(shì)場(chǎng)算法井下應(yīng)用存在的問(wèn)題

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

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

2.2 基于巷道斥力勢(shì)場(chǎng)的人工勢(shì)場(chǎng)算法改進(jìn)

針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)算法存在的局部最優(yōu)解和目標(biāo)不可達(dá)問(wèn)題，筆者將從2個(gè)方面進(jìn)行改進(jìn)，并建立移動(dòng)障礙物相對(duì)速度斥力勢(shì)場(chǎng)。

..斥力勢(shì)場(chǎng)修正因子

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

(4)

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

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

圖7 巷道空間約束勢(shì)場(chǎng)Fig.7 Roadway space constrained potential field

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

引入斥力勢(shì)場(chǎng)修正因子對(duì)障礙物斥力勢(shì)場(chǎng)進(jìn)行修正，修正后的斥力勢(shì)場(chǎng)函數(shù)為

(5)

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

由式(5)可知，無(wú)人車到達(dá)目標(biāo)地點(diǎn)時(shí)，=，無(wú)人車不受斥力勢(shì)場(chǎng)影響，因此引入修正因子可使無(wú)人車到達(dá)終點(diǎn)時(shí)處于0勢(shì)能點(diǎn)。

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

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

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

(6)

(7)

(8)

式中，與為的2個(gè)分力，這2個(gè)分力指向不同方向，的方向?yàn)檎系K物到無(wú)人車，的方向與相反。

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

(1) 0<<1時(shí)

(9)

(10)

無(wú)人車接近目標(biāo)位置時(shí)，→0，→∞，無(wú)人車在和的合力作用下駛向目標(biāo)位置。

(2)=1時(shí)

(11)

(12)

無(wú)人車接近目標(biāo)位置時(shí)，趨近于0，趨近于一個(gè)常量，無(wú)人車在和的合力作用下駛向目標(biāo)位置。

(3)>1時(shí)

(13)

(14)

無(wú)人車接近目標(biāo)位置時(shí)，趨近于0，也趨近于0，無(wú)人車在的作用下駛向目標(biāo)位置。

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

圖9 解決目標(biāo)不可達(dá)仿真實(shí)例Fig.9 Simulation example of solving target unreachable

..出逃力引入

對(duì)斥力函數(shù)進(jìn)行修正后，無(wú)人車的目標(biāo)不可達(dá)問(wèn)題得到解決，但仍無(wú)法解決局部極小值問(wèn)題。為此，筆者引入了出逃力。在無(wú)人車陷入局部極小點(diǎn)后，判斷是否到達(dá)目標(biāo)點(diǎn)，如若沒(méi)有，由出逃力幫助無(wú)人車逃出局部極小值點(diǎn)，出逃力的大小隨機(jī)賦值，但其方向需與引力方向或斥力方向垂直。然而，巷道比較特殊，如若在單車道巷道中陷入局部極小值點(diǎn)，代表前方有障礙物不可通行，如圖10所示，此種情況下不用引入出逃力，應(yīng)立即停車,引入出逃力后程序運(yùn)行流程如圖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，起點(diǎn)設(shè)置為(0,0)，終點(diǎn)設(shè)置為(90,90)，出逃力以隨機(jī)數(shù)的方式給出，仿真實(shí)例如圖12所示，在陷入局部極小值后，出逃力將無(wú)人車帶離局部極小值點(diǎn)，繼續(xù)向前行駛，到達(dá)目標(biāo)點(diǎn)。

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

..相對(duì)速度勢(shì)場(chǎng)建立

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

(15)

無(wú)軌膠輪車與動(dòng)態(tài)障礙物相對(duì)速度勢(shì)場(chǎng)斥力為

(16)

式中，為無(wú)軌膠輪車當(dāng)前速度矢量；為動(dòng)態(tài)障礙物當(dāng)前速度矢量；為相對(duì)速度勢(shì)場(chǎng)系數(shù)。

2.3 基于改進(jìn)人工勢(shì)場(chǎng)算法的局部規(guī)劃仿真

無(wú)軌膠輪車在巷道內(nèi)的局部路徑規(guī)劃場(chǎng)景需求主要有：?jiǎn)诬嚨烙鰧?duì)向來(lái)車、雙車道遇對(duì)向來(lái)車以及交叉巷道中2個(gè)及以上方向有來(lái)車時(shí)的會(huì)車避讓，下面將對(duì)上述3種場(chǎng)景進(jìn)行局部路徑規(guī)劃仿真試驗(yàn)。

..單車道會(huì)車路徑規(guī)劃仿真試驗(yàn)

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

圖13 單車道巷道會(huì)車過(guò)程示意Fig.13 Schematic diagram of the meeting process of single-lane roadway

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

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

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

圖15 雙車道巷道會(huì)車過(guò)程示意Fig.15 Schematic diagram of the two-lane roadway meeting process

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

..交叉巷道會(huì)車仿真

圖17 交叉巷道會(huì)車過(guò)程示意Fig.17 Schematic diagram of the crossing roadway meeting process

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

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

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

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

3.1 試驗(yàn)場(chǎng)景搭建

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖26 人工勢(shì)場(chǎng)法試驗(yàn)起點(diǎn)與終點(diǎn)設(shè)置Fig.26 Setting of test starting point and end point of APF

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

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

圖28 微型無(wú)人車避障過(guò)程Fig.28 Mini driverless vehicle obstacle avoidance process

圖29 全局路徑與實(shí)際路徑對(duì)比Fig.29 Comparison between global path and actual path

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

4 結(jié) 論

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

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

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