蔣林，方東君，周和文，黃惠保

1 引言

路徑規(guī)劃算法［1］的目標是為移動機器人在障礙物地圖中規(guī)劃出一條可行的移動路徑，該路徑對移動機器人導(dǎo)航效果有著至關(guān)重要的作用. 目前主流的路徑規(guī)劃算法有Dijkstra［2］、A*［3］（A Star）、人工勢場法［4］、遺傳算法［5］、粒子群算法［6］、視覺圖法［7］等算法.

其中，A*作為目前最為經(jīng)典的路徑搜索算法廣泛應(yīng)用于各種移動機器人導(dǎo)航場景中［8］，但由A*得到的路徑存在大量冗余節(jié)點、拐點多、拐角大、折線多且離障礙物較近，這些缺陷嚴重降低了路徑質(zhì)量，且不利于移動機器人的控制，甚至?xí)l(fā)生碰撞. 故此，在漫長的移動機器人路徑規(guī)劃算法發(fā)展史中，國內(nèi)外學(xué)者提出了很多路徑規(guī)劃算法［9］，在以A*為基礎(chǔ)的8 大變種中，以JPS［10］（Jump Point Search）搜索效果最優(yōu)，用跳點搜索的方式實現(xiàn)了高效計算，但隨著地圖復(fù)雜度增加，遞歸層次加深會導(dǎo)致計算量呈指數(shù)增長. 張哲等［11］在A*的基礎(chǔ)上引入大于1 的加權(quán)因子以增加算法搜索深度和改變單步擴展搜索方式來保證最優(yōu)解，但合適的單步擴展形狀又取決于機器人前面的障礙物環(huán)境，這會導(dǎo)致每次改變位姿都要重新求解單步擴展形狀，無法適應(yīng)復(fù)雜的環(huán)境. 王洪斌等［12］提出結(jié)合A*和人工勢場算法對局部路徑進行優(yōu)化，提高路徑的平滑性，但二次A*算法平滑處理和B 樣條曲線優(yōu)化［13］類似，在實際應(yīng)用中對機器人的控制要求很高. 王中玉等［14］用不同的搜索矩陣處理不同的位置關(guān)系，提高了工作效率和平滑性，但該算法效果主要依賴搜索矩陣的效果，需要引入采樣函數(shù)［15］進行輔助，且不適用于復(fù)雜或者差異較大的環(huán)境.

針對上述所提到的問題，本文提出一種基于射線模型的全局路徑搜索算法. 該搜索算法以射線模型作為主要搜索方案，更快速、精確地朝目標點進行搜索，此外，還對每個路徑節(jié)點根據(jù)射線模型進行逆向優(yōu)化，以代價最小的方式實現(xiàn)路徑的整體優(yōu)化，實現(xiàn)了擁有更少折點、更小轉(zhuǎn)角、路程更短、更安全的移動路徑規(guī)劃.

2 基于射線模型的路徑搜索

該全局路徑規(guī)劃算法是基于已知環(huán)境障礙物信息的環(huán)境地圖運行的，在此基礎(chǔ)上，該算法以射線模型為核心，從起點開始不斷向終點發(fā)射射線，以更高效的搜索方式實現(xiàn)更優(yōu)質(zhì)的路徑搜索.

2.1 地圖的預(yù)處理

由于柵格地圖［16］擁有簡單實用且容易實現(xiàn)的特點，本文采用柵格地圖對目標環(huán)境進行建模. 柵格地圖由多個代表地圖點的小柵格組成，普通搜索算法按照此地圖進行搜索時，容易忽視小柵格的尺寸，從而導(dǎo)致實際機器人本體在行駛到某柵格點時與附近障礙物柵格點發(fā)生碰撞. 圖1 是常見柵格地圖上的路徑簡圖，藍圓表示起點，綠圓表示終點，紅色表示路徑，黑色表示障礙物，從圖1 可以看出，紅色路徑從柵格頂角處跨越了黑色障礙物，在實際環(huán)境中，由于移動機器人本體有一定尺寸，故在執(zhí)行該段路徑時必然會與障礙物發(fā)生碰撞. 此外，在狹窄的巷道區(qū)域會存在僅能通過機器人本體的環(huán)境，若籠統(tǒng)地對障礙物邊緣進行擴展會導(dǎo)致原本可通過的路徑不可通過.

