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

當(dāng)機(jī)器人在含有靜態(tài)障礙物或動態(tài)障礙物的未知環(huán)境中工作時，突然出現(xiàn)的障礙物一般只會對之前路徑的一部分產(chǎn)生影響，而剩余部分對于接下來的搜索仍然有效。若能充分利用這一部分有效信息，無疑可以加速重規(guī)劃過程（類似于D*Lite算法和 AD*算法）。ERRT用之前可行路徑的Waypoint進(jìn)行偏置采樣，但每次重新生成采樣樹的方式降低了算法效率。Dy?namic RRT（DRRT）則在ERRT的基礎(chǔ)上融合D*算法的思想，從目標(biāo)構(gòu)型反向搜索可行路徑，且僅丟棄發(fā)生碰撞的構(gòu)型及其子節(jié)點(diǎn)，提高了采樣樹的重建速度（參見圖8）。與DRRT完全刪除被障礙物影響的分枝不同，Mul?tipartite RRT（MP-RRT）依然保留這些不連通分量，并試圖將其重新連接到采樣樹上。Vanden Berg等結(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也已將AD*的思想擴(kuò)展至解決單疑問運(yùn)動規(guī)劃問題的RRT算法中。圖8 DRRT算法流程復(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ù)，并計(jì)算能使智能體達(dá)到這個目標(biāo)的策略或路徑猜想。其中目標(biāo)函數(shù)可以預(yù)先指定，亦可通過學(xué)習(xí)得到（如利用逆強(qiáng)化學(xué)習(xí)技術(shù)等）。更多詳細(xì)內(nèi)容可參考 Lefe?vre和Rudenko等的綜述文章，此處不再贅述。1.2.5 利用學(xué)習(xí)算法加速傳統(tǒng)運(yùn)動規(guī)劃問題的求解過程機(jī)器學(xué)習(xí)方法與傳統(tǒng)運(yùn)動規(guī)劃算法的結(jié)合最近已成為研究人員的關(guān)注熱點(diǎn)，此類方法的優(yōu)勢主要體現(xiàn)在兩個方面：①相較傳統(tǒng)運(yùn)動規(guī)劃算法，能更有效地找到近優(yōu)路徑；②可直接基于原始圖像進(jìn)行路徑規(guī)劃。一些文章已將深度學(xué)習(xí)術(shù)如收縮自編碼器（Contractive Auto En?coder，CAE）、條件變分自編碼器（Condi?tional Variational Auto Encoder，CVAE）、卷積神經(jīng)網(wǎng)絡(luò)（Convolutional Neural Network，CNN）、圖神經(jīng)網(wǎng)絡(luò)（Graph Neural Networks，GNN）及它們的變體成功應(yīng)用于SBMP中，且大多數(shù)結(jié)合方式都聚焦于①用神經(jīng)網(wǎng)絡(luò)編碼C-Space，并改善SBMP的采樣點(diǎn)分布；②直接端到端地生成路徑。Deep Sampling-based Motion Planner（DeepSMP）由編碼器和采樣點(diǎn)生成器構(gòu)成，前者用原始點(diǎn)云數(shù)據(jù)作為CAE的輸入以將障礙物空間編碼為不變的、魯棒的特征空間；后者用RRT*產(chǎn)生的近優(yōu)路徑訓(xùn)練基于Drop?out的統(tǒng)計(jì)前饋深度神經(jīng)網(wǎng)絡(luò)，使其能在給定起始構(gòu)型、目標(biāo)構(gòu)型、障礙物空間編碼信息的情況下，在包含解路徑的構(gòu)型空間區(qū)域內(nèi)為 SBMP迭代生成采樣點(diǎn)。Neural RRT*通過學(xué)習(xí)大量由A*算法生成的最優(yōu)路徑來訓(xùn)練CNN，該模型可為新的路徑規(guī)劃問題快速提供最優(yōu)路徑的預(yù)測概率分布，用于指導(dǎo)RRT*的采樣過程。Ichter等認(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算法的性能。其在另一文章中用以往成功的規(guī)劃結(jié)果和機(jī)器人經(jīng)驗(yàn)訓(xùn)練CVAE，然后根據(jù)給定的初始構(gòu)型、目標(biāo)區(qū)域和障礙物信息，可以對CVAE的隱空間（Latent Space）進(jìn)行采樣，并將其投影到狀態(tài)空間中更有希望包含最優(yōu)路徑的區(qū)域。但這種預(yù)測最短路徑采樣點(diǎn)的做法其實(shí)把所有負(fù)擔(dān)都壓給了 Learner，任何由近似或訓(xùn)練-測試不匹配造成的誤差均可使算法失效。故LEGO提取多條不同近優(yōu)路徑上的瓶頸點(diǎn)，并以其作為CVAE的訓(xùn)練輸入數(shù)據(jù)。針對CNN和全連接網(wǎng)絡(luò)（Fully-Connected Network，F(xiàn)CN）容易丟失環(huán)境結(jié)構(gòu)信息而引起的泛化不良問題，Khan等利用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í)對象級語義信息產(chǎn)生的采樣分布也可以改善未知環(huán)境中的導(dǎo)航結(jié)果。與上述偏置采樣點(diǎn)分布的方法不同，MPNet則直接生成可行近優(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）在運(yùn)動規(guī)劃領(lǐng)域也有一些成功應(yīng)用的案例。Q-function sampling RRT（QSRRT）根據(jù)學(xué)習(xí)到的狀態(tài)-行為值函數(shù)（Qfunction），提出Softmax節(jié)點(diǎn)選擇方法，加速了RRT的規(guī)劃過程并避免陷入局部極小值。Chen等建立了一個基于Tabular Q-learning和值迭代網(wǎng)絡(luò)（Value Iteration Networks，VIN）的可學(xué)習(xí)神經(jīng)擴(kuò)展算子，以為基于樹的運(yùn)動規(guī)劃算法學(xué)習(xí)有潛力的搜索方向。Bhardwaj等將Lazy搜索中邊的選擇問題建模為馬爾可夫決策過程（Markov Decision Process，MDP），并引模仿學(xué)習(xí)（Imitation Learning）進(jìn)行求解。Zhang等利用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)還有待驗(yàn)證。根據(jù)以上關(guān)于傳統(tǒng)運(yùn)動規(guī)劃問題的討論可知：相比CMP，SBMP取得成功的原因在于其以犧牲完備性為代價，換取對復(fù)雜的連通性擁有較強(qiáng)的表示能力（即通過“采樣”的方式構(gòu)建包含解路徑的離散結(jié)構(gòu)），而非最初版本中所帶有的“采樣隨機(jī)性”和“搜索盲目性”。相反該特性恰恰使算法拋棄了大量可用的有效信息，阻滯了性能的進(jìn)一步提升。因此針對不同場景，如何在SBMP算法的設(shè)計(jì)過程中妥善地 融入1.2.1~1.2.5節(jié)的5類信息，從而提高解路徑品質(zhì)并避免在無價值節(jié)點(diǎn)上浪費(fèi)大量計(jì)算時間，將是傳統(tǒng)運(yùn)動規(guī)劃研究中要考慮的重要問題。02　考慮微分約束的開環(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ī)劃在許多情況下可以節(jié)省大量計(jì)算時間，但同時也丟失了完備性和最優(yōu)性保證。更好的選擇是在規(guī)劃過程中直接考慮微分約束，這樣便可得到服從系統(tǒng)自然運(yùn)動特性的軌跡，同時降低反饋控制器的跟蹤誤差。此類問題一般也被稱為 Kinodynamic Motion Plan?ning（KMP）。本質(zhì)上，KMP可被視為經(jīng)典兩點(diǎn)邊值 問題（Two-point Boundary Value Prob?lem，TPBVP）的變體。后者通常是給定初始狀態(tài)和目標(biāo)狀態(tài)的情況下，在狀態(tài)空間中計(jì)算一條連接初末狀態(tài)并滿足微分約束的（最優(yōu)）路徑；而規(guī)劃問題則牽扯到一類附加的復(fù)雜性：避免與狀態(tài)空間中的障礙物發(fā)生碰撞，用控制理論的術(shù)語來講即是為包含非凸?fàn)顟B(tài)約束和控制約束的非線性系統(tǒng)設(shè)計(jì)（最優(yōu)）開環(huán)軌跡。但求解TPBVP的技術(shù)并不能很好地適用于考慮微分約束的運(yùn)動規(guī)劃，因?yàn)槠浔揪筒皇菫樘幚砣终系K物約束而設(shè)計(jì)的，或者說很難得到受非凸?fàn)顟B(tài)和控制約束的非線性系統(tǒng)的最優(yōu)必要條件。文獻(xiàn)中處理KMP的方式一般有基于采樣的方法和基于優(yōu)化的方法兩類。關(guān)于前者，由于微分約束破壞了CMP所需的良好特性，故第1節(jié)中僅有SBMP可能實(shí)現(xiàn)較好移植。關(guān)于后者，其一般有3種應(yīng)用場景：一是在前述解耦規(guī)劃中，用于平滑和縮短由其它規(guī)劃算法（如SBMP）生成的路徑；二是直接從較差初始猜想（可能是與障礙物相交的線段）開始計(jì)算局部最優(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)空間中的某個采樣點(diǎn)要么是不可行的（圖9中的橙色區(qū)域?yàn)闄C(jī)器人有限時間前向可達(dá)集，其在一定時間范圍內(nèi)無法到達(dá)可達(dá)集之外的采樣點(diǎn)），要么則需解決復(fù)雜的TPBVP問題（這亦是很少應(yīng)用路圖類算法的原因，即目前不存在有效的LPM方法），故考慮微分約束的SBMP 通過離散動作空間和時間，并進(jìn)行前向仿真來遞增地生成采樣樹。離散的動作空間和時間其實(shí)暗含著初始狀態(tài)的可達(dá)圖，若狀態(tài)的一階微分函數(shù)滿足Lipschitz條件，則隨著離散度（以概率1）趨于零，動作序列將任意接近相應(yīng)動作軌跡，即可達(dá)圖的節(jié)點(diǎn)將在可達(dá)集中（以概率1）變得稠密，這也是對基于采樣方法的KMP最重要的要求。加之“系統(tǒng)性”搜索，便保證了算法的分辨率（概率）完備概念。圖9 機(jī)器人有限時間前向可達(dá)集RRT與EST最初便是為解決含微分約束的運(yùn)動規(guī)劃問題而設(shè)計(jì)，而非傳統(tǒng)運(yùn)動規(guī)劃。其算法流程與上一節(jié)基本相同，稍微的區(qū)別在于—此處的算法一般在固定的離散動作集中選擇能最小化積分后狀態(tài)與采樣點(diǎn)間距的離散動作，作為樹上新加入的邊所對應(yīng)的控制輸入（積分時間可以固定，也可以在某個區(qū)間內(nèi)隨機(jī)選擇）。盡管RRT為含微分約束的運(yùn)動規(guī)劃問題提供了較好解決方案，但它的缺點(diǎn)是性能對度量函數(shù)較為敏感，差的度量可能導(dǎo)致一些注定發(fā)生碰撞的狀態(tài)和位于可達(dá)集邊界的狀態(tài)被重復(fù)選擇，重復(fù)擴(kuò)展，從而大大增加了運(yùn)行時間，延緩了樹的生長。如圖10（a）所示：橙色為初始狀態(tài)可達(dá)集，而可達(dá)集邊界狀態(tài)（紅色）有較大的Voronoi區(qū)域，故容易被重復(fù)擴(kuò)展；又如圖10（b）：紅色狀態(tài)有較大Voronoi 區(qū)域，但其擴(kuò)展結(jié)果大概率會發(fā)生碰撞。理想距離函數(shù)應(yīng)是滿足某種最優(yōu)性的代價泛函，但除非對于像無約束、有二次形式代價的線性系統(tǒng)存在解析解，一般來講設(shè)計(jì)這樣的偽度量與解決原始運(yùn)動規(guī)劃問題一樣困難。雖然也有學(xué)者提出適應(yīng)于弱非線性系統(tǒng)的近似度量，不過一個更重要的問題是如何令算法在較差的度量函數(shù)下依然有良好的性能？或者說如何令采樣樹在較差的度量函數(shù)下依然能（以概率1）快速稠密地覆蓋初始狀態(tài)的無碰可達(dá)集？另外，引入微分約束也對RRT*提供最優(yōu)性的方式構(gòu)成了挑戰(zhàn)，發(fā)展新的考慮微分約束的漸近最優(yōu)算法是又一亟需解決的問題。圖10 SBMP算法的度量敏感性示意