苗紅霞，郭章旺，齊本勝，鄒 楊，李成林

(1.河海大學(xué) 物聯(lián)網(wǎng)工程學(xué)院，江蘇 常州 213000;2.江蘇省輸配電裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，江蘇 常州 213000)

0 引言

路徑規(guī)劃是移動(dòng)機(jī)器人研究領(lǐng)域的關(guān)鍵內(nèi)容[1]，它是指在障礙物環(huán)境下，機(jī)器人自主地規(guī)劃出一條從起始點(diǎn)運(yùn)行至目標(biāo)點(diǎn)的無碰撞路徑。為解決機(jī)器人的路徑規(guī)劃問題，各國科研人員主要提出了以下四類路徑規(guī)劃算法[2]：1)基于智能搜索算法，如：蟻群算法、粒子群算法、快速搜索隨機(jī)數(shù)算法等；2)基于人工智能算法，如：深度學(xué)習(xí)、Q-Learning算法等；3)基于幾何模型算法，如：Dijkstra算法、Voronoi圖等；4)基于局部避障算法，如：人工勢場法、動(dòng)態(tài)窗口法等。

在上述的四類路徑規(guī)劃算法中，基于智能搜索算法的路徑規(guī)劃其最大的特點(diǎn)是具有隨機(jī)性，它首先隨機(jī)生成初始解或采樣點(diǎn)，然后通過迭代的方式來逼近最優(yōu)解，因此它的解是漸進(jìn)最優(yōu)且非唯一的，當(dāng)?shù)貓D中的最優(yōu)路徑比較狹窄時(shí)，該方法可能會(huì)規(guī)劃出較長的冗余路徑?；谌斯ぶ悄芩惴ǖ穆窂揭?guī)劃能夠讓機(jī)器人自主對環(huán)境進(jìn)行學(xué)習(xí)以實(shí)現(xiàn)自主路徑規(guī)劃，其通常只使用在具有大量數(shù)據(jù)樣本的場景中。基于避障算法的路徑規(guī)劃主要適用于移動(dòng)機(jī)器人在局部地圖中的避障場景，主要用于提高移動(dòng)機(jī)器人的安全性?；趲缀文Ｐ偷穆窂揭?guī)劃算法能夠?qū)崟r(shí)調(diào)節(jié)根據(jù)最優(yōu)策略得到的可行解，其在工業(yè)、農(nóng)業(yè)、醫(yī)療等領(lǐng)域的移動(dòng)機(jī)器人路徑規(guī)劃中均得到廣泛應(yīng)用，極大地提高了生產(chǎn)生活的效率，適用于大規(guī)模靜態(tài)場景下的移動(dòng)機(jī)器人路徑規(guī)劃問題。

基于幾何模型的路徑規(guī)劃先需根據(jù)已知的環(huán)境信息構(gòu)建地圖幾何模型，再在幾何模型中規(guī)劃合適的路徑。常見的地圖模型有3種：1)語義地圖；2)柵格地圖；3)拓?fù)涞貓D。構(gòu)建語義地圖[3]需要與同步定位與建圖技術(shù)相結(jié)合，其主要應(yīng)用于識(shí)別環(huán)境中獨(dú)立個(gè)體，以應(yīng)對復(fù)雜場景及完成更加智能的任務(wù)。柵格地圖能夠反應(yīng)物體真實(shí)的物理尺寸,在實(shí)時(shí)環(huán)境中有助于機(jī)器人的定位與導(dǎo)航[4]。拓?fù)涞貓D是對地理空間的抽象和形式化描述[5]，其采用節(jié)點(diǎn)和連線來表述環(huán)境信息，通常關(guān)鍵位置以節(jié)點(diǎn)的形式表示，連線表示節(jié)點(diǎn)之間是否允許通行。相較于只表示不同節(jié)點(diǎn)之間連通關(guān)系的拓?fù)涞貓D而言，柵格地圖能夠唯一確定地表示機(jī)器人、障礙物、可行路徑的確切位置信息。與此同時(shí)，柵格地圖還有具有易于構(gòu)建和保存的特點(diǎn)，因此各國的工程師和科研人員研究提出了諸多建立在柵格地圖基礎(chǔ)上的路徑規(guī)劃算法。

A*算法是一種可應(yīng)用在柵格地圖中的啟發(fā)式路徑規(guī)劃算法，其在搜索過程中具有一定的方向性，具有運(yùn)行效率較高的特點(diǎn)[6]。該方法廣泛應(yīng)用于單機(jī)器人在全局靜態(tài)環(huán)境中的路徑規(guī)劃。文獻(xiàn)[7]通過在估價(jià)函數(shù)中加入車身輪廓代價(jià)和障礙物距離代價(jià)對A*算法進(jìn)行改進(jìn)；文獻(xiàn)[8]參考了人工勢場法的引力思想，提升了A*算法的效率。即便如此，在障礙物柵格數(shù)量較少的空曠區(qū)域，A*算法仍需對柵格進(jìn)行逐個(gè)搜索，這導(dǎo)致了冗余的搜索柵格增多，降低了算法的效率[9]。

