1.自主機(jī)器人近距離操作運(yùn)動規(guī)劃體系 在研究自主運(yùn)動規(guī)劃問題之前，首先需建立相對較為完整的自主運(yùn)動規(guī)劃體系，再由該體系作為指導(dǎo)，對自主運(yùn)動規(guī)劃的各項具體問題進(jìn)行深入研究。本節(jié)將根據(jù)自主機(jī)器人的思維方式、運(yùn)動形式、任務(wù)行為等特點(diǎn)，建立與之相適應(yīng)的自主運(yùn)動規(guī)劃體系。并按照機(jī)器人的數(shù)量與規(guī)模，將自主運(yùn)動規(guī)劃分為單個機(jī)器人的運(yùn)動規(guī)劃與多機(jī)器人協(xié)同運(yùn)動規(guī)劃兩類規(guī)劃體系。

1.1 單個自主機(jī)器人的規(guī)劃體系

運(yùn)動規(guī)劃系統(tǒng)是自主控制系統(tǒng)中主控單元的核心部分，因此有必要先研究自主控制系統(tǒng)和其主控單元的體系結(jié)構(gòu)問題。 自主控制技術(shù)研究至今，先后出現(xiàn)了多種體系結(jié)構(gòu)形式，目前被廣泛應(yīng)用于實踐的是分布式體系結(jié)構(gòu)，其各個功能模塊作為相對獨(dú)立的單元參與整個體系。隨著人工智能技術(shù)的不斷發(fā)展，基于多Agent的分布式體系結(jié)構(gòu)逐漸成為了主流，各功能模塊作為獨(dú)立的智能體參與整個自主控制過程，該體系結(jié)構(gòu)應(yīng)用的基本形式如圖1所示。一方面，主控單元與測控介入處理、姿態(tài)控制系統(tǒng)、軌道控制系統(tǒng)、熱控系統(tǒng)、能源系統(tǒng)、數(shù)傳、有效載荷控制等功能子系統(tǒng)相互獨(dú)立為智能體，由總線相連；另一方面，主控單元為整個系統(tǒng)提供整體規(guī)劃，以及協(xié)調(diào)、管理各子系統(tǒng)Agent的行為。測控介入處理Agent保證地面系統(tǒng)對整個系統(tǒng)任意層面的控制介入能力，可接受上行的使命級任務(wù)、具體的飛行規(guī)劃和底層的控制指令；各子系統(tǒng)Agent存儲本分系統(tǒng)的各種知識和控制算法，自主完成主控單元發(fā)送的任務(wù)規(guī)劃，并將執(zhí)行和本身的健康等信息傳回主控單元，作為主控單元Agent運(yùn)行管理和調(diào)整計劃的依據(jù)。

圖1 基于多Agent的分布式自主控制系統(tǒng)體系結(jié)構(gòu)基本形式示意圖 主控單元Agent采用主流的分層遞階式結(jié)構(gòu)，這種結(jié)構(gòu)層次鮮明，并且十分利于實現(xiàn)，其基本結(jié)構(gòu)如圖2所示。主控單元由任務(wù)生成與調(diào)度、運(yùn)動行為規(guī)劃和控制指令生成三層基本結(jié)構(gòu)組成，由任務(wù)生成與調(diào)度層獲得基本的飛行任務(wù)，經(jīng)過運(yùn)動行為規(guī)劃層獲得具體的行為規(guī)劃，再由控制指令生成層得到最終的模塊控制指令，發(fā)送給其它功能Agent。各功能Agent發(fā)送狀態(tài)信息給主控單元的狀態(tài)檢測系統(tǒng)，狀態(tài)檢測系統(tǒng)將任務(wù)執(zhí)行情況和子系統(tǒng)狀態(tài)反饋回任務(wù)生成與調(diào)度層，以便根據(jù)具體情況對任務(wù)進(jìn)行規(guī)劃調(diào)整。當(dāng)遇到突發(fā)情況時，還可啟用重規(guī)劃模塊，它可根據(jù)當(dāng)時情況迅速做出反應(yīng)快速生成行為規(guī)劃，用以指導(dǎo)控制指令生成層得到緊急情況的控制指令。 此外，地面控制系統(tǒng)在三個層次上都分別具有介入能力。圖2中，點(diǎn)劃線內(nèi)是主控單元全部模塊，虛線內(nèi)為運(yùn)動規(guī)劃系統(tǒng)，包括運(yùn)動行為規(guī)劃模塊和重規(guī)劃模塊，這也是運(yùn)動規(guī)劃系統(tǒng)的主要功能。

圖2 主控單元基本結(jié)構(gòu)示意圖 明確了自主控制系統(tǒng)與其主控單元的基本結(jié)構(gòu)，以及運(yùn)動規(guī)劃系統(tǒng)在主控單元中的基本功能，便可建立運(yùn)動規(guī)劃系統(tǒng)的體系結(jié)構(gòu)。運(yùn)動規(guī)劃系統(tǒng)的體系結(jié)構(gòu)如圖3所示，該系統(tǒng)由規(guī)劃器和重規(guī)劃器兩大執(zhí)行單元組成，分別承擔(dān)對飛行任務(wù)的一般規(guī)劃和對突發(fā)事件緊急處理的運(yùn)動規(guī)劃。當(dāng)然，這兩部分也可理解為離線規(guī)劃與在線規(guī)劃兩種，離線規(guī)劃一般解決平時按部就班的飛行任務(wù)，在線規(guī)劃一般解決突然下達(dá)的飛行任務(wù)。除規(guī)劃器以外，系統(tǒng)還配有知識域模塊，用以利用特定語言描述相關(guān)知識。知識域包括行為域和模型域兩個部分，行為域用來存儲服務(wù)系統(tǒng)一般的運(yùn)動行為描述和緊急情況下的一些運(yùn)動行為方面的處理方法（如急停、轉(zhuǎn)向等），模型域用來存儲規(guī)劃所需模型知識，包括環(huán)境模型、組裝體模型、組裝任務(wù)對象模型和任務(wù)模型等等。

圖3 運(yùn)動規(guī)劃系統(tǒng)體系結(jié)構(gòu)示意圖

