機器人運動規劃方法綜述

唐永興，朱戰霞，*，張紅文，羅建軍，袁建平

1.西北工業大學 航天學院，西安 710072

2.航天飛行動力學技術國家級重點實驗室，西安 710072

3.之江實驗室，杭州 311121

機器人學是研究如何通過計算機控制裝置感知并操縱客觀世界的學科，現今已被應用于各個領域，如行星探索中的移動平臺［1］、空間機械臂［2］、無人機［3］、自動駕駛汽車［4］等（圖1）。設想這樣的場景：一架無人機正在高樓聳立的城市中穿行，未知環境要求其應利用新接收到的信息不斷修正它的運動；雜亂的障礙物要求其應避免與眾多物體發生碰撞；傳感器誤差、陣風干擾、模型誤差、控制約束等則要求真實運動與規劃運動間的誤差應盡量小。因此為了能在充滿未知性和不確定性的真實世界中可靠地完成任務，機器人必須具有自主感知［5］、快速決策［6-7］、運動規劃［8-10］與控制［11］的能力。其中運動規劃作為上層決策與底層控制的連接模塊，負責將決策序列轉化為控制器可執行的參考路徑（軌跡），在機器人研究中占有重要地位。

圖1 典型的機器人系統Fig.1 Typical robotic systems

運動規劃領域早期側重于處理有障礙物的二維或三維世界中的開環運動規劃問題［8-10］，其結果可以是一條無碰路徑，也可以是一條無碰軌跡。前者是幾何意義上的連續（光滑）曲線，后者則是這一曲線的時間參數化表達。對機器人而言，路徑規劃（本文稱其為傳統運動規劃）一般在構型空間（Configuration Space，C-space）［12］內進行，軌跡規劃（本文稱其為考慮微分約束的開環運動規劃）則在狀態空間（即構型空間或相空間）內進行。但無論構型空間亦或相空間，都是無限不可數的，故開環運動規劃的一個中心主題是將連續空間離散化，進而借助人工智能領域的離散搜索思想［13］，將求解運動規劃問題視為在高維自由構型空間（Free Configuration Space，Cfree）或自由相空間中的搜索過程。除一般的可行性問題外，滿足某類性能指標最優（如時間最優、路徑長度最優等）的開環運動規劃問題亦是學者們的關注焦點。一條最優路徑往往可參數化為許多條軌跡，它們的速度時間歷程各不相同，而最優軌跡僅是其中一條。不幸的是，計算最優路徑（軌跡）的時間復雜度往往較高（甚至對某些問題來講，計算可行路徑就已十分困難），很難滿足機器人實時規劃的需求，這便促使研究人員不斷開發新的算法［14-39］以實現兩者間的平衡。另外，經典控制理論［11］的發展歷史表明：反饋是應對不確定性的有效手段。但開環運動規劃過程常常忽略了這一點，從而容易造成機器人沿規劃路徑（軌跡）運動時意外碰撞的發生（圖2）。反饋運動規劃則通過對不確定性進行建模，并將該模型融入規劃過程，為機器人的實際運動提供了安全保障。雖然近年關于反饋運動規劃的研究取得了一系列進展［40-48］，但鮮有文章［49-50］對這一主題進行歸納總結。即使有所提煉［51］，包括反饋運動規劃在內的各類內容［49-51］也已稍顯陳舊，而且將運動規劃的研究范圍局限于基于采樣的運動規劃（Sampling-based Motion Planning，SBMP）算法的做法［51］無疑限制了讀者的視野。

圖2 運動規劃與控制的解耦可能造成意外碰撞Fig.2 Decoupling of motion planning and control can cause accidental collisions

根據是否考慮微分約束和反饋，Lavalle 的經典專著［8］將運動規劃問題分為表1 中的4 項子課題，本文則不區分最右側2 項，并將其統稱為反饋運動規劃。進而在這種分類方式的基礎上以單機器人為研究對象，依次總結了現有的傳統運動規劃、考慮微分約束的開環運動規劃和反饋運動規劃相關算法，形成了類似于“譜”的運動規劃算法歸類，便于讀者對比分析各種方法間的區別與聯系。其中針對解路徑（軌跡）品質與求解效率間存在的矛盾，重點詳述了如何利用已有信息來加速漸近最（近）優算法。另外則對不確定性建模方式、動態環境中的規劃、學習算法與運動規劃算法的融合等先進課題的最新成果進行了總結，以期為后續研究提供思路。

表1 運動規劃問題分類［8］Table 1 Classification of motion planning problems［8］

1 傳統運動規劃

設G（V，E）為映射到Cfree的拓撲圖（可參考圖3～圖6），其中V為圖中節點的集合，每個節點相對應一個構型q。E為圖中邊的集合，每條邊相對應兩個構型間的無碰撞路徑。對于所有edge∈E，定義

式中：edge（［0 1］）?Cfree表示路徑edge 的像；S為可以通過G（V，E）到達的所有構型的集合。解決傳統運動規劃問題的思路正是用包含可數個節點的圖結構G（V，E）捕捉Cfree的連通信息，并基于此離散結構搜索解路徑。由于G（V，E）的構建過程顯然受障礙物形狀及其位置分布的影響，故一般可根據是否在C-space 中顯式表示障礙物區域（Obstacle Region，Cobs），將處理傳統運動規劃問題的算法分為組合運動規劃（Combinatorial Motion Planning，CMP）算法和基于采樣的運動規劃算法兩類。

1.1 組合運動規劃

基于顯式表示的障礙物，CMP 首先構造滿足下列條件［10］：

1）可達性（Accessibility）：對于任一q∈Cfree，可以簡單高效地計算一條以其為起點，以S中任一點s為終點的路徑τ：［0 1］→Cfree，即τ（0）=q，（τ1）=s。

2）確保連通性（Connectivity-Preserving）：對于起始構型qI、目標構型qG及它們在S中的連接點s1、s2，如果存在一條路徑τ：［0 1］→Cfree，使得（τ0）=qI，（τ1）=qG，那么也將存在一條路徑τ′：［0 1］→S，使得τ（′0）=s1，τ（′1）=s2的路圖，其或由Cfree的胞腔剖分間接生成，如垂直胞腔剖分［52］、圓柱代數剖分［53］等；或通過其他方法直接構造，如可視圖法［54］、廣義維羅尼圖法［55］、輪廓法［56］等。進而利用圖搜索算法 如Dijkstra 算 法［57］、A*算法［58］、ARA*算法［59］、D*Lite 算法［60］、AD*算法［61］及Theta*算法［62-63］等尋找解路徑。后四者都是A*的變體：ARA*通過放松對啟發式的一致性要求并重復使用先前的搜索信息，可快速產生因子可控的次優路徑，具有Anytime 特性，適用于計算時間受限的靜態環境；D*Lite 是一種遞增搜索算法，其先用A*算法從目標頂點反向計算相關節點的最優路徑代價，待環境發生改變后，再以此有效信息為基礎進一步調整相關節點的最優路徑代價，適用于含動態障礙物的場景；AD*則是結合了ARA*和D*Lite 的優點，既具有Anytime特性，又有遞增特性，真正實現了動態場景中的實時規劃。另外由于A*算法找出的解路徑角度往往被網格劃分方式所限制，因而其并非真實地形中的最優路徑，Theta*通過解除這一約束，可在相同時間復雜度下得到更高質量的解路徑。

條件1）其實保證了Cfree內的任一起始、目標構型對（qI，qG）均可被連接到G中，條件2）則保證了在解路徑存在的情況下，搜索總是可以成功。而這正是CMP 完備性的來源，即在有限時間內，算法要么找到一條解路徑，要么正確地報告無解。雖然其幾乎能解決任何運動規劃問題，而且在一些簡單情形（如二維世界中，Cobs為多邊形，機器人僅進行平移運動）下可以高效地得到較好解，但對于大多數具有高維C-space 且障礙物數量巨大的問題，較長的運行時間和執行困難使此類方法失去了吸引力。

1.2 基于采樣的運動規劃

與CMP 不同，SBMP 通過碰撞檢測模塊來避免顯式構建Cobs，并且利用可數的采樣點集或采樣序列及滿足一定條件的連接方式近似捕捉無限不可數的Cfree的連通特性。盡管其永遠不可能知曉Cfree的精確形狀，但卻節省了大量計算時間，是一種行之有效的折中方式，可用于高維空間中的運動規劃。不過與此同時也犧牲了算法的完備性，并代之以更弱的概念：使用隨機采樣方法的運動規劃算法是概率完備（Probabilistically Complete）的，使用確定性采樣方法的運動規劃算法是分辨率完備（Resolution Complete）的。為滿足弱完備性要求：①Cfree中的采樣序列必須稠密；②算法的搜索過程應具備“系統性”。更多關于SBMP 產生的歷史原因和早期發展過程可參考Lindemann 和Lavalle 的綜述文章［64］。

