# Research on Visual Servo Grasping of Household Objects for Nonholonomic Mobile Manipulator

Authors: Huangsheng Xie; Guodong Li; Yuexin Wang; Zhihe Fu; Fengyu Zhou
Journal: Journal of Control Science and Engineering
 (2014)

---

## Abstract

This paper focuses on the problem of visual servo grasping of household objects for nonholonomic mobile manipulator. Firstly, a new kind of artificial object mark based on QR (Quick Response) Code is designed, which can be affixed to the surface of household objects. Secondly, after summarizing the vision-based autonomous mobile manipulation system as a generalized manipulator, the generalized manipulator’s kinematic model is established, the analytical inverse kinematic solutions of the generalized manipulator are acquired, and a novel active vision based camera calibration method is proposed to determine the hand-eye relationship. Finally, a visual servo switching control law is designed to control the service robot to finish object grasping operation. Experimental results show that QR Code-based artificial object mark can overcome the difficulties brought by household objects’ variety and operation complexity, and the proposed visual servo scheme makes it possible for service robot to grasp and deliver objects efficiently.


---

## Body

## 1. Introduction

A classical mobile manipulator system (MMS) consists of a manipulator which is mounted on a nonholonomic mobile platform. This type of arrangement extends manipulator’s workspace apparently and is widely used in service robot applications [1, 2]. The development of MMS mainly involves two classical items, namely, motion planning [3–8] and coordinating control [9–13], which are used to overcome the mobile platform’s nonholonomic constraint and make the MMS move quickly and efficiently.When robots operate in unstructured environments, it is essential to include exteroceptive sensory information in the control loop. In particular, visual information provided by vision sensor such as charge-coupled device (CCD) cameras guarantees accurate positioning, robustness of calibration uncertainties, and reactivity of environmental changes. Much of the work related to CCD cameras and manipulators has focused on the applications about the manipulator’s visual servo control, which specifies robotic tasks (such as object grasping, assembling) in terms of desired image features extracted from a target object. The overview of visual servo can be seen in literature [14–16]. In general, visual servo approaches can be divided into three different kinds, namely,position-based visual servoing (PBVS) [17, 18],image-based visual sevoing (IBVS) [19, 20], andhybrid visual servoing (HYBVS) [21–23]. In PBVS, the feedback signals in vision loop are the intuitive relative 3D pose between current and desired cameras estimated by current and desired image features using homography matrix or fundamental matrix estimation and decomposition. In IBVS, the feedback signals are image features whose changing velocities are related to the velocity twist of the camera via theimage jacobian matrix(also calledinteraction matrix). Compared with the PBVS, IBVS is robust to perturbations of the robot/camera models and can maintain the image features in thefield of view(FOV) of the camera through path planning [24]. But the drawbacks are also obvious; the interaction matrix’s depth information needs to be estimated and only local stability can be guaranteed for most IBVS schemes. To combine the advantages of PBVS and IBVS, HYBVS is proposed. In HYBVS, the feedback signals consist of relative 3D pose and image features, the former is used to control a subset of the camera configuration vector while the latter are used to regulate the remaining camera configuration vector.Relating CCD cameras and the mobile robots lead to the applications of vision-based autonomous navigation control. Ma et al. [25] have developed a vision-guided navigation system, where a nonholonomic mobile robot tracks an arbitrarily shaped continuous ground curve. Dixon et al. [26] present an adaptive tracking controller of a wheeled mobile robot via an uncalibrated camera system that the controller copes with the parameter uncertainty of the mechanical dynamics and the camera system. Amarasinghe et al. [27] have developed a vision-based hybrid control scheme for autonomous parking of a mobile robot that its controller consists of a discrete-event controller and a pixel-error-driven proportional controller. Vassallo et al. [28] present a similar project where a vision-based mobile robot attempts to autonomously navigate in a building.The newest trend is to integrate CCD cameras into mobile manipulator to form a vision-based mobile manipulation system (VBMMS). Thanks to the capabilities of the vision subsystem, the VBMMS can work in an unstructured environment and has wider applications than a fixed-base manipulator and a mobile platform. Due to the lack of accurate and robust positioning performance of VBMMS, very few physical implementations have been reported. de Luca et al. [29] have considered the task-oriented modeling of the differential kinematics of nonholonomic mobile manipulators and has developed an image-based controller for VBMMS, but their approach is illustrated through simulation, not physical implementation. Mansard et al. [30] have attempted to control a humanoid robot to grasp an object while walking. Wang et al. [31] have developed a robust vision-based mobile manipulation system for wheeled mobile robots. In their research, an innovative controller with machine learning using Q-learning is proposed to guarantee visibility of visual features in servo process.This paper presents a physical implementation of VBMMS in service robot intelligent space. It consists of two basic contributions. First, after summarizing the VBMMS as a generalized manipulator, the kinematics is analyzed analytically and an active vision-based camera calibration method is proposed to determine the hand-eye relationship consequently. Second, a novel switching control strategy is proposed which switches between eye-fixed approximation and position-based static look-and-move grasping. The remainder of the paper is organized as follows. Section1 will introduce the design of the QR Code-based artificial object mark. In Section 2, the VBMMS is summarized as a generalized manipulator, then the kinematics, inverse kinematics, and hand-eye relationship determination are discussed. In Section 3, the switching control strategy which switches between eye-fixed approximation and static look-and-move grasping is designed. Two experiments will be presented in Section 4 to validate the designed switching controller. Conclusions will be drawn in Section 5.


## 2. Design of QR Code-Based Artificial Object Mark

As shown in Figure1, the QR Code-based artificial object mark is composed of two parts: the internal information representation part which includes the object’s property and operation information and the blue concentric ring region which is called external identification part. Due to the facilitated detection of the external identification part, the mark can be recognized from complex home environment rapidly using vision sensor.Figure 1
QR Code-based artificial object mark.The coding of information stored in internal information representation part of the mark is shown in Table1.Table 1
Coding of information stored in QR Code-based artificial object mark.
Info type
Description
Bytes
Name
Object’s name’s first two words
2
Serial number
Such as1,2,…
1
Sizes
Long * width * height, 2 bytes for each items
8
Material
p (plastic), g (glass), z (paper), w (wood), s (metal), t (textile)
1
Operation force
H (huge), L (large), M (middle), S (small), T (tiny)
1
Operation position
u (upper), m (middle), b (bottom)
1
Operation orientation
a (above), f (front), b (back), l (left), r (right)
1It can be seen from Table1 that there are two different kinds of information, namely, object’s property information and object’s operation information. The object’s property information includes name, serial number, sizes, and material, in which the properties of name and serial number are used as the unique identification of an object and the property of sizes is to let robot know the gripper’s opening degree. The object’s operation information includes operation force, position, and orientation, which can assist the robot in finishing grasping operation in an appropriate way.


## 3. Kinematics, Inverse Kinematics, and Hand-Eye Relationship Determination of VBMMS

