扭矩輸入有界的機(jī)器人模糊PD軌跡跟蹤

劉華山， 朱世強(qiáng)， 王宣銀

(浙江大學(xué)流體傳動(dòng)及控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，浙江杭州 310027)

0 引言

軌跡跟蹤一直以來都是機(jī)器人運(yùn)動(dòng)控制中的熱點(diǎn)問題。以往的各種控制算法幾乎都是以驅(qū)動(dòng)器能夠輸出任意所需扭矩為前提［1－3］。直到近年來，控制輸入有界的軌跡跟蹤控制才逐漸得到關(guān)注。

Loria和Nijmeijer［4］為確保扭矩控制輸入有界，率先將具有飽和特性的雙曲正切函數(shù)引入到控制律中。這一策略對(duì)后續(xù)的相關(guān)研究產(chǎn)生了較大影響，但其系統(tǒng)穩(wěn)定性局限于半全局穩(wěn)定，且證明和約束條件復(fù)雜。Burkov［5］首次將奇異攝動(dòng)理論應(yīng)用于僅有位置反饋的機(jī)器人軌跡跟蹤系統(tǒng)的穩(wěn)定性證明中，簡(jiǎn)化了系統(tǒng)的穩(wěn)定性證明，但其并未考慮控制輸入有界的問題。Stantibanez和 Kelly［6］在文獻(xiàn)［4］基礎(chǔ)上考慮了機(jī)器人關(guān)節(jié)處的粘性摩擦因素，并證明當(dāng)粘性摩擦系數(shù)足夠大時(shí)能夠保證系統(tǒng)的全局漸進(jìn)穩(wěn)定性，同時(shí)還運(yùn)用奇異攝動(dòng)穩(wěn)定性理論給出了輸入有界軌跡跟蹤系統(tǒng)的穩(wěn)定性證明，并通過在雙曲正切函數(shù)中引入增益系數(shù)，一定程度改善了軌跡跟蹤性能［7］，但該算法的控制律中比例增益P和微分增益D固定不變，使得跟蹤效果的改善程度非常有限。

本文提出一種僅需位置反饋檢測(cè)的模糊自適應(yīng)PD輸入有界軌跡跟蹤控制算法，在控制律中引入一種新型的改進(jìn)反正切函數(shù)確保了控制輸入有界，并結(jié)合模糊自適應(yīng)原理實(shí)現(xiàn)了PD增益的在線自整定。同時(shí)，通過引入適當(dāng)?shù)募s束條件，使得PD增益在自整定過程中，系統(tǒng)始終保持穩(wěn)定，較大幅度改善了機(jī)器人軌跡跟蹤性能。

1 機(jī)器人動(dòng)力學(xué)方程

n自由度旋轉(zhuǎn)關(guān)節(jié)機(jī)器人動(dòng)力學(xué)方程為

其中:q∈Rn為n維關(guān)節(jié)角向量;M(q)∈Rn×n為正定對(duì)稱慣性矩陣;C(q，)∈Rn×n為離心力和哥氏力矩陣;G(q)∈Rn為重力向量;Fv=diag(fv1，…，fvn)∈Rn×n(fvi>0，i=1，…，n)為粘性摩擦常系數(shù)對(duì)角矩陣;τ∈Rn為n維扭矩控制輸入向量。

動(dòng)力學(xué)方程(1)具有如下特性［4，7］

λm{M(q)}‖x‖2≤xTM(q)x≤λM{M(q)}‖x‖2，其中λm{·}(λM{·})表示矩陣最小(大)特征值，下同;

3)對(duì)于任意x，y，z∈Rn，離心力和哥氏力矩陣有

4)對(duì)于任意q∈Rn，有

其中:kG>0;kC>0;‖·‖M表示‖·‖的最大值(‖·‖m表示最小值)，下同。

2 軌跡跟蹤控制律

2.1 問題描述

2.2 反正切飽和函數(shù)

