論文導讀：CHOMP——用梯度下降直接把軌跡最佳化到平滑又避障

Matt Zucker, Nathan Ratliff, Anca D. Dragan, Mihail Pivtoraiko, Matthew Klingensmith, Christopher M. Dellin, J. Andrew Bagnell, Siddhartha S. Srinivasa · 2013原文連結

論文資訊卡

標題：CHOMP: Covariant Hamiltonian Optimization for Motion Planning
作者：Matt Zucker、Nathan Ratliff、Anca D. Dragan、Mihail Pivtoraiko、Matthew Klingensmith、Christopher M. Dellin、J. Andrew Bagnell、Siddhartha S. Srinivasa（CMU、Google 等）
發表：The International Journal of Robotics Research (IJRR), 2013（前身為 2009 年 ICRA 論文）

問題背景

RRT 這類取樣式規劃器擅長在高維、含障礙物的組態空間裡「找到一條可行路徑」，但取樣式方法產生的原始路徑通常是曲折、不平滑的折線，直接拿去驅動機械手臂或機器人執行，會產生不自然、不必要的加減速與抖動，實務上幾乎都需要額外的後處理平滑化。CHOMP 從完全不同的思路切入：與其先靠取樣找到一條可行路徑再去平滑化，不如直接把整條軌跡當成一個連續函數，用最佳化的方式，同時對平滑度與避障這兩個目標做聯合最佳化。

方法核心：泛函梯度下降

CHOMP 定義一個目標函數，由兩部分加權組合而成：平滑度成本（懲罰軌跡上速度、加速度過大的變化，讓軌跡本身更順暢）與避障成本（懲罰軌跡上任何一點跟障礙物的距離過近，距離用一個事先計算好的符號距離場（signed distance field）快速查詢）。CHOMP 用泛函梯度（functional gradient）技巧，把整條軌跡（而不是軌跡上單一個點）視為最佳化問題裡的變數，計算目標函數對整條軌跡的梯度方向，再讓軌跡沿著這個梯度方向整體變形，逐步降低成本。

這個方法的一個關鍵優勢是軌跡的重新參數化不變性（reparametrization invariance，這也是論文標題裡「Covariant」的來源）——不管軌跡用什麼樣的時間或弧長參數化方式表示，最佳化過程算出的梯度方向在幾何上是一致的，這讓演算法的行為不會因為軌跡離散化的方式不同而產生不一致的結果。CHOMP 可以用一條初始猜測軌跡（甚至是完全不可行、直接穿過障礙物的直線軌跡）啟動最佳化流程，透過反覆的梯度下降迭代，逐步把軌跡「推」到既平滑又避開障礙物的可行解，而不需要像取樣式方法那樣先靠隨機探索找到可行路徑。為了緩解梯度下降容易卡進高成本局部極小值的問題，論文額外採用 Hamiltonian Monte Carlo 技巧，讓最佳化過程在合理範圍內帶有一定隨機性，藉此提升逃離劣質局部解的能力，同時維持機率完備性。

實驗結果

論文在七自由度機械手臂的操作任務，以及崎嶇地形四足機器人的步態軌跡規劃任務上都做了驗證，顯示 CHOMP 這套泛函梯度最佳化框架不只適用於靜態機械手臂的避障路徑規劃，也能延伸應用到需要考慮動態穩定性的足式機器人運動軌跡最佳化，展現了這套方法在不同機器人形態與任務類型之間的通用性。

影響與定位

CHOMP 代表的「軌跡最佳化」（trajectory optimization）路線，跟 RRT/RRT* 代表的「取樣式規劃」路線，是機器人運動規劃領域兩條並行發展的技術脈絡：取樣式方法的優勢是機率完備性與對複雜組態空間的探索能力，軌跡最佳化方法的優勢是能直接產生平滑、符合動力學限制的高品質軌跡。實務上這兩條路線經常被組合使用——先用取樣式方法快速找到一條可行的粗略路徑，再用 CHOMP 或後續發展的 TrajOpt、STOMP 這類軌跡最佳化方法把這條路徑精煉成平滑可執行的最終軌跡，這種「取樣找可行解、最佳化做精煉」的兩階段架構，至今仍是機械手臂運動規劃管線裡常見的設計模式。