針對A*算法在空曠區(qū)域搜索效率十分有限的問題，跳點(diǎn)搜索算法(JPS，jump point search)在A*算法的基礎(chǔ)上進(jìn)行了優(yōu)化。文獻(xiàn)[10]的驗(yàn)結(jié)果表明，由于跳點(diǎn)搜索算法在擴(kuò)展過程中裁剪了無用的節(jié)點(diǎn)，其路徑規(guī)劃速度高于A*算法。文獻(xiàn)[11]提出了一種正六邊形柵格跳點(diǎn)搜索算法，并以此解決智能體在障礙物地圖中的路徑規(guī)劃問題；文獻(xiàn)[12]提出通過“塊”操作的方法減少跳點(diǎn)搜索過程中不必要的節(jié)點(diǎn)，進(jìn)一步加快跳點(diǎn)搜索算法的速度。然而，在障礙物位置分布隨機(jī)的情況下，障礙物個(gè)數(shù)的增加導(dǎo)致了跳點(diǎn)數(shù)量的攀升，跳點(diǎn)搜索算法的路徑規(guī)劃的時(shí)間仍大幅度增加。此外，由于跳點(diǎn)搜索算法沒有考慮機(jī)器人的體積大小，其規(guī)劃出的路徑存在緊貼障礙物、軌跡不平滑[13]問題，給機(jī)器人的運(yùn)行帶來安全風(fēng)險(xiǎn)。

在障礙物位置隨機(jī)的柵格地圖中，針對跳點(diǎn)搜索算法路徑規(guī)劃時(shí)間大幅度增加的問題，本文提出了并行-交替式雙向跳點(diǎn)搜索(PA-BJPS，parallel alternate bidirectional jump point search)算法，其首先在障礙物位置隨機(jī)的柵格地圖里確定一個(gè)中心熱點(diǎn)區(qū)域，在該中心熱點(diǎn)區(qū)域的外部采用并行運(yùn)算的方式規(guī)劃路徑，在中心熱點(diǎn)區(qū)域內(nèi)部采用交替運(yùn)算的方式規(guī)劃路徑，有效地縮短了路徑規(guī)劃的時(shí)間。針對跳點(diǎn)搜索算法規(guī)劃的路徑存在安全性和平滑性不足的問題，本文提出迭代式路徑修正方法來改良危險(xiǎn)路徑，并采用3次B-樣條曲線替代拐角來平滑路徑。仿真實(shí)驗(yàn)結(jié)果表明，本文算法有效地縮短了路徑規(guī)劃的時(shí)間，同時(shí)兼顧了路徑的安全性和平滑性，具有良好的工程應(yīng)用前景。

1 跳點(diǎn)搜索算法及其不足

跳點(diǎn)搜索算法[14]在保留A*算法框架的同時(shí)，優(yōu)化了A*算法對后續(xù)節(jié)點(diǎn)擴(kuò)展的操作。通過對擴(kuò)展節(jié)點(diǎn)的篩選和處理，跳點(diǎn)搜索算法不僅縮短了路徑規(guī)劃的時(shí)間，而且減小了節(jié)點(diǎn)信息的內(nèi)存占用量。然而，跳點(diǎn)搜索算法只將移動(dòng)機(jī)器人視為一個(gè)質(zhì)點(diǎn)，因此由跳點(diǎn)搜索算法規(guī)劃的路徑可能會(huì)由于移動(dòng)機(jī)器人緊貼障礙物而產(chǎn)生碰撞風(fēng)險(xiǎn)。此外，若采用跳點(diǎn)搜索算法進(jìn)行雙向的路徑規(guī)劃，還可能會(huì)造成路徑規(guī)劃時(shí)間增長和路徑冗余的問題。

1.1 跳點(diǎn)搜索算法

在進(jìn)行路徑規(guī)劃時(shí)，跳點(diǎn)搜索算法對擴(kuò)展過程中的無用節(jié)點(diǎn)進(jìn)行裁剪，只保留了用于擴(kuò)展的跳點(diǎn)。當(dāng)跳點(diǎn)搜索算法擴(kuò)展至目標(biāo)點(diǎn)后，即得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的無障礙路徑。

跳點(diǎn)搜索算法進(jìn)行路徑規(guī)劃時(shí)，首要篩選柵格中的強(qiáng)迫鄰居(Forced Neighbour)。當(dāng)柵格節(jié)點(diǎn)x的八鄰域中有障礙物，且節(jié)點(diǎn)x的父節(jié)點(diǎn)p經(jīng)過x到達(dá)節(jié)點(diǎn)n的代價(jià)距離，比父節(jié)點(diǎn)p經(jīng)過任何其他節(jié)點(diǎn)到達(dá)節(jié)點(diǎn)n的代價(jià)距離更小，則稱n為x的強(qiáng)迫鄰居，強(qiáng)迫鄰居的篩選規(guī)則如式(1)：

len()x)

(1)

強(qiáng)迫鄰居主要包含兩種類型，橫向強(qiáng)迫鄰居如圖1(a)所示，在圖1(a)中畫圈的節(jié)點(diǎn)為x的斜向強(qiáng)迫鄰居；斜向強(qiáng)迫鄰居如圖1(b)所示，在圖1(b)中畫圈的節(jié)點(diǎn)為x的橫向強(qiáng)迫鄰居。