1.2 多自主機(jī)器人協(xié)同規(guī)劃體系

多智能體系統(tǒng)的群體體系結(jié)構(gòu)一般分為集中式、分散式兩種基本結(jié)構(gòu)，分散式結(jié)構(gòu)又可以進(jìn)一步分為分層式和分布式結(jié)構(gòu)。集中式結(jié)構(gòu)通常由一個主控單元掌握全部環(huán)境和受控機(jī)器人信息，運(yùn)用規(guī)劃算法對任務(wù)進(jìn)行分解，并分配給各受控機(jī)器人，組織它們完成任務(wù)。其優(yōu)點(diǎn)是理論條理清晰，實現(xiàn)較為直觀；缺點(diǎn)是容錯性、靈活性和對環(huán)境的適應(yīng)性較差，與各受控機(jī)器人存在通訊瓶頸問題。相對于集中式結(jié)構(gòu)，分散式結(jié)構(gòu)無法得到全局最優(yōu)解，但它憑借著可靠性、靈活性和較強(qiáng)的環(huán)境適應(yīng)性越來越受到廣泛的青睞。分散式結(jié)構(gòu)中的分布式結(jié)構(gòu)沒有主控單元，各智能體地位平等，通過各智能體間的通訊和信息交流達(dá)到協(xié)商的目的，實現(xiàn)最終的決策，但該結(jié)構(gòu)容易片面強(qiáng)調(diào)個體，導(dǎo)致占用資源過多，且難于得到磋商結(jié)果。分層式結(jié)構(gòu)介乎于集中式和分布式之間，存在主控單元，但并不是由主控單元掌控一切，各智能體也具備一定的自主性，上下級之間按照一定的規(guī)則，通過信息流形成完整的整體，共同完成協(xié)同任務(wù)。 多自主機(jī)器人系統(tǒng)應(yīng)采用分層式結(jié)構(gòu)，以保證整個系統(tǒng)既適于統(tǒng)一領(lǐng)導(dǎo)，又滿足系統(tǒng)靈活、快速的需求。多自主機(jī)器人協(xié)同規(guī)劃體系結(jié)構(gòu)如圖4所示，按照分層式結(jié)構(gòu)建立兩種工作模式：事先的離線規(guī)劃由主控單元負(fù)責(zé)，首先獲得協(xié)同任務(wù)，經(jīng)過規(guī)劃器得到具體的行為運(yùn)動規(guī)劃，并分發(fā)給各分系統(tǒng)執(zhí)行單元，相關(guān)的知識域中主要是用于描述各分系統(tǒng)協(xié)商規(guī)則的協(xié)商域，主控單元從外界獲取環(huán)境信息，從各分系統(tǒng)獲取狀態(tài)信息；當(dāng)遇到突發(fā)事件或緊急任務(wù)變更以及主控單元停止工作時，各分系統(tǒng)采用分布式結(jié)構(gòu)，單獨(dú)規(guī)劃各自運(yùn)動行為，并從各自的知識域中獲取協(xié)商方式，外界環(huán)境信息由主控單元發(fā)送和自我感知相結(jié)合獲得（主控單元停止工作時，僅靠自我感知獲取信息），其它機(jī)器人信息的傳輸由機(jī)器人間的數(shù)據(jù)鏈實現(xiàn)。

圖4 多自主機(jī)器人協(xié)同規(guī)劃體系結(jié)構(gòu)示意圖

2.路徑規(guī)劃研究 當(dāng)給定了某一特定的任務(wù)之后，如何規(guī)劃機(jī)器人的運(yùn)動方式將至關(guān)重要。機(jī)器人的規(guī)劃包括兩部分內(nèi)容：基座移動到適合操作的位置和轉(zhuǎn)動手臂關(guān)節(jié)完成操作。包括三個問題：基座點(diǎn)到點(diǎn)運(yùn)動規(guī)劃；關(guān)節(jié)空間規(guī)劃；綜合規(guī)劃。 本章研究幾種常用的運(yùn)動規(guī)劃算法：圖搜索法、RRT算法、人工勢場法、BUG算法。并對部分算法的自身缺陷進(jìn)行了一些改進(jìn)。

2.1 圖搜索法

圖搜索法依靠已知的環(huán)境地圖以及地圖中的障礙物信息構(gòu)造從起點(diǎn)到終點(diǎn)的可行路徑。主要分成深度優(yōu)先和廣度優(yōu)先兩個方向。深度優(yōu)先算法優(yōu)先擴(kuò)展搜索深度大的節(jié)點(diǎn)，可以快速的得到一條可行路徑，但是深度優(yōu)先算法得到的第一條路徑往往是較長的路徑。廣度優(yōu)先算法優(yōu)先擴(kuò)展深度小的節(jié)點(diǎn)，呈波狀的搜索方式。廣度優(yōu)先算法搜索到的第一條路徑就是最短路徑。

2.1.1 可視圖法

可視圖法由Lozano-Perez和Wesley于1979年提出，是機(jī)器人全局運(yùn)動規(guī)劃的經(jīng)典算法。可視圖法中，機(jī)器人用點(diǎn)來描述，障礙物用多邊形描述。將起始點(diǎn)?、目標(biāo)點(diǎn)??和多邊形障礙物的各頂點(diǎn)(設(shè)??是所有障礙物的頂點(diǎn)構(gòu)成的集合)進(jìn)行組合連接，要求起始點(diǎn)和障礙物各頂點(diǎn)之間、目標(biāo)點(diǎn)和障礙物各頂點(diǎn)之間以及各障礙物頂點(diǎn)與頂點(diǎn)之間的連線均不能穿越障礙物，即直線是“可視的”。給圖中的邊賦權(quán)值，構(gòu)造可見圖??。其中點(diǎn)集??，??為所有弧段即可見邊的集合。然后釆用某種優(yōu)化算法搜索從起始點(diǎn)??到目標(biāo)點(diǎn)??的最優(yōu)路徑，那么根據(jù)累加和比較這些直線的距離就可以獲得從起始點(diǎn)到目標(biāo)點(diǎn)的最短路徑。 ?

