曹 凱，高佳佳，高 嵩，陳超波

（西安工業(yè)大學(xué) 電子信息工程學(xué)院，西安 710021）

目前，許多移動機(jī)器人平臺變得越來越流行，它們的應(yīng)用已經(jīng)從靜態(tài)實(shí)驗(yàn)室場景轉(zhuǎn)移到人與機(jī)器人之間復(fù)雜交互的高度動態(tài)環(huán)境中，使得找到合適的路徑變得更加困難，同時可能需要處理周圍可移動的物體，因此移動機(jī)器人的路徑規(guī)劃顯得尤為重要。路徑規(guī)劃是在靜態(tài)和動態(tài)環(huán)境、模型約束以及不確定性的情況下，自動生成到達(dá)預(yù)定目標(biāo)點(diǎn)的可行和最佳路徑的過程[1]。

目前，尚未有一種“通用算法”可以解決任何條件、任何配置環(huán)境下出現(xiàn)的問題，例如：路徑的最優(yōu)性，機(jī)器人的運(yùn)動學(xué)和動力學(xué)約束，有限的或無效的環(huán)境信息，計(jì)算負(fù)荷和效率等。在此，著重分析路徑規(guī)劃問題不同的解決方法，分別研究了A*，人工勢場（APF），BRRT和PRM等4種路徑規(guī)劃算法的工作原理、存在的局限性以及相應(yīng)的改進(jìn)算法，然后利用設(shè)計(jì)的模擬器，在不同的場景下對4種方法分別進(jìn)行在線仿真試驗(yàn)。通過分析試驗(yàn)結(jié)果，針對每種方法的性能表現(xiàn)給出了適用的場景和改進(jìn)方法。

1 路徑規(guī)劃算法的分類與概述

1.1 路徑規(guī)劃算法分類

目前，路徑規(guī)劃存在很多不同的算法，可以大致分為基于圖形、基于智能仿生和基于采樣等規(guī)劃算法。

基于圖形的規(guī)劃算法是將狀態(tài)空間定義為占據(jù)網(wǎng)格，障礙物定義為不可訪問的網(wǎng)格點(diǎn)。如果網(wǎng)格分辨率足夠，搜索可用的網(wǎng)格點(diǎn)就可以保證產(chǎn)生解決方案，如可視圖法、人工勢場、A*以及D*等[2-6]，較多用于城市環(huán)境探索、多智能體協(xié)作探索。然而，此類方法依舊存在著局限性，例如：人工勢場容易陷入局部最?。籄*和D*算法在網(wǎng)格分辨率較小時，搜索效率低，計(jì)算時間長的問題尤其明顯。

基于智能仿生的規(guī)劃算法通過研究自然界的一些社會行為，并且根據(jù)它們的原理歸納總結(jié)運(yùn)動規(guī)律，最終模仿結(jié)構(gòu)、行為，達(dá)到求解問題的目的。如蟻群算法、遺傳算法、粒子群算法、神經(jīng)絡(luò)等[7-10]，目前大多用于機(jī)器人協(xié)調(diào)控制、電力系統(tǒng)優(yōu)化、網(wǎng)絡(luò)路由優(yōu)化問題。但是，此類方法本身的算法結(jié)構(gòu)比較復(fù)雜，應(yīng)用于復(fù)雜動態(tài)環(huán)境時會導(dǎo)致計(jì)算量激增，因而影響收斂速度，無法快速給出解決方案。

基于采樣規(guī)劃SBP算法具有簡單通用、高效和概率完備的特點(diǎn)，是目前最受歡迎和最具影響力的路徑規(guī)劃算法[11-12]。它可以在沒有明確的障礙物信息情況下，選擇非結(jié)構(gòu)有限數(shù)量的點(diǎn)相互建立連接并構(gòu)建出可行軌跡的路線圖，如概率路線圖（PRM），快速隨機(jī)探索樹（RRT）以及變種算法RRT*，雙向RRT（BRRT）等[13-16]。此類算法常應(yīng)用于飛機(jī)機(jī)身清潔維護(hù)，高維空間環(huán)境探索，無人車輛的三維航跡規(guī)劃以及具有非完整性運(yùn)動約束的環(huán)境。

1.2 算法概述

（1）A* 算法

A*算法由斯坦福研究所的 Peter Hart，Nils Nilsson和Bertram Raphael于1986年提出，它在Dijkstra算法的基礎(chǔ)上加入了啟發(fā)式信息，解決了盲目遍歷搜索的問題，并找到了解決從原點(diǎn)到目標(biāo)的最小成本解決方案。啟發(fā)式信息使啟發(fā)搜索過程朝著可能“最快的”方向擴(kuò)展，直至達(dá)到目標(biāo)點(diǎn)。

