基于雙目視覺的雙臂模糊軌跡跟蹤控制

王 勇 ，劉 治

（1.廣東工業(yè)大學(xué) 自動(dòng)化學(xué)院，廣州 510006；2.廣東外語外貿(mào)大學(xué) 教育技術(shù)中心，廣州 510420）

基于雙目視覺的雙臂模糊軌跡跟蹤控制

王 勇1，2，劉 治1

（1.廣東工業(yè)大學(xué) 自動(dòng)化學(xué)院，廣州 510006；2.廣東外語外貿(mào)大學(xué) 教育技術(shù)中心，廣州 510420）

在雙目視覺系統(tǒng)的監(jiān)視下，雙臂系統(tǒng)夾持目標(biāo)執(zhí)行視覺軌跡跟蹤任務(wù)。隨意的目標(biāo)夾持姿態(tài)引起的動(dòng)力學(xué)不確定性會(huì)影響控制性能，尤其目標(biāo)質(zhì)量較大時(shí)。因此，該文在控制器設(shè)計(jì)中引入模糊系統(tǒng)來逼近系統(tǒng)的動(dòng)力學(xué)特征。同時(shí)，采用分散控制策略解決雙臂系統(tǒng)的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)協(xié)同問題，然后在位置/力混合控制的框架下設(shè)計(jì)了基于雙目視覺的雙臂模糊軌跡跟蹤控制器。最后，通過仿真證明了控制器的有效性。

雙臂系統(tǒng)；雙目視覺伺服；位置/力混合控制

機(jī)器人是計(jì)算機(jī)技術(shù)、機(jī)器視覺、自動(dòng)控制和傳感器等多種技術(shù)融合的載體，近年來隨著人工智能的快速發(fā)展，機(jī)器人在工業(yè)生產(chǎn)方面發(fā)揮著越來越重要的作用。同時(shí)，在生活服務(wù)方面的應(yīng)用也越來越多。與單臂機(jī)器人相比，多臂機(jī)器人具有功能豐富靈活的特點(diǎn)，可以完成復(fù)雜的家務(wù)勞動(dòng)，同時(shí)，可以代替人類從事復(fù)雜危險(xiǎn)的工作。因此多臂機(jī)器人一直是機(jī)器人領(lǐng)域的研究熱點(diǎn)，其研究的重點(diǎn)和難點(diǎn)就是多臂之間的協(xié)同控制問題。當(dāng)多個(gè)末端執(zhí)行器夾緊目標(biāo)對(duì)象時(shí)將會(huì)形成運(yùn)動(dòng)學(xué)閉鏈，在目標(biāo)運(yùn)動(dòng)過程中，目標(biāo)與末端執(zhí)行器之間的相對(duì)位姿要保持不變。同時(shí)，還需控制目標(biāo)和系統(tǒng)中的內(nèi)應(yīng)力，避免破壞目標(biāo)和系統(tǒng)。

在多臂系統(tǒng)協(xié)同控制方法中，位置/力混合控制是常用的控制方法之一。由于位置控制和力控制存在正交性特點(diǎn)，因此通過2個(gè)控制環(huán)路分別實(shí)現(xiàn)位置和力的控制。在動(dòng)力學(xué)建模方面，由于目標(biāo)和機(jī)械臂耦合在一起，因此精確建模十分困難，為此引入模糊系統(tǒng)和神經(jīng)網(wǎng)絡(luò)來解決此問題[1-8]。

由于相機(jī)具有非接觸和信息量豐富的優(yōu)點(diǎn)，因此在控制領(lǐng)域被廣泛使用，且發(fā)展出了視覺伺服控制方法。根據(jù)誤差定義的不同，視覺伺服一般可分成兩類：分別是基于圖像的視覺伺服[9-13]和基于位置的視覺伺服[14-16]。因此本文在雙臂控制中引入雙目視覺系統(tǒng)，研究基于視覺的雙臂協(xié)同控制方法。首先采用模糊系統(tǒng)解決隨意夾持姿態(tài)引起的動(dòng)力學(xué)不確定問題，然后在位置/力混合控制框架下，采用分散控制策略設(shè)計(jì)了基于雙目視覺的模糊軌跡跟蹤控制器，并且通過仿真驗(yàn)證了控制方法的有效性，同時(shí)此方法還可應(yīng)用于一般的多機(jī)械臂系統(tǒng)。

1 雙目視覺模型

