本文章總結(jié)于大疆前技術(shù)總監(jiān)，目前在卡內(nèi)基梅隆大學(xué)讀博的楊碩博士在深藍(lán)學(xué)院的關(guān)于機(jī)器人的帶約束軌跡規(guī)劃的公開(kāi)課演講內(nèi)容。

筆者不是機(jī)器人領(lǐng)域的，因此特地去了解了一下楊碩博士，深感佩服，不僅是他的履歷，更多的是他關(guān)于學(xué)術(shù)上的至臻的追求和執(zhí)著的探索。本次公開(kāi)課可以算是一個(gè)機(jī)器人軌跡規(guī)劃的入門(mén)介紹，同時(shí)也能夠激勵(lì)我們，在科研學(xué)習(xí)中，仰望星空與腳踏實(shí)地。

整個(gè)公開(kāi)課分為以下幾個(gè)部分：

1、復(fù)雜機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃的定義和其中的挑戰(zhàn)

2、基礎(chǔ)知識(shí)

3、軌跡規(guī)劃方法之一：Differential Dynamic Programming

4、軌跡規(guī)劃方法之二：Direct Collocation

5、軌跡規(guī)劃方法之三：Planning As Inference

6、相關(guān)的論文和學(xué)習(xí)資料

原視頻地址：https://www.shenlanxueyuan.com/open/course/83

（以下全文約5000字，建議收藏后配合視頻觀看學(xué)習(xí)~）

?一、復(fù)雜機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃的定義和其中的挑戰(zhàn)

什么是復(fù)雜機(jī)器人？

復(fù)雜的機(jī)器人系統(tǒng)往往為高自由度、欠驅(qū)動(dòng)、非線性系統(tǒng)。所謂欠驅(qū)動(dòng)系統(tǒng)，是輸入比要控制的量少的一類(lèi)典型系統(tǒng)。在機(jī)器人世界中，存在著許許多多欠驅(qū)動(dòng)的機(jī)器人，如多足機(jī)器人等。而如何通過(guò)關(guān)節(jié)的運(yùn)動(dòng)實(shí)現(xiàn)實(shí)現(xiàn)機(jī)器人特定肢體的位移是軌跡規(guī)劃的目標(biāo)。

其中一個(gè)比較大的挑戰(zhàn)是，對(duì)每個(gè)關(guān)節(jié)進(jìn)行獨(dú)立的PID控制不一定能夠?qū)崿F(xiàn)整體的軌跡執(zhí)行目標(biāo)，同時(shí)復(fù)雜機(jī)器人系統(tǒng)會(huì)發(fā)生和環(huán)境的交互，特別是摩擦力和碰撞，而且軌跡規(guī)劃和控制都必須通過(guò)機(jī)載計(jì)算機(jī)實(shí)時(shí)進(jìn)行。

楊碩博士舉了幾個(gè)復(fù)雜機(jī)器人軌跡規(guī)劃的例子。

第一個(gè)是：猴子機(jī)器人。

該機(jī)器人有3個(gè)關(guān)節(jié)，晃動(dòng)過(guò)程中，只有兩個(gè)關(guān)節(jié)有驅(qū)動(dòng)。這是一個(gè)非常典型的欠驅(qū)動(dòng)系統(tǒng)。只控制兩個(gè)關(guān)節(jié)，并不一定能實(shí)現(xiàn)整體的移動(dòng)抓桿的目標(biāo)。因此要考慮整個(gè)系統(tǒng)的軌跡規(guī)劃，軌跡控制器就需要考慮如何通過(guò)控制兩個(gè)關(guān)節(jié)的電機(jī)，考慮上機(jī)器人自身的動(dòng)力學(xué)，來(lái)實(shí)現(xiàn)整個(gè)機(jī)器人的移動(dòng)目標(biāo)。

第二個(gè)是：機(jī)械臂。

