柔性關(guān)節(jié)機械臂最優(yōu)控制方法研究

崔士鵬， 孫永軍， 劉伊威， 劉宏

(哈爾濱工業(yè)大學(xué) 機器人技術(shù)與系統(tǒng)國家重點實驗室，哈爾濱 150001)

0 引 言

隨著機器人應(yīng)用在智能制造、醫(yī)療康復(fù)、助老助殘服務(wù)、教育娛樂等領(lǐng)域的不斷擴展，以實現(xiàn)人機共融為設(shè)計目標(biāo)的柔性關(guān)節(jié)機械臂近年來備受世界各國研究人員的關(guān)注。目前，柔性關(guān)節(jié)機械臂主要包括2類：基于串聯(lián)彈性驅(qū)動器(series elastic actuator,SEA)的柔性關(guān)節(jié)機械臂和基于可變剛度驅(qū)動器(variable stiffness actuator,VSA)的柔性關(guān)節(jié)機械臂。針對柔性關(guān)節(jié)機械臂的位置控制、軌跡跟蹤控制和阻抗/導(dǎo)納控制，學(xué)者們已經(jīng)開展了大量的研究工作。但這些傳統(tǒng)的控制方法無法利用柔性關(guān)節(jié)機械臂的固有柔順性使機械臂的運動性能達(dá)到最佳。此外，對于每個關(guān)節(jié)具有多個驅(qū)動單元的VSA機械臂來說，傳統(tǒng)的控制方法更是無法確定如何進行驅(qū)動單元的控制分配以實現(xiàn)機械臂在執(zhí)行任務(wù)時表現(xiàn)出最優(yōu)的位置、力或阻抗特性。以目標(biāo)函數(shù)為驅(qū)動的最優(yōu)控制方法為解決上述問題提供了基本思路。

?zparpucu M.C[1]、Chen L[2]等人由極大值原理計算出使SEA在固定時間點處達(dá)到最大連桿速度的電機端控制力矩的解析解；針對VSA的速度最優(yōu)[3-4]、能耗最優(yōu)[5]控制問題，Garabini M、Haddadin S等人通過打靶法得到了原最優(yōu)控制問題的數(shù)值近似解；張世杰[6]運用多重打靶法對高速重載碼垛機器人能耗最優(yōu)軌跡規(guī)劃及運動控制問題進行數(shù)值求解，得到了機器人能耗最優(yōu)軌跡；Patel A[7]通過正交配置法將機械臂的最優(yōu)軌跡規(guī)劃問題轉(zhuǎn)化為非線性規(guī)劃(NLP)問題，以便利用成熟的NLP算法進行求解；胡友忠[8]提出了一種給定運動時間的機械臂能量最優(yōu)軌跡規(guī)劃方法，通過動態(tài)規(guī)劃算法在時間軸上進行搜索得到了能量最優(yōu)軌跡；針對存在耦合交聯(lián)不確定性的可重構(gòu)機械臂系統(tǒng)分散控制問題，劉富[9]等人提出一種基于自適應(yīng)動態(tài)規(guī)劃的能耗保代價最優(yōu)控制方法；Pham Q[10]等人采用基于極大值原理的數(shù)值積分法，解決了存在驅(qū)動飽和約束下的冗余機器人時間最優(yōu)軌跡規(guī)劃問題；此外，Kim J[11]、Machmudah A[12]等人基于粒子群優(yōu)化算法研究了機械臂的最優(yōu)軌跡規(guī)劃問題。