A*算法流程可以概括為：

1）指定機(jī)器人的允許動作。

2）定義成本函數(shù)

式中：g（n）為移動成本，即對應(yīng)于將當(dāng)前位置放入到某個鄰居位置的成本；h（n）為啟發(fā)式成本，即對應(yīng)于從實(shí)際位置移動到目標(biāo)位置的成本。

3）評估總成本f（n），并移至最低值的單元格。

4）重復(fù)1～3步驟，直到達(dá)到目標(biāo)位置。

5）達(dá)到目標(biāo)時，以最低成本計(jì)算最終路徑。

A*算法的優(yōu)點(diǎn)：如果解決方案存在，A*將根據(jù)其成本函數(shù)找到最優(yōu)解；該算法在概念上簡單且易于實(shí)現(xiàn)，且計(jì)算效率高。A*算法的缺點(diǎn)是，A*必須存儲整個地圖，內(nèi)存要求較大，依賴于網(wǎng)格分辨率的大小，故在大型場景或高維操作空間中可能會出現(xiàn)問題。

文獻(xiàn)[17]將分層策略應(yīng)用到A*算法中并對其進(jìn)行改進(jìn)；文獻(xiàn)[18]通過改進(jìn)A*算法中的數(shù)據(jù)結(jié)構(gòu)來提高效率。同時，也可將這些改進(jìn)的策略結(jié)合起來以相互彌補(bǔ)不足，如雙向搜索與方向誘導(dǎo)相結(jié)合、雙向搜索與分層搜索相結(jié)合等。

（2）人工勢場 APF

人工勢場APF在1986年由Oussama Khatib提出，被廣泛應(yīng)用于機(jī)器人避障和路徑規(guī)劃。APF將環(huán)境（地圖）建模為吸引或排斥機(jī)器人的場地，所有障礙物以與距離成反比的幅度排斥機(jī)器人，相反，它吸引了目標(biāo)位置。勢場總和可以定義為

式中：Utot為總的勢場；Uattra為引力場；Urep為斥力場；X，Y，Xg，Yg分別為機(jī)器人的當(dāng)前位置和目標(biāo)位置；Di為物體對機(jī)器人的影響距離；Rd為到最近的障礙物的距離。

雖然APF方法快速有效，但也有以下缺點(diǎn)和局限性：①容易陷入局部最??；②緊密間隔的障礙物之間不能形成路徑；③在狹窄通道的情況下容易發(fā)生振蕩。

APF的局部最小問題已被廣泛研究，并有一些避免的方法，例如增加一個虛擬障礙來逃離局部最小值，使用模擬退火或自適應(yīng)地調(diào)整障礙物勢場函數(shù)以改進(jìn)排斥場模型，使得機(jī)器人從局部最小值中逃脫。

（3）快速探索隨機(jī)樹RRT

快速探索隨機(jī)樹RRT算法由Steven Lavalle于1998年開發(fā)，它通過隨機(jī)探索自由空間，構(gòu)建一棵從初始狀態(tài)開始尋找朝向目標(biāo)狀態(tài)的可行路徑的樹。

RRT算法的探索流程如下：①在搜索空間中選取一個隨機(jī)樣本；②找到該樣本的最近鄰節(jié)點(diǎn)；③從鄰近節(jié)點(diǎn)中選擇一個朝向隨機(jī)樣本的節(jié)點(diǎn)進(jìn)行擴(kuò)展；④根據(jù)鄰近節(jié)點(diǎn)擴(kuò)展的結(jié)果創(chuàng)建一個新節(jié)點(diǎn)；⑤將新的節(jié)點(diǎn)添加到現(xiàn)有樹中并將其連接到其鄰近節(jié)點(diǎn)；⑥當(dāng)節(jié)點(diǎn)到達(dá)目標(biāo)位置時停止循環(huán)。

RRT算法在過去的20年間陸續(xù)出現(xiàn)了許許多多的 變 種 ， 例如 RRT*，RRT*-Smart，Bidirectional-RRT等。RRT*通過考慮最小長度成本以顯著降低路徑成本，表現(xiàn)出漸近最優(yōu)性；RRT*-Smart旨在提高RRT*的收斂速度，產(chǎn)生最佳或接近最佳的路徑，從而縮短執(zhí)行時間；RRT-connect和Bidirectional-RRT分別從起始位置和目標(biāo)位置創(chuàng)建兩棵樹，當(dāng)兩棵樹相遇時找到解決方案；Expand-RRT使用新的隨機(jī)采樣點(diǎn)和無效路徑中的節(jié)點(diǎn)概率性地重新規(guī)劃路徑。