左圖是通過(guò)控制上方機(jī)械臂的兩個(gè)關(guān)節(jié)，來(lái)通過(guò)摩擦力使輪子轉(zhuǎn)動(dòng)。右圖是通過(guò)控制上方的機(jī)械臂，來(lái)慢慢放平下方的立方體。

第三個(gè)是：波士頓動(dòng)力的人形機(jī)器人。

楊碩博士的觀察和理解是波士頓動(dòng)力機(jī)器人采用Direct Collocation作為機(jī)器人的運(yùn)動(dòng)規(guī)方法，在線生成軌跡和處理約束。

通過(guò)以上三個(gè)例子，我們大概可以知道復(fù)雜機(jī)器人的軌跡規(guī)劃領(lǐng)域是想要關(guān)注什么樣的機(jī)器人。

如何定義軌跡規(guī)劃，如下圖所示：

上圖是一個(gè)六足機(jī)器人，想要讓這個(gè)機(jī)器人從A點(diǎn)移動(dòng)到B點(diǎn)，如何通過(guò)控制每個(gè)關(guān)節(jié)的控制量，同時(shí)滿足動(dòng)力學(xué)方程和額外的約束，以滿足上述的移動(dòng)目標(biāo)。

而這里的約束可以是：機(jī)器人的關(guān)節(jié)有位置、速度、力矩的限制；機(jī)器人多個(gè)關(guān)節(jié)的運(yùn)動(dòng)可能會(huì)被限制耦合在一起的限制；機(jī)器人一段時(shí)間內(nèi)消耗的能量為定值的限制。

軌跡規(guī)劃的要求是：

1、實(shí)時(shí)。解算運(yùn)行2-5秒時(shí)長(zhǎng)的軌跡的求解速度必須小于0.5秒甚至達(dá)到50Hz，這樣才能做MPC（MPC是模型預(yù)測(cè)控制）。

2、盡量精確地符合約束。所有的等式約束不能有較大的違反值。

3、最好可以解出反饋控制器。反饋控制器可以提高控制穩(wěn)定性。

二、基礎(chǔ)知識(shí)

基礎(chǔ)知識(shí)分為三個(gè)部分進(jìn)行講解：動(dòng)力學(xué)方程、線性化、LQR(線性二次型調(diào)節(jié)器)。

首先是動(dòng)力學(xué)方程，我們利用牛頓定律，將物體的力、位移、速度、加速度等變量構(gòu)建成方程的形式。

最后我們都可以將系統(tǒng)的動(dòng)力學(xué)方程寫(xiě)成如上

的形式。后續(xù)我們并不會(huì)很關(guān)注如何獲取這個(gè)系統(tǒng)動(dòng)力學(xué)方程，而是更多關(guān)注有了這個(gè)方程之后，我們可以做什么。

當(dāng)然楊碩博士也舉了三個(gè)動(dòng)力學(xué)方程的例子，來(lái)展示動(dòng)力學(xué)方程具體是怎么獲得的。

第一個(gè)是下落的球：

這兩個(gè)例子不用細(xì)說(shuō)，大家仔細(xì)看一下題目和公式，有高中物理學(xué)知識(shí)就能看明白。

第二個(gè)是倒立擺小車(chē)：

這個(gè)時(shí)候，想寫(xiě)成一個(gè)比較好看的微分表達(dá)式還是有些難度的，但是仍然是可以寫(xiě)出的。我們推薦密歇根的一個(gè)教學(xué)網(wǎng)站(https://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum§ion=SystemModeling)，上面有這道題的詳細(xì)推導(dǎo)過(guò)程。

第三個(gè)是多足機(jī)器人：

第二個(gè)基礎(chǔ)知識(shí)是線性化：

對(duì)于

我們將右邊一階泰勒展開(kāi)，如上圖，然后進(jìn)行誤差小量代換，就得到

. 參考點(diǎn)為

.

以之前的倒立擺為例，之前倒立擺的動(dòng)力學(xué)方程可以看到，方程里存在非線性的平方項(xiàng)，因此是非線性的。我們?cè)谒牟环€(wěn)定平衡點(diǎn)，即豎直點(diǎn)做一個(gè)線性化，即狀態(tài)取[0,0,pi,0]，此時(shí)

