郭書(shū)杰，田 華，王 偉

(大連東軟信息學(xué)院 智能與電子工程學(xué)院，遼寧 大連 116023)

0 引 言

路徑規(guī)劃是在有障礙物的環(huán)境中，按照某種評(píng)價(jià)標(biāo)準(zhǔn)尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑[1]。路徑規(guī)劃在移動(dòng)機(jī)器人、電子游戲、汽車(chē)導(dǎo)航等多個(gè)領(lǐng)域均有重要的應(yīng)用價(jià)值，是一個(gè)具有較高使用價(jià)值的研究方向。常用的路徑規(guī)劃算法有兩種：基于采樣的路徑規(guī)劃算法和基于搜索的路徑規(guī)劃算法。Dijstra[2]和A*算法[3]是典型的基于搜索的路徑規(guī)劃算法。這類算法是完備的和最優(yōu)的。由于要使用較小的步長(zhǎng)遍歷規(guī)劃空間，其規(guī)劃效率相對(duì)較低。典型的基于采樣的路徑規(guī)劃算法有PRM[4]、 RRT[5]和EST[6]等。 這類算法使用隨機(jī)采樣的方式探索規(guī)劃空間，所以具有比基于搜索的路徑規(guī)劃更高的效率?；诓蓸拥囊?guī)劃方式有一定的隨機(jī)性，僅僅是概率完備的，而且不是最優(yōu)的。為了進(jìn)一步提高算法的搜索效率，JJK Jr和Steven M. LaValle提出了一種改進(jìn)算法RRT-connect[7]。RRT-connect能夠提高RRT算法的效率，不過(guò)它仍然不是最優(yōu)的，甚至不是漸近最優(yōu)的。為了提高規(guī)劃路徑的質(zhì)量，出現(xiàn)了一些改進(jìn)算法，如RRT*、PRM*[8]、LBT-RRT[9]。這類改進(jìn)算法能夠達(dá)到漸近最優(yōu)。還有一個(gè)改進(jìn)方向是將基于搜索的相關(guān)技術(shù)引入到基于采樣的規(guī)劃中來(lái)，結(jié)合基于采樣的規(guī)劃算法和基于搜索的規(guī)劃算法的優(yōu)勢(shì)，兼顧了效率和路徑質(zhì)量，如HRRT[10]、Anytime RRT[11]、Bi-RRT*[12]、C-FOREST[13]、Informed RRT*[14]等。這類算法在一定程度上提高了算法的收斂速度，但由于它們均是基于RRT的改進(jìn)算法，所以仍然難以保證規(guī)劃出來(lái)的路徑質(zhì)量的最優(yōu)性。為了進(jìn)一步提高路徑質(zhì)量，出現(xiàn)了BIT*、AIT*和ABIT*[15-16]等算法，通過(guò)引入節(jié)點(diǎn)排序和邊排序，在限定的子集中執(zhí)行排序搜索，從而達(dá)到提高路徑質(zhì)量的目的。還有其他A*算法的改進(jìn)算法[17-18]和基于智能優(yōu)化算法的路徑規(guī)劃算法[19]。

實(shí)驗(yàn)發(fā)現(xiàn)，盡管這些改進(jìn)算法在一定程度上提高了規(guī)劃效率和路徑質(zhì)量，但在限制較多的空間中，特別是在窄通道場(chǎng)景和Z字形場(chǎng)景中，它們的規(guī)劃效率會(huì)急劇降低，難以在有限的時(shí)間內(nèi)規(guī)劃出一條路徑來(lái)。為了解決復(fù)雜場(chǎng)景下的路徑規(guī)劃問(wèn)題，提出了一種相對(duì)高效的關(guān)鍵點(diǎn)路徑規(guī)劃算法(key point path planning，KPP)，該算法在提高路徑優(yōu)化效率的同時(shí)，也優(yōu)化了規(guī)劃出來(lái)的路徑的質(zhì)量，較好地解決了特殊環(huán)境下的路徑規(guī)劃問(wèn)題。實(shí)驗(yàn)結(jié)果表明，KPP不僅能夠高效地完成窄通道場(chǎng)景和Z字形場(chǎng)景中的路徑規(guī)劃，在其他常見(jiàn)場(chǎng)景的應(yīng)用中也有不俗的表現(xiàn)。

1 路徑規(guī)劃問(wèn)題