RRT算法在低維、高維空間中均可進(jìn)行規(guī)劃，均勻采樣可以有效地避免陷入局部最小，并且無需考慮運(yùn)動目標(biāo)的完整性和運(yùn)動約束條件。但是隨機(jī)采樣也導(dǎo)致生成的路徑并不是最優(yōu)，從而導(dǎo)致路徑成本、時間成本可能高于其他方法。

（4）概率路線圖PRM

1996 年，Kavraki和Svestka開發(fā)了概率路線圖PRM算法，該算法在配置空間中隨機(jī)抽取樣本并相互連接，然后使用基于圖搜索的算法來確定起始點(diǎn)和目標(biāo)點(diǎn)之間路徑。

PRM算法主要分為2個階段：

1）采樣階段（構(gòu)建離線路線圖）在工作區(qū)中繪制圖形，方法是隨機(jī)選擇不在障礙物內(nèi)部的點(diǎn)，并使用直線連接所有頂點(diǎn)對，檢查所有頂點(diǎn)和邊緣是否無碰撞。

2）學(xué)習(xí)階段（在線規(guī)劃）由于圖形已經(jīng)明確，因此可以使用基于圖搜索的算法進(jìn)行路徑規(guī)劃，定義連接點(diǎn)之間的歐式距離作為本地成本，歐幾里德距離目標(biāo)作為啟發(fā)式成本，然后從起始點(diǎn)到目標(biāo)點(diǎn)搜索出一條最優(yōu)路徑。

PRM方法是概率完備的，這意味著當(dāng)時間趨于無窮時，它一定會找到解決方案。該算法在沒有障礙物明確信息的情況下，也可以構(gòu)建出可行軌跡的路線圖。但是，當(dāng)采樣點(diǎn)太少或分布不合理時，可能無法找到最優(yōu)解。計(jì)算具有復(fù)雜幾何圖形的精確解或大型場景規(guī)劃時，所需時間可能呈指數(shù)倍增。

2 試驗(yàn)仿真與結(jié)果分析

為了分析前述4種算法，使用MatLab 2016開發(fā)了一種在線模擬器工具，它允許更改某些設(shè)置，如地圖場景、算法類型和模擬類型。

該模擬器的GUI界面如圖1所示。使用模擬器對不同的場景分別進(jìn)行4種算法的測試，以便闡明并解釋算法的主要特點(diǎn)。

圖1 模擬器的GUI界面Fig.1 GUI interface of simulator

2.1 場景1

圖2 場景1的模擬規(guī)劃Fig.2 Simulation plan for scenario 1

表1 仿真結(jié)果Tab.1 Simulation results

場景1的模擬規(guī)劃如圖2所示，其仿真結(jié)果見表1。場景1中密集的小型障礙物相互交錯，模擬移動機(jī)器人在行進(jìn)過程中的傳感器動態(tài)交叉采集的情況。由圖可見，4種方法均可找到可行路徑，其中APF的計(jì)算時間最短，表明APF對于局部避障具有良好的性能。

2.2 場景2

場景2模擬了結(jié)構(gòu)對稱的凹形障礙物，該場景模擬規(guī)劃如圖3所示，仿真結(jié)果見表1。由圖可見，A*算法給出了最佳路徑，雖然執(zhí)行時間相比于BRRT多0.276 s，但是路徑成本遠(yuǎn)遠(yuǎn)小于BRRT。而APF在該結(jié)構(gòu)化地圖中，由于虛擬力的分布都是對稱的，因此找不到可行路徑。

圖3 場景2的模擬規(guī)劃Fig.3 Simulation plan for scenario 2

2.3 場景3

場景3考慮了結(jié)構(gòu)對稱的室內(nèi)走廊環(huán)境，該場景模擬規(guī)劃如圖4所示，仿真結(jié)果見表1。由圖可見，最佳路徑由A*和PRM給出，但這2種方法的計(jì)算時間相對較長：APF雖然跳出局部最優(yōu)，并找到可行路徑，但是執(zhí)行時間較長；BRRT的執(zhí)行時間最少，但路徑不平滑導(dǎo)致路徑成本較大。如果對BRRT算法的路徑進(jìn)行優(yōu)化，可大大降低路徑成本。