分析上述國內(nèi)外研究現(xiàn)狀可以發(fā)現(xiàn)，現(xiàn)有關(guān)于機器人最優(yōu)控制方法的研究主要是圍繞剛性機器人或柔性關(guān)節(jié)展開的，關(guān)于柔性關(guān)節(jié)機械臂最優(yōu)控制方法的研究成果較少，柔性關(guān)節(jié)機械臂通常會受到控制量、狀態(tài)量，甚至是控制量/狀態(tài)量復(fù)雜耦合形式下的不等式約束的限制。但目前關(guān)于機器人最優(yōu)控制方法的研究大多不考慮不等式約束或僅考慮狀態(tài)量不等式約束，針對柔性關(guān)節(jié)機械臂所受約束的有效處理方法以及可處理任意不等式約束的機器人最優(yōu)控制方法的相關(guān)研究基本沒有。本文將可有效處理約束的障礙函數(shù)法從數(shù)學(xué)規(guī)劃領(lǐng)域引入到最優(yōu)控制領(lǐng)域，提出可處理具有任意不等式約束最優(yōu)控制問題的改進型ILQR算法；設(shè)計柔性關(guān)節(jié)機械臂所受約束的有效處理方案，將柔性關(guān)節(jié)機械臂最優(yōu)控制系統(tǒng)的軟約束轉(zhuǎn)化為控制量上下界約束和系統(tǒng)硬約束，減少系統(tǒng)軟約束的數(shù)量，建立易于改進型ILQR算法求解的數(shù)學(xué)模型。通過仿真實驗驗證所提出的柔性關(guān)節(jié)機械臂約束處理方案和改進型ILQR算法的可行性和有效性。

1 最優(yōu)控制問題描述

據(jù)柔性關(guān)節(jié)機械臂的實際情況，本文僅考慮結(jié)束時間T固定、帶不等式約束的最優(yōu)控制問題，其數(shù)學(xué)描述如下：

(1)

(2)

gj(x(t),u(t))≤0,j=1,2,…,G；

(3)

x(0)=x0；

(4)

0≤t≤T。

(5)

其中，x(t)∈Rn為狀態(tài)向量，u(t)∈Rm為控制向量，被控對象由狀態(tài)方程(2)描述，其初始狀態(tài)為x0。J是最優(yōu)控制的目標(biāo)函數(shù)，其中φ和c分別為目標(biāo)函數(shù)的終值項和積分項。值得注意的是，系統(tǒng)受到的不等式約束由式(3)表示，它既可以表示關(guān)于控制量的不等式約束，又可以表示關(guān)于狀態(tài)量的不等式約束，還可以表示它們的復(fù)雜組合，j為不等式約束的數(shù)量。

最優(yōu)控制問題式(1)～ 式(5)可描述為：在系統(tǒng)初始狀態(tài)式(4)下，從滿足約束條件式(2)和式(3)的可行控制集中找到最優(yōu)控制量u*(t)，使目標(biāo)函數(shù)式(1)達(dá)到最小。

2 改進型ILQR算法研究

2.1 基于障礙函數(shù)法的改進型ILQR算法

由于具有高階數(shù)、多維度、強耦合、非線性等特性，柔性關(guān)節(jié)機械臂的最優(yōu)控制方法研究一直是機器人技術(shù)領(lǐng)域的一個難點。由Li和Todorov[13-14]提出的ILQR算法可有效處理復(fù)雜非線性系統(tǒng)的最優(yōu)控制問題。作為微分動態(tài)規(guī)劃(differential dynamic programming，DDP)算法的簡化形式，ILQR算法的計算效率更高，并已經(jīng)成功應(yīng)用于VSA機械臂的最優(yōu)控制中[15-16]。但只能處理無約束最優(yōu)控制問題或僅帶控制量上下界約束最優(yōu)控制問題的不足，限制了ILQR算法的應(yīng)用范圍。

為解決帶任意不等式約束的最優(yōu)控制問題，本節(jié)將可有效處理約束的障礙函數(shù)法同ILQR算法相結(jié)合，提出了可處理任意不等式約束最優(yōu)控制問題的改進型ILQR算法。該算法主要由內(nèi)環(huán)迭代和外環(huán)迭代2個部分組成，其偽代碼如下：

result:最優(yōu)控制量u*和對應(yīng)的最優(yōu)狀態(tài)量x*

repeat://外環(huán)迭代

通過式(6)構(gòu)建障礙函數(shù)并得到增廣目標(biāo)函數(shù)(7)

repeat://內(nèi)環(huán)迭代

ILQR算法求解無約束最優(yōu)控制序列u(i)

untilconvergence

u(i+1)←u(i)