圖1 常見柵格路徑示意圖

針對上訴問題，本文算法采用的是結(jié)合機器人尺寸先對地圖進行形態(tài)學(xué)處理，將部分極危險區(qū)域變成不可行區(qū)域，然后根據(jù)距離對臨障區(qū)域進行代價估值.該方法不同于普遍的對整幅地圖做腐蝕處理，將前面所述的障礙物鄰近區(qū)域運行成本增加，使得此區(qū)域是“不建議走”而非“不能走”，從而限制在僅當其余路徑成本過高的情況下才走這部分與障礙物相近但不會發(fā)生觸碰的區(qū)域，這樣既可以提高遠離障礙物的柵格的搜索優(yōu)先性，也可以確保機器人能在狹窄的巷道中搜索出可移動路徑.

2.2 射線模型

圖2表達了當障礙物存在時射線的狀態(tài)，其中藍圓表示發(fā)射點，綠點表示接收點，從圖2可以看出，當無障礙物時，綠點可以直接接收到射線；當有障礙物阻擋時，射線會被障礙物擋住，得到碰撞點的位置.

圖2 射線模型簡圖

這種方法可以在無障礙的兩點之間找出最短的路徑，只保留起點和終點. 本文提出的路徑規(guī)劃算法將該模型應(yīng)用于各子路徑搜索，最終快速搜索出簡捷、平滑的完整路徑.

模型應(yīng)用：從當前點向目標點發(fā)出射線，若無障礙則直行，若有障礙則返回碰撞點并向周圍擴散，直到到達下一個障礙物或者目標點.

圖3簡要表達了該搜索模型的應(yīng)用，其中①—⑤表示在起點與目標點之間有障礙物的環(huán)境下搜索過程，⑥表示在起點與終點之間無障礙的環(huán)境下搜索過程.在有障礙時，經(jīng)過路徑①到達碰撞點，再以碰撞點為中心向周圍未搜索過的方向擴散出②③④個路徑，僅存在路徑④可以離開障礙物，然后以路徑⑤到達目標點，最終得到路徑為①→④→⑤，在無障礙時，可以從起點直接到達目標點，最終路徑為⑥.

圖3 射線搜索示意圖

2.3 路徑搜索

射線模型為點到點之間的搜索提供了便利，但當射線遇到障礙后需要從周圍的區(qū)域找到合適的避障點，此處需要根據(jù)符合條件的柵格節(jié)點的成本大小進行辨識，從而找出更合適的路線. 本文在A*的代價函數(shù)［17］基礎(chǔ)上引入一個碰撞估值，具體成本計算方法如下：

其中，λ為地圖中無障礙柵格與最近障礙物柵格之間的曼哈頓距離，τ是設(shè)定的估值函數(shù)覆蓋范圍，C為地圖中柵格的碰撞估值，F(xiàn)為總成本，G為已有成本，H為啟發(fā)成本，x、y為過程節(jié)點的坐標，xgoal、ygoal為目標點的坐標.

柵格地圖中每個柵格的碰撞估值均在預(yù)處理中完成，當無障礙柵格離障礙柵格越近時，由式（1）計算出的估值越高，反之越小，當與障礙物的距離超過設(shè)定的覆蓋距離τ后，估值為0，由此可為障礙物附近的區(qū)域設(shè)置梯度估值.

