針對(duì)RRT對(duì)度量函數(shù)的敏感性問(wèn)題，Resolution-Complete RRT（RC-RRT）利用Constraint Violation Function（CVF）描述每個(gè)頂點(diǎn)發(fā)生碰撞的概率，并通過(guò)記錄已成功應(yīng)用的動(dòng)作，在避免重復(fù)擴(kuò)展的同時(shí)可以尋找到更有價(jià)值的頂點(diǎn)。RRT-blossom選擇待擴(kuò)展節(jié)點(diǎn)的方式與RC-RRT類似，不同的是RRTblossom同時(shí)應(yīng)用所有可能的行為，并移除碰撞軌跡和回退軌跡。Reachability-Guided RRT（RG-RRT）的顯著特點(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）算法將兩種策略合并。Path-Directed Subdivision Tree（PDST）用路徑段表示“采樣點(diǎn)”，并將“采樣點(diǎn)”按優(yōu)先擴(kuò)展順序排列，同時(shí)用狀態(tài)空間的非均勻細(xì)分來(lái)估計(jì)覆蓋率，從而避免了度量敏感問(wèn)題。GRIP（Greedy，Incre?mental，Path-directed）通過(guò)用簡(jiǎn)單度量代替路徑細(xì)分過(guò)程而進(jìn)一步改進(jìn)了PDST算法，并以此偏置采樣，加之DRRT重復(fù)使用先前信息的優(yōu)勢(shì)，實(shí)現(xiàn)了未知環(huán)境中含微分約束的重規(guī)劃。另一種與PDST類似的方法是 KPIECE（Kinodynamic Planning by Interior-Exterior Cell Exploration），其用網(wǎng)格離散低維投影空間，并標(biāo)記為外胞腔和內(nè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ù)，它們通過(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)和存在較好Steering方法的非完整系統(tǒng)。Karaman和 Frazzoli再次將RRT*算法擴(kuò)展至含微分約束的運(yùn)動(dòng)規(guī)劃問(wèn)題中，并在存在局部最優(yōu)Steering方法的前提下，針對(duì)局部可控系統(tǒng)，提出了保證算法漸進(jìn)最優(yōu)性的充分條件。同樣假設(shè)存在局部最優(yōu)Steering方法，滿足動(dòng)態(tài)環(huán)境中實(shí)時(shí)Kinodynamic 重規(guī)劃的RRTX算法也已被提出。類似于Kim等的工作，LQR-RRT*將基于線性二次調(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ù)各不相同，而且此類軌跡無(wú)法準(zhǔn)確到達(dá)目標(biāo)狀態(tài)，也就無(wú)法確定結(jié)果的最優(yōu)性。Kinodynamic RRT*針對(duì)能控線性系統(tǒng)，利用fixed-final-state-free-final-time控制器來(lái)精確且最優(yōu)地連接狀態(tài)對(duì)，從而滿足了上述充分條件，使算法有了漸進(jìn)最優(yōu)性保證，類似工作還包括 Goretkin等關(guān)于LQR-RRT*算法的改進(jìn)。與傳統(tǒng)運(yùn)動(dòng)規(guī)劃類似，如何利用已有信息改善采樣分布以求加速算法，已經(jīng)成為研究人員關(guān)心的一個(gè)問(wèn)題。Xie等以歐式距離與最大速度的商做為BIT*的保守啟發(fā)式，用序列二次規(guī)劃（Sequential Quadratic Programming，SQP）的變體求解局部BVP，提出了一種有較好性能提升的KMP方法。同時(shí)FMT*的Kinody?namic 版本見(jiàn)于Schmerling等的工作。不考慮微分約束的系統(tǒng)的信息集形成了一個(gè)參數(shù)化的橢圓，直接在該橢圓中采樣可有效降低算法運(yùn)行時(shí)間。但對(duì)含微分約束的系統(tǒng)來(lái)講，顯式表示信息集非常困難，而一般的拒絕采樣方法在高維情況下又效率低下。故針對(duì)存在局部Steering方法的系統(tǒng)，Kunz等提出分層拒絕采樣（Hier?archcal Reject Sampling，HRS）技術(shù)來(lái)緩解這一問(wèn)題。Yi等則將其重新定義為在隱非凸函數(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等利用橢球工具箱離線求解并保存線性系統(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*類方法獲得漸進(jìn)最優(yōu)性的方案很難繼續(xù)應(yīng)用。因此只通過(guò)模型前向仿真以收獲最優(yōu)性的思路便吸引了研究人員的注意力。AO-X算法將最優(yōu)KMP問(wèn)題表述為可行KMP問(wèn)題。首先利用任意可行的KMP算法（文中為RRT和EST）在State-Cost空間中生成可行軌跡，再通過(guò)逐次縮小軌跡代價(jià)的上界以滿足漸近最優(yōu)性要求。且可以證明算法的期望運(yùn)行時(shí)間為，其中是述解軌跡次優(yōu)性的參數(shù)。SST（Stable Sparse RRT）和SST*通過(guò)蒙特卡洛前向傳播（Monte-Carlo Forward Propagation）建立采樣樹(shù)，以修剪樹(shù)上的局部次優(yōu)分枝并維持稀疏結(jié)構(gòu)，分別保證了算法的漸近幾乎最優(yōu)性和漸進(jìn)最優(yōu)性。另外，一些不要求Steering方法的加速方案也已被提出：Dominance-Informed Re?gion Tree（DIRT）引入Dominance Informed Region來(lái)謀求Voronoi偏置與路徑質(zhì)量間的平衡，并采用類似于RRT-blossom的邊擴(kuò)展策略和圖修剪技術(shù)以縮短找到高質(zhì)量解軌跡所需的時(shí)間。同樣在類似于RRT-blossom的邊擴(kuò)展策略的基礎(chǔ)上，Informed SST（iSST）模仿A*引入啟發(fā)式來(lái)起到有序選擇待擴(kuò)展頂點(diǎn)和修剪的作用，提高了有限時(shí)間內(nèi)的求解成功率和相同時(shí)間內(nèi)的路徑質(zhì)量。除上述兩類獲得漸進(jìn)最優(yōu)性的方法外，狀態(tài)柵格（State Lattice）方法也可獲得分辨率下的最優(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ī)劃類似，基于學(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等提出的c2g-HOF（cost to goHigh 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）是MPNet在KMP領(lǐng)域的進(jìn)一步擴(kuò)展，算法框架包括神經(jīng)網(wǎng)絡(luò)生成器（Neural Generator）、神經(jīng)網(wǎng)絡(luò)鑒別器（Neural Discriminator）和并行模型預(yù)測(cè)控制器（Parallel?izable 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等和Faust等合并了PRM 的規(guī)劃效率和RL的魯棒性，并提出由深度確定性策略梯度（Deep Deter?ministic Policy Gradient， DDPG）或連續(xù)動(dòng)作擬合值迭代（Continuous Action Fitted Value It?eration， CAFVI）訓(xùn)練的RL agent決定路圖連通性。結(jié)果表明無(wú)論相比RL agent自身還是傳統(tǒng)的基于采樣的規(guī)劃器，PRM-RL的任務(wù)完成度都有所提高。RL-RRT仍將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）方法由自編碼網(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 La?tent Rapidly-exploring Random Trees）可以有效地、全局地探索學(xué)習(xí)到的流形。CoMPNet（Con?strained Motion Planning Networks）針對(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ì)提升策略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ī)劃算法受益于最優(yōu)控制直接法，即顯式考慮障礙物約束。其將函數(shù)空間中的無(wú)窮維優(yōu)化問(wèn)題轉(zhuǎn)化為有限維非線性參數(shù)優(yōu)化問(wèn)題，在一定意義上可被統(tǒng)一至模型預(yù)測(cè)控制（Model Predic?tive Control，MPC）的框架下（參見(jiàn)圖11，其根據(jù)當(dāng)前測(cè)量到的系統(tǒng)狀態(tài)反復(fù)解一個(gè)開(kāi)環(huán)最優(yōu)控制問(wèn)題，這里為控制量，為系統(tǒng)模型，為擾動(dòng)，和分別為可行的狀態(tài)集合和控制集合，為目標(biāo)狀態(tài)集合，為優(yōu)化指標(biāo)。上標(biāo)“”表示最優(yōu)值，“~”表示標(biāo)稱量，“·”表示導(dǎo)數(shù)）。文獻(xiàn)中目前大致存在類基于優(yōu)化的運(yùn)動(dòng)規(guī)劃算法：圖11 標(biāo)稱MPC的一般框架1）無(wú)約束優(yōu)化方法，其目標(biāo)函數(shù)被由障礙物表示的人工勢(shì)場(chǎng)所增強(qiáng)，或通過(guò)確定性協(xié)變方法，或通過(guò)概率梯度下降方法減小目標(biāo)函數(shù)值。雖不需要高質(zhì)量的初始猜想，但并未從理論上提供收斂保證，而且在有雜亂障礙物的環(huán)境中的失敗率較高，不適用于實(shí)時(shí)運(yùn)動(dòng)規(guī)劃問(wèn)題。2）序列凸規(guī)劃方法，對(duì)有約束的非凸優(yōu)化問(wèn)題來(lái)講，通用類非線性規(guī)劃算法的收斂表現(xiàn)嚴(yán)重依賴于初始猜想，無(wú)法提供收斂保證并提前預(yù)知所需的計(jì)算時(shí)間，很難應(yīng)用于實(shí)時(shí)任務(wù)。而凸優(yōu)化問(wèn)題可保證在多項(xiàng)式時(shí)間內(nèi)可靠地得到全局最優(yōu)解，為借助這一優(yōu)勢(shì)，必須將非凸的最優(yōu)運(yùn)動(dòng)規(guī)劃問(wèn)題進(jìn)行凸化。其中的代表性方法包括 TrajOpt、SCvx、GuSTO，且后兩者提供了理論證明，促進(jìn)了其在實(shí)時(shí)任務(wù)中的 應(yīng)用。此類方法的詳細(xì)介紹可參考Malyuta等的文章，這里不再展開(kāi)。除問(wèn)題的凸化外，該類方法面臨的另一困難則在于如何建立恰當(dāng)?shù)谋苷霞s束。3）凸分解方法，為了避免由避障需求帶來(lái)的非凸約束的影響，凸分解方法通常將已知自由空間分解為一系列重疊的凸胞腔，并保證數(shù)個(gè)插值曲線片段分別位于各凸胞腔內(nèi)以滿足機(jī)器人運(yùn)動(dòng)過(guò)程的安全性要求（參見(jiàn)圖12，其中紫色為障礙物區(qū)域，綠色為已知自由空間分解后的凸胞腔，藍(lán)色為規(guī)劃的軌跡）。Deits和 Tedrake用IRIS（Iterative Regional Inflation by Semidefi?nite Programming）計(jì)算安全凸區(qū)域，由混合整數(shù)優(yōu)化完成多項(xiàng)式軌跡分配，最后利用一種基于平方和（Sums-of-Squares，SOS）規(guī)劃的技術(shù)確保了整個(gè)分段多項(xiàng)式軌跡不發(fā)生碰撞。相比于IRIS，Liu等根據(jù)由JPS（Jump Point Search）算法求得的初始分段路徑建立安全飛行走廊（Safe Flight Corridor， SFC），減少了凸分解的時(shí)間。同時(shí)SFC為二次規(guī)劃（Quadratic Program，QP）過(guò)程提供了一組線性不等式約束，允許進(jìn)行實(shí)時(shí)運(yùn)動(dòng)規(guī)劃。Chen等逐步構(gòu)建基于八叉樹(shù)的環(huán)境表示，并通過(guò)在八叉樹(shù)數(shù)據(jù)結(jié)構(gòu)中的有效操作來(lái)在線生成由大型重疊三維網(wǎng)格組成的自由空間飛行走廊。Gao等則在環(huán)境點(diǎn)云地圖的基礎(chǔ)上，將SFC的構(gòu)建交付于SBMP。除此之外的另一個(gè)關(guān)鍵問(wèn)題是區(qū)間分配（Interval Al?location）方式和時(shí)間分配（Time Allocation）方式，前者決定每個(gè)凸胞腔內(nèi)的軌跡間隔，而后者則處理在每個(gè)間隔上所花費(fèi)的時(shí)間。圖12 凸分解方法示意雖然基于優(yōu)化的運(yùn)動(dòng)規(guī)劃算法采用了3種不同的處理思路，但其本質(zhì)上都是建立在有約束的非線性優(yōu)化問(wèn)題的基礎(chǔ)上的，所以優(yōu)化技術(shù)未來(lái)可預(yù)見(jiàn)的重大進(jìn)展將是此類算法性能提升的主要渠道。表3對(duì)本文所討論的幾種開(kāi)環(huán)最優(yōu)路徑（軌跡）規(guī)劃算法進(jìn)行了總結(jié)，其中和分別代表在路圖上選取鄰域點(diǎn)時(shí)的半徑和數(shù)量，為采樣點(diǎn)數(shù)量，為的維數(shù)，為體積測(cè)度，為維單位球的體積，為最優(yōu)誤差。對(duì)于為最優(yōu)路徑代價(jià)，，而含微分約束的運(yùn)動(dòng)規(guī)劃不需要幾何鄰域的定義。表3 考慮微分約束的最優(yōu)算法總結(jié)