在篩選出強(qiáng)迫鄰居的基礎(chǔ)上，還需對跳點(diǎn)進(jìn)行篩選。如果節(jié)點(diǎn)x滿足下列3個(gè)條件之一，則該節(jié)點(diǎn)為跳點(diǎn)。

1)節(jié)點(diǎn)x是起始點(diǎn)或目標(biāo)點(diǎn)；

2)節(jié)點(diǎn)x至少有一個(gè)強(qiáng)迫鄰居；

3)在斜向搜索情況下，節(jié)點(diǎn)x的水平或垂直方向上有滿足1)，2)的節(jié)點(diǎn)。

在進(jìn)行節(jié)點(diǎn)擴(kuò)展時(shí)，如果搜索到符合要求的跳點(diǎn)，則將該跳點(diǎn)加入OpenList列表。在OpenList列表中，每次擴(kuò)展只選取評價(jià)函數(shù)最小的節(jié)點(diǎn)作為下一個(gè)待擴(kuò)展的節(jié)點(diǎn)[15]。跳點(diǎn)搜索的評價(jià)函數(shù)公式定義如式(2)：

f(n)=g(n)+h(n)

(2)

式(2)中，f(n)為節(jié)點(diǎn)n的評價(jià)函數(shù)，它由實(shí)際代價(jià)函數(shù)g(n)和預(yù)計(jì)代價(jià)函數(shù)h(n)組成。實(shí)際代價(jià)函數(shù)g(n)反映了從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)的實(shí)際距離；預(yù)計(jì)代價(jià)函數(shù)h(n)估算了當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的剩余距離，通常預(yù)計(jì)代價(jià)函數(shù)h(n)采用歐幾里得距離[16]、曼哈頓距離[17]或切比雪夫距離[18]進(jìn)行計(jì)算。

跳點(diǎn)搜索算法的路徑規(guī)劃結(jié)果如圖2所示。圖2中，柵格s為起始點(diǎn)，柵格g為目標(biāo)點(diǎn)，黑色柵格為障礙物，灰色柵格為路徑規(guī)劃過程中擴(kuò)展的跳點(diǎn)，箭頭標(biāo)識(shí)了從起始點(diǎn)到目標(biāo)點(diǎn)依次經(jīng)過的路徑。從圖2中可知，跳點(diǎn)搜索算法對路徑規(guī)劃過程中的冗余節(jié)點(diǎn)進(jìn)行了裁剪，實(shí)現(xiàn)了對節(jié)點(diǎn)的跳躍搜索，從而加快了路徑規(guī)劃的速度。

圖2 跳點(diǎn)搜索路徑規(guī)劃

1.2 跳點(diǎn)搜索算法的不足

與A*算法相比，跳點(diǎn)搜索算法雖然加速了路徑規(guī)劃的速度，但是它仍存在一些不足。在路徑規(guī)劃時(shí)，跳點(diǎn)搜索算法只將移動(dòng)機(jī)器人視為一個(gè)質(zhì)點(diǎn)，并未考慮移動(dòng)機(jī)器人的體積大小，因此由跳點(diǎn)搜索算法規(guī)劃出來的路徑存在機(jī)器人緊貼障礙物的問題[19]。緊貼障礙物的危險(xiǎn)路徑示意圖如圖3所示，在圖3中用銀白色圓圈標(biāo)記了危險(xiǎn)路徑的位置，如果機(jī)器人按照圖3中的路徑運(yùn)行，將在銀白色圓圈位置與障礙物發(fā)生碰撞。此外，由跳點(diǎn)搜索算法規(guī)劃的路徑由柵格節(jié)點(diǎn)依次相連而成，因此路徑存在不連續(xù)，拐角很多且不夠平滑的缺點(diǎn)。在路徑的拐角處，移動(dòng)機(jī)器人必須停下，原地旋轉(zhuǎn)至設(shè)定角度，再向前運(yùn)行。頻繁的急停急起操作，不僅不符合移動(dòng)機(jī)器人的運(yùn)動(dòng)特性，還加大了移動(dòng)機(jī)器人的后期維護(hù)成本。

圖3 緊貼障礙物的危險(xiǎn)路徑

在移動(dòng)機(jī)器人路徑規(guī)劃問題中，若采用雙向搜索策略，從起始點(diǎn)和目標(biāo)點(diǎn)同時(shí)規(guī)劃路徑，將有效地提高路徑規(guī)劃的效率[20]。然而，當(dāng)使用跳點(diǎn)搜索算法進(jìn)行雙向的路徑規(guī)劃時(shí)，可能會(huì)發(fā)生以下3種可能導(dǎo)致冗余路徑增多或路徑規(guī)劃時(shí)間增長的情況。