在考慮算法、地圖和機(jī)器人位置之間的不同場景和組合之后，可以得出以下結(jié)論：

1）A*算法的收斂速度較慢，但可以保證最佳路徑。可以通過降低地圖分辨率來加速計(jì)算時間，但是如果分辨率太小，則A*無法保證最終路徑的可行性和最佳性。

圖4 場景3的模擬規(guī)劃Fig.4 Simulation plan for scenario 3

2）APF算法通常計(jì)算時間較短，但是在充滿狹窄通道的場景下，或當(dāng)某個點(diǎn)的引力和斥力大小相等、方向相反時，APF方法往往容易陷入局部最優(yōu)或出現(xiàn)震蕩。因此，在控制環(huán)中加入一種局部避障算法，可以有效地解決APF存在的問題。

3）同為概率采樣方法，PRM生成的路徑比BRRT的路徑更平滑和更短，一方面是BRRT節(jié)點(diǎn)擴(kuò)展預(yù)定義的最大步長和盲目探索，都會影響路徑優(yōu)化。另一方面，PRM在整個空間中隨機(jī)采樣，并基于它的隨機(jī)點(diǎn)可以找到最短距離。當(dāng)然，由于這2種算法都存在隨機(jī)性，因此也會存在BRRT比PRM更優(yōu)的情形。

3 路徑規(guī)劃算法的發(fā)展趨勢

在路徑規(guī)劃的研究中，每個規(guī)劃算法都有適用的場景，但是也存在一些不足之處，因此路徑規(guī)劃的研究重點(diǎn)依然是研究新的、高效的改進(jìn)路徑規(guī)劃算法。此外混合路徑規(guī)劃也將成為未來路徑規(guī)劃研究發(fā)展的方向。具體表現(xiàn)在以下幾方面：

（1）傳統(tǒng)算法與隨機(jī)算法相結(jié)合。

a.利用A*的啟發(fā)式特性引導(dǎo)RRT進(jìn)行目標(biāo)偏向采樣，以減少執(zhí)行時間、擴(kuò)展節(jié)點(diǎn)數(shù)量以及路徑成本；b.RRT的隨機(jī)性可以幫助APF跳出局部最優(yōu)解，并快速找到成本較低的路徑。

（2）遺傳算法與神經(jīng)網(wǎng)絡(luò)相結(jié)合。

利用神經(jīng)網(wǎng)絡(luò)控制機(jī)器人的運(yùn)動規(guī)則和神經(jīng)元感知器獲取未知環(huán)境的信息，再利用遺傳算法實(shí)現(xiàn)神經(jīng)網(wǎng)絡(luò)的權(quán)值設(shè)置，實(shí)現(xiàn)未知環(huán)境下的機(jī)器人路徑規(guī)劃。

（3）蟻群算法與人工神經(jīng)網(wǎng)絡(luò)相結(jié)合。

將蟻群算法與人工神經(jīng)網(wǎng)絡(luò)相結(jié)合，可以減少空間復(fù)雜度，同時提高路徑規(guī)劃準(zhǔn)確率。

（4）多機(jī)器人合作的路徑規(guī)劃。

多機(jī)器人合作進(jìn)行路徑規(guī)劃已經(jīng)成為新的研究熱點(diǎn)之一，如何劃分未知環(huán)境，如何對機(jī)器人進(jìn)行分功，如何設(shè)置機(jī)器人的體系結(jié)構(gòu)及機(jī)器人間的通信方式等成為新的研究問題，這些問題的解決能夠更好地減少機(jī)器人間的沖突問題，以便進(jìn)行更好的路徑規(guī)劃。

4 結(jié)語

路徑規(guī)劃算法通常都有很多變種，因?yàn)樗鼈円话悴荒芡瑫r滿足所有要求，這也就意味著沒有任何一種算法可以適用于任何場景，且快速、低成本的規(guī)劃出最優(yōu)路徑。全局路徑規(guī)劃和局部路徑規(guī)劃結(jié)合使用就可以保證一個相對快速而且最優(yōu)的路徑，并且在復(fù)雜的動態(tài)場景、高維環(huán)境以及非完整和運(yùn)動約束環(huán)境中均是有效的。路徑算法混合使用不僅可以利用彼此的優(yōu)勢，還可以彌補(bǔ)自身所存在的缺點(diǎn)，以此提高路徑規(guī)劃的效率和魯棒性。期望本文對于從事相關(guān)問題研究的相關(guān)人員和未來的發(fā)展具有一定的參考價(jià)值。