對于給定數個起始-目標構型對的多疑問（Multiple-Query）運動規劃問題，如果環境結構不發生改變，則非常有必要花費時間對模型進行預計算以生成路圖，從而提高后續搜索效率。概率路圖法（Probabilistic Roadmap，PRM）［65］是其中的典型代表，它最初是為應對高自由度運動規劃問題而發展的，算法流程如圖3 所示。PRM 構建的路圖可看作是對CMP 所導出路圖的一種近似，因此也常被與CMP 共同歸入基于圖搜索的運動規劃算法中［50］。Kavraki 等［66］通過對簡化的PRM（Simplified PRM，s-PRM）進行分析，建立了算法失敗概率Pfail與路徑長度L、路徑和障礙物間距R、采樣點數量n之間的函數關系，其中Pfail隨L的增加而線性增加，隨n的增加而指數減小到0，從而證明了PRM 的概率完備性。但由于路徑特性很難提前得知，故該完備性分析方法不易使用。另一分析方法［67］則借助ε-goodness［68］、β-lookout 和（ε，α，β）-expansive 的概 念，證明 算法失敗概率為

圖3 PRM 算法流程Fig.3 Procedure of extending PRM

式中：c1、c2、c3為正常量。從式（2）不僅能得出Pfail與n的指數關系，而且可以發現如果暗含Cfree可視特性的ε、α、β越大，那么完成求解所需的采樣點就越少，算法運行時間就越短。因為引起可視特性下降的狹窄通道不可能偶然出現［69］，故實際中遇到的許多Cfree都具有良好的可視特性，即相應的ε、α、β較大。另外可視特性是用體積比率定義的，而不直接依賴于C-space 維數，這便解釋了為什么當維數在一定范圍內增加時，PRM 仍能較好地運行。

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

步驟1 初始化，無向拓撲圖G（V，E）的節點集V初始時一般包含起始構型qI和目標構型qG。

步驟2 節點選擇方法（Node Selection Method，NSM），選擇一 個節點qcur∈V進 行擴展。

步驟3 局部規劃方法（Local Planning Method，LPM），選擇新的構型點qnew∈Cfree，試圖構造路徑τ：［0 1］→Cfree，使得（τ0）=qcur且（τ1）=qnew。用碰撞 檢測算 法確保τ?Cfree，否則返 回步驟2。

步驟4 在圖中插入一條邊，將τ作為連接qcur和qnew的邊插入E中。若qnew不在V中，也將其插入。

步驟5 對解進行檢驗，確定G是否已經編碼了一條解路徑。

步驟6 返回步驟2。

步驟2 中的NSM 類似于圖搜索算法中的優先隊列（Priority Queue），而步驟3 中的LPM 之所以稱為局部的，是因為其并不嘗試解決整個規劃問題，而是每次僅產生較短且簡單的無碰路徑段。對于非完整系統，LPM 也可稱為Steering 方法。現有算法大多都是步驟2 和步驟3 有所不同，其中最著名的應是擴張空間樹（Expansive-Spaces Tree，EST）［67］和快速擴展隨機樹（Rapidly Exploring Random Tree，RRT）［70-73］。前者將待擴展節點被選擇的概率設置為關于樹上節點分布密度的反比例函數，并在該節點周圍一定范圍內隨機選擇新的構型點進行擴展，從而保障了采樣樹的稠密生長。后者則通過定義一個無窮、稠密采樣序列，并利用NEAREST 函數為每個采樣點選擇其在S中的最近點來迭代生長（參見圖4［51］）。正是因為每個節點被選擇的概率與其對應Voronoi 區域的測度成正比，才保證了算法的快速擴展特性。兩種方法的概率完備性也已在原文中得到證明。值得一提的是，PRM 算法的變體［74-75］也可用于單疑問運動規劃問題：其先在忽略障礙物的情況下構建概率路圖，待找到可行路徑時僅對該路徑進行碰撞檢測。若某條邊或節點發生碰撞，則將其移除并重新增強路圖進行路徑搜索和碰撞檢測。Lazy PRM 的思想來源是碰撞檢測耗費了算法90%以上的時間，且對單疑問運動規劃問題來說大部分邊的碰撞檢測是無用的。

圖4 RRT 算法流程［51］Fig.4 Procedure of extending RRT［51］

在大多數應用中，解路徑的品質［76］亦十分重要，一般除可行性外還存在最優性要求，如最短長度路徑、最短時間路徑等。但可以證明，標準的PRM 算法和RRT 算法均不具備漸進最優性［77］。經典方法是利用啟發式圖搜索算法提供最優性保證，不過這種最優性受限于網格分辨率，且最壞情況下的運行時間隨空間維數呈指數增長。Karaman 和Frazzoli［77］通過修 改鄰近點選取方法和局部節點連接方式（在文章中分別對應Near Vertices 和Rewire），提出了概率完備的漸進最優算法—PRM*、快速擴展隨機圖（Rapidlyexploring Random Graph，RRG）和RRT*（算法流程參見圖5［78］和 圖6［78］），且后兩種算法具有Anytime 特性。其雖不能完全克服經典方法的缺點，但的確在當時提供了一個保證算法漸進最優性的新視角。以RRT*為例，算法在Near vertices 步驟中用變化球半徑的方式選擇qnew的鄰近采樣點，其中半徑為

圖5 PRM*算法流程［78］Fig.5 Procedure of extending PRM*［78］

圖6 RRT*算法流程［78］Fig.6 Procedure of extending RRT*［78］

式（3）是采樣離散度的函數，并隨采樣點數量n的增加而減小，d為構型空間維數，γ是與環境相關的參數。在Rewire 步驟中，首先將qnew連接至能為其提供更低代價的鄰近節點上，其次若qnew能為剩余某些鄰近節點提供更低的代價，則將該鄰近節點的父節點設置為qnew。一些關于PRM*和RRT*理論分析的改進近來也已被提出［79-81］。

上面的論述詳細探究了SBMP 的思想內涵，列舉了幾種經典的SBMP 算法以及它們的優缺點和適用場景。至于該算法框架中最近點函數［82］、距離度量［83-84］、局部規劃器［84-85］、碰撞檢測模塊［86-87］的選擇，在此不過多贅述。另外在經典SBMP 算法的基礎上，目前也已產生了許多算法加速策略，本節余下部分將圍繞這一主題展開。

1.2.1 利用確定性采樣方式提升算法性能

SBMP 算法最初都使用了隨機采樣方式，那么一個問題是：如果以確定性方式進行采樣，相關的理論保證和實際性能還會成立嗎？答案是肯定的，確定性規劃器可以簡化證明過程，可以將一部分在線計算量變為離線計算（這對考慮微分約束的規劃和高維空間中的規劃尤其有意義）。另外對于柵格序列，亦可簡化操作量（如定位相鄰點）。以PRM 為例，實質上“概率性”對其是不重要的，反而會導致采樣點的不規則分布，使連通性信息的捕捉變得更加復雜。Lavalle等［88］將局部規劃器引入經典網格搜索，發展了子采樣網格搜索（Subsampled Grid Search，SGS）算法，并將確定性的低差異度采樣技術引入PRM，發展了擬隨機路圖算法（Quasi-Random Roadmap）［89］和柵格路圖（Lattice Roadmap）算法，進行對比實驗后發現：相較于分辨率完備的確定性采樣方法（包括網格搜索），原始的PRM算法并沒有體現出優勢。故文章對“隨機性是打破高維空間維數詛咒的必要分量”這一說法進行了駁斥，事實上為了維持固定的離散度，任何采樣方案都需要與維數成指數關系的采樣點數量［90］。但Lavalle 等的工作［88］僅限于討論可行路徑，就收斂到最優路徑而言，獨立同分布采樣是否還有優勢？Jason 等［91］針對PRM 算法進行了討論（設g1、g2為從自然數集到實數集的映射，約定g1∈O（g2）表示存在某個自然數n0和正實數m，使得對于所有n≥n0，|g（n1）|≤m|g（n2）|；g1∈Ω（g2）表示存在某個自然數n0和正實數m，使得對于所有n≥n0，|g1（n）| ≥m|g2（n）|；g1∈ω（g2）表 示limn→∞g1(n)/g2(n)=∞）：