，把這個(gè)小量等式代入動(dòng)力學(xué)方程，化簡(jiǎn)就可以得到如下圖的線性化方程，原方程中的二次項(xiàng)因?yàn)閟inpi = 0而消失。接方程組整理可以得到線性化的動(dòng)力學(xué)方程形式，此處需要讀者自己動(dòng)手帶進(jìn)去算算。

第三個(gè)基礎(chǔ)知識(shí)是二次型調(diào)節(jié)器(LQR):

這一部分如果沒(méi)有相關(guān)背景知識(shí)的學(xué)生可能聽(tīng)起來(lái)非常吃力，筆者推薦一個(gè)博客，大家可以對(duì)為什么要做LQR以及LQR的直觀的motivation做法有個(gè)簡(jiǎn)單認(rèn)識(shí)：

https://blog.csdn.net/tauyangdao/article/details/108058222#31__53?

在不指定優(yōu)化標(biāo)準(zhǔn)的前提下，控制領(lǐng)域中的“最優(yōu)”體現(xiàn)在“輸出能夠完全跟蹤控制，即在每一時(shí)刻輸出量與控制量完全一致”。實(shí)際過(guò)程并不是這樣完美的過(guò)程，每一時(shí)刻都會(huì)存在誤差。退而求其次，追求在整個(gè)工作時(shí)間的范圍內(nèi)誤差最小,與軌跡誤差類(lèi)似，我們研究狀態(tài)誤差。

因此，把整個(gè)工作時(shí)間內(nèi)每一時(shí)刻狀態(tài)的誤差都累加起來(lái)，只要累加值更小，便會(huì)更加接近系統(tǒng)性能的期望。

由上我們通過(guò)一系列推導(dǎo)，可以定義一個(gè)代價(jià)函數(shù)：

?

這個(gè)代價(jià)函數(shù)就將所有的誤差累加起來(lái)，我們?nèi)プ钚』@個(gè)代價(jià)函數(shù)，即化為如下的優(yōu)化問(wèn)題：

這就是著名的連續(xù)時(shí)間無(wú)窮時(shí)域線性二次型調(diào)節(jié)器。

上述的優(yōu)化問(wèn)題的解如下：

最優(yōu)的u就是一個(gè)關(guān)于x的反饋形式。

上述是無(wú)窮時(shí)域的問(wèn)題，當(dāng)轉(zhuǎn)換到有窮時(shí)域，即代價(jià)函數(shù)變?yōu)椋?/p>

?

對(duì)于有窮時(shí)域的優(yōu)化問(wèn)題，解如下：

如上圖所示，和無(wú)窮時(shí)域不同的是，有窮時(shí)域的半正定矩陣S是關(guān)于t的函數(shù)，最優(yōu)解u關(guān)于x的反饋系數(shù)也是關(guān)于t的函數(shù)。

對(duì)于離散化的有窮時(shí)域的優(yōu)化問(wèn)題：

?

可以看出來(lái)最優(yōu)解的形式上和連續(xù)時(shí)間的優(yōu)化問(wèn)題是有很大的相似性。

我們對(duì)上述離散時(shí)間有窮時(shí)域線性二次型調(diào)節(jié)器做一個(gè)總結(jié)：

直觀點(diǎn)以倒立擺為例，我們希望通過(guò)不斷控制F，使得小車(chē)在時(shí)間T內(nèi)，小車(chē)到達(dá)x0位置，并在那里保持穩(wěn)定。離散時(shí)間有窮時(shí)域線性二次型調(diào)節(jié)器可以以上圖所示，在時(shí)刻0的時(shí)候，位置處于x，通過(guò)控制外力

