基于廣義Hamilton系統(tǒng)的機(jī)械人雙目視覺(jué)伺服控制器*

鄒立穎 ，朱 磊 ，苗鳳娟 ，楊 波

（1.齊齊哈爾大學(xué) 通信與電子工程學(xué)院，黑龍江 齊齊哈爾 161006；2.山西大學(xué) 數(shù)學(xué)科學(xué)學(xué)院，山西 太原 030000）

近年來(lái)，機(jī)器人視覺(jué)伺服控制成為機(jī)器人領(lǐng)域內(nèi)研究的熱點(diǎn)之一[1-2]。視覺(jué)伺服是利用視覺(jué)傳感器獲得目標(biāo)物體的圖像信息作為反饋信號(hào)，對(duì)機(jī)器人進(jìn)行閉環(huán)控制[3]。目前視覺(jué)伺服控制所面臨的主要問(wèn)題是全局漸近穩(wěn)定性、物體深度信息的獲取、干擾抑制、系統(tǒng)鎮(zhèn)定、控制精度等問(wèn)題。

廣義Hamilton系統(tǒng)的Hamilton函數(shù)是系統(tǒng)的總能量，在一定條件下可構(gòu)成系統(tǒng)的李亞普諾夫函數(shù)。因此，關(guān)于廣義Hamilton系統(tǒng)理論的研究得到了研究人員的廣泛關(guān)注[4-5]。參考文獻(xiàn)[5]開(kāi)發(fā)了一種新的數(shù)學(xué)工具，構(gòu)建了機(jī)器人系統(tǒng)廣義Hamilton框架，為廣義Hamilton系統(tǒng)在機(jī)器人控制領(lǐng)域的發(fā)展奠定了基礎(chǔ)。參考文獻(xiàn)[6]研究了基于圖像的直接視覺(jué)伺服控制器的設(shè)計(jì)與仿真。參考文獻(xiàn)[7]基于機(jī)器人動(dòng)力學(xué)特性和位置的視覺(jué)反饋信息，建立了一類視覺(jué)反饋控制系統(tǒng)。但參考文獻(xiàn)[7]中由于考慮了機(jī)器人動(dòng)力學(xué)特性，李亞普諾夫函數(shù)的設(shè)計(jì)過(guò)于復(fù)雜，導(dǎo)致系統(tǒng)穩(wěn)定性分析較困難。而廣義Hamilton系統(tǒng)可以克服參考文獻(xiàn)[7]的不足。因此，本文基于廣義Hamilton系統(tǒng)理論，研究了視覺(jué)伺服控制器設(shè)計(jì)問(wèn)題。

本文基于參考文獻(xiàn)[6]提出的雙目視覺(jué)模型，考慮機(jī)器人動(dòng)力學(xué)特性，在參考文獻(xiàn)[5]的基礎(chǔ)上構(gòu)建了廣義哈密爾頓視覺(jué)伺服系統(tǒng)（GHVFS）。針對(duì)GHVFS，提出了基于廣義Hamilton框架下的視覺(jué)伺服控制器設(shè)計(jì)方法。該方法采用的視覺(jué)模型無(wú)需獲取深度信息，也不需要物體的模型，簡(jiǎn)化了控制器結(jié)構(gòu)。選用Hamilton函數(shù)為李亞普諾夫函數(shù),使系統(tǒng)穩(wěn)定性分析變得簡(jiǎn)單。對(duì)于GHVFS下的L2增益干擾抑制問(wèn)題，提出了具有L2增益性能視覺(jué)伺服控制器，給出了閉環(huán)系統(tǒng)穩(wěn)定性證明。最后給出的仿真實(shí)驗(yàn)驗(yàn)證了該方法的可行性和有效性。

1 視覺(jué)模型

[6]提出的雙目視覺(jué)模型如下[6]：

mi=[x y c]T為目標(biāo)物體的第i個(gè)特征點(diǎn)，vC=[TXTYTZ]T為攝像機(jī)線速度，ωC=[ωXωYωZ]T為攝像機(jī)角速度，攝像機(jī)運(yùn)動(dòng)速度VC=[vTcwTc]T，圖像特征點(diǎn)速度m˙i與 VC滿足：

