1.自主機(jī)器人近距離作業(yè)運(yùn)動(dòng)規(guī)劃系統(tǒng)

在研究自主運(yùn)動(dòng)規(guī)劃問題之前，首先需要建立一個(gè)比較完善的自主運(yùn)動(dòng)規(guī)劃系統(tǒng)，然后以該系統(tǒng)為指導(dǎo)，對自主運(yùn)動(dòng)規(guī)劃的各種具體問題進(jìn)行深入研究。本節(jié)將根據(jù)自主機(jī)器人的思維方式、運(yùn)動(dòng)形式、任務(wù)行為等特點(diǎn)，建立相應(yīng)的自主運(yùn)動(dòng)規(guī)劃系統(tǒng)。根據(jù)機(jī)器人的數(shù)量和規(guī)模，自主運(yùn)動(dòng)規(guī)劃分為兩種規(guī)劃系統(tǒng)：單機(jī)器人運(yùn)動(dòng)規(guī)劃和多機(jī)器人協(xié)同運(yùn)動(dòng)規(guī)劃。

1.1 單個(gè)自主機(jī)器人的規(guī)劃系統(tǒng)

運(yùn)動(dòng)規(guī)劃系統(tǒng)是自主控制系統(tǒng)中主控單元的核心部分，因此有必要首先研究自主控制系統(tǒng)及其主控單元的架構(gòu)。

自自主控制技術(shù)研究以來，各種架構(gòu)形式相繼出現(xiàn)。目前，分布式架構(gòu)在實(shí)踐中應(yīng)用廣泛，各個(gè)功能模塊作為一個(gè)相對獨(dú)立的單元參與到整個(gè)系統(tǒng)中。隨著人工智能技術(shù)的不斷發(fā)展，基于多智能體的分布式架構(gòu)逐漸成為主流。每個(gè)功能模塊作為一個(gè)獨(dú)立的代理參與整個(gè)自主控制過程。該架構(gòu)的應(yīng)用基本形式如圖1所示。一方面，主控單元及測控干預(yù)處理、姿態(tài)控制系統(tǒng)、軌道控制系統(tǒng)、熱控系統(tǒng)、能量系統(tǒng)、數(shù)據(jù)傳輸、載荷控制等功能子系統(tǒng)作為智能體相互獨(dú)立，通過總線連接；另一方面，主控單元對整個(gè)系統(tǒng)進(jìn)行整體規(guī)劃，協(xié)調(diào)和管理各個(gè)子系統(tǒng)Agent的行為。TT&C干預(yù)處理Agent保證了地面系統(tǒng)對整個(gè)系統(tǒng)任何層面的控制和干預(yù)能力，可以接受上游任務(wù)級任務(wù)、具體飛行計(jì)劃和底層控制指令；每個(gè)子系統(tǒng)Agent存儲子系統(tǒng)的各種知識和控制算法，自主完成主控單元下發(fā)的任務(wù)規(guī)劃，并將執(zhí)行和自身健康等信息回傳給主控單元，

圖1 基于多智能體的分布式自主控制系統(tǒng)架構(gòu)基本形式示意圖