，在時(shí)刻T，到達(dá)x0，

三、軌跡規(guī)劃方法之一：Differential Dynamic Programming

微分動(dòng)態(tài)軌跡規(guī)劃可以這么理解，上一節(jié)講的是二次型調(diào)節(jié)器，基于的是在某個(gè)點(diǎn)對(duì)f函數(shù)進(jìn)行泰勒線性展開(kāi)，那在軌跡規(guī)劃中，在整條軌跡上，我們?cè)谲壽E的每個(gè)點(diǎn)都各自線性展開(kāi)，這樣就可以保持足夠的精度。這就是動(dòng)態(tài)。

我們基于上一節(jié)的離散線性二次型調(diào)節(jié)器重新定義一個(gè)類(lèi)似的問(wèn)題，這個(gè)問(wèn)題正好反過(guò)來(lái)，初始時(shí)間是在x0，通過(guò)控制外力

，在時(shí)刻T，使得x在xd，即

。

則這種情況下，我們的優(yōu)化問(wèn)題變成：

注意到在第N項(xiàng)中，我們期望

是等于

d的。

如本節(jié)開(kāi)始所述，因?yàn)榉蔷€性系統(tǒng)的線性化精度依賴(lài)于參考點(diǎn)，且偏離參考點(diǎn)太遠(yuǎn)則線性化的方程會(huì)不準(zhǔn)確。所以我們需要沿著軌跡進(jìn)行重復(fù)線性化。如下圖所示：

我們每移動(dòng)一點(diǎn)，就進(jìn)行一次線性化，直到到達(dá)目標(biāo)。此時(shí)優(yōu)化的條件函數(shù)中的A和B是時(shí)變的，即隨著位置的變化而變化。

值得一提的是Differential Dynamic Programming (DDP)、Sequential Linear Quadratic (SLQ)和 Iterative Linear Quadratic Regular (iLQR)三個(gè)是等價(jià)的，都是微分動(dòng)態(tài)規(guī)劃。

以iLQR對(duì)上述的微分動(dòng)態(tài)規(guī)劃做一個(gè)總結(jié)。

對(duì)于一個(gè)非線性系統(tǒng)，我們希望規(guī)劃它的軌跡使得它從某個(gè)起始狀態(tài)運(yùn)動(dòng)到某個(gè)目標(biāo)狀態(tài)，并且我們希望得到反饋控制器。我們首先根據(jù)經(jīng)驗(yàn)和對(duì)模型的理解設(shè)定一個(gè)初始控制器，然后用這個(gè)控制器生成初始的軌跡。接著重復(fù)進(jìn)行沿軌跡線性化、解LQR、用LQR的解更新初始的控制器的過(guò)程。

以上圖為例，我們構(gòu)造一個(gè)初始的控制器，通過(guò)系統(tǒng)動(dòng)力學(xué)方程，初始狀態(tài)和代價(jià)函數(shù)，我們就可以依據(jù)初始控制器一步一步解出在該控制器下的軌跡，然后我們按照這個(gè)軌跡走一會(huì)，開(kāi)始循環(huán)構(gòu)造局部的LQR問(wèn)題，即我們?cè)谲壽E上進(jìn)行局部的線性化，接觸LQR的解，更新控制器(外力)，所以這是一個(gè)不斷修正的過(guò)程，最后達(dá)到xd。

通過(guò)仿真，我們舉例如下圖，通過(guò)不斷修正，實(shí)時(shí)規(guī)劃路徑，最終到達(dá)目標(biāo)。

?

?

對(duì)于之前所說(shuō)的猴子機(jī)器人，我們可以讓初始控制器設(shè)為重力，然后通過(guò)微分動(dòng)態(tài)規(guī)劃，不斷修正控制器，即每個(gè)關(guān)節(jié)的力，來(lái)實(shí)現(xiàn)機(jī)器人抓取的功能。

