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

王 勇 ，劉 治

（1.廣東工業大學 自動化學院，廣州 510006；2.廣東外語外貿大學 教育技術中心，廣州 510420）

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

王 勇1，2，劉 治1

（1.廣東工業大學 自動化學院，廣州 510006；2.廣東外語外貿大學 教育技術中心，廣州 510420）

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

雙臂系統；雙目視覺伺服；位置/力混合控制

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

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

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

1 雙目視覺模型

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

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

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

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

2 運動學

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

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

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

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

3 動力學

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

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

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

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

另外，根據力對目標運動的影響，Fcej可以分解成：

式中：fj表示機械臂j與目標之間的內應力；foj表示作用于目標運動的力，并且有：

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

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

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

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

4 控制器設計

4.1 參考變量

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

式（17）可寫成：

同時，機械臂j關節空間的參考速度定義為

4.2 模糊系統

目標對象的動力學不確定性會引起控制性能下降，甚至出現系統不穩定的情況，因此控制設計中引入模糊邏輯系統逼近系統的動力學特征。定義模糊邏輯系統，根據模糊規則可以將和寫成如下形式

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

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

5 仿真

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

相機參數如表1所示，控制器設計中的參數和控制增益設置為 λ＝1.5，Ks1＝Ks2＝diag（10，10，10），Ksy=diag（0.00002，…，0.00002），KΦ1＝KΦ2＝I5×5，理想內應力fd1=fd2=0。模糊系統由5個使用高斯函數的模糊規則構成。

表1 相機參數Tab.1 Camera parameter

選定的2個特征點在末端執行器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之間的相對位置關系計算得到。

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

圖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 左臂上內力跟蹤誤差Fig.6 Internal force tracking error on arm 1

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

6 結語

本文研究了雙臂系統在雙目視覺的監控下執行軌跡跟蹤任務。首先，針對目標的動力學不確定性，在控制設計中引入模糊邏輯系統。接下來在位置/力混合控制框架下，設計了基于雙目視覺的雙臂模糊軌跡跟蹤控制器，并且通過仿真驗證了控制方法的有效性。

[1] W.Gueaieb，F.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.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.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.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—），男，博士，研究方向為視覺伺服，自適應控制；劉治（1977—），男，教授，博士生導師，研究方向為非線性控制、機器人控制。