主控單元Agent采用主流的層次結(jié)構(gòu)，層次分明，非常有利于實(shí)現(xiàn)。其基本結(jié)構(gòu)如圖2所示。主控制單元由三個(gè)基本結(jié)構(gòu)組成：任務(wù)生成與調(diào)度、運(yùn)動(dòng)行為規(guī)劃和控制命令生成。基本飛行任務(wù)從任務(wù)生成和調(diào)度層得到，具體的行為規(guī)劃通過運(yùn)動(dòng)行為規(guī)劃層得到，進(jìn)而產(chǎn)生控制命令。該層獲得最終的模塊控制命令并將其發(fā)送給其他功能代理。每個(gè)功能代理向主控單元的狀態(tài)檢測系統(tǒng)發(fā)送狀態(tài)信息，狀態(tài)檢測系統(tǒng)將任務(wù)執(zhí)行情況和子系統(tǒng)狀態(tài)反饋給任務(wù)生成調(diào)度層，以根據(jù)具體情況對任務(wù)進(jìn)行規(guī)劃和調(diào)整。遇到緊急情況時(shí)，也可以開啟重規(guī)劃模塊，根據(jù)當(dāng)前情況快速反應(yīng)，快速生成行為計(jì)劃，指導(dǎo)控制指令生成層獲取應(yīng)急控制指令。此外，地面控制系統(tǒng)在三個(gè)層面都具有干預(yù)能力。圖2中虛線為主控單元的所有模塊，虛線為運(yùn)動(dòng)規(guī)劃系統(tǒng)，包括運(yùn)動(dòng)行為規(guī)劃模塊和再規(guī)劃模塊，也是運(yùn)動(dòng)的主要功能規(guī)劃系統(tǒng)。以便根據(jù)具體情況規(guī)劃和調(diào)整任務(wù)。遇到緊急情況時(shí)，也可以開啟重規(guī)劃模塊，根據(jù)當(dāng)前情況快速反應(yīng)，快速生成行為計(jì)劃，指導(dǎo)控制指令生成層獲取應(yīng)急控制指令。此外，地面控制系統(tǒng)在三個(gè)層面都具有干預(yù)能力。圖2中虛線為主控單元的所有模塊，虛線為運(yùn)動(dòng)規(guī)劃系統(tǒng)，包括運(yùn)動(dòng)行為規(guī)劃模塊和再規(guī)劃模塊，也是運(yùn)動(dòng)的主要功能規(guī)劃系統(tǒng)。以便根據(jù)具體情況規(guī)劃和調(diào)整任務(wù)。遇到緊急情況時(shí)，也可以開啟重規(guī)劃模塊，根據(jù)當(dāng)前情況快速反應(yīng)，快速生成行為計(jì)劃，指導(dǎo)控制指令生成層獲取應(yīng)急控制指令。此外，地面控制系統(tǒng)在三個(gè)層面都具有干預(yù)能力。圖2中虛線為主控單元的所有模塊，虛線為運(yùn)動(dòng)規(guī)劃系統(tǒng)，包括運(yùn)動(dòng)行為規(guī)劃模塊和再規(guī)劃模塊，也是運(yùn)動(dòng)的主要功能規(guī)劃系統(tǒng)。可以根據(jù)當(dāng)前情況快速反應(yīng)，快速生成行為計(jì)劃，指導(dǎo)控制指令生成層獲取應(yīng)急控制指令。此外，地面控制系統(tǒng)在三個(gè)層面都具有干預(yù)能力。圖2中虛線為主控單元的所有模塊，虛線為運(yùn)動(dòng)規(guī)劃系統(tǒng)，包括運(yùn)動(dòng)行為規(guī)劃模塊和再規(guī)劃模塊，也是運(yùn)動(dòng)的主要功能規(guī)劃系統(tǒng)。可以根據(jù)當(dāng)前情況快速反應(yīng)，快速生成行為計(jì)劃，指導(dǎo)控制指令生成層獲取應(yīng)急控制指令。此外，地面控制系統(tǒng)在三個(gè)層面都具有干預(yù)能力。圖2中虛線為主控單元的所有模塊，虛線為運(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)的架構(gòu)了。運(yùn)動(dòng)規(guī)劃系統(tǒng)架構(gòu)如圖3所示。系統(tǒng)由規(guī)劃器和再規(guī)劃器兩個(gè)執(zhí)行單元組成，分別承擔(dān)飛行任務(wù)的總體規(guī)劃和突發(fā)事件應(yīng)急處理的運(yùn)動(dòng)規(guī)劃。當(dāng)然，這兩部分也可以理解為線下規(guī)劃和線上規(guī)劃。線下規(guī)劃一般解決平時(shí)的預(yù)定飛行任務(wù)，線上規(guī)劃一般解決突發(fā)性飛行任務(wù)。除了策劃師，該系統(tǒng)還配備了知識領(lǐng)域模塊，以特定語言描述相關(guān)知識。知識域包括行為域和模型域兩部分。行為域用于存儲服務(wù)系統(tǒng)的一般運(yùn)動(dòng)行為描述以及緊急情況下（如急停、轉(zhuǎn)向等）某些運(yùn)動(dòng)行為的處理方法，模型域用于存儲所需的模型知識用于規(guī)劃，包括環(huán)境模型、裝配模型、裝配任務(wù)對象模型和任務(wù)模型等。

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

1.2 多自主機(jī)器人協(xié)同規(guī)劃系統(tǒng)

