機(jī)器人運(yùn)動(dòng)規(guī)劃方法綜述（2）
加入技術(shù)交流群
掃碼加入
和技術(shù)大咖面對(duì)面交流
海量資料庫查詢
步驟1初始化，無向拓?fù)鋱D的節(jié)點(diǎn)集
初始時(shí)一般包含起始構(gòu)型
和 目標(biāo)構(gòu)型
。步驟2節(jié)點(diǎn)選擇方法（Node Selection Method，NSM），選擇一個(gè)節(jié)點(diǎn)
進(jìn)行擴(kuò)展。步驟3局部規(guī)劃方法（Local Planning Method，LPM），選擇新的構(gòu)型點(diǎn)
，試圖構(gòu)造路徑
，使得
且
。用碰撞檢測(cè)算法確保
，否則返回步驟2。步驟4在圖中插入一條邊，將
作為連接
和
的邊插入
中。若
不在
中，也將其插入。步驟5對(duì)解進(jìn)行檢驗(yàn)，確定
是否已經(jīng)編碼了一條解路徑。步驟6返回步驟2。步驟2中的NSM類似于圖搜索算法中的優(yōu)先隊(duì)列（Priority Queue），而步驟3中的 LPM之所以稱為局部的，是因?yàn)槠洳⒉粐L試解決整個(gè)規(guī)劃問題，而是每次僅產(chǎn)生較短且簡(jiǎn)單的無碰路徑段。對(duì)于非完整系統(tǒng)，LPM也可稱為Steering方法?，F(xiàn)有算法大多都是步驟2和步驟3有所不同，其中最著名的應(yīng)是擴(kuò)張空間樹（Expansive Spaces Tree，EST）和快速擴(kuò)展隨機(jī)樹（Rap?idly Exploring Random Tree，RRT）。前者將待擴(kuò)展節(jié)點(diǎn)被選擇的概率設(shè)置為關(guān)于樹上節(jié)點(diǎn)分布密度的反比例函數(shù)，并在該節(jié)點(diǎn)周圍一定范圍內(nèi)隨機(jī)選擇新的構(gòu)型點(diǎn)進(jìn)行擴(kuò)展，從而保障了采樣樹的稠密生長(zhǎng)。后者則通過定義一個(gè)無窮、稠密采樣序列，并利用 NEAREST函數(shù)為每個(gè)采樣點(diǎn)選擇其在
中的最近點(diǎn)來迭代生長(zhǎng)（參見圖4）。
圖4 RRT算法流程正是因?yàn)槊總€(gè)節(jié)點(diǎn)被選擇的概率與其對(duì)應(yīng)Voronoi區(qū)域的測(cè)度成正比，才保證了算法的快速擴(kuò)展特性。兩種方法的概率完備性也已在原文中得到證明。值得一提的是，PRM算法的變體也可用于單疑問運(yùn)動(dòng)規(guī)劃問題：其先在忽略障礙物的情況下構(gòu)建概率路圖，待找到可行路徑時(shí)僅對(duì)該路徑進(jìn)行碰撞檢測(cè)。若某條邊或節(jié)點(diǎn)發(fā)生碰撞，則將其移除并重新增強(qiáng)路圖進(jìn)行路徑搜索和碰撞檢測(cè)。Lazy PRM的思想來源是碰撞檢測(cè)耗費(fèi)了算法90%以上的時(shí)間，且對(duì)單疑問運(yùn)動(dòng)規(guī)劃問題來說大部分邊的碰撞檢測(cè)是無用的。在大多數(shù)應(yīng)用中，解路徑的品質(zhì)亦十分重要，一般除可行性外還存在最優(yōu)性要求，如最短長(zhǎng)度路徑、最短時(shí)間路徑等。但可以證明，標(biāo)準(zhǔn)的PRM算法和RRT算法均不具 備漸進(jìn)最優(yōu)性。經(jīng)典方法是利用啟發(fā)式圖搜索算法提供最優(yōu)性保證，不過這種最優(yōu)性受限于網(wǎng)格分辨率，且最壞情況下的運(yùn)行時(shí)間隨空間維數(shù)呈指數(shù)增長(zhǎng)。Karaman和Frazzoli通過修改鄰近點(diǎn)選取方法和局部節(jié)點(diǎn)連接方式（在文章中分別對(duì)應(yīng)Near Vertices和Rewire），提出了概率完備的漸進(jìn)最優(yōu)算法—PRM*、快速擴(kuò)展隨機(jī)圖（Rapidlyexploring Random Graph，RRG）和RRT*（算法流程參見圖5和圖6），且后兩種算法具有Anytime特性。其雖不能完全克服經(jīng)典方法的缺點(diǎn)，但的確在當(dāng)時(shí)提供了一個(gè)保證算法漸進(jìn)最優(yōu)性的新視角。以RRT*為例，算法在Near verti?ces步驟中用變化球半徑的方式選擇
的鄰近采樣點(diǎn)，其中半徑為
圖5 PRM*算法流程式（3）是采樣離散度的函數(shù)，并隨采樣點(diǎn)數(shù)量
的增加而減小，
為構(gòu)型空間維數(shù)，
是與環(huán)境相關(guān)的參數(shù)。在Rewire步驟中，首先將qnew連接至能為其提供更低代價(jià)的鄰近節(jié)點(diǎn)上，其次若
能為剩余某些鄰近節(jié)點(diǎn)提供更低的代價(jià)，則將該鄰近節(jié)點(diǎn)的父節(jié)點(diǎn)設(shè)置為
。一些關(guān)于PRM*和RRT*理論分析的改進(jìn)近來也已被提出。上面的論述詳細(xì)探究了SBMP的思想內(nèi)涵，列舉了幾種經(jīng)典的SBMP 算法以及它們的優(yōu)缺點(diǎn)和適用場(chǎng)景。至于該算法框架中最近點(diǎn)函數(shù)、距離度量、局部規(guī)劃器、碰撞檢測(cè)模塊的選擇，在此不過多贅述。另外在經(jīng)典SBMP算法的基礎(chǔ)上，目前也已產(chǎn)生了許多算法加速策略，本節(jié)余下部分將圍繞這一主題展開。
圖6 RRT*算法流程
1.2.1 利用確定性采樣方式提升算法性能
SBMP算法最初都使用了隨機(jī)采樣方式，那么一個(gè)問題是：如果以確定性方式進(jìn)行采樣，相關(guān)的理論保證和實(shí)際性能還會(huì)成立嗎？答案是肯定的，確定性規(guī)劃器可以簡(jiǎn)化證明過程，可以將一部分在線計(jì)算量變?yōu)殡x線計(jì)算（這對(duì)考慮微分約束的規(guī) 劃和高維空間中的規(guī)劃尤其有意義）。另外對(duì)于柵格序列，亦可簡(jiǎn)化操作量（如定位相鄰點(diǎn)）。以PRM為例，實(shí)質(zhì)上“概率性”對(duì)其是不重要的，反而會(huì)導(dǎo)致采樣點(diǎn)的不規(guī)則分布，使連通性信息的捕捉變得更加復(fù)雜。Lavalle等將局部規(guī)劃器引入經(jīng)典網(wǎng)格搜索，發(fā)展了子采樣網(wǎng)格搜索（Subsampled Grid Search，SGS）算法，并將確定性的低差異度采樣技術(shù)引入PRM，發(fā)展了 擬隨機(jī)路圖算法（Quasi-Random Roadmap）和柵格路圖（Lattice Roadmap）算法，進(jìn)行對(duì)比實(shí)驗(yàn)后發(fā)現(xiàn)：相較于分辨率完備的確定性采樣方法（包括網(wǎng)格搜索），原始的PRM算法并沒有體現(xiàn)出優(yōu)勢(shì)。故文章對(duì)“隨機(jī)性是打破高維空間維數(shù)詛咒的必要分量”這一說法進(jìn)行了駁斥，事實(shí)上為了維持固定的離散度，任何采樣 方案都需要與維數(shù)成指數(shù)關(guān)系的采樣點(diǎn)數(shù)量。但Lavalle等的工作僅限于討論可行路徑，就收斂到最優(yōu)路徑而言，獨(dú)立同分布采樣是否還有優(yōu)勢(shì)？Jason等針對(duì) PRM算法進(jìn)行了討論（設(shè)為從自然數(shù)集到實(shí)數(shù)集的映射，約定
表示存在某個(gè)自然數(shù)
和正實(shí)數(shù)
，使得對(duì)于所有
，
；
表示存在某個(gè)自然數(shù)
和正實(shí)數(shù)
，使得對(duì)于所有
，
；
表示
:1）確定性漸進(jìn)最優(yōu)性，在
維空間中，使用
離散度的上界為
，連接半徑
的確定性采樣序列的PRM算法是確定性漸進(jìn)最優(yōu)的。而使用獨(dú)立同分布隨機(jī)采樣序列的PRM*算法是概率性漸進(jìn)最優(yōu)的，且需要更大的連接半徑
。2）收斂率，在沒有障礙物的情況下，采用確定性采樣序列的PRM的次優(yōu)因子上界為
，其中
是采樣序列的
離散度（存在障礙物時(shí)的結(jié)果更復(fù)雜一點(diǎn)）。而采用獨(dú)立同分布采樣的類似結(jié)果僅是概率成立，且更加繁瑣和難以理解。3）計(jì)算復(fù)雜度和空間復(fù)雜度，在低離散度柵格上運(yùn)行PRM的計(jì)算復(fù)雜度和空間 復(fù)雜度為
。由于可以用
來獲得漸近最優(yōu)性，故PRM存在一個(gè)計(jì)算復(fù)雜度和空間復(fù)雜度為
的漸近最優(yōu)版本。而使用獨(dú)立同分布采樣的復(fù)雜度結(jié)果為
量級(jí)。可以看出確定性方法在需要更小的連接半徑的同時(shí)，具有更好的計(jì)算復(fù)雜度和時(shí)間復(fù)雜度。本質(zhì)原因在于確定性低離散度序列和獨(dú)立同分布序列間離散度的差異，二者分別為
和
。另外，從使用低離散度柵格的PRM中得到的結(jié)果在其他一些情況下也精確或近似地成立，如k-nearest-neighbor算法、批處理算法、非柵格的低離散度采樣序列（如Halton序列）、非均勻采樣和含微分約束的規(guī)劃等。多類實(shí)驗(yàn)結(jié)果表明：對(duì)于給定數(shù)量的采樣點(diǎn)，確定性低離散度采樣不會(huì)差于獨(dú)立同分布采樣。因而結(jié)合Lavalle的結(jié)論，Jason等建議基于SBMP算法都應(yīng)使用非獨(dú)立同分布的低離散度采樣序列。1.2.2 利用收集到的
形狀信息改善采樣分布事實(shí)上
中的可視特性并非均勻分布，且描述整個(gè)
可視特性的
取決于其中最差的構(gòu)型和lookouts。當(dāng)含有狹窄通道時(shí)，以上3個(gè)參數(shù)就會(huì)減小。而為了保證算法的失敗概率不超過
，由式（2）可知所需的均勻采樣點(diǎn)數(shù)量隨即大幅增加。如果規(guī)劃器能根據(jù)已有的或運(yùn)行過程中收集到的信息對(duì)
的形狀做出概率上的推測(cè)，則可以此推測(cè)來偏置采樣點(diǎn)分布，使其能更好地捕捉C-space的連通特性，從而減少所需的采樣點(diǎn)數(shù)量，提高算法效率。但顯式維持該概率模型的代價(jià)很高，因此早期研究?jī)H是用啟發(fā)式來近似最優(yōu)采樣策略，其本質(zhì)上是一種離線方法。包括基于工作空間的策略、基于障礙物的策略、基于可視性的策略、Bridgetest采樣策略等。但由于基于可視性的策略采用的是拒絕采樣方式，故實(shí)際使用中的效果不太理想。自適應(yīng)采樣則在線調(diào)整采樣點(diǎn)的分布。Hsu等將以上離線偏置采樣方法線性加權(quán)，并在每次采樣后調(diào)整權(quán)重，稱為混合PRM采樣策略。由于“邊界點(diǎn)”一般有更大的Voronoi偏置卻不能被成功擴(kuò)展，Yershova等針對(duì)含有復(fù)雜幾何的運(yùn)動(dòng)規(guī)劃問題，提出了將“邊界點(diǎn)”采樣域限制在以預(yù)設(shè)值為半徑的球內(nèi)的Dynamic Domain RRT（DD-RRT）方法。Jaillet等則根據(jù)“邊界點(diǎn)”成功擴(kuò)展的概率來調(diào)整邊界域半徑，實(shí)驗(yàn)結(jié)果表明Adaptive Dynamic-Domain RRT（ADD-RRT）在大多數(shù)實(shí)驗(yàn)場(chǎng)景下都比原始RRT的性能高出數(shù)個(gè)量級(jí)。對(duì)于多疑問運(yùn)動(dòng)規(guī)劃問題，Burns和Brock建立了表示
形狀的混合高斯模型和k-nearest-neighbor模型］，并用采樣信息更新該模型。前者最大程度地減小模型方差，后者則用期望效用來引導(dǎo)采樣。相應(yīng)結(jié)果同樣被擴(kuò)展至單疑問運(yùn)動(dòng)規(guī)劃問題，在提升計(jì)算效率的同時(shí)也增強(qiáng)了規(guī)劃器的魯棒性。1.2.3 利用解路徑代價(jià)和尚需代價(jià)的估值導(dǎo)引采樣分布隨著采樣點(diǎn)數(shù)量趨于無窮，雖然RRT*可以保證漸進(jìn)收斂到最優(yōu)解，但這種按照隨機(jī)次序進(jìn)行搜索的方式在無用狀態(tài)上浪費(fèi)了大量計(jì)算時(shí)間，降低了算法效率，使其難以應(yīng)用到一些實(shí)際問題中，如****空間（即室外環(huán)境中的規(guī)劃）和高維空間（即機(jī)械臂的規(guī)劃）。因而啟發(fā)式被用來或縮小搜索區(qū)域，或安排搜索次序。heuristically-guided RRT（hRRT）算法以與啟發(fā)代價(jià)成反比的概率選擇采樣點(diǎn)，使采樣樹朝著低代價(jià)區(qū)域生長(zhǎng)，從而得到質(zhì)量更好的次優(yōu)路徑。Anytime RRT迭代地用前次的解來界定本次的搜索范圍，用與hRRT類似的思路選擇待擴(kuò)展節(jié)點(diǎn)，使解路徑的代價(jià)隨運(yùn)行次數(shù)不斷下降，但并未提供最優(yōu)性保證，且前次采樣點(diǎn)被不斷丟棄 ，信息復(fù)用率不高。Transition-based RRT將構(gòu)型空間代價(jià)地圖與規(guī)劃問題作為輸入，用隨機(jī)優(yōu)化方法決定是否保留新的采樣點(diǎn)，以使RRT朝著構(gòu)型空間代價(jià)地圖的谷地和鞍點(diǎn)進(jìn)行擴(kuò)展。Karaman等通過“圖修剪”技術(shù)，周期性地移除那些當(dāng)前代價(jià)與啟發(fā)式代價(jià)之和大于已有最路徑代價(jià)的頂點(diǎn)，發(fā)展了RRT*的Anytime版本。Hauser則在“圖修剪”技術(shù)的基礎(chǔ)上結(jié)合Lazy檢測(cè)，提出了Lazy-PRM*算法和Lazy-RRG*算法。Akgun和Stilman、Islam等均采用了路徑偏置方法，即通過增加當(dāng)前解路徑節(jié)點(diǎn)鄰域的采樣頻率來加速RRT*的收斂，但該方法基于不切實(shí)際的同倫假設(shè)且需持續(xù)全局采樣以規(guī)避局部極小值。除此之外，前者還用到了雙向搜索和采樣點(diǎn)拒絕（Sample-Rejection）技術(shù)，雖然一次采樣迭代消耗的時(shí)間極短，但隨著關(guān)注區(qū)域與
間體積比率的減小，找到一個(gè)可接受的采樣點(diǎn)所需的迭代次數(shù)也會(huì)增加，此時(shí)的計(jì)算量便再不容忽視。RRT#利用啟發(fā)式在由RRG遞增建立的圖上尋找并更新樹，并通過LPA*將變化傳播到整個(gè)圖中，從而有效維持了到每個(gè)頂點(diǎn)的最優(yōu)連接。雖然用啟發(fā)式縮小搜索區(qū)域的方法帶來了一定的性能提升，但其仍采用了與RRT*類似的擴(kuò)展次序，使得重要的、難以采樣的狀態(tài)由于目前不能連接到樹而被簡(jiǎn)單丟棄，同時(shí)還可能浪費(fèi)計(jì)算 時(shí)間。Informed RRT*首先運(yùn)行原始的RRT*算法，待找到解后再直接在不斷縮小的橢球形信息集內(nèi)進(jìn)行采樣（參見圖7）。相比于RRT#，Informed RRT*在橢球體積減少時(shí)，仍能有效提高收斂率和解路徑質(zhì)量。
圖7 Informed RRT*算法流程至此所述方法雖然均在最優(yōu)解的收斂速度上有所改進(jìn)，但本質(zhì)上其生長(zhǎng)樹的方式都是無序的。Jason等用一種Marching方法，按Costto-Come遞增的次序（類似于 Dijkstra算法）搜索一批采樣點(diǎn)。其中空間近似和搜索的分離使得搜索次序獨(dú)立于采樣次序，但卻犧牲了Any?time特性。在搜索完成之前，F(xiàn)MT*不會(huì)返回解路徑。另外也與其它先驗(yàn)離散方法一樣，如果解不存在，則搜索必須重新開始。Gammell等試圖將有信息的圖搜索算法和具有Anytime特性的RRT*算法統(tǒng)一至同一框架下，從而發(fā)展了一種可以有效解決連續(xù)空間路徑規(guī)劃問題的有信息、有 Anytime特性、有漸進(jìn)最優(yōu)保證且避免大量計(jì)算無用狀態(tài)的基于采樣的運(yùn)動(dòng)規(guī)劃算法—Batch Informed Trees（BIT*），該方法的主要貢獻(xiàn)在于維持潛在解路徑質(zhì)量的優(yōu)先級(jí)序列的同時(shí)利用分批采樣技術(shù)，使空間近似和空間搜索得以交替進(jìn)行。一些BIT*的改進(jìn)算法也已被提出：Advanced BIT*（ABIT*）使用類似于ARA*的次優(yōu)啟發(fā)式因子快速找到初始解路徑，再以Anytime形式向最優(yōu)解路徑收斂。Adaptively Informed Trees （AIT*）通過對(duì)稱雙向搜索來同時(shí)估計(jì)并使用一個(gè)精確的、針對(duì)特定問題的啟發(fā)式，可較好地適用于局部路徑評(píng)估較昂貴的情況。Regionally Accelerated BIT*（RABIT*）利用局部?jī)?yōu)化器來解決不能通過直線連接的狀態(tài)之間的連接問題，有助于在難以采樣的同倫類中找到路徑。除上述算法之外，為平衡最優(yōu)性與計(jì)算效率間的矛盾，一些文章也通過放松關(guān)于最優(yōu)性的要求，對(duì)漸近近優(yōu)（Asymptotically Near Optimal）算法進(jìn)行了討論。
*博客內(nèi)容為網(wǎng)友個(gè)人發(fā)布，僅代表博主個(gè)人觀點(diǎn)，如有侵權(quán)請(qǐng)聯(lián)系工作人員刪除。