當路徑搜索至障礙物附近的柵格時，若該柵格碰撞估值不為0，但小于255，則表明該區(qū)域可走但附近有障礙物，且值越高，離障礙物越近，此時非0估值的加入會提高該處的行走成本，僅當其他路徑的成本均高于該處成本時，才走該區(qū)域. 這種方式可以按照需求對臨近障礙物的區(qū)域進行梯度賦值，以提高行走代價的方式保留該區(qū)域的可行性和遠離障礙物的優(yōu)先性.

該成本計算方法充分考慮了遠離障礙物和可穿越狹窄區(qū)域這兩類情形，解決了常見路徑算法在避障和通過狹窄區(qū)域之間只能滿足一項的問題，使路徑規(guī)劃更靈活，能滿足機器人在復(fù)雜環(huán)境下的路徑搜索.

射線模型的應(yīng)用需要遍歷一條射線經(jīng)過的所有柵格值，并從起點開始直到障礙物或者目標點，這才滿足搜索的嚴謹性，在本文中使用的直線函數(shù)以及遍歷步長計算如下：

其中，x、y為節(jié)點坐標，Δx、Δy分別為橫縱坐標的差值，d為遍歷步長. 步長d的選擇對能否成功遍歷該射線所涉及的每個柵格值有至關(guān)重要的作用，而且應(yīng)進行歸一化后投入函數(shù)計算. 當d為縱坐標差值時，應(yīng)先計算縱坐標，然后根據(jù)新的縱坐標結(jié)合直線函數(shù)計算出新的坐標點，反之則先計算橫坐標，在該處不能混淆步長的計算，否則會導(dǎo)致跨越障礙物、誤判等問題. 該計算方式能提升路徑搜索的判斷精確性，確保路徑的每個節(jié)點是安全可行的.

圖4 簡要展示了射線搜索策略從藍色起點到綠色目標點的搜索過程，其歸納如下：

步驟1：從起點向終點發(fā)射一條射線，若存在障礙物則到達首個碰撞點，反之直接抵達目標點，如圖4 中過程①；

步驟2：以當前碰撞點為中心并將其標記為路徑節(jié)點，檢索八鄰域非障礙物且未確定為路徑節(jié)點的子節(jié)點，采用曼哈頓距離計算各子節(jié)點的成本；

步驟3：選擇鄰域中成本最小的子節(jié)點作為新的路徑節(jié)點并向終點進行延伸，若存在障礙物則重復(fù)步驟2、步驟3，如圖4過程②；

步驟4：直到當前搜索節(jié)點與終點之間沒有障礙物時，到達目標點，如圖4中過程③.

在上述過程中，每個碰撞點會被標記，用于解決在某一障礙物處反復(fù)碰撞導(dǎo)致迷路的搜索問題；在計算上，除了對必要的路徑節(jié)點進行父節(jié)點和成本等計算外，僅需要對當前點和目標點之間形成的射線所經(jīng)過的柵格判斷是否為障礙物，并啟發(fā)式地由當前點開始向目標點檢索，這可以大幅減少搜索過程中的計算損耗，從而提高路徑搜索的速度.

圖4 射線搜索過程圖

3 逆向路徑優(yōu)化

常見的路徑規(guī)劃算法所求出的路徑總會存在很多不必要的拐點，于圖4 而言，若能直接從起點到達障礙物下方的拐點再到達終點會更有利于機器人的控制，也能節(jié)省不少運動成本，在這將提出一種逆向優(yōu)化算法，旨在進一步減少移動路徑中不必要的拐點，以更安全、平滑的路徑和更少的轉(zhuǎn)向控制節(jié)點來提高機器人移動效率.

圖5展示了一條路徑的逆向優(yōu)化過程，其中①—③指藍色起點和綠色當前點之間的中間節(jié)點，紅色路徑表示優(yōu)化前（后）的路徑，歸納如下：

步驟1：以當前點為中心，向起點發(fā)射射線，圖中路徑顯示有障礙，如圖5（a）的淺藍虛線；