解決驅(qū)動(dòng)器輸入扭矩受限的問題，到目前為止，幾乎所有的策略都是在控制律中引入雙曲正切函數(shù)［4，6－7］。本文提出的軌跡跟蹤控制算法則采用經(jīng)過改進(jìn)的更為廣泛使用的反正切函數(shù)以保證控制輸入的有界性。如圖1所示，反正切函數(shù)atan的值域范圍較雙曲正切函數(shù)tanh更寬泛，且隨自變量趨近飽和的變化速度更平緩。同時(shí)，通過加入適當(dāng)增益系數(shù)，可調(diào)整其過零點(diǎn)斜率和趨近飽和速度，此特點(diǎn)將有助于改善軌跡跟蹤效果。

圖1 atan和tanh函數(shù)曲線Fig.1 Function curves of atan and tanh

令Bη={p∈Rn:‖p‖≤η}，Δ =diag(σ1，…，σn)∈Rn×n，p=［p1，…，pn］T∈Rn。其中常數(shù) σi≥1，η >0，σm和 σM分別表示最小和最大的 σi，i=1，…，n，下同。對(duì)于形如 Atan(Δp)=［atan(σ1p1)，…，atan(σnpn)］T的向量函數(shù)，得到

1)atan(σipi)為R域單調(diào)遞增奇函數(shù)，且滿足

2)對(duì)于任意pi∈R，有

3)Atan(Δp)對(duì)p一階連續(xù)可導(dǎo)，且有

5)對(duì)于任意p∈Bη，總存在常數(shù)

使得

2.3 控制律設(shè)計(jì)

為表述方便，分別用M，C，Cd和G替代M(q)，C(q，)，C(q)和G(q)。

扭矩輸入控制律設(shè)計(jì)如圖2所示，即

其中:Kp和Kd分別表示比例增益陣和微分增益陣;Ke和Kξ分別表示位置誤差增益陣和速度誤差增益陣，即

圖2 模糊自適應(yīng)軌跡跟蹤控制器Fig.2 Fuzzy self-tuning tracking controller

式(2)中，ξ作為真實(shí)速度誤差的替代信號(hào)，由式(3)所示的線性濾波器得到，使得系統(tǒng)可以在僅有位置檢測(cè)的情況下實(shí)現(xiàn)輸出反饋閉環(huán)控制，從而避免了速度、加速度測(cè)量干擾或速度計(jì)算誤差，增強(qiáng)了系統(tǒng)抗干擾能力，提高了控制精確度。

其中:r∈Rn為中間變量;U=diag(μ，…，μ)∈Rn×n，即 μi=μ >0，i=1，…，n。

此時(shí)，由式(2)和前述性質(zhì)可知，只需滿足

即可保證各關(guān)節(jié)驅(qū)動(dòng)力矩限制約束，即|τi|<τimax。其中:Mi和Cdi分別表示矩陣M和Cd的第i行向量;Gi和分別表示向量G和的第i個(gè)分量。

2.4 穩(wěn)定性分析

令x=［eTeT］T，z=ξ，ε=1/μ，將式(2)代入式(1)，整理可得

顯然，式(5)～式(7)所描述的系統(tǒng)即為時(shí)變非線性奇異攝動(dòng)系統(tǒng)［8］。令ε=0，得到式(5)的孤立根 ξ=，將其代入式(6)，得

此時(shí)，式(5)和式(8)構(gòu)成文獻(xiàn)［8］定理11.4中的降階系統(tǒng)(reduced system)，式(7)則為邊界層系統(tǒng)(boundary-layer system)。

命題1對(duì)于任意初始狀態(tài)［e(0)T(0)Tξ(0)T］T∈Br，其中Br={x∈R3n:‖x‖≤r}，r>0且任意大。如果滿足

總存在ε*>0，使得對(duì)于所有ε*>ε，能夠保證系統(tǒng)(5)～(7)在狀態(tài)原點(diǎn)指數(shù)穩(wěn)定。