多智能體系統(tǒng)的群架構(gòu)一般分為集中式和分散式兩種基本結(jié)構(gòu)，分散式結(jié)構(gòu)又可以進(jìn)一步分為層次結(jié)構(gòu)和分布式結(jié)構(gòu)。在集中式結(jié)構(gòu)中，一個(gè)主控單元通常掌握環(huán)境和被控機(jī)器人的所有信息，利用規(guī)劃算法對任務(wù)進(jìn)行分解，分配給被控機(jī)器人，并組織它們完成任務(wù)。優(yōu)點(diǎn)是理論清晰，實(shí)現(xiàn)更直觀；缺點(diǎn)是容錯(cuò)性、靈活性和對環(huán)境的適應(yīng)性較差，每個(gè)受控機(jī)器人存在通信瓶頸問題。與集中式結(jié)構(gòu)相比，分散式結(jié)構(gòu)雖然無法獲得全局最優(yōu)解，但由于其可靠性、靈活性和環(huán)境適應(yīng)性強(qiáng)，越來越受到人們的青睞。分散式結(jié)構(gòu)中的分布式結(jié)構(gòu)沒有主控單元，每個(gè)代理的地位都是平等的。通過代理之間的溝通和信息交換，達(dá)到協(xié)商的目的，達(dá)到最終的決定。但這種結(jié)構(gòu)容易片面強(qiáng)調(diào)個(gè)人，造成資源占用。太多了，很難得到咨詢結(jié)果。層次結(jié)構(gòu)介于集中式和分布式之間。有主控單元，但不受主控單元控制。每個(gè)代理也有一定程度的自主權(quán)。

多自主機(jī)器人系統(tǒng)應(yīng)采用分層結(jié)構(gòu)，保證整個(gè)系統(tǒng)既適合統(tǒng)一領(lǐng)導(dǎo)，又能滿足系統(tǒng)靈活性和速度的需要。多自主機(jī)器人協(xié)同規(guī)劃架構(gòu)如圖4所示。根據(jù)層次結(jié)構(gòu)建立兩種工作模式。主控單元負(fù)責(zé)前期離線規(guī)劃。首先得到協(xié)同任務(wù)，通過得到具體的行為運(yùn)動(dòng)規(guī)劃。分布到各個(gè)子系統(tǒng)執(zhí)行單元，相關(guān)知識域主要用于描述各個(gè)子系統(tǒng)協(xié)商規(guī)則的協(xié)商域直線搜索方法,無約束優(yōu)化方法,約束優(yōu)化方法，主控單元從外界獲取環(huán)境信息，從各個(gè)子系統(tǒng)獲取狀態(tài)信息; 當(dāng)遇到緊急情況時(shí)，當(dāng)緊急任務(wù)發(fā)生變化或主控單元停止工作時(shí)，各子系統(tǒng)采用分布式結(jié)構(gòu)，獨(dú)立規(guī)劃自己的運(yùn)動(dòng)行為，從自己的知識域中獲取協(xié)商方法。外部環(huán)境信息由主控單元發(fā)送并結(jié)合自我感知。獲取（當(dāng)主控單元停止工作時(shí)，僅依靠自身感知獲取信息），其他機(jī)器人信息的傳遞通過機(jī)器人之間的數(shù)據(jù)鏈路實(shí)現(xiàn)。外部環(huán)境信息由主控單元發(fā)送并結(jié)合自我感知。獲取（當(dāng)主控單元停止工作時(shí)，僅依靠自身感知獲取信息），其他機(jī)器人信息的傳遞通過機(jī)器人之間的數(shù)據(jù)鏈路實(shí)現(xiàn)。外部環(huán)境信息由主控單元發(fā)送并結(jié)合自我感知。獲取（當(dāng)主控單元停止工作時(shí)，僅依靠自身感知獲取信息），其他機(jī)器人信息的傳遞通過機(jī)器人之間的數(shù)據(jù)鏈路實(shí)現(xiàn)。

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

2.路徑規(guī)劃研究

當(dāng)被賦予特定任務(wù)時(shí)，如何規(guī)劃機(jī)器人的運(yùn)動(dòng)將至關(guān)重要。機(jī)器人的規(guī)劃由兩部分組成：將底座移動(dòng)到適合操作的位置和轉(zhuǎn)動(dòng)手臂關(guān)節(jié)完成操作。它由三個(gè)問題組成：基礎(chǔ)點(diǎn)對點(diǎn)運(yùn)動(dòng)規(guī)劃；聯(lián)合空間規(guī)劃；綜合規(guī)劃。