步驟2：以當前點為中心，向節(jié)點①發(fā)射射線，圖中所示無障礙，如圖5（a）的淺藍實線；

步驟3：刪除中間節(jié)點②、③，新的路徑為：起點→①→當前點，至此完成該段路徑的路徑優(yōu)化，如圖5（b）中紅色路徑.

圖5 逆向優(yōu)化過程圖

綜合步驟1~3可將逆向優(yōu)化總結(jié)為：以當前節(jié)點為中心，按順序依次向當前節(jié)點之前的節(jié)點逐個發(fā)射射線檢測是否可直行，直到檢測到當前節(jié)點的前一節(jié)點，在這一過程中若發(fā)現(xiàn)存在可直行的直線則刪除被檢測到的可行節(jié)點與當前點之間的中間節(jié)點，新的路徑為：起點到被檢測出的可行節(jié)點的路徑加上當前節(jié)點.

將該優(yōu)化加入射線搜索策略中，可以在求解路徑的同時將路徑優(yōu)化，保證整體最優(yōu)性. 該逆向優(yōu)化是在已求解出的路徑基礎(chǔ)上依靠射線函數(shù)和檢測途經(jīng)柵格是否為障礙物進行實現(xiàn)，若某段路徑的節(jié)點數(shù)為n，最大檢測數(shù)為m，其計算關(guān)系如式（8）所示.

m=n?(n+ 1) /2 （8）

由于該優(yōu)化與路徑搜索同時進行，單次優(yōu)化需要檢測的過程節(jié)點定然不會太多，故能避免增加額外的計算負擔. 此外，該優(yōu)化還為前文的路徑搜索添加閉環(huán)檢測，解決局部最優(yōu)問題［18］. 經(jīng)過這兩者的融合，本文算法在路徑搜索過程中不用再對地圖上明顯不是障礙物的節(jié)點進行成本計算以及對連接順序的更替判斷，取代的是僅讀取該節(jié)點的地圖值（0/1），無論是搜索效率還是路徑效果上都有很大的提升.

圖6 所示路徑為圖4 障礙環(huán)境經(jīng)完整的本文算法搜索過后的路徑效果，從該效果可以看出，本文所實現(xiàn)的全局路徑規(guī)劃算法不僅計算量小，而且得到的路徑更簡潔，沒有多余的拐點，更利于機器人安全、高效地移動.

圖6 路徑搜索結(jié)果展示

綜上所述，本文所提出的全局路徑規(guī)劃算法整體流程歸納如圖7所示. 此外，A*由于始終以八鄰域擴展方式從一個柵格節(jié)點中心向鄰近柵格點的中心搜索，這類“井”字格局必然會限制A*得到的路徑拐角一定是45°的倍數(shù)，這不僅局限了搜索的方向還降低了整體路徑的靈活性，而本文算法則打破了這種45°轉(zhuǎn)角限制，以點到點的方式實現(xiàn)任意轉(zhuǎn)角，解決了在柵格地圖中以柵格為基求解的算法在角度上對路徑的局限問題，更全面地提升路徑搜索的效率和路徑質(zhì)量. 下文將進行多組仿真實驗和實際機器人運行檢測該算法的搜索效果.

圖7 路徑搜索總流程圖

4 實驗與結(jié)果

為在多環(huán)境下驗證本文算法的路徑規(guī)劃效果，該實驗采用常規(guī)房間地圖以及AIID 2010 STARCRAFT［19］競賽中使用的三個STAR CRAFT 地圖，如圖8 所示，結(jié)合opencv3 與Vistudio2015 對地圖進行預(yù)處理，并模擬出機器人在對應(yīng)環(huán)境地圖中用本文搜索算法得到的移動路徑，最后提煉出時間、路程、拐點等多個參數(shù)進行數(shù)據(jù)分析比對.

4.1 本文路徑規(guī)劃效果

圖8 原始實驗地圖

