基于三步法的機(jī)械臂軌跡跟蹤控制

于樹友 孟凌宇 許 芳 曲 婷陳 虹

(1.吉林大學(xué)汽車仿真與控制國家重點(diǎn)實(shí)驗(yàn)室,吉林長春 130022;2.吉林大學(xué)控制科學(xué)與工程系,吉林長春 130022;3.同濟(jì)大學(xué)新能源汽車工程中心,上海 200092)

1 引言

機(jī)械臂在工業(yè)生產(chǎn)過程中起著十分重要的作用,它可以代替人類在復(fù)雜、惡劣的環(huán)境下工作[1–2].機(jī)械臂是一個(gè)多輸入多輸出的、具有時(shí)變和強(qiáng)耦合特性的非線性系統(tǒng).高精度的軌跡跟蹤控制是工業(yè)機(jī)械臂大規(guī)模應(yīng)用的基礎(chǔ).文獻(xiàn)[3–5]基于改進(jìn)的PID或PD控制方法設(shè)計(jì)機(jī)械臂的軌跡跟蹤控制器,但設(shè)計(jì)實(shí)現(xiàn)起來復(fù)雜繁瑣.文獻(xiàn)[6–7]提出了基于PD的逆動(dòng)力學(xué)控制,將機(jī)械臂非線性模型解耦,通過將線性位置和速度跟蹤誤差替換為指數(shù)型誤差來實(shí)現(xiàn)對(duì)機(jī)械臂系統(tǒng)的控制,并利用李雅普諾夫直接法證明了系統(tǒng)的全局有限時(shí)間穩(wěn)定性.文獻(xiàn)[8]提出了一個(gè)受驅(qū)動(dòng)器約束的、飽和PID控制器,該控制器可以全局漸近調(diào)節(jié)機(jī)器人的運(yùn)動(dòng),并且能夠消除執(zhí)行器由于輸入轉(zhuǎn)矩過大而失效的可能性.針對(duì)具有不確定性和擾動(dòng)的機(jī)器人的軌跡跟蹤控制問題,文獻(xiàn)[9–10]設(shè)計(jì)了模糊滑?？刂破?在嚴(yán)重參數(shù)變化和干擾的情況下,從提高跟蹤性能的角度證明了該方法相對(duì)于傳統(tǒng)滑?？刂频膬?yōu)越性,但沒有解決滑?？刂频亩墩駟栴}.文獻(xiàn)[11]設(shè)計(jì)了滑模控制器與神經(jīng)網(wǎng)絡(luò)自適應(yīng)觀測(cè)器,在保證機(jī)器人能夠跟蹤上理想軌跡的基礎(chǔ)上減小了滑模的抖振.文獻(xiàn)[12]提出了一種基于自適應(yīng)反演非奇異快速末端滑?？刂频臋C(jī)器人跟蹤控制方法處理機(jī)械臂的擾動(dòng)和不確定性,該控制器的瞬態(tài)響應(yīng)快,跟蹤誤差小,抖動(dòng)小.文獻(xiàn)[13]設(shè)計(jì)了兩個(gè)滑?？刂破?分別用來補(bǔ)償不確定運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)的影響,保證控制精度.文獻(xiàn)[14]利用自適應(yīng)雅克比矩陣估計(jì)了未知運(yùn)動(dòng)學(xué)參數(shù),采用了3層神經(jīng)網(wǎng)絡(luò)對(duì)動(dòng)力學(xué)和未建模項(xiàng)動(dòng)態(tài)進(jìn)行補(bǔ)償.文獻(xiàn)[15]提出了一種基于狀態(tài)空間模型的約束預(yù)測(cè)控制方法,并結(jié)合前饋–反饋控制結(jié)構(gòu)抑制輸出端擾動(dòng),在仿真實(shí)驗(yàn)中,該算法體現(xiàn)了良好的控制性能以及抗擾動(dòng)能力.文獻(xiàn)[16]提出了一種徑向基函數(shù)(radial-basis function,RBF)神經(jīng)網(wǎng)絡(luò)控制器逼近機(jī)械臂中的不確定項(xiàng),并采用遺傳算法優(yōu)化出最佳的網(wǎng)絡(luò)權(quán)值,減少了控制器的計(jì)算時(shí)間,并且提高了系統(tǒng)的性能.針對(duì)重力對(duì)機(jī)械臂的影響,文獻(xiàn)[17]提出一種自適應(yīng)魯棒控制器對(duì)重力加速度進(jìn)行在線估計(jì),并通過魯棒控制器對(duì)系統(tǒng)的不確定性進(jìn)行了補(bǔ)償;文獻(xiàn)[18]在文獻(xiàn)[17]基礎(chǔ)上考慮了外部擾動(dòng),設(shè)計(jì)了一個(gè)自抗擾控制器并與PD控制器進(jìn)行了對(duì)比,證明該算法不僅可以抵抗擾動(dòng)還具有很強(qiáng)的魯棒性.