微分動(dòng)態(tài)規(guī)劃的優(yōu)點(diǎn)有：能獲得最優(yōu)軌跡，也能獲得最優(yōu)的反饋控制器；局部LQR問(wèn)題的求解可以并行化，能達(dá)到非常高的求解速度；據(jù)說(shuō)（我自己并沒(méi)有實(shí)驗(yàn)驗(yàn)證過(guò)），比其他方法有更好的數(shù)值精度。

微分動(dòng)態(tài)規(guī)劃的缺點(diǎn)有：需要較好的初始化軌跡；比較難考慮額外的約束。

四、軌跡規(guī)劃方法之二：Direct Collocation

直接配點(diǎn)法，放棄獲得反饋控制器，而是將軌跡上每一時(shí)刻的狀態(tài)和控制量看做一個(gè)非線性?xún)?yōu)化問(wèn)題的決策變量，通過(guò)成熟的非線性?xún)?yōu)化領(lǐng)域的技術(shù)來(lái)處理約束。我們定義如下圖所示的整個(gè)軌跡中的所有狀態(tài)和所有控制，然后定義代價(jià)函數(shù)和約束，來(lái)求解這樣的優(yōu)化問(wèn)題。

以六足機(jī)器人為例，我們將所有的每個(gè)時(shí)刻的狀態(tài)都看作決策變量，如下圖xn表示在第n時(shí)刻的決策變量，如身體的位置，身體的角度，速度，角速度，每個(gè)關(guān)節(jié)的角度等等，然后所有時(shí)刻就有N個(gè)xn這樣的決策變量。

代價(jià)函數(shù)可以設(shè)置為系統(tǒng)速度限制或者關(guān)節(jié)速度限制等等。直接配點(diǎn)法關(guān)鍵在于約束條件。接下來(lái)我們介紹一些常見(jiàn)的約束。

約束一：機(jī)器人的起始姿態(tài)和終止姿態(tài)是給定的，這兩個(gè)姿態(tài)由其他的基于地形的優(yōu)化算法得到。我們可以給定機(jī)器人周?chē)牡匦螆D，優(yōu)化在地形圖上的站立姿態(tài)。

約束二：足尖受到的地面作用力（或者說(shuō)機(jī)器人施加給地面的推力）必須處在一個(gè)摩擦錐內(nèi)，這樣才能避免足端打滑。

約束三：機(jī)器人運(yùn)動(dòng)過(guò)程中，足在swing和stance兩個(gè)模式間切換。Swing mode：足在空中移動(dòng)，足上的受力必為0；Stance mode：足支撐在地面上，足的運(yùn)動(dòng)速度必為0。這兩個(gè)模式導(dǎo)致了足在不同時(shí)刻受到不同的約束。

約束四：對(duì)于運(yùn)動(dòng)軌跡上相鄰的兩個(gè)點(diǎn)，兩者的差必然等于動(dòng)力學(xué)方程在兩個(gè)時(shí)刻之間的積分量，如下式所示。這也是直接配點(diǎn)法的最核心的約束。

在實(shí)際過(guò)程中，我們把軌跡表示成分段多項(xiàng)式，決策變量不是軌跡上的點(diǎn)，而是多項(xiàng)式的系數(shù)，這樣可以大大減少問(wèn)題的維度，也自然地保證了運(yùn)動(dòng)的平滑性，如下圖所示。

將分段配點(diǎn)法應(yīng)用到六足機(jī)器人，可以將決策變量維度和約束維度大大簡(jiǎn)化，計(jì)算時(shí)間也有很大的減少。

?直接配點(diǎn)法的優(yōu)點(diǎn)：可以處理任意高精度的系統(tǒng)動(dòng)力學(xué)方程；可以處理非常復(fù)雜的約束。

直接配點(diǎn)法的缺點(diǎn)：只能解出運(yùn)動(dòng)軌跡，不能獲得反饋控制器；非線性?xún)?yōu)化算法對(duì)于有些問(wèn)題可能非常低效。?