如圖1所示，雙目視覺系統(tǒng)由2個(gè)內(nèi)參數(shù)完全相同的針孔相機(jī)構(gòu)成。相機(jī)1坐標(biāo)系Σc1和相機(jī)2坐標(biāo)系Σc2的 x 軸，xc1和 xc2同軸同向，同時(shí) zc1和 zc2平行且同向。另外，原點(diǎn)oc1和oc2之間的距離是b。

圖1 雙目視覺模型Fig.1 Binocular vision model

式中 ：M（k）表示 相 機(jī) k 的參 數(shù)；表 示特 征 點(diǎn) pi在相機(jī)k坐標(biāo)系下的深度。由于雙目視覺系統(tǒng)中相機(jī)特殊的位置配置，有

對(duì)式（1）求導(dǎo)可得特征點(diǎn)pi在相機(jī)k的圖像平面上的速度為

2 運(yùn)動(dòng)學(xué)

目標(biāo)在雙臂的夾持下進(jìn)行運(yùn)動(dòng)，在世界坐標(biāo)系下，根據(jù)前向運(yùn)動(dòng)學(xué)可以得到

式中：r0∈RN表示目標(biāo)質(zhì)心坐標(biāo)；No表示目標(biāo)的自由度；qj∈RNq表示機(jī)械臂 j的關(guān)節(jié)向量，j=1，2；Nq表示機(jī)械臂j的關(guān)節(jié)自由度。對(duì)式（3）求導(dǎo)可得目標(biāo)質(zhì)心速度與關(guān)節(jié)速度的關(guān)系為

式中：I3×3表示3階單位矩陣；Λci表示由特征點(diǎn) pi與目標(biāo)質(zhì)心的相對(duì)距離構(gòu)成的矩陣，詳見文獻(xiàn)[16]。

Z=diag（z1，z1，…，zNp，zNp），式（6）可以寫成

3 動(dòng)力學(xué)

根據(jù)拉格朗日方法建立機(jī)械臂j的動(dòng)力學(xué)方程為

式中：Hj（qj）∈RNq×Nq表示機(jī)械臂 j的正定慣性矩陣；Cj（qj，q˙j）∈RNq×Nq表示哥式力和向心力矩陣；Gj（qj）∈RNq表示重力向量；Fej表示目標(biāo)作用在執(zhí)行器j上的作用力；τj表示機(jī)械臂j上的驅(qū)動(dòng)力。另外，對(duì)目標(biāo)可建立如下動(dòng)力學(xué)方程：

式中：H0（r0）∈RNo×No表示目標(biāo)物體的正定慣性矩陣；C0（r0，˙0）∈RNo×No表示目標(biāo)的哥式力和向心力矩陣；G0（r0）∈RNo表示重力向量；F0表示雙臂對(duì)目標(biāo)的作用合力。根據(jù)機(jī)械臂j的受力分析可以得到：

式中：Fcej表示目標(biāo)質(zhì)心對(duì)末端執(zhí)行器j的作用力。根據(jù)力的合成與分解，目標(biāo)受到的合力為

另外，根據(jù)力對(duì)目標(biāo)運(yùn)動(dòng)的影響，F(xiàn)cej可以分解成：

式中：fj表示機(jī)械臂j與目標(biāo)之間的內(nèi)應(yīng)力；foj表示作用于目標(biāo)運(yùn)動(dòng)的力，并且有：

將式（11）和式（12）帶入式（9）可得：

假設(shè)在t時(shí)刻機(jī)械臂j分擔(dān)負(fù)載的情況用函數(shù)cj（t）表示，那么 foj可以寫成：

因此，機(jī)械臂j耦合目標(biāo)的動(dòng)力學(xué)方程可以寫成

式中：Hj（qj）=Hj（qj）+cj（t）H0（qj）；

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

4.1 參考變量

特征點(diǎn)pi在圖像平面k上的理想位置，理想速度和理想加速度分別用和表示，那么特征點(diǎn)pi在圖像平面k上的參考速度定義為

式（17）可寫成：

同時(shí)，機(jī)械臂j關(guān)節(jié)空間的參考速度定義為

4.2 模糊系統(tǒng)

目標(biāo)對(duì)象的動(dòng)力學(xué)不確定性會(huì)引起控制性能下降，甚至出現(xiàn)系統(tǒng)不穩(wěn)定的情況，因此控制設(shè)計(jì)中引入模糊邏輯系統(tǒng)逼近系統(tǒng)的動(dòng)力學(xué)特征。定義模糊邏輯系統(tǒng)，根據(jù)模糊規(guī)則可以將和寫成如下形式