1）確定性漸進最優性，在d維空間中，使用l2離散度的上界為γn-1/d，連接半徑rn∈ω（n-1/d）的確定性采樣序列的PRM 算法是確定性漸進最優的。而使用獨立同分布隨機采樣序列的PRM*算法是概率性漸進最優的，且需要更大的連接半徑Ω（（log（n）/n）1/d）。

2）收斂率，在沒有障礙物的情況下，采用確定性采樣序列的PRM 的次優因子上界為2D（nrn-2Dn），其中Dn是采樣序列的l2離散度（存在障礙物時的結果更復雜一點）。而采用獨立同分布采樣的類似結果僅是概率成立，且更加繁瑣和難以理解。

3）計算復雜度和空間復雜度，在低離散度柵格上運行PRM 的計算復雜度和空間復雜度為。由于可以用rn∈ω（n-1/d）來獲得漸近最優性，故PRM 存在一個計算復雜度和空間復雜度為ω（n）的漸近最優版本。而使用獨立同分布采樣的復雜度結果為O（nlogn）量級。

可以看出確定性方法在需要更小的連接半徑的同時，具有更好的計算復雜度和時間復雜度。本質原因在于確定性低離散度序列和獨立同分布序列間離散度的差異，二者分別為O（n-1/d）和O（（log（n）/n）1/d）［92］。另外，從使用低離散度柵格的PRM 中得到的結果在其他一些情況下也精確或近似地成立，如k-nearestneighbor 算法、批處理算法、非柵格的低離散度采樣序列（如Halton 序列）、非均勻采樣和含微分約束的規劃［93］等。多類實驗結果表明：對于給定數量的采樣點，確定性低離散度采樣不會差于獨立同分布采樣。因而結合Lavalle 的結論［88］，Jason等［91］建議基于SBMP 算法都應使用非獨立同分布的低離散度采樣序列。

1.2.2 利用收集到的Cfree形狀信息改善采樣分布

事實上Cfree中的可視特性并非均勻分布，且描述整個Cfree可視特性的ε、α、β取決于其中最差的構型和lookouts。當含有狹窄通道時，以上3 個參數就會減小。而為了保證算法的失敗概率不超過Pfail，由式（2）可知所需的均勻采樣點數量隨即大幅增加。如果規劃器能根據已有的或運行過程中收集到的信息對Cfree的形狀做出概率上的推測，則可以此推測來偏置采樣點分布，使其能更好地捕捉C-space 的連通特性，從而減少所需的采樣點數量，提高算法效率。但顯式維持該概率模型的代價很高，因此早期研究僅是用啟發式［94］來近似最優采樣策略，其本質上是一種離線方法。包括基于工作空間的策略［95-98］、基于障礙物的策略［99-100］、基于可視性的策略［101］、Bridgetest 采樣策略［102］等。但由于基于可視性的策略［101］采用的是拒絕采樣方式，故實際使用中的效果不太理想［85］。

自適應采樣則在線調整采樣點的分布。Hsu 等［103］將以上離線偏置采樣方法線性加權，并在每次采樣后調整權重，稱為混合PRM 采樣策略。由于“邊界點”一般有更大的Voronoi 偏置卻不能被成功擴展，Yershova 等［104］針對含有復雜幾何的運動規劃問題，提出了將“邊界點”采樣域限制在以預設值為半徑的球內的Dynamic-Domain RRT（DD-RRT）方法。Jaillet 等［105］則根據“邊界點”成功擴展的概率來調整邊界域半徑，實驗結果表明Adaptive Dynamic-Domain RRT（ADD-RRT）在大多數實驗場景下都比原始RRT 的性能高出數個量級。對于多疑問運動規劃問題，Burns 和Brock 建立了表示Cfree形狀的混合高斯模型［106］和k-nearest-neighbor 模型［107］，并用采樣信息更新該模型。前者最大程度地減小模型方差，后者則用期望效用來引導采樣。相應結果同樣被擴展至單疑問運動規劃問題［108］，在提升計算效率的同時也增強了規劃器的魯棒性。

1.2.3 利用解路徑代價和尚需代價的估值導引采樣分布

隨著采樣點數量趨于無窮，雖然RRT*可以保證漸進收斂到最優解，但這種按照隨機次序進行搜索的方式在無用狀態上浪費了大量計算時間，降低了算法效率，使其難以應用到一些實際問題中，如無界空間（即室外環境中的規劃）和高維空間（即機械臂的規劃）。因而啟發式被用來或縮小搜索區域，或安排搜索次序。heuristically-guided RRT（hRRT）算法［109］以 與啟發代價成反比的概率選擇采樣點，使采樣樹朝著低代價區域生長，從而得到質量更好的次優路徑。Anytime RRT［110］迭代地用前次的解來界定本次的搜索范圍，用與hRRT 類似的思路選擇待擴展節點，使解路徑的代價隨運行次數不斷下降，但并未提供最優性保證，且前次采樣點被不斷丟棄，信息復用率不高。Transition-based RRT［111］將構型空間代價地圖與規劃問題作為輸入，用隨機優化方法決定是否保留新的采樣點，以使RRT 朝著構型空間代價地圖的谷地和鞍點進行擴展。Karaman 等［112］通過“圖修剪”技術，周期性地移除那些當前代價與啟發式代價之和大于已有最好路徑代價的頂點，發展了RRT*的Anytime 版本。Hauser［113］則在“圖修剪”技術的基礎上結合Lazy 檢測，提出了Lazy-PRM*算法和Lazy-RRG*算法。Akgun 和Stilman［114］、Islam等［115］均采用了路徑偏置方法，即通過增加當前解路徑節點鄰域的采樣頻率來加速RRT*的收斂，但該方法基于不切實際的同倫假設且需持續全局采樣以規避局部極小值。除此之外，前者還用到了雙向搜索和采樣點拒絕（Sample-Rejection）技術，雖然一次采樣迭代消耗的時間極短，但隨著關注區域與Cfree間體積比率的減小，找到一個可接受的采樣點所需的迭代次數也會增加，此時的計算量便再不容忽視。RRT#［116］利用啟發式在由RRG 遞增建立的圖上尋找并更新樹，并通過LPA*［117］將變化傳播到整個圖中，從而有效維持了到每個頂點的最優連接。雖然用啟發式縮小搜索區域的方法帶來了一定的性能提升，但其仍采用了與RRT*類似的擴展次序，使得重要的、難以采樣的狀態由于目前不能連接到樹而被簡單丟棄，同時還可能浪費計算時間。Informed RRT*［14］首先運行原始的RRT*算法，待找到解后再直接在不斷縮小的橢球形信息集內進行采樣（參見圖7［118］）。相比于RRT#，Informed RRT*在橢球體積減少時，仍能有效提高收斂率和解路徑質量［118］。

圖7 Informed RRT*算法流程［118］Fig.7 Procedure of extending Informed RRT*［118］

至此所述方法雖然均在最優解的收斂速度上有所改進，但本質上其生長樹的方式都是無序的。Jason 等［15］用一種Marching 方法，按Costto-Come 遞增的次序（類似于Dijkstra 算法［57］）搜索一批采樣點。其中空間近似和搜索的分離使得搜索次序獨立于采樣次序，但卻犧牲了Anytime 特性。在搜索完成之前，FMT*不會返回解路徑。另外也與其它先驗離散方法一樣，如果解不存在，則搜索必須重新開始。Gammell 等［16-17］試圖將有信息的圖搜索算法和具有Anytime 特性的RRT*算法統一至同一框架下，從而發展了一種可以有效解決連續空間路徑規劃問題的有信息、有Anytime 特性、有漸進最優保證且避免大量計算無用狀態的基于采樣的運動規劃算法—Batch Informed Trees（BIT*），該方法的主要貢獻在于維持潛在解路徑質量的優先級序列的同時利用分批采樣技術，使空間近似和空間搜索得以交替進行。一些BIT*的改進算法也已被提出：Advanced BIT*（ABIT*）［119］使用類似于ARA*［59］的次優啟發式因子快速找到初始解路徑，再以Anytime 形式向最優解路徑收斂。Adaptively Informed Trees（AIT*）［120］通過對稱雙向搜索來同時估計并使用一個精確的、針對特定問題的啟發式，可較好地適用于局部路徑評估較昂貴的情況。Regionally Accelerated BIT*（RABIT*）［121］利用局部優化器來解決不能通過直線連接的狀態之間的連接問題，有助于在難以采樣的同倫類中找到路徑。

除上述算法之外，為平衡最優性與計算效率間的矛盾，一些文章［122-124］也通過放松關于最優性的要求，對漸近近優（Asymptotically Near-Optimal）算法進行了討論。

1.2.4 重復使用之前有效的搜索信息并降低重規劃的頻率