軌跡跟蹤廣泛應(yīng)用于機(jī)械臂的控制,也是機(jī)械臂控制的基本控制問題.目前廣泛存在的PID控制器高度依賴參數(shù)整定并且無法適用于系統(tǒng)所有工況;計(jì)算力矩法依賴系統(tǒng)精確的動(dòng)力學(xué)模型;自適應(yīng)控制器要求輔助系統(tǒng)狀態(tài)必須在約束子集中初始化,否則無法實(shí)現(xiàn)全局跟蹤;滑?？刂破鞔嬖诘亩墩駟栴}會(huì)導(dǎo)致機(jī)械臂的磨損,去抖振控制器設(shè)計(jì)復(fù)雜并會(huì)延長控制時(shí)間.這些都要求考慮設(shè)計(jì)參數(shù)整定簡(jiǎn)單、對(duì)系統(tǒng)中存在的不確定性魯棒、不依賴于系統(tǒng)精確數(shù)學(xué)模型、控制量變化平穩(wěn)且速度快的控制方法.

三步法控制是從現(xiàn)代汽車工程中提煉出來的一種基于模型的控制方法,主要解決跟蹤控制問題.目前主要應(yīng)用于單輸入單輸出系統(tǒng)以及反饋控制動(dòng)態(tài)之間相互獨(dú)立或是解耦的兩輸入兩輸出非線性系統(tǒng)的控制[19–21],該方法采用了“前饋–反饋”的控制結(jié)構(gòu),具有很強(qiáng)的魯棒性.本文將三步法控制器擴(kuò)展到狀態(tài)相互耦合的(三輸入三輸出)3自由度機(jī)械臂動(dòng)力學(xué)系統(tǒng),并且證明了機(jī)械臂末端軌跡可以漸近收斂到期望軌跡.在3自由度機(jī)械臂上進(jìn)行了仿真實(shí)驗(yàn)驗(yàn)證設(shè)計(jì)的軌跡跟蹤控制器的有效性.

本文第2節(jié)介紹3自由度機(jī)械臂的動(dòng)力學(xué)模型和正、逆運(yùn)動(dòng)學(xué)方程,同時(shí)描述系統(tǒng)的控制問題.第3節(jié)完成了三輸入三輸出的機(jī)械臂三步法控制器的設(shè)計(jì).第4節(jié)通過仿真驗(yàn)證了該控制器的有效性.第5節(jié)對(duì)本文進(jìn)行總結(jié).

2 問題描述與系統(tǒng)建模

3自由度機(jī)械臂模型如圖1所示.

圖1 3自由度機(jī)械臂模型Fig.1 3?DOF manipulator model

根據(jù)拉格朗日功能平衡法[22],建立其動(dòng)力學(xué)模型.為了建模方便,忽略了未建模項(xiàng)以及外界擾動(dòng),只考慮了摩擦項(xiàng)(忽略了手腕的部分,只考慮底座,第1個(gè)連桿和第2個(gè)連桿).以底座與前臂的轉(zhuǎn)軸處為原點(diǎn),建立如圖所示基礎(chǔ)坐標(biāo)系.其中:第1個(gè)連桿的長度為l2,質(zhì)量為m2;第2個(gè)連桿的長度為l3,質(zhì)量為m3;r2為第1個(gè)連桿的質(zhì)心距離關(guān)節(jié)2的距離,r3為第2個(gè)連桿的質(zhì)心距離關(guān)節(jié)3的距離.機(jī)械臂動(dòng)力學(xué)模型如下所示:

u為每一個(gè)關(guān)節(jié)的驅(qū)動(dòng)力矩u=(τ1τ2τ3)T.

根據(jù)空間幾何法,建立3自由度機(jī)械臂運(yùn)動(dòng)學(xué)方程如下:

其中(x,y,z)為機(jī)械臂末端位置.

對(duì)于已知末端軌跡,本文采用代數(shù)法對(duì)機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解[24],可得到4組逆解:

由于機(jī)械臂是一個(gè)漸變系統(tǒng),本文選取與上一時(shí)刻機(jī)械臂3個(gè)關(guān)節(jié)角度誤差絕對(duì)值之和最小作為逆運(yùn)動(dòng)學(xué)的解,并且把它作為當(dāng)前時(shí)刻機(jī)械臂的參考關(guān)節(jié)角度.

機(jī)械臂的軌跡跟蹤問題可以描述為:給定一個(gè)參考軌跡,找到一個(gè)容許控制u=(τ1τ2τ3)T,使得實(shí)際的關(guān)節(jié)角度與參考的關(guān)節(jié)角度誤差最小,從而使得機(jī)械臂末端軌跡跟蹤上參考軌跡.

3 三步法控制器設(shè)計(jì)

基于文獻(xiàn)[25]所提出的三步法非線性控制策略,設(shè)計(jì)了一種機(jī)械臂軌跡跟蹤控制器.本文采用了2階三步法,并應(yīng)用于控制量存在耦合的三輸入三輸出仿射非線性系統(tǒng).三步法控制器由3部分組成、依次分3步推導(dǎo),控制器推導(dǎo)的思路簡(jiǎn)單明了、結(jié)構(gòu)層次清晰.

首先將機(jī)械臂仿射非線性系統(tǒng)寫為如下標(biāo)準(zhǔn)形式:

其中:

f1,f2,g1和g2在定義域D ?內(nèi)是充分光滑的,并且g1(x)0,g2(x)0.

假設(shè)參考信號(hào)y?,?,?已經(jīng)通過求解逆運(yùn)動(dòng)學(xué)在線獲得.

對(duì)系統(tǒng)(4)的輸出端求導(dǎo),可得

3.1 第1步:類穩(wěn)態(tài)控制

該控制算法與當(dāng)前測(cè)量或估計(jì)狀態(tài)相關(guān),而不依賴系統(tǒng)真實(shí)的穩(wěn)態(tài),所以稱為類穩(wěn)態(tài)控制.

3.2 第2步:變參考前饋控制

針對(duì)復(fù)雜系統(tǒng),只通過穩(wěn)態(tài)控制很難達(dá)到期望的性能.引入一個(gè)前饋控制uf,使控制律u=us+uf滿足式(5).令則參考前饋控制律為

由式(7)可見uf與可變參考相關(guān),反映了期望變化時(shí)系統(tǒng)相應(yīng)的調(diào)節(jié)作用;同時(shí)uf的增益也與系統(tǒng)狀態(tài)相關(guān).

3.3 第3步:誤差反饋控制

由推導(dǎo)的假設(shè)條件看出,系統(tǒng)在類穩(wěn)態(tài)控制us與參考前饋控制uf作用下并不能保證輸出達(dá)到參考值.為了使得實(shí)際軌跡漸近跟蹤期望軌跡,在以上設(shè)計(jì)的基礎(chǔ)上引入了誤差反饋控制ue,得到控制律u=us+uf+ue.

定義系統(tǒng)跟蹤誤差為e1=y??y,將控制律代入式(5)中,可以將閉環(huán)系統(tǒng)誤差寫為

令e2=,則

為了使誤差趨近于零,本文采用Backstepping方法推導(dǎo)控制律.將e2看作是虛擬控制,定義第1個(gè)李雅普諾夫函數(shù)為

選擇虛擬控制輸入

當(dāng)e3→0時(shí),e2趨近于,系統(tǒng)=?k1e1?e3漸近穩(wěn)定.誤差e3的動(dòng)態(tài)特性滿足

定義包含誤差e3的李雅普諾夫函數(shù)

對(duì)上述李雅普諾夫函數(shù)求導(dǎo),可得

當(dāng)k3>0時(shí),因此可保證實(shí)際軌跡對(duì)期望軌跡的漸近跟蹤性能.

綜上,系統(tǒng)整體的控制律

3.4 穩(wěn)定性與魯棒性分析

由于機(jī)械臂系統(tǒng)存在未建模動(dòng)態(tài)、參數(shù)不確定性和外界擾動(dòng).為了分析問題方便,將系統(tǒng)存在的不確定性和擾動(dòng)統(tǒng)一定義為加性擾動(dòng)d∈R1,則有