在進行預(yù)處理后，對同一地圖劃分出簡單、一般以及復(fù)雜3 個等級的路徑情況，分別在4 張地圖中驗證該搜索算法的路徑效果、適應(yīng)性以及穩(wěn)定性. 其中，簡單路況是指起點和終點間障礙物較少且路徑較短的環(huán)境；一般路況指兩點之間路徑較長、障礙物較少的環(huán)境；復(fù)雜路況則是指路徑長、障礙物多的環(huán)境. 本文算法在上述3 種環(huán)境下的路徑搜索效果如表1所示.

表1 三種復(fù)雜程度下各地圖中的路徑效果

由表1可以看出，無論是路徑復(fù)雜程度的變化還是環(huán)境的變化，本文算法都能有效地搜索出可行的移動路徑，而且從紅色路徑效果上看，該路徑全程僅保留了必要的拐點，其余均以直線的方式行駛，由起點十分簡潔、高效地到達目標點.

故此，本文算法可以適應(yīng)多種環(huán)境，適應(yīng)性和穩(wěn)定性強，且得出的路徑簡潔、安全，全程以最簡便的方式直行，僅保留必要的轉(zhuǎn)向節(jié)點.

為進一步分析本文算法的搜索效果，對其搜索出的路徑進行參數(shù)提煉，如表2 所示. 其中，時間指路徑搜索時間；路程指得出的路徑距離；節(jié)點指得出的路徑中需要停頓調(diào)整的控制點數(shù)；拐點指路徑中需要拐彎的控制點數(shù)；角度是指機器人在走該路徑過程中需要的最大轉(zhuǎn)彎角度.

結(jié)合表1 和表2 可以看出，本文的全局路徑規(guī)劃算法搜索速度快，可以適應(yīng)多種不同復(fù)雜度的地圖和搜索環(huán)境，所得到的路徑短且沒有多余的路徑控制節(jié)點，轉(zhuǎn)彎節(jié)點少且全程保持著安全距離，每個節(jié)點的轉(zhuǎn)向角均為銳角，便于移動機器人轉(zhuǎn)向控制.

表2 三種復(fù)雜程度下各地圖中的路徑參數(shù)

4.2 與A*路徑搜索算法比對

本文算法與A*在相同條件下搜索出的路徑效果對比如表3 所示. 其中紅色代表本文算法，藍色代表A*算法，在進行多次試驗后，取在相同條件下運行多次的參數(shù)平均值，并在表4 中對兩種路徑的參數(shù)進行對比.

表3 本文算法與A*路徑效果對比

表4 本文算法與A*路徑參數(shù)對比

結(jié)合表3和表4可以看出，A*得到的路徑在任意環(huán)境下均有貼行障礙物的路段，實際機器人執(zhí)行該段路徑時極易發(fā)生碰撞，而且整體路徑蜿蜒曲折，十分耗費機器人控制資源. 而本文算法得到的路徑相比之下要更安全，除了在必要的拐點外，大部分區(qū)域是與障礙物相隔了一定距離，而且路徑更平滑，減小了機器人的控制負擔. 此外，在相同條件下本文算法的搜索速度比A*快約50%，路程減少約數(shù)十個像素點，全程需調(diào)整位姿的控制節(jié)點數(shù)極少，且本文路徑突破了45°倍數(shù)角的約束，轉(zhuǎn)向角比A*小.

由此可得，本文算法得到的路徑比A*更為簡潔、安全且易于控制. 而且在路程、控制節(jié)點數(shù)以及路徑平滑性等方面比A*擁有明顯的優(yōu)勢.

5 掃地機器人實驗

5.1 機器人實驗場地介紹及試驗