情況1：如圖4所示，當(dāng)障礙物形狀為對稱圖形時(shí)，從起始點(diǎn)規(guī)劃的正向路徑從障礙物的上方經(jīng)過，從目標(biāo)點(diǎn)規(guī)劃的反向路徑從障礙的下方經(jīng)過，正向和反向的路徑有一定概率分別從兩側(cè)經(jīng)過障礙物兩側(cè)，規(guī)劃出了兩條不相交的路徑。若發(fā)生這種情況，路徑規(guī)劃的時(shí)間不減反增，同時(shí)還增加了移動(dòng)機(jī)器人路徑規(guī)劃的計(jì)算量。

圖4 對稱障礙物下的雙向路徑規(guī)劃

情況2：跳點(diǎn)搜索算法采用了跳躍搜索的方式，它裁剪了路徑規(guī)劃的過程中不能成為跳點(diǎn)的柵格。跳躍搜索的特性雖然能夠達(dá)到較少內(nèi)存及提高路徑規(guī)劃的速度的目的，但是在雙向路徑規(guī)劃的情況下，可能會(huì)在一側(cè)跳點(diǎn)擴(kuò)展的過程中，越過另一側(cè)路徑正在擴(kuò)展的跳點(diǎn)，從而導(dǎo)致路徑冗余的產(chǎn)生，同時(shí)也大大增加了路徑規(guī)劃的時(shí)間。

情況3：在障礙物位置隨機(jī)的柵格地圖中，從起始點(diǎn)和目標(biāo)點(diǎn)出發(fā)的跳點(diǎn)搜索算法的方向和距離均存在很大的不確定性。由于障礙物的位置是不規(guī)則的，且跳點(diǎn)搜索算法具有跳躍擴(kuò)展的特性，所以從起始點(diǎn)和目標(biāo)點(diǎn)規(guī)劃的兩條路徑，其相遇節(jié)點(diǎn)可能距離兩點(diǎn)連線的中心較遠(yuǎn)，這也將導(dǎo)致路徑規(guī)劃的時(shí)間大大增加。

為避免發(fā)生上述導(dǎo)致路徑規(guī)劃時(shí)間增長的情況，同時(shí)進(jìn)一步提升機(jī)器人路徑規(guī)劃的速度，并兼顧運(yùn)行路徑的安全性和平滑性，本文提出了基于并行-交替式雙向跳點(diǎn)搜索算法的機(jī)器人路徑規(guī)劃方法。

2 并行-交替式雙向跳點(diǎn)搜索算法

與傳統(tǒng)的跳點(diǎn)搜索算法不同，并行-交替式跳點(diǎn)搜索算法首先在起始點(diǎn)與目標(biāo)點(diǎn)間確定了一個(gè)中心熱點(diǎn)區(qū)域，從而增加雙向并行搜索的方向性；其次，采用并行式雙向跳點(diǎn)搜索算法，分別規(guī)劃從起始點(diǎn)抵達(dá)中心熱點(diǎn)區(qū)域以及目標(biāo)點(diǎn)抵達(dá)中心熱點(diǎn)區(qū)域的路徑，在該過程中無需進(jìn)行信息交換和狀態(tài)同步，因此縮短了移動(dòng)機(jī)器人路徑規(guī)劃的時(shí)間；最后，采用交替式雙向跳點(diǎn)搜索算法，相向地規(guī)劃中心熱點(diǎn)區(qū)域內(nèi)部的路徑，將中心熱點(diǎn)區(qū)域外部的路徑與內(nèi)部的路徑拼接，即得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的完整路徑。

2.1 中心熱點(diǎn)區(qū)域

在雙向的跳點(diǎn)搜索算法進(jìn)行路徑規(guī)劃時(shí)，從起始點(diǎn)和目標(biāo)點(diǎn)出發(fā)的兩條路徑，其搜索方向存在不確定性，因此可能會(huì)發(fā)生在1.2節(jié)中所述的導(dǎo)致冗余搜索節(jié)點(diǎn)增多、路徑規(guī)劃時(shí)間增加的3種情況。

為解決上述問題，本算法首先在柵格地圖中確定一個(gè)如圖5所示的中心熱點(diǎn)區(qū)域，再采用并行式雙向跳點(diǎn)搜索算法，分別規(guī)劃從起始點(diǎn)和目標(biāo)點(diǎn)抵達(dá)中心熱點(diǎn)區(qū)域的路徑。確定中心熱點(diǎn)區(qū)域之后，并行式雙向跳點(diǎn)搜索算法以中心熱點(diǎn)區(qū)域作為全局路徑規(guī)劃初始目標(biāo)。中心熱點(diǎn)區(qū)域不僅能夠增強(qiáng)雙向路徑規(guī)劃的方向性，還能夠?qū)蓚?cè)路徑的相遇位置限制在中心熱點(diǎn)區(qū)域內(nèi)部，避免了冗余路徑的產(chǎn)生。本算法所確定的中心熱點(diǎn)區(qū)域的形狀是一個(gè)圓形，其軌跡的計(jì)算公式如式(3)：

圖5 中心熱點(diǎn)區(qū)域

(x-xcenter)2+(y-ycenter)2=R2

(3)