證明 由奇異攝動(dòng)系統(tǒng)穩(wěn)定性理論可知，如果降階系統(tǒng)和邊界層系統(tǒng)的指數(shù)穩(wěn)定性能夠保證在上述條件下成立，命題即得證。證明過程分為兩個(gè)步驟:

(1)降階系統(tǒng)穩(wěn)定性分析。構(gòu)造Lyapunov函數(shù)V(t，e)，簡(jiǎn)寫為V則有

其中，常數(shù)υ>0。由式(1)的性質(zhì)2)和Atan(Δp)的性質(zhì)(5)，不等式的左邊得

其中:s11=υkpm，s22=T1－υT2，s12=s21= －υT3;

此時(shí)，可得

同理，不等式的右邊可得

其中，Q為常數(shù)對(duì)稱陣。

由式(15)可得

即對(duì)于任意［eT］T∈Br，只需滿足式(6)以及0< υ

(2)邊界層系統(tǒng)穩(wěn)定性分析。令t/ε=δ，構(gòu)造Lyapunov函數(shù)W(δ，ξ)，簡(jiǎn)寫為W則

其中，常數(shù)ω>0且足夠小。進(jìn)而得

即邊界層系統(tǒng)(7)在原點(diǎn)是指數(shù)穩(wěn)定的，且關(guān)于(t，x)是一致的。其中ξ無(wú)任何約束條件，δ越大即ε越小，ξ趨近于的速度越快。

因此，根據(jù)文獻(xiàn)［8］定理11.4，命題得證。

2.5 模糊自適應(yīng)PD

PD增益自適應(yīng)模糊控制器以關(guān)節(jié)位置誤差信號(hào)e和速度誤差替代信號(hào)ξ作為模糊控制輸入，輸出為PD增益Kp和Kd，如圖2所示。

(a)變量模糊化將ei，ξi，kpi和kdi的物理論域量化為模糊集上的論域{－3，－2，－1，0，1，2，3}，對(duì)應(yīng)的模糊子集為{NB，NM，NS，Z，PS，PM，PB}，子集中各元素分別表示“負(fù)大，負(fù)中，負(fù)小，零，正小，正中，正大”。ei，ξi，kpi和kdi的隸屬度函數(shù)如圖3所示。

PD增益自適應(yīng)調(diào)整過程中，確保系統(tǒng)的一致穩(wěn)定性是比較困難的［9］。為解決這一問題，對(duì)PD增益模糊化時(shí)，結(jié)合前述穩(wěn)定性分析，在考慮式(4)和式(9)的約束下進(jìn)行物理論域的選擇，以分別保證輸入控制律τ的有界性和系統(tǒng)的指數(shù)穩(wěn)定性。

圖 3 ei，ξi，kpi，kdi隸屬度函數(shù)Fig.3 Degree of membership for ei，ξi，kpi，kdi

(b)知識(shí)庫(kù)建立。知識(shí)庫(kù)包括數(shù)據(jù)庫(kù)和規(guī)則庫(kù)。數(shù)據(jù)庫(kù)存放所有輸入輸出變量模糊子集的隸屬度函數(shù)值，規(guī)則庫(kù)存放模糊控制規(guī)則［10］。模糊規(guī)則的制定依賴于經(jīng)驗(yàn)和多次嘗試，其語(yǔ)言表達(dá)形式如:

R:if(E1 is Y1＆ E2is Y2，…，＆ En is Yn)and(EC1 is Yn+1＆ EC2 is Yn+2，…，＆ ECn is Y2n).then(KP1 is Y2n+1，KP2 is Y2n+2，…，KPn is Y3n)and(KD1 is Y3n+1，KD2 is Y3n+2，…，KDn is Y4n)。其中 Ei，ECi，KPi和 KDi分別為變量 ei，ξi，kpi和 kdi的語(yǔ)言變量，Yi為模糊子集元素。