路徑規(guī)劃就是在給定空間中，規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑，使得該路徑的代價(jià)盡可能小。記空間X?Rn為P，Xobs表示P上被障礙物占據(jù)的空間；Xfree=XXobs表示P上可以通行的空間；Dstart表示起始點(diǎn)；Dgoal表示目標(biāo)點(diǎn)；σ：[0,1]X表示連接Dstart和Dgoal之間的路徑；∑表示Dstart和Dgoal之間所有的路徑組成的集合；S：∑R表示一條路徑的代價(jià)函數(shù)；則路徑規(guī)劃問(wèn)題可以描述為：

Dgoal, ?t∈[0,1],σ(1)∈Xfree}

2 關(guān)鍵點(diǎn)路徑規(guī)劃算法

關(guān)鍵點(diǎn)路徑規(guī)劃算法基于“兩點(diǎn)之間直線最短”的原理，首先找出阻礙起始點(diǎn)與目標(biāo)點(diǎn)直線連通的障礙物KObses；然后找出繞過(guò)KObses的最近路線必須經(jīng)過(guò)的點(diǎn)作為關(guān)鍵點(diǎn)；接著以這些關(guān)鍵點(diǎn)為引導(dǎo)，將路徑規(guī)劃從隨機(jī)搜索轉(zhuǎn)換為啟發(fā)式搜索，從而提高規(guī)劃效率和路徑質(zhì)量；最后通過(guò)路徑壓縮來(lái)減少路徑長(zhǎng)短，進(jìn)一步提高規(guī)劃出的路徑的質(zhì)量。

圖1 KPP處理過(guò)程

關(guān)鍵點(diǎn)路徑規(guī)劃算法的處理過(guò)程如圖1所示。第一步：找出起始點(diǎn)到目標(biāo)點(diǎn)之間的關(guān)鍵點(diǎn)(見(jiàn)圖1(a))。這些關(guān)鍵點(diǎn)是通過(guò)對(duì)起始點(diǎn)和目標(biāo)點(diǎn)連線上的障礙物的某些頂點(diǎn)外推得到的。第二步：選用某種路徑規(guī)劃算法規(guī)劃出各個(gè)關(guān)鍵點(diǎn)之間的子路徑；再以關(guān)鍵點(diǎn)為連接點(diǎn)，將這些子路徑連接在一起，形成起始點(diǎn)到目標(biāo)點(diǎn)間的初始路徑。第三步：對(duì)第二步中得到的初始路徑進(jìn)行壓縮，刪除其中的冗余路徑段，得到最終路徑(見(jiàn)圖1(c))。

2.1 獲取關(guān)鍵點(diǎn)

