Robust Visual Servoing of Robot Manipulators Based on Passivity

Servo applications (regulation and tracking) are an important class of tasks for robots. Robustness in a servo controller must be guaranteed when the robot manipulator operates in an uncertain environment to ensure the stability and performance in the presence of uncertain robot manipulator dynamics, objects to be manipulated by the robot, and obstacles to be avoid. Also, robots must have sensing capability to adapt to the new tasks without reprogramming. Servoing based on visual measurements, also referred to as visual servoing (Hutchinson et al. (1996)), provided an alternative solution to these applications. Commonly used schemes include position-based and image-based visual servoing, being the main difference how to use the visual measurements: if the visual measurements are used to infer the end-effector pose to implement a cartesian control, we get a position-based visual servoing (PBVS) (see, e.g., Fujita et al. (2007)); if the visual measurements are used directly to calculate the control torque to the manipulator, we get an image-based visual servoing (IBVS) (see, e.g., Espiau et al. (1992) andKelly (1996)). It is generally recognized that IBVS has the advantages of having less on-line computation burden, being more accurate, while the PBVS is more flexible for its implementation. Dynamic visual servoing was proposed by Weiss et al. (1987). A key point in this approach is to view the visual measurements as the output of a dynamic system. By adopting this point of view, Dickmanns & Graefe (1988) set up a dynamic model of curvature evolution of the road in a driving application. However, stability and robustness issues were not addressed . To address these important questions and to investigate further the applications of IBVS for general scenarios, Hashimoto et al. (1996) andMa et al. (1999) studied the image dynamics, i.e., how the image feature of an object moving in a 3-D space evolutes in the 2-D image plane. The visual system in a robotics application is linearized in Hashimoto et al. (1996) at the desired point yielding a linear-time-invariant (LTI) multi-input-multi-output (MIMO)model. Ma et al. (1999) proposed a curve dynamic model in vision guided navigation application and based on it designed a linearizing control law that controls the curvature dynamics in the image plane using only perspective projection thanks to a state-observer. By combining passivity of the visual feedback system and the manipulator dynamics, Fujita et al. (2007) addressed the PBVS to track a 3-D object in a camera-in-hand configuration. However, the resulting control law was more complicated than that obtained with the transposed jacobian approach 9


Introduction
Servo applications (regulation and tracking) are an important class of tasks for robots.Robustness in a servo controller must be guaranteed when the robot manipulator operates in an uncertain environment to ensure the stability and performance in the presence of uncertain robot manipulator dynamics, objects to be manipulated by the robot, and obstacles to be avoid.Also, robots must have sensing capability to adapt to the new tasks without reprogramming.Servoing based on visual measurements, also referred to as visual servoing (Hutchinson et al. (1996)), provided an alternative solution to these applications.Commonly used schemes include position-based and image-based visual servoing, being the main difference how to use the visual measurements: if the visual measurements are used to infer the end-effector pose to implement a cartesian control, we get a position-based visual servoing (PBVS) (see, e.g., Fujita et al. (2007)); if the visual measurements are used directly to calculate the control torque to the manipulator, we get an image-based visual servoing (IBVS) (see, e.g., Espiau et al. (1992) and Kelly (1996)).It is generally recognized that IBVS has the advantages of having less on-line computation burden, being more accurate, while the PBVS is more flexible for its implementation.Dynamic visual servoing was proposed by Weiss et al. (1987).A key point in this approach is to view the visual measurements as the output of a dynamic system.By adopting this point of view, Dickmanns & Graefe (1988) set up a dynamic model of curvature evolution of the road in a driving application.However, stability and robustness issues were not addressed .To address these important questions and to investigate further the applications of IBVS for general scenarios, Hashimoto et al. (1996) and Ma et al. (1999) studied the image dynamics, i.e., how the image feature of an object moving in a 3-D space evolutes in the 2-D image plane.The visual system in a robotics application is linearized in Hashimoto et al. (1996) at the desired point yielding a linear-time-invariant (LTI) multi-input-multi-output (MIMO) model.Ma et al. (1999) proposed a curve dynamic model in vision guided navigation application and based on it designed a linearizing control law that controls the curvature dynamics in the image plane using only perspective projection thanks to a state-observer.By combining passivity of the visual feedback system and the manipulator dynamics, Fujita et al. (2007) addressed the PBVS to track a 3-D object in a camera-in-hand configuration.However, the resulting control law was more complicated than that obtained with the transposed jacobian approach (Kelly et al. (2000)) and had to be restricted to consider a steady object in order to establish the stability of the control system.Motivated by these works, we consider in this chapter the servo problem in a robot manipulator based only on the visual measurements by following the dynamic visual servoing approach to design an IBVS with a fixed camera configuration.To model the whole visual servoing system, we "lift" the manipulator dynamics up to the image space, reconstructed based on the perspective projection of the robot space in the image plane, and model it with the lagrangian formalism by formulating the kinetic and potential energy in the image space.The resulting motion equation has the same structure as that obtained in the joint space using the lagrangian modeling (Spong et al. (2006)), and therefore inheriting the passivity property.Robust control schemes based on the passivity of the motion equation are then designed for visual servoing.The main features of this robust control law are (1) No image derivative is required as it uses only the image position for feedback, (2) no camera parameters and robot inertia parameters are needed for the implementation, making it robust to the parameter uncertainties in both the camera and robot manipulator, (3) no other measurements as from optical encoders are needed but the visual measurements from a single fixed camera.The rest of this chapter is organized as follows: Section 2 presents the robot image dynamics obtained by gathering together manipulator dynamics, manipulator kinematics and the camera model into a single dynamic system, and its experimental validation in a laboratory set.Section 3 gives the controller design and the main results.Experiments results are shown in Section 4 to illustrate the performance of the proposed IBVS.Concluding remarks and future works are given in Section 5.