As shown in Figure2, the eye-in-hand type VBMMS is combined with a nonholonomic tracked mobile robot (TMR), a 4-DOF (Degree of Freedom) manipulator, and a CMOS camera. The Grandar AS-RF type TMR uses differential drive structure and is easier to carry out steering control. The Schunk Powercube 4-DOF manipulator is mounted on the TMR, and the Gsou V80 CMOS camera is mounted on the manipulator’s end-effector. In addition, the VBMMS is equipped with an onboard computer, whose computational capability can support real-time performance of the system.Figure 2
Vision-based mobile manipulation system and its link coordinate frames.


### 3.1. Kinematics

Due to the nonholonomic constraint of the TMR and the nonredundancy of the 4-DOF manipulator, using VBMMS to complete grasping task is very difficult, and so far, barely no related works can be found. Take the difficulties of controlling the TMR and manipulator separately into account, the VBMMS is summarized as a generalized manipulator shown in Figure2. For the generalized manipulator, the TMR is considered as a 3-DOF (rotation-translation-rotation) manipulator and the manipulator’s first degree is aborted because of its coincidence with the TMR’s third degree. In that case, the generalized manipulator has six degrees, which guarantees that the end-effector can approach an arbitrary position at any pose.Table2 illustrates the modified D-H parameters of each link for serial-link manipulator, where αi,  ai,  θi,  di mean twist, length, angle, and offset of the ith link, respectively. In addition to the last column of the table, R is for revolute while T is for prismatic. For the prismatic joint, di is the variable whose value range is di>0, while for the revolute joint, θi is the variable whose value range is -π~π.Table 2
D-H parameters of the generalized manipulator.
Linki
α
i (rad)
a
i (cm)
θ
i (rad)
d
i (cm)
Type
1
0
0
π
/
2
+
θ
1
0
R
2
π
/
2
0
0
0
+
d
2
T
3
-
π
/
2
0
-
π
/
2
+
θ
3
25
R
4
-
π
/
2
−6.5
0
+
θ
4
0
R
5
π
/
2
6.5
0
+
θ
5
28.5
R
6
π
/
2
0
0
+
θ
6
0
R
Tool
-
π
/
2
0
0
28Table2 results in a 4 × 4 homogeneous transformation matrix i-1Ti representing each link’s coordinate frame with respect to the previous link’s coordinate frame. Thereby, the representation of end-effector coordinate frame {E} with respect to base coordinate frame {0} can be shown in the following:
(1)0TE=[nxoxaxpxnyoyaypynzozazpz0001],
where
(2)nx=c13c4c5c6+c13s4s6-s13s5c6(3)ny=s13c4c5c6+s13s4s6+c13s5c6(4)nz=-s4c5c6+c4s6(5)ox=-c13c4s5-s13c5(6)oy=-s13c4s5+c13c5(7)oz=s4s5(8)ax=-c13c4c5s6+c13s4c6+s13s5s6(9)ay=-s13c4c5s6+s13s4c6-c13s5s6(10)az=s4c5s6+c4c6(11)px=-28c13c4c5s6+28c13s4c6+6.5c13c4+28.5c13s4-6.5c13+28s13s5s6+d2c1(12)py=-28s13c4c5s6+28s13s4c6+6.5s13c4+28.5s13s4-6.5s13-28c13s5s6+d2s1(13)pz=28s4c5s6+28c4c6-6.5s4+28.5c4+25;
in (2)–(13), ci=cos⁡θi, si=sinθi, cij=cos⁡(θi+θj), sij=sin(θi+θj),  i,j=1,3,4,5,6.It is well known that the symbolEJ can be used to note the generalized manipulator Jacobian matrix expressed in end-effector coordinate frame {E} that transforms velocities in joint space to velocities of the end-effector in Cartesian space. For the 6-DOF generalized manipulator, the end-effector Cartesian velocity is
(14)EX˙=JE(q)q˙,
where the 6-vector represented by EX˙=[EvTEωT]T is the end-effector’s Cartesian velocity with respect to its own frame {E}. Based upon the resolved transformation matrix Tii-1 and TE6, TEi can be determined as
(15)TEi=Ti+1i·Ti+2i+1⋯TE6=[inxioxiaxipxinyioyiayipyinzioziazipz0001],
for prismatic joint corresponding i=2; the ith column of EJ can be constructed as follows:
(16)EJi=[inzioziaz000]T
for revolute joint corresponding i=1,3,4,5,6; the ith column of EJ:
(17)EJi=[-nxipyi+nyipxi,-oxipyi+oyipxi,-axipyi+ayipxi,nzi,ozi,azi]T.
The EJ can finally be constructed as follows:
(18)EJ=[EJ1EJ2EJ3EJ4EJ5EJ6].


### 3.2. Inverse Kinematics

From (2)–(13) we can achieve that the manipulator has 6 joints to allow arbitrary end-effector pose. In order to reach the specified end-effector position, the inverse kinematic solutions can be acquired through separating the unknown variable θ1, d2, θ3 in (2)–(13).Note that0T3=T10T21T32, by premultiplicating T3-10 in (1), we can acquire TE3. On the other hand, TE3 can be computed by TE3=T43T54T65TE6. Let the two different forms of TE3 be equal; four sets of analytical inverse kinematic solutions can be achieved:
(19)θ4=Atan2(28.5,6.5)+Atan2(28az-pz+256.52+28.52,±1-(28az-pz+25)26.52+28.52)θ1+θ3=Atan2(-c4ozs4ox2+oy2,±1-(c4oz)2s42(ox2+oy2))-Atan2(ox,oy)θ5=Atan2(ozs4,-s13ox+c13oy)θ6=Atan2(s13ax-c13ays5,-s13nx+c13nys5)d2={(-28s5s6+s13px-c13py)2(c13px+s13py+28c4c5s6-28s4c6-6.5c4-28.5s4+6.5c13px+s13py+28c4c5s6-28s4c6)2+(-28s5s6+s13px-c13py)2}1/2θ3=Atan2(-28s5s6+s13px-c13py,c13px+s13py+28c4c5s6-28s4c6-6.5c4-28.5s4+6.5-28s5s6+s13px-c13py).Given an arbitrary end-effector pose, whose orientation is expressed by RPY description method, the four sets of inverse kinematic solutions can be solved by (19), as shown in Table 3.Table 3
Inverse kinematic results of the generalized manipulator.
Roll/(rad)
Pitch/(rad)
Yaw/(rad)
p
x/(cm)
p
y/(cm)
p
z/(cm)
θ
1/(rad)
d
2/(cm)
θ
3/(rad)
θ
4/(rad)
θ
5/(rad)
θ
6/(rad)
0.4449
0.2566
−1.2466
129.4143
15.7087
76.7968
0.2417
122.6
−0.7854
0.1366
−0.6283
−0.3927
0.1922
123.7487
1.1579
0.1366
−2.5133
−0.6142
0.0880
127.4757
−1.3771
−0.5851
0.1454
−1.0836
−5.9305
121.3515
1.7428
−0.5851
2.9962
0.0767
−0.2411
−0.4617
−0.2266
12.5929
22.4547
69.2228
1.0203
19.3710
−1.0229
0.5987
−0.2428
1.0851
0.2661
40.1966
2.4693
0.5987
−2.8988
−0.0848
0.1653
52.60
−0.4488
−1.0472
0.1571
−0.5417
−4.5806
20.1375
1.3136
−1.0472
2.9845
1.5420After solving all the solutions, they can be processed so that they verify the joints’ value ranges, and then the optimal set of solution can be acquired using a certain optimality criteria such as the shortest path.