圖5 可視圖 由此可見，利用可視圖法規(guī)劃避障路徑主要在于構(gòu)建可視圖，而構(gòu)建可視圖的關(guān)鍵在于障礙物各頂點(diǎn)之間可見性的判斷。判斷時主要分為兩種情況，同一障礙物各頂點(diǎn)之間可見性的判斷以及不同障礙物之間頂點(diǎn)可見性的判斷。

同一障礙物中，相鄰頂點(diǎn)可見(通常不考慮凹多邊形障礙物中不相鄰頂點(diǎn)也有可能可見的情況)，不相鄰頂點(diǎn)不可見，權(quán)值賦為?。

不同障礙物之間頂點(diǎn)可見性的判斷則轉(zhuǎn)化為判斷頂點(diǎn)連線是否會與其它頂點(diǎn)連線相交的幾何問題。如下圖虛線所示，、?分別是障礙物?、?的頂點(diǎn)，但??與??連線與障礙物其它頂點(diǎn)連線相交，故?、?之間不可見；而實線所示的??與??連線不與障礙物其它頂點(diǎn)連線相交，故??、??之間可見。

圖6 頂點(diǎn)可見性判斷 可視圖法能求得最短路徑，但搜索時間長，并且缺乏靈活性，即一旦機(jī)器人的起始點(diǎn)和目標(biāo)點(diǎn)發(fā)生改變，就要重新構(gòu)造可視圖，比較麻煩。可視圖法適用于多邊形障礙物，對于圓形障礙物失效。切線圖法和Voronoi圖法對可視圖法進(jìn)行了改進(jìn)。切線圖法用障礙物的切線表示弧，因此是從起始點(diǎn)到目標(biāo)點(diǎn)的最短路徑的圖，移動機(jī)器人必須幾乎接近障礙物行走。其缺點(diǎn)是如果控制過程中產(chǎn)生位置誤差，機(jī)器人碰撞障礙物的可能性會很高。Voronoi圖法用盡可能遠(yuǎn)離障礙物和墻壁的路徑表示弧。因此，從起始點(diǎn)到目標(biāo)點(diǎn)的路徑將會增長，但采用這種控制方式時，即使產(chǎn)生位置誤差，移動機(jī)器人也不會碰到障礙物。

2.1.2 Dijkstra算法

Dijkstra算法由荷蘭計算機(jī)科學(xué)家艾茲赫爾·戴克斯特拉（Edsger Wybe Dijkstra）發(fā)明，通過計算初始點(diǎn)到自由空間內(nèi)任何一點(diǎn)的最短距離可以得到全局最優(yōu)路徑。算法從初始點(diǎn)開始計算周圍4個或者8個點(diǎn)與初始點(diǎn)的距離，再將新計算距離的點(diǎn)作為計算點(diǎn)計算其周圍點(diǎn)與初始點(diǎn)的距離，這樣計算像波陣面一樣在自由空間內(nèi)傳播，直到到達(dá)目標(biāo)點(diǎn)。這樣就可以計算得到機(jī)器人的最短路徑。 Dijkstra算法是一種經(jīng)典的廣度優(yōu)先的狀態(tài)空間搜索算法，即算法會從初始點(diǎn)開始一層一層地搜索整個自由空間直到到達(dá)目標(biāo)點(diǎn)。這樣會大大增加計算時間和數(shù)據(jù)量。而且搜索得到的大量對于機(jī)器人運(yùn)動是無用的。

2.1.3 A*算法

為了解決Dijkstra算法效率低的問題，A*算法作為一種啟發(fā)式算法被提出。該算法在廣度優(yōu)先的基礎(chǔ)上加入了一個估價函數(shù)。

2.2 RRT算法

快速搜索隨機(jī)樹（RRT）算法是一種增量式采樣的搜索方法，該方法在應(yīng)用中不需要任何參數(shù)整定，具備良好的使用性能。它利用增量式方法構(gòu)建搜索樹，以逐漸提高分辨能力，而無須設(shè)置任何分辨率參數(shù)。在極限情況，該搜索樹將稠密的布滿整個空間，此時搜索樹由很多較短曲線或路經(jīng)構(gòu)成，以實現(xiàn)充滿整個空間的目的。增量式方法構(gòu)建的搜索樹其導(dǎo)向取決于稠密采樣序列，當(dāng)該序列為隨機(jī)序列時，該搜索樹稱為快速搜索隨機(jī)樹（Rapidly Exploring Random Tree，RRT），而不論該序列為隨機(jī)還是確定性序列，都被稱為快速搜索稠密樹（Rapidly Exploring Dense Trees，RDTs），這種規(guī)劃方法可處理微分等多種約束。

2.2.1 算法步驟

考慮二維和三維工作空間，環(huán)境中包含靜態(tài)障礙物。初始化快速隨機(jī)搜索樹T，只包括根節(jié)點(diǎn)，即初始狀態(tài)S。在自由空間中隨機(jī)選取一個狀態(tài)點(diǎn)?，遍歷當(dāng)前的快速隨機(jī)搜索樹T，找到T上距離??最近的節(jié)點(diǎn)??，考慮機(jī)器人的動力學(xué)約束從控制輸入集??中選擇輸入??，從狀態(tài)??開始作用，經(jīng)過一個控制周期??到達(dá)新的狀態(tài)??。滿足??與??的控制輸入??為最佳控制量。將新狀態(tài)??添加到快速隨機(jī)搜索樹T中。按照這樣得到方法不斷產(chǎn)生新狀態(tài)，直到到達(dá)目標(biāo)狀態(tài)G。完成搜索樹構(gòu)建后，從目標(biāo)點(diǎn)開始，逐次找到父節(jié)點(diǎn)直到到達(dá)初始狀態(tài)，即搜索樹的根節(jié)點(diǎn)。 ?

圖7 隨機(jī)樹構(gòu)建過程 由于在搜索過程中考慮了機(jī)器人的動力學(xué)約束，因此生成的路徑的可行性很好。但是算法的隨機(jī)性導(dǎo)致其只具備概率完備性。

2.2.2 改進(jìn)算法