當機器人在含有靜態障礙物或動態障礙物的未知環境中工作時，突然出現的障礙物一般只會對之前路徑的一部分產生影響，而剩余部分對于接下來的搜索仍然有效。若能充分利用這一部分有效信息，無疑可以加速重規劃過程（類似于D*Lite［60］算法和AD*［61］算法）。ERRT［125］用之前可行路徑的Waypoint 進行偏置采樣，但每次重新生成采樣樹的方式降低了算法效率。Dynamic RRT（DRRT）［126］則在ERRT 的基礎上融合D*［127］算法的思想，從目標構型反向搜索可行路徑，且僅丟棄發生碰撞的構型及其子節點，提高了采樣樹的重建速度（參見圖8［126］）。與DRRT 完全刪除被障礙物影響的分枝不同，Multipartite RRT（MP-RRT）［128］依然保留這些不連通分量，并試圖將其重新連接到采樣樹上。Van den Berg 等［129］結合PRM 與AD*算法：首先構建一個僅考慮靜態環境的路圖，通過簡單預測動態障礙物軌跡，以Anytime 方式從目標反向搜索次優軌跡。若執行軌跡時環境發生改變，則更新路圖及啟發式，修復規劃軌跡。Ferguson 和Stentz［130］也已將AD*的思想擴展至解決單疑問運動規劃問題的RRT 算法中。

圖8 DRRT 算法流程［126］Fig.8 Procedure of extending DRRT［126］

復用之前有效的搜索信息雖然可以加速算法，但前述的反應式避障（Reactive Avoidance）方案同時也可能造成機器人在某些情況下不斷地進行重規劃，從而增加完成任務所需的時間。相比之下，利用感知到的信息預測動態障礙物軌跡，并與已有運動規劃算法進行融合顯然是一種更好的避障手段。該規劃框架可以提高解路徑的品質（即延長路徑可行性的持續時間），降低重規劃的頻率。其中的關鍵技術是運動物體未來行為或軌跡的預測，現有文獻中的解決方案可以分為基于物理定律的方法（即感知-預測方案）、基于模式的方法（即感知-學習-預測方案）和基于規劃的方法（即感知-推理-預測方案）3 類。前者根據牛頓運動定律顯示地建立單個或多個動力學或運動學模型，并通過某種機制融合或選出一個模型進行前向仿真，以達到軌跡預測的目的；中者適用于含未知復雜動態的環境，其通過用不同的函數近似器（即神經網絡、隱馬爾可夫模型及高斯過程等）擬合數據來學習待預測目標的運動行為；后者則融合了理性智能體的概念，即假設智能體在長期運動過程中需最小化一個與其一系列行為相關的目標函數，并計算能使智能體達到這個目標的策略或路徑猜想。其中目標函數可以預先指定，亦可通過學習得到（如利用逆強化學習技術等）。更多詳細內容可參考Lefevre［131］和Rudenko［132］等的綜述文章，此處不再贅述。

1.2.5 利用學習算法加速傳統運動規劃問題的求解過程

機器學習方法與傳統運動規劃算法的結合最近已成為研究人員的關注熱點，此類方法的優勢主要體現在兩個方面：①相較傳統運動規劃算法，能更有效地找到近優路徑；②可直接基于原始圖像進行路徑規劃。一些文章已將深度學習技術如收縮自編碼器（Contractive AutoEncoder，CAE）［133］、條件變 分自編碼器（Conditional Variational AutoEncoder，CVAE）［134］、卷積神經網絡（Convolutional Neural Network，CNN）、圖神經網絡（Graph Neural Networks，GNN）［135］及它們的變體成功應用于SBMP 中，且大多數結合方式都聚焦于①用神經網絡編碼C-Space，并改善SBMP 的采樣點分布；②直接端到端地生成路徑。Deep Sampling-based Motion Planner（DeepSMP）［136］由編碼器和采樣點生成器構成，前者用原始點云數據作為CAE 的輸入以將障礙物空間編碼為不變的、魯棒的特征空間；后者用RRT*產生的近優路徑訓練基于Dropout［137］的統計前饋深度神經網絡，使其能在給定起始構型、目標構型、障礙物空間編碼信息的情況下，在包含解路徑的構型空間區域內為SBMP迭代生成采樣點。Neural RRT*［138］通過學習大量由A*算法生成的最優路徑來訓練CNN，該模型可為新的路徑規劃問題快速提供最優路徑的預測概率分布，用于指導RRT*的采樣過程。Ichter 等［139］認為解路徑僅依賴于由C-space 結構決定的幾個關鍵構型。因此其通過圖論技術識別這些關鍵構型，并僅用局部環境特征作為CNN的輸入來學習預測關鍵構型，從而提升了PRM算法的性能。其在另一文章［18］中用以往成功的規劃結果和機器人經驗訓練CVAE，然后根據給定的初始構型、目標區域和障礙物信息，可以對CVAE 的隱空間（Latent Space）進行采樣，并將其投影到狀態空間中更有希望包含最優路徑的區域。但這種預測最短路徑采樣點的做法其實把所有負擔都壓給了Learner，任何由近似或訓練-測試不匹配造成的誤差均可使算法失效。故LEGO［140］提取多條不同近優路徑上的瓶頸點，并以其作為CVAE 的訓練輸入數據。針對CNN 和全連接網絡（Fully-Connected Network，FCN）容易丟失環境結構信息而引起的泛化不良問題，Khan 等［141］利用GNN 的置換不變特性魯棒地編碼C-space 的拓撲結構，并計算采樣分布的參數以生成采樣點。實驗結果表明：在學習關鍵采樣點方面，GNN-CVAEs 的表現大大優于CNN，而GNN 則優于在高維空間中規劃的FCN 模型。除了用原始點云數據和C-space 障礙物信息作為輸入外，利用CNN 學習對象級語義信息產生的采樣分布也可以改善未知環境中的導航結果［142］。與上述偏置采樣點分布的方法不同，MPNet［19，143］則直接生成可行近優路徑。其由編碼網絡（Enet）和規劃網絡（Pnet）組成，前者將機器人環境信息編碼入隱空間，而后者則利用起始構型、目標構型和Enet 作為輸入生成路徑。

除深度學習外，強化學習（Reinforcement Learning，RL）［144］在運動規劃領域也有一些成功應用的案例。Q-function sampling RRT（QSRRT）［145］根據學習到的狀態-行為值函數（Qfunction），提出Softmax 節點選擇方法，加速了RRT 的規劃過程并避免陷入局部極小值。Chen等［146］建立了一個基于Tabular Q-learning 和值迭代網絡（Value Iteration Networks，VIN）［147］的可學習神經擴展算子，以為基于樹的運動規劃算法學習有潛力的搜索方向。Bhardwaj 等［148］將Lazy搜索中邊的選擇問題建模為馬爾可夫決策過程（Markov Decision Process，MDP），并引入模仿學 習（Imitation Learning）［149］進行求解。Zhang等［150］利用MDP 建立拒絕采樣模型，并通過策略梯度方法優化采樣分布以降低如碰撞檢測次數和樹的尺寸之類的規劃代價，從而得以加速現有的最優運動規劃器。

綜上，盡管基于學習的技術在運動規劃領域取得了一些進步，但此類方法在未經歷環境中的性能表現還有待驗證。

根據以上關于傳統運動規劃問題的討論可知：相比CMP，SBMP 取得成功的原因在于其以犧牲完備性為代價，換取對復雜Cfree的連通性擁有較強的表示能力（即通過“采樣”的方式構建包含解路徑的離散結構），而非最初版本中所帶有的“采樣隨機性”和“搜索盲目性”。相反該特性恰恰使算法拋棄了大量可用的有效信息，阻滯了性能的進一步提升。因此針對不同場景，如何在SBMP 算法的設計過程中妥善地融入1.2.1～1.2.5 節的5 類信息，從而提高解路徑品質并避免在無價值節點上浪費大量計算時間，將是傳統運動規劃研究中要考慮的重要問題。

2 考慮微分約束的開環運動規劃

大多數運動規劃問題都會涉及來自機器人運動學或動力學的微分約束。一般的處理方式是先在規劃過程中忽略這些約束，并通過傳統運動規劃算法生成幾何可行路徑，然后再在問題的改進過程中利用軌跡規劃/軌跡優化技術處理它們。雖然這種解耦規劃［151-152］在許多情況下可以節省大量計算時間，但同時也丟失了完備性和最優性保證。更好的選擇是在規劃過程中直接考慮微分約束，這樣便可得到服從系統自然運動特性的軌跡，同時降低反饋控制器的跟蹤誤差。此類問題一般也被稱為Kinodynamic Motion Planning（KMP）［153］。本質上，KMP 可被視為經典兩點邊值問題（Two-point Boundary Value Problem，TPBVP）的變體。后者通常是給定初始狀態和目標狀態的情況下，在狀態空間中計算一條連接初末狀態并滿足微分約束的（最優）路徑；而規劃問題則牽扯到一類附加的復雜性：避免與狀態空間中的障礙物發生碰撞，用控制理論的術語來講即是為包含非凸狀態約束和控制約束的非線性系統設計（最優）開環軌跡。但求解TPBVP的技術并不能很好地適用于考慮微分約束的運動規劃，因為其本就不是為處理全局障礙物約束而設計的，或者說很難得到受非凸狀態和控制約束的非線性系統的最優必要條件［154］。