將控制律(18)代入式(19)中,經(jīng)過整理可以得到閉環(huán)誤差系統(tǒng)動(dòng)態(tài)

誤差變量為χ,e1,e3,可見積分項(xiàng)的引入增加了誤差系統(tǒng)的維數(shù).取李雅普諾夫函數(shù)

求導(dǎo)可得

利用楊氏不等式,可以進(jìn)一步得到

因此誤差系統(tǒng)(20)?(22)對(duì)于擾動(dòng)輸入是輸入?狀態(tài)穩(wěn)定的[26].

接下來對(duì)跟蹤靜差進(jìn)行討論,首先分別對(duì)誤差系統(tǒng)進(jìn)行拉氏變換.

在未引入積分項(xiàng)的情況下:

將E3代入E1(s)中,整理可得

在引入積分項(xiàng)的情況下:

將E3I代入E1I(s)中,整理可得

對(duì)式(28)和式(31)使用終值定理,可以計(jì)算得到擾動(dòng)D(s)引起的跟蹤誤差,

其中:kI=k1+k2,kw=1+k0+k1k2.

下面討論不同類型擾動(dòng)下穩(wěn)態(tài)誤差的大小.

1)當(dāng)擾動(dòng)d是脈沖信號(hào)時(shí),其拉氏變換為D(s)=1,此時(shí)式(32)和式(33)中的穩(wěn)態(tài)誤差均為零,即e1(∞)=e1I(∞)=0;

2)當(dāng)擾動(dòng)d是階躍信號(hào)時(shí),拉氏變換此時(shí)未引入積分情況下的誤差終值為

而引入積分后的誤差終值仍為零,即e1I(∞)=0,這也說明積分有消除靜差的作用;

3)進(jìn)一步討論積分修正后靜差,如果擾動(dòng)d是斜率為ˉd的斜坡信號(hào),則

一般來說,擾動(dòng)的形式可能并不單一,通常很難確切知道,因此難以給出減小靜差的k0,k2的選取原則.

根據(jù)對(duì)系統(tǒng)魯棒性和穩(wěn)態(tài)誤差的分析,總結(jié)出如下的參數(shù)調(diào)整原則:

1)k1決定了跟蹤誤差e1的衰減快慢,k2決定e3的衰減快慢.從這個(gè)角度上看,k1和k2應(yīng)該選擇盡可能的大.但較大的k1和k2可能引起控制的高增益,造成噪聲放大.

3)盡管很難給出k0的具體選取原則,但從式(35)來看,根據(jù)可接受的靜差范圍應(yīng)該選擇盡可能大的k0.

4 仿真結(jié)果

為了驗(yàn)證本文提出的控制器的有效性,采用MATLAB 2016b 針對(duì)3 自由度機(jī)械臂進(jìn)行仿真實(shí)驗(yàn),機(jī)械臂具體參數(shù)如表1所示.

表1 機(jī)械臂具體參數(shù)Fig.1 Parameters of mechanical arm

4.1 雙紐線(lemniscate曲線)軌跡

設(shè)置參考軌跡為

圖2為初值(x0,y0,z0)=(0.465,0.264,0.107)時(shí)基于三步法的末端無負(fù)載和負(fù)載10 kg的機(jī)械臂雙紐線軌跡跟蹤仿真結(jié)果.

圖2 雙紐線軌跡跟蹤控制仿真結(jié)果Fig.2 Simulation results of lemniscate trajectory tracking control

圖3為初值(x0,y0,z0)=(0.234,0.365,?0.134)時(shí)的末端負(fù)載10 kg的機(jī)械臂雙紐線軌跡跟蹤仿真結(jié)果.

圖3 變初值雙紐線軌跡跟蹤仿真結(jié)果Fig.3 Simulation results of lemniscate trajectory tracking with variable initial value

圖4為初值(x0,y0,z0)=(0.597,0.408,0.109)時(shí)的末端負(fù)載10 kg的機(jī)械臂雙紐線軌跡跟蹤仿真結(jié)果.

圖4 變初值雙紐線軌跡跟蹤仿真結(jié)果Fig.4 Simulation results of lemniscate trajectory tracking with variable initial value

4.2 空間圓柱螺線軌跡

