徐 旭，于海生，于金鵬，劉旭東，吳賀榮

（青島大學(xué) 自動化與電氣工程學(xué)院，青島 266071）

0 引言

作為當(dāng)今工業(yè)領(lǐng)域最常見的工業(yè)機(jī)器人之一的關(guān)節(jié)機(jī)器人，主要表現(xiàn)形式為工業(yè)機(jī)械臂，在自動化裝配領(lǐng)域應(yīng)用廣泛。永磁同步電機(jī)（PMSM）廣泛的應(yīng)用于機(jī)器人關(guān)節(jié)驅(qū)動系統(tǒng)中，得益于其具有良好的可控性能、結(jié)構(gòu)簡單、調(diào)速范圍寬、準(zhǔn)確度高、噪聲低等特點[1]。當(dāng)前應(yīng)用于機(jī)器人驅(qū)動系統(tǒng)的控制方法中，比例微分（PD）控制方法應(yīng)用廣泛、設(shè)計簡單，但受參數(shù)影響較大，而且抗干擾能力較差，很難廣泛的應(yīng)用于實際工程當(dāng)中去；拉格朗日公式法可以推導(dǎo)出機(jī)器人動力學(xué)一般形式模型，有效解決了多關(guān)節(jié)機(jī)器人動態(tài)建模問題，適用于大部分機(jī)器人動力學(xué)求解問題[2,3]，但推導(dǎo)過程相對麻煩，而且很難和機(jī)器人驅(qū)動電機(jī)控制相結(jié)合；魯棒自適應(yīng)控制能夠有效解決系統(tǒng)模型中不確定性問題，能夠?qū)崿F(xiàn)對機(jī)器人關(guān)節(jié)位置及速度的有效跟蹤，既能夠保證不確定系統(tǒng)的穩(wěn)定性而且能夠根據(jù)實際系統(tǒng)的需求修正控制規(guī)律及參數(shù)[4,5]，但是設(shè)計復(fù)雜；文獻(xiàn)[6]提出一種簡單的滑?？刂品桨?，不依賴獨立穩(wěn)定的不連續(xù)曲面，減少了設(shè)計的復(fù)雜性；文獻(xiàn)[7]介紹了一種基于干擾上界的滑?？刂?，但單純的滑?？刂葡涣硕墩駟栴}；文獻(xiàn)[8]設(shè)計基于反饋線性化控制算法，具有良好的控制效果，但是忽略非線性部分的線性化模型將不再能反映系統(tǒng)特性；神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制可以逼近機(jī)器人動態(tài)模型，有效解決復(fù)雜的機(jī)器人控制系統(tǒng)的非線性和不確定性，使機(jī)器人在理想動態(tài)下有效的跟蹤給定軌跡，具有良好的控制性能[9,10]；神經(jīng)網(wǎng)絡(luò)控制通過不斷的學(xué)習(xí)，使系統(tǒng)具有更好的適應(yīng)性以及更好的動態(tài)性能[11]；模糊控制可以很好的和其他控制方法結(jié)合使用，能夠?qū)崿F(xiàn)對復(fù)雜系統(tǒng)的有效控制，但是信息簡單的模糊存在精度低、動態(tài)品質(zhì)差的缺點[12,13]；反步法設(shè)計簡單，能夠?qū)崿F(xiàn)PMSM控制系統(tǒng)的完全解耦，可用于機(jī)器人的高精度運動控制[14,15]。基于能量受控的哈密頓方法廣泛應(yīng)用于永磁同步電機(jī)的控制[16,17]，為實現(xiàn)機(jī)器人動力學(xué)與驅(qū)動電機(jī)控制結(jié)合，本文采用哈密頓控制方法設(shè)計機(jī)器人關(guān)節(jié)位置控制器，使系統(tǒng)具有更好的穩(wěn)態(tài)特性，但是單獨采用PCH控制系統(tǒng)動態(tài)響應(yīng)較慢，所以設(shè)計協(xié)調(diào)控制策略，與PD加重力補(bǔ)償進(jìn)行協(xié)調(diào)控制，此時系統(tǒng)既具有PD快速的動態(tài)響應(yīng)，也會保持PCH控制的穩(wěn)態(tài)特性；用反步法對驅(qū)動電機(jī)電流進(jìn)行控制，然后通過電流轉(zhuǎn)換將兩個控制器結(jié)合，實現(xiàn)機(jī)器人與驅(qū)動電機(jī)的整體控制，而且系統(tǒng)具有較好的抗干擾能力。