untilconvergence

(6)

將狀態(tài)量不等式約束和控制量/狀態(tài)量耦合不等式約束作為積分形式的障礙項，增廣到原最優(yōu)控制問題的目標(biāo)函數(shù)(1)中，得到增廣目標(biāo)函數(shù)，即

(7)

2.2 收斂性證明

為方便證明改進型ILQR算法的收斂性，首先將式(1)描述的最優(yōu)控制問題轉(zhuǎn)化為如下離散時間最優(yōu)化問題1，即：

(8)

定理1 采用改進型ILQR算法求解問題1，狀態(tài)量軌跡和控制量軌跡{x,u}將收斂于局部最優(yōu)解{x*,u*}，即滿足如下KKT條件：

(9)

(10)

(11)

(12)

Step1：

由2.1節(jié)的內(nèi)容可知，改進型ILQR算法中的外環(huán)迭代通過障礙函數(shù)法將不等式約束添加至目標(biāo)函數(shù)中，則問題1轉(zhuǎn)化為如下形式，即：

(13)

式(13)可改寫為無約束最優(yōu)化問題2，即：

(14)

Step2：

定義如下Hamiltonian函數(shù)，即

(15)

則問題2的Lagrangian函數(shù)可表示為

(16)

根據(jù)式(16)的Lagrangian函數(shù)的微分，即

(17)

通過將式(17)中各項系數(shù)設(shè)置為零，可得到問題2的KKT條件：

(18)

由2.1節(jié)的內(nèi)容可知，改進型ILQR算法通過內(nèi)環(huán)迭代求解問題2。由于ILQR算法是DDP算法的簡化形式，因此，ILQR算法計算的最優(yōu)解滿足Pontryagin極大值原理，即：

(19)

Step3：

由問題2的平穩(wěn)性條件可得到

(20)

(21)

(22)

(23)

(24)

通過比較可以發(fā)現(xiàn)，除了互補松弛條件(24)以外，式(21)～式(24)和問題1的KKT條件完全相同。因此，式(21)～式(24)可視為問題1的變形KKT條件。由2.1節(jié)的內(nèi)容可知，bj隨著改進型ILQR算法的外環(huán)迭代逐漸增大至無窮，式(24)可表示為

(25)

則問題2的KKT條件最終轉(zhuǎn)變?yōu)閱栴}1的KKT條件。

綜上所述，改進型ILQR算法通過障礙項將問題1轉(zhuǎn)化為問題2；問題2由內(nèi)環(huán)ILQR算法求解，且求解的狀態(tài)量和控制量滿足KKT條件；問題2的KKT條件是問題1的KKT條件的變形形式，且隨著改進型ILQR算法的外環(huán)迭代轉(zhuǎn)變?yōu)閱栴}1的KKT條件。因此，改進型ILQR算法求解的最優(yōu)狀態(tài)量和控制量滿足問題1的KKT條件，即改進型ILQR算法處理問題1時收斂。

3 柔性關(guān)節(jié)機械臂約束處理方法研究

3.1 柔性關(guān)節(jié)機械臂動力學(xué)模型建立

柔性關(guān)節(jié)機械臂動力學(xué)方程可表示為：

(26)

當(dāng)柔性關(guān)節(jié)機械臂為SEA機械臂時，m=n；當(dāng)柔性關(guān)節(jié)機械臂為VSA機械臂時，m=2n。根據(jù)剛度定義，柔性關(guān)節(jié)機械臂的剛度可以表示為

(27)

通常柔性關(guān)節(jié)機械臂會受到2類不等式約束的限制，分別為電機端運動軌跡約束和彈性元件變形約束，具體如下：

1)電機端運動軌跡約束。電機端運動軌跡會受到運動范圍和運動速度的限制，其容許集Θ可表示為

(28)

2)彈性元件變形約束。由于彈性元件只能在一定范圍內(nèi)變形，柔性關(guān)節(jié)機械臂還會受到關(guān)于彈性元件變形的不等式約束的限制。彈性元件變形容許集Φ可表示為