LaValle等人的工作奠定了RRT方法的基礎(chǔ)。在采樣策略方面，RRTGoalBiaS方法在控制機(jī)器人隨機(jī)運(yùn)動的同時，以一定概率向最終目標(biāo)運(yùn)動；RRTGoalZoom方法分別在整個空間和目標(biāo)點(diǎn)周圍的空間進(jìn)行采樣；RRTCon方法則通過加大隨機(jī)步長改進(jìn)規(guī)劃速度。雙向規(guī)劃思想也被采用，衍生出RRTExtExt，RRTExtCon，RRTConCon等多種算法。 基本RRT算法收斂到終點(diǎn)位姿的速度可能比較慢。為了提高算法的效率和性能，需不斷對該算法進(jìn)行改進(jìn)。如為了提高搜索效率采用雙向隨機(jī)搜索樹(Bi~RRT)，從起始點(diǎn)和目標(biāo)點(diǎn)并行生成兩棵RRT，直至兩棵樹相遇，算法收斂。由于這個算法相比于原始RRT有更好的收斂性，因此在目前路徑規(guī)劃中是很常見的。NikAMelchior提出的粒子RRT算法，考慮了地形的不確定性，保證了在不確定性環(huán)境下搜索樹的擴(kuò)展。 Kuffner和Lavane又提出RRT-connectlv，使得節(jié)點(diǎn)的擴(kuò)展效率大大提高。運(yùn)動規(guī)劃中，距離的定義非常復(fù)雜，Pengcheng研究了在RRT生長過程中距離函數(shù)不斷學(xué)習(xí)的算法以降低距離函數(shù)對環(huán)境的敏感性。考慮到基本RRT規(guī)劃器得到的路徑長度一般是最優(yōu)路徑的1.3~1.5倍，英國的J.desmithl研究了變分法技術(shù)使其達(dá)到最優(yōu)。Amna A引入KD樹作為二級數(shù)據(jù)結(jié)構(gòu)加速查找距離從環(huán)境中取出的隨機(jī)點(diǎn)最近的葉節(jié)點(diǎn)，降低了搜索成本。該算法在動態(tài)障礙物、高維狀態(tài)空間和存在運(yùn)動學(xué)、動力學(xué)等微分約束的環(huán)境中的運(yùn)動規(guī)劃已經(jīng)得到廣泛的應(yīng)用。

2.3 滾動在線RRT算法

基本RRT算法傾向于遍歷整個自由空間直到獲得可行路徑，這使其不可能用于未知或動態(tài)環(huán)境中的機(jī)器人在線運(yùn)動規(guī)劃。利用滾動規(guī)劃的思想可以將RRT算法進(jìn)行改進(jìn)，使其具備在線規(guī)劃能力。

2.3.1 滾動規(guī)劃

機(jī)器人在未知或動態(tài)環(huán)境中運(yùn)動時，只能探知其傳感器范圍內(nèi)有限區(qū)域內(nèi)的環(huán)境信息。機(jī)器人利用局部信息進(jìn)行局部運(yùn)動規(guī)劃，并根據(jù)一定的評價準(zhǔn)則得到局部目標(biāo)。機(jī)器人到達(dá)局部目標(biāo)后再次進(jìn)行新的局部規(guī)劃。如此反復(fù)進(jìn)行直到到達(dá)全局目標(biāo)。 滾動規(guī)劃算法的基本原理：

環(huán)境信息預(yù)測：在滾動的每一步，機(jī)器人根據(jù)探測到的視野內(nèi)的信息、或所有已知的環(huán)境信息，建立環(huán)境模型，包括設(shè)置已知區(qū)域內(nèi)的節(jié)點(diǎn)類型信息等；

局部滾動優(yōu)化：將上述環(huán)境信息模型看成一個優(yōu)化的窗口，在此基礎(chǔ)上，根據(jù)目標(biāo)點(diǎn)的位置和特定的優(yōu)化策略計算出下一步的最優(yōu)子目標(biāo)，然后根據(jù)子目標(biāo)和環(huán)境信息模型，選擇局部規(guī)劃算法，確定向子目標(biāo)行進(jìn)的局部路徑，并實施當(dāng)前策略，即依所規(guī)劃的局部路徑行進(jìn)若干步，窗口相應(yīng)向前滾動；

反饋信息校正：根據(jù)局部最優(yōu)路徑，驅(qū)動機(jī)器人行走一段路徑后，機(jī)器人會探測到新的未知信息，此時可以根據(jù)機(jī)器人在行走過程探測到的新信息補(bǔ)充或校正原來的環(huán)境模型，用于滾動后下一步的局部規(guī)劃。

其中，局部子目標(biāo)是在滾動窗口中尋找一個全局目標(biāo)的映射，它必須避開障礙物，且滿足某種優(yōu)化指標(biāo)。子目標(biāo)的選擇方法反映了全局優(yōu)化的要求與局部有限信息約束的折衷，是在給定信息環(huán)境下企圖實現(xiàn)全局優(yōu)化的自然選擇。 基于滾動窗口的路徑規(guī)劃算法依靠實時探測到的局部環(huán)境信息，以滾動方式進(jìn)行在線規(guī)劃。在滾動的每一步，根據(jù)探測到的局部信息，用啟發(fā)式方法生成優(yōu)化子目標(biāo)，在當(dāng)前滾動窗口內(nèi)進(jìn)行局部路徑規(guī)劃，然后實施當(dāng)前策略(依局部規(guī)劃路徑移動一步)，隨滾動窗口推進(jìn)，不斷取得新的環(huán)境信息，從而在滾動中實現(xiàn)優(yōu)化與反饋的結(jié)合。由于規(guī)劃問題壓縮到滾動窗口內(nèi)，與全局規(guī)劃相比其計算量大大下降。 基于滾動窗口的路徑規(guī)劃算法的具體步驟如下：

步驟0：對起點(diǎn)、終點(diǎn)、工作環(huán)境、機(jī)器人的視野半徑、步長進(jìn)行初始化；

步驟1：如果終點(diǎn)到達(dá)，規(guī)劃中止；

步驟2：對當(dāng)前滾動窗口內(nèi)的環(huán)境信息進(jìn)行刷新；

步驟3：產(chǎn)生局部子目標(biāo)；

步驟4：根據(jù)子目標(biāo)及已知環(huán)境信息，在當(dāng)前滾動窗口內(nèi)規(guī)劃一條優(yōu)化的局部可行路徑；

