歐勁松, 李 蓉, 尹 輝, 王華建
(湖南大學(xué) 機(jī)械與運(yùn)載工程學(xué)院, 長(zhǎng)沙 410082)
隨著科技的不斷發(fā)展,機(jī)械臂被廣泛應(yīng)用于醫(yī)療、工業(yè)、民生、服務(wù)等領(lǐng)域.機(jī)械臂主要包含剛性機(jī)械臂、柔性關(guān)節(jié)機(jī)械臂和柔性臂桿機(jī)械臂[1-3].相比于剛性機(jī)械臂,柔性機(jī)械臂具有能耗低、質(zhì)量輕、傳動(dòng)平緩、靈活方便等優(yōu)點(diǎn)[4-5].然而,柔性機(jī)械臂采用的諧波減速器與皮帶等柔性關(guān)節(jié),使柔性關(guān)節(jié)機(jī)械臂的動(dòng)力學(xué)分析比剛性機(jī)械臂復(fù)雜得多,這對(duì)運(yùn)動(dòng)控制的穩(wěn)定性和準(zhǔn)確性有極大的影響.柔性關(guān)節(jié)的引進(jìn)使得系統(tǒng)的控制輸入小于系統(tǒng)的自由度,因此柔性關(guān)節(jié)機(jī)械臂系統(tǒng)是一個(gè)非線性、欠驅(qū)動(dòng)、強(qiáng)耦合的系統(tǒng),容易產(chǎn)生跟蹤誤差,導(dǎo)致較長(zhǎng)的收斂時(shí)間、較大的超調(diào)量,甚至機(jī)械損壞[6].
奇異攝動(dòng)法首次應(yīng)用于線性時(shí)不變系統(tǒng)的經(jīng)典穩(wěn)定性分析是由Klimushev于20世紀(jì)60年代通過“快慢分解” 思想得到的,即如果慢、快子系統(tǒng)是穩(wěn)定的,則攝動(dòng)參數(shù)必存在一個(gè)穩(wěn)定上界,在此范圍之內(nèi),奇異攝動(dòng)系統(tǒng)是穩(wěn)定的.非線性奇異攝動(dòng)系統(tǒng)的穩(wěn)定性分析則是基于Lyapunov函數(shù)的方法.針對(duì)快、慢系統(tǒng)分別建立Lyapunov函數(shù),將兩系統(tǒng)Lyapunov函數(shù)相加作為原系統(tǒng)的Lyapunov函數(shù),以保證原系統(tǒng)的漸進(jìn)收斂[7].1987年,Spong[8]提出了機(jī)器人柔性關(guān)節(jié)的簡(jiǎn)化模型,并基于奇異攝動(dòng)理論和積分流,將柔性關(guān)節(jié)機(jī)器人分解成快、慢兩個(gè)子系統(tǒng).隨后諸多學(xué)者基于奇異攝動(dòng)法提出了不同的解決方案:洪昭斌、陳力[9]基于奇異攝動(dòng)法的魯棒跟蹤控制和柔性振動(dòng)主動(dòng)控制,提出了對(duì)空間柔性臂進(jìn)行控制;劉華山等[10]利用奇異攝動(dòng)理論將柔性關(guān)節(jié)機(jī)器人動(dòng)力學(xué)模型解耦成快、慢兩個(gè)子系統(tǒng),引入一類平滑飽和函數(shù)和徑向基函數(shù)神經(jīng)網(wǎng)絡(luò)非線性逼近手段,根據(jù)反步策略設(shè)計(jì)了針對(duì)慢子系統(tǒng)的有界控制器.在快子系統(tǒng)的有界控制器設(shè)計(jì)中,通過關(guān)節(jié)彈性力矩跟蹤誤差的濾波處理加速了系統(tǒng)的收斂.
基于U-K(Udwadia-Kalaba)方程的約束跟隨方法,該方法不需要借助Lagrange乘子和偽廣義速度等輔助變量,可以同時(shí)處理完整約束和非完整約束.Kalaba和Udwadia等[11-15]利用Gauss最小約束原理將描述系統(tǒng)受約束運(yùn)動(dòng)的Lagrange方程重新定義為一個(gè)二次規(guī)劃問題,給出了這個(gè)約束二次極小化問題的顯式解析解,從而得到了一個(gè)描述受Pfaffian約束的離散機(jī)械系統(tǒng)約束運(yùn)動(dòng)的精確顯式方程組;將Lagrange力學(xué)擴(kuò)展到允許約束力對(duì)虛位移系統(tǒng)做功的情況,總結(jié)了約束力做功的U-K方程,并應(yīng)用于多體系統(tǒng)的建模和控制,較好地解決了不確定多體動(dòng)力學(xué)系統(tǒng)的控制問題.Chen等[16-17]首次將U-K方程拓展至全驅(qū)動(dòng)的機(jī)械系統(tǒng)控制中,使其滿足一系列的完整或非完整約束,并首次提出了基于U-K方程的狀態(tài)反饋約束跟隨控制方法.該方法的思路是將一系列的系統(tǒng)控制目標(biāo)設(shè)計(jì)成完整約束和非完整約束等一些需要滿足的等式方程,因此其控制任務(wù)即為驅(qū)動(dòng)系統(tǒng)滿足這些約束方程.相較于其他控制方法, 該方法不需要對(duì)非線性系統(tǒng)進(jìn)行線性化,且可在不出現(xiàn)任何輔助變量(如Lagrange乘子)或偽變量(如廣義速度)的情況下得到系統(tǒng)狀態(tài)反饋控制律的顯式方程[18].近年來,基于U-K方法的控制研究得到了一定的發(fā)展.Yin等[19]進(jìn)一步將約束控制跟隨方法拓展至欠驅(qū)動(dòng)系統(tǒng),實(shí)現(xiàn)了欠驅(qū)動(dòng)二輪式機(jī)器人的控制.韓江等[20-21]基于U-K方法實(shí)現(xiàn)了平面冗余并聯(lián)機(jī)器人的建模與帶有不確定性的軌跡跟蹤控制.Zhao等[22]將該方法引入柔性關(guān)節(jié)機(jī)械臂,建立了柔性關(guān)節(jié)機(jī)械臂的動(dòng)力學(xué)模型.Han等[23]基于反步法并結(jié)合U-K方法提出了一種柔性關(guān)節(jié)機(jī)械臂的魯棒控制方法.
在上述研究的基礎(chǔ)上,針對(duì)當(dāng)前柔性關(guān)節(jié)機(jī)械臂存在的柔性振蕩與約束跟隨問題,本文提出了一種新的針對(duì)柔性關(guān)節(jié)機(jī)械臂的控制方法.首先基于奇異攝動(dòng)法將系統(tǒng)降階為快、慢兩個(gè)子系統(tǒng).針對(duì)快、慢兩個(gè)子系統(tǒng),將運(yùn)動(dòng)軌跡設(shè)計(jì)成相應(yīng)的完整約束或非完整約束,基于U-K方程的約束跟隨方法設(shè)計(jì)控制器.本文對(duì)所提出的方法基于MATLAB進(jìn)行動(dòng)力學(xué)仿真,仿真結(jié)果驗(yàn)證了該方法的可行性,并與傳統(tǒng)PID控制方法相比,驗(yàn)證了在處理柔性關(guān)節(jié)機(jī)械臂的柔性振蕩與約束跟隨時(shí),其具有一定的優(yōu)越性.
Spong[8]首次提出了柔性關(guān)節(jié)的簡(jiǎn)化彈簧阻尼模型:
(1)
(2)
以圖1所示的二連桿柔性關(guān)節(jié)機(jī)械臂為例,假設(shè)連桿的質(zhì)量均勻分布,連桿的結(jié)構(gòu)標(biāo)準(zhǔn)對(duì)稱.結(jié)合剛性二連桿機(jī)械臂的動(dòng)力學(xué)方程和柔性關(guān)節(jié)的簡(jiǎn)化彈簧阻尼模型,即可得到動(dòng)力學(xué)方程:
圖1 二連桿柔性關(guān)節(jié)機(jī)械臂
(3)
(4)
(5)
其中
(6)
mi(i=1,2)是連桿1,2的質(zhì)量,li(i=1,2)是連桿1,2的長(zhǎng)度,lci(i=1,2)是連桿1,2長(zhǎng)度的一半,Ii(i=1,2)是連桿1,2相對(duì)于連桿中心的轉(zhuǎn)動(dòng)慣量.
奇異攝動(dòng)法可以將一個(gè)高階系統(tǒng)的求解降階為兩個(gè)低階系統(tǒng),然后分別求解,這就降低了求解系統(tǒng)的階次,大大減小了計(jì)算量.針對(duì)二連桿柔性關(guān)節(jié)機(jī)械臂選取連桿轉(zhuǎn)角q2和關(guān)節(jié)彈性力矩誤差ef作為慢變量和快變量.奇異攝動(dòng)法的基本假設(shè)如下:在快變量的暫態(tài)過程中慢變量保持常量,當(dāng)慢變量變化比較明顯時(shí),快變量的暫態(tài)過程就已經(jīng)結(jié)束并達(dá)到穩(wěn)態(tài)值.使用奇異攝動(dòng)法可以把二連桿柔性關(guān)節(jié)機(jī)械臂系統(tǒng)分解成慢系統(tǒng)、快系統(tǒng).當(dāng)慢系統(tǒng)和快系統(tǒng)都收斂于系統(tǒng)控制要求時(shí),則有整體系統(tǒng)滿足控制要求.具體分解過程如下:
令μ=1/k,其中k是K(關(guān)節(jié)剛性矩陣)的最小值.令K=μ-1Kμ,
e=K(q1-q2)=μ-1Kμ(q1-q2).
(7)
把式(7)代入式(3)、(4),可得
(8)
(9)
令μ=0,可得
(10)
把式(10)代入式(3),可得到慢系統(tǒng)表達(dá)式:
(11)
(12)
(13)
由式(12)減去式(13)可得
(14)
(15)
所以可以得到快、慢子系統(tǒng)分別為
(16)
(17)
其中us是慢系統(tǒng)中連桿輸入的力矩,uf是快系統(tǒng)中連桿輸入的力矩,實(shí)際系統(tǒng)輸入力矩u=us+uf.
一般動(dòng)力學(xué)方程如下:
(18)
約束表達(dá)如下:
(19)
由式(19),對(duì)t求導(dǎo)有
(20)
整理式(20)可得
(21)
式(19)、(21)寫成矩陣的形式可以表達(dá)為
(22)
(23)
b=[b1,b2,…,bm]T,c=[c1,c2,…,cm]T.
定義1 在矩陣A和b確定的情況下,約束(22)至少存在一個(gè)解,則稱約束(22)是一致的.
假設(shè)1 約束(22)是一致的.
不論是完整約束還是非完整約束,都可以用式(23)表達(dá),還包括控制目標(biāo)(軌跡跟蹤、系統(tǒng)鎮(zhèn)定和最優(yōu)控制等)都可以先設(shè)計(jì)成相應(yīng)的約束(23), 再通過求導(dǎo)轉(zhuǎn)換成約束(22)的形式[14].在初始條件滿足方程(23)的情況下,控制律滿足式(22)就可以保證對(duì)約束(23)的跟蹤.
引理1 約束(22)一致的充要條件是AA+b=b,A+是A的Moore-Penrose廣義逆矩陣.
對(duì)給定的系統(tǒng)(18),對(duì)于給定約束(22),初始條件滿足式(23)的條件下,控制律的求解過程如下:設(shè)
式(18)可以改寫為
(24)
令Φ1=AM-1/2,式(22)可改寫為
(25)
定義2 如果給定矩陣Φ1和b,約束(25)存在至少一個(gè)解,則稱約束(25)一致.
因?yàn)榧s束(25)是約束(22)的改寫式,所以只要假設(shè)1滿足,那么必然約束(25)是一致的.
式(24)需要滿足式(25),有
(26)
令Φ2=M-1/2B可得
(27)
(28)
(29)
在解決一些實(shí)際問題的時(shí)候,大部分情況都是初始狀態(tài)不滿足約束.基于U-K方法提出一種初始狀態(tài)不滿足約束的控制方法.
令u=τs+τn,代入式(18)可得到
(30)
(AM-1B)τs=C1z.
(31)
假設(shè)2 式(31)是一致的.
因?yàn)榧僭O(shè)2中式(31)一致,因此根據(jù)引理1會(huì)有(AM-1B)(AM-1B)+(C1z)=C1z.
令τs=C1(AM-1B)+z,C1<0,式(30)可改寫為
(32)
式(32)對(duì)于每一個(gè)維度,都可以看作齊次一階微分方程.其解為
zi=C0eC1t,i≤m.
(33)
代入t=0的初始條件zi(0)=C0可以得到
(34)
其中|C1|越大,收斂速率越快.
綜上得到控制律
u=τn+τs.
(35)
定理1 滿足假設(shè)1、2的情況下,對(duì)于系統(tǒng)(18),控制律(35)可以得到以下的控制表現(xiàn):
1)一致穩(wěn)定性 對(duì)于任意ε>0,當(dāng)存在‖z(t0)‖<ε,t∈[t0,∞],會(huì)有‖z(t)‖<ε.
2)收斂性 當(dāng)t→∞時(shí),z(t∞)=0.
證明選取Lyapunov函數(shù)式:
V=zTz.
(36)
V(z)當(dāng)且僅當(dāng)z=0時(shí)為0.當(dāng)z≠0時(shí),V(z)>0,所以V(z)正定,滿足Lyapunov函數(shù)的要求.
(37)
2zT(AM-B(AM-B)+(C1z))=2C1‖z‖2.
(38)
二連桿柔性關(guān)節(jié)機(jī)械臂是一個(gè)欠驅(qū)動(dòng)、非線性的動(dòng)力學(xué)方程.借助奇異攝動(dòng)法把二連桿柔性關(guān)節(jié)機(jī)械臂動(dòng)力學(xué)方程降階為兩個(gè)全驅(qū)動(dòng)模型.慢系統(tǒng)表達(dá)了二連桿柔性關(guān)節(jié)機(jī)械臂的運(yùn)動(dòng)約束.快系統(tǒng)表達(dá)了柔性關(guān)節(jié)對(duì)運(yùn)動(dòng)過程中擾動(dòng)的影響.所以系統(tǒng)的控制目標(biāo)分為兩個(gè)部分:① 慢系統(tǒng)控制律滿足約束方程,實(shí)現(xiàn)包括位置、速度、軌跡控制等宏觀上的控制目標(biāo).② 快系統(tǒng)控制律使力矩跟蹤誤差快速收斂到0,減小柔性對(duì)系統(tǒng)的影響.
控制器的設(shè)計(jì)目標(biāo)是使系統(tǒng)對(duì)控制約束進(jìn)行跟蹤.慢系統(tǒng)主要負(fù)責(zé)二連桿柔性關(guān)節(jié)機(jī)械臂的約束跟蹤,快系統(tǒng)負(fù)責(zé)抑制關(guān)節(jié)柔性帶來的振動(dòng)與誤差.由式(16)、(17)可以得到控制目標(biāo)的數(shù)學(xué)表達(dá)式:
(39)
ef=0.
(40)
因此,控制目標(biāo)是被控系統(tǒng)的慢子系統(tǒng),滿足約束方程(39),快子系統(tǒng)滿足約束方程(40).
慢系統(tǒng)的控制律設(shè)計(jì), 不論是完整約束還是非完整約束, 或者是其他的軌跡約束、優(yōu)化控制等, 都可以寫成式(23)的形式.式(23)對(duì)時(shí)間t求導(dǎo)后, 得到約束(22), 然后根據(jù)式(29)、(31)得到控制律.具體過程如下:
慢系統(tǒng)動(dòng)力學(xué)方程為
(41)
約束可以表達(dá)為式(22)、(23)形式,如下:
(42)
控制律可以設(shè)計(jì)為
(43)
快系統(tǒng)的控制律設(shè)計(jì),快系統(tǒng)的目標(biāo)是讓式(17)的變量ef快速收斂到零.快系統(tǒng)的動(dòng)力學(xué)表達(dá)為
(44)
約束可以表達(dá)為
(45)
采用控制律
(46)
整體控制流程如圖2所示.其中控制系統(tǒng)采集的輸入變量,慢系統(tǒng)需要參數(shù)為連桿1和連桿2的角度和角速度,采用角度傳感器,常見有光電、Hall、機(jī)械幾類.角速度可以采用角度的微分或者現(xiàn)有的角速度傳感器進(jìn)行測(cè)量;快系統(tǒng)需要參數(shù)為柔性關(guān)節(jié)傳遞的力矩大小與扭矩的變化速度,可以采用扭矩傳感器,常見有接觸式和非接觸式的,其原理都是測(cè)量傳感器的變形量來計(jì)算扭矩,扭矩的變化速度可以對(duì)扭矩進(jìn)行微分得到,扭矩傳感器的采樣率要在較高的頻率下.其余一些靜態(tài)的變量都可以采用常用的方法測(cè)量,例如連桿1和連桿2的質(zhì)量、轉(zhuǎn)動(dòng)慣量、長(zhǎng)度,柔性關(guān)節(jié)的轉(zhuǎn)動(dòng)慣量,柔性關(guān)節(jié)的彈性系數(shù).
圖2 控制律設(shè)計(jì)流程
本節(jié)通過MATLAB數(shù)值模擬的方法,驗(yàn)證了基于奇異攝動(dòng)的柔性關(guān)節(jié)機(jī)械臂約束跟隨控制的可行性.當(dāng)然,柔性關(guān)節(jié)機(jī)械臂建模中所包含的動(dòng)力學(xué)參數(shù),如關(guān)節(jié)彈性系數(shù)、關(guān)節(jié)轉(zhuǎn)動(dòng)慣量、連桿長(zhǎng)度、連桿質(zhì)量、連桿質(zhì)心位置和連桿相對(duì)質(zhì)心的轉(zhuǎn)動(dòng)慣量等都是已知的.具體數(shù)據(jù)如表1所示.
表1 二連桿柔性機(jī)械臂參數(shù)
(47)
一般會(huì)要求機(jī)械臂做轉(zhuǎn)角的角度控制與機(jī)械臂末端執(zhí)行器的軌跡控制.本小節(jié)以三個(gè)示例進(jìn)行驗(yàn)證.第一個(gè)示例是對(duì)機(jī)械臂角度的準(zhǔn)確控制.慢系統(tǒng)約束如下:
(48)
qd為期望角度.
代入式(28)得
rank(Φ1Φ2)=rank(AM-1B)=rank(A(D+J)-1B)=2>1.
(49)
設(shè)計(jì)k=5,C1=-20,代入式(43)可得慢系統(tǒng)控制律:
(50)
快系統(tǒng)約束為
(51)
由式(28)可得
rank(Φ1Φ2)=rank(AM-1B)=rank(A(J)-1KμB)=2>1.
(52)
設(shè)計(jì)k=10,C1=-100,代入式(46)可得快系統(tǒng)控制律:
(53)
最后根據(jù)奇異攝動(dòng)理論結(jié)合式(50)、(53)得到控制律:
u=us+uf,
(54)
圖3 轉(zhuǎn)桿1的角度軌跡q(2) 圖4 轉(zhuǎn)桿2的角度軌跡q(4)
控制律設(shè)計(jì)為
考慮力的輸出大小,兩種控制方式的控制力都限定在1 000 N·m以內(nèi).
對(duì)機(jī)械臂的連桿角速度進(jìn)行控制.慢系統(tǒng)約束為
(55)
(56)
由圖5、圖6可證明該方法對(duì)柔性關(guān)節(jié)所產(chǎn)生的振蕩有很好的抑制作用,并且在PID有很大的跟蹤誤差的情況下,該方法的跟蹤誤差僅為1×10-4rad·s-1.
圖5 轉(zhuǎn)桿1的角速度 圖6 轉(zhuǎn)桿2的角速度
在使用機(jī)械臂的場(chǎng)景中會(huì)經(jīng)常對(duì)機(jī)械臂的末端執(zhí)行器的軌跡提出要求.本例中使用橢圓軌跡進(jìn)行仿真驗(yàn)證.橢圓軌跡可以寫成
(57)
慢系統(tǒng)約束可以寫成
(58)
按照?qǐng)D2進(jìn)行控制率設(shè)計(jì),設(shè)計(jì)k=5,C1=-20,可以得到控制律:
(59)
uf和式(53)中的控制律相同.
相比于傳統(tǒng)PID控制方法,圖7驗(yàn)證了該方法對(duì)二連桿柔性機(jī)械臂的末端執(zhí)行器控制具有更小的軌跡跟蹤誤差.圖8是具體的末端位置誤差隨著時(shí)間變化的曲線.從圖中可以明顯看出,本文提出的控制方法在x,y位置的跟蹤精度上都優(yōu)于傳統(tǒng)PID控制方法.
圖7 末端軌跡 圖8 末端軌跡位置誤差
由圖9、圖10可知,輸出力矩在開始階段都達(dá)到了輸出力矩的最大值.本文提出的方法相對(duì)傳統(tǒng)PID控制方法輸出力矩穩(wěn)定時(shí)間更短,且穩(wěn)定之后輸出力矩大小基本一致,只存在小幅度的波動(dòng).
圖9 柔性關(guān)節(jié)1輸出力矩u1 圖10 柔性關(guān)節(jié)2輸出力矩u2
圖11闡述了橢圓軌跡在加載控制律2 s后的位置誤差和慢系統(tǒng)控制律設(shè)計(jì)參數(shù)k,C1的關(guān)系.可見,隨著參數(shù)C1的不斷加大,收斂速度越來越快.當(dāng)C1約束越大,收斂越快,這和式(34)中的結(jié)果保持一致.
圖11 參數(shù)與誤差關(guān)系
二連桿柔性關(guān)節(jié)機(jī)械臂系統(tǒng)是欠驅(qū)動(dòng)系統(tǒng)和柔性系統(tǒng),傳統(tǒng)PID控制方法會(huì)產(chǎn)生振蕩和較大的跟蹤誤差.為解決這類柔性控制的問題,本文提出了結(jié)合奇異攝動(dòng)法和U-K方法的控制策略,將系統(tǒng)拆分為慢系統(tǒng)、快系統(tǒng)兩個(gè)全驅(qū)動(dòng)系統(tǒng).慢系統(tǒng)解決了系統(tǒng)的約束跟隨問題,快系統(tǒng)解決了系統(tǒng)的振蕩與誤差過大的問題,本文提出的控制方法不需要對(duì)非線性系統(tǒng)進(jìn)行線性化,可以直接得到控制律的顯式方程.通過Lyapunov穩(wěn)定性證明和MATLAB數(shù)值仿真驗(yàn)證的方式,驗(yàn)證了該方法的穩(wěn)定性與對(duì)柔性系統(tǒng)控制的有效性.同時(shí)對(duì)比了傳統(tǒng)PID控制方法,仿真結(jié)果顯示所提出方法具有更好的控制精度.未來我們將開展所提出方法的控制參數(shù)優(yōu)化研究和建模時(shí)參數(shù)不確定性的魯棒控制方法研究.