值得注意的是，在具有強(qiáng)耦合性的多關(guān)節(jié)機(jī)器人控制中，一般還應(yīng)考慮各關(guān)節(jié)之間的耦合作用，即kpi，kdi參數(shù)的整定，需考慮 ei，ξi的影響，其中 i≠j。同時(shí)，為簡(jiǎn)化模糊規(guī)則，可酌情對(duì)小慣量關(guān)節(jié)所產(chǎn)生的耦合作用進(jìn)行適當(dāng)忽略，以提高控制效率。

(c)解模糊采用重心法解模糊，即

其中，xi為輸出變量論域中的元素，μ(i)為相應(yīng)隸屬度為解模糊輸出量。再經(jīng)量化處理至物理論域即可得到實(shí)際控制量。

3 仿真結(jié)果

兩自由度旋轉(zhuǎn)關(guān)節(jié)直驅(qū)機(jī)器人如圖4所示。

圖4 兩自由度直驅(qū)機(jī)器人Fig.4 2-DOF direct-driven robot manipulator

其動(dòng)力學(xué)各參數(shù)為

令U=diag(500，500)，并通過多次試驗(yàn)擇優(yōu)選取位置和速度誤差增益陣為Ke=Kξ=diag(3，2)。

假定各關(guān)節(jié)期望跟蹤軌跡分別為:

實(shí)際控制場(chǎng)合中初始軌跡跟蹤誤差不總為零，這種情況下在未考慮輸入有界的傳統(tǒng)扭矩控制中通常會(huì)導(dǎo)致初始扭矩控制輸入過大，甚至超出關(guān)節(jié)扭矩最大允許值。不失一般性，此處在1，2關(guān)節(jié)的期望軌跡中分別設(shè)置5°和10°的初始值，以制造初始誤差不為零的情況，從而驗(yàn)證算法的有效性。

考慮到第2關(guān)節(jié)慣量和質(zhì)量等參數(shù)與第1關(guān)節(jié)比較懸殊，模糊規(guī)則制定時(shí)，將誤差e1，ξ1對(duì)第2關(guān)節(jié)PD參數(shù)kp2，kd2的耦合影響忽略不記，得到簡(jiǎn)化的規(guī)則如表1所示。

表1 模糊規(guī)則表Table 1 Fuzzy rules for kpi，kdi

模糊邏輯推理策略采用為Mamdani型。模糊控制器各參數(shù)設(shè)置如表2所示。其中，為保證扭矩控制輸入的有界且系統(tǒng)穩(wěn)定，kpi，kdi物理論域的選擇需考慮式(4)和(9)所產(chǎn)生的約束，即kp1+kd1<56.2，kp2+kd2<7.8;kdm=min{kd1，kd2}>0.38。

表2 模糊控制器參數(shù)Table 2 Parameters of fuzzy controller

數(shù)值仿真中，將本文算法與文獻(xiàn)［7］算法在相同參數(shù)條件下進(jìn)行對(duì)比。由于存在初始跟蹤誤差，文獻(xiàn)［7］算法PD參數(shù)的范圍選擇具有一定的局限性，尤其是比例增益P不能過大，否則引起嚴(yán)重的超調(diào)和震蕩。結(jié)合經(jīng)驗(yàn)并通過多次試驗(yàn)擇優(yōu)選取Kp=［4.5 4］T，Kd=［1.5 0.5］T。本文算法 PD 增益則可在模糊控制器作用下實(shí)現(xiàn)在線自適應(yīng)整定，如圖5所示，從而使得控制系統(tǒng)的超調(diào)量和調(diào)整時(shí)間得到較大幅度改善，其中大慣量關(guān)節(jié)(第1關(guān)節(jié))尤為明顯，如圖6～7所示。