本章研究了幾種常用的運(yùn)動(dòng)規(guī)劃算法：圖搜索法、RRT算法、人工勢場法、BUG算法。并對一些算法的缺陷進(jìn)行了一些改進(jìn)。

2.1 圖搜索方法

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

2.1.1 可視化查看方法

可見視圖法由-Perez和1979提出，是機(jī)器人全局運(yùn)動(dòng)規(guī)劃的經(jīng)典算法。在視覺方法中，機(jī)器人用點(diǎn)來描述，障礙物用多邊形來描述。起點(diǎn)

，目標(biāo)

和多邊形障礙物的頂點(diǎn)（設(shè)置

是所有障礙物的頂點(diǎn)的集合）進(jìn)行組合連接，要求起點(diǎn)與障礙物的頂點(diǎn)之間的連接，目標(biāo)點(diǎn)與障礙物的頂點(diǎn)之間的連接，以及障礙物的頂點(diǎn)與障礙物的頂點(diǎn)之間的連接。頂點(diǎn)不能穿過障礙物。物體，即直線是“可見的”。為圖中的邊分配權(quán)重以構(gòu)建可見圖

. 點(diǎn)集

,

是所有可見邊的弧段的集合。然后使用一些優(yōu)化算法從起點(diǎn)開始搜索

到目標(biāo)點(diǎn)

，那么通過對這些直線的距離進(jìn)行累加比較，就可以得到從起點(diǎn)到目標(biāo)點(diǎn)的最短路徑。

圖 5 可見視圖

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

在同一個(gè)障礙物內(nèi)，相鄰頂點(diǎn)可見（通常不考慮凹多邊形障礙物中不相鄰的頂點(diǎn)也可能可見），不相鄰的頂點(diǎn)不可見，權(quán)重賦值為

.

不同障礙物之間頂點(diǎn)可見性的判斷轉(zhuǎn)化為判斷頂點(diǎn)連接是否會與其他頂點(diǎn)連接相交的幾何問題。如下圖虛線所示，

,

障礙

,

頂點(diǎn)，但是

和

連接線與障礙物其他頂點(diǎn)的連接線相交，所以

,

之間不可見；而實(shí)線顯示

和

連接線不與障礙物其他頂點(diǎn)的連接線相交，所以

,

之間可見。

圖 6 頂點(diǎn)可見性判斷

視覺法可以找到最短路徑，但搜索時(shí)間長，缺乏靈活性，即一旦機(jī)器人的起點(diǎn)和目標(biāo)點(diǎn)發(fā)生變化，重建視覺就很麻煩。可見視圖法適用于多邊形障礙物，不適用于圓形障礙物。切線法和圖法改進(jìn)了可視圖法。切線圖法用障礙物的切線表示圓弧，因此是從起點(diǎn)到目標(biāo)點(diǎn)的最短路徑圖，移動(dòng)機(jī)器人必須幾乎接近障礙物行走。缺點(diǎn)是如果在控制過程中出現(xiàn)位置誤差，機(jī)器人與障礙物發(fā)生碰撞的可能性會很大。圖形方法表示具有盡可能遠(yuǎn)離障礙物和墻壁的路徑的弧。

2.1.2 算法

該算法由荷蘭計(jì)算機(jī)科學(xué)家 Wybe 發(fā)明，通過計(jì)算從初始點(diǎn)到自由空間任意點(diǎn)的最短距離可以得到全局最優(yōu)路徑。該算法從初始點(diǎn)開始計(jì)算4或8個(gè)周圍點(diǎn)與初始點(diǎn)的距離，然后以新計(jì)算的距離點(diǎn)為計(jì)算點(diǎn)，計(jì)算其周圍點(diǎn)與初始點(diǎn)的距離，使得計(jì)算像波前一樣在自由空間中進(jìn)行。傳播直到到達(dá)目標(biāo)點(diǎn)。這樣就可以計(jì)算出機(jī)器人的最短路徑。

該算法是經(jīng)典的廣度優(yōu)先狀態(tài)空間搜索算法，即算法會從初始點(diǎn)逐層搜索整個(gè)自由空間，直到到達(dá)目標(biāo)點(diǎn)。這大大增加了計(jì)算時(shí)間和數(shù)據(jù)量。并且大量的搜索結(jié)果對機(jī)器人運(yùn)動(dòng)毫無用處。