步驟5：依規(guī)劃的局部路徑行進(jìn)一步，步長小于視野半徑；

步驟6：返回步驟1。

2.3.2 滾動在線RRT算法流程

在一個滾動窗口內(nèi)，隨機(jī)樹以當(dāng)前位置為起始點(diǎn)，構(gòu)建傳感器范圍內(nèi)的隨機(jī)樹。構(gòu)建方法與基本RRT算法一致。為了使全局環(huán)境中隨機(jī)樹具有向目標(biāo)方向生長的趨勢，在運(yùn)動規(guī)劃時引入啟發(fā)信息，減少隨機(jī)樹的隨機(jī)性，提高搜索效率。 令?代表隨機(jī)樹中兩個位姿節(jié)點(diǎn)間的路徑代價，??代表隨機(jī)樹中兩個位姿節(jié)點(diǎn)間的歐幾里德距離。類似于A*算法，本算法為隨機(jī)樹中每個節(jié)點(diǎn)定義一個估價函數(shù)：??。其中??是隨機(jī)節(jié)點(diǎn)??到樹中節(jié)點(diǎn)??所需的路徑代價。??為啟發(fā)估價函數(shù)，這里取隨機(jī)節(jié)點(diǎn)??到目標(biāo)點(diǎn)??的距離為估價值，??。因此??表示從節(jié)點(diǎn)??經(jīng)隨機(jī)節(jié)點(diǎn)??到目標(biāo)節(jié)點(diǎn)??的路徑估計值。遍歷滾動窗口內(nèi)隨機(jī)樹T，取估價函數(shù)最小值的節(jié)點(diǎn)??，有??。這使得隨機(jī)樹沿著到目標(biāo)節(jié)點(diǎn)估價值??最小的方向進(jìn)行擴(kuò)展。 ? 由于在隨機(jī)樹生長中引入了導(dǎo)向目標(biāo)的啟發(fā)估價因子，葉節(jié)點(diǎn)??總是選擇離目標(biāo)最近的節(jié)點(diǎn)，這可能會使隨機(jī)樹遇到局部極小值問題。因此隨機(jī)樹生長的新節(jié)點(diǎn)??必須要克服這個問題，引導(dǎo)隨機(jī)樹更好的探索未知空間。 ? 這里利用統(tǒng)計學(xué)中回歸分析生成新節(jié)點(diǎn)，將RRT算法探索未知空間的能力進(jìn)一步增強(qiáng)以避免因啟發(fā)估價因子導(dǎo)致的局部極小。其思想是探索以前到過的空間是無用的，而且容易陷入局部極小。引進(jìn)回歸分析(regression analysis)是考察新節(jié)點(diǎn)與其他節(jié)點(diǎn)之間關(guān)系，利用回歸函數(shù)約束，使得隨機(jī)樹不探索以前到過的空間，因此避免了局部極小。 ? 新節(jié)點(diǎn)生成方法是遍歷隨機(jī)樹，如果??與其父節(jié)點(diǎn)??的距離小于??與擴(kuò)展樹上其他任意節(jié)點(diǎn)的距離，即??，則選擇該節(jié)點(diǎn)為隨機(jī)樹新生節(jié)點(diǎn)。下圖解釋了新節(jié)點(diǎn)的判斷過程。 ?

圖8 新節(jié)點(diǎn)的判斷 上圖中各個空心點(diǎn)是中間的父節(jié)點(diǎn)的可能擴(kuò)展。橢圓圈起的空心點(diǎn)表示這個新節(jié)點(diǎn)不符合回歸函數(shù)約束，剩下的兩個未被圈起的空心節(jié)點(diǎn)到其父節(jié)點(diǎn)的距離小于該節(jié)點(diǎn)到隨機(jī)樹上任意節(jié)點(diǎn)的距離，這兩個點(diǎn)可以成為隨機(jī)樹的新節(jié)點(diǎn)。 綜上，滾動窗口內(nèi)隨機(jī)樹構(gòu)建的具體步驟如下：

對滾動窗口隨機(jī)樹T初始化，T開始只包含初始位置S；

滾動窗口自由空間中隨機(jī)選擇一個狀態(tài)；

根據(jù)最短路徑思想尋找樹T中和?距離最近的節(jié)點(diǎn)?；

選擇輸入?，使機(jī)器人狀態(tài)由??到?；

確定?是否符合回歸分析，不符合則回到第4步；

將?作為隨機(jī)樹T的一個新節(jié)點(diǎn)，??則被記錄在連接節(jié)點(diǎn)??和??的邊上。

滾動窗口狀態(tài)空間進(jìn)行K次采樣后，遍歷隨機(jī)樹，根據(jù)啟發(fā)估價思想尋找滾動窗口子目標(biāo)。??是當(dāng)前滾動窗口中的子樹中估價函數(shù)最小的點(diǎn)。確定子目標(biāo)后，機(jī)器人前進(jìn)到子目標(biāo)點(diǎn)，進(jìn)行下一輪的滾動RRT規(guī)劃。如此反復(fù)，直到到達(dá)目標(biāo)點(diǎn)G。 ?

2.4 人工勢場法

