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

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

2.航天飛行動(dòng)力學(xué)技術(shù)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，西安 710072

3.之江實(shí)驗(yàn)室，杭州 311121

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

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

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

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

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

表1 運(yùn)動(dòng)規(guī)劃問(wèn)題分類(lèi)［8］Table 1 Classification of motion planning problems［8］

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

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

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

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

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

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

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

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

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

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

對(duì)于給定數(shù)個(gè)起始-目標(biāo)構(gòu)型對(duì)的多疑問(wèn)（Multiple-Query）運(yùn)動(dòng)規(guī)劃問(wèn)題，如果環(huán)境結(jié)構(gòu)不發(fā)生改變，則非常有必要花費(fèi)時(shí)間對(duì)模型進(jìn)行預(yù)計(jì)算以生成路圖，從而提高后續(xù)搜索效率。概率路圖法（Probabilistic Roadmap，PRM）［65］是其中的典型代表，它最初是為應(yīng)對(duì)高自由度運(yùn)動(dòng)規(guī)劃問(wèn)題而發(fā)展的，算法流程如圖3 所示。PRM 構(gòu)建的路圖可看作是對(duì)CMP 所導(dǎo)出路圖的一種近似，因此也常被與CMP 共同歸入基于圖搜索的運(yùn)動(dòng)規(guī)劃算法中［50］。Kavraki 等［66］通過(guò)對(duì)簡(jiǎn)化的PRM（Simplified PRM，s-PRM）進(jìn)行分析，建立了算法失敗概率Pfail與路徑長(zhǎng)度L、路徑和障礙物間距R、采樣點(diǎn)數(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可視特性的ε、α、β越大，那么完成求解所需的采樣點(diǎn)就越少，算法運(yùn)行時(shí)間就越短。因?yàn)橐鹂梢曁匦韵陆档莫M窄通道不可能偶然出現(xiàn)［69］，故實(shí)際中遇到的許多Cfree都具有良好的可視特性，即相應(yīng)的ε、α、β較大。另外可視特性是用體積比率定義的，而不直接依賴(lài)于C-space 維數(shù)，這便解釋了為什么當(dāng)維數(shù)在一定范圍內(nèi)增加時(shí)，PRM 仍能較好地運(yùn)行。

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

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

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

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

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

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

步驟6 返回步驟2。

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

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

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

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

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

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

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

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

SBMP 算法最初都使用了隨機(jī)采樣方式，那么一個(gè)問(wèn)題是：如果以確定性方式進(jìn)行采樣，相關(guān)的理論保證和實(shí)際性能還會(huì)成立嗎？答案是肯定的，確定性規(guī)劃器可以簡(jiǎn)化證明過(guò)程，可以將一部分在線計(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等［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)行對(duì)比實(shí)驗(yàn)后發(fā)現(xiàn)：相較于分辨率完備的確定性采樣方法（包括網(wǎng)格搜索），原始的PRM算法并沒(méi)有體現(xiàn)出優(yōu)勢(shì)。故文章對(duì)“隨機(jī)性是打破高維空間維數(shù)詛咒的必要分量”這一說(shuō)法進(jìn)行了駁斥，事實(shí)上為了維持固定的離散度，任何采樣方案都需要與維數(shù)成指數(shù)關(guān)系的采樣點(diǎn)數(shù)量［90］。但Lavalle 等的工作［88］僅限于討論可行路徑，就收斂到最優(yōu)路徑而言，獨(dú)立同分布采樣是否還有優(yōu)勢(shì)？Jason 等［91］針對(duì)PRM 算法進(jìn)行了討論（設(shè)g1、g2為從自然數(shù)集到實(shí)數(shù)集的映射，約定g1∈O（g2）表示存在某個(gè)自然數(shù)n0和正實(shí)數(shù)m，使得對(duì)于所有n≥n0，|g（n1）|≤m|g（n2）|；g1∈Ω（g2）表示存在某個(gè)自然數(shù)n0和正實(shí)數(shù)m，使得對(duì)于所有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）收斂率，在沒(méi)有障礙物的情況下，采用確定性采樣序列的PRM 的次優(yōu)因子上界為2D（nrn-2Dn），其中Dn是采樣序列的l2離散度（存在障礙物時(shí)的結(jié)果更復(fù)雜一點(diǎn)）。而采用獨(dú)立同分布采樣的類(lèi)似結(jié)果僅是概率成立，且更加繁瑣和難以理解。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

