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

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