公開課精華 | 機械人的帶約束軌跡規劃
本文章總結於大疆前技術總監，目前在卡內基梅隆大學讀博的楊碩博士在深藍學院的關於機械人的帶約束軌跡規劃的公開課演講內容。
——–全文約5000字——–
筆者不是機械人領域的，因此特地去了解了一下楊碩博士，深感佩服，不僅是他的履歷，更多的是他關於學術上的至臻的追求和執着的探索。本次公開課可以算是一個機械人軌跡規劃的入門介紹，同時也能夠激勵我們，在科研學習中，仰望星空與腳踏實地。
整個公開課分為以下幾個部分：
1、複雜機械人運動軌跡規劃的定義和其中的挑戰
2、基礎知識
3、軌跡規劃方法之一：Differential Dynamic Programming
4、軌跡規劃方法之二：Direct Collocation
5、軌跡規劃方法之三：Planning As Inference
6、相關的論文和學習資料
複雜機械人運動軌跡規劃的定義和其中的挑戰
什麼是複雜機械人？
複雜的機械人系統往往為高自由度、欠驅動、非線性系統。所謂欠驅動系統，是輸入比要控制的量少的一類典型系統。在機械人世界中，存在着許許多多欠驅動的機械人，如多足機械人等。而如何通過關節的運動實現實現機械人特定肢體的位移是軌跡規劃的目標。
其中一個比較大的挑戰是，對每個關節進行獨立的PID控制不一定能夠實現整體的軌跡執行目標，同時複雜機械人系統會發生和環境的交互，特別是摩擦力和碰撞，而且軌跡規劃和控制都必須通過機載計算機實時進行。
楊碩博士舉了幾個複雜機械人軌跡規劃的例子。
第一個是：猴子機械人。
該機械人有3個關節，晃動過程中，只有兩個關節有驅動。這是一個非常典型的欠驅動系統。只控制兩個關節，並不一定能實現整體的移動抓桿的目標。因此要考慮整個系統的軌跡規劃，軌跡控制器就需要考慮如何通過控制兩個關節的電機，考慮上機械人自身的動力學，來實現整個機械人的移動目標。
第二個是：機械臂。
左圖是通過控制上方機械臂的兩個關節，來通過摩擦力使輪子轉動。右圖是通過控制上方的機械臂，來慢慢放平下方的立方體。
第三個是：波士頓動力的人形機械人。
楊碩博士的觀察和理解是波士頓動力機械人採用Direct Collocation作為機械人的運動規方法，在線生成軌跡和處理約束。
通過以上三個例子，我們大概可以知道複雜機械人的軌跡規劃領域是想要關注什麼樣的機械人。
如何定義軌跡規劃，如下圖所示：
上圖是一個六足機械人，想要讓這個機械人從A點移動到B點，如何通過控制每個關節的控制量，同時滿足動力學方程和額外的約束，以滿足上述的移動目標。
而這裡的約束可以是：機械人的關節有位置、速度、力矩的限制；機械人多個關節的運動可能會被限制耦合在一起的限制；機械人一段時間內消耗的能量為定值的限制。
軌跡規劃的要求是：
1、實時。解算運行2-5秒時長的軌跡的求解速度必須小於0.5秒甚至達到50Hz，這樣才能做MPC（MPC是模型預測控制）。
2、盡量精確地符合約束。所有的等式約束不能有較大的違反值。
3、最好可以解出反饋控制器。反饋控制器可以提高控制穩定性。
基礎知識
基礎知識分為三個部分進行講解：動力學方程、線性化、LQR(線性二次型調節器)。
首先是動力學方程，我們利用牛頓定律，將物體的力、位移、速度、加速度等變量構建成方程的形式。
最後我們都可以將系統的動力學方程寫成如上
的形式。後續我們並不會很關注如何獲取這個系統動力學方程，而是更多關注有了這個方程之後，我們可以做什麼。
當然楊碩博士也舉了三個動力學方程的例子，來展示動力學方程具體是怎麼獲得的。
第一個是下落的球：
這兩個例子不用細說，大家仔細看一下題目和公式，有高中物理學知識就能看明白。
第二個是倒立擺小車：
這個時候，想寫成一個比較好看的微分表達式還是有些難度的，但是仍然是可以寫出的。我們推薦密歇根的一個教學網站(//ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum§ion=SystemModeling)，上面有這道題的詳細推導過程。
第三個是多足機械人：
這是一個更複雜的系統，我們要考慮機械人系統的位置p、角度是θ、每個關節的角度q以及地面對每個腳尖的反作用力。這個動力學方程就會有很高的維度，分析起來就會有一定的難度。關於這個，可以具體查看Dai,Hongkai的論文(Dai, Hongkai, Andrés Valenzuela, and Russ Tedrake. “Whole-body motion planning with centroidal dynamics and full kinematics.” 2014 IEEE-RAS International Conference on Humanoid Robots. IEEE, 2014.)。
第二個基礎知識是線性化：
>>點擊查看大圖<<
對於
我們將右邊一階泰勒展開，如上圖，然後進行誤差小量代換，就得到
. 參考點為
.
以之前的倒立擺為例，之前倒立擺的動力學方程可以看到，方程里存在非線性的平方項，因此是非線性的。我們在它的不穩定平衡點，即豎直點做一個線性化，即狀態取[0,0,pi,0]，此時
，把這個小量等式代入動力學方程，化簡就可以得到如下圖的線性化方程，原方程中的二次項因為sinpi = 0而消失。接方程組整理可以得到線性化的動力學方程形式，此處需要讀者自己動手帶進去算算。
第三個基礎知識是二次型調節器(LQR):
這一部分如果沒有相關背景知識的學生可能聽起來非常吃力，筆者推薦一個博客，大家可以對為什麼要做LQR以及LQR的直觀的motivation做法有個簡單認識：
//blog.csdn.net/tauyangdao/article/details/108058222#31__53
在不指定優化標準的前提下，控制領域中的「最優」體現在「輸出能夠完全跟蹤控制，即在每一時刻輸出量與控制量完全一致」。實際過程並不是這樣完美的過程，每一時刻都會存在誤差。退而求其次，追求在整個工作時間的範圍內誤差最小,與軌跡誤差類似，我們研究狀態誤差。
因此，把整個工作時間內每一時刻狀態的誤差都累加起來，只要累加值更小，便會更加接近系統性能的期望。
由上我們通過一系列推導，可以定義一個代價函數：
這個代價函數就將所有的誤差累加起來，我們去最小化這個代價函數，即化為如下的優化問題：
這就是著名的連續時間無窮時域線性二次型調節器。
上述的優化問題的解如下：
>>點擊查看大圖<<
最優的u就是一個關於x的反饋形式。
上述是無窮時域的問題，當轉換到有窮時域，即代價函數變為：
對於有窮時域的優化問題，解如下：
>>點擊查看大圖<<
如上圖所示，和無窮時域不同的是，有窮時域的半正定矩陣S是關於t的函數，最優解u關於x的反饋係數也是關於t的函數。
對於離散化的有窮時域的優化問題：
>>點擊查看大圖<<
可以看出來最優解的形式上和連續時間的優化問題是有很大的相似性。
我們對上述離散時間有窮時域線性二次型調節器做一個總結：
>>點擊查看大圖<<
直觀點以倒立擺為例，我們希望通過不斷控制F，使得小車在時間T內，小車到達x0位置，並在那裡保持穩定。離散時間有窮時域線性二次型調節器可以以上圖所示，在時刻0的時候，位置處於x，通過控制外力
，在時刻T，到達x0，
。
軌跡規劃方法之一：Differential Dynamic Programming
微分動態軌跡規劃可以這麼理解，上一節講的是二次型調節器，基於的是在某個點對f函數進行泰勒線性展開，那在軌跡規劃中，在整條軌跡上，我們在軌跡的每個點都各自線性展開，這樣就可以保持足夠的精度。這就是動態。
我們基於上一節的離散線性二次型調節器重新定義一個類似的問題，這個問題正好反過來，初始時間是在x0，通過控制外力
，在時刻T，使得x在xd，即
。
則這種情況下，我們的優化問題變成：
注意到在第N項中，我們期望
是等於
d的。
如本節開始所述，因為非線性系統的線性化精度依賴於參考點，且偏離參考點太遠則線性化的方程會不準確。所以我們需要沿着軌跡進行重複線性化。如下圖所示：
>>點擊查看大圖<<
我們每移動一點，就進行一次線性化，直到到達目標。此時優化的條件函數中的A和B是時變的，即隨着位置的變化而變化。
值得一提的是Differential Dynamic Programming (DDP)、Sequential Linear Quadratic (SLQ)和 Iterative Linear Quadratic Regular (iLQR)三個是等價的，都是微分動態規劃。
以iLQR對上述的微分動態規劃做一個總結。
對於一個非線性系統，我們希望規劃它的軌跡使得它從某個起始狀態運動到某個目標狀態，並且我們希望得到反饋控制器。我們首先根據經驗和對模型的理解設定一個初始控制器，然後用這個控制器生成初始的軌跡。接着重複進行沿軌跡線性化、解LQR、用LQR的解更新初始的控制器的過程。
>>點擊查看大圖<<
以上圖為例，我們構造一個初始的控制器，通過系統動力學方程，初始狀態和代價函數，我們就可以依據初始控制器一步一步解出在該控制器下的軌跡，然後我們按照這個軌跡走一會，開始循環構造局部的LQR問題，即我們在軌跡上進行局部的線性化，接觸LQR的解，更新控制器(外力)，所以這是一個不斷修正的過程，最後達到xd。
通過仿真，我們舉例如下圖，通過不斷修正，實時規劃路徑，最終到達目標。
對於之前所說的猴子機械人，我們可以讓初始控制器設為重力，然後通過微分動態規劃，不斷修正控制器，即每個關節的力，來實現機械人抓取的功能。
微分動態規劃的優點有：能獲得最優軌跡，也能獲得最優的反饋控制器；局部LQR問題的求解可以並行化，能達到非常高的求解速度；據說（我自己並沒有實驗驗證過），比其他方法有更好的數值精度。
微分動態規劃的缺點有：需要較好的初始化軌跡；比較難考慮額外的約束。
軌跡規劃方法之二：Direct Collocation
直接配點法，放棄獲得反饋控制器，而是將軌跡上每一時刻的狀態和控制量看做一個非線性優化問題的決策變量，通過成熟的非線性優化領域的技術來處理約束。我們定義如下圖所示的整個軌跡中的所有狀態和所有控制，然後定義代價函數和約束，來求解這樣的優化問題。
以六足機械人為例，我們將所有的每個時刻的狀態都看作決策變量，如下圖xn表示在第n時刻的決策變量，如身體的位置，身體的角度，速度，角速度，每個關節的角度等等，然後所有時刻就有N個xn這樣的決策變量。
代價函數可以設置為系統速度限制或者關節速度限制等等。直接配點法關鍵在於約束條件。接下來我們介紹一些常見的約束。
約束一：機械人的起始姿態和終止姿態是給定的，這兩個姿態由其他的基於地形的優化算法得到。我們可以給定機械人周圍的地形圖，優化在地形圖上的站立姿態。
約束二：足尖受到的地面作用力（或者說機械人施加給地面的推力）必須處在一個摩擦錐內，這樣才能避免足端打滑。
約束三：機械人運動過程中，足在swing和stance兩個模式間切換。Swing mode：足在空中移動，足上的受力必為0；Stance mode：足支撐在地面上，足的運動速度必為0。這兩個模式導致了足在不同時刻受到不同的約束。
約束四：對於運動軌跡上相鄰的兩個點，兩者的差必然等於動力學方程在兩個時刻之間的積分量，如下式所示。這也是直接配點法的最核心的約束。
在實際過程中，我們把軌跡表示成分段多項式，決策變量不是軌跡上的點，而是多項式的係數，這樣可以大大減少問題的維度，也自然地保證了運動的平滑性，如下圖所示。
將分段配點法應用到六足機械人，可以將決策變量維度和約束維度大大簡化，計算時間也有很大的減少。
直接配點法的優點：可以處理任意高精度的系統動力學方程；可以處理非常複雜的約束。
直接配點法的缺點：只能解出運動軌跡，不能獲得反饋控制器；非線性優化算法對於有些問題可能非常低效。
軌跡規劃方法之三：Planning As Inference
Planning As Inference，規劃即推理，是近年來發展的新軌跡規劃方法，是現代應用深度增強學習進行機械人控制的主要理論基礎之一，它將軌跡規劃看做最大後驗概率估計問題，建立最優控制和概率估計之間的聯繫。
我們將直接配點法的概念推廣為條件概率分佈，如上圖所示，按照筆者的理解，我們把離散的值推廣到概率領域，我們就可以將一個優化問題推廣到回歸問題上，這種解決回歸問題的方式，我們就可以使用最大後驗概率估計，當然也就可以使用深度學習等方式去做。
我們把軌跡規劃看作最大後驗概率估計，對於比較一般性的概率分佈，最大後驗概率估計必須要通過統計學的方法獲得，但是如果概率分佈都為高斯分佈，我們可以用均值和方差來參數化高斯分佈，從而得到概率分佈的解析表達式。
因子圖(factor graph)是一種常用的處理多個隨機變量間的高斯分佈的工具，因此我們引入因子圖，如下圖所示，因子圖中的圓圈表示隨機變量，圓點表示因子。
因子圖中的每一個因子都是一個高斯分佈，因為每個因子都是條件概率，所以所有因子總共的概率為因子的積。由下圖中的推導，由於是高斯分佈，我們可以把這個情況下的最大後驗概率轉化成一個矩陣的最小二乘問題。這個最小二乘問題可以通過QR分解來快速求解。
我們可以將因子圖應用於最優控制，我們把LQR問題表示成因子圖的形式，即將LQR中的系統動力學方程和代價函數都轉化成條件概率分佈，寫成因子圖的形式，然後我們就可以通過這個因子去求解最優反饋控制器。
值得一提的是，Planning As Inference的方法，目前的研究工作比較初步。其他已有的工作沒有太多考慮約束， 主要討論將概率分佈推廣到無模型、非高斯分佈的情況。目前楊碩博士在進行有模型、高斯分佈下如何考慮更多不同類型的約束的研究。
相關的論文和學習資料
最後，楊碩博士推薦了一些學習資料，分別是：
1、關於直接配點法
推薦MIT的一個課程：Underactuated Robotics.
2、 關於微分動態規劃
推薦」Control-limited differential dynamic programming」這篇論文。以及//studywolf.wordpress.com/2016/02/03/the-iterative-linear-quadratic-regulator-method/ 這份教材。
3、關於因子圖
推薦」Factor graphs and GTSAM: A hands-on introduction.」、」Factor graphs for robot perception.」這兩篇文章。
4、楊碩博士的關於如何使用因子圖求解LQR問題的論文：」Equality constrained linear optimal control with factor graphs.」。
感謝雍洋同學對本次公開課內容的整理，同時感謝分享嘉賓楊碩博士的審核
