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

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

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

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

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

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

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

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

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

?

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

圖搜索法

圖搜索法依靠已知的環(huán)境地圖以及地圖中的障礙物信息構(gòu)造從起點(diǎn)到終點(diǎn)的可行路徑。主要分成深度優(yōu)先和廣度優(yōu)先兩個(gè)方向。深度優(yōu)先算法優(yōu)先擴(kuò)展搜索深度大的節(jié)點(diǎn)，可以快速的得到一條可行路徑，但是深度優(yōu)先算法得到的第一條路徑往往是較長(zhǎng)的路徑。廣度優(yōu)先算法優(yōu)先擴(kuò)展深度小的節(jié)點(diǎn)，呈波狀的搜索方式。廣度優(yōu)先算法搜索到的第一條路徑就是最短路徑。 可視圖法 可視圖法由Lozano-Perez和Wesley于1979年提出，是機(jī)器人全局運(yùn)動(dòng)規(guī)劃的經(jīng)典算法?？梢晥D法中，機(jī)器人用點(diǎn)來(lái)描述，障礙物用多邊形描述。將起始點(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)造可見(jiàn)圖??。其中點(diǎn)集??，??為所有弧段即可見(jiàn)邊的集合。然后釆用某種優(yōu)化算法搜索從起始點(diǎn)??到目標(biāo)點(diǎn)??的最優(yōu)路徑，那么根據(jù)累加和比較這些直線的距離就可以獲得從起始點(diǎn)到目標(biāo)點(diǎn)的最短路徑。

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

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

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

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

Dijkstra算法

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

A*算法

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

RRT算法

快速搜索隨機(jī)樹(shù)（RRT）算法是一種增量式采樣的搜索方法，該方法在應(yīng)用中不需要任何參數(shù)整定，具備良好的使用性能。它利用增量式方法構(gòu)建搜索樹(shù)，以逐漸提高分辨能力，而無(wú)須設(shè)置任何分辨率參數(shù)。在極限情況，該搜索樹(shù)將稠密的布滿整個(gè)空間，此時(shí)搜索樹(shù)由很多較短曲線或路經(jīng)構(gòu)成，以實(shí)現(xiàn)充滿整個(gè)空間的目的。增量式方法構(gòu)建的搜索樹(shù)其導(dǎo)向取決于稠密采樣序列，當(dāng)該序列為隨機(jī)序列時(shí)，該搜索樹(shù)稱為快速搜索隨機(jī)樹(shù)（Rapidly Exploring Random Tree，RRT），而不論該序列為隨機(jī)還是確定性序列，都被稱為快速搜索稠密樹(shù)（Rapidly Exploring Dense Trees，RDTs），這種規(guī)劃方法可處理微分等多種約束。 算法步驟 考慮二維和三維工作空間，環(huán)境中包含靜態(tài)障礙物。初始化快速隨機(jī)搜索樹(shù)T，只包括根節(jié)點(diǎn)，即初始狀態(tài)S。在自由空間中隨機(jī)選取一個(gè)狀態(tài)點(diǎn)??，遍歷當(dāng)前的快速隨機(jī)搜索樹(shù)T，找到T上距離??最近的節(jié)點(diǎn)??，考慮機(jī)器人的動(dòng)力學(xué)約束從控制輸入集??中選擇輸入??，從狀態(tài)??開(kāi)始作用，經(jīng)過(guò)一個(gè)控制周期??到達(dá)新的狀態(tài)??。滿足??與??的控制輸入??為最佳控制量。將新?tīng)顟B(tài)??添加到快速隨機(jī)搜索樹(shù)T中。按照這樣得到方法不斷產(chǎn)生新?tīng)顟B(tài)，直到到達(dá)目標(biāo)狀態(tài)G。完成搜索樹(shù)構(gòu)建后，從目標(biāo)點(diǎn)開(kāi)始，逐次找到父節(jié)點(diǎn)直到到達(dá)初始狀態(tài)，即搜索樹(shù)的根節(jié)點(diǎn)。

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

改進(jìn)算法

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

滾動(dòng)在線RRT算法