詳情請參考：路徑規(guī)劃-算法：

2.1.3 A*算法

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

詳情請參考：路徑規(guī)劃-A*算法：

2.2RRT算法

快速搜索隨機(jī)樹（RRT）算法是一種增量采樣搜索方法，在應(yīng)用中不需要任何參數(shù)調(diào)優(yōu)，性能良好。它在不設(shè)置任何分辨率參數(shù)的情況下，逐步構(gòu)建搜索樹以逐漸提高分辨率。在極端情況下，搜索樹會密集地填充整個(gè)空間，搜索樹由許多較短的曲線或路徑組成，以達(dá)到填充整個(gè)空間的目的。增量法構(gòu)建的搜索樹的方向取決于密集的采樣序列。當(dāng)序列為隨機(jī)序列時(shí)，搜索樹稱為快速搜索隨機(jī)樹（Tree，RRT），無論序列是隨機(jī)的還是確定的。兩者都稱為快速搜索密集樹（RDT），

2.2.1 算法步驟

考慮環(huán)境中有靜態(tài)障礙物的 2D 和 3D 工作空間。初始化一棵快速隨機(jī)搜索樹T，只包括根節(jié)點(diǎn)，即初始狀態(tài)S。在自由空間中隨機(jī)選取一個(gè)狀態(tài)點(diǎn)

，遍歷當(dāng)前快速隨機(jī)搜索樹T，求T上的距離

最近的節(jié)點(diǎn)

，從控制輸入集中考慮機(jī)器人的動(dòng)態(tài)約束

選擇輸入

, 從狀態(tài)

在一個(gè)控制周期后開始運(yùn)行

達(dá)到一個(gè)新的狀態(tài)

. 滿足

和

控制輸入

以獲得最佳控制。新狀態(tài)

加入到快速隨機(jī)搜索樹T中。按照這樣得到的方法，不斷產(chǎn)生新的狀態(tài)，直到達(dá)到目標(biāo)狀態(tài)G。搜索樹構(gòu)建完成后，從目標(biāo)點(diǎn)開始，逐個(gè)找到父節(jié)點(diǎn)，直到到達(dá)初始狀態(tài)，即搜索樹的根節(jié)點(diǎn)。

圖 7 隨機(jī)樹構(gòu)建過程

由于在搜索過程中考慮了機(jī)器人的動(dòng)態(tài)約束，因此生成路徑的可行性很好。然而，算法的隨機(jī)性只導(dǎo)致概率完整性。

2.2.2 改進(jìn)算法

等人的工作。為 RRT 方法奠定了基礎(chǔ)。在采樣策略方面，該方法控制機(jī)器人的隨機(jī)運(yùn)動(dòng)，以一定的概率移動(dòng)到最終目標(biāo)；該方法分別對整個(gè)空間和目標(biāo)點(diǎn)周圍的空間進(jìn)行采樣；該方法通過增加隨機(jī)步長來提高規(guī)劃速度。也采用了雙向規(guī)劃的思想，衍生出各種算法。

基本的 RRT 算法收斂到最終姿勢可能很慢。為了提高算法的效率和性能，需要不斷地改進(jìn)算法。例如，為了提高搜索效率，采用雙向隨機(jī)搜索樹（Bi~RRT），從起點(diǎn)和目標(biāo)點(diǎn)并行生成兩棵RRT，直到兩棵樹相遇，算法收斂。由于該算法比原來的RRT算法收斂性更好，在目前的路徑規(guī)劃中很常見。所提出的粒子RRT算??法考慮了地形的不確定性，保證了搜索樹在不確定環(huán)境中的擴(kuò)展。并且還提出了RRT-，大大提高了節(jié)點(diǎn)的擴(kuò)展效率。在運(yùn)動(dòng)規(guī)劃中，距離的定義非常復(fù)雜。研究了RRT生長過程中距離函數(shù)的持續(xù)學(xué)習(xí)算法，以降低距離函數(shù)對環(huán)境的敏感性。考慮到基本RRT規(guī)劃器得到的路徑長度一般為最優(yōu)路徑的1.3~1.5倍，英國的J.研究了變分法使其最優(yōu)。Amna A 引入 KD 樹作為二級數(shù)據(jù)結(jié)構(gòu)，以加快搜索最接近從環(huán)境中取出的隨機(jī)點(diǎn)的葉節(jié)點(diǎn)，降低搜索成本。該算法已廣泛應(yīng)用于動(dòng)態(tài)障礙物、高維狀態(tài)空間以及運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)等具有微分約束的環(huán)境中的運(yùn)動(dòng)規(guī)劃。