Φ={φ∈Rm:φm(θ)≤φ(q,θ)≤φM(θ)}。

(29)

其中，φ為彈性元件變形量，如：彈性元件長度；φM、φm分別為彈性元件變形量的上下界，它們同電機端狀態(tài)量有關(guān)。

3.2 電機端閉環(huán)控制及約束處理方法

式(26)所表達(dá)的電機端動力學(xué)模型可改寫為

(30)

采用PD控制對電機端位置進行控制，即

(31)

(32)

其中：

風(fēng)影一直離群索居，幾乎不與村民們來往，獨來獨往的他看上去總是一副心事重重的樣子。紅琴的面孔倒是逐漸豐潤起來，那種知足常樂的樣子，乍一看根本看不出她過去有什么傷痛的經(jīng)歷與未來有什么美好的夢想。有時候，他會朝山坡上那個迷蒙的地方看，有時候風(fēng)和日麗，有時候云霧繚繞，他一看就是大半天。紅琴見了就開玩笑，和尚，你的靈魂是不是還丟在那間破廟里，如果你還想出家去當(dāng)和尚，那就盡管去，這回你要是想受戒，就盡管讓那老和尚拿香火燙，我絕不攔你。風(fēng)影有些惱火，但也沒有發(fā)作，只在心底里罵了一聲鬼女子，便悄無聲息地走開了。

(33)

β2=B-1kp；

(34)

(35)

(36)

(37)

(38)

通過電機端PD控制，實現(xiàn)了柔性關(guān)節(jié)機械臂系統(tǒng)控制輸入由電機端力矩到電機端期望位置的轉(zhuǎn)變，并且電機端位置θ可無超調(diào)地跟蹤控制輸入θd。

由以上分析可以看出，采用本節(jié)所述電機端閉環(huán)控制方法，可將電機端運動軌跡約束轉(zhuǎn)化為控制量上下界約束，并通過β嵌入到電機端動力學(xué)模型當(dāng)中，從而達(dá)到減少系統(tǒng)約束數(shù)量的目的，方便應(yīng)用改進型ILQR算法求解。

3.3 柔性關(guān)節(jié)機械臂最優(yōu)控制問題求解

定義向量

(39)

其中：x∈R2(n+m)為柔性關(guān)節(jié)機械臂的狀態(tài)向量；u∈Rm為柔性關(guān)節(jié)機械臂的控制向量。則柔性關(guān)節(jié)機械臂可由狀態(tài)方程表示為

(40)

由3.2節(jié)的內(nèi)容可知，電機端運動軌跡約束可通過對角矩陣β和控制量約束θm≤u≤θM嵌入到柔性關(guān)節(jié)機械臂電機端動力學(xué)模型中。此外，有些柔性關(guān)節(jié)機械臂還會受到彈性元件變形不等式約束的限制，彈性元件變形不等式約束采用狀態(tài)向量可表示為φm(x3)≤φ(x1,x3)≤φM(x3)。

4 仿真實驗

4.1 仿真設(shè)計

圖1 柔性關(guān)節(jié)機械臂結(jié)構(gòu)簡圖Fig.1 Schematic diagram of flexible joint manipulator

圖2 擲球任務(wù)簡圖Fig.2 Schematic diagram of throwing task

(41)

機械臂擲球任務(wù)的目標(biāo)函數(shù)可表示為

(42)

(43)

4.2 仿真結(jié)果

1) 由MACCEPA構(gòu)成的二連桿機械臂。

MACCEPA二連桿機械臂和MACCEPA樣機分別如圖3(a)和圖3(b)所示。

機械臂的電機端位置向量可表示為θ=[θ1,θ2,θ3,θ4]T，由MACCEPA結(jié)構(gòu)原理圖3(c)可知：MACCEPA的主電機控制電機端位置θ1和θ2；剛度調(diào)節(jié)電機控制線性彈簧的預(yù)緊，對應(yīng)的電機端位置為θ3和θ4；連桿端彈性力矩向量τq和二連桿機械臂剛度矩陣K可表示為：

圖3 可變剛度柔性關(guān)節(jié)機械臂Fig.3 Variable stiffness joint manipulator