Robot forward kinematics
The forward kinematics f : ℜ 3 →ℜ 3 gives the cartesian position X of a feature point in the robot coordinate frame in terms of the joint position q and the velocity kinematics J(q)=∂ f (q)/∂q ∈ℜ 3×3 relates the feature point velocity Ẋ with the joint velocity q Ẋ = J(q) q. (7)

Camera model
The pin-hole model of a CCD camera (Hutchinson et al. (1996)) is considered.In this model, a point in the robot space seen in the camera frame X =[X 1 X 2 X 3 ] (meter) is transformed into the image position x ∈ℜ 2 (pixel) in the image plane by perspective projection (Fig. 1) where R(θ) ∈ SO(3) is the rotation matrix generated by clockwise rotating the camera about its optical axis by θ radians, with λ the focus length, X 3 the depth (the distance from the lens to the image plane), α 1 and α 2 (pixels/m) the scale factors of length units in the image plane, X 0 the intersection point of the optical axis at the robot plane.

Robot image dynamics
By robot image dynamics we refer to the dynamics of the robot image evolving in the image space (defined below) when the robot moves in the robot space.
Let the robot plane (Y-Z) be the plane perpendicular to the camera optical axis and containing the origin of the robot coordinate frame.The robot manipulator moves in the robot space defined by the robot coordinate frame (X-Y-Z) (Fig. 1).The robot image dynamics we consider in this chapter is obtained by lumping together manipulator dynamics, forward kinematics and the camera transformation into a single dynamics (Fig. 2).Consider the kinetic energy K φ (φ, φ)=1 2 φT M φ (φ) φ and potential energy P φ (φ) in the image space, where, similar to modeling the manipulator in the joint space, φ is the joint image position (rad) 1 , M φ (φ) the inertia matrix seen in the image space.The motion equation as seen in the image space is obtained by modeling the visual servo system (manipulator and camera) with lagragian formalism: As in the joint-space model, this motion equation has the following properties: Property 1': The inertia matrix M φ (φ) is positive definite, ı.e., Property 2': The matrix Ṁφ (φ) − 2C φ (φ, φ) is skew-symmetric, ı.e., Property 3': The dynamics of ( 9) is linearly parameterizable: For this 3-DOF articulated robot manipulator, a parametrization with a φ , Y φ (φ, φ, φ) ∈ℜ 15 may be obtained (Spong et al. (2006)).
Property 4': The gravity torque satisfies ∂g φ (φ) ∂φ ≤ c g φ , for some c g φ > 0 and ∀φ ∈ℜ 3 .( 13) Remark 1: As a consequence of this paradigm, the parameters of the manipulator dynamics, forward kinematics and camera transformation are all lumped together into the parameters of the model ( 9).The passivity property followed from Property 2' will simplify significantly the control design based on visual measurements.
Remark 2: The aforementioned robot plane (Y-Z) and the image-plane (y-z) in Fig. 1 are uniquely defined once the camera position and orientation are set, and are parallel from perspective projection (8).In the experiments, X 0 will be defined as the anti-image of the principal point in the image plane determined using the method in Grammatikopoulos et al. (2004).Before moving to consider the control design, we validate our point of view through experiments.The validation was carried out by first identifying the robot image dynamics using an off-line least-square algorithm, and then by comparing the output of the robot image system in Fig. 2 with that of the model ( 9).The experiment platform is shown in Fig 3 .It consists of a three-link manipulator (made in the laboratory) moving in the (robot) space, and a fixed IEEE 1394 digital camera from Basler (model A601 FC).The camera has focus length λ = 0.9091 (cm), and scaling factors α 1 = α 2 = 4.6 (pixels/cm).The rotation angle of the camera about its optical axis was set to θ = 0 (rad).The image plane has a resolution of (horizontal x vertical) 320 × 240 pixels.
Before the experiments, the feature point corresponding to the robot base point (origin of the robot coordinate frame) and length of the link 1, link 2 and link 3 are determined in the image plane.Using this information, the joint image position (φ) is obtained by measuring two feature points at the second and the third joint (end-effector).
To attenuate the noise in the image measurements image thresholding was first applied to the images of the feature points, then the centroid of each feature point image was calculated, which was used as the image positions of the feature points to calculate the joint image position.
The applied voltage, which is proportional to the applied torque, is fed through a D/A converter (AI-1608AY card from CONTEC) to the power amplification unit.A PC Pentium D running at 3 GHz was used in the experiments.The sampling time was set to 50 (frames/sec).
The joint image position together with the applied voltage is shown in Fig. 4. First the robot image dynamics ( 9) is identified using an off-line least square algorithm and the parametrization (12).In order to avoid from using the image acceleration, a first order low-pass filter is applied to both sides of the parametrization ( 12) to get a filtered version of the parametrization (12): where are the filtered regressor and the filtered input torque, respectively, and β/(s + β), with β = 0.9 and s the Laplace transform variable, the filter transfer function.In order to further avoid from using the derivatives of the joint image position, φ is substituted by its approximate derivative φ = s 1+ǫ f s φ, with ǫ f = 0.001.900 samples were used.The estimated parameters are shown in Table 1.After the parameter vector of the robot image dynamics ( 12) is obtained, this dynamical model is validated by comparing the output of the robot image dynamics (Fig. 2) and the output of the identified robot image dynamics (9).The applied torques and the measured joint image positions and the outputs of the identified robot image dynamics are shown in Fig. 5.