改進(jìn)RRT算法的詳細(xì)內(nèi)容請參考：路徑規(guī)劃——改進(jìn)RRT算法：

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

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

2.3.1 滾動(dòng)計(jì)劃

當(dāng)機(jī)器人在未知或動(dòng)態(tài)環(huán)境中移動(dòng)時(shí)，它只能在其傳感器范圍內(nèi)的有限區(qū)域內(nèi)檢測環(huán)境信息。機(jī)器人利用局部信息進(jìn)行局部運(yùn)動(dòng)規(guī)劃，根據(jù)一定的評價(jià)標(biāo)準(zhǔn)獲取局部目標(biāo)。機(jī)器人到達(dá)局部目標(biāo)后，再次進(jìn)行新的局部規(guī)劃。重復(fù)此過程，直到達(dá)到全球目標(biāo)。

滾動(dòng)規(guī)劃算法的基本原理：

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

局部滾動(dòng)優(yōu)化：將上述環(huán)境信息模型視為一個(gè)優(yōu)化窗口。在此基礎(chǔ)上，根據(jù)目標(biāo)點(diǎn)的位置和具體的優(yōu)化策略計(jì)算出下一個(gè)最優(yōu)子目標(biāo)，然后根據(jù)子目標(biāo)和環(huán)境信息模型，選擇局部規(guī)劃算法，確定局部路徑到子目標(biāo)，執(zhí)行當(dāng)前策略，即按照規(guī)劃的局部路徑走幾步，窗口會相應(yīng)地向前滾動(dòng)；

反饋信息修正：根據(jù)局部最優(yōu)路徑，驅(qū)動(dòng)機(jī)器人行走一定路徑后，機(jī)器人會檢測到新的未知信息。此時(shí)，可以根據(jù)機(jī)器人在行走過程中檢測到的新信息對原有的環(huán)境模型進(jìn)行補(bǔ)充或修正，進(jìn)行滾動(dòng)。下一步的本地規(guī)劃。

其中，局部子目標(biāo)是在滾動(dòng)窗口中找到一個(gè)全局目標(biāo)的映射，它必須避開障礙物并滿足一些優(yōu)化指標(biāo)。子目標(biāo)的選擇方法體現(xiàn)了全局優(yōu)化的要求和局部有限信息約束之間的折衷，是在給定信息環(huán)境下試圖實(shí)現(xiàn)全局優(yōu)化的自然選擇。

基于滾動(dòng)窗口的路徑規(guī)劃算法依靠實(shí)時(shí)檢測到的局部環(huán)境信息以滾動(dòng)方式進(jìn)行在線規(guī)劃。在滾動(dòng)的每一步，根據(jù)檢測到的局部信息，采用啟發(fā)式方法生成優(yōu)化子目標(biāo)，在當(dāng)前滾動(dòng)窗口內(nèi)進(jìn)行局部路徑規(guī)劃，然后執(zhí)行當(dāng)前策略（根據(jù)局部規(guī)劃路徑移動(dòng)一步） )，隨著滾動(dòng)窗口前進(jìn)，不斷獲取新的環(huán)境信息，從而在滾動(dòng)中實(shí)現(xiàn)優(yōu)化與反饋相結(jié)合。由于規(guī)劃問題被壓縮到滾動(dòng)窗口中，與全局規(guī)劃相比，計(jì)算量大大減少。

基于滾動(dòng)窗口的路徑規(guī)劃算法具體步驟如下：

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

在滾動(dòng)窗口內(nèi)，隨機(jī)樹從當(dāng)前位置開始，在傳感器范圍內(nèi)構(gòu)建隨機(jī)樹。構(gòu)造方法與基本的RRT算法一致。為了使全局環(huán)境中的隨機(jī)樹趨向于向目標(biāo)方向生長，在運(yùn)動(dòng)規(guī)劃中引入啟發(fā)式信息以降低隨機(jī)樹的隨機(jī)性，提高搜索效率。

制作

表示隨機(jī)樹中兩個(gè)姿勢節(jié)點(diǎn)之間的路徑成本，