1 機(jī)器人模型

1.1 機(jī)器人動力學(xué)與運動學(xué)模型

n關(guān)節(jié)機(jī)器人拉格朗日動力學(xué)方程可表示為[18]：

式中，q為n維關(guān)節(jié)轉(zhuǎn)動角位移向量；為n維關(guān)節(jié)轉(zhuǎn)動角速度向量；為n維關(guān)節(jié)轉(zhuǎn)動角加速度向量；D(q)∈Rn×n為對稱正定慣性方陣；為哥式力與離心力矩陣；Rf為摩擦阻力系數(shù)矩陣，G(q)∈Rn為重力矩向量；τ為輸入力矩向量。

兩關(guān)節(jié)機(jī)器人如圖1所示，L1、L2分別為關(guān)節(jié)機(jī)器人的兩個關(guān)節(jié)長度，q1、q2分別為兩個關(guān)節(jié)的角位移，m1、m2分別表示兩個關(guān)節(jié)的質(zhì)量。

圖1 兩關(guān)節(jié)機(jī)器人空間坐標(biāo)示意圖

式(1)中參數(shù)矩陣和向量可求得為：

機(jī)器人末端C點與基座A點的關(guān)系可表示為：

式中，c12=cos(q1+q2)；s12=sin(q1+q2)；s1=sinq1；c1=cosq1。

由機(jī)器人末端期望位置(xd,yd)，根據(jù)機(jī)器人逆運動學(xué)，可分別求得兩個關(guān)節(jié)的角位移：

1.2 PMSM數(shù)學(xué)模型

PMSM在d-q坐標(biāo)系下數(shù)學(xué)模型[19]：

式中，i=1,2；Lid和Liq分別d軸和q軸的定子電感；iid和iiq分別為d軸和q軸的定子電流；uid和uiq分別為d軸和q軸的定子電壓；Ris為定子電阻；nip為極對數(shù)；wi為轉(zhuǎn)子機(jī)械角速度；Jim為轉(zhuǎn)動慣量；τiL為負(fù)載轉(zhuǎn)矩；Φi為永磁同步電機(jī)產(chǎn)生的磁鏈；θi為永磁同步電機(jī)角位移；qi為機(jī)器人關(guān)節(jié)角位移。

2 機(jī)器人控制方案

機(jī)器人系統(tǒng)控制方案如圖2所示，給定機(jī)器人期望的軌跡坐標(biāo)，根據(jù)機(jī)器人逆運動學(xué)可求得兩個關(guān)節(jié)期望的角位移，通過機(jī)器人關(guān)節(jié)位置PCH和PD控制器，分別求得到機(jī)器人關(guān)節(jié)驅(qū)動力矩，設(shè)計協(xié)調(diào)控制策略得到協(xié)調(diào)后的力矩，然后經(jīng)過電流變換，采用反步法對PMSM電流進(jìn)行控制，從而實現(xiàn)機(jī)器人與PMSM控制的有機(jī)結(jié)合。

圖2 二自由度關(guān)節(jié)機(jī)器人驅(qū)動系統(tǒng)控制框圖

2.1 機(jī)器人位置協(xié)調(diào)控制設(shè)計

選取哈密頓函數(shù)為：

其中， q∈R2、p∈R2分別為角位移和動量向量，V(q)為勢能，且q、p的關(guān)系可表示為：

則機(jī)器人動力學(xué)的哈密頓模型可表示為[20]：

式中I2為二維單位矩陣，τ1=[τ11τ21]T為輸入力矩向量，取g=diag{1 1}。

取機(jī)器人動力學(xué)系統(tǒng)的期望哈密頓函數(shù)：

由式(12)可求得PCH控制器：

取Dd(q)=D(q)，J2(q,p)=0，可求得：

設(shè)計機(jī)器人末端位置PD加重力補(bǔ)償控制器。選取PD加重力補(bǔ)償控制律：