其中，Ji(mi)∈R3×6表示特征點(diǎn)mi的圖像雅克比矩陣。

機(jī)械手關(guān)節(jié)速度與終端速度的關(guān)系：

其中，J(q)為機(jī)械手雅克比矩陣，q∈Rn為關(guān)節(jié)位置向量。

具有n個(gè)特征點(diǎn)的物體圖像速度與關(guān)節(jié)速度關(guān)系為：

其中m=[mT1mT2…mTn]T為物體圖像特征向量，J(m)=[JT1(m1) JT1(m2)…JTn(mn)]T為物體的圖像雅克比矩陣，(m，q)=J(m)J(q)為物體復(fù)合雅克比矩陣。

2 機(jī)器人動(dòng)力學(xué)模型

機(jī)械人動(dòng)力學(xué)模型為：

其中τ∈Rn為關(guān)節(jié)輸入力矩向量，M(q)為正定對(duì)稱的慣性矩陣，C(q，q˙)為非線性離心力項(xiàng)，G(q)為重力矩向量。

3 雙目視覺(jué)伺服系統(tǒng)的廣義Hamilton框架

為建立雙目視覺(jué)伺服系統(tǒng)下的廣義Hamilton框架，給出如下命題：

命題1[5]：

系統(tǒng)（5）變?yōu)椋?/p>

命題2[5]：

系統(tǒng)（6）對(duì)下列關(guān)系對(duì)任意的p∈Rn和 q∈Rn成立：

其中 En(i，j)∈Rn×n為交換矩陣，(j-1)n+i]。

取系統(tǒng)Hamilton函數(shù)為：

其中q∈Rn是關(guān)節(jié)位置向量，p=M(q)˙是系統(tǒng)的廣義動(dòng)量，m*為期望圖像特征向量，m為圖像特征向量，φ1和φ2為相應(yīng)維數(shù)的系數(shù)矩陣，β是與G(q)的不確定部分相關(guān)聯(lián)的未知向量，是 β的估計(jì)。

對(duì)式(8)求導(dǎo)得：

由 p=M(q)q˙得 ：

假設(shè)G(q)的不確定部分線性地依賴于未知向量β∈Rl，即存在矩陣 φ(q)∈Rn×l使得 G(q)=G0(q)+φ(q)β，G0(q)∈Rn表示G(q)的標(biāo)稱部分。

為構(gòu)建廣義Hamilton框架，設(shè)計(jì)反饋律：

其中u∈Rn為新的控制輸入，KD=KTD∈Rn×n。

將(7)、(14)代入(13)中得：

自適應(yīng)估計(jì)律為：

式(9)、(15)、(16)構(gòu)成了 GHVFS 系統(tǒng)的機(jī)器人動(dòng)力學(xué)特性部分。

在機(jī)械手末端端速度中引入一個(gè)新的控制輸入：

其中，u1∈Rn是新的控制輸入，J+(m)是 J(m)的廣義逆矩陣。

將（17）代入（4）中，再由（10）、（12）式得 GHVFS 系統(tǒng)的視覺(jué)伺服部分：

其中KP=KTP∈Rm×m。

由式(9)、（15）、（16）、（18）得到 GHVFS 系統(tǒng)：

4 L2控制器設(shè)計(jì)

考慮不確定性，雙目視覺(jué)伺服系統(tǒng)的廣義哈密爾頓實(shí)現(xiàn)為：

其 中 ，g2(X)=[0 In0 0]T∈R(2n+l+m)×b，h(X)是 權(quán) 重 矩 陣 ，X∈R2n+l+m，u∈RS，ω∈L2為等值干擾信號(hào)，y∈RP是輸出，z∈Rq是罰信號(hào)。

定義：考慮系統(tǒng)(21)及 γ 供給率 s(ω，z)=(1/2)(γ2||ω||2-||z||2)，||ω||、||z||均指向量的歐幾里得范數(shù)， 對(duì)于 γ>0，若存在控制律u及半正定的光滑存儲(chǔ)函數(shù)H，使得耗散不等式H˙≤(γ2||ω||2-||z||2)對(duì)于任意的干擾 ω 成立，則稱該系統(tǒng)是γ耗散的[8]。

