來源 | 知乎

知圈 |?進(jìn)“高精度地圖社群”，請加微信15221054164，備注地圖

目錄

1 自主機(jī)器人近距離操作運動規(guī)劃體系········1.1 單個自主機(jī)器人的規(guī)劃體系········1.2 多自主機(jī)器人協(xié)同規(guī)劃體系2 路徑規(guī)劃研究········2.1 圖搜索法················2.1.1 可視圖法················2.1.2 Dijkstra算法················2.1.3 A*算法········2.2 RRT算法················2.2.1 算法步驟················2.2.2 改進(jìn)算法········2.3 滾動在線RRT算法················2.3.1 滾動規(guī)劃················2.3.2 滾動在線RRT算法流程········2.4 人工勢場法················2.4.1 基本人工勢場法················2.4.2 人工勢場法算法改進(jìn)········2.5 BUG算法················2.5.1 BUG1算法················2.5.2 BUG2算法················2.5.3 TangentBUG算法········2.6 增量式啟發(fā)算法················2.6.1 LPA*算法················2.6.2 D* Lite算法········2.7 小結(jié)自主機(jī)器人近距離操作運動規(guī)劃體系在研究自主運動規(guī)劃問題之前，首先需建立相對較為完整的自主運動規(guī)劃體系，再由該體系作為指導(dǎo)，對自主運動規(guī)劃的各項具體問題進(jìn)行深入研究。本節(jié)將根據(jù)自主機(jī)器人的思維方式、運動形式、任務(wù)行為等特點，建立與之相適應(yīng)的自主運動規(guī)劃體系。并按照機(jī)器人的數(shù)量與規(guī)模，將自主運動規(guī)劃分為單個機(jī)器人的運動規(guī)劃與多機(jī)器人協(xié)同運動規(guī)劃兩類規(guī)劃體系。

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