如圖8～9所示，在保證扭矩輸入的有界性方面，本文和文獻(xiàn)［7］的算法幾乎一致，第1，2關(guān)節(jié)的最大控制輸入扭矩值分別約為80 N·m和5 N·m，均遠(yuǎn)小于最大限制120 N·m和20 N·m，并在初始誤差不為零的情況下，能夠有效避免初始扭矩控制輸入過大的現(xiàn)象。

圖5 各關(guān)節(jié)PD參數(shù)Fig.5 PD gains of each joint

圖6 第1關(guān)節(jié)軌跡跟蹤誤差比較Fig.6 Comparison of tracking errors of 1st joint

圖7 第2關(guān)節(jié)軌跡跟蹤誤差比較Fig.7 Comparison of tracking errors of 2nd joint

圖8 第1關(guān)節(jié)扭矩控制輸入比較Fig.8 Comparison of torque inputs of 1st joint

圖9 第2關(guān)節(jié)扭矩控制輸入比較Fig.9 Comparison of torque inputs of 2nd joint

4 結(jié)語(yǔ)

本文結(jié)合模糊自適應(yīng)控制策略，設(shè)計(jì)了扭矩輸入有界的機(jī)器人軌跡跟蹤控制器。利用改進(jìn)反正切函數(shù)的飽和性質(zhì)，保證了扭矩輸入控制律的有界性。對(duì)位置跟蹤誤差信息進(jìn)行線性濾波產(chǎn)生速度誤差替代信號(hào)，避免了閉環(huán)控制中的轉(zhuǎn)速反饋測(cè)量。通過PD增益的模糊自適應(yīng)在線整定，較大幅度改善了控制系統(tǒng)動(dòng)態(tài)響應(yīng)特性。理論分析和仿真比較結(jié)果表明在PD增益自適應(yīng)調(diào)整過程中，系統(tǒng)始終保持穩(wěn)定，并在同等參數(shù)條件下，軌跡跟蹤效果更優(yōu)。

［1］PADEN B，PANJA R.Globally asymptotically stable PD+controller for robot manipulators［J］.International Journal of Control，1988，47(6):1697－1712.

［2］LORIA A，ORTEGA R.On tracking control of rigid and flexible joint robots［J］.International Journal of Applied Mathematics and Computer Science，special issue on Mathematical Methods in Robotics，1995，5(2):329 －341.

［3］DE QUEIROZ M S，HU J，DAWSON D M.Adaptive position/force control of robot manipulators without velocity measurements:theory and experiments［J］.IEEE Transactions on Systems，Man，and Cybernetics-Part B:Cybernetics，1997，27(5):796 －809.

［4］LORIA A，NIJMEIJER H.Bounded output feedback tracking control of fully actuated Euler-Lagrange systems［J］.Systems＆Control Letters，1998，33:151 －161.

［5］BURKOV I V.Stabilization of a natural mechanical system without measuring its velocities with application to the control of a rigid body［J］.Journal of Applied Mathematics and Mechanics，1998，62(6):853－862.

［6］SANTIBANEZ V，KELLY R.Global asymptotic stability of bounded output feedback tracking control for robot manipulators［C］//Proceedings of the40th IEEE Conference on Decision and Control.Orlando:IEEE，2001:1378－1379.

［7］MORENO-VALENZUELA J，SANTIBANEZ V，CAMPA R.On output feedback tracking control of robot manipulators with bounded torque input［J］.International Journal of Control，Automation，and systems，2008，6(1):76－85.

［8］KHALIL H K.Nonlinear Systems［M］.Beijing:Publishing House of Electronics Industry，2007:456－469.

［9］MEZA J L，SANTIBANEZ V，CAMPA R.An estimate of the domain of attraction for the PID regulator of manipulators［J］.International Journal of Robotics and Automation，2007，22(3):187－195.

［10］CARVAJAL J，CHEN G R，OGMEN H.Fuzzy PID controller:Design，performance evaluation，and stability analysis［J］.Information Sciences，2000，123(3/4):249 －270.