五、軌跡規(guī)劃方法之三：Planning As Inference

Planning As Inference，規(guī)劃即推理，是近年來(lái)發(fā)展的新軌跡規(guī)劃方法，是現(xiàn)代應(yīng)用深度增強(qiáng)學(xué)習(xí)進(jìn)行機(jī)器人控制的主要理論基礎(chǔ)之一，它將軌跡規(guī)劃看做最大后驗(yàn)概率估計(jì)問(wèn)題，建立最優(yōu)控制和概率估計(jì)之間的聯(lián)系。

我們將直接配點(diǎn)法的概念推廣為條件概率分布，如上圖所示，按照筆者的理解，我們把離散的值推廣到概率領(lǐng)域，我們就可以將一個(gè)優(yōu)化問(wèn)題推廣到回歸問(wèn)題上，這種解決回歸問(wèn)題的方式，我們就可以使用最大后驗(yàn)概率估計(jì)，當(dāng)然也就可以使用深度學(xué)習(xí)等方式去做。

我們把軌跡規(guī)劃看作最大后驗(yàn)概率估計(jì)，對(duì)于比較一般性的概率分布，最大后驗(yàn)概率估計(jì)必須要通過(guò)統(tǒng)計(jì)學(xué)的方法獲得，但是如果概率分布都為高斯分布，我們可以用均值和方差來(lái)參數(shù)化高斯分布，從而得到概率分布的解析表達(dá)式。

因子圖(factor graph)是一種常用的處理多個(gè)隨機(jī)變量間的高斯分布的工具，因此我們引入因子圖，如下圖所示，因子圖中的圓圈表示隨機(jī)變量，圓點(diǎn)表示因子。

因子圖中的每一個(gè)因子都是一個(gè)高斯分布，因?yàn)槊總€(gè)因子都是條件概率，所以所有因子總共的概率為因子的積。由下圖中的推導(dǎo)，由于是高斯分布，我們可以把這個(gè)情況下的最大后驗(yàn)概率轉(zhuǎn)化成一個(gè)矩陣的最小二乘問(wèn)題。這個(gè)最小二乘問(wèn)題可以通過(guò)QR分解來(lái)快速求解。

?我們可以將因子圖應(yīng)用于最優(yōu)控制，我們把LQR問(wèn)題表示成因子圖的形式，即將LQR中的系統(tǒng)動(dòng)力學(xué)方程和代價(jià)函數(shù)都轉(zhuǎn)化成條件概率分布，寫(xiě)成因子圖的形式，然后我們就可以通過(guò)這個(gè)因子去求解最優(yōu)反饋控制器。

值得一提的是，Planning As Inference的方法，目前的研究工作比較初步。其他已有的工作沒(méi)有太多考慮約束， 主要討論將概率分布推廣到無(wú)模型、非高斯分布的情況。目前楊碩博士在進(jìn)行有模型、高斯分布下如何考慮更多不同類(lèi)型的約束的研究。

六、相關(guān)的論文和學(xué)習(xí)資料

最后，楊碩博士推薦了一些學(xué)習(xí)資料，分別是：

1、關(guān)于直接配點(diǎn)法

推薦MIT的一個(gè)課程：Underactuated Robotics.

2、?關(guān)于微分動(dòng)態(tài)規(guī)劃

推薦”Control-limited differential dynamic programming”這篇論文。以及https://studywolf.wordpress.com/2016/02/03/the-iterative-linear-quadratic-regulator-method/?這份教材。

3、關(guān)于因子圖

推薦”Factor graphs and GTSAM: A hands-on introduction.”、”Factor graphs for robot perception.”這兩篇文章。

4、楊碩博士的關(guān)于如何使用因子圖求解LQR問(wèn)題的論文：”Equality constrained linear optimal control with factor graphs.”。

大家有什么想法可以評(píng)論區(qū)和我交流喲~