式(3)中，xcenter和ycenter是圓心的橫坐標(biāo)和縱坐標(biāo)，它是起始點(diǎn)與目標(biāo)點(diǎn)中點(diǎn)，R是圓的半徑。經(jīng)過多次實(shí)驗(yàn)測試得出，當(dāng)中心熱點(diǎn)區(qū)域的半徑長度為起始點(diǎn)與目標(biāo)點(diǎn)歐式距離的1/6左右時(shí)，本算法能較好地兼顧路徑規(guī)劃的時(shí)效性和方向性。

在中心熱點(diǎn)區(qū)域外部，采用并行的方式規(guī)劃路徑，當(dāng)擴(kuò)展的跳點(diǎn)進(jìn)入中心熱點(diǎn)區(qū)域內(nèi)部時(shí)，停止并行規(guī)劃，并記錄下由并行式雙向跳點(diǎn)搜索算法規(guī)劃的進(jìn)入中心熱點(diǎn)區(qū)域的第一個(gè)跳點(diǎn)。在中心熱點(diǎn)區(qū)域內(nèi)部，從記錄下的跳點(diǎn)開始，采用雙向交替的方式相向地規(guī)劃路徑，從而確保兩條相向的路徑能夠連接在一起。

在障礙物位置隨機(jī)的柵格地圖中，非障礙物節(jié)點(diǎn)之間往往具有良好的連通性，因此當(dāng)起始點(diǎn)和目標(biāo)點(diǎn)并行規(guī)劃的路徑抵達(dá)中心熱點(diǎn)區(qū)域之后，在中心熱點(diǎn)區(qū)的內(nèi)部亦能相向地規(guī)劃出一條相遇的路徑，最后將中心熱點(diǎn)區(qū)域內(nèi)部和外部的路徑相連，就得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的全局路徑。

2.2 并行式雙向跳點(diǎn)搜索算法

在中心熱點(diǎn)區(qū)域外部，并行式雙向跳點(diǎn)搜索算法分別了從起始點(diǎn)和目標(biāo)點(diǎn)規(guī)劃抵達(dá)中心熱點(diǎn)區(qū)域的路徑。在進(jìn)行路徑規(guī)劃的過程中，其采用了并行計(jì)算的思想，并且在該過程中無需進(jìn)行信息交換和狀態(tài)同步，因此加快了路徑規(guī)劃的速度。以從起始點(diǎn)側(cè)出發(fā)的并行式雙向跳點(diǎn)搜索算法為例，進(jìn)行路徑規(guī)劃的步驟為：

步驟1：將起始點(diǎn)加入OpenList_S；

步驟2：尋找OpenList_S中h(n)最小的點(diǎn)，設(shè)置為current_S；

步驟3：判斷路徑1的當(dāng)前節(jié)點(diǎn)current_S是否位于中心熱點(diǎn)區(qū)域內(nèi)部，若是則結(jié)束路徑1搜索，并將current_S標(biāo)記為A0，若否則進(jìn)行步驟4；

步驟4：OpenList_S刪除當(dāng)前節(jié)點(diǎn)current_S，在CloseList_S加入當(dāng)前節(jié)點(diǎn)current_S；

步驟5：根據(jù)跳點(diǎn)擴(kuò)展規(guī)則，拓展下一個(gè)跳點(diǎn)point；

步驟6：判斷跳點(diǎn)point是否在OpenList_S中，如果否則將跳點(diǎn)point加入OpenList_S，如果是則更新g(n)的值以及父節(jié)點(diǎn)的位置，并返回步驟2。

由于并行式雙向跳點(diǎn)搜索算法在路徑規(guī)劃過程中不進(jìn)行信息交換和狀態(tài)同步，因此，為了進(jìn)一步加強(qiáng)雙向搜索的方向性，并行式跳點(diǎn)搜索算法對預(yù)計(jì)代價(jià)函數(shù)進(jìn)行了改進(jìn)。按照改進(jìn)后的預(yù)計(jì)代價(jià)函數(shù)進(jìn)行路徑規(guī)劃，能夠同時(shí)兼顧中心熱點(diǎn)區(qū)域的位置和目標(biāo)點(diǎn)的位置。

以從起始點(diǎn)側(cè)出發(fā)的并行式雙向跳點(diǎn)搜索算法為例，其預(yù)計(jì)代價(jià)函數(shù)計(jì)算公式如式(4)：

hs(n)=p·hs1(n)+(1-p)·hs2(n)

(4)

式(4)中，p是權(quán)值，是節(jié)點(diǎn)n到中心熱點(diǎn)區(qū)域圓心的歐幾里得距離，節(jié)點(diǎn)n到目標(biāo)點(diǎn)的歐幾里得距離。

從目標(biāo)點(diǎn)側(cè)出發(fā)的路徑，與從起始點(diǎn)出發(fā)的路徑規(guī)劃是并行運(yùn)算的，其路徑規(guī)劃步驟與起點(diǎn)側(cè)的步驟對稱一致，它的預(yù)計(jì)代價(jià)函數(shù)的計(jì)算公式如式(5)：

hg(n)=q·hg1(n)+(1-q)·hg2(n)

(5)

式(5)中，q是權(quán)值，是節(jié)點(diǎn)到中心熱點(diǎn)區(qū)域圓心的歐幾里得距離，是節(jié)點(diǎn)到起始點(diǎn)的歐幾里得距離。