在本實驗中使用一臺直徑0.32 m、高0.09 m且配有攝像頭和雷達等傳感器的掃地機器人作為測試設(shè)備，測試環(huán)境為長50 m、寬40 m 的室內(nèi)環(huán)境，設(shè)置有各類障礙物和巷道，足以考驗路徑規(guī)劃算法的各方面性能，環(huán)境如圖9（a）所示. 測試步驟如下：（1）將本算法程序燒錄至掃地機器人，使其按照本算法進行路徑規(guī)劃；（2）實現(xiàn)測試環(huán)境的地圖構(gòu)建，圖9（b）所示為該實驗環(huán)境經(jīng)過地圖建模后得到的柵格環(huán)境地圖，柵格大小為0.05 m×0.05 m；（3）在地圖中指定起點和終點，讓移動機器人搜索路徑并執(zhí)行；（4）整理實驗數(shù)據(jù)進行分析.

圖9 實驗環(huán)境

基于上述環(huán)境，使機器人從當前點移動到目標點，并記錄規(guī)劃出的路徑. 圖10展示了6種路徑，其中紅點表示機器人當前所在位置，黃標表示目標點，黃色的虛線表示機器人規(guī)劃出來的整體路徑.

從圖10 中可以看出，本文算法搜索出的路徑始終與障礙物保持著安全的距離，拐點少，整體路徑簡潔.為了證明本文算法的靈活性，在圖10（f）中添加了兩處虛擬障礙墻進行測驗，結(jié)果顯示本文算法擁有較強的適應(yīng)性，可以在多種復(fù)雜程度的環(huán)境中成功搜索適合機器人執(zhí)行的移動路徑.

圖10 機器人實際運行展示

5.2 真實環(huán)境中與A*運行對比

以上述運行環(huán)境為基礎(chǔ)，此處將提取局部路徑搜索細節(jié)，更細致地展現(xiàn)本文算法與A*在實際運行中的效果差異. 如圖11 所示，橙色點為起點，綠色點為終點，青色點為在路徑中需要停頓、調(diào)整位姿或在搜索過程中占用計算較大的節(jié)點.

圖11 本文算法與A*在搜索過程中的細節(jié)對比

由圖11 可見，A*得到的路徑中占用計算較大的點和需要機器人改變位姿的點明顯比本文算法得到的路徑結(jié)果多. 本文僅使用了兩個過渡節(jié)點和三段直線即完成了從起點到終點的規(guī)劃任務(wù)，而A*在圖中的青色節(jié)點近乎遍布了整體路徑的90%，故本文算法得到的路徑相對更加簡潔且執(zhí)行時更為簡便.

A*雖然考慮了路徑最短以及方向兩個問題［20］，但是其對每個節(jié)點進行八鄰域搜索必然會進行許多不必要的計算，浪費計算資源. 此外，在障礙物邊緣進行八鄰域搜索必然導(dǎo)致得到的路徑節(jié)點與障礙物較近，而本文算法主要以射線模型為基礎(chǔ)進行搜索，沒有局限于八領(lǐng)域搜索和相鄰節(jié)點之間的運動，以檢索射線所經(jīng)過的柵格標記值代替直接對各柵格的成本等計算，在路徑安全性、簡潔性以及搜索效率等多方面相比于A*有明顯的優(yōu)勢.

5.3 導(dǎo)航過程效果觀察

圖12 和圖13 展示了圖10（f）路徑的機器人實際運行情況，從中可以看出，機器人在移動過程中沒有觸碰障礙物，即便在拐角處也保留了一定的安全距離，證明經(jīng)本文算法求解出的移動路徑實際可行且安全可靠.

圖12 路線圖

圖13 實際機器人導(dǎo)航圖

6 結(jié)論

本文針對傳統(tǒng)路徑規(guī)劃算法得到的路徑平滑性低且搜索速度慢等問題，通過射線搜索提高了搜索速度，結(jié)合碰撞估值使機器人既可優(yōu)先遠離障礙物行走也可通過狹窄的巷道區(qū)域，并用逆向優(yōu)化策略對路徑進一步優(yōu)化，解決局部最優(yōu)問題，增強路徑的平滑性，提高了路徑的整體質(zhì)量和算法的搜索效率.