王 勇 ，劉 治

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

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

王 勇1，2，劉 治1

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

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

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

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

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

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

1 雙目視覺模型

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

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

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

對式（1）求導可得特征點pi在相機k的圖像平面上的速度為

2 運動學

目標在雙臂的夾持下進行運動，在世界坐標系下，根據(jù)前向運動學可以得到

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

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

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

3 動力學

根據(jù)拉格朗日方法建立機械臂j的動力學方程為

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

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

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

另外，根據(jù)力對目標運動的影響，F(xiàn)cej可以分解成：

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

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

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

因此，機械臂j耦合目標的動力學方程可以寫成

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

4 控制器設(shè)計

4.1 參考變量

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

式（17）可寫成：

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

4.2 模糊系統(tǒng)

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

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

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

5 仿真

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

相機參數(shù)如表1所示，控制器設(shè)計中的參數(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個使用高斯函數(shù)的模糊規(guī)則構(gòu)成。

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

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

表2 理想軌跡Tab.2 Disired trajectory

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

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

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

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

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

圖5 相機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ù)。首先，針對目標的動力學不確定性，在控制設(shè)計中引入模糊邏輯系統(tǒng)。接下來在位置/力混合控制框架下，設(shè)計了基于雙目視覺的雙臂模糊軌跡跟蹤控制器，并且通過仿真驗證了控制方法的有效性。

[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

國家自然科學基金項目（61573108）

王勇（1977—），男，博士，研究方向為視覺伺服，自適應(yīng)控制；劉治（1977—），男，教授，博士生導師，研究方向為非線性控制、機器人控制。