關(guān)鍵點(diǎn)是指繞過(guò)某一障礙物的最短路徑上，與該障礙物的距離小于某一設(shè)定閾值的點(diǎn)。假設(shè)障礙物Obsi的外邊界為boundryi，從起始點(diǎn)到目標(biāo)點(diǎn)的路徑中，一條繞過(guò)Obsi的最短路徑為pathi，則稱集合{pointi|pointi∈Xfree∧pointi∈pathi∧distance(pointi,boundryi<ε}中的點(diǎn)為關(guān)鍵點(diǎn)。這些關(guān)鍵點(diǎn)是距離障礙物較近的可通行節(jié)點(diǎn)，所以經(jīng)過(guò)關(guān)鍵點(diǎn)的路徑是繞過(guò)某障礙物的近似最優(yōu)路線。關(guān)鍵點(diǎn)的獲取要經(jīng)過(guò)兩步完成，第一步是獲取所有潛在關(guān)鍵點(diǎn)，第二步是從潛在關(guān)鍵點(diǎn)中選出相對(duì)較好的點(diǎn)作為最終的關(guān)鍵點(diǎn)。

2.1.1 獲取潛在關(guān)鍵點(diǎn)

為了便于討論，該文以實(shí)際障礙物的正外接矩形來(lái)代表障礙物本身，文中的障礙物均指實(shí)際障礙物的正外接矩形。在尋找可能的關(guān)鍵點(diǎn)時(shí)，首先找到所有與起始點(diǎn)和目標(biāo)點(diǎn)的連線相交的障礙物。然后對(duì)這些障礙物的四個(gè)頂點(diǎn)分別向遠(yuǎn)離中心點(diǎn)的方向外推，如對(duì)左上點(diǎn)進(jìn)行左推和上推，對(duì)右下點(diǎn)進(jìn)行右推和下推。通過(guò)這種外推得到的點(diǎn)就是可能的關(guān)鍵點(diǎn)。左上點(diǎn)的外推過(guò)程如下：首先完成對(duì)左上點(diǎn)A的左推，即以步長(zhǎng)step向左尋找障礙物的左側(cè)邊界，若在地圖邊界內(nèi)找到了該障礙物的左側(cè)邊界則把當(dāng)前點(diǎn)拉回到障礙物內(nèi)，并以步長(zhǎng)step向上尋找障礙物的上側(cè)邊界；若在地圖邊界內(nèi)找到了該障礙物的上側(cè)邊界，則對(duì)該點(diǎn)進(jìn)行一步左推，并返回左推后的點(diǎn)，即A的外推點(diǎn)。具體過(guò)程如圖2所示。障礙物的其他頂點(diǎn)的外推過(guò)程與此類似，不再贅述。需要說(shuō)明的是，為了便于后續(xù)操作，起始點(diǎn)和目標(biāo)點(diǎn)也分別被視為可能的關(guān)鍵點(diǎn)。

圖2 左上點(diǎn)的外推過(guò)程

2.1.2 關(guān)鍵點(diǎn)選擇

在一次路徑規(guī)劃中，可能的關(guān)鍵點(diǎn)數(shù)量較多，其中有一些點(diǎn)能夠在路徑規(guī)劃過(guò)程中起到正向的啟發(fā)作用，也就是說(shuō)經(jīng)過(guò)這些關(guān)鍵點(diǎn)的路徑代價(jià)會(huì)比較小，另一些則不能。關(guān)鍵點(diǎn)選擇的目的就是從可能的關(guān)鍵點(diǎn)中，選出那些具有正向啟發(fā)作用的點(diǎn)作為最終的關(guān)鍵點(diǎn)。

在進(jìn)行關(guān)鍵點(diǎn)選擇時(shí)，從起始點(diǎn)開(kāi)始，在可能的關(guān)鍵點(diǎn)組成的集合SP中查找距離最近的無(wú)碰撞連接點(diǎn)p，若找到則將p加入到關(guān)鍵點(diǎn)集合SKP中，并將p點(diǎn)從SP中刪除，然后從p開(kāi)始在SP中繼續(xù)查找距離最近的無(wú)碰撞連接點(diǎn)。重復(fù)上述過(guò)程，直至找不到無(wú)碰撞連接點(diǎn)或找到的點(diǎn)是目標(biāo)點(diǎn)。如果目標(biāo)點(diǎn)不在SKP中，則從目標(biāo)點(diǎn)開(kāi)始，在SP中查找距離最近的無(wú)碰撞連接點(diǎn)p，若找到則將p加入到關(guān)鍵點(diǎn)集合SKP中，并將p點(diǎn)從SP中刪除，然后從p開(kāi)始在SP中查找距離最近的無(wú)碰撞連接點(diǎn)。重復(fù)上述過(guò)程，直至找不到無(wú)碰撞連接點(diǎn)或找到的點(diǎn)已經(jīng)存在于SKP中。簡(jiǎn)單來(lái)說(shuō)，關(guān)鍵點(diǎn)選擇的過(guò)程，就是分別從起始點(diǎn)和目標(biāo)點(diǎn)開(kāi)始，在SP中查找最近的可以無(wú)碰撞連接的點(diǎn)的過(guò)程。

2.2 實(shí)施路徑規(guī)劃

通過(guò)關(guān)鍵點(diǎn)的選擇，得到了一個(gè)起始點(diǎn)到目標(biāo)點(diǎn)之間的關(guān)鍵點(diǎn)數(shù)組。數(shù)組中相鄰兩個(gè)關(guān)鍵點(diǎn)之間的路徑稱為子路徑。接下來(lái)根據(jù)需要選用某種路徑規(guī)劃算法完成子路徑的規(guī)劃。最后以關(guān)鍵點(diǎn)為連接點(diǎn)，將這些子路徑連接在一起，形成初步路徑。由于難以找到適合于任何情況的最優(yōu)關(guān)鍵點(diǎn)選擇策略，所以選出的關(guān)鍵點(diǎn)并不都是對(duì)路徑規(guī)劃具有正向的啟發(fā)作用，如圖1(b)中的關(guān)鍵點(diǎn)B；同時(shí)，因?yàn)樗x擇的路徑規(guī)劃算法不一定是最優(yōu)算法，甚至可能不是漸近最優(yōu)的算法，所以初步規(guī)劃出來(lái)的路徑通常不是最短路徑。為了解決這一問(wèn)題，需要對(duì)初始路徑做進(jìn)一步優(yōu)化，也就是路徑壓縮。