機(jī)器學(xué)習(xí)方法與傳統(tǒng)運(yùn)動(dòng)規(guī)劃算法的結(jié)合最近已成為研究人員的關(guān)注熱點(diǎn)，此類(lèi)方法的優(yōu)勢(shì)主要體現(xiàn)在兩個(gè)方面：①相較傳統(tǒng)運(yùn)動(dòng)規(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 的采樣點(diǎn)分布；②直接端到端地生成路徑。Deep Sampling-based Motion Planner（DeepSMP）［136］由編碼器和采樣點(diǎn)生成器構(gòu)成，前者用原始點(diǎn)云數(shù)據(jù)作為CAE 的輸入以將障礙物空間編碼為不變的、魯棒的特征空間；后者用RRT*產(chǎn)生的近優(yōu)路徑訓(xùn)練基于Dropout［137］的統(tǒng)計(jì)前饋深度神經(jīng)網(wǎng)絡(luò)，使其能在給定起始構(gòu)型、目標(biāo)構(gòu)型、障礙物空間編碼信息的情況下，在包含解路徑的構(gòu)型空間區(qū)域內(nèi)為SBMP迭代生成采樣點(diǎn)。Neural RRT*［138］通過(guò)學(xué)習(xí)大量由A*算法生成的最優(yōu)路徑來(lái)訓(xùn)練CNN，該模型可為新的路徑規(guī)劃問(wèn)題快速提供最優(yōu)路徑的預(yù)測(cè)概率分布，用于指導(dǎo)RRT*的采樣過(guò)程。Ichter 等［139］認(rèn)為解路徑僅依賴(lài)于由C-space 結(jié)構(gòu)決定的幾個(gè)關(guān)鍵構(gòu)型。因此其通過(guò)圖論技術(shù)識(shí)別這些關(guān)鍵構(gòu)型，并僅用局部環(huán)境特征作為CNN的輸入來(lái)學(xué)習(xí)預(yù)測(cè)關(guān)鍵構(gòu)型，從而提升了PRM算法的性能。其在另一文章［18］中用以往成功的規(guī)劃結(jié)果和機(jī)器人經(jīng)驗(yàn)訓(xùn)練CVAE，然后根據(jù)給定的初始構(gòu)型、目標(biāo)區(qū)域和障礙物信息，可以對(duì)CVAE 的隱空間（Latent Space）進(jìn)行采樣，并將其投影到狀態(tài)空間中更有希望包含最優(yōu)路徑的區(qū)域。但這種預(yù)測(cè)最短路徑采樣點(diǎn)的做法其實(shí)把所有負(fù)擔(dān)都?jí)航o了Learner，任何由近似或訓(xùn)練-測(cè)試不匹配造成的誤差均可使算法失效。故LEGO［140］提取多條不同近優(yōu)路徑上的瓶頸點(diǎn)，并以其作為CVAE 的訓(xùn)練輸入數(shù)據(jù)。針對(duì)CNN 和全連接網(wǎng)絡(luò)（Fully-Connected Network，F(xiàn)CN）容易丟失環(huán)境結(jié)構(gòu)信息而引起的泛化不良問(wèn)題，Khan 等［141］利用GNN 的置換不變特性魯棒地編碼C-space 的拓?fù)浣Y(jié)構(gòu)，并計(jì)算采樣分布的參數(shù)以生成采樣點(diǎn)。實(shí)驗(yàn)結(jié)果表明：在學(xué)習(xí)關(guān)鍵采樣點(diǎn)方面，GNN-CVAEs 的表現(xiàn)大大優(yōu)于CNN，而GNN 則優(yōu)于在高維空間中規(guī)劃的FCN 模型。除了用原始點(diǎn)云數(shù)據(jù)和C-space 障礙物信息作為輸入外，利用CNN 學(xué)習(xí)對(duì)象級(jí)語(yǔ)義信息產(chǎn)生的采樣分布也可以改善未知環(huán)境中的導(dǎo)航結(jié)果［142］。與上述偏置采樣點(diǎn)分布的方法不同，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)動(dòng)規(guī)劃領(lǐng)域也有一些成功應(yīng)用的案例。Q-function sampling RRT（QSRRT）［145］根據(jù)學(xué)習(xí)到的狀態(tài)-行為值函數(shù)（Qfunction），提出Softmax 節(jié)點(diǎn)選擇方法，加速了RRT 的規(guī)劃過(guò)程并避免陷入局部極小值。Chen等［146］建立了一個(gè)基于Tabular Q-learning 和值迭代網(wǎng)絡(luò)（Value Iteration Networks，VIN）［147］的可學(xué)習(xí)神經(jīng)擴(kuò)展算子，以為基于樹(shù)的運(yùn)動(dòng)規(guī)劃算法學(xué)習(xí)有潛力的搜索方向。Bhardwaj 等［148］將Lazy搜索中邊的選擇問(wèn)題建模為馬爾可夫決策過(guò)程（Markov Decision Process，MDP），并引入模仿學(xué) 習(xí)（Imitation Learning）［149］進(jìn)行求解。Zhang等［150］利用MDP 建立拒絕采樣模型，并通過(guò)策略梯度方法優(yōu)化采樣分布以降低如碰撞檢測(cè)次數(shù)和樹(shù)的尺寸之類(lèi)的規(guī)劃代價(jià)，從而得以加速現(xiàn)有的最優(yōu)運(yùn)動(dòng)規(guī)劃器。

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

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

2 考慮微分約束的開(kāi)環(huán)運(yùn)動(dòng)規(guī)劃

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

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

2.1 基于采樣方法的KMP

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

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

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

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

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

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

2.1.2 最優(yōu)性問(wèn)題

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.1 顯式建模不確定性

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

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

3.2 隱式建模不確定性

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

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

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

圖14 Tube 示意Fig.14 Illustration of Tube

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

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

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

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

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

4 結(jié)論

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

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

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

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

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

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