Controller design for IBVS
In this section, motivated from joint space control in Escobar et al. (1999); Kelly (1999); Tomei (1991) we design an output feedback control law with bounded control action and desired gravity compensation to regulate the joint image position.The results are summarized in the following.Theorem 1: Consider the robot image dynamics (9) and the control law where is the desired gravity compensation term, φ = φ − φ d the image position error, φ d ∈ℜ 3 the desired joint image position,  continuous functions satisfying ) where c g φ is given in Property 4' and ẇ = −α(w + φ), α > 0, Then all the signals in the control loop are bounded and φ → 0 asymptotically.
Proof.Substituting the control law (15) into the robot image dynamics (9) gets the closed-loop equation First, we show that ( φ, φ, z) has a unique equilibrium at the origin, and then using the Lyapunov analysis to show this equilibrium is asymptotically stable.
In fact, at the equilibria we have On the other hand, it follows from Property 4' and (17) that The above two expressions imply that φ = φ d .
Rewriting the observer dynamics (18) as Therefore, ( φ, φ, z)=(0, 0, 0) is the unique equilibrium of the closed-loop system.Consider the following potential energy-like function for the closed-loop system Its time derivative along the error dynamics ( 19) is In order to conclude the asymptotic stability of the equilibrium, we invoke LaSalle theorem (Khalil (2002)) by considering the invariant set which contains only the equilibrium.Therefore the asymptotic stability of the equilibrium follows.
Remark 3: Since the control law is developed based on the robot image dynamics, no other measurements than the joint image positions are need for its implementation.This is important because taking time derivative of image measurements are in general not acceptable given noisy image measurements.Also, notice that the only parameters in the control law are the gravity term of the robot image dynamics, which may be also tuned on-line as in Tomei (1991).geometrical method used here.For practical purposes, the given desired image joint position φ d1 must be restricted away ±20 (deg) away from the camera optical axis.Remark 5: Typically, ] is taken a sigmoid function, e.g., 2k 1 π atan( x k 2 ) with k 1 , k 2 > 0 determining the magnitude and shape of the sigmoid function, to avoid the control signal from saturating (Escobar et al. (1999)).