文獻中處理KMP 的方式一般有基于采樣的方法和基于優化的方法兩類。關于前者，由于微分約束破壞了CMP 所需的良好特性，故第1 節中僅有SBMP 可能實現較好移植。關于后者，其一般有3 種應用場景：一是在前述解耦規劃中，用于平滑和縮短由其它規劃算法（如SBMP）生成的路徑；二是直接從較差初始猜想（可能是與障礙物相交的線段）開始計算局部最優的無碰軌跡；三是在已知自由空間的凸胞腔族中規劃微分約束可行的（最優）軌跡。后兩類場景將在2.2 節詳述。而2.1 節將首先介紹如何利用SBMP 處理考慮微分約束的運動規劃問題。

2.1 基于采樣方法的KMP

要求一般的機器人系統在微分約束下精準到達狀態空間中的某個采樣點要么是不可行的（圖9 中的橙色區域為機器人有限時間前向可達集，其在一定時間范圍內無法到達可達集之外的采樣點），要么則需解決復雜的TPBVP 問題（這亦是很少應用路圖類算法的原因，即目前不存在有效的LPM 方法），故考慮微分約束的SBMP 通過離散動作空間和時間，并進行前向仿真來遞增地生成采樣樹。離散的動作空間和時間其實暗含著初始狀態的可達圖，若狀態的一階微分函數滿足Lipschitz 條件［8］，則隨著離散度（以概率1）趨于零，動作序列將任意接近相應動作軌跡，即可達圖的節點將在可達集中（以概率1）變得稠密，這也是對基于采樣方法的KMP 最重要的要求。加之“系統性”搜索，便保證了算法的分辨率（概率）完備概念。

圖9 機器人有限時間前向可達集Fig.9 Finite time forward reachable set of the robot

RRT［71］與EST［155］最初便是為解決含微分約束的運動規劃問題而設計，而非傳統運動規劃。其算法流程與上一節基本相同，稍微的區別在于—此處的算法一般在固定的離散動作集中選擇能最小化積分后狀態與采樣點間距的離散動作，作為樹上新加入的邊所對應的控制輸入（積分時間可以固定，也可以在某個區間［0，ΔTmax］內隨機選擇）。盡管RRT 為含微分約束的運動規劃問題提供了較好解決方案，但它的缺點是性能對度量函數較為敏感，差的度量可能導致一些注定發生碰撞的狀態和位于可達集邊界的狀態被重復選擇，重復擴展，從而大大增加了運行時間，延緩了樹的生長。如圖10（a）［156］所示：橙色為初始狀態可達集，而可達集邊界狀態（紅色）有較大的Voronoi 區域，故容易被重復擴展；又如圖10（b）［157］：紅色狀態有較大Voronoi 區域，但其擴展結果大概率會發生碰撞。理想距離函數應是滿足某種最優性的代價泛函［158］，但除非對于像無約束、有二次形式代價的線性系統存在解析解［159］，一般來講設計這樣的偽度量與解決原始運動規劃問題一樣困難。雖然也有學者提出適應于弱非線性系統的近似度量［156，160］，不過一個更重要的問題是如何令算法在較差的度量函數下依然有良好的性能？或者說如何令采樣樹在較差的度量函數下依然能（以概率1）快速稠密地覆蓋初始狀態的無碰可達集？另外，引入微分約束也對RRT*提供最優性的方式構成了挑戰，發展新的考慮微分約束的漸近最優算法是又一亟需解決的問題。

圖10 SBMP 算法的度量敏感性示意［156-157］Fig.10 Metric sensitivity of SBMP algorithm［156-157］

2.1.1 度量函數敏感性問題

針對RRT 對度量函數的敏感性問題，Resolution-Complete RRT（RC-RRT）［161-162］利用Constraint Violation Function（CVF）描述每個頂點發生碰撞的概率，并通過記錄已成功應用的動作，在避免重復擴展的同時可以尋找到更有價值的頂點。RRT-blossom［163］選擇待擴展節點的方式與RC-RRT 類似，不同的是RRTblossom 同時應用所有可能的行為，并移除碰撞軌跡和回退軌跡。Reachability-Guided RRT（RG-RRT）［164］的顯著特點是為采樣樹上各頂點計算離散可達集，通過位于可達集Voronoi 區域內的采樣點來引導搜索，從而在不需要特殊啟發式的情況下緩解了對距離度量的敏感性。但由于RG-RRT 算法在狹窄通道環境中可能持續發生碰撞，而RC-RRT 則可能偏向選擇那些有較小CVF 和較大Voronoi 區域面積的無價值頂點，因此促使 Environmentally Guided RRT（EGRRT）［157］算法將兩種策略合并。Path-Directed Subdivision Tree（PDST）［165-166］用路徑 段表示“采樣點”，并將“采樣點”按優先擴展順序排列，同時用狀態空間的非均勻細分來估計覆蓋率，從而避免了度量敏感問題。GRIP（Greedy，Incremental，Path-directed）［167］通過用簡單度量代替路徑細分過程而進一步改進了PDST 算法，并以此偏置采樣，加之DRRT［126］重復使用先前信息的優勢，實現了未知環境中含微分約束的重規劃。另一種與PDST 類似的方法是KPIECE（Kinodynamic Planning by Interior-Exterior Cell Exploration）［168-169］，其用網格離散低維投影空間，并標記為外胞腔和內胞腔兩類。因為前者較好捕捉了采樣樹的邊界，故為實現更快的空間探索，文章以75%的概率選擇外胞腔進行擴展。其次算法也將更偏愛于覆蓋率較低、相鄰胞腔較少、擴展次數較少等有利于采樣樹生長的胞腔。實驗結果表明：KPIECE 的運行時間和內存消耗顯著下降。最近一些機器學習方法也被用來離線學習度量函數和Steering 函數［170-173］，它們通過最優控制算法提供數據集，以有監督的方式近似兩狀態間的尚需代價函數和對應的控制輸入，從而為RRT 的在線執行提供有價值的信息。

2.1.2 最優性問題

正如前文所指出：為了獲得漸近最優性，RRT*要求精確且最優地連接狀態對，但其實這僅適用于完整系統［174］和存在較好Steering 方法的非完 整系統。Karaman 和Frazzoli［175］再次將RRT*算法擴展至含微分約束的運動規劃問題中，并在存在局部最優Steering 方法的前提下，針對局部可控系統，提出了保證算法漸進最優性的充分條件。同樣假設存在局部最優Steering 方法，滿足動態環境中實時Kinodynamic 重規劃的RRTX［176］算法也已被提出。類似于Kim 等［156］的工 作，LQR-RRT*［177］將基于 線性二次調節器（Linear Quadratic Regulators，LQR）的啟發式應用于RRT*，即用LQR 代價函數作為度量函數來選擇鄰近頂點，用LQR 控制策略生成軌跡。但因為每次都是在新的采樣點處進行系統線性化，所以代價函數各不相同，而且此類軌跡無法準確到達目標狀態，也就無法確定結果的最優性。Kinodynamic RRT*［178］針對能控線性系統，利用fixed-final-state-free-final-time［159］控制器來精確且最優地連接狀態對，從而滿足了上述充分條件［175］，使算法有了漸進最優性保證，類似工作還包 括Goretkin 等［179］關 于LQR-RRT*算法的改進。

