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

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

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

0 引 言

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

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

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

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

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

(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)控制的目標函數，其中φ和c分別為目標函數的終值項和積分項。值得注意的是，系統(tǒng)受到的不等式約束由式(3)表示，它既可以表示關于控制量的不等式約束，又可以表示關于狀態(tài)量的不等式約束，還可以表示它們的復雜組合，j為不等式約束的數量。

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

2 改進型ILQR算法研究

2.1 基于障礙函數法的改進型ILQR算法

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

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

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

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

通過式(6)構建障礙函數并得到增廣目標函數(7)

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

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

untilconvergence

u(i+1)←u(i)

untilconvergence

(6)

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

(7)

2.2 收斂性證明

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

(8)

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

(9)

(10)

(11)

(12)

Step1：

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

(13)

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

(14)

Step2：

定義如下Hamiltonian函數，即

(15)

則問題2的Lagrangian函數可表示為

(16)

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

(17)

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

(18)

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

(19)

Step3：

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

(20)

(21)

(22)

(23)

(24)

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

(25)

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

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

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

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

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

(26)

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

(27)

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

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

(28)

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

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

(29)

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

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

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

(30)

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

(31)

(32)

其中：

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

(33)

β2=B-1kp；

(34)

(35)

(36)

(37)

(38)

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

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

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

定義向量

(39)

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

(40)

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

4 仿真實驗

4.1 仿真設計

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

圖2 擲球任務簡圖Fig.2 Schematic diagram of throwing task

(41)

機械臂擲球任務的目標函數可表示為

(42)

(43)

4.2 仿真結果

1) 由MACCEPA構成的二連桿機械臂。

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

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

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

(44)

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

(45)

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

表1 MACCEPA二連桿機械臂模型參數

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

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

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

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

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

(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í)行擲球任務時，不僅可由主電機輸入能量，還可通過剛度調節(jié)電機在調節(jié)剛度的過程中輸入部分能量。這使得VSA機械臂的輸出能量大于SEA機械臂的輸出能量(如圖5(f)所示)，從而使VSA機械臂釋放的小球的速度大于SEA機械臂釋放的小球的速度(如圖5(e)所示)，最終導致VSA機械臂擲球的距離更遠。由于傳統(tǒng)機械臂沒有彈性元件，機械臂的輸入功率同輸出功率相等，即pin=pout，因此傳統(tǒng)機械臂無法實現功率放大，也就無法像柔性關節(jié)機械臂一樣利用固有柔順性來提高機械臂的運動性能。

2)由FSJ構成的二連桿機械臂。

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

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

(48)

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

表2 FSJ二連桿機械臂模型參數

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

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

5 結 論

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

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