表示隨機(jī)樹中兩個(gè)姿勢節(jié)點(diǎn)之間的歐幾里得距離。與 A* 算法類似，該算法為隨機(jī)樹中的每個(gè)節(jié)點(diǎn)定義了一個(gè)評估函數(shù)：

. 在

是一個(gè)隨機(jī)節(jié)點(diǎn)

到樹中的節(jié)點(diǎn)

所需的路徑成本。

對于啟發(fā)式評估函數(shù)，這里我們?nèi)‰S機(jī)節(jié)點(diǎn)

到目標(biāo)點(diǎn)

距離是估計(jì)值，

. 所以

表示從節(jié)點(diǎn)

通過隨機(jī)節(jié)點(diǎn)

到目標(biāo)節(jié)點(diǎn)

路徑估計(jì)。在滾動(dòng)窗口中遍歷隨機(jī)樹T，取評價(jià)函數(shù)最小值的節(jié)點(diǎn)

，有

. 這使得隨機(jī)樹沿著目標(biāo)節(jié)點(diǎn)的評估值

向最小方向擴(kuò)展。

由于在隨機(jī)樹生長中引入了面向目標(biāo)的啟發(fā)式評估因子，葉節(jié)點(diǎn)

總是選擇離目標(biāo)最近的節(jié)點(diǎn)，這會使隨機(jī)樹遇到局部最小值問題。所以隨機(jī)樹生長的新節(jié)點(diǎn)

需要克服這個(gè)問題，引導(dǎo)隨機(jī)樹更好地探索未知空間。

這里通過統(tǒng)計(jì)回歸分析生成新節(jié)點(diǎn)，進(jìn)一步增強(qiáng)RRT算法探索未知空間的能力，避免啟發(fā)式評價(jià)因素造成的局部極小值。這個(gè)想法是探索以前訪問過的空間是無用的，并且容易陷入局部最小值。引入回歸分析（ ）是為了檢查新節(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ù)的約束。其余兩個(gè)未被圈出的空心節(jié)點(diǎn)與其父節(jié)點(diǎn)的距離小于該節(jié)點(diǎn)與隨機(jī)樹上任意節(jié)點(diǎn)的距離。這兩個(gè)點(diǎn)可以成為隨機(jī)樹的一個(gè)新節(jié)點(diǎn)。

綜上所述，滾動(dòng)窗口中隨機(jī)樹構(gòu)建的具體步驟如下：

初始化滾動(dòng)窗口隨機(jī)樹T，T最初只包含初始位置S；

在滾動(dòng)窗口的空閑空間中隨機(jī)選擇一個(gè)狀態(tài)

;

根據(jù)最短路徑思想找到樹T中和

最近的節(jié)點(diǎn)

;

選擇輸入

，因此機(jī)器人狀態(tài)由下式給出

到達(dá)

;

當(dāng)然

是否符合回歸分析，如果不符合，返回步驟4；

將要

作為隨機(jī)樹 T 的一個(gè)新節(jié)點(diǎn)，

記錄在連接節(jié)點(diǎn)中

和

在邊緣。

在滾動(dòng)窗口狀態(tài)空間中采樣K次后，遍歷隨機(jī)樹，根據(jù)啟發(fā)式評估思想找到滾動(dòng)窗口子目標(biāo)

.

是當(dāng)前滾動(dòng)窗口中子樹中評價(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。

2.4 人工勢場法

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

本節(jié)為機(jī)器人平移的在線運(yùn)動(dòng)規(guī)劃構(gòu)建人工勢場直線搜索方法,無約束優(yōu)化方法,約束優(yōu)化方法，并通過沿墻行走的方法改進(jìn)了基本的人工勢場方法。

2.4.1 基本人工勢場法

作用在機(jī)器人上的假想引力和斥力是勢函數(shù)的負(fù)梯度，因此人工勢函數(shù)應(yīng)具有以下特點(diǎn)：

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

排斥勢越靠近障礙物越強(qiáng)；

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

空間中的合成勢場是引力勢場和排斥勢場的總和：

在，

是目標(biāo)產(chǎn)生的引力勢場；

是每個(gè)障礙物產(chǎn)生的排斥勢場之和，即：

.

這里構(gòu)造了以下引力和排斥勢函數(shù)：

在，

表示引力勢的相對影響；

意味著第一個(gè)

障礙物排斥勢的相對影響，