[ieee 2013 ieee international conference on robotics and automation (icra) - karlsruhe, germany...

6
IMPROVING IMAGE-BASED VISUAL SERVOING WITH REFERENCE FEATURES FILTERING J. Ernesto Solanes and Leopoldo Armesto and Josep Tornero and Vicent Girb´ es Abstract— In this paper, Image-Based Visual Servoing (IBVS) is addressed via a new control structure where reference features are previously filtered based on an estimation of these reference features using standard filters such as Kalman Filter (KF) or a combination of a Kalman Filter and a Smoother (KFS). In this sense, one of the key aspects of the paper is to predict feasible reference features for the low-level IBVS controller. Along the paper, we discuss and analyze the improvements introduced with the new control structure in terms of convergence time and reachability, that is, the ability to converge in complex scenarios such as rotations around the camera axis and large displacements. Validations are also provided through real experimentation with an industrial robot of 6 DOF in eye-in-hand configuration. I. INTRODUCTION During the last decade has been an increment of visual servoing applications in many industrial sectors. Technolo- gical advances have allowed the implementation of visual control systems, since control actions can be generated in real-time based on visual data. In regard to the kind of feedback information considered, three main approaches can be distinguished: image-based visual servoing (IBVS) where the feedback is defined in the image plane, position-based visual servoing (PBVS) where the feedback is composed of 3D data such as the robotic system pose, and the 2-1/2D visual servoing where the feedback combines both 2D and 3D data. Further details about the different approaches can be found in [1], [2]. This paper is focused on IBVS strategy based on the interaction matrix [2]. The control law is based on the error between current and desired features on the image plane, which can be static for positioning problems or dynamic for tracking problems. IBVS approach is known to be robust even with modelling errors and is one of the most used in the practice, however, several drawbacks can be mentioned. One of the most inves- tigated problem is the local minima and singularities in the interaction matrix [3]. Numerous works have addressed this problem. For example, in [4] authors try to solve the problem designing adequate visual features like lines, spheres, circles, etc., and combined in order to obtain linearized properties. The control law is a decoupled exponential decreasing law. This work was supported by VALi+d Program (Generalitat Valenciana), DIVISAMOS Project (Spanish Ministry), PROMETEO Program (Con- selleria d’Educaci´ o, Generalitat Valenciana) and SAFEBUS: Ministry of Economy and Competitivity, IPT-2011-1165-370000) J. Ernesto Solanes, Leopoldo Armesto, Josep Tornero and Vicent Girb´ es are with Institute of Design and Manufacturing at Universitat Polit` ecnica de Val` encia, Cam´ ı de Vera s/n, 46022, Val` encia, Spain. Corresponding author: J. Ernesto Solanes {[email protected]} Other point of view is the use of path-planning and trajectory tracking approaches [5], [6]. Usually, this solution allows take into account 2D and 3D visibility constraints. Other way is to modify the control law. Many works can be found such as predictive control [7], [8], optimal control [9] or adaptive control [10] among others. For example, in [7] authors show an alternative approach where the visual servoing task is formulated into a nonlinear optimization problem. The advantage is the capability of dealing with visibility constraints and 3D constraints. The contribution of this paper is to propose a new control structure based on the filtering of the reference features in order to provide more feasible image trajectories from the current state to the desired one. In this paper, Kalman Filter and Kalman Filter-Smoother [11] techniques are used for feature filtering, and obviously it is easily extensible to other type of filters/smoothers. The filtered reference features are used as references for an underlying IBVS controller, which have shown greater performance in terms of convergence time as well as reachability, that is, the ability to solve complex scenarios such as rotations around the camera axis and large displacements. Comparison of both filtering approaches are analyzed, discussed and validated in a real scenario with a 6 DOF robot arm. II. MATHEMATICAL BACKGROUND Without loss of generality, this paper considers points as object features (s t ) to perform the visual servoing task, being the approach extensible to any other kind of features. Moreover, the interaction matrix L s (s t ) [3] is non-linear and the depth Z t is correspondingly estimated based on Dementhon linear approach for estimating 3D pose for planar objects (see [12] for further information). Point-like features s t =(u t ,v t ,Z t ) T are expressed in normalized coordinates such that u t = X t /Z t , v t = Y t /Z t and thus the interaction matrix L s (s t ) is given by [13]: L s (s t )= - 1 Z t 0 u t Z t uv - (1 + u 2 t ) vt 0 - 1 Z t v t Z t 1+ v 2 t - ut vt - ut 0 0 - 1 - yt Zt xt Zt 0 (1) This representation of the interaction matrix, which in- cludes 3D data related to the depth between the camera and the features detected, will be used in all control structures presented in this paper. In addition to this, we deal with the non-free flying camera case, where motions are restricted by the kinematic constrains imposed by a manipulator carrying the vision sensor (eye-in-hand configuration). 2013 IEEE International Conference on Robotics and Automation (ICRA) Karlsruhe, Germany, May 6-10, 2013 978-1-4673-5643-5/13/$31.00 ©2013 IEEE 3083

Upload: vicent

Post on 23-Dec-2016

215 views

Category:

Documents


1 download

TRANSCRIPT

Page 1: [IEEE 2013 IEEE International Conference on Robotics and Automation (ICRA) - Karlsruhe, Germany (2013.05.6-2013.05.10)] 2013 IEEE International Conference on Robotics and Automation

IMPROVING IMAGE-BASED VISUAL SERVOINGWITH REFERENCE FEATURES FILTERING

J. Ernesto Solanes and Leopoldo Armesto and Josep Tornero and Vicent Girbes

Abstract— In this paper, Image-Based Visual Servoing (IBVS)is addressed via a new control structure where referencefeatures are previously filtered based on an estimation ofthese reference features using standard filters such as KalmanFilter (KF) or a combination of a Kalman Filter and aSmoother (KFS). In this sense, one of the key aspects of thepaper is to predict feasible reference features for the low-levelIBVS controller. Along the paper, we discuss and analyze theimprovements introduced with the new control structure interms of convergence time and reachability, that is, the abilityto converge in complex scenarios such as rotations aroundthe camera axis and large displacements. Validations are alsoprovided through real experimentation with an industrial robotof 6 DOF in eye-in-hand configuration.

I. INTRODUCTION

During the last decade has been an increment of visualservoing applications in many industrial sectors. Technolo-gical advances have allowed the implementation of visualcontrol systems, since control actions can be generated inreal-time based on visual data. In regard to the kind offeedback information considered, three main approaches canbe distinguished: image-based visual servoing (IBVS) wherethe feedback is defined in the image plane, position-basedvisual servoing (PBVS) where the feedback is composed of3D data such as the robotic system pose, and the 2-1/2Dvisual servoing where the feedback combines both 2D and3D data. Further details about the different approaches canbe found in [1], [2].

This paper is focused on IBVS strategy based on theinteraction matrix [2]. The control law is based on the errorbetween current and desired features on the image plane,which can be static for positioning problems or dynamic fortracking problems.

IBVS approach is known to be robust even with modellingerrors and is one of the most used in the practice, however,several drawbacks can be mentioned. One of the most inves-tigated problem is the local minima and singularities in theinteraction matrix [3]. Numerous works have addressed thisproblem. For example, in [4] authors try to solve the problemdesigning adequate visual features like lines, spheres, circles,etc., and combined in order to obtain linearized properties.The control law is a decoupled exponential decreasing law.

This work was supported by VALi+d Program (Generalitat Valenciana),DIVISAMOS Project (Spanish Ministry), PROMETEO Program (Con-selleria d’Educacio, Generalitat Valenciana) and SAFEBUS: Ministry ofEconomy and Competitivity, IPT-2011-1165-370000)

J. Ernesto Solanes, Leopoldo Armesto, Josep Tornero and Vicent Girbesare with Institute of Design and Manufacturing at Universitat Politecnicade Valencia, Camı de Vera s/n, 46022, Valencia, Spain.

Corresponding author: J. Ernesto Solanes {[email protected]}

Other point of view is the use of path-planning and trajectorytracking approaches [5], [6]. Usually, this solution allowstake into account 2D and 3D visibility constraints. Otherway is to modify the control law. Many works can befound such as predictive control [7], [8], optimal control[9] or adaptive control [10] among others. For example, in[7] authors show an alternative approach where the visualservoing task is formulated into a nonlinear optimizationproblem. The advantage is the capability of dealing withvisibility constraints and 3D constraints.

The contribution of this paper is to propose a new controlstructure based on the filtering of the reference features inorder to provide more feasible image trajectories from thecurrent state to the desired one. In this paper, Kalman Filterand Kalman Filter-Smoother [11] techniques are used forfeature filtering, and obviously it is easily extensible to othertype of filters/smoothers. The filtered reference features areused as references for an underlying IBVS controller, whichhave shown greater performance in terms of convergencetime as well as reachability, that is, the ability to solvecomplex scenarios such as rotations around the cameraaxis and large displacements. Comparison of both filteringapproaches are analyzed, discussed and validated in a realscenario with a 6 DOF robot arm.

II. MATHEMATICAL BACKGROUND

Without loss of generality, this paper considers pointsas object features (st) to perform the visual servoing task,being the approach extensible to any other kind of features.Moreover, the interaction matrix Ls(st) [3] is non-linearand the depth Zt is correspondingly estimated based onDementhon linear approach for estimating 3D pose for planarobjects (see [12] for further information).

Point-like features st = (ut, vt, Zt)T are expressed in

normalized coordinates such that ut = Xt/Zt, vt = Yt/Zt

and thus the interaction matrix Ls(st) is given by [13]:

Ls(st)=

− 1Zt

0 utZt

uv −(1 + u2t ) vt

0 − 1Zt

vtZt

1 + v2t −utvt −ut

0 0 −1 −ytZt xtZt 0

(1)

This representation of the interaction matrix, which in-cludes 3D data related to the depth between the camera andthe features detected, will be used in all control structurespresented in this paper. In addition to this, we deal with thenon-free flying camera case, where motions are restricted bythe kinematic constrains imposed by a manipulator carryingthe vision sensor (eye-in-hand configuration).

2013 IEEE International Conference on Robotics and Automation (ICRA)Karlsruhe, Germany, May 6-10, 2013

978-1-4673-5643-5/13/$31.00 ©2013 IEEE 3083

Page 2: [IEEE 2013 IEEE International Conference on Robotics and Automation (ICRA) - Karlsruhe, Germany (2013.05.6-2013.05.10)] 2013 IEEE International Conference on Robotics and Automation

The relation between the velocity associated to the manip-ulator and the image parameters rates is:

st=Lst(st)·cVr ·rJr(qt)·qt (2)

where cVr is the twist velocity matrix, rJr(qt) is therobot end-effector Jacobean and qt and qt are the robotconfiguration and their corresponding velocities.

The most common approach to image-based visual ser-voing (IBVS) is to generate a proportional control law (see[14] under the form:

qt=−λ·J+1 (st,qt)·(st − s∗t ) (3)

where λ is a proportional gain, J1(st,qt) = Ls(st) ·cVr ·rJr(qt), J+

1 (st,qt) its pseudo-inverse and st and s∗t are thecurrent and the desired features, respectively.

Using this classic approach and depending on the choice ofZt, it is well know that the convergence is only guaranteed inthe neighborhood of the desired position [15]. In that sense,we can analyze the stability of this task function consideringthe following error function [13]:

et= L+s (st) · (st − s∗t ) (4)

where L+s (st) is the pseudo-inverse of the interaction matrix.

Considering the most general case, where Ls(st) is a time-varying matrix due to scale variations, the derivative of thetask function is:

et=∂L+

s (st)

∂t(st − s∗t ) + L+

s (st) · st (5)

and considering eqs. (2) and (3) we have,

et=

(∂L+

s (st)

∂t−λL+

s (st)·J1(st,qt)·J+1(st,qt)

)·et (6)

where et = (st − s∗t ) is the feature’s error.Thus, the error et is asymptotically stable in a neighbor-

hood of st ' s∗t if and only if the linearized system isstable. This implies that, in an IBVS problem with referencefeatures “far” from detected ones, the stability of the systemcannot be guaranteed. Many contributions cope with thisproblem by minimizing the calibration errors of the visionsystem [16], minimizing modeling errors of the roboticsystem [17] as well as improving and limiting the control lawby considering additional constrains, such as kinematic orfeature constrains [18]. However, to the author’s knowledge,none of this approaches have ever considered to minimizethe instantaneous effect of et (in terms of magnitude butalso in direction).

III. REFERENCE FEATURES FILTERING

A. Control Structure

In this section, the main contribution of the paper relatedwith the use of standard filtering tools, such Kalman Filter(KF) and also using a combination of a filter an any smoothersuch as the Kalman Filter-Smoother [11], in order to providea filtered trajectory of the desired features depending on the

Fig. 1. KF/KFS-VS structure. Compared to the classic IBVS approach,reference features s∗t are filtered, thus in classic IBVS s∗d,t = s∗t , while inKF/KFS-VS s∗d,t 6= s∗t .

0 10 20 30

−10

−5

0

log M

SE

(s)

[m2]

time [sec]

Fig. 2. Log-scale of Mean Squared Error (MSE) of features with respectto the reference for IBVS (continuous blue line) or modified features forKF/KFS-VS (dashed red line).

current features are. It is coined as Kalman Filter Visual Ser-voing (KF-VS) or Kalman Filter-Smoother Visual Servoing(KFS-VS), if a KF or smoother is used, respectively.

The key idea of the paper is to propose an original controlstructure based on IBVS controller as described in Section IIand a estimation to modify the desired reference features by aset of modified features. With the proposed control structure,shown in Figure 1 we will:

• improve local stability because modified features are“closer” to the current measured features with respectto the original desired reference features. This allows usto apply higher control gains λ in Equation (3) whilestabilizing the system. In classic IBVS the error can bepotentially large during the first iterations, so it requiressmall steps to keep the system stable, which causes veryslow progression when measured and target featuresbecome closer (analogous to fixed-step gradient basedmethods in optimization techniques). Our method keepsa lower error during the whole execution that allowsus to apply higher control gains, and consequently weobtain better time-performance as shown in Figure 2.

• improve global stability because modified featuresbelong to a trajectory, that is, asymptotically convergeto the desired reference features (analogous to modifythe search direction of gradient-based methods). As it

3084

Page 3: [IEEE 2013 IEEE International Conference on Robotics and Automation (ICRA) - Karlsruhe, Germany (2013.05.6-2013.05.10)] 2013 IEEE International Conference on Robotics and Automation

Fig. 3. Simulation set-up with an inverted 6 DOF robot arm.

will be shown later in Section IV, our method is able tosolve more complex situations and thus provides larger“reachability” regions.

B. Algorithm

The algorithm provides a new filtered reference features,which are closer to the current features than the desired oness∗t , in order to guarantee local stability and, in that sense, tobe able solve problems that the classic IBVS cannot solve.

At the first time, it is necessary to obtain a model of thesystem. In this paper, we use a local model based on theinteraction matrix [7], where the state is augmented xt =[sTt qT

t ]T in order to take into account the robot coordinates:

xt+1=Atxt +Btqt (7)st=Htxt (8)

where,

At=I(3×i+n,3×i+n) (9)

Bt=

[Ls(st)·cVr ·rJer

Ts · I(n×n)

](3×i+n,n)

(10)

Ht=[I(3×i,3×i) 0(3×i,n)

](11)

and Ts is the sampling time, i the number of features andn the number of robot’s degrees of freedom. Control actionsare computed based on the standard IBVS algorithm.

The KFS-VS algorithm generates a smoothed distributionunder constant observation s∗t (over the prediction step).The algorithm used is well-known so only necessary detailsare given. It decomposes in two stages [11]: filtering andsmoothing. At the KF step, the estimation starts with nullcovariance Pt|t = 0 and ideally converges to s∗t with infinitetime horizon, although due to computational time constraintsthe system only evolves during a given time horizon T =t+Np, where Np is the number of iterations. During filtering,the system evolves in “open-loop” mode at the predictionstep since control inputs are unknown. Thus, the KF providesa trajectory {xt+1|t+1, ..., xT |T }. Later, a Kalman Smoother(KS) produces a trajectory which takes into account “future”observations under the form {xT |T , xT−1|T , ..., xt+1|T }. Atthe final step the algorithm establishes as “desired” featuress∗d = st+1|T , which comprise all the knowledge about how

the system must evolve to reach the real target features and,as a consequence of the combination of the KF and KS, theresulting trajectories show up a predictive behavior.

IV. ANALYSIS

In despite of the fact that the proposed method has greaternumber of parameters than classic IBVS approach, the addi-tional parameters are those coming from a conventional KFso tuning of the algorithm is straight forward and analogousto KF tuning. Therefore the covariance matrix Qt and Rt

affect to the filtering results of the KFS and thus to the overallperformance. For instance, small measurement covariancesimply high confidence on measurements and thus, the tra-jectory will evolve faster/sharper to final reference featuresand thus the behavior is similar to classic IBVS approach.On the contrary, large measurement covariances imply smallconfidence on observations and thus the trajectory evolveslower (the kinematics of the KFS-VS approach can bepotentially slower than the closed-loop dynamics of IBVSif they are not properly set). Also the prediction horizon Np

has some influence over the response but important influenceover the computational time.

The results shown in this section, are obtained using KFS-VS since KF is a particular case when Np = 1.

A. Simulation Set-up Description

For all simulations and experimental results, the samplingtime is Ts = 40ms, which corresponds to the real-timeprocessing at 25 fps of the vision system. The controltask consist of positioning a perspective camera mountedon an inverted 6 DOF industrial robot with eye-in-handconfiguration (Figure 3). The target is composed of fourvertexes forming a squared object of 30 cm side length.

B. Reachability and Convergence Time Analysis

1) Pure rotation around the optical axis: Rotation aroundthe optical axis of the camera is one of the most representa-tive and challenging cases in visual servoing tasks. On “free-camera” problem, IBVS classic algorithm fails only whenthe rotation is 180o. Our approach is focused in a “non-free camera” problem, which is subject to robot kinematicconstraints. In this particular case, the classic IBVS approachcannot reach rotation higher than 90o around the optical axis(see Figure 4(a)) despite of the value for the control gain λ(red region). On the contrary, for a correct parameter settingof covariances matrices and prediction horizon, the KFS-VSis able to solve rotations up to 180o degrees, as depictedin Figure 4(b). In addition to this, the convergence time isabout three times smaller compared to classic IBVS. Previousfigures show that values of λ are higher in KFS-VS thanclassical IBVS without become unstable.

2) Influence of covariance matrices: In general, the co-variance matrix Qt will have little influence, while thefiltering is more sensible to different values on covariancematrix Rt. If the covariance matrix Qt is too large, then thedynamics of the filter become slower than the closed-loopdynamics and thus convergence time increases exponentially

3085

Page 4: [IEEE 2013 IEEE International Conference on Robotics and Automation (ICRA) - Karlsruhe, Germany (2013.05.6-2013.05.10)] 2013 IEEE International Conference on Robotics and Automation

(a) Using classic IBVS structure. (b) Using KFS-VS structure.

Fig. 4. λ sensibility analysis of reachability regions and convergence time for different rotation angles. The red regions indicate that KFS-VS was unableto converge.

(a) Qt=Q · I, Rt = I and λ = 2.5. (b) Qt = I, Rt=R · I and λ = 2.5.

Fig. 5. Influence of covariance matrices magnitudes over reachability and convergence time for different rotation angles. The red regions indicate thatKFS-VS was unable to converge.

Fig. 6. Influence of the prediction horizon Np over reachability andcomputational time with (λ = 2.5 Rt = I and Qt = I).

as shown in Figure 5(a) for larger values of Qt. If thecovariance matrix Rt is too large, then the trajectoriesmodified by KF converge too quick to the reference featuresand thus the “filtered” effect is cancel out, which impliesthat reachability is similar to the classic IBVS case, (Figure5(b)).

3) Horizon prediction influence: Horizon prediction pa-rameter Np has little influence over the reachability, whileclearly affects to the convergence time as shown in Figure 6.On the other hand, the computational time is linear O(Np)with respect to this parameter1 as shown in Figure 6.

V. EXPERIMENTAL RESULTS

In order to validate the proposed control structure, it hasbeen implemented to perform visual servoing with a Kuka

1Results have been obtained with a ASUS laptop with Ubuntu 12.04 asOS and hardware configuration: Intel Core i7-2670QM, 8GB of RAM @2.2GHz and a GeForce GTX 560M 2GB

KR5 sixx R650 industrial robot arm. The experimentation isbased on pure rotation around camera optical axis studied inSection IV-B.1 and, in addition, to a large displacements.

Figure 7(a) depicts the robot initial configuration for therotational case, where the initial pose is turned an angle of150o degrees with respect Z axis and the distance of 0.205m below the camera reference system. On the other hand,a large displacement case is shown in Figure 7(b), wherethe object is at XY Z =

[0.312 0.202 0.819

]meters

and rotated RPY =[5.65 39.23 54.84

]degrees w.r.t the

camera. The camera is at XY Z =[0.280 0.046 −0.524

]meters and RPY =

[177.23 12.30 50.31

]degrees w.r.t

the robot base. The above measures are obtained with respectto the world reference system located at the robot base. Thisproblem is even harder to solve since it implies large scalevariations, rotations and Cartesian shifts.

Parameterization setting for both experiments is λ = 4,Qt = I and Rt = 10 · I. Both cases show a comparisonbetween KF-VS and KFS-VS has made in order to comparetwo filtering strategies and how affect them to the systembehavior.

Figures 8(a) and 8(b) show image plane trajectories de-scribed with the KF-VS and KFS-VS algorithms, using thesame covariance matrices and the control gain. In the rotationproblem, the trajectories result are quite similar but thecontrol action are more smoothed in the case of KFS-VSthan in the KF-VS, providing a better movement of therobot joints (see Figures 8(c) and 8(e)). In large displacementproblem, both of them solve it but the behavior in the imageplane are different, see Figure 8(b). This is because, in

3086

Page 5: [IEEE 2013 IEEE International Conference on Robotics and Automation (ICRA) - Karlsruhe, Germany (2013.05.6-2013.05.10)] 2013 IEEE International Conference on Robotics and Automation

(a) Set-up for rotation case: the object is rotated 150o with respect to theinitial position.

(b) Set-up for large displacement problem: the object is rotated and translatedwith respect to the initial position.

Fig. 7. Experimentation set-up.

250 300 350 400

150

200

250

300

u [pixel]

v [

pix

el]

250 300 350 400

150

200

250

300

350

u [pixel]

v [

pix

el]

1

2*

1*

4

2

3*

4*

3

4*

3

3*

2

2*

1

4

1*

(a) Image plane trajectories for Rotation problem (Object rotation 150o): leftfigure KFS-VS; right figure KF-VS.

200 300 400 500 600 700

200

250

300

350

400

450

x [pixel]

v [

pix

el]

(b) Image plane trajectories for large displacement problem: dashed linesKF-VS; solid lines KFS-VS.

0 1 2 3 4 5 6 7 8 9−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

time [sec.]

∂q

/∂t

∂q1/∂t

∂q2/∂t

∂q3/∂t

∂q4/∂t

∂q5/∂t

∂q6/∂t

(c) Control actions using KF-VS structure for Rotation problem (Objectrotation 150o).

0 1 2 3 4 5−1

−0.5

0

0.5

time [sec]

∂q

/∂t

[ra

d/s

]

∂q1/∂t

∂q2/∂t

∂q3/∂t

∂q4/∂t

∂q5/∂t

∂q6/∂t

(d) Control actions using KF-VS structure for large displacement problem.

0 2 4 6 8 10

−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

time [sec.]

∂q

/∂t

∂q1/∂t

∂q2/∂t

∂q3/∂t

∂q4/∂t

∂q5/∂t

∂q6/∂t

(e) Control actions using KFS-VS structure for rotation problem (Objectrotation 150o).

0 1 2 3 4 5−1

−0.5

0

0.5

time [sec]

∂q

/∂t

[ra

d/s

]

∂q1/∂t

∂q2/∂t

∂q3/∂t

∂q4/∂t

∂q5/∂t

∂q6/∂t

(f) Control actions using KFS-VS structure for large displacement problem.

Fig. 8. Comparative between KF-VS and KFS-VS both with the same parameters (Q = I , R = 10.0 · I and λ = 4.0)

3087

Page 6: [IEEE 2013 IEEE International Conference on Robotics and Automation (ICRA) - Karlsruhe, Germany (2013.05.6-2013.05.10)] 2013 IEEE International Conference on Robotics and Automation

TABLE IPURE ROTATION PROBLEM (OBJECT ROTATION 150O ).

Algorithm Time [ms] Convergence Time [s]

KF-VS 0.2971 20.295KFS-VS Np = 10 1.4735 18.943KFS-VS Np = 20 2.9906 18.317KFS-VS Np = 30 4.34 17.865

KFS-VS, the smooth part of the filter is considering the Zparameter during more prediction (T = t+Np) than KF-VS(that only predicts the T = t + 1), that in this problem isquite different from the desired Z∗, and this aspect producesdifferent trajectories filtered. An important aspect is that,although both algorithms have similar magnitudes of thecontrol actions in the beginning, KFS-VS has more smoothedcontrol actions than KF-VS with the same convergence time,due to the trajectory predicted (see Figures 8(d) and 8(f)).

Although the KFS-VS behavior can provide better be-haviors than KF-VS, its convergence time depends on theparameter Np (see Table I). The KFS-VS filtering time growslinearly with Np and this implies that the convergence timeof the whole algorithm (time between the start pose until thesystem reach the desired pose) is greater.

All the behaviors presented in this paper and the badbehavior of classical structure can be seen in video includedin [19]. Moreover, it is possible to see the solution adoptedfor KFS-VS for the problem of 180o rotation around theoptical axis, which cannot be solved for classic IBVS.

VI. CONCLUSIONS

In this paper we have proposed a new control structurefor image-based visual servoing (IBVS) problems. The newapproach proposes a set of modified reference features basedon the current feature detection as well as the final referencefeatures based on standard filters such as KF or a combina-tion of a filter and a smoother such as KFS smoother. Therest of the control structure is kept as in the classic IBVSapproach. Therefore the comparison and analysis with classicIBVS controller is straight forward.

Real experimentations prove that the new control structureimproves significantly inherent problems of classic IBVSapproaches. In our opinion, the key aspect is that the filteredreference features are always closer to the measured ones,which allows us to be closer of the current measure features,meeting the local stability of the IBVS approach. In additionthe new approach makes faster the convergence to finalfeatures. Moreover, the “predictive” behaviour of the KFS-VS improves global convergence properties or reachability,and therefore new solutions are found in typical cases whereclassic IBVS approaches tend to fail.

The new control structure has been validated with anindustrial robot arm forcing to overcome complicated situa-tions where the classic IBVS approach fails. In the paper, twoof those situation are described in Section V using the newapproach. It is possible to see the bad behavior of classicalstructure in video included in [19].

The proposed control structure can be extended too manyother approaches where filtering of references may introduceimproved performances. Indeed, we have also tested a similarapproach in kinematic control problems of non-holonomicwheeled mobile robots with satisfactory results.

Future work is oriented to extend our results from KFS-VS to other types such those based on Unscented transformor particle filters. In addition to this, we plan to extendthe proposed control structure to multi-rate systems whereseveral sampling rates are present.

ACKNOWLEDGMENTAuthors would like to acknowledge V. Franch and V.

Colomer for fixing the robot structure to perform the ex-perimentation.

REFERENCES

[1] F. Chaumette and S. Hutchinson, “Visual servo control, part ii:Advanced approaches,” IEEE Robotics and Automation Magazine,vol. 14, no. 1, pp. 109–118, March 2007.

[2] S. Hutchinson, G. Hager, and P. Corke, “A tutorial on visual servocontrol,” Robotics and Automation, IEEE Transactions on, vol. 12,no. 5, pp. 651 –670, oct 1996.

[3] F. Chaumette, “Potential problems of stability and convergence inimage-based and position-based visual servoing,” 1998.

[4] R. Mahony, P. Corke, and F. Chaumette, “Choice of image features fordepth-axis control in image based visual servo control,” in In IEEE/RSJInt. Conf. on Intellingent Robots and Systems, 2002, pp. 390–395.

[5] G. Chesi, “Visual servoing path planning via homogeneous forms andlmi optimizations,” Trans. Rob., Apr. 2009.

[6] M. Kazemi, K. Gupta, and M. Mehrandezh, “Global path planning forrobust visual servoing in complex environments,” in Proceedings ofthe 2009 IEEE international conference on Robotics and Automation,ser. ICRA’09, 2009, pp. 1726–1732.

[7] M. Sauvee, P. Poignet, E. Dombre, and E. Courtial, “Image BasedVisual Servoing through Nonlinear Model Predictive Control,” in IEEEInternational Conference on Decision and Control, 2006, p. 6.

[8] G. Allibert and E. Courtial, “What can prediction bring to image-basedvisual servoing?” in Proceedings of the 2009 IEEE/RSJ internationalconference on Intelligent robots and systems, Piscataway, USA, 2009.

[9] H.-G. Li, X.-D. Zhu, and P.-S. Wang, “Optimal control law of robotbased on delta operator in visual servoing,” in Machine Learning andCybernetics, 2004. Proceedings of International Conference on, 2004.

[10] Y. Liu and H. Wang, “An adaptive controller for image-based visualservoing of robot manipulators,” in Intelligent Control and Automation(WCICA), 2010 8th World Congress on, july 2010.

[11] H. E. Rauch, S. C. T., and F. Tung, “Maximum likelihood estimatesof linear dynamic systems,” Journal of the American Institute ofAeronautics and Astronautics, vol. 3, no. 8, pp. 1445–1450, 1965.

[12] D. F. DeMenthon and L. S. Davis, “Model-based object pose in 25lines of code,” International Journal of Computer Vision, 1995.

[13] Chaumette and S. Hutchinson, “Visual servo control, part i: Basicapproaches,” IEEE Robotics and Automation Magazine, vol. 13, 2006.

[14] P. I. Corke, Robotics, Vision & Control: Fundamental Algorithms inMatlab. Springer, 2011.

[15] F. Chaumette, “Potential problems of stability and convergence inimage-based and position-based visual servoing,” in The Confluenceof Vision and Control, D. Kriegman, G. . Hager, and A. Morse, Eds.LNCIS Series, No 237, Springer-Verlag, 1998, pp. 66–78.

[16] E. Malis and P. Rives, “Robustness of image-based visual servoingwith respect to depth distribution errors,” in in IEEE InternationalConference on Robotics and Automation, 2003, pp. 1056–1061.

[17] M. Abderrahim, A. Khamis, S. Garrido, and L. Moreno, “Accuracy andCalibration Issues of Industrial Manipulators.pdf,” Industrial Robotics:Programming, Simulation and Application, Dec. 2006.

[18] G. Allibert, E. Courtial, and F. Chaumette, “Visual servoing via Non-linear Predictive control,” in Visual Servoing via Advanced NumericalMethods, G. Chesi and K. Hashimoto, Eds. LNCIS 401, Springer-Verlag, 2010.

[19] “Image-based visual servoing with reference features filtering -videos.” [Online]. Available: https://vimeo.com/59223704

3088