Experiments results
In order to evaluate the robustness of the proposed control in the presence of quantization errors in camera lens distortion, possible misalignment of the camera rotation angle about its optical axis, laboratory experiments were carried out.The control law (15) with gravity compensation term g φ (φ d ) was applied to the robot the experiment platform in Fig. 3.The sigmoid function as in Remark 5 was used with k 1 = 7.65, 2.25, 1.75 and k 2 = 4.35, 1.98, 1.15 for the joint 1, 2 3, respectively, and α = 5 in (18).

151
Robust Visual Servoing of Robot Manipulators Based on Passivity www.intechopen.com

Fig. 1 .
Fig. 1.The robot plane (Y-Z) is the plane perpendicular to the camera optical axis and containing the origin of the robot coordinate frame.The image-plane (y-z) is its perspective projection.
The image-plane (y-z) is the perspective projection of the robot plane (Y-Z), and the image space (x-y-z) is the space reconstructed based on the perspective projection of the robot space in the image plane (y-z).Common methods used for this reconstruction include range identification(Chen & Kano (2002);Dixon et al. (2003);Karagiannis & Astolfi (2005)), utilization of multiple feature points attached to the end effector as well as to the target(Kelly et al. (2006);Yuan (1989)).In this work, we use the feature points attached to the links to reconstruct the joint angles of the robot image in the image space needed for the controller implementation.

Fig. 4 .
Fig. 4. Identification of the robot image dynamics: Left column shows the torque inputs, right column shows the joint image position for Link 1 (up), 2 (middle) and 3 (down).

Robust
Visual Servoing of Robot Manipulators Based on Passivity www.intechopen.com

Fig. 5 .
Fig. 5. Validation of the robot image dynamics: Left column shows the torque inputs, right column shows the joint image position measured from the robot image dynamics (Fig. 2) (solid line) and the joint image position from the model (9) (dashed line) for Link 1 (up), 2 (middle) and 3 (down).
20) 156 Recent Advances in Robust Control -Theory and Applications in Robotics and Electromechanics www.intechopen.com Remark 4: Due to the occlusion effect in a single fixed camera configuration, it is impossible to determine uniquely the joint image positions for certain poses of the manipulator by the 157 Robust Visual Servoing of Robot Manipulators Based on Passivity www.intechopen.com Fig. 6 depicts the regulation of the joint image position to its desired position, which corresponds to moving the end-effector from an initial position corresponding to φ(0)= [10 -100 -110] (deg) to a desired position φ d (0)=[ −30 − 30 − 30] (deg) at t = 0 (sec.) and φ d (5)=[ 30 40 40] (deg) at t = 5 (sec.).Although not established in theory here, the results of tracking a desired image trajectory, which corresponds to drawing a figure in the image space shown in Fig. 7, with the proposed control law are shown in Fig. 8 and Fig. 9.

Fig. 6 .
Fig. 6.Experiments of the visual servoing: regulation of the joint image position for joint 1 (up), 2 (middle) and 3 (down).Left column shows the torque inputs, right column shows the joint image position and the desired position.

Fig. 7 .
Fig. 7. Experiments of the visual servoing: tracking of the joint image trajectory in the image space.

159
Robust Visual Servoing of Robot Manipulators Based on Passivity www.intechopen.com

Table 1 .
154 Recent Advances in Robust Control -Theory and Applications in Robotics and Electromechanics www.intechopen.comEstimated parameters of the robot image dynamics (12).