2.3 交替式雙向跳點(diǎn)搜索算法

在規(guī)劃中心熱點(diǎn)區(qū)域內(nèi)部的路徑之前，先讀取由并行式雙向跳點(diǎn)搜索算法規(guī)劃的跳點(diǎn)標(biāo)A0和B0，再采用交替式雙向跳點(diǎn)搜索算法在A0點(diǎn)和B0點(diǎn)之間相向地路徑規(guī)劃。交替式雙向跳點(diǎn)搜索算法的步驟為：

步驟1：將A0和B0分別放進(jìn)OpenList_A和OpenList_B中；

步驟2：尋找OpenList_A中最小的點(diǎn)，將其設(shè)置為current_A；

步驟3：OpenList_A刪除current_A節(jié)點(diǎn)，在CloseList_A中加入當(dāng)前節(jié)點(diǎn)；

步驟4：根據(jù)跳點(diǎn)擴(kuò)展規(guī)則，尋找不在CloseList_A中的跳點(diǎn)A1；

步驟5：判斷A1是否在OpenList_A中，若否則加入OpenList_A，若是則更新的值以及父節(jié)點(diǎn)的位置；

步驟6：廣播A1的位置；

步驟7：尋找OpenList_B中最小的點(diǎn)，將其設(shè)置為current_B；

步驟8：OpenList_B刪除current_B節(jié)點(diǎn)，在CloseList_B中加入當(dāng)前節(jié)點(diǎn)；

步驟9：根據(jù)跳點(diǎn)擴(kuò)展規(guī)則，尋找不在CloseList_B中的跳點(diǎn)B1；

步驟10：判斷B1是否在OpenList_B中，若否則加入OpenList_B，若是則更新的值以及父節(jié)點(diǎn)的位置；

步驟11：廣播B1的位置；

步驟12：判斷是否搜索到一個(gè)公共重合節(jié)點(diǎn)，若是找到完整路徑，并結(jié)束搜索，若否則返回步驟2。

在交替式雙向跳點(diǎn)搜索算法中，每進(jìn)行一次跳點(diǎn)擴(kuò)展，都需要對新跳點(diǎn)的位置坐標(biāo)進(jìn)行廣播。當(dāng)另一側(cè)進(jìn)行跳點(diǎn)擴(kuò)展時(shí)，需以本側(cè)廣播的新跳點(diǎn)作為目標(biāo)位置。

從A0點(diǎn)一側(cè)規(guī)劃的路徑，在進(jìn)行跳點(diǎn)擴(kuò)展時(shí)，其預(yù)計(jì)代價(jià)函數(shù)的計(jì)算公式如式(6)：

從B0點(diǎn)一側(cè)規(guī)劃的路徑，在進(jìn)行跳點(diǎn)擴(kuò)展時(shí)，其預(yù)計(jì)代價(jià)函數(shù)的計(jì)算公式如式(7)：

式(6)和式(7)中，xA_cur、yA_cur為A0點(diǎn)一側(cè)當(dāng)前跳點(diǎn)位置坐標(biāo)，xB_cur、yB_cur為B0點(diǎn)一側(cè)當(dāng)前跳點(diǎn)位置坐標(biāo)。

得到中心熱點(diǎn)區(qū)域內(nèi)部的完整路徑后，將中心熱點(diǎn)區(qū)域外部的路徑與內(nèi)部的路徑拼接，即可得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的完整路徑。

3 局部路徑優(yōu)化

跳點(diǎn)搜索算法在進(jìn)行路徑規(guī)劃時(shí)，沒有考慮移動(dòng)機(jī)器人的體積，因此規(guī)劃的路徑存在移動(dòng)機(jī)器人與障礙物發(fā)生碰撞的風(fēng)險(xiǎn)。此外，跳點(diǎn)搜索算法規(guī)劃的路徑由多個(gè)跳點(diǎn)柵格依次相連而成，因此規(guī)劃出來的路徑具有不連續(xù)、拐角多、不平滑的缺陷。這不僅不符合機(jī)器人的運(yùn)動(dòng)學(xué)特征，同時(shí)也給機(jī)器人帶來安全隱患。針對上述問題，提出了迭代式路徑修正方法，并采用3次B-樣條曲線對路徑進(jìn)行平滑處理。

3.1 迭代式路徑修正

在基于跳點(diǎn)搜索算法的路徑規(guī)劃過程中，根據(jù)跳點(diǎn)擴(kuò)展的規(guī)則，路徑僅先保留了跳點(diǎn)的信息，再將跳點(diǎn)依次相連，繼而得到從起始點(diǎn)到目標(biāo)點(diǎn)路徑。因此，由跳點(diǎn)搜索算法規(guī)劃的路徑具有稀疏性，要獲得路徑的完整坐標(biāo)信息，需要對路徑跳點(diǎn)進(jìn)行雙線性插值[21]處理。

相鄰跳點(diǎn)之間的柵格數(shù)是不固定的，因此將插值點(diǎn)間的插值步長設(shè)定為單個(gè)柵格的長度。雙線性插值完成后，得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的完整路徑，該條路徑中每個(gè)柵格的坐標(biāo)信息均已悉知，雙線性插值的公式如式(8)：