(44)

其中ki為線性彈簧的剛度。式(44)中其它參數(shù)具有如下關(guān)系式：

(45)

式中：lsi、l0i分別為拉伸后的線性彈簧長度和彈簧原長；Bi、Ci和ri為MACCEPA的幾何尺寸，MACCEPA二連桿機械臂模型參數(shù)見表1。

表1 MACCEPA二連桿機械臂模型參數(shù)

采用改進型ILQR算法求解使目標(biāo)函數(shù)(42)最小，即二連桿機械臂擲球最遠(yuǎn)的機械臂各狀態(tài)量軌跡。此外，通過采用將剛度調(diào)節(jié)電機控制量設(shè)置為常值(u=[u1(t),u2(t),u3,u4]T)的對比實驗來研究主動剛度調(diào)節(jié)在擲球任務(wù)中的作用。此時，二連桿機械臂無法主動調(diào)節(jié)關(guān)節(jié)剛度，VSA機械臂退化為SEA關(guān)節(jié)機械臂。2種情況的仿真結(jié)果如圖4所示。

由頻閃觀測圖4(a)和圖4(b)可知，VSA機械臂和SEA機械臂擲球的距離分別為9.69 m和7.67 m；無論是VSA機械臂和SEA機械臂，在擲球的過程中都有一個明顯的回擺動作。值得注意的是，人類在進行諸如擊打、跳躍等爆發(fā)性運動時也會采用類似的“收縮-伸展循環(huán)”策略。因此，本文提出的最優(yōu)控制方法在柔性關(guān)節(jié)機械臂擲球任務(wù)中的數(shù)值預(yù)測符合生物運動規(guī)律。

圖4 MACCEPA二連桿機械臂最優(yōu)控制仿真結(jié)果Fig.4 Simulation results of optimal control for MACCEPA two-link manipulator

VSA機械臂和SEA機械臂位置信息和關(guān)節(jié)剛度變化情況分別由圖4(c)至圖4(h)中的黑色曲線和藍(lán)色曲線表示。從圖4中可以看出，在機械臂投擲小球的過程中，電機端運動軌跡始終滿足約束條件。由圖4(e)和圖4(f)可知，VSA機械臂的剛度調(diào)節(jié)電機位置可由最優(yōu)控制進行周期性調(diào)節(jié)，而SEA機械臂電機位置不受最優(yōu)控制調(diào)節(jié)。因此，如圖4(g)和4(h)所示，VSA機械臂可在剛度調(diào)節(jié)范圍內(nèi)主動調(diào)節(jié)關(guān)節(jié)剛度，而SEA機械臂的關(guān)節(jié)剛度始終處于剛度邊界值。由于可主動調(diào)節(jié)剛度，VSA機械臂的連桿端可同電機端有效“解耦”，在更大的范圍內(nèi)更加自由地運動。這使得VSA機械臂較SEA機械臂具有更佳的任務(wù)表現(xiàn)，即擲球距離更遠(yuǎn)。

相比于傳統(tǒng)的機械臂，柔性關(guān)節(jié)機械臂可利用彈性元件儲存并釋放能量來提高機械臂的運動性能。接下來，將從功率和能量的角度進一步解釋MACCEPA二連桿機械臂是如何利用固有柔順性來實現(xiàn)小球更遠(yuǎn)的投擲。在柔性關(guān)節(jié)機械臂執(zhí)行投擲任務(wù)的過程中，電機的輸入功率為

(46)

(47)

MACCEPA二連桿機械臂的能量流如圖5所示。

圖5 MACCEPA二連桿機械臂的能量流Fig.5 Power flow of MACCEPA two-link manipulator