人工勢場法是由Khatib提出的一種用于機(jī)器人運(yùn)動規(guī)劃的虛擬力方法。其基本思想是將目標(biāo)和障礙物對機(jī)器人運(yùn)動的影響具體化成人造勢場。目標(biāo)處勢能低，障礙物處勢能高。這種勢差產(chǎn)生了目標(biāo)對機(jī)器人的引力和障礙物對機(jī)器人的斥力，其合力控制機(jī)器人沿勢場的負(fù)梯度方向向目標(biāo)點(diǎn)運(yùn)動。人工勢場法計算方便，得到的路徑安全平滑，但是復(fù)雜的勢場環(huán)境可能在目標(biāo)點(diǎn)之外產(chǎn)生局部極小點(diǎn)導(dǎo)致機(jī)器人無法到達(dá)目標(biāo)。 為了解決人工勢場法的局部極小點(diǎn)問題，學(xué)者們提出了各種改進(jìn)方法。主要分成兩個方向：一個是構(gòu)造合適的勢函數(shù)以減小或避免局部極小點(diǎn)的出現(xiàn)；另一種是在機(jī)器人遇到局部極小點(diǎn)后結(jié)合其他的方法使機(jī)器人離開局部極小點(diǎn)。前者一般需要全局地圖信息，并且依賴于障礙物的形狀。當(dāng)環(huán)境復(fù)雜時難以應(yīng)用。后者多利用搜索法、多勢場法和沿墻行走法等方法使機(jī)器人離開局部極小點(diǎn)。搜索法利用最佳優(yōu)先、模擬退火、隨即搜索等策略尋找比局部極小點(diǎn)勢場值更低的點(diǎn)使機(jī)器人繼續(xù)移動。由于未知環(huán)境中大多缺乏啟發(fā)信息，搜索方法的效率很低。多勢場法構(gòu)造多個全局極小點(diǎn)相同，而局部極小點(diǎn)不同的勢函數(shù)，在機(jī)器人陷入某個局部極小點(diǎn)時，規(guī)劃器就切換勢函數(shù)使機(jī)器人離開該點(diǎn)。 但是在未知的環(huán)境中這樣的多個勢場很難構(gòu)造，而且該方法可能導(dǎo)致機(jī)器人在回到曾逃離的局部極小點(diǎn)。由于局部極小點(diǎn)是某個或多個障礙物的斥力勢場與引力勢場共同作用產(chǎn)生，其位置與障礙物距離必然不遠(yuǎn)，沿墻行走法正是利用這樣的遠(yuǎn)離，使機(jī)器人在遇到局部極小點(diǎn)后參照類似BUG算法的環(huán)繞行為繞過產(chǎn)生局部極小點(diǎn)的障礙物繼續(xù)前進(jìn)。這種方法可靠性高，不依賴環(huán)境的先驗信息和障礙物形狀。 本節(jié)構(gòu)造人工勢場進(jìn)行機(jī)器人平動的在線運(yùn)動規(guī)劃，利用一種沿墻行走法對基本的人工勢場法進(jìn)行改進(jìn)。

2.4.1 基本人工勢場法

作用在機(jī)器人上的假想引力和斥力為勢函數(shù)的負(fù)梯度，因而人工勢函數(shù)應(yīng)該具有以下特征：

非負(fù)且連續(xù)可微；

斥力勢強(qiáng)度距離障礙物越近其強(qiáng)度越大；

引力勢強(qiáng)度離目標(biāo)位置越近其強(qiáng)度越小。

空間中的合勢場是引力勢場與斥力勢場之和： 其中，??是目標(biāo)產(chǎn)生的引力勢場；??是各個障礙物產(chǎn)生的斥力勢場之和，即：?

。 這里構(gòu)造如下的引力勢函數(shù)和斥力勢函數(shù)：

其中，?表示引力勢的相對影響；??表示第??個障礙物的斥力勢的相對影響，??表示機(jī)器人當(dāng)前位置，??表示目標(biāo)點(diǎn)位置，??表示機(jī)器人距目標(biāo)的距離，??的作用是在機(jī)器人距離目標(biāo)較遠(yuǎn)時，削弱目標(biāo)引力勢的作用，??表示機(jī)器人距離第??個障礙物的距離，??表示第??個障礙物的斥力勢作用范圍。 ? ?和??對勢場形狀的影響很大，適當(dāng)?shù)脑龃??能夠增強(qiáng)引力勢場的作用，有助于減少產(chǎn)生局部極小點(diǎn)的可能，并加快機(jī)器人向目標(biāo)運(yùn)動。??影響機(jī)器人在障礙物附近的運(yùn)動特性，??比較大可以使機(jī)器人距離障礙物更遠(yuǎn)，運(yùn)動路徑更安全；??比較小，機(jī)器人在避開障礙物時運(yùn)動比較平滑。 ? 利用上面勢函數(shù)的梯度可以計算機(jī)器人收到的假想引力和斥力： ?

2.4.2 人工勢場法算法改進(jìn)

當(dāng)機(jī)器人的運(yùn)行環(huán)境中包含形狀復(fù)雜或者距離很近的障礙物時，可能出現(xiàn)勢場局部極小點(diǎn)，導(dǎo)致機(jī)器人在該處停止或在其周圍振動。如下圖所示，當(dāng)環(huán)境中出現(xiàn)“陷阱”形障礙物或者與目標(biāo)成特定位置關(guān)系的障礙物時，可能在人工勢場中產(chǎn)生局部極小點(diǎn)（圖中L點(diǎn)），當(dāng)機(jī)器人運(yùn)動到局部極小點(diǎn)附近時，勢場的負(fù)梯度方向指向L點(diǎn)。機(jī)器人將在L點(diǎn)處停止或在其附近振動或作圓周運(yùn)動。

圖9 人工勢場法的局部極小點(diǎn) 為了使機(jī)器人從局部極小點(diǎn)中逃離，在人工勢場法的基礎(chǔ)上引入應(yīng)激行為，即增加繞行行為。當(dāng)機(jī)器人遇到局部極小點(diǎn)時，忽略目標(biāo)引力勢的作用，沿著斥力勢的等勢面方向移動，直到機(jī)器人離開局部極小區(qū)域。改進(jìn)的算法流程如下：

根據(jù)傳感器信息計算當(dāng)前位置的引力和斥力；

判斷是否處于繞行行為，若是，執(zhí)行3；若否，執(zhí)行4；

判斷是否離開局部極小區(qū)域，若是，機(jī)器人沿著合力方向運(yùn)動，結(jié)束繞行行為；若否，機(jī)器人沿著斥力場等勢線運(yùn)動，繼續(xù)繞行行為；

判斷是否遇到局部極小點(diǎn)，若是，機(jī)器人沿著斥力場等勢線運(yùn)動，開始繞行行為；若否，機(jī)器人沿著合力方向運(yùn)動；

判斷是否到達(dá)目標(biāo)，若是，退出算法；若否，繼續(xù)1；