路徑規(guī)劃的具體過(guò)程如下：假設(shè)經(jīng)過(guò)關(guān)鍵點(diǎn)的生成與選擇后，得到了關(guān)鍵點(diǎn)數(shù)組arrayKeypoint。對(duì)于arrayKeypoint中的每一個(gè)點(diǎn)arrayKeypoint[i]，使用選定的路徑規(guī)劃算法，規(guī)劃以arrayKeypoint[i]和arrayKeypoint[i+1]為起始點(diǎn)和目標(biāo)點(diǎn)的路徑，得到子路徑subPath_i，并將subPath_i加入到初始路徑Path中。最后對(duì)Path進(jìn)行壓縮，以提高路徑的質(zhì)量。路徑壓縮是為了減少路徑長(zhǎng)度，提高路徑質(zhì)量，其實(shí)質(zhì)就是刪除一條路徑中的冗余路徑點(diǎn)。冗余路徑點(diǎn)的定義如下：

冗余路徑點(diǎn)：對(duì)于一條路徑Path上的一段子路徑sPath，令sPath的起止點(diǎn)分別是PointStart和PointEnd，如果由PointStart和PointEnd兩個(gè)點(diǎn)構(gòu)成的子路徑也是無(wú)碰撞的可行路徑，則稱sPath上除起止點(diǎn)之外的其他路徑點(diǎn)為冗余路徑點(diǎn)。

圖3 冗余路徑點(diǎn)

如圖3所示，因?yàn)橛蒔ointStart和PointEnd兩點(diǎn)構(gòu)成的路徑是無(wú)碰撞的可行路徑，所以以PointStart和PointEnd為起止點(diǎn)的子路徑sPath上的其他點(diǎn)均為冗余路徑點(diǎn)，子路徑sPath就可以被壓縮成compressed path，由此來(lái)降低路徑長(zhǎng)度。路徑壓縮從路徑的目標(biāo)點(diǎn)開(kāi)始，逐一查找距離當(dāng)前點(diǎn)curr_point最遠(yuǎn)的無(wú)障礙連接點(diǎn)farthest_collision_free_point，并刪除curr_point與farthest_collision_free_point之間的冗余路徑點(diǎn)。路徑壓縮算法首先將目標(biāo)點(diǎn)賦值給當(dāng)前點(diǎn)curr_point，并將curr_point加入到壓縮后的路徑中；然后查找路徑中距離當(dāng)前點(diǎn)最遠(yuǎn)的無(wú)碰撞點(diǎn)farthest_collision_free_point，并將該點(diǎn)加入到壓縮后的路徑中；最后以farthest_collision_free_point為當(dāng)前點(diǎn)，繼續(xù)尋找。循環(huán)執(zhí)行上述過(guò)程，直到找到的最遠(yuǎn)無(wú)碰撞點(diǎn)為起始點(diǎn)。

2.3 算法的總流程

關(guān)鍵點(diǎn)路徑規(guī)劃算法首先獲取可能的關(guān)鍵點(diǎn)，潛在關(guān)鍵點(diǎn)的獲取是通過(guò)對(duì)起始點(diǎn)與目標(biāo)點(diǎn)連線上的障礙物的頂點(diǎn)進(jìn)行外推實(shí)現(xiàn)的。然后進(jìn)行關(guān)鍵點(diǎn)選擇，依據(jù)步長(zhǎng)最小且可連通為原則，在可能的關(guān)鍵點(diǎn)中選出路徑規(guī)劃所需的關(guān)鍵點(diǎn)。接下來(lái)進(jìn)行初步的路徑規(guī)劃，選用某種路徑規(guī)劃算法在兩個(gè)相鄰的關(guān)鍵點(diǎn)之間規(guī)劃子路徑，并將這些子路徑合并成總路徑。最后進(jìn)行路徑壓縮，采用刪除總路徑中的冗余路徑點(diǎn)的方式，對(duì)原始路徑進(jìn)行優(yōu)化，得到最終路徑。

2.4 算法分析

