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

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

0 引 言

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

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

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

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

據(jù)柔性關(guān)節(jié)機(jī)械臂的實(shí)際情況，本文僅考慮結(jié)束時(shí)間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為控制向量，被控對(duì)象由狀態(tài)方程(2)描述，其初始狀態(tài)為x0。J是最優(yōu)控制的目標(biāo)函數(shù)，其中φ和c分別為目標(biāo)函數(shù)的終值項(xiàng)和積分項(xiàng)。值得注意的是，系統(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 改進(jìn)型ILQR算法研究

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

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

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

result:最優(yōu)控制量u*和對(duì)應(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)量耦合不等式約束作為積分形式的障礙項(xiàng)，增廣到原最優(yōu)控制問題的目標(biāo)函數(shù)(1)中，得到增廣目標(biāo)函數(shù)，即

(7)

2.2 收斂性證明

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

(8)

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

(9)

(10)

(11)

(12)

Step1：

由2.1節(jié)的內(nèi)容可知，改進(jìn)型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)中各項(xiàng)系數(shù)設(shè)置為零，可得到問題2的KKT條件：

(18)

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

(19)

Step3：

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

(20)

(21)

(22)

(23)

(24)

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

(25)

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

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

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

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

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

(26)

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

(27)

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

1)電機(jī)端運(yùn)動(dòng)軌跡約束。電機(jī)端運(yùn)動(dòng)軌跡會(huì)受到運(yùn)動(dòng)范圍和運(yùn)動(dòng)速度的限制，其容許集Θ可表示為

(28)

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

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

(29)

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

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

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

(30)

采用PD控制對(duì)電機(jī)端位置進(jìn)行控制，即

(31)

(32)

其中：

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

(33)

β2=B-1kp；

(34)

(35)

(36)

(37)

(38)

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

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

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

定義向量

(39)

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

(40)

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

4 仿真實(shí)驗(yàn)

4.1 仿真設(shè)計(jì)

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

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

(41)

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

(42)

(43)

4.2 仿真結(jié)果

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

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

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

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

(44)

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

(45)

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

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

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

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

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

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

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

(46)

(47)

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

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

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

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

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

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

(48)

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

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

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

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

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