使用下面的判別條件判斷機(jī)器人是否遇到局部極小點(diǎn)。 條件1： 條件2：? ? 當(dāng)條件1或者條件2出現(xiàn)時，就認(rèn)為機(jī)器人遇到了局部極小點(diǎn)。條件1中??是一個很小的正數(shù)，其含義是機(jī)器人受到的虛擬合力接近0。這是最直接局部極小點(diǎn)判斷方法。條件2中??為0,1之間某一正數(shù)，??為機(jī)器人運(yùn)動過程中某一狀態(tài)，??表示機(jī)器人從??到達(dá)當(dāng)前位置??的總路程，條件2成立意味著機(jī)器人在運(yùn)動很長路程后，位移很小。用來檢測機(jī)器人在局部極小點(diǎn)附近發(fā)生的振動和圓周運(yùn)動。 ?

2.5 BUG算法

BUG算法是一種完全應(yīng)激的機(jī)器人避障算法。其算法原理類似昆蟲爬行的運(yùn)動決策策略。在未遇到障礙物時，沿直線向目標(biāo)運(yùn)動；在遇到障礙物后，沿著障礙物邊界繞行，并利用一定的判斷準(zhǔn)則離開障礙物繼續(xù)直行。這種應(yīng)激式的算法計算簡便，不需要獲知全局地圖和障礙物形狀，具備完備性。但是其生成的路徑平滑性不夠好，對機(jī)器人的各種微分約束適應(yīng)性比較差。

2.5.1 BUG1算法

該算法的基本思想是在沒有障礙物時，沿著直線向目標(biāo)運(yùn)動可以得到最短的路線。當(dāng)傳感器檢測到障礙物時，機(jī)器人繞行障礙物直到能夠繼續(xù)沿直線項目標(biāo)運(yùn)動。BUG1算法實現(xiàn)了最基本的向目標(biāo)直行和繞行障礙物的思想。 假設(shè)機(jī)器人能夠計算兩點(diǎn)之間的距離，并且不考慮機(jī)器人的定位誤差。初始位置和目標(biāo)位置分別用?和??表示；機(jī)器人在??時刻的位置表示為??；??表示連接機(jī)器人位置??和目標(biāo)點(diǎn)的直線。初始時，??。若沒有探測到障礙物，那么機(jī)器人就沿著??向目標(biāo)直行，直到到達(dá)目標(biāo)點(diǎn)或者遇到障礙物。當(dāng)遇到障礙物時，記下當(dāng)前位置??。然后機(jī)器人環(huán)繞障礙物直到又一次到達(dá)??，找到環(huán)繞路線上距離目標(biāo)最近的點(diǎn)??，并沿著障礙物邊界移動到該點(diǎn)。隨后，直線??更新，機(jī)器人繼續(xù)沿直線向目標(biāo)運(yùn)動。如果沿這條直線運(yùn)動時還會遇到該障礙物，那么機(jī)器人不能到達(dá)目標(biāo)點(diǎn)。否則算法不斷循環(huán)直到機(jī)器人到達(dá)目標(biāo)點(diǎn)或者規(guī)劃器認(rèn)為機(jī)器人無法到達(dá)目標(biāo)點(diǎn)。 ?

圖10 BUG1算法運(yùn)動規(guī)劃

圖11 BUG1算法中認(rèn)為機(jī)器人無法到達(dá)目標(biāo)點(diǎn)的情況

圖12 BUG1算法偽代碼

2.5.2 BUG2算法

BUG2算法也有兩種運(yùn)動：朝向目標(biāo)的直行和沿邊界繞行。與BUG1算法不同的是，BUG2算法中的直線?是連接初始點(diǎn)和目標(biāo)點(diǎn)的直線，在計算過程中保持不變。當(dāng)機(jī)器人在點(diǎn)遇到障礙物時，機(jī)器人開始繞行障礙物，如果機(jī)器人在繞行過程中在距離目標(biāo)更近的點(diǎn)再次遇到直線??，那么就停止繞行，繼續(xù)沿著直線??向目標(biāo)直行。如此循環(huán)，直到機(jī)器人到達(dá)目標(biāo)點(diǎn)??。如果機(jī)器人在繞行過程中未遇到直線??上與目標(biāo)更近的??點(diǎn)而回到了??點(diǎn)，那么得出結(jié)論，機(jī)器人不能到達(dá)目標(biāo)。 ?

圖13 BUG2算法運(yùn)動規(guī)劃

圖14 BUG2算法中認(rèn)為機(jī)器人無法到達(dá)目標(biāo)點(diǎn)的情況

圖15 BUG2算法偽代碼 BUG1和BUG2算法提供了搜索問題的兩種基本方法：比較保守的BUG1算法進(jìn)行詳細(xì)的搜索來獲得最佳的離開點(diǎn)。這需要機(jī)器人環(huán)繞整個障礙物的邊界。而BUG2算法使用一種投機(jī)的方法。機(jī)器人不環(huán)繞完整的障礙物，而選擇第一個可用的點(diǎn)作為離開點(diǎn)。對于一般的環(huán)境，BUG2算法的效率更高；而對于復(fù)雜形狀的障礙物，保守的BUG1算法性能更優(yōu)。

2.5.3 TangentBUG算法

TangentBUG算法是對BUG2算法的提高。它利用機(jī)器人上距離傳感器的讀數(shù)對障礙物做出提前規(guī)避，可以獲得更短更平滑的機(jī)器人路徑。在一個靜態(tài)環(huán)境中，傳感器讀數(shù)?是機(jī)器人位置??和傳感器掃描角度??的函數(shù)，具體點(diǎn)說，??是沿著??的射線以角度??到達(dá)最近障礙物的距離， ?

其中

。 對于某一個固定的位置?，??被傳感器視野內(nèi)的障礙物分割成多個連續(xù)區(qū)間。如下圖所示。??出現(xiàn)不連續(xù)或者到達(dá)傳感器最大測量范圍的角度就是這些連續(xù)區(qū)間的端點(diǎn)。TangentBUG算法利用者些區(qū)間的端點(diǎn)避開工作空間中的障礙物。 ?

圖16 距離傳感器掃描障礙物 對?不連續(xù)的情況做出說明（如圖17所示）：點(diǎn)??，??，??，??，??，??，??和??與障礙物的不連續(xù)性相關(guān)；請注意，這里的射線與障礙物相切。點(diǎn)??是不連續(xù)的，因為障礙物邊界落在傳感器的范圍之外。??和??，??和??，??和??，??和??之間的free space邊界上的點(diǎn)集是連續(xù)性的間隔（加粗線部分）。 ?