由于系統(tǒng)的γ耗散性與L2性能之間關(guān)系是等價(jià)的[9]。L2性能準(zhǔn)則設(shè)計(jì)問(wèn)題可以敘述如下：對(duì)于給定系統(tǒng)(21)，設(shè)計(jì)控制器 u，使得閉環(huán)系統(tǒng)對(duì)任意正數(shù) γ，滿足如下性能準(zhǔn)則：

(1)當(dāng)ω=0時(shí)，閉環(huán)系統(tǒng)在平衡點(diǎn)m*處是漸近穩(wěn)定的。

(2)存在半正定存儲(chǔ)函數(shù) H(X)，對(duì)于任意干擾 ω，使得耗散不等式成立。

(3)閉環(huán)系統(tǒng)在圖像平面期望特征點(diǎn)m*是可檢測(cè)的。

滿足準(zhǔn)則（1）、（3）的控制律為：

證明：

又由 Φ(X)≥0，所以 Q(X)=▽HTΦ(X)▽H≥0。根據(jù)定義 1及 γ耗散性與 L2性能等價(jià)性可得，控制律（23）是系統(tǒng)（21）的一個(gè)L2干擾抑制控制器。

定理 2：假設(shè) ω=0，由式(14)、式(23)、式(21)構(gòu)成的閉環(huán)系統(tǒng)在目標(biāo)平衡點(diǎn)Xe=[q 0β(m*)T]是漸近穩(wěn)定的。

證明：由 H(X)的構(gòu)成可知，H(X)正定并且在平衡點(diǎn)Xe取得嚴(yán)格極小。

當(dāng) ω=0 時(shí)，由式（22）及（24）得：

因此，閉環(huán)系統(tǒng)穩(wěn)定且收斂于包含在下列集合內(nèi)的最大不變集上。

當(dāng)?t≥0時(shí)，由于z=hgT1▽H=0，h(X)列滿秩，y=gT2▽H=0，可得≡0，≡0。即p(t)=M(q)q˙=M(q)≡0，由≡0知 φ(m-m*)=0，又由φ正定，所以 m=m*，即▽H(X(t))=▽HX(t))=0，?X∈S。 因此，S 中僅包含一個(gè)點(diǎn)Xe，由LaSalle不變?cè)淼瞄]環(huán)系統(tǒng)漸近穩(wěn)定。

5 系統(tǒng)仿真試驗(yàn)

為驗(yàn)證本文所提出的方法，采用圖1所示兩關(guān)節(jié)機(jī)械手視覺(jué)伺服系統(tǒng)進(jìn)行仿真分析。

圖1 帶有兩個(gè)攝像機(jī)的2DOF機(jī)器人系統(tǒng)

考慮具有不確定性的兩關(guān)節(jié)機(jī)械手動(dòng)力學(xué)方程：

取矩形圖形頂點(diǎn)為目標(biāo)的特征點(diǎn)，初始位置圖像坐標(biāo)為m1=[44.755 1；71.737 3；-44.755 1]pixels，m2=[-44.755 1；71.737 3；-134.265 2]pixels，m3=[-44.755 0；-17.772 9；-134.265 2]pixels，m4=[44.755 1；-17.772 9；-44.755 1]pixels。期望位置圖像坐標(biāo)為 m1*=[68.448 9；27.182 1；-68.448 9]pixels，m2*=[-68.448 9；27.182 1；-205.346 8]pixels，m3*=[-68.448 9；-109.715 8；-205.346 8]pixels，m4*=[68.448 9；-109.715 8；-68.448 9]。 q1(0)=0°，q2(0)=90°，(0)=0，(0)=0，ω=[20sin(t)20sin(t)]T，γ=0.252，KD=diag{0.1，0.8}，h(X)=diag{0.05，0.05}。