綜上，在位置/力混合控制框架下，采用分散控制策略，基于雙目視覺的雙臂模糊軌跡跟蹤控制器設(shè)計(jì)如下：

式中：KΦj∈RNl1×Nq是正定矩陣，i=1，2。

5 仿真

為了證明上文提出的控制器的有效性，在Matlab平臺(tái)上進(jìn)行仿真驗(yàn)證。雙臂系統(tǒng)由2個(gè)完全相同的三關(guān)節(jié)機(jī)械臂構(gòu)成，機(jī)械臂1的基坐標(biāo)系與世界坐標(biāo)重合，機(jī)械臂2的基坐標(biāo)系原點(diǎn)在世界坐標(biāo)系下坐標(biāo)是（1，0，0）T，其姿態(tài)與機(jī)械臂 1 的基坐標(biāo)系相同。

相機(jī)參數(shù)如表1所示，控制器設(shè)計(jì)中的參數(shù)和控制增益設(shè)置為 λ＝1.5，Ks1＝Ks2＝diag（10，10，10），Ksy=diag（0.00002，…，0.00002），KΦ1＝KΦ2＝I5×5，理想內(nèi)應(yīng)力fd1=fd2=0。模糊系統(tǒng)由5個(gè)使用高斯函數(shù)的模糊規(guī)則構(gòu)成。

表1 相機(jī)參數(shù)Tab.1 Camera parameter

選定的2個(gè)特征點(diǎn)在末端執(zhí)行器1和2坐標(biāo)系下的坐標(biāo)分別是（-0.03，0，0.06），（-0.03，0，0.18），（0.03，0，0.18），（0.03，0，0.06）。 特征點(diǎn) 1 在圖像平面1和2上的理想軌跡的定義如表2所示。

表2 理想軌跡Tab.2 Disired trajectory

由于相機(jī)成像存在非線性，因此特征點(diǎn)2在圖像平面1和2上的理想軌跡和很難直接定義給出，因此，仿真中和的值通過特征點(diǎn)1和2之間的相對(duì)位置關(guān)系計(jì)算得到。

從圖2～圖3可以看到2個(gè)特征點(diǎn)在圖像平面1和2上都能很好的跟蹤上理想的軌跡。圖4～圖5顯示的是位置跟蹤誤差的變化情況，從圖中可以詳細(xì)的看到跟蹤能夠快速收斂，最后收斂到零。圖6～圖7顯示的是雙臂上內(nèi)應(yīng)力跟蹤誤差的情況，從圖中可看出內(nèi)應(yīng)力也能很好地跟蹤上理想值。從仿真結(jié)果可以證明本文提出的控制方法是有效的。

圖2 相機(jī)1上的跟蹤軌跡Fig.2 Tracking trajectory on camera 1

圖3 相機(jī)2上的跟蹤軌跡Fig.3 Tracking trajectory on camera 2

圖4 相機(jī)1上的跟蹤誤差Fig.4 Tracking error on camera 1

圖5 相機(jī)2上的跟蹤誤差Fig.5 Tracking error on camera 2

圖6 左臂上內(nèi)力跟蹤誤差Fig.6 Internal force tracking error on arm 1

圖7 右臂上內(nèi)力跟蹤誤差Fig.7 Internal force tracking error on arm 2

6 結(jié)語

本文研究了雙臂系統(tǒng)在雙目視覺的監(jiān)控下執(zhí)行軌跡跟蹤任務(wù)。首先，針對(duì)目標(biāo)的動(dòng)力學(xué)不確定性，在控制設(shè)計(jì)中引入模糊邏輯系統(tǒng)。接下來在位置/力混合控制框架下，設(shè)計(jì)了基于雙目視覺的雙臂模糊軌跡跟蹤控制器，并且通過仿真驗(yàn)證了控制方法的有效性。

[1] W.Gueaieb，F(xiàn).Karray，S.Al-Sharhan.A robust adaptive fuzzy position/force controlscheme forcooperative manipulators[J].Control Systems Technology，IEEE Transactions on，2003（4）：516-528.

[2] W.Gueaieb，F(xiàn).Karray，S.Al-Sharhan.A robust hybrid intelligent position/force control scheme for cooperative manipulators[J].IEEE-ASME Transactions on Mechatronics，2007（12）：109-125.