與傳統運動規劃類似，如何利用已有信息改善采樣分布以求加速算法，已經成為研究人員關心的一個問題。Xie 等［180］以歐式距離與最大速度的商做為BIT*的保守啟發式，用序列二次規劃（Sequential Quadratic Programming，SQP）［181］的變體求解局部BVP，提出了一種有較好性能提升的KMP 方法。同時FMT*的Kinodynamic 版本見 于Schmerling 等［182］的工作。不 考慮微分約束的系統的信息集形成了一個參數化的橢圓，直接在該橢圓中采樣可有效降低算法運行時間［118］。但對含微分約束的系統來講，顯式表示信息集非常困難，而一般的拒絕采樣方法在高維情況下又效率低下。故針對存在局部Steering方法的系統，Kunz 等［183］提出分層拒絕采樣（Hierarchcal Reject Sampling，HRS）技術來緩解這一問題。Yi 等［184］則將其重新定義為在隱非凸函數的子水平集內的均勻采樣問題，從而使蒙特卡羅采樣方法得以應用：給定信息集中先前的一個采樣 點，HNR-MCMC（Hit-and-Run Markov Chain Monte-Carlo）首先對某個隨機方向進行采樣，然后通過拒絕采樣找到最大步長，使新采樣點位于信息集內。Joshi 等［185］利用橢球工具箱［186］離線求解并保存線性系統的初始構型前向可達集和目標構型后向可達集，導出時間信息集（Time-Informed Set，TIS）。一旦找到初始解，后續搜索將被限定在TIS 中，從而加速KMP 的收斂。

對含有更復雜微分約束的系統，RRT*類方法獲得漸進最優性的方案很難繼續應用。因此只通過模型前向仿真以收獲最優性的思路便吸引了研究人員的注意力。AO-X［20-21］算法將最優KMP 問題表述為可行KMP 問題。首先利用任意可行的KMP 算法（文中為RRT 和EST）在State-Cost 空間中生成可行軌跡，再通過逐次縮小軌跡代價的上界以滿足漸近最優性要求。且可以證明算法的期望運行時間為O（δ-2lnlnδ-1），其中δ是描述解軌跡次優性的參數。SST（Stable Sparse RRT）和SST*［22］通過蒙特卡洛前向傳播（Monte-Carlo Forward Propagation）建立采樣樹，以修剪樹上的局部次優分枝并維持稀疏結構，分別保證了算法的漸近幾乎最優性和漸進最優性。另外，一些不要求Steering 方法的加速方案也已被提出：Dominance-Informed Region Tree（DIRT）［187］引 入 Dominance-Informed Region 來謀求Voronoi 偏置與路徑質量間的平衡，并采用類似于RRT-blossom 的邊擴展策略和圖修剪技術以縮短找到高質量解軌跡所需的時間。同樣在類似于RRT-blossom［161］的邊擴展策略的基礎上，Informed SST（iSST）［188］模仿A*引入啟發式來起到有序選擇待擴展頂點和修剪的作用，提高了有限時間內的求解成功率和相同時間內的路徑質量。

除上述兩類獲得漸進最優性的方法外，狀態柵格（State Lattice）方法［189-190］也可獲得分辨率下的最優性。其由離線計算的運動基元庫（Motion Primitives Library）在線導出，是對初始狀態無碰可達集的近似。通過在狀態柵格上的搜索過程，可求得符合要求的解軌跡。

2.1.3 考慮微分約束的基于學習的運動規劃方法

與傳統運動規劃類似，基于學習的方法也被應用于考慮微分約束的運動規劃問題，現有文獻中的改進措施主要集中于：①端到端地生成軌跡；②學習在無碰可達集內生成稠密（最優）的采樣點分布；③在不考慮障礙物的情況下，學習針對復雜系統的LPM；④學習有關NSM 的度量函數。Huh 等［191-192］提出的c2g-HOF（cost to go-High Order Function）神經網絡架構以工作空間為輸入，輸出給定構型空間和目標構型的連續cost-to-go 函數，而據其梯度信息便可直接生成軌 跡。MPC-MPNet（Model Prective Motion Planning Network）［23］是MPNet［19，142］在KMP 領域的進一步擴展，算法框架包括神經網絡生成器（Neural Generator）、神經網絡鑒別器（Neural Discriminator）和并行模型預測控制器（Parallelizable Model Predictive Controller ）。前者迭代生成批量采樣點，中者選出有最小代價的采樣點并通過后者進行最優連接（也可為所有批量采樣點在樹上找出最近點，并用MPC 并行計算局部最優軌跡，即文中的MPC-MPNet Tree 算法），實驗結果表明MPC-MPNet 相較現有算法在計算時間、路徑質量和成功率方面有較大提升。為研究任務約束、環境不確定性和系統模型不確定性場景中的長范圍路圖構建問題，Francis 等［24］和Faust 等［25］合并了PRM 的規劃效率和RL 的魯棒性，并提出由深度確定性策略梯度（Deep Deterministic Policy Gradient，DDPG）［193］或連續動作擬合值迭代（Continuous Action Fitted Value Iteration，CAFVI）［194］訓練的RL agent 決定路圖連通性。結果表明無論相比RL agent 自身還是傳統的基于采樣的規劃器，PRM-RL 的任務完成度都有所提高。RL-RRT［26］仍將RL agent 作為局部規劃器，同時訓練一個有監督的可達性估計器作為度量函數。該估計器以激光雷達等局部觀測數據為輸入，預測存在障礙物時RL agent 連接兩狀態所需的時間，以起到偏置采樣分布的作用。L-SBMP（Latent Sampling-based Motion Planning）［27］方法由自編碼網絡、動力學網絡和碰撞檢測網絡構成（分別模仿基于采樣算法中的狀態采樣、局部Steering 和碰撞檢測），前者隱式地編碼了嵌入在狀態空間的系統動力學低維流形，并提供了對隱空間直接采樣的能力，加之動力學網絡和碰撞檢測網絡，使L2RRT（Learned Latent Rapidly-exploring Random Trees）可以有效地、全局地探索學習到的流形。CoMPNet（Constrained Motion Planning Networks）［28］針對操作規劃和有運動學約束的規劃問題而提出，由環境感知和約束編碼器組成，它的輸出作為神經規劃網絡的輸入，并與雙向規劃算法一起，在約束流形上生成起始構型和目標構型間的可行路徑。

經過2.1 節的討論，可以直觀地覺察到引入微分約束后SBMP 算法所面臨的新困難：首先算法的搜索范圍發生了變化，即局部無碰可達集的概念代替了傳統運動規劃中可視區域的概念，用局部無碰可達集外的構型引導采樣樹的生長顯然浪費了寶貴的計算時間。但在可達集中直接采樣的思路目前卻仍存在2 個難點：一是高維非線性系統可達集的實時計算，二是可達集形狀的不規則；其次更一般的TPBVP 問題的求解很難逾越，即使代之以采樣樹的前向仿真方案，過長的運行時間也將使算法在實際應用中很難獲得最優性，甚至變得不可行。綜上，如何像傳統運動規劃那樣，借助已有或學習到的信息限制搜索范圍、安排搜索次序、設計度量函數，以加速考慮微分約束的運動規劃算法，將是未來的發展方向。另外，前述SBMP 的加速策略或解品質提升策略已被總結在表2 中。

表2 SBMP 的加速策略或解品質提升策略總結Table 2 Summary of acceleration strategies or solution quality improvement strategies for SBMP

2.2 基于優化的運動規劃算法

雖然考慮微分約束的基于采樣的運動規劃算法具有全局搜索特性且不需要初始猜想，但其在高維空間中的應用確需消耗較長計算時間，這使得一些研究人員訴諸于局部最優的基于優化的運動規劃算法。與SBMP 將障礙物約束滿足與否交予碰撞檢測這一黑箱不同，基于優化的方法受益于最優控制直接法［195-196］，即顯式考慮障礙物約束。其將函數空間中的無窮維優化問題轉化為有限維非線性參數優化問題［180］，在一定意義上可被統一至模型預測控制（Model Predictive Control，MPC）［197-198］的框架下（參見圖11，其根據當前測量到的系統狀態x（t0）反復解一個開環最優控制問題，這里u為控制量，f為系統模型，d為擾動，X和U分別為可行的狀態集合和控制集合，Xf為目標狀態集合，J為優化指標。上標“*”表示最優值，“～”表示標稱量，“·”表示導數）。文獻中目前大致存在3 類基于優化的運動規劃算法：

圖11 標稱MPC 的一般框架Fig.11 General framework for nominal MPC

1）無約束優化方法［29-31，199］，其目標函數被由障礙物表示的人工勢場所增強，或通過確定性協變方法［29-30］，或通過概率梯度下降方法［31］減小目標函數值。雖不需要高質量的初始猜想，但并未從理論上提供收斂保證，而且在有雜亂障礙物的環境中的失敗率較高，不適用于實時運動規劃問題。

2）序列凸規劃方法，對有約束的非凸優化問題來講，通用類非線性規劃算法［200-201］的收斂表現嚴重依賴于初始猜想，無法提供收斂保證并提前預知所需的計算時間，很難應用于實時任務。而凸優化問題可保證在多項式時間內可靠地得到全局最優解［202］，為借助這一優勢，必須將非凸的最優運動規劃問題進行凸化。其中的代表性方法包括TrajOpt［32-33］、SCvx［34-35］、GuSTO［36］，且后兩者提供了理論證明，促進了其在實時任務中的應用。此類方法的詳細介紹可參考Malyuta等［203］的文章，這里不再展開。除問題的凸化外，該類方法面臨的另一困難則在于如何建立恰當的避障約束［204］。