設(shè)置參考軌跡為

三步法不僅可以實(shí)現(xiàn)雙紐線軌跡跟蹤,還可以完成對(duì)空間的圓柱螺線等軌跡的跟蹤.

圖5為初值

時(shí)基于三步法控制的末端無負(fù)載、負(fù)載10 kg的空間圓柱螺旋跟蹤仿真結(jié)果.

圖6?7分別為初值

時(shí)無負(fù)載和負(fù)載10 kg時(shí)基于滑膜控制與三步法控制的空間圓形軌跡跟蹤仿真對(duì)比結(jié)果.

圖5 圓柱螺線軌跡跟蹤仿真結(jié)果Fig.5 Simulation results of cylindrical spiral trajectory tracking

圖6 無負(fù)載圓形軌跡跟蹤仿真對(duì)比結(jié)果Fig.6 Comparison of simulation results of unloaded circular trajectory tracking

圖7 負(fù)載10 kg圓形軌跡跟蹤仿真對(duì)比結(jié)果Fig.7 Comparison results of circular trajectory tracking simulation for load 10 kg

4.3 空間圓形軌跡

設(shè)置考軌跡為

圖8為初值

時(shí)的末端無負(fù)載且存在擾動(dòng)d=3 sin(2t)的基于滑膜控制與三步法控制的空間圓軌跡跟蹤仿真對(duì)比結(jié)果.

圖8 擾動(dòng)下的圓形軌跡軌跡跟蹤仿真對(duì)比結(jié)果Fig.8 Simulation comparison results of circular trajectory tracking with disturbance

由圖2可以看出當(dāng)負(fù)載由零變?yōu)?0 kg時(shí),三步法控制器可以很好的控制機(jī)械臂跟蹤雙紐線軌跡,使得軌跡漸近收斂,各個(gè)關(guān)節(jié)的控制輸入保持在合理的范圍內(nèi),系統(tǒng)動(dòng)態(tài)變化率不大,各關(guān)節(jié)角度與參考角度的穩(wěn)態(tài)誤差均在10?5rad以內(nèi),調(diào)整時(shí)間也均在1 s以內(nèi),兩種負(fù)載情況在穩(wěn)態(tài)誤差與調(diào)整時(shí)間上沒有明顯的差距.由圖3?4可以看出,三步法控制器的軌跡跟蹤精度與初值無關(guān).由圖5可以看出,當(dāng)負(fù)載由零變?yōu)?0 kg時(shí),三步法控制器依舊可以“穩(wěn)、準(zhǔn)、快”地使機(jī)械臂跟蹤空間圓柱螺線,使得軌跡漸近收斂,并且在改變初值的情況下也可以快速且穩(wěn)定地跟蹤上參考軌跡,穩(wěn)態(tài)誤差在10?5rad以內(nèi),調(diào)整時(shí)間在1 s內(nèi).

圖6?7可以看出,當(dāng)負(fù)載由零變?yōu)?0 kg時(shí),在沒有擾動(dòng)存在的情況下,三步法控制器比滑模控制器的跟蹤速度快,精度高并且不存在控制量抖振問題.圖8可以看出,當(dāng)存在一個(gè)正弦擾動(dòng)時(shí),三步法控制器相比于滑?？刂破鞯囊种茢_動(dòng)能力更好,控制輸出平穩(wěn),誤差小且跟蹤速度快.由上述仿真結(jié)果可以得出三步法控制器可以很好地控制機(jī)械臂漸近跟蹤期望軌跡,并且對(duì)末端負(fù)載質(zhì)量變化和擾動(dòng)具有魯棒性.

5 總結(jié)

本文設(shè)計(jì)了3自由度機(jī)械臂軌跡的三步法跟蹤控制器,該控制器包括類穩(wěn)態(tài)控制、參考前饋控制和誤差反饋控制三部分.通過控制機(jī)械臂的關(guān)節(jié)角度使機(jī)械臂末端跟蹤上期望軌跡.閉環(huán)系統(tǒng)是輸入到狀態(tài)穩(wěn)定的,因而對(duì)擾動(dòng)和未建模動(dòng)態(tài)具有一定的抑制能力.仿真結(jié)果表明本文設(shè)計(jì)的控制器可以使機(jī)械臂快速地跟蹤上期望軌跡、并且可以抑制末端負(fù)載質(zhì)量變化等不確定性的影響.