[3] Z.Liu，C.Chen，Y.Zhang.Decentralized robust fuzzy adaptive control of humanoid robot manipulation with unknown input backlash：An experimental study[J].Fuzzy Systems，IEEE Transactions on，2015，23（3）：605-616.

[4] N.Kumar，V.Panwar，N.Sukavanam.Neural network control of coordinated multiple manipulator systems[C]//Computing：Theory and Applications，2007，International Conference on，2007：250-256.

[5] Z.Liu，C.Chen，Y.Zhang，et al.Adaptive neural control for dualarm coordination of humanoid robot with unknown nonlinearities in output mechanism[J].Cybernetics，IEEE Transactions on，2015，45（3）：521-532.

[6] K.K.Tan，S.Huang，T.H.Lee.Decentralized adaptive controller design of large-scale uncertain robotic systems[J].Automatica，2009，45（1）：161-166.

[7] Z.J.Yang，Y.Fukushima，P.Qin.Decentral ized adaptive robust control of robot manipulators using disturbance observers[J].IEEE Transactions on Control Systems Technology，2012，20（5）：1357-1365.

[8] M.Namvar，F(xiàn).Aghili.Adaptive force-motion control of coordinated robots interacting with geometrically unknown environments[J].IEEE Transactions on Robotics，2005，21（4）：678-694.

[9] C.C.Cheah，S.P.Hou，Y.Zhao，J.-J.Slotine.adaptive vision and force tracking control for robots with constraint uncertainty[J].Mechatronics，IEEE/ASME Transactions on，2010，15 （3）：389-399.

[10]F.Nadi，V.Derhami，M.Rezaeian.Visual servoing control of robot manipulator with jacobian matrix estimation[C]//Robotics and Mechatronics 2014 Second RSI/ISM International Conference，2014：405-409.

[11]H.Wang.Adaptive visual tracking for robotic systems without image-space velocity measurement[J].Automatica，2015，55（C）：294-301.

[12]H.Wang，M.Jiang，W.Chen，Y.-H.Liu.Visual servoing of robots with uncalibrated robot and camera parameters[J].Mechatronics，2012，22（6）：661-668.

[13]K.Wang，Y.Liu，L.Li.Visual servoing trajectory tracking of nonholonomic mobile robots without direct position measurement[J].Robotics，IEEE Transactions on，2014，30（4）：1026-1035.

[14]J.Qu，F(xiàn).Zhang，Y.L.Fu，etal.Approach movementofredundant manipulator using stereo vision[C]//Robotics and Biomimetics，2014 IEEE International Conference，2014：2489-2494.

[15]Y.Wang，G.lu Zhang，H.Lang，et al.A modified image-based visual servo controller with hybrid camera configuration for robust robotic grasping[J].Robotics and Autonomous Systems，2014，62（10）：1398-1407.

[16]H.Wang，Y.-H.Liu，D.Zhou.Dynamic visual tracking for manipulators using an uncalibrated fixed camera[J].Robotics，IEEE Transactions on，2007，23（3）：610-617.

Binocular Vision Based on Fuzzy Trajectory Tracking Control for Dual-arm System

WANG Yong1，2，LIU Zhi1
（1.Faculty of Automation，Guangdong University of Technology，Guangzhou 510006，China；2.The center of education technology，Guangdong University of Foreign Studies，Guangzhou 510420，China）

The object held by a dual-arm system is to perform a visual trajectory tracking task in the monitoring of a binocular vision system.The control performance should be degraded by the dynamics uncertainties generated from the random holding manner，especially when the object with large mass.As a result，the fuzzy system is then introduced to approximate the dynamics characteristic.At the same time，the decentralized control method is used to address the cooperation between the dual arms，and the binocular vision based fuzzy trajectory tracking controller for dual-arm system is designed based on position/force hybrid control framework.Finally，the effectiveness of the proposed controller is verified in simulation.

dual-arm system；binocular vision servoing；position/force hybrid control

TP273

A

1001-9944（2017）11-0010-05

10.19557/j.cnki.1001-9944.2017.11.003

2017-04-19；

2017-07-24

國(guó)家自然科學(xué)基金項(xiàng)目（61573108）

王勇（1977—），男，博士，研究方向?yàn)橐曈X伺服，自適應(yīng)控制；劉治（1977—），男，教授，博士生導(dǎo)師，研究方向?yàn)榉蔷€性控制、機(jī)器人控制。