從圖5(a)至圖5(d)可以看出，無論是VSA機械臂還是SEA機械臂，在改進型ILQR算法的控制下，機械臂都處于功率放大和能量儲存模式的能量流循環(huán)之中，且在機械臂釋放小球前，存在顯著的輸出功率放大過程。VSA機械臂在執(zhí)行擲球任務(wù)時，不僅可由主電機輸入能量，還可通過剛度調(diào)節(jié)電機在調(diào)節(jié)剛度的過程中輸入部分能量。這使得VSA機械臂的輸出能量大于SEA機械臂的輸出能量(如圖5(f)所示)，從而使VSA機械臂釋放的小球的速度大于SEA機械臂釋放的小球的速度(如圖5(e)所示)，最終導(dǎo)致VSA機械臂擲球的距離更遠(yuǎn)。由于傳統(tǒng)機械臂沒有彈性元件，機械臂的輸入功率同輸出功率相等，即pin=pout，因此傳統(tǒng)機械臂無法實現(xiàn)功率放大，也就無法像柔性關(guān)節(jié)機械臂一樣利用固有柔順性來提高機械臂的運動性能。

2)由FSJ構(gòu)成的二連桿機械臂。

FSJ樣機及其模型圖如圖6(a)和6(b)所示，F(xiàn)SJ二連桿機械臂電機端位置向量同樣可表示為θ=[θ1,θ2,θ3,θ4]T。由文獻(xiàn)[18]可知，F(xiàn)SJ二連桿機械臂連桿端彈性力矩向量τq可表示為

圖6 FSJ關(guān)節(jié)Fig.6 DLR floating spring joint

(48)

其中：τmax=10N·m；τchar=12；φmax=15°。FSJ二連桿機械臂模型參數(shù)見表2。

表2 FSJ二連桿機械臂模型參數(shù)

采用改進型ILQR算法求解使FSJ二連桿機械臂擲球最遠(yuǎn)的機械臂各狀態(tài)量軌跡，仿真結(jié)果如圖7所示。由頻閃觀測圖7(a)可知，VSA機械臂的最遠(yuǎn)擲球距離為9.42 m。根據(jù)圖7(b)和6(c)，并結(jié)合剛度調(diào)節(jié)電機端位置圖7(h)和7(i)可知，機械臂關(guān)節(jié)狀態(tài)量滿足彈性元件變形約束條件。主電機端運動軌跡如圖7(d)至7(g)所示，可以看出，無論是位置曲線還是速度曲線都位于相應(yīng)的約束范圍內(nèi)。以上仿真結(jié)果表明，改進型ILQR算法可處理具有電機端運動軌跡約束和彈性元件變形約束的柔性關(guān)節(jié)機械臂最優(yōu)控制問題。

圖7 FSJ二連桿機械臂最優(yōu)控制仿真結(jié)果Fig.7 Simulation results of optimal control for FSJ two-link manipulator

5 結(jié) 論

本文將數(shù)學(xué)規(guī)劃領(lǐng)域中的障礙函數(shù)法引入到ILQR算法當(dāng)中，提出了可處理具有任意不等式約束最優(yōu)控制問題的改進型ILQR算法，通過KKT條件系統(tǒng)地證明了算法的收斂性；選取合適的PD控制參數(shù)對電機端進行控制，將柔性關(guān)節(jié)機械臂電機端動力學(xué)模型轉(zhuǎn)化為二階臨界阻尼系統(tǒng)，實現(xiàn)了電機端運動軌跡約束轉(zhuǎn)化為控制量上下界約束，并通過二階臨界阻尼系統(tǒng)的系統(tǒng)參數(shù)嵌入到電機端動力學(xué)模型當(dāng)中，從而達(dá)到減少系統(tǒng)軟約束數(shù)量、方便應(yīng)用改進型ILQR算法對系統(tǒng)最優(yōu)控制問題進行求解的目的；通過MACCEPA二連桿機械臂仿真實驗驗證了本文提出的改進型ILQR算法的有效性，并通過能量流解釋了柔性關(guān)節(jié)機械臂利用固有柔順性達(dá)到最佳運動性能的機理。最后，通過FSJ二連桿機械臂擲球仿真實驗驗證了所提出的改進型ILQR算法的普遍適用性。

本文從理論上證明了所提出的最優(yōu)控制方法的可行性，使用該控制方法對真實的柔性關(guān)節(jié)機械臂進行最優(yōu)控制將作為日后工作的重點。