### 3.3. Hand-Eye Relationship Determination

Hand-eye relationship determination is a key issue in visual servo control of a robot hand-eye system, and much work has been done. In this paper, we propose a novel method which is based on Zhang’s camera calibration method and tensor theory. As we know, Zhang’s algorithm proposed in 1999 is very representative because of its easy use, flexibility, and high accuracy [32]. The algorithm only requires the camera to observe a planar pattern (such as planar checkerboard) shown at a few different orientations. In consequence, a closed-form solution of camera intrinsic parameter matrix K and the pose of planar object’s coordinate frame {o} with respect to the camera frame {c} noted as Toc can be computed, which are followed by a nonlinear refinement based on the maximum likelihood criterion so as to improve the accuracy.Figure3 shows the scheme of hand-eye relationship determination, where {E}, {c} are the current end-effector frame and current camera frame, respectively.Figure 3
Scheme of hand-eye relationship determination.When the manipulator executes theith (i=1,2,…,n) known movement, they move to a new position noted as {Ei} and {ci}.From Figure3, there is
(20)ETc·cTo=ETEi·EiTci·ciTo.
In (20), ETc=TciEi is the unknown hand-eye relationship, ETEi can be known from the known motion of the manipulator, and cTo and ciTo can be known from Zhang’s method.Note that(21)EiTE=0TEi-1·0TE=[RAitAi0T1]ETc=[Rt0T1]cTci=cTo·ciTo-1=[RBitBi0T1].
By substituting (21) into (20), one obtains
(22)RAi·R·RBi=R(23)RAi·R·tBi+RAi·t+tAi=t.It can be seen from (22) that once we solve the unknown R, then t will be solved easily with (23). On the other hand, (22) can be summarized as the form of AXB=X, considering a M×N matrix X to be a second-order mixed tensor whose element of pth row and qth column is noted as xqp; the equation AXB=X can be written as follows:
(24)akpxlkbql=xqp,
where p, q are free indexes, and k, l are dummy indexes. Take Kronecker δinto account:
(25)δβα={1,ifα=β0,ifα≠β.
Equation (24) can be written as
(26)(akpbql-δkpδql)xlk=0.According to the different values of the free indexesp and q, we can acquire M×N sets of equations from (26). Let x=[x1T,x2T,…,xmT]T, where xiT is the ith row vector of Matrix X. Equation (26) can finally be written as
(27)Hx=0,
where H∈R(M×N)×(M×N), and its element
(28)h(k-1)·N+l(p-1)·N+q=akpbql-δkpδql.Consider that there aren sets of known manipulator movement, Hi, corresponding to the ith movement, therefore we obtain
(29)[H1⋯Hn]x=Bx=0.Based upon least-squares solution of a homogeneous system of linear equations,x is the last column of V, where B=UDVT. Note
(30)R′=[x11x21x31x12x22x32x13x23x33]
because R is a rotation matrix whose Frobenius norm is 3 and det⁡(R)>0, we can determine R as
(31)R=±3R′.Till then, the uniqueR verifying constraint det⁡(R)>0 can be distinguished from (31). Furthermore, we choose RF=UVT, where R=UDVT as the final solution of (22).After solvingR, the unknown t can also be solved using the next equation:
(32)t=[I-RA1⋯I-RAn]+·[RA1·R·tB1+tA1⋯RAn·R·tBn+tAn].


## 3.1. Kinematics

Due to the nonholonomic constraint of the TMR and the nonredundancy of the 4-DOF manipulator, using VBMMS to complete grasping task is very difficult, and so far, barely no related works can be found. Take the difficulties of controlling the TMR and manipulator separately into account, the VBMMS is summarized as a generalized manipulator shown in Figure2. For the generalized manipulator, the TMR is considered as a 3-DOF (rotation-translation-rotation) manipulator and the manipulator’s first degree is aborted because of its coincidence with the TMR’s third degree. In that case, the generalized manipulator has six degrees, which guarantees that the end-effector can approach an arbitrary position at any pose.Table2 illustrates the modified D-H parameters of each link for serial-link manipulator, where αi,  ai,  θi,  di mean twist, length, angle, and offset of the ith link, respectively. In addition to the last column of the table, R is for revolute while T is for prismatic. For the prismatic joint, di is the variable whose value range is di>0, while for the revolute joint, θi is the variable whose value range is -π~π.Table 2
D-H parameters of the generalized manipulator.
Linki
α
i (rad)
a
i (cm)
θ
i (rad)
d
i (cm)
Type
1
0
0
π
/
2
+
θ
1
0
R
2
π
/
2
0
0
0
+
d
2
T
3
-
π
/
2
0
-
π
/
2
+
θ
3
25
R
4
-
π
/
2
−6.5
0
+
θ
4
0
R
5
π
/
2
6.5
0
+
θ
5
28.5
R
6
π
/
2
0
0
+
θ
6
0
R
Tool
-
π
/
2
0
0
28Table2 results in a 4 × 4 homogeneous transformation matrix i-1Ti representing each link’s coordinate frame with respect to the previous link’s coordinate frame. Thereby, the representation of end-effector coordinate frame {E} with respect to base coordinate frame {0} can be shown in the following:
(1)0TE=[nxoxaxpxnyoyaypynzozazpz0001],
where
(2)nx=c13c4c5c6+c13s4s6-s13s5c6(3)ny=s13c4c5c6+s13s4s6+c13s5c6(4)nz=-s4c5c6+c4s6(5)ox=-c13c4s5-s13c5(6)oy=-s13c4s5+c13c5(7)oz=s4s5(8)ax=-c13c4c5s6+c13s4c6+s13s5s6(9)ay=-s13c4c5s6+s13s4c6-c13s5s6(10)az=s4c5s6+c4c6(11)px=-28c13c4c5s6+28c13s4c6+6.5c13c4+28.5c13s4-6.5c13+28s13s5s6+d2c1(12)py=-28s13c4c5s6+28s13s4c6+6.5s13c4+28.5s13s4-6.5s13-28c13s5s6+d2s1(13)pz=28s4c5s6+28c4c6-6.5s4+28.5c4+25;
in (2)–(13), ci=cos⁡θi, si=sinθi, cij=cos⁡(θi+θj), sij=sin(θi+θj),  i,j=1,3,4,5,6.It is well known that the symbolEJ can be used to note the generalized manipulator Jacobian matrix expressed in end-effector coordinate frame {E} that transforms velocities in joint space to velocities of the end-effector in Cartesian space. For the 6-DOF generalized manipulator, the end-effector Cartesian velocity is
(14)EX˙=JE(q)q˙,
where the 6-vector represented by EX˙=[EvTEωT]T is the end-effector’s Cartesian velocity with respect to its own frame {E}. Based upon the resolved transformation matrix Tii-1 and TE6, TEi can be determined as
(15)TEi=Ti+1i·Ti+2i+1⋯TE6=[inxioxiaxipxinyioyiayipyinzioziazipz0001],
for prismatic joint corresponding i=2; the ith column of EJ can be constructed as follows:
(16)EJi=[inzioziaz000]T
for revolute joint corresponding i=1,3,4,5,6; the ith column of EJ:
(17)EJi=[-nxipyi+nyipxi,-oxipyi+oyipxi,-axipyi+ayipxi,nzi,ozi,azi]T.
The EJ can finally be constructed as follows:
(18)EJ=[EJ1EJ2EJ3EJ4EJ5EJ6].


## 3.2. Inverse Kinematics

From (2)–(13) we can achieve that the manipulator has 6 joints to allow arbitrary end-effector pose. In order to reach the specified end-effector position, the inverse kinematic solutions can be acquired through separating the unknown variable θ1, d2, θ3 in (2)–(13).Note that0T3=T10T21T32, by premultiplicating T3-10 in (1), we can acquire TE3. On the other hand, TE3 can be computed by TE3=T43T54T65TE6. Let the two different forms of TE3 be equal; four sets of analytical inverse kinematic solutions can be achieved:
(19)θ4=Atan2(28.5,6.5)+Atan2(28az-pz+256.52+28.52,±1-(28az-pz+25)26.52+28.52)θ1+θ3=Atan2(-c4ozs4ox2+oy2,±1-(c4oz)2s42(ox2+oy2))-Atan2(ox,oy)θ5=Atan2(ozs4,-s13ox+c13oy)θ6=Atan2(s13ax-c13ays5,-s13nx+c13nys5)d2={(-28s5s6+s13px-c13py)2(c13px+s13py+28c4c5s6-28s4c6-6.5c4-28.5s4+6.5c13px+s13py+28c4c5s6-28s4c6)2+(-28s5s6+s13px-c13py)2}1/2θ3=Atan2(-28s5s6+s13px-c13py,c13px+s13py+28c4c5s6-28s4c6-6.5c4-28.5s4+6.5-28s5s6+s13px-c13py).Given an arbitrary end-effector pose, whose orientation is expressed by RPY description method, the four sets of inverse kinematic solutions can be solved by (19), as shown in Table 3.Table 3
Inverse kinematic results of the generalized manipulator.
Roll/(rad)
Pitch/(rad)
Yaw/(rad)
p
x/(cm)
p
y/(cm)
p
z/(cm)
θ
1/(rad)
d
2/(cm)
θ
3/(rad)
θ
4/(rad)
θ
5/(rad)
θ
6/(rad)
0.4449
0.2566
−1.2466
129.4143
15.7087
76.7968
0.2417
122.6
−0.7854
0.1366
−0.6283
−0.3927
0.1922
123.7487
1.1579
0.1366
−2.5133
−0.6142
0.0880
127.4757
−1.3771
−0.5851
0.1454
−1.0836
−5.9305
121.3515
1.7428
−0.5851
2.9962
0.0767
−0.2411
−0.4617
−0.2266
12.5929
22.4547
69.2228
1.0203
19.3710
−1.0229
0.5987
−0.2428
1.0851
0.2661
40.1966
2.4693
0.5987
−2.8988
−0.0848
0.1653
52.60
−0.4488
−1.0472
0.1571
−0.5417
−4.5806
20.1375
1.3136
−1.0472
2.9845
1.5420After solving all the solutions, they can be processed so that they verify the joints’ value ranges, and then the optimal set of solution can be acquired using a certain optimality criteria such as the shortest path.


## 3.3. Hand-Eye Relationship Determination

Hand-eye relationship determination is a key issue in visual servo control of a robot hand-eye system, and much work has been done. In this paper, we propose a novel method which is based on Zhang’s camera calibration method and tensor theory. As we know, Zhang’s algorithm proposed in 1999 is very representative because of its easy use, flexibility, and high accuracy [32]. The algorithm only requires the camera to observe a planar pattern (such as planar checkerboard) shown at a few different orientations. In consequence, a closed-form solution of camera intrinsic parameter matrix K and the pose of planar object’s coordinate frame {o} with respect to the camera frame {c} noted as Toc can be computed, which are followed by a nonlinear refinement based on the maximum likelihood criterion so as to improve the accuracy.Figure3 shows the scheme of hand-eye relationship determination, where {E}, {c} are the current end-effector frame and current camera frame, respectively.Figure 3
Scheme of hand-eye relationship determination.When the manipulator executes theith (i=1,2,…,n) known movement, they move to a new position noted as {Ei} and {ci}.From Figure3, there is
(20)ETc·cTo=ETEi·EiTci·ciTo.
In (20), ETc=TciEi is the unknown hand-eye relationship, ETEi can be known from the known motion of the manipulator, and cTo and ciTo can be known from Zhang’s method.Note that(21)EiTE=0TEi-1·0TE=[RAitAi0T1]ETc=[Rt0T1]cTci=cTo·ciTo-1=[RBitBi0T1].
By substituting (21) into (20), one obtains
(22)RAi·R·RBi=R(23)RAi·R·tBi+RAi·t+tAi=t.It can be seen from (22) that once we solve the unknown R, then t will be solved easily with (23). On the other hand, (22) can be summarized as the form of AXB=X, considering a M×N matrix X to be a second-order mixed tensor whose element of pth row and qth column is noted as xqp; the equation AXB=X can be written as follows:
(24)akpxlkbql=xqp,
where p, q are free indexes, and k, l are dummy indexes. Take Kronecker δinto account:
(25)δβα={1,ifα=β0,ifα≠β.
Equation (24) can be written as
(26)(akpbql-δkpδql)xlk=0.According to the different values of the free indexesp and q, we can acquire M×N sets of equations from (26). Let x=[x1T,x2T,…,xmT]T, where xiT is the ith row vector of Matrix X. Equation (26) can finally be written as
(27)Hx=0,
where H∈R(M×N)×(M×N), and its element
(28)h(k-1)·N+l(p-1)·N+q=akpbql-δkpδql.Consider that there aren sets of known manipulator movement, Hi, corresponding to the ith movement, therefore we obtain
(29)[H1⋯Hn]x=Bx=0.Based upon least-squares solution of a homogeneous system of linear equations,x is the last column of V, where B=UDVT. Note
(30)R′=[x11x21x31x12x22x32x13x23x33]
because R is a rotation matrix whose Frobenius norm is 3 and det⁡(R)>0, we can determine R as
(31)R=±3R′.Till then, the uniqueR verifying constraint det⁡(R)>0 can be distinguished from (31). Furthermore, we choose RF=UVT, where R=UDVT as the final solution of (22).After solvingR, the unknown t can also be solved using the next equation:
(32)t=[I-RA1⋯I-RAn]+·[RA1·R·tB1+tA1⋯RAn·R·tBn+tAn].


## 4. Design of Visual Servo Switch Control Scheme for Grasping

The visual servo control scheme designed in this section consists of two steps: eye-fixed approximation of the household object and static look-and-move grasping. Once the VBMMS is commanded to grasp a household object that is in its camera’s FOV, the VBMMS starts to approximate the object with its eye gazing it. When the distance between VBMMS’s camera and household object reaches a certain value, the process of approximation switches to the static look-and-move grasping.


### 4.1. Eye-Fixed Approximation

Figure4 illustrates the scheme of eye-fixed approximation. In Figure 4, m00 and m00* correspond to current image and desired image, respectively, and both are called zero-order moment of the blue concentric ring region to the object mark. mc and mc* are called nonhomogeneous projective coordinate of the center of blue concentric ring region to the object mark, which also correspond to current image and desired image, respectively.Figure 4
Scheme of eye-fixed approximation.It is well known that for nonhomogeneous projective coordinatemc=[x,y]T=[Xc/Zc,Yc/Zc]T, its time derivative m˙c is linearly related to the joints’ velocity q˙ through the interaction matrix Lcm:
(33)Lcm=[-1cZ0xcZxy-(1+y2)y0-1cZycZ1+y2-xy-x]×[ERcT-ERcT[Etc]×0ERcT]J(q)E,
where P=[cX,cY,cZ]T is the coordinates of a 3D point relative to current camera frame, ERc, Etc are the results of hand-eye relationship, and J(q)E is the generalized manipulator’s Jacobian in end-effector coordinate frame {E}. For the unknown variable cZ in Lcm, its estimation cZ^ can be chosen as follows:
(34)cZ^=Zc*m00*m00,
where Zc* is the constant depth of the 3D point relative to the desired camera frame.We partition the interaction matrix so as to isolate the second degree of freedom of the generalized manipulator. Note the following:(35)Ld=Lcmdiag⁡(010000)Ld⊥=Lcmdiag⁡(100111)
in which diag (·) is a diagonal matrix, Ld only includes the second column of Lcm, and Ld⊥ includes the first, fourth, fifth, and sixth columns of Lcm. We get
(36)cm˙=Ldq˙d+Ld⊥q˙d⊥.Notee2=m00-αm00*, where α is a constant coefficient; a simple approximation control law can be acquired by using feature e2, as
(37)q˙d=[0-λ1sgn⁡(e2)0000]T,
where sgn(·) is sign function and λ1is a time-invariant coefficient. Note e1=mc-mc*; let e1 be exponential convergent e˙1=-λe1. There is
(38)q˙d⊥=-Ld⊥+(λe1+Ldq˙d).The vectorλe1+Ldq˙d can be considered as a modified error that incorporates the original error e1 while taking into account the error that will be induced by robot motion q˙d.Under the influence of control input shown in (37) and (38), the VBMMS approaches to object with a constant speed until e2=0, and during the approximation process, the object is maintained in the FOV from beginning to end.


### 4.2. Static Look-and-Move Grasping

Figure5 illustrates the scheme of static look-and-move grasping.Figure 5
Scheme of static look-and-move grasping.In Figure5, 3D object point P is on the plane π whose normal is n. Meanwhile, there are three different camera frames: the desired camera frame {c*}, the initial camera frame {c}, and the current camera frame {c′}, that is, after a manipulator’s known movement from qi to qc. The homogeneous transformation c′Tc can be computed as
(39)c′Tc=[R′t′0T1]=ETc-1·0TE′-1·0TE·ETc,
where ETc is the hand-eye relationship, 0TE is the initial end-effector’s pose, and 0TE′ is the current end-effector’s pose. Our ultimate objective is to determine the pose of {c′} relative to {c*}, namely, R,t. In order to solve them, two steps are needed.Step 1.
Structure reconstruction of planeπ with respect to camera frame {c′}.Take current camera frame{c} as the reference frame, the plane π can be noted as cπ=[cnT,dc]T and point [cmT,1]T in frame {c} is related to the corresponding point [mTc′,1]T in frame {c′} by Euclidean homography H′:
(40)[mc′1]≅(R′-t′·cnTcd)[cm1]≅H′[cm1],
where ≅ means that the H′ transforms one vector into the other, up to a scale factor. Given at least four sets of image point correspondences c′mi↔cmi,  i=1,…,n, the H′ satisfying ∥H′∥F=1 can be derived using direct linear transformation (DLT) method.After solvingH′, written as H′=[h1′h2′h3′] and R′=[r1r2r3], cn=[nxnynz]T, there are
(41)λh1′=r1-nxcdt′λh2′=r2-nycdt′λh3′=r3-nzcdt′.
Furthermore
(42)[r1Th1′r1Tt′00r2Th2′0r2Tt′0r3Th3′00r3Tt′r2Th1′r2Tt′00r3Th1′r3Tt′00r1Th2′0r1Tt′0r3Th2′0r3Tt′0r1Th3′00r1Tt′r2Th3′00r2Tt′][λnxdcnydcnzdc]=[111000000].By solving (42), πc can be determined. Finally, using
(43)nc′=R′·cnc′d=dc-nTc′·t′
we can acquire the structure of plane πc′ with respect to camera frame {c′}.Step 2.
Computation ofR, t of camera frame {c′} with respect to desired camera frame {c*}.Similar to Step1, point [mTc′,1]T in frame {c′} is related to the corresponding point [mTc*,1]T in frame {c*} by Euclidean homography H:
(44)[dm1]≅(R-t·nTc′c′d)[mc′1]≅H[mc′1]
and the H satisfying ∥H∥F = 1 can also be derived using DLT method if there are n>4 sets of image point correspondences.Adjust the solvedH so that it satisfies det⁡(H)>0; there are
(45)αH=R-t·c′nTc′d,
where the scale factor α>0. Furthermore,
(46)α[nc′]×HT=[nc′]×RT.As we all know, the rotation matrixRk,θ whose axis and angle are k and θ can be described as
(47)Rk,θ=cos⁡θI3×3+sinθ[k]×+(1-cos⁡θ)kkT.
Note e3=[001]T. Choose
(48)k1=nc′×e3∥nc′×e3∥2θ1=arccosnTc′e3
then
(49)Rk1,θ1nc′=e3.Substitutenc′=Rk1,θ1Te3 into (46); according to Nanson’s equation AT[At]×A=[t]×, one obtains
(50)α[0-10100000]Rk1,θ1HT=[0-10100000]Rk1,θ1RT.Equation (50) means that the first two rows of matrix αRk1,θ1HT and Rk1,θ1RT are equal, respectively. Note
(51)Rk1,θ1HT=[r1Tr2TcT]
then the positive scale factor α and the rotation matrix R are
(52)α=2∥r1∥2+∥r2∥2(53)R=[αr1,αr2,α2(r1×r2)]Rk1,θ1.To refine the solvedR, apply singular value decomposition to it. As R=UDVT, choose R=UVT as the ultimate result. By substituting (52) and refined R into (45), the unknown t can finally be determined:
(54)t=dc′(R-αH)nc′.Till then, the homogeneous transformationc*Tc′ can be acquired:
(55)c*Tc′=[Rt0T1].
Furthermore, the transformation 0TE*, which represents the pose of the desired end-effector frame {E*} relative to the base frame {0}, can be computed as
(56)0TE*=0TE′·ETc·c*Tc′-1·ETc-1.In (56), 0TE′ can be known from the VBMMS’s kinematic model established in Section 3.1, and the hand-eye relationship ETc has already been known from Section 3.3. Finally, we can acquire the VBMMS’s control input q˙ from the analytical inverse kinematics proposed in Section 3.2.


## 4.1. Eye-Fixed Approximation

Figure4 illustrates the scheme of eye-fixed approximation. In Figure 4, m00 and m00* correspond to current image and desired image, respectively, and both are called zero-order moment of the blue concentric ring region to the object mark. mc and mc* are called nonhomogeneous projective coordinate of the center of blue concentric ring region to the object mark, which also correspond to current image and desired image, respectively.Figure 4
Scheme of eye-fixed approximation.It is well known that for nonhomogeneous projective coordinatemc=[x,y]T=[Xc/Zc,Yc/Zc]T, its time derivative m˙c is linearly related to the joints’ velocity q˙ through the interaction matrix Lcm:
(33)Lcm=[-1cZ0xcZxy-(1+y2)y0-1cZycZ1+y2-xy-x]×[ERcT-ERcT[Etc]×0ERcT]J(q)E,
where P=[cX,cY,cZ]T is the coordinates of a 3D point relative to current camera frame, ERc, Etc are the results of hand-eye relationship, and J(q)E is the generalized manipulator’s Jacobian in end-effector coordinate frame {E}. For the unknown variable cZ in Lcm, its estimation cZ^ can be chosen as follows:
(34)cZ^=Zc*m00*m00,
where Zc* is the constant depth of the 3D point relative to the desired camera frame.We partition the interaction matrix so as to isolate the second degree of freedom of the generalized manipulator. Note the following:(35)Ld=Lcmdiag⁡(010000)Ld⊥=Lcmdiag⁡(100111)
in which diag (·) is a diagonal matrix, Ld only includes the second column of Lcm, and Ld⊥ includes the first, fourth, fifth, and sixth columns of Lcm. We get
(36)cm˙=Ldq˙d+Ld⊥q˙d⊥.Notee2=m00-αm00*, where α is a constant coefficient; a simple approximation control law can be acquired by using feature e2, as
(37)q˙d=[0-λ1sgn⁡(e2)0000]T,
where sgn(·) is sign function and λ1is a time-invariant coefficient. Note e1=mc-mc*; let e1 be exponential convergent e˙1=-λe1. There is
(38)q˙d⊥=-Ld⊥+(λe1+Ldq˙d).The vectorλe1+Ldq˙d can be considered as a modified error that incorporates the original error e1 while taking into account the error that will be induced by robot motion q˙d.Under the influence of control input shown in (37) and (38), the VBMMS approaches to object with a constant speed until e2=0, and during the approximation process, the object is maintained in the FOV from beginning to end.


## 4.2. Static Look-and-Move Grasping

Figure5 illustrates the scheme of static look-and-move grasping.Figure 5
Scheme of static look-and-move grasping.In Figure5, 3D object point P is on the plane π whose normal is n. Meanwhile, there are three different camera frames: the desired camera frame {c*}, the initial camera frame {c}, and the current camera frame {c′}, that is, after a manipulator’s known movement from qi to qc. The homogeneous transformation c′Tc can be computed as
(39)c′Tc=[R′t′0T1]=ETc-1·0TE′-1·0TE·ETc,
where ETc is the hand-eye relationship, 0TE is the initial end-effector’s pose, and 0TE′ is the current end-effector’s pose. Our ultimate objective is to determine the pose of {c′} relative to {c*}, namely, R,t. In order to solve them, two steps are needed.Step 1.
Structure reconstruction of planeπ with respect to camera frame {c′}.Take current camera frame{c} as the reference frame, the plane π can be noted as cπ=[cnT,dc]T and point [cmT,1]T in frame {c} is related to the corresponding point [mTc′,1]T in frame {c′} by Euclidean homography H′:
(40)[mc′1]≅(R′-t′·cnTcd)[cm1]≅H′[cm1],
where ≅ means that the H′ transforms one vector into the other, up to a scale factor. Given at least four sets of image point correspondences c′mi↔cmi,  i=1,…,n, the H′ satisfying ∥H′∥F=1 can be derived using direct linear transformation (DLT) method.After solvingH′, written as H′=[h1′h2′h3′] and R′=[r1r2r3], cn=[nxnynz]T, there are
(41)λh1′=r1-nxcdt′λh2′=r2-nycdt′λh3′=r3-nzcdt′.
Furthermore
(42)[r1Th1′r1Tt′00r2Th2′0r2Tt′0r3Th3′00r3Tt′r2Th1′r2Tt′00r3Th1′r3Tt′00r1Th2′0r1Tt′0r3Th2′0r3Tt′0r1Th3′00r1Tt′r2Th3′00r2Tt′][λnxdcnydcnzdc]=[111000000].By solving (42), πc can be determined. Finally, using
(43)nc′=R′·cnc′d=dc-nTc′·t′
we can acquire the structure of plane πc′ with respect to camera frame {c′}.Step 2.
Computation ofR, t of camera frame {c′} with respect to desired camera frame {c*}.Similar to Step1, point [mTc′,1]T in frame {c′} is related to the corresponding point [mTc*,1]T in frame {c*} by Euclidean homography H:
(44)[dm1]≅(R-t·nTc′c′d)[mc′1]≅H[mc′1]
and the H satisfying ∥H∥F = 1 can also be derived using DLT method if there are n>4 sets of image point correspondences.Adjust the solvedH so that it satisfies det⁡(H)>0; there are
(45)αH=R-t·c′nTc′d,
where the scale factor α>0. Furthermore,
(46)α[nc′]×HT=[nc′]×RT.As we all know, the rotation matrixRk,θ whose axis and angle are k and θ can be described as
(47)Rk,θ=cos⁡θI3×3+sinθ[k]×+(1-cos⁡θ)kkT.
Note e3=[001]T. Choose
(48)k1=nc′×e3∥nc′×e3∥2θ1=arccosnTc′e3
then
(49)Rk1,θ1nc′=e3.Substitutenc′=Rk1,θ1Te3 into (46); according to Nanson’s equation AT[At]×A=[t]×, one obtains
(50)α[0-10100000]Rk1,θ1HT=[0-10100000]Rk1,θ1RT.Equation (50) means that the first two rows of matrix αRk1,θ1HT and Rk1,θ1RT are equal, respectively. Note
(51)Rk1,θ1HT=[r1Tr2TcT]
then the positive scale factor α and the rotation matrix R are
(52)α=2∥r1∥2+∥r2∥2(53)R=[αr1,αr2,α2(r1×r2)]Rk1,θ1.To refine the solvedR, apply singular value decomposition to it. As R=UDVT, choose R=UVT as the ultimate result. By substituting (52) and refined R into (45), the unknown t can finally be determined:
(54)t=dc′(R-αH)nc′.Till then, the homogeneous transformationc*Tc′ can be acquired:
(55)c*Tc′=[Rt0T1].
Furthermore, the transformation 0TE*, which represents the pose of the desired end-effector frame {E*} relative to the base frame {0}, can be computed as
(56)0TE*=0TE′·ETc·c*Tc′-1·ETc-1.In (56), 0TE′ can be known from the VBMMS’s kinematic model established in Section 3.1, and the hand-eye relationship ETc has already been known from Section 3.3. Finally, we can acquire the VBMMS’s control input q˙ from the analytical inverse kinematics proposed in Section 3.2.


## 5. Experiments and Analysis

In this section, we will discuss two experiments: hand-eye relationship determination and the VBMMS’s switch control scheme separately.


### 5.1. Experiment of Hand-Eye Relationship Determination

In order to simplify the complexity of grid corner extraction, choose a model plane which contains a pattern of 4 × 4 squares as the calibration object; the size of each square is 22 mm × 22 mm. Attach an object frame{o} to it; the upper left corner of the first square is selected to be associated to the origin point of {o}, the Z axis is perpendicular to the model plane pointing outward, the XY-plane of {o} is aligned with the plane, so that points on the plane have zero Z-coordinate, and X, Y axes are paralleled to the sides of the checkerboard’s square, respectively.Let the TMR be static; take six images of the plane under different orientations caused by several known manipulator movements. The images are shown in Figure6, whose resolutions are all 640 × 480, and the corners are detected as the intersection of straight lines fitted to each square.Six images of a model plane under different orientation, together with the corners (indicated by red cross and blue square).
(a)
(b)
(c)
(d)
(e)
(f)Using Zhang’s calibration method, the refined camera intrinsic parameter matrixK, together with the refined extrinsic parameters Toci(i=1,…,6) corresponding to the six images respectively, can be acquired:
(57)K=[816.099240391.054090800.56461221.07601001].The known movements of the manipulator corresponding to six images are shown in Table4.Table 4
The known movements of the generalized manipulator.
Image
θ
1, d2, θ3
θ
4
θ
5
θ
6
1
0
0.5756
−0.0245
−1.1241
2
0
0.5710
−0.2224
−1.0983
3
0
0.4560
−0.2256
−1.2930
4
0
0.2143
0.0360
−1.5370
5
0
−0.0012
0.1302
−1.6249
6
0
−0.3974
−0.1872
−1.9276From Table4, the homogeneous transformation 0TEi can be computed using generalized manipulator’s kinematic model. The computed 0TEi and corresponding ciTo are shown in Table 5.Table 5
The pose information corresponding to known movements of the generalized manipulator.
Roll
Pitch
Yaw
p
x
p
y
p
z
I1
0
T
E
1
2.9713
1.4401
−2.9833
42.2281
−0.6184
41.7720
c
1
T
o
−2.5304
−0.1911
−1.6459
−6.2760
−7.2463
45.1793
I2
0
T
E
2
1.9856
1.3546
−2.0969
41.7209
−5.4980
43.0438
c
2
T
o
−2.6322
−0.0152
−1.7485
−17.8379
−1.7928
44.5287
I3
0
T
E
3
2.2297
1.2953
−2.3112
38.8304
−6.0230
43.0616
c
3
T
o
−2.5396
0.0100
−1.7451
−20.4513
−6.5954
45.0891
I4
0
T
E
4
−2.9439
1.3868
2.9484
33.4385
1.0060
46.4443
c
4
T
o
−2.4884
−0.2583
−1.6244
−2.9982
−11.5786
54.2764
I5
0
T
E
5
−1.9581
1.4303
1.9545
27.6891
3.6303
52.0272
c
5
T
o
−2.6579
−0.3423
−1.6116
2.7396
−2.0687
63.8751
I6
0
T
E
6
1.3769
1.3922
−1.3078
16.0177
−4.8820
54.7535
c
6
T
o
−2.7377
−0.0032
−1.5332
−23.9188
1.5681
71.5180Combine image 1 and image 2 (noted as{12}). Using calibration method mentioned in Section 3.3, matrix H1∈R9×9 can be determined. Similarly, combinations of {13}, {14}, {15}, and {16} can determine H2, H3, H4, H5, respectively. The hand-eye relationship ETc can finally be determined:
(58)ETc=[0.06440.9090.4117-10.4233-0.98790.1164-0.1023-6.1634-0.1409-0.40020.9055-17.24310001].


### 5.2. Experiment of VBMMS’s Switch Control Scheme

The images corresponding to the desired and initial camera position and the object mark recognition using Gaussian model and Hough transformation are illustrated in Figure7.Images of the target for the desired/initial camera position and object mark recognition using Gaussian model and Hough transformation.
(a)
Desired image
(b)
Initial image
(c)
Desired binary image
(d)
Initial binary imageAs can be seen in Figure7, the blue concentric ring region of the object mark can be detected robustly and conveniently using Gaussian model for color-based segmentation and Hough transformation for segmented region’s ellipse fitting.Chooseα=0.5, and the gain involved in the approximation control law (37) λ1=0.01. The gain involved in the eye-fixed control law (38) λ=0.15, c*Z is set to be 25 cm while its true value is 31 cm; the computed eye-fixed approximation control input is given in Figure 8.The computed eye-fixed approximation control input.
(a)
(b)Figure9 shows the images corresponding to the camera position {c}, where the eye-fixed approximation process terminated, and the position {c′}, where the VBMMS does a known movement from q=[0,0,0,1.2820,-0.0240,-0.2175]T to q′=[0,0,0,1.3894,-0.1964,-0.0416]T, together with the extracted corner correspondences, matched by RANSAC (random sample consensus) method. In addition to the corner extraction, the ROI (region of interest) is selected to be within the concentric ring.Two images of the object mark corresponding to{c} and {c′}, together with the extracted corner correspondences.
(a)
Image corresponding to{c}
(b)
Image corresponding to{c′}
(c)
The extracted corner correspondences matched by RANSACThe corner correspondences given in Figure9 is leading to the homography H′ satisfying ∥H′∥F=1. Using Step 1 mentioned in Section 3.2, the 3D structure of the object mark plane with respect to the camera frame {c′} can be computed:
(59)πc′=[-0.24280.2620-0.934015.7297]T.Figure10 shows the images corresponding to the camera position {c′} and the desired camera position {c*}, together with the extracted corner correspondences matched by RANSAC.Two images of the object mark corresponding to{c′} and {c*}, together with the extracted corner correspondences.
(a)
Image corresponding to{c′}
(b)
Image corresponding to{c*}
(c)
The extracted corner correspondences matched by RANSACApplying DLT to the corner correspondences, the homographyH satisfying det⁡(H)>0 can be acquired; then using Step 2 mentioned in Section 3.2, the transformation c′Tc* can be computed. Finally the 0TE* and the control input of the VBMMS can be determined from the inverse kinematics discussed in Section 3.2:
(60)q˙=[-0.18475.38290.12371.3305-0.0058-0.1220]T.Affected by the computed control input, the VBMMS moved to a position corresponding to the desired camera frame{c*}. Thereby, the visual servo grasping task is executed successfully.


## 5.1. Experiment of Hand-Eye Relationship Determination

In order to simplify the complexity of grid corner extraction, choose a model plane which contains a pattern of 4 × 4 squares as the calibration object; the size of each square is 22 mm × 22 mm. Attach an object frame{o} to it; the upper left corner of the first square is selected to be associated to the origin point of {o}, the Z axis is perpendicular to the model plane pointing outward, the XY-plane of {o} is aligned with the plane, so that points on the plane have zero Z-coordinate, and X, Y axes are paralleled to the sides of the checkerboard’s square, respectively.Let the TMR be static; take six images of the plane under different orientations caused by several known manipulator movements. The images are shown in Figure6, whose resolutions are all 640 × 480, and the corners are detected as the intersection of straight lines fitted to each square.Six images of a model plane under different orientation, together with the corners (indicated by red cross and blue square).
(a)
(b)
(c)
(d)
(e)
(f)Using Zhang’s calibration method, the refined camera intrinsic parameter matrixK, together with the refined extrinsic parameters Toci(i=1,…,6) corresponding to the six images respectively, can be acquired:
(57)K=[816.099240391.054090800.56461221.07601001].The known movements of the manipulator corresponding to six images are shown in Table4.Table 4
The known movements of the generalized manipulator.
Image
θ
1, d2, θ3
θ
4
θ
5
θ
6
1
0
0.5756
−0.0245
−1.1241
2
0
0.5710
−0.2224
−1.0983
3
0
0.4560
−0.2256
−1.2930
4
0
0.2143
0.0360
−1.5370
5
0
−0.0012
0.1302
−1.6249
6
0
−0.3974
−0.1872
−1.9276From Table4, the homogeneous transformation 0TEi can be computed using generalized manipulator’s kinematic model. The computed 0TEi and corresponding ciTo are shown in Table 5.Table 5
The pose information corresponding to known movements of the generalized manipulator.
Roll
Pitch
Yaw
p
x
p
y
p
z
I1
0
T
E
1
2.9713
1.4401
−2.9833
42.2281
−0.6184
41.7720
c
1
T
o
−2.5304
−0.1911
−1.6459
−6.2760
−7.2463
45.1793
I2
0
T
E
2
1.9856
1.3546
−2.0969
41.7209
−5.4980
43.0438
c
2
T
o
−2.6322
−0.0152
−1.7485
−17.8379
−1.7928
44.5287
I3
0
T
E
3
2.2297
1.2953
−2.3112
38.8304
−6.0230
43.0616
c
3
T
o
−2.5396
0.0100
−1.7451
−20.4513
−6.5954
45.0891
I4
0
T
E
4
−2.9439
1.3868
2.9484
33.4385
1.0060
46.4443
c
4
T
o
−2.4884
−0.2583
−1.6244
−2.9982
−11.5786
54.2764
I5
0
T
E
5
−1.9581
1.4303
1.9545
27.6891
3.6303
52.0272
c
5
T
o
−2.6579
−0.3423
−1.6116
2.7396
−2.0687
63.8751
I6
0
T
E
6
1.3769
1.3922
−1.3078
16.0177
−4.8820
54.7535
c
6
T
o
−2.7377
−0.0032
−1.5332
−23.9188
1.5681
71.5180Combine image 1 and image 2 (noted as{12}). Using calibration method mentioned in Section 3.3, matrix H1∈R9×9 can be determined. Similarly, combinations of {13}, {14}, {15}, and {16} can determine H2, H3, H4, H5, respectively. The hand-eye relationship ETc can finally be determined:
(58)ETc=[0.06440.9090.4117-10.4233-0.98790.1164-0.1023-6.1634-0.1409-0.40020.9055-17.24310001].


## 5.2. Experiment of VBMMS’s Switch Control Scheme

The images corresponding to the desired and initial camera position and the object mark recognition using Gaussian model and Hough transformation are illustrated in Figure7.Images of the target for the desired/initial camera position and object mark recognition using Gaussian model and Hough transformation.
(a)
Desired image
(b)
Initial image
(c)
Desired binary image
(d)
Initial binary imageAs can be seen in Figure7, the blue concentric ring region of the object mark can be detected robustly and conveniently using Gaussian model for color-based segmentation and Hough transformation for segmented region’s ellipse fitting.Chooseα=0.5, and the gain involved in the approximation control law (37) λ1=0.01. The gain involved in the eye-fixed control law (38) λ=0.15, c*Z is set to be 25 cm while its true value is 31 cm; the computed eye-fixed approximation control input is given in Figure 8.The computed eye-fixed approximation control input.
(a)
(b)Figure9 shows the images corresponding to the camera position {c}, where the eye-fixed approximation process terminated, and the position {c′}, where the VBMMS does a known movement from q=[0,0,0,1.2820,-0.0240,-0.2175]T to q′=[0,0,0,1.3894,-0.1964,-0.0416]T, together with the extracted corner correspondences, matched by RANSAC (random sample consensus) method. In addition to the corner extraction, the ROI (region of interest) is selected to be within the concentric ring.Two images of the object mark corresponding to{c} and {c′}, together with the extracted corner correspondences.
(a)
Image corresponding to{c}
(b)
Image corresponding to{c′}
(c)
The extracted corner correspondences matched by RANSACThe corner correspondences given in Figure9 is leading to the homography H′ satisfying ∥H′∥F=1. Using Step 1 mentioned in Section 3.2, the 3D structure of the object mark plane with respect to the camera frame {c′} can be computed:
(59)πc′=[-0.24280.2620-0.934015.7297]T.Figure10 shows the images corresponding to the camera position {c′} and the desired camera position {c*}, together with the extracted corner correspondences matched by RANSAC.Two images of the object mark corresponding to{c′} and {c*}, together with the extracted corner correspondences.
(a)
Image corresponding to{c′}
(b)
Image corresponding to{c*}
(c)
The extracted corner correspondences matched by RANSACApplying DLT to the corner correspondences, the homographyH satisfying det⁡(H)>0 can be acquired; then using Step 2 mentioned in Section 3.2, the transformation c′Tc* can be computed. Finally the 0TE* and the control input of the VBMMS can be determined from the inverse kinematics discussed in Section 3.2:
(60)q˙=[-0.18475.38290.12371.3305-0.0058-0.1220]T.Affected by the computed control input, the VBMMS moved to a position corresponding to the desired camera frame{c*}. Thereby, the visual servo grasping task is executed successfully.


## 6. Conclusion

It is nearly impossible for VBMMS to finish household objects’ grasping and delivering operation without some prior knowledge (such as objects’ color, texture, sizes, and localization) provided by people, not only for the household objects’ variety and operation complexity, but also for the difficulties of the VBMMS’s kinematic modeling and its nonholonomic constraint’s handling. On the one hand, a new QR Code-based artificial object mark is designed, which can store object’s property information and operation information and can be easily distinguished from complex family environment. On the other hand, in order to model the VBMMS, we summarize it as a generalized manipulator, followed by acquiring its analytical inverse kinematic solutions and determining the hand-eye relationship based upon active vision. Meanwhile, in order to deal with the VBMMS’s nonholonomic constraint, a visual servo switching control law which is composed of eye-fixed approximation part and static look-and-move grasping part is designed. The proposed scheme can solve the household objects’ grasping and delivering problem well, and makes it possible to let VBMMS-type service robot provide better housekeeping service.



---

*Source: 315396-2014-09-24.xml*