圖17 不連續(xù)的示意圖 與其他的BUG算法一樣，TangentBUG算法也有兩種行為：直行（motion-to-go）和環(huán)繞障礙物（boundary-following）。 首先機(jī)器人沿著直線向目標(biāo)運(yùn)動，直到它利用傳感器觀測到在其運(yùn)動方向的前方有障礙物。不在機(jī)器人前方的障礙物對其向目標(biāo)運(yùn)動沒有影響。比如下圖中的障礙物，障礙物??在傳感器視野內(nèi)，但是不阻礙機(jī)器人的運(yùn)動。機(jī)器人在剛剛探測到障礙物時，傳感器視野圓與障礙物邊界相切。隨著機(jī)器人繼續(xù)向前移動，這個切點(diǎn)分裂成兩個交點(diǎn)??和??（??和??），??和??之間的障礙物邊界區(qū)間與機(jī)器人運(yùn)動直線相交。 ?

圖18 機(jī)器人向目標(biāo)運(yùn)動遇到障礙物 此時，機(jī)器人不能繼續(xù)向目標(biāo)運(yùn)動，而從兩個交點(diǎn)中選擇一個作為局部目標(biāo)。為比較兩個交點(diǎn)對于機(jī)器人向目標(biāo)運(yùn)動的優(yōu)劣性，定義探索距離。在沒有關(guān)于障礙物的其他信息時，可以將探索距離??定義為從??經(jīng)過一個交點(diǎn)到目標(biāo)點(diǎn)的折線長度，即：?。 例如圖19，機(jī)器人“看見”了障礙??，并選擇向??移動，因為??。 當(dāng)機(jī)器人位于??時，它無法知道??阻擋了從??到目標(biāo)??的路徑。 ? 在圖20中，當(dāng)機(jī)器人位于??但目標(biāo)??不同時，它具有足夠的傳感器信息來得出結(jié)論：??確實阻擋了從??到目標(biāo)??的路徑，從而朝??行駛。 ? 因此，選擇向??行駛剛開始的時候可能使得??最小化，而不是向??行駛，但是planner有效地為??分配無限大的成本代價，因為它有足夠的信息來推斷任何通過??的路徑都不是最理想的。 ?

圖19

圖20 機(jī)器人將選擇探索距離短的交點(diǎn)作為局部目標(biāo)，向之運(yùn)動。隨著機(jī)器人不斷運(yùn)動，交點(diǎn)不斷更新，探索距離也不斷減小。如下圖所示。當(dāng)?時，機(jī)器人還沒有探測到障礙物，因而它向目標(biāo)作直線運(yùn)動；當(dāng)??時，機(jī)器人開始探測到障礙物，并朝向障礙物探索距離近的一側(cè)運(yùn)動；當(dāng)??和??時，機(jī)器人繼續(xù)移動，同時更新探測區(qū)域，在這個過程中探索距離不斷減小。 ?

圖21 機(jī)器人運(yùn)動時不斷更新局部目標(biāo)和探測距離 在機(jī)器人運(yùn)動過程中，探索距離不再減小時，就停止向目標(biāo)運(yùn)動行為，切換到環(huán)繞邊界行為。此時，機(jī)器人找到了探測距離的一個極小值，并可計算已探測的障礙物邊界與目標(biāo)?的最近距離?。機(jī)器人按照原來的方向環(huán)繞障礙物運(yùn)動，同時機(jī)器人更新當(dāng)前探測的障礙物邊界與目標(biāo)的最近距離?。當(dāng)發(fā)現(xiàn)??時，機(jī)器人停止障礙物環(huán)繞行為，繼續(xù)向目標(biāo)運(yùn)動。 ?

圖22 機(jī)器人環(huán)繞障礙物運(yùn)動 如上圖所示，當(dāng)機(jī)器人探索到障礙物上的?點(diǎn)后，探索距離就不再減小，即??點(diǎn)是機(jī)器人探索距離在障礙物邊界上的局部極小點(diǎn)。機(jī)器人開始沿著障礙物邊界進(jìn)行環(huán)繞，圖中虛線路徑就是機(jī)器人環(huán)繞障礙物時所走的路徑。當(dāng)機(jī)器人探測到與目標(biāo)距離相比??點(diǎn)更近的點(diǎn)時，重新開始接近目標(biāo)的運(yùn)動。 ?

2.6 增量式啟發(fā)算法

2.6.1 LPA*算法

LPA*算法，即Lifelong Planning A*算法，該算法于2001年由Sven Koenig和Maxim Likhachev提出，是一種增量啟發(fā)式搜索版本的A*算法，這種路徑規(guī)劃算法適用于隨著時間改變導(dǎo)致有限柵格地圖上的邊緣代價c(s1,s2)改變的問題，也就是隨著時間改變障礙物增多或減少，網(wǎng)格點(diǎn)發(fā)生增刪等，在許多場合下比再利用A*重新搜索更高效。

2.6.2 D* Lite算法

D* Lite算法是以LPA*為基礎(chǔ)，是Maxim Likhachev和Sven Koenig于2002年基于LPA*，結(jié)合A*算法思想，提出一種增量啟發(fā)式算法，適用于在未知環(huán)境中的導(dǎo)航以及路徑規(guī)劃，廣泛用于目前各種移動機(jī)器人和自主車輛載具，例如“機(jī)遇號”和“勇氣號”火星車測試的原型導(dǎo)航系統(tǒng)。

2.7 小結(jié)

本章研究了幾種常用的運(yùn)動規(guī)劃算法。其中，人工勢場法應(yīng)用靈活，可以在保證安全的情況下獲得一條平滑路徑，并且對于動態(tài)環(huán)境可以實現(xiàn)實時運(yùn)動控制。適合用于長距離機(jī)動且障礙物較少的情況。而基于隨機(jī)采樣的搜索樹方法可以在復(fù)雜約束環(huán)境中獲得可行解，適合用于機(jī)械臂近距離操作。

審核編輯 ：李倩