基于并行-交替式雙向JPS算法的機(jī)器人路徑規(guī)劃

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

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

0 引言

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

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

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

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

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

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

1 跳點搜索算法及其不足

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

1.1 跳點搜索算法

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

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

len()x)

(1)

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

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

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

2)節(jié)點x至少有一個強迫鄰居；

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

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

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

(2)

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

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

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

1.2 跳點搜索算法的不足

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

圖3 緊貼障礙物的危險路徑

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

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

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

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

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

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

2 并行-交替式雙向跳點搜索算法

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

2.1 中心熱點區(qū)域

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

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

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

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

(3)

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

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

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

2.2 并行式雙向跳點搜索算法

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

步驟1：將起始點加入OpenList_S；

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

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

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

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

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

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

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

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

(4)

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

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

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

(5)

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

2.3 交替式雙向跳點搜索算法

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

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

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

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

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

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

步驟6：廣播A1的位置；

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

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

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

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

步驟11：廣播B1的位置；

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

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

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

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

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

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

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

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

3.1 迭代式路徑修正

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

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

(8)

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

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

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

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

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

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

P1={(xk,yk+1)}

P2={(xk+1,yk)}

(9)

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

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

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

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

3.2 路徑平滑

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

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

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

(10)

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

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

4 實驗結(jié)果分析

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

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

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

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

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

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

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

4.3 路徑規(guī)劃長度

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

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

4.4 路徑的平滑性

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

(11)

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

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

5 結(jié)束語

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