機器人運動規劃方法綜述（3）

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