(8)

式(8)中，和是插值點(diǎn)兩端跳點(diǎn)的坐標(biāo)，是插值點(diǎn)的坐標(biāo)。

在獲取全局路徑的信息之后，采用迭代式路徑修正方法對存在安全隱患的局部路徑進(jìn)行修正，迭代式路徑修正方法的步驟為：

步驟1：循環(huán)遍歷路徑route；

步驟2：判斷路徑route中第k個(gè)元素的上下左右鄰域中是否存在障礙物obstacle，如果是執(zhí)行步驟3，如果否退出當(dāng)前路徑修正；

步驟3：判斷路徑route中第k+1個(gè)元素的上下左右鄰域中是否存在步驟2中得到的障礙物obstacle，如果是執(zhí)行步驟4，如果否則退出當(dāng)前路徑修正；

步驟4：根據(jù)式(9)，計(jì)算路徑轉(zhuǎn)角周圍需要檢測柵格的位置；

P1={(xk,yk+1)}

P2={(xk+1,yk)}

(9)

式(9)中，xk和yk是路徑中第k個(gè)元素的橫坐標(biāo)和縱坐標(biāo)，xk+1和yk+1是路徑中第k+1個(gè)元素的橫坐標(biāo)和縱坐標(biāo)。

步驟5：將路徑修正到P1或P2對應(yīng)的位置，并返回步驟1。

修正前路徑如圖6(a)所示，修正后的路徑如圖6(b)所示。修正后的路徑與障礙物的最短距離維持在半個(gè)柵格長度以上，從而有效地避免了移動(dòng)機(jī)器人在運(yùn)行過程中與障礙物發(fā)生碰撞的問題。

圖6 迭代式路徑修正示意圖

3.2 路徑平滑

移動(dòng)機(jī)器人若按照經(jīng)過迭代式修正后的路徑運(yùn)行，雖可以有效地避免與障礙物發(fā)生碰撞，但是路徑中拐角數(shù)量的增多限制了移動(dòng)機(jī)器人的運(yùn)行效率。在柵格地圖中，如果按照跳點(diǎn)搜索算法規(guī)劃的路徑運(yùn)行，機(jī)器人不得不在拐角處停下，原地旋轉(zhuǎn)至設(shè)定角度，再繼續(xù)向前運(yùn)行，這不符合機(jī)器人的運(yùn)動(dòng)特性。

非均勻B-樣條具有仿射不變性等優(yōu)勢[22]，因此廣泛應(yīng)用于計(jì)算機(jī)輔助設(shè)計(jì)、制造等工程應(yīng)用中。遞推式B-樣條曲線的基函數(shù)的定義如下：

設(shè)U={u0,u1，…，un}為一不減的實(shí)數(shù)序列，以U作為樣條節(jié)點(diǎn)，第i個(gè)p次B-樣條基函數(shù)Ni,p(u)的定義如式(10)，并約定，當(dāng)式(10)出現(xiàn)0/0形式的商時(shí)，其比值為0。

(10)

本文采用3次B-樣條曲線對路徑進(jìn)行平滑處理。平滑前后的路徑對比如圖7所示，從圖7中可以看到，平滑后的路徑對拐角進(jìn)行了優(yōu)化，移動(dòng)機(jī)器人按照平滑后的路徑運(yùn)行，在拐角處可以柔滑地通過，無需停下并原地旋轉(zhuǎn)，更加符合移動(dòng)機(jī)器人的運(yùn)動(dòng)特性。

圖7 平滑前后的路徑對比示意圖

4 實(shí)驗(yàn)結(jié)果分析

為驗(yàn)證并行-交替式雙向跳點(diǎn)搜索算法的有效性，分別在柵格地圖中放置不同比例的隨機(jī)障礙物進(jìn)行仿真實(shí)驗(yàn)。仿真使用的設(shè)備配置為：Windows10操作系統(tǒng)，四核Core i5-6300處理器，主頻為2.3 GHz，內(nèi)存大小為8 GB，仿真運(yùn)行平臺(tái)為MATLAB R2016a。

4.1 全局路徑規(guī)劃結(jié)果