3）凸分解方法，為了避免由避障需求帶來的非凸約束的影響，凸分解方法通常將已知自由空間分解為一系列重疊的凸胞腔，并保證數個插值曲線片段分別位于各凸胞腔內以滿足機器人運動過程的安全性要求（參見圖12，其中紫色為障礙物區域，綠色為已知自由空間分解后的凸胞腔，藍色為規劃的軌跡）。Deits 和Tedrake［205］用IRIS（Iterative Regional Inflation by Semidefinite Programming）計算安全凸區域，由混合整數優化完成多項式軌跡分配，最后利用一種基于平方和（Sums-of-Squares，SOS）規劃［206］的技術確保了整個分段多項式軌跡不發生碰撞。相比于IRIS，Liu 等［37］根據由JPS（Jump Point Search）［207］算法求得的初始分段路徑建立安全飛行走廊（Safe Flight Corridor，SFC），減少了凸分解的時間。同時SFC 為二次規劃（Quadratic Program，QP）［180］過程提供了一組線性不等式約束，允許進行實時運動規劃。Chen 等［38］逐步構建基于八叉樹的環境表示，并通過在八叉樹數據結構中的有效操作來在線生成由大型重疊三維網格組成的自由空間飛行走廊。Gao 等［39］則在環境點云地圖的基礎上，將SFC 的構建交付于SBMP。除此之外的另一個關鍵問題是區間分配（Interval Allocation）方式和時間分配（Time Allocation）方式，前者決定每個凸胞腔內的軌跡間隔，而后者則處理在每個間隔上所花費的時間。

圖12 凸分解方法示意Fig.12 Illustration of convex decomposition method

雖然基于優化的運動規劃算法采用了3 種不同的處理思路，但其本質上都是建立在有約束的非線性優化問題的基礎上的，所以優化技術未來可預見的重大進展將是此類算法性能提升的主要渠道。表3［78］對本文所討論的幾種開環最優路徑（軌跡）規劃算法進行了總結，其中rn和kn分別代表在路圖上選取鄰域點時的半徑和數量，n為采樣點數量，d為Cfree的維數，μ為體積測度，ζd為d維單位球的體積，φ∈（0，1）為最優誤差。對于RRT*：c*為最優路徑代價，θ∈（0，1/4），ψ∈（0，1），而含微分約束的運動規劃不需要幾何鄰域的定義。

表3 考慮微分約束的最優算法總結［78］Table 3 Summary of optimal algorithms considering differential constraints［78］

3 反饋運動規劃

真實物理世界中的大多數問題都需要反饋，這一需求來自于傳感器測量的不確定性、環境的不確定性和未來狀態的不確定性，因而規劃結果應該以某種方式依賴于執行過程中收集到的信息。反饋運動規劃有兩種不確定性建模方法：顯式方法和隱式方法。前者通過建立能顯式考慮傳感器誤差和控制器誤差的模型，并將其融入規劃算法來解決不確定性下的運動規劃問題。而后者則源于控制理論，即為系統設計反饋控制律或稱為控制策略，而非開環控制律（一條動作軌跡或動作的時間序列），以使其即使處于不期望的狀態，也知曉該采取何種動作。但在傳統做法中，規劃與控制往往是解耦的，由外部干擾、模型誤差及控制約束等導致的跟蹤誤差可能造成意外碰撞（參見圖2），因此有必要將兩者放在一起來考慮。本節剩余部分將分別對顯式建模和隱式建模兩類方法進行綜述。

3.1 顯式建模不確定性

顯式方法主要發生在信念空間（Belief Space）［208］，可在部分可觀測的馬爾可夫過程（Partially Observable Markov Decision Process，POMDP）的框架下進行描述。但同時也面臨著維數詛咒和歷史詛咒，即計算復雜度關于規劃問題的維數和時域長度指數增長。基于點（Point-based）的方法［209-212］一定程度上緩解了這個問題，其核心思想是用信念空間中的采樣點集近似表示信念空間，用離線值迭代方式求得在線執行所依賴的動作策略，可在合理時間內解決具有數十萬狀態的中等復雜度的POMDP。相反，在線方法［40-41，213-214］將規劃和執行規劃結果交替進行：在每個時間步中，僅為當前信念搜索一個最優動作，進而執行該動作并更新信念。從而避免了預先為所有未來事件計算策略，具備更強的可擴展性。目前最快速的在線方法為POMCP（Partially Observable Monte-Carlo Planning）［40］和DESPOT（Determinized Sparse Partially Observable Tree）［41］，兩者均采用了蒙特卡洛采樣技 術［215］，可解決 狀態數高達1056的大規 模POMDP 問題。POMCP 在信念樹（Belief Tree）上進行蒙特卡羅樹搜索（Monte-Carlo Tree Search，MCTS），并使用PO-UCT（Partially Observable-Upper Confidence Boun-ds for Trees）［215］來權衡探索（Exploration）與利用（Exploitation）。DESPOT 則通過一組采樣場景得以在稀疏信念樹中執行Anytime 啟發式搜索，并且相較POMCP 提供了更好的理論保證。借助CPU 和GPU 的并行計算能力，HyP-DESPOT（Hybrid Parallel DESPOT）［216］進一步 發展了DESPOT，并通過實驗驗證了機器人在行人間進行導航的實時性能。Adaptive Belief Tree（ABT）［214］專門為適應環境變化而設計，使信念樹的搜索過程不必從頭開始。

早期POMDP 文獻主要聚焦于離散狀態空間、離散動作空間和離散觀測空間。但對于機器人任務來講，連續模型無疑是更合理的。一些工作已將基于點的離線規劃方法擴展至連續狀態空間［217-219］和連續觀測空間［218，220］。至于在線規劃，前述方法可容納連續狀態空間，且關于連續動作空間［221］和連續觀測空間［42-43］的有效方法也已提出。其 中POMCPOW［42］和DESPOT-α［43］采用了一種受粒子濾波啟發的加權方案，可較好應對具有連續觀測空間的真實問題。除POMDP表述外，另外還包括結合傳統運動規劃和隨機最優控制的方法［222-225］，但這些工作均基于高斯噪聲假設，存在一定的局限性。

3.2 隱式建模不確定性

隱式方法可用魯棒模型預測控制（Robust Model Predictive Control，RMPC）［226］的框架進行統一描述。其直接考慮不確定性，通過優化選擇最壞情形下的控制策略并同時生成軌跡。更正式地講，優化器是構建了一個反饋，以使在所有可能的不確定性下約束都被滿足，且代價函數被最小化。但由于RMPC 需要對任意函數進行優化，因此計算復雜度很高。而且由于維數詛咒，離散化也不可 行，如Scokaert 和Mayne［227］證明，即使只考慮不確定性集合的極值，線性魯棒模型預測控制的計算復雜度關于預測時域仍是指數級的。對于非線性系統，這一情況則更加嚴重。

上述原因促使研究人員把精力集中在被稱作Tube MPC［226］的解耦策略上，它將RMPC 分解為開環最優控制問題和輔助跟蹤控制器的設計問題，即通過在線求解標稱MPC，并且離線設計魯棒跟蹤控制器來使系統狀態在不確定性作用下，一直保留在以期望軌跡為中心的一個魯棒控制不變（Robust Control Invariant）Tube 內，從而將決策變量由控制策略π（x（t），t）轉變為開環控制輸入u（*t），具體參見圖13（優化器反復求解開環最優軌跡；輔助控制器κ根據當前測量到的系統狀態x、開環軌跡上對應的狀態x*和控制輸入u*，計算實際動作指令u）、圖14（若初始狀態x（t0）位于以開環軌跡為中心的Tube 內，則后續狀態亦不會逃離Tube）。盡管從計算復雜度講這種解耦設計是可行的，但同時也產生了較大的對偶間隙（Duality Gap）［228-229］，Homothetic［230］、Parameteri-zed［231］、Elastic Tube MPC［232］雖致力于緩解該問題，但目前僅適用線性系統。

圖13 Tube MPC 的一般框架Fig.13 General framework for nominal Tube MPC

圖14 Tube 示意Fig.14 Illustration of Tube