圖2～圖7為目標(biāo)深度在攝像機(jī)坐標(biāo)系中變化時(shí)的仿真結(jié)果。圖2表示圖像誤差漸近收斂到零，表明特征點(diǎn)在圖像平面的位置收斂到期望位置。圖3表示了攝像機(jī)的運(yùn)動(dòng)軌跡。圖4為機(jī)械手的運(yùn)動(dòng)軌跡，機(jī)械手的初始狀態(tài)為 q1=0°，q2=90°。圖 5為不確定參數(shù)的變化曲線。圖6～圖7分別為左、右攝像機(jī)在像平面的運(yùn)動(dòng)軌跡。由仿真結(jié)果可以看出，在存在不確定性干擾的前提下，所設(shè)計(jì)的控制器能夠使當(dāng)前圖像特征漸近收斂于期望圖像特征，圖像誤差逐漸趨近于零，即機(jī)器人運(yùn)動(dòng)到期望位置。

圖2 特征點(diǎn)誤差范數(shù)

圖3 攝像機(jī)運(yùn)動(dòng)軌跡立體圖

圖4 機(jī)械手運(yùn)動(dòng)軌跡圖

圖5 不確定參數(shù)β的變化曲線

圖6 左攝像機(jī)平面目標(biāo)運(yùn)動(dòng)軌跡

圖7 右攝像機(jī)平面目標(biāo)運(yùn)動(dòng)軌跡

對(duì)于手眼機(jī)器人，考慮動(dòng)力學(xué)特性構(gòu)造了視覺(jué)伺服系廣義哈密爾頓系統(tǒng)，本文提出了一種新的視覺(jué)伺服控制器，能夠使機(jī)械手穩(wěn)定地收斂到期望位置，解決了視覺(jué)伺服控制的干擾抑制問(wèn)題。本設(shè)計(jì)將廣義Hamilton理論應(yīng)用到視覺(jué)伺服控制器設(shè)計(jì)中，構(gòu)造了GHFVS模型，將系統(tǒng)的Hamilton函數(shù)用作李亞普諾夫函數(shù)，使得穩(wěn)定性分析和控制器設(shè)計(jì)簡(jiǎn)單。此外，所采用的視覺(jué)模型中不需要獲取深度信息，簡(jiǎn)化了控制器結(jié)構(gòu)。理論分析和實(shí)驗(yàn)均證明了該方法具有全局漸近穩(wěn)定性、魯棒性和快速性等控制性能。

參考文獻(xiàn)

[1]鄒立穎，李惠光．基于新雙目模型的變結(jié)構(gòu)視覺(jué)伺服控制方法[J]．武漢理工大學(xué)學(xué)報(bào)，2009，31(10)：117－120．

[2]馬燦，毛建旭，邵思敏.基于視覺(jué)傳感器的PCB缺陷檢測(cè)系統(tǒng)的研究與實(shí)現(xiàn)[J]．電子技術(shù)應(yīng)用，2012，38(6)：129－131．

[3]HUTCHINSON S，HAGER G D，CORKE P I．A tutorial on visual servo control[J]．IEEE Trans.Robot.Automat.，1996，12(10)：651－670．

[4]Wang Yuzhen，F(xiàn)eng Gang，Cheng Daizhan．Simultaneous stabilization of a set of nonlinear port-controlled Hamiltonian systems[J]．Automatica，2007(43)：403－415．

[5]Wang Yuzhen，Ge Shuzhi．Augmented Hamiltonian formulation and energy-based control design of uncertain mechanical systems[J]．IEEE Transactions on Control Systems Technology，2008，16(2)：202－213．

[6]Li Huiguang，Jin Mei，Zou Liying．A new binocular stereo visual servoing model[C]．In：Proceedings of 2008 Pacific-Asia Workshop on Computational Intelligence and Industrial Application，PACIIA.WuHan，China：IEEE，2008：461－465．

[7]武波，李惠光．基于圖像的直接視覺(jué)伺服控制器設(shè)計(jì)與仿真[J]．系統(tǒng)仿真學(xué)報(bào)，2007，19(22)：5214－5218．

[8]金梅，李惠光，鄒麗穎，等．具有 L2性能的視覺(jué)伺服控制器設(shè)計(jì)[J]．系統(tǒng)仿真學(xué)報(bào)，2009，21（13）：4101－4105．

[9]梅生偉，申鐵龍，劉康志．現(xiàn)代魯棒控制理論與應(yīng)用[M]．北京：清華大學(xué)出版社，2004．