運動規(guī)劃系統(tǒng)是自主控制系統(tǒng)中主控單元的核心部分，因此有必要先研究自主控制系統(tǒng)和其主控單元的體系結(jié)構(gòu)問題。自主控制技術(shù)研究至今，先后出現(xiàn)了多種體系結(jié)構(gòu)形式，目前被廣泛應(yīng)用于實踐的是分布式體系結(jié)構(gòu)，其各個功能模塊作為相對獨立的單元參與整個體系。隨著人工智能技術(shù)的不斷發(fā)展，基于多Agent的分布式體系結(jié)構(gòu)逐漸成為了主流，各功能模塊作為獨立的智能體參與整個自主控制過程，該體系結(jié)構(gòu)應(yīng)用的基本形式如圖1所示。一方面，主控單元與測控介入處理、姿態(tài)控制系統(tǒng)、軌道控制系統(tǒng)、熱控系統(tǒng)、能源系統(tǒng)、數(shù)傳、有效載荷控制等功能子系統(tǒng)相互獨立為智能體，由總線相連；另一方面，主控單元為整個系統(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運行管理和調(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)度、運動行為規(guī)劃和控制指令生成三層基本結(jié)構(gòu)組成，由任務(wù)生成與調(diào)度層獲得基本的飛行任務(wù)，經(jīng)過運動行為規(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中，點劃線內(nèi)是主控單元全部模塊，虛線內(nèi)為運動規(guī)劃系統(tǒng)，包括運動行為規(guī)劃模塊和重規(guī)劃模塊，這也是運動規(guī)劃系統(tǒng)的主要功能。圖2? 主控單元基本結(jié)構(gòu)示意圖明確了自主控制系統(tǒng)與其主控單元的基本結(jié)構(gòu)，以及運動規(guī)劃系統(tǒng)在主控單元中的基本功能，便可建立運動規(guī)劃系統(tǒng)的體系結(jié)構(gòu)。運動規(guī)劃系統(tǒng)的體系結(jié)構(gòu)如圖3所示，該系統(tǒng)由規(guī)劃器和重規(guī)劃器兩大執(zhí)行單元組成，分別承擔(dān)對飛行任務(wù)的一般規(guī)劃和對突發(fā)事件緊急處理的運動規(guī)劃。當(dāng)然，這兩部分也可理解為離線規(guī)劃與在線規(guī)劃兩種，離線規(guī)劃一般解決平時按部就班的飛行任務(wù)，在線規(guī)劃一般解決突然下達(dá)的飛行任務(wù)。除規(guī)劃器以外，系統(tǒng)還配有知識域模塊，用以利用特定語言描述相關(guān)知識。知識域包括行為域和模型域兩個部分，行為域用來存儲服務(wù)系統(tǒng)一般的運動行為描述和緊急情況下的一些運動行為方面的處理方法(如急停、轉(zhuǎn)向等)，模型域用來存儲規(guī)劃所需模型知識，包括環(huán)境模型、組裝體模型、組裝任務(wù)對象模型和任務(wù)模型等等。圖3 運動規(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ī)器人信息，運用規(guī)劃算法對任務(wù)進(jìn)行分解，并分配給各受控機(jī)器人，組織它們完成任務(wù)。其優(yōu)點是理論條理清晰，實現(xià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ī)劃器得到具體的行為運動規(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)，單獨規(guī)劃各自運動行為，并從各自的知識域中獲取協(xié)商方式，外界環(huán)境信息由主控單元發(fā)送和自我感知相結(jié)合獲得(主控單元停止工作時，僅靠自我感知獲取信息)，其它機(jī)器人信息的傳輸由機(jī)器人間的數(shù)據(jù)鏈實現(xiàn)。圖4 多自主機(jī)器人協(xié)同規(guī)劃體系結(jié)構(gòu)示意圖路徑規(guī)劃研究當(dāng)給定了某一特定的任務(wù)之后，如何規(guī)劃機(jī)器人的運動方式將至關(guān)重要。機(jī)器人的規(guī)劃包括兩部分內(nèi)容：基座移動到適合操作的位置和轉(zhuǎn)動手臂關(guān)節(jié)完成操作。包括三個問題：基座點到點運動規(guī)劃；關(guān)節(jié)空間規(guī)劃；綜合規(guī)劃。本章研究幾種常用的運動規(guī)劃算法：圖搜索法、RRT算法、人工勢場法、BUG算法。并對部分算法的自身缺陷進(jìn)行了一些改進(jìn)。

2.1 圖搜索法

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

2.1.1 可視圖法

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

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

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

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

2.1.2 Dijkstra算法

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

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é)點，即初始狀態(tài)S。在自由空間中隨機(jī)選取一個狀態(tài)點?，遍歷當(dāng)前的快速隨機(jī)搜索樹T，找到T上距離?最近的節(jié)點?，考慮機(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)點開始，逐次找到父節(jié)點直到到達(dá)初始狀態(tài)，即搜索樹的根節(jié)點。圖7 隨機(jī)樹構(gòu)建過程由于在搜索過程中考慮了機(jī)器人的動力學(xué)約束，因此生成的路徑的可行性很好。但是算法的隨機(jī)性導(dǎo)致其只具備概率完備性。

2.2.2 改進(jìn)算法

LaValle等人的工作奠定了RRT方法的基礎(chǔ)。在采樣策略方面，RRTGoalBiaS方法在控制機(jī)器人隨機(jī)運動的同時，以一定概率向最終目標(biāo)運動；RRTGoalZoom方法分別在整個空間和目標(biāo)點周圍的空間進(jìn)行采樣；RRTCon方法則通過加大隨機(jī)步長改進(jìn)規(guī)劃速度。雙向規(guī)劃思想也被采用，衍生出RRTExtExt，RRTExtCon，RRTConCon等多種算法。基本RRT算法收斂到終點位姿的速度可能比較慢。為了提高算法的效率和性能，需不斷對該算法進(jìn)行改進(jìn)。如為了提高搜索效率采用雙向隨機(jī)搜索樹(Bi~RRT)，從起始點和目標(biāo)點并行生成兩棵RRT，直至兩棵樹相遇，算法收斂。由于這個算法相比于原始RRT有更好的收斂性，因此在目前路徑規(guī)劃中是很常見的。NikAMelchior提出的粒子RRT算法，考慮了地形的不確定性，保證了在不確定性環(huán)境下搜索樹的擴(kuò)展。Kuffner和Lavane又提出RRT-connectlv，使得節(jié)點的擴(kuò)展效率大大提高。運動規(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ī)點最近的葉節(jié)點，降低了搜索成本。該算法在動態(tài)障礙物、高維狀態(tài)空間和存在運動學(xué)、動力學(xué)等微分約束的環(huán)境中的運動規(guī)劃已經(jīng)得到廣泛的應(yīng)用。

2.3 滾動在線RRT算法

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

2.3.1 滾動規(guī)劃

機(jī)器人在未知或動態(tài)環(huán)境中運動時，只能探知其傳感器范圍內(nèi)有限區(qū)域內(nèi)的環(huán)境信息。機(jī)器人利用局部信息進(jì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é)點類型信息等；

• 局部滾動優(yōu)化：將上述環(huán)境信息模型看成一個優(yōu)化的窗口，在此基礎(chǔ)上，根據(jù)目標(biāo)點的位置和特定的優(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：對起點、終點、工作環(huán)境、機(jī)器人的視野半徑、步長進(jìn)行初始化；

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

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

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

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

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

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

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

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

2.4 人工勢場法

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

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

當(dāng)機(jī)器人的運行環(huán)境中包含形狀復(fù)雜或者距離很近的障礙物時，可能出現(xiàn)勢場局部極小點，導(dǎo)致機(jī)器人在該處停止或在其周圍振動。如下圖所示，當(dāng)環(huán)境中出現(xiàn)“陷阱”形障礙物或者與目標(biāo)成特定位置關(guān)系的障礙物時，可能在人工勢場中產(chǎn)生局部極小點(圖中L點)，當(dāng)機(jī)器人運動到局部極小點附近時，勢場的負(fù)梯度方向指向L點。機(jī)器人將在L點處停止或在其附近振動或作圓周運動。圖9 人工勢場法的局部極小點為了使機(jī)器人從局部極小點中逃離，在人工勢場法的基礎(chǔ)上引入應(yīng)激行為，即增加繞行行為。當(dāng)機(jī)器人遇到局部極小點時，忽略目標(biāo)引力勢的作用，沿著斥力勢的等勢面方向移動，直到機(jī)器人離開局部極小區(qū)域。改進(jìn)的算法流程如下：

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

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

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

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

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

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

2.5 BUG算法

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

2.5.1 BUG1算法

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

2.5.2 BUG2算法

BUG2算法也有兩種運動：朝向目標(biāo)的直行和沿邊界繞行。與BUG1算法不同的是，BUG2算法中的直線?是連接初始點和目標(biāo)點的直線，在計算過程中保持不變。當(dāng)機(jī)器人在點遇到障礙物時，機(jī)器人開始繞行障礙物，如果機(jī)器人在繞行過程中在距離目標(biāo)更近的點再次遇到直線?，那么就停止繞行，繼續(xù)沿著直線?向目標(biāo)直行。如此循環(huán)，直到機(jī)器人到達(dá)目標(biāo)點?。如果機(jī)器人在繞行過程中未遇到直線?上與目標(biāo)更近的?點而回到了?點，那么得出結(jié)論，機(jī)器人不能到達(dá)目標(biāo)。圖13? BUG2算法運動規(guī)劃圖14? BUG2算法中認(rèn)為機(jī)器人無法到達(dá)目標(biāo)點的情況圖15 BUG2算法偽代碼BUG1和BUG2算法提供了搜索問題的兩種基本方法：比較保守的BUG1算法進(jìn)行詳細(xì)的搜索來獲得最佳的離開點。這需要機(jī)器人環(huán)繞整個障礙物的邊界。而BUG2算法使用一種投機(jī)的方法。機(jī)器人不環(huá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ù)，具體點說，?是沿著?的射線以角度?到達(dá)最近障礙物的距離，其中?。對于某一個固定的位置?，?被傳感器視野內(nèi)的障礙物分割成多個連續(xù)區(qū)間。如下圖所示。?出現(xiàn)不連續(xù)或者到達(dá)傳感器最大測量范圍的角度就是這些連續(xù)區(qū)間的端點。TangentBUG算法利用者些區(qū)間的端點避開工作空間中的障礙物。圖16 距離傳感器掃描障礙物對?不連續(xù)的情況做出說明(如圖17所示)：點?，?，?，?，?，?，?和?與障礙物的不連續(xù)性相關(guān)；請注意，這里的射線與障礙物相切。點?是不連續(xù)的，因為障礙物邊界落在傳感器的范圍之外。?和?，?和?，?和?，?和?之間的free space邊界上的點集是連續(xù)性的間隔(加粗線部分)。圖17 不連續(xù)的示意圖與其他的BUG算法一樣，TangentBUG算法也有兩種行為：直行(motion-to-go)和環(huán)繞障礙物(boundary-following)。首先機(jī)器人沿著直線向目標(biāo)運動，直到它利用傳感器觀測到在其運動方向的前方有障礙物。不在機(jī)器人前方的障礙物對其向目標(biāo)運動沒有影響。比如下圖中的障礙物?，障礙物?在傳感器視野內(nèi)，但是不阻礙機(jī)器人的運動。機(jī)器人在剛剛探測到障礙物時，傳感器視野圓與障礙物邊界相切。隨著機(jī)器人繼續(xù)向前移動，這個切點分裂成兩個交點?和?(?和?)，?和?之間的障礙物邊界區(qū)間與機(jī)器人運動直線相交。圖18 機(jī)器人向目標(biāo)運動遇到障礙物此時，機(jī)器人不能繼續(xù)向目標(biāo)運動，而從兩個交點中選擇一個作為局部目標(biāo)。為比較兩個交點對于機(jī)器人向目標(biāo)運動的優(yōu)劣性，定義探索距離?。在沒有關(guān)于障礙物的其他信息時，可以將探索距離?定義為從?經(jīng)過一個交點到目標(biāo)點的折線長度，即：?。例如圖19，機(jī)器人“看見”了障礙，并選擇向移動，因為?當(dāng)機(jī)器人位于?時，它無法知道?阻擋了從?到目標(biāo)?的路徑。在圖20中，當(dāng)機(jī)器人位于?但目標(biāo)?不同時，它具有足夠的傳感器信息來得出結(jié)論：?確實阻擋了從?到目標(biāo)?的路徑，從而朝?行駛。因此，選擇向?行駛剛開始的時候可能使得?最小化，而不是向?行駛，但是planner有效地為?分配無限大的成本代價，因為它有足夠的信息來推斷任何通過?的路徑都不是最理想的。圖19圖20機(jī)器人將選擇探索距離短的交點作為局部目標(biāo)，向之運動。隨著機(jī)器人不斷運動，交點不斷更新，探索距離也不斷減小。如下圖所示。當(dāng)?時，機(jī)器人還沒有探測到障礙物，因而它向目標(biāo)作直線運動；當(dāng)?時，機(jī)器人開始探測到障礙物，并朝向障礙物探索距離近的一側(cè)運動；當(dāng)?和?時，機(jī)器人繼續(xù)移動，同時更新探測區(qū)域，在這個過程中探索距離不斷減小。圖21 機(jī)器人運動時不斷更新局部目標(biāo)和探測距離在機(jī)器人運動過程中，探索距離不再減小時，就停止向目標(biāo)運動行為，切換到環(huán)繞邊界行為。此時，機(jī)器人找到了探測距離的一個極小值，并可計算已探測的障礙物邊界與目標(biāo)?的最近距離?。機(jī)器人按照原來的方向環(huán)繞障礙物運動，同時機(jī)器人更新當(dāng)前探測的障礙物邊界與目標(biāo)的最近距離?。當(dāng)發(fā)現(xiàn)?時，機(jī)器人停止障礙物環(huán)繞行為，繼續(xù)向目標(biāo)運動。圖22 機(jī)器人環(huán)繞障礙物運動如上圖所示，當(dāng)機(jī)器人探索到障礙物上的?點后，探索距離就不再減小，即?點是機(jī)器人探索距離在障礙物邊界上的局部極小點。機(jī)器人開始沿著障礙物邊界進(jìn)行環(huán)繞，圖中虛線路徑就是機(jī)器人環(huán)繞障礙物時所走的路徑。當(dāng)機(jī)器人探測到與目標(biāo)距離相比?點更近的點時，重新開始接近目標(biāo)的運動。

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)格點發(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é)

本章研究了幾種常用的運動規(guī)劃算法。其中，人工勢場法應(yīng)用靈活，可以在保證安全的情況下獲得一條平滑路徑，并且對于動態(tài)環(huán)境可以實現(xiàn)實時運動控制。適合用于長距離機(jī)動且障礙物較少的情況。而基于隨機(jī)采樣的搜索樹方法可以在復(fù)雜約束環(huán)境中獲得可行解，適合用于機(jī)械臂近距離操作。參考文獻(xiàn)：【1】Choset H , Kantor G A , Thrun S . Principles of Robot Motion: Theory, Algorithms, and Implementations[M]. MIT Press, 2005.這本書作者有pdf原文，如果需要請知乎私信作者(ID：搬磚的旺財)。(PS：這本書真的很經(jīng)典，如果做運動規(guī)劃挺適合細(xì)讀并且推導(dǎo)里面的公式。)說明：文章觀點僅供分享交流，不代表焉知自動駕駛的立場，轉(zhuǎn)載請注明出處，如涉及版權(quán)等問題，請您告知(小老虎13636581676微信同)，我們將及時溝通處理。原文鏈接https://zhuanlan.zhihu.com/p/51372134

總結(jié)

以上是生活随笔為你收集整理的多机器人路径规划的代码_知荐 | 地平线机器人算法工程师总结六大路径规划算法...的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。