關(guān)鍵點(diǎn)路徑規(guī)劃算法的優(yōu)點(diǎn)是效率較高，規(guī)劃的路徑質(zhì)量也較好。與其他路徑規(guī)劃算法相比，關(guān)鍵點(diǎn)路徑規(guī)劃算法多了兩個(gè)步驟，一個(gè)是關(guān)鍵點(diǎn)的獲取，另一個(gè)是路徑壓縮。通過(guò)關(guān)鍵點(diǎn)獲取，算法找到了繞過(guò)從起始點(diǎn)到目標(biāo)點(diǎn)之間的障礙物的關(guān)鍵點(diǎn)，并使用這些關(guān)鍵點(diǎn)來(lái)引導(dǎo)路徑規(guī)劃算法完成規(guī)劃。這些關(guān)鍵點(diǎn)將一條未知路徑分割成多條子路徑。通過(guò)分割將規(guī)劃空間切分成更小的子空間，從而減小了問(wèn)題規(guī)模，提高了規(guī)劃效率。另一方面，由于大多數(shù)關(guān)鍵點(diǎn)之間是可以無(wú)障礙連通的，所以這些子路徑的規(guī)劃就變得非常簡(jiǎn)單，只要將兩個(gè)關(guān)鍵點(diǎn)連接起來(lái)就行了，路徑規(guī)劃的效率得到了較大的提高。同時(shí)，關(guān)鍵點(diǎn)的引入將隨機(jī)搜索轉(zhuǎn)換為啟發(fā)式搜索，能夠使算法朝著目標(biāo)點(diǎn)的方向進(jìn)行搜索，所以算法規(guī)劃出了更短的路徑。盡管關(guān)鍵點(diǎn)的引入極大地提高了算法的效率和路徑的質(zhì)量，然而，由于地圖環(huán)境復(fù)雜多樣，起始點(diǎn)和目標(biāo)點(diǎn)的位置又不確定，導(dǎo)致難以找到一個(gè)最優(yōu)的關(guān)鍵點(diǎn)選擇策略，障礙物的一個(gè)頂點(diǎn)在某種情況下是最優(yōu)路徑經(jīng)過(guò)的點(diǎn)，在另外一種情況下又不是了。所以照固定的關(guān)鍵點(diǎn)選擇策略選出的關(guān)鍵點(diǎn)中，會(huì)出現(xiàn)少量壞點(diǎn)，這些壞點(diǎn)降低了路徑的質(zhì)量。另外用于完成關(guān)鍵點(diǎn)間路徑規(guī)劃的算法不一定是最優(yōu)算法，規(guī)劃出來(lái)的子路徑也可能會(huì)出現(xiàn)較多的冗余路徑點(diǎn)。對(duì)此，路徑壓縮很好地解決了這兩個(gè)問(wèn)題。路徑壓縮能夠以較高的效率找出路徑中的冗余部分，并將其刪除，從而提高最終路徑的質(zhì)量。所以通過(guò)關(guān)鍵點(diǎn)和路徑壓縮兩個(gè)操作，可以提高路徑規(guī)劃的效率和路徑的質(zhì)量，特別是在狹窄通道環(huán)境和“Z”字形通道環(huán)境下。

關(guān)鍵點(diǎn)路徑規(guī)劃算法的缺點(diǎn)是需要進(jìn)行頻繁的碰撞檢測(cè)。在進(jìn)行關(guān)鍵點(diǎn)獲取和路徑壓縮時(shí)，都需要多次進(jìn)行碰撞檢測(cè)。當(dāng)?shù)貓D上障礙物數(shù)量過(guò)多時(shí)，會(huì)在一定程度上影響規(guī)劃效率。

3 實(shí) 驗(yàn)

為了驗(yàn)證關(guān)鍵點(diǎn)路徑規(guī)劃算法的性能，進(jìn)行了多組對(duì)比實(shí)驗(yàn)。實(shí)驗(yàn)中選用RRT-connect完成關(guān)鍵點(diǎn)之間的路徑規(guī)劃。

3.1 與RRT-connect算法的對(duì)比實(shí)驗(yàn)

3.1.1 實(shí)驗(yàn)過(guò)程