由于設計非線性控制器及計算相關不變集的復雜性，非線性Tube MPC 相較線性Tube MPC 面臨著更大的挑戰。不過即使如此，一些方案也已被發展來嘗試解決這個問題，可達性分析便是其中之一。Althoff［233］將非線性視為有界干擾，利用線性化后的動力學方程估計非線性系統可達集，并成功在地面車輛［234］和飛行器［235］上進行了測試。但由于無法保證所有模型的線性部分都占據主導，故這種處理方式有很強的保守性。而HJ 可達性［236］分析借助水平集［237］這一有力工具，在狀態空間的離散網格上求解哈密頓-雅可比-艾薩克（Hamilton-Jacobi-Isaacs，HJI）偏微分方程，可以應用于一般的非線性系統，且能夠表示各種形狀的集合、較容易地處理控制和擾動變量。Chen 和Herbert 等［44，238］利用上述手段分析跟蹤系統和規劃系統間的追逃博弈模型，離線計算并保存兩系統間由干擾和控制約束造成的最大跟蹤誤差，同時將使用簡化模型進行實時規劃的路徑或軌跡規劃器融入其中，發展了一種名為FaSTrack 的快速安全規劃算法（參見圖15）。然而隨著狀態空間維數的增加，HJ 可達性的計算將逐漸變得不可行。Singh 等［239］通過用SOS［206］代替HJ 可達性分析，以期緩 解FaSTrack 在高維空間中的計算復雜性。其他的一些改進措施也已被提出，包括分解方法［240-241］、Warm-start 方法［242］、基于采樣的方法［243］和基于學習的方法［244-246］。

圖15 搭載FaSTrack 算法的無人機運動過程Fig.15 Motion of UAV with FaSTrack

李雅普諾夫函數［247］因不用求解方程便可驗證系統穩定性而在非線性控制中具有重要地位，其水平集可被用來描述魯棒不變Tube，然而為非線性系統計算這樣的函數很有挑戰性。近年得益于凸優化技術的發展，SOS［206］常被用來檢查多項式形式的李雅普諾夫函數的正定性，以近似系統的吸引域。Tedrake 等［248］基于此想法并結合RRT 提出了LQR-Trees 算法，該算法通過建立一個局部穩定控制器樹，可以驅使狀態空間中某些有界區域內的任一初始狀態達到目標，不過并不適于實時任務使用。與LQR-Trees 最大化后向可達集的內部近似不同，Majumdar 和Tedrake［45］利用SOS 技術為標稱軌跡集離線計算最小尺寸的Funnels，然后通過序列組合達到在線避障的目的。但預先指定軌跡庫的做法同時也使標稱軌跡的靈活性受到限制，且每條軌跡的離線計算時間過于漫長（約為20～25 min）。控制李雅普諾夫函數（Control Lyapunov Function，CLF）可以保證漸近穩定性，控制障礙函數（Control Barrier Function，CBF）可以保證安全性，即將狀態維持在一個前向不變集內。針對控制仿射系統，Ames 等［46-47］通過將軟約束處理為前者，將硬約束處理為后者，并同時表示在二次規劃的框架下，也發展出了一種可以保證安全性的在線軌跡跟蹤技術，并被應用于多機器人系統［249］。收縮理論（Contraction Theory）［250-251］是研究非線性系統軌跡對間收斂性質的方法，因其在分析跟蹤控制器的穩定特性時不需提前指定標稱軌跡，故而特別適合于反饋運動規劃問題。Singh 等［48］利用收縮理論得以將開環運動規劃器作為黑箱，并通過SOS 優化離線計算最優控制收縮測度（Control Contraction Metrics，CCMs）（CLF 的推廣概念），從而擴大了優化的可行區域，得到了尺寸固定、界面最小的不變Tube 及與之相關的反饋跟蹤控制器。

至此本節已詳細介紹了兩類針對不確定性的反饋運動規劃算法（參見表4），通過以上討論可知：顯式方法主要通過建立不確定性的概率模型，并進行離散搜索來達到規劃目的，但其計算過程及計算結果顯然受到搜索方式及模型精確程度的影響，目前一些更先進的采樣搜索策略（如重要性采樣方法）已被成功用于加速POMDP的求解［252］，這也將是此類方法未來的發展方向之一。而隱式方法的焦點在于計算非線性系統的魯棒控制不變Tube，不過上述研究均采用了有界干擾假設，從而使得Tube 的計算趨于保守。如果可以從收集到的數據中構造并不斷更新動力學模型，那么將大大提升現有算法的性能。Hewing 等［253］的文章對這一問題進行了詳細討論，此處限于篇幅不再展開。

表4 反饋運動規劃算法總結Table 4 Summary of feedback motion planning algorithms

4 結論

運動規劃是機器人研究中的基礎問題，過去四十年中出現了許多可同時保證求解效率和解路徑（軌跡）質量的算法，包括組合運動規劃、基于采樣的運動規劃、基于優化的運動規劃和反饋運動規劃算法。它們成功解決了一些復雜場景中的規劃問題，顯示了巨大優勢。本文首先介紹了運動規劃問題的內涵和幾種經典的運動規劃算法；隨后按照傳統運動規劃、考慮微分約束的開環運動規劃和反饋運動規劃的順序綜述了大量相關文獻，并重點討論了漸近最（近）優算法的加速策略、不確定性下的反饋規劃、動態環境中的運動規劃、學習算法與運動規劃算法的融合等具有挑戰性的課題。通過以上評述可知，CMP 能嚴格保證算法的完備性，但在面臨高維空間和障礙物數量巨大的場景時卻無能為力，這進一步促使了SBMP 的出現。“采樣”即意味著“離散”，故其優勢正是在于以犧牲算法的完備性為代價，用可數點集或序列及它們間的連接關系來近似C-space 的連通特性。不過早期RRT 類算法將搜索過程完全交由固定搜索域內的隨機采樣點進行引導的做法無疑在無用狀態上浪費了大量時間，當為了得到符合系統運動特性的軌跡而直接考慮微分約束時，這一問題則更加凸顯。因此求解最優運動規劃問題的另一思路是放棄全局搜索，將問題轉換為有限維的非線性參數優化問題，從而借助優化領域豐富的工具進行求解，此類方法的關鍵研究點之一是為算法提供收斂保證。值得注意的是，上面的規劃過程均未考慮現實中的不確定性，而由此產生的跟蹤誤差往往會造成意外碰撞的發生。反饋運動規劃中的不確定性顯式建模方法雖然提供了優美的分析框架，但建立這樣的模型無疑是富有挑戰性的。而隱式建模方法雖然可以保障機器人運動過程中的絕對安全性，但現有方法的計算復雜度和計算結果的保守性仍然較高。因此在綜合考量各類算法優缺點（參見表5）的基礎上，本文歸納了4 個未來值得研究的方向：

表5 各類運動規劃算法的優劣勢總結Table 5 Summary of advantages and disadvantages of various motion planning algorithms

1）運動規劃算法的漸進最優性和實時性之間存在著矛盾。當考慮高維問題和微分約束時，這一矛盾被進一步激化。因此如何在算法設計過程中妥善地融入表2 中的各類已有信息來降低時間復雜度，并提高解的質量，仍是需要深入思考的一個問題。如為BIT*算法設計確定性采樣序列和較好的啟發函數，并與更先進的圖搜索算法（ARA*、D*Lite、AD*等）進行融合；利用可達集及其對應的最優控制律信息引導算法的采樣和局部連接等。

2）學習算法為運動規劃問題提供了一個新的視角。如何在已有不精確模型的基礎上，利用數據緩和開環運動規劃算法中最優性與實時性的矛盾、降低反饋運動規劃的保守性，將是后續研究的重點。如學習算法與無擾可達集計算過程的結合、學習算法與Tube 計算過程的結合［253］等。另外由于學習算法自身的特性，基于學習的運動規劃算法對于環境的遷移性仍是待解決的問題。

3）借助計算機視覺領域的豐富成果，預測動態障礙物的行為或軌跡［130-131］，并與已有運動規劃算法框架進行融合也已成為最近的關注點之一。但除預測未來狀態外，對預測效果的評估也至關重 要。如Fridovich-Keil 等［254］提出的Confidence-aware 方法可使機器人對當前預測模型的準確性進行推理，提高了動態環境中規劃結果的魯棒性。因此如何對軌跡預測模型或行為預測模型的不確定性進行建模也是未來值得研究的問題［255］。

4）運動規劃問題的復雜度與狀態空間的維數緊密相關，而后者隨參與任務的機器人數量的增多而快速增加，將機器人簡單地處理為獨立個體的做法無疑為解決復雜問題設置了阻礙。雖然本文的關注范圍限于單機器人運動規劃問題，但更加耦合的多機器人協同或對抗亦將是未來主要的發展趨勢。