可求得PD加重力補(bǔ)償控制器為：

構(gòu)造協(xié)調(diào)函數(shù)：

式中C1(t)，C2(t)分別為PCH和PD加重力補(bǔ)償控制器的協(xié)調(diào)參數(shù)；a，b為常數(shù)。

設(shè)計協(xié)調(diào)控制策略：

根據(jù)實際需求設(shè)定機(jī)器人關(guān)節(jié)角位移最大誤差值，當(dāng)，(i=1,2)時，系統(tǒng)投入?yún)f(xié)調(diào)。

式中Kd，Rf均為正定矩陣，，所以協(xié)調(diào)控制系統(tǒng)是漸近穩(wěn)定的。

2.2 電流控制器設(shè)計

利用隱極(Lid=Liq)PMSM的=0控制原理可知[22]，由式(5)可求得：

利用反步法原理設(shè)計電流控制器[23,24]。

進(jìn)而可求得：

進(jìn)而可求得：

則電流控制器可表示為：

2.3 穩(wěn)定性分析

取整個系統(tǒng)Lyapunov函數(shù)V=V2+V1q+V2q，由以上分析可得機(jī)器人關(guān)節(jié)位置PCH控制系統(tǒng)和PMSM電流控制系統(tǒng)均為漸近穩(wěn)定，因此整個系統(tǒng)是漸近穩(wěn)定的。

3 仿真結(jié)果

機(jī)器人參數(shù)：L1=L2=1m，m1=m2=1kg，Rf=diag{(2.1 0.3)}，Kv=diag{(10.75 6)}，ρ=1，Kp=diag{20 100)}，Kd=diag{3 10)}。PMSM參數(shù)：Lid=Liq=0.0085H，Ris=2.875Ω，nip=4，Φi=0.175Wb，Jim=0.02kg.m2，k1d=k2d=k1q=k2q=10000，a=0.2，b=0.5。機(jī)器人末端位置由點（1.5，0.5）運動到點（0.5，1.5），t=4s時系統(tǒng)負(fù)載轉(zhuǎn)矩發(fā)生變化。由圖3、圖4可以看出協(xié)調(diào)控制與PD加重力補(bǔ)償控制動態(tài)特性曲線幾乎重合；穩(wěn)態(tài)特性曲線與PCH控制幾乎重合。協(xié)調(diào)控制能夠很好的結(jié)合兩種控制方法的優(yōu)點，既具有很快的動態(tài)響應(yīng)速度，而且具有一定的抗負(fù)載轉(zhuǎn)矩擾動能力。

圖3 機(jī)器人關(guān)節(jié)末端x軸坐標(biāo)

圖4 機(jī)器人關(guān)節(jié)末端y軸坐標(biāo)

4 結(jié)束語

本文針對二自由度機(jī)器人關(guān)節(jié)驅(qū)動系統(tǒng)進(jìn)行研究，為提高機(jī)器人末端位置控制效果，運用依靠互聯(lián)配置、阻尼注入和能量成型的PCH和PD加重力補(bǔ)償控制方法分別求取機(jī)器人關(guān)節(jié)位置控制器，并設(shè)計協(xié)調(diào)控制方案；用反步法將復(fù)雜的機(jī)器人驅(qū)動電機(jī)控制系統(tǒng)分解為中間子系統(tǒng)，然后把子系統(tǒng)集成起來完成對機(jī)器人驅(qū)動電機(jī)進(jìn)行控制，并通過電流變換將機(jī)器人關(guān)節(jié)位置控制與驅(qū)動電機(jī)電流控制相結(jié)合。由仿真結(jié)果可以看出協(xié)調(diào)控制機(jī)器人末端位置可以快速并且準(zhǔn)確的到達(dá)期望位置，而且與PD加重力補(bǔ)償和PCH控制方法單獨控制做出對比，證明此方案既具有較快的動態(tài)過程，而且有更好的控制性能和抗負(fù)載轉(zhuǎn)矩擾動的能力，所提出的控制方法具有很好的應(yīng)用前景。

圖5 協(xié)調(diào)控制關(guān)節(jié)末端軌跡

圖6 協(xié)調(diào)函數(shù)曲線圖