實(shí)驗(yàn)?zāi)M了四種場(chǎng)景(scenario)，如圖4所示。分別是離散障礙物場(chǎng)景、窄通道場(chǎng)景、Z字形場(chǎng)景和迷宮場(chǎng)景。實(shí)驗(yàn)中選用規(guī)劃效率和規(guī)劃出來(lái)的路徑質(zhì)量作為評(píng)價(jià)算法優(yōu)劣的標(biāo)準(zhǔn)。每個(gè)場(chǎng)景選擇五組典型的起始點(diǎn)和目標(biāo)點(diǎn)進(jìn)行路徑規(guī)劃，圖4中的S[i]表示第i組的起始點(diǎn)，G[i]表示第i組的目標(biāo)點(diǎn)。每組運(yùn)行50次，記錄每次運(yùn)行的時(shí)間和路徑長(zhǎng)度。通過(guò)比較平均運(yùn)行時(shí)間和平均路徑長(zhǎng)度來(lái)對(duì)比算法的規(guī)劃效率和規(guī)劃出來(lái)的路徑質(zhì)量。實(shí)驗(yàn)中RRT-connect算法的迭代次數(shù)設(shè)置為50 000。

圖4 四種場(chǎng)景

3.1.2 實(shí)驗(yàn)結(jié)果

離散障礙物場(chǎng)景實(shí)驗(yàn)的結(jié)果如圖5所示。圖5(a)是路徑規(guī)劃所用時(shí)間的對(duì)比圖，其縱坐標(biāo)是規(guī)劃路徑所用的平均時(shí)長(zhǎng)，單位為毫秒；橫坐標(biāo)是路徑起止點(diǎn)的組序，如橫坐標(biāo)的“1”就表示規(guī)劃的是以圖4(a)中的S[1]為起始點(diǎn)、G[1]為目標(biāo)點(diǎn)的路徑；圖例中的KPP代表關(guān)鍵點(diǎn)路徑規(guī)劃算法的運(yùn)行結(jié)果，RRT-C是RRT-connect算法的運(yùn)行結(jié)果。圖5(b)是規(guī)劃出來(lái)的路徑長(zhǎng)度對(duì)比圖，其縱坐標(biāo)表示算法規(guī)劃出來(lái)的平均路徑長(zhǎng)度，橫坐標(biāo)及圖例所表示的意義與圖5(a)相同。如無(wú)特殊說(shuō)明，后續(xù)實(shí)驗(yàn)結(jié)果圖中各元素的意義均與圖5相同，不再贅述。

圖5 離散障礙物場(chǎng)景實(shí)驗(yàn)結(jié)果

由圖5不難看出，在對(duì)圖4(a)中的5組路徑規(guī)劃中，KPP算法在規(guī)劃效率和路徑質(zhì)量方面都比RRT-connect算法好很多。KPP算法的平均耗時(shí)僅占RRT-connect算法的1.66%，KPP算法規(guī)劃的平均路徑長(zhǎng)度僅占RRT-connect算法的13.22%。

窄通道場(chǎng)景的實(shí)驗(yàn)結(jié)果如圖6所示。在窄通道場(chǎng)景下，KPP算法在效率方面的提高更加明顯，KPP算法的平均耗時(shí)僅占RRT-connect算法的0.16%。KPP算法規(guī)劃的平均路徑長(zhǎng)度占RRT-connect算法的21.67%。這一比例有所升高，其原因是窄通道限定了路徑的大致方向。不管哪種算法規(guī)劃的路徑，都要從窄通道中通過(guò)，而該通道的長(zhǎng)度是固定的，所以在通道內(nèi)部，這些路徑的長(zhǎng)度差就被限定在一個(gè)較小的范圍內(nèi)了。

圖6 窄通道場(chǎng)景實(shí)驗(yàn)結(jié)果

Z字形場(chǎng)景的實(shí)驗(yàn)結(jié)果如圖7所示。在Z字形場(chǎng)景實(shí)驗(yàn)中，圖4(c)中的5組起止點(diǎn)中，RRT-connect算法僅成功規(guī)劃出了兩組，分別是起止點(diǎn)相距較近的S[1]到G[1]的路徑和S[2]到G[2]的路徑。這兩組路徑規(guī)劃過(guò)程中，KPP算法的平均耗時(shí)僅占RRT-connect算法的0.09%；KPP算法規(guī)劃的平均路徑長(zhǎng)度占RRT-connect算法的23.18%。與窄通道相比，Z字形通路更多地限制了路徑的走向，所以Z字形場(chǎng)景中，KPP算法規(guī)劃的平均路徑長(zhǎng)度與RRT-connect算法規(guī)劃的平均路徑長(zhǎng)度進(jìn)一步提高。