基本RRT算法傾向于遍歷整個(gè)自由空間直到獲得可行路徑，這使其不可能用于未知或動(dòng)態(tài)環(huán)境中的機(jī)器人在線運(yùn)動(dòng)規(guī)劃。利用滾動(dòng)規(guī)劃的思想可以將RRT算法進(jìn)行改進(jìn)，使其具備在線規(guī)劃能力。 滾動(dòng)規(guī)劃 機(jī)器人在未知或動(dòng)態(tài)環(huán)境中運(yùn)動(dòng)時(shí)，只能探知其傳感器范圍內(nèi)有限區(qū)域內(nèi)的環(huán)境信息。機(jī)器人利用局部信息進(jìn)行局部運(yùn)動(dòng)規(guī)劃，并根據(jù)一定的評(píng)價(jià)準(zhǔn)則得到局部目標(biāo)。機(jī)器人到達(dá)局部目標(biāo)后再次進(jìn)行新的局部規(guī)劃。如此反復(fù)進(jìn)行直到到達(dá)全局目標(biāo)。 滾動(dòng)規(guī)劃算法的基本原理：

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

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

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

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

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

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

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

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

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

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

步驟6：返回步驟1。

滾動(dòng)在線RRT算法流程

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

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

對(duì)滾動(dòng)窗口隨機(jī)樹(shù)T初始化，T開(kāi)始只包含初始位置S；

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

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

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

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

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

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

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

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

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

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

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

空間中的合勢(shì)場(chǎng)是引力勢(shì)場(chǎng)與斥力勢(shì)場(chǎng)之和：? 其中，??是目標(biāo)產(chǎn)生的引力勢(shì)場(chǎng)；??是各個(gè)障礙物產(chǎn)生的斥力勢(shì)場(chǎng)之和，即：?。 這里構(gòu)造如下的引力勢(shì)函數(shù)和斥力勢(shì)函數(shù)：

?

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

?

人工勢(shì)場(chǎng)法算法改進(jìn)

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

?

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

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

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

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

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

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

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

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

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

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

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

?圖12 BUG1算法偽代碼

BUG2算法

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

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

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

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

TangentBUG算法

TangentBUG算法是對(duì)BUG2算法的提高。它利用機(jī)器人上距離傳感器的讀數(shù)對(duì)障礙物做出提前規(guī)避，可以獲得更短更平滑的機(jī)器人路徑。在一個(gè)靜態(tài)環(huán)境中，傳感器讀數(shù)??是機(jī)器人位置??和傳感器掃描角度??的函數(shù)，具體點(diǎn)說(shuō)，??是沿著??的射線以角度??到達(dá)最近障礙物的距離， 其中??。 對(duì)于某一個(gè)固定的位置??，??被傳感器視野內(nèi)的障礙物分割成多個(gè)連續(xù)區(qū)間。如下圖所示。??出現(xiàn)不連續(xù)或者到達(dá)傳感器最大測(cè)量范圍的角度就是這些連續(xù)區(qū)間的端點(diǎn)。TangentBUG算法利用者些區(qū)間的端點(diǎn)避開(kāi)工作空間中的障礙物。

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

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

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

圖19

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

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

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

增量式啟發(fā)算法

LPA*算法

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

D* Lite算法

D* Lite算法是以LPA*為基礎(chǔ)，是Maxim Likhachev和Sven Koenig于2002年基于LPA*，結(jié)合A*算法思想，提出一種增量啟發(fā)式算法，適用于在未知環(huán)境中的導(dǎo)航以及路徑規(guī)劃，廣泛用于目前各種移動(dòng)機(jī)器人和自主車輛載具，例如“機(jī)遇號(hào)”和“勇氣號(hào)”火星車測(cè)試的原型導(dǎo)航系統(tǒng)。 本章研究了幾種常用的運(yùn)動(dòng)規(guī)劃算法。其中，人工勢(shì)場(chǎng)法應(yīng)用靈活，可以在保證安全的情況下獲得一條平滑路徑，并且對(duì)于動(dòng)態(tài)環(huán)境可以實(shí)現(xiàn)實(shí)時(shí)運(yùn)動(dòng)控制。適合用于長(zhǎng)距離機(jī)動(dòng)且障礙物較少的情況。而基于隨機(jī)采樣的搜索樹(shù)方法可以在復(fù)雜約束環(huán)境中獲得可行解，適合用于機(jī)械臂近距離操作。

審核編輯：黃飛

?