機器人運動規(guī)劃方法綜述（2）

針對僅有一個起始-目標(biāo)構(gòu)型對的單疑問（Single-Query）運動規(guī)劃問題，僅需覆蓋與構(gòu)型對相關(guān)的部分即可，預(yù)計算不能帶來任何好處。大多數(shù)基于采樣的單疑問規(guī)劃算法都選擇以逐步構(gòu)建稠密搜索樹的方式來探索C-space，而不用提前設(shè)定采樣點數(shù)量。只要算法構(gòu)建的路徑集足夠豐富，那么將找到一個解，該特點使其適合于在線執(zhí)行。基于采樣的單疑問運動規(guī)劃算法可被統(tǒng)一至遞增采樣與搜索（Incremental Sampling and Searching）的框架下：

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