在大小為100*100，隨機(jī)障礙物比例為1.5%柵格地圖中，使用并行-交替式雙向跳點(diǎn)搜索算法跳點(diǎn)搜索算法進(jìn)行路徑規(guī)劃結(jié)果如圖8所示。在圖8中，中心熱點(diǎn)區(qū)域外部的是由并行式跳點(diǎn)搜索算法規(guī)劃的，A0和B0分別是起始點(diǎn)側(cè)和目標(biāo)點(diǎn)側(cè)的路徑抵達(dá)中心熱點(diǎn)區(qū)域的第一個(gè)跳點(diǎn)。A0與B0之間的路徑，是采用交替式雙向跳點(diǎn)搜索算法規(guī)劃的。對貼近障礙物的局部路徑進(jìn)行放大觀察，優(yōu)化后的路徑(實(shí)線)與優(yōu)化前的路徑(虛線)相比，優(yōu)化后的路徑更好地兼顧了安全性和平滑性，按照優(yōu)化后的路徑運(yùn)行，移動(dòng)機(jī)器人不僅避免了與障礙物發(fā)生碰撞風(fēng)險(xiǎn)，同時(shí)也較少了在拐角處急停急起的次數(shù)，確保了移動(dòng)機(jī)器人安全平穩(wěn)運(yùn)行。在中心熱點(diǎn)區(qū)域外部，并行式雙向跳點(diǎn)搜索算法無需進(jìn)行信息交換與狀態(tài)同步，在沒有冗余路徑的前提下，加快了路徑規(guī)劃的速度。在中心熱點(diǎn)區(qū)域內(nèi)部，交替式雙向跳點(diǎn)搜索算法相向地規(guī)劃到了重合的節(jié)點(diǎn)，得到了區(qū)域內(nèi)部最短路徑。將中心熱點(diǎn)區(qū)域內(nèi)外的路徑相連，得到從起始點(diǎn)到目標(biāo)點(diǎn)的全局路徑。

圖8 路徑規(guī)劃仿真結(jié)果

4.2 路徑規(guī)劃的時(shí)效性

不同隨機(jī)障礙物比例下的路徑規(guī)劃時(shí)間見表1。從表1可以看出，隨著障礙物比例逐漸升高，A*算法的路徑規(guī)劃時(shí)間基本保持不變，這是由于A*算法需要逐個(gè)計(jì)算路徑中每個(gè)節(jié)點(diǎn)的信息，而障礙物比例升高對路徑中節(jié)點(diǎn)數(shù)量的影響較小。對于跳點(diǎn)搜索算法而言，障礙物比例的升高導(dǎo)致了跳點(diǎn)數(shù)量的攀升，因此跳點(diǎn)搜索算法路徑規(guī)劃時(shí)間也隨之增長。與跳點(diǎn)搜索算法相比，并行-交替式雙向跳點(diǎn)搜索算法在中心熱點(diǎn)區(qū)域外部，采用并行的方式進(jìn)行路徑規(guī)劃，有效地縮短了路徑規(guī)劃的時(shí)間。

表1 不同障礙物比例下的路徑規(guī)劃時(shí)間 s

4.3 路徑規(guī)劃長度

不同隨機(jī)障礙物比例下的路徑規(guī)劃長度見表2。從表2可以看出，在不同障礙物比例下，基于并行-交替式雙向跳點(diǎn)搜索算法路徑規(guī)劃長度都略大于A*算法和跳點(diǎn)搜索算法。通過對全局路徑位置信息進(jìn)行逐一分析可知，路徑長度增加是由于對局部路徑進(jìn)行修正和平滑造成的。相較于移動(dòng)機(jī)器人可能存在的安全隱患而言，微量的路徑長度增加幾乎可以忽略不計(jì)。

表2 不同障礙物比例下的路徑規(guī)劃距離 m

4.4 路徑的平滑性

路徑的曲率K描述了移動(dòng)機(jī)器人運(yùn)行過程中法向加速度關(guān)于弧長參數(shù)的變化情況，曲率越大，路徑越不平滑。根據(jù)式(11)，計(jì)算從起始點(diǎn)到目標(biāo)點(diǎn)完整路徑的曲率變化情況。

(11)

使用3次B-樣條曲線進(jìn)行平滑處理前后，路徑的曲率變化情況如圖9所示。如圖9(a)所示，平滑處理前的路徑曲率數(shù)值很大，其變化過程也十分尖銳。經(jīng)過3次B-樣條曲線平滑處理后的路徑曲率如圖9(b)所示，路徑的最大曲率相較于處理前大幅度降低，變化的過程也比平滑前更加平緩，按照平滑處理后的路徑運(yùn)行，移動(dòng)機(jī)器人無需在拐角處停下，避免了急停急起的問題。

圖9 平滑處理前后曲率變化對比

5 結(jié)束語

針對在障礙物隨機(jī)的柵格地圖中，跳點(diǎn)搜索算法路徑規(guī)劃時(shí)間較長的問題，本文提出了基于并行-交替式雙向跳點(diǎn)搜索算法的機(jī)器人路徑規(guī)劃算法，并行式雙向跳點(diǎn)搜索算法采用了并行運(yùn)算的思想，無需進(jìn)行信息交互和狀態(tài)同步，加快了路徑規(guī)劃的速度。交替式雙向跳點(diǎn)搜索算法每進(jìn)行一次跳點(diǎn)擴(kuò)展，需實(shí)時(shí)更新跳點(diǎn)的位置信息，以確保找到中心熱點(diǎn)區(qū)域內(nèi)部的最短路徑。為避免機(jī)器人緊貼障礙物而產(chǎn)生的安全問題，本文提出迭代式路徑修正方法，并使用3次B-樣條曲線對路徑進(jìn)行平滑處理。仿真實(shí)驗(yàn)結(jié)果表明，本文所提算法能夠有效地縮短路徑規(guī)劃時(shí)間，同時(shí)所規(guī)劃路徑兼顧了安全性和平滑性，具有良好的學(xué)術(shù)研究和工程應(yīng)用價(jià)值。