機(jī)器人運(yùn)動規(guī)劃方法綜述

唐永興，朱戰(zhàn)霞，*，張紅文，羅建軍，袁建平

1.西北工業(yè)大學(xué) 航天學(xué)院，西安 710072

2.航天飛行動力學(xué)技術(shù)國家級重點實驗室，西安 710072

3.之江實驗室，杭州 311121

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

圖1 典型的機(jī)器人系統(tǒng)Fig.1 Typical robotic systems

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

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

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

表1 運(yùn)動規(guī)劃問題分類［8］Table 1 Classification of motion planning problems［8］

1 傳統(tǒng)運(yùn)動規(guī)劃

設(shè)G（V，E）為映射到Cfree的拓?fù)鋱D（可參考圖3～圖6），其中V為圖中節(jié)點的集合，每個節(jié)點相對應(yīng)一個構(gòu)型q。E為圖中邊的集合，每條邊相對應(yīng)兩個構(gòu)型間的無碰撞路徑。對于所有edge∈E，定義

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

1.1 組合運(yùn)動規(guī)劃

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

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

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

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

1.2 基于采樣的運(yùn)動規(guī)劃

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

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

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

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

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

步驟1 初始化，無向拓?fù)鋱DG（V，E）的節(jié)點集V初始時一般包含起始構(gòu)型qI和目標(biāo)構(gòu)型qG。

步驟2 節(jié)點選擇方法（Node Selection Method，NSM），選擇一 個節(jié)點qcur∈V進(jìn) 行擴(kuò)展。

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

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

步驟5 對解進(jìn)行檢驗，確定G是否已經(jīng)編碼了一條解路徑。

步驟6 返回步驟2。

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

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

在大多數(shù)應(yīng)用中，解路徑的品質(zhì)［76］亦十分重要，一般除可行性外還存在最優(yōu)性要求，如最短長度路徑、最短時間路徑等。但可以證明，標(biāo)準(zhǔn)的PRM 算法和RRT 算法均不具備漸進(jìn)最優(yōu)性［77］。經(jīng)典方法是利用啟發(fā)式圖搜索算法提供最優(yōu)性保證，不過這種最優(yōu)性受限于網(wǎng)格分辨率，且最壞情況下的運(yùn)行時間隨空間維數(shù)呈指數(shù)增長。Karaman 和Frazzoli［77］通過修 改鄰近點選取方法和局部節(jié)點連接方式（在文章中分別對應(yīng)Near Vertices 和Rewire），提出了概率完備的漸進(jìn)最優(yōu)算法—PRM*、快速擴(kuò)展隨機(jī)圖（Rapidlyexploring Random Graph，RRG）和RRT*（算法流程參見圖5［78］和 圖6［78］），且后兩種算法具有Anytime 特性。其雖不能完全克服經(jīng)典方法的缺點，但的確在當(dāng)時提供了一個保證算法漸進(jìn)最優(yōu)性的新視角。以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）是采樣離散度的函數(shù)，并隨采樣點數(shù)量n的增加而減小，d為構(gòu)型空間維數(shù)，γ是與環(huán)境相關(guān)的參數(shù)。在Rewire 步驟中，首先將qnew連接至能為其提供更低代價的鄰近節(jié)點上，其次若qnew能為剩余某些鄰近節(jié)點提供更低的代價，則將該鄰近節(jié)點的父節(jié)點設(shè)置為qnew。一些關(guān)于PRM*和RRT*理論分析的改進(jìn)近來也已被提出［79-81］。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.2.4 重復(fù)使用之前有效的搜索信息并降低重規(guī)劃的頻率

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

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

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

1.2.5 利用學(xué)習(xí)算法加速傳統(tǒng)運(yùn)動規(guī)劃問題的求解過程

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

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

綜上，盡管基于學(xué)習(xí)的技術(shù)在運(yùn)動規(guī)劃領(lǐng)域取得了一些進(jìn)步，但此類方法在未經(jīng)歷環(huán)境中的性能表現(xiàn)還有待驗證。

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

2 考慮微分約束的開環(huán)運(yùn)動規(guī)劃

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

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

2.1 基于采樣方法的KMP

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

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

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

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

2.1.1 度量函數(shù)敏感性問題

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

2.1.2 最優(yōu)性問題

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

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

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

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

2.1.3 考慮微分約束的基于學(xué)習(xí)的運(yùn)動規(guī)劃方法

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

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

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

2.2 基于優(yōu)化的運(yùn)動規(guī)劃算法

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

圖11 標(biāo)稱MPC 的一般框架Fig.11 General framework for nominal MPC

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

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

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

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

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

表3 考慮微分約束的最優(yōu)算法總結(jié)［78］Table 3 Summary of optimal algorithms considering differential constraints［78］

3 反饋運(yùn)動規(guī)劃

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

3.1 顯式建模不確定性

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

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

3.2 隱式建模不確定性

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

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

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

圖14 Tube 示意Fig.14 Illustration of Tube

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

圖15 搭載FaSTrack 算法的無人機(jī)運(yùn)動過程Fig.15 Motion of UAV with FaSTrack

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

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

表4 反饋運(yùn)動規(guī)劃算法總結(jié)Table 4 Summary of feedback motion planning algorithms

4 結(jié)論

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

表5 各類運(yùn)動規(guī)劃算法的優(yōu)劣勢總結(jié)Table 5 Summary of advantages and disadvantages of various motion planning algorithms

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

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

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

4）運(yùn)動規(guī)劃問題的復(fù)雜度與狀態(tài)空間的維數(shù)緊密相關(guān)，而后者隨參與任務(wù)的機(jī)器人數(shù)量的增多而快速增加，將機(jī)器人簡單地處理為獨(dú)立個體的做法無疑為解決復(fù)雜問題設(shè)置了阻礙。雖然本文的關(guān)注范圍限于單機(jī)器人運(yùn)動規(guī)劃問題，但更加耦合的多機(jī)器人協(xié)同或?qū)挂鄬⑹俏磥碇饕陌l(fā)展趨勢。