迷宮場(chǎng)景的實(shí)驗(yàn)結(jié)果如圖8所示。KPP算法的平均耗時(shí)僅占RRT-connect算法的0.08%；KPP算法規(guī)劃的平均路徑長(zhǎng)度占RRT-connect算法的12.32%。

由實(shí)驗(yàn)結(jié)果可以看出，在四種不同的場(chǎng)景中，KPP算法的規(guī)劃效率和規(guī)劃出來(lái)的路徑長(zhǎng)度均比RRT-connect算法好很多。特別是在窄通道場(chǎng)景和Z字形場(chǎng)景中，當(dāng)以高效著稱的RRT-connect算法都難以成功規(guī)劃處路線時(shí)，KPP仍然可以在較短時(shí)間內(nèi)規(guī)劃出一條質(zhì)量較高的路徑。

圖7 Z字形場(chǎng)景實(shí)驗(yàn)結(jié)果

圖8 迷宮場(chǎng)景實(shí)驗(yàn)結(jié)果

3.2 與BIT*算法的對(duì)比實(shí)驗(yàn)

為了進(jìn)一步檢驗(yàn)KPP算法的性能，與BIT*算法做了對(duì)比實(shí)驗(yàn)，實(shí)驗(yàn)選用了3組起止點(diǎn)。每組起止點(diǎn)完成50次規(guī)劃，通過(guò)對(duì)比平均規(guī)劃用時(shí)和平均路徑長(zhǎng)度來(lái)評(píng)價(jià)算法性能。

實(shí)驗(yàn)結(jié)果如下：進(jìn)行第一組路徑規(guī)劃時(shí)，BIT*算法的平均耗時(shí)為27 343.75毫秒，平均路徑長(zhǎng)度為26.976 321；KPP算法的平均耗時(shí)為0.27毫秒，平均路徑長(zhǎng)度為26.131 984。進(jìn)行第二組路徑規(guī)劃時(shí)，BIT*算法的平均耗時(shí)為25 312.5毫秒，平均路徑長(zhǎng)度為23.484 871；KPP算法的平均耗時(shí)為0.21毫秒，平均路徑長(zhǎng)度為23.332 335。進(jìn)行第三組路徑規(guī)劃時(shí)，BIT*算法的平均耗時(shí)為29 484.375毫秒，平均路徑長(zhǎng)度為33.495 748；KPP算法的平均耗時(shí)為15.625毫秒，平均路徑長(zhǎng)度為34.640 041。三組路徑規(guī)劃中，KPP算法平均耗時(shí)明顯要小于BIT*算法；在路徑長(zhǎng)度方面，除第三組外，KPP規(guī)劃的路徑長(zhǎng)度均略小于BIT*算法規(guī)劃的路徑長(zhǎng)度。在第三組路徑規(guī)劃時(shí)，由于關(guān)鍵點(diǎn)選擇時(shí)，以最小步長(zhǎng)為原則，選取距離最近的節(jié)點(diǎn)，所選節(jié)點(diǎn)并不是最好的，從而導(dǎo)致KPP算法所規(guī)劃的路徑略長(zhǎng)。

4 結(jié)束語(yǔ)

KPP算法通過(guò)引入關(guān)鍵點(diǎn)，將較復(fù)雜的長(zhǎng)路徑規(guī)劃轉(zhuǎn)化為規(guī)模較小的子路徑規(guī)劃；同時(shí)將隨機(jī)搜索轉(zhuǎn)化為啟發(fā)式搜索，所以其規(guī)劃效率較高，特別是在窄通道、Z字形或障礙物較密集的場(chǎng)景中，當(dāng)其他算法無(wú)法成功規(guī)劃路徑時(shí)，KPP算法仍然可以較快地完成規(guī)劃。同時(shí)，路徑壓縮的操作也減少了KPP算法規(guī)劃出來(lái)的路徑長(zhǎng)度。盡管KPP算法是用于解決2D問(wèn)題路徑規(guī)劃問(wèn)題的，可以很容易地將其擴(kuò)展到立體空間中，求解3D問(wèn)題路徑規(guī)劃的問(wèn)題。

由于選出的關(guān)鍵點(diǎn)不一定是最合理的，部分關(guān)鍵點(diǎn)可能會(huì)給規(guī)劃帶來(lái)錯(cuò)誤的引導(dǎo)，進(jìn)而導(dǎo)致KPP規(guī)劃出來(lái)的路徑質(zhì)量降低，所以KPP算法不是最優(yōu)的。