Stable Visual PID Control of a planar parallel robot

Viewer
Transcript

Stable Visual PID Control of a planar parallel robot Ruben Garrido Moctezuma, Alberto Soria López & Miguel Trujano Cabrera Department of Automatic Control, CINVESTAV-IPN, Mexico D.F., Mexico Phone: (5255) 5747-3744 Fax: (5255) 5747-3982 E-mails: [email protected], [email protected], [email protected] ABSTRACT In this paper, we study an image-based PID control of a redundant planar parallel robot using a fixed camera configuration. The control objective is to move the robot end effector to the desired image reference position. The control law has a PD term plus an integral term with a nonlinear function of the position error. The proportional and integral actions use image loop information whereas the derivative action adds task space damping using joint level measurements. The Lyapunov method and the LaSalle invariance principle allow assessing asymptotic closed loop stability. Experiments show the feasibility of the proposed approach. Keywords. Parallel Robots, Visual Servoing, Robot Control, PID Control.

1. INTRODUCTION Parallel robots are generally composed of a fixed base connected to a common end effector by means of several mechanical chains. This type of structure results on a closed kinematics chain possessing high structural stiffness, high bandwidth motion capability, high load capacity, and high precision compared to open chain kinematics robots. Furthermore, position errors due to mechanical friction are averaged and are not accumulated as in open chain kinematics robots. It is interesting to point out that many control schemes employ the robot forward kinematics to determine position and orientation of the robot end effector; thus the control loop is closed via the direct kinematics [1], [2]. Therefore, errors in the forward kinematics solution will produce errors in determining the position and orientation of the end effector. In this work, we will consider the redundant parallel robot studied in [2] and [3]. In order to avoid the use of direct kinematics for closing the loop, a Visual Servoing approach allows measuring directly the end effector position. Hence, it would not be necessary to know the robot kinematics parameters exactly when computing the control law. Visual Servoing systems use visual information to close the control loop. One of the interesting features when using Visual Servoing is the fact that it allows in some cases to cope with mechanical uncertainties [4]. Two common Visual Servoing schemes are used: fixed camera and camera in hand. [5]. Robot systems using a fixed camera, the vision system takes images related to the robot workspace coordinate system. The goal is to move the robot towards a desired position or object. In the camera in hand configuration, the camera is mounted on the robot end effector and the vision system takes images of the robot workspace. The goal of this configuration is to move the robot so the projection of a static or moving object remains at a desired position in the image plane. Furthermore, another way to classify Visual Servoing is as position-based and image-based schemes. Position-based Visual Servoing uses the vision system to determine the object and/or robot position relative to the robot workspace coordinate frame consequently requiring camera calibration and a geometric model of the robot workspace. On the other hand, in the Image-based Visual Servoing the vision system determines the object and/or robot position relative to the camera coordinate frame thus not requiring camera calibration or a geometric model to move the robot to the desired position. The approach proposed in this work employs a fixed camera image-based Visual Servoing. A well known in Robot Manipulator control is the fact that a linear Proportional Integral Derivative (PID) controller is able to compensate for unknown constant disturbances and to provide local asymptotic stability [6] in the case of Robot Manipulator joint control. In [7] a class of global regulators having a linear PD feedback term plus a nonlinear integral term depending on the position error is studied. In [8] a class of non-linear global PID regulator for robot manipulator employing a nonlinear proportional term produced by a non-quadratic artificial potential is addressed.

The control law proposed here has a similar structure as in [7] and [8]. However, in this paper the aim is to solve the regulation problem for redundant parallel planar robot with revolute joint using a direct measurement of the robot end effector. A vision system measures the end effector position of the robot. The integral action goal is to compensate constant disturbance terms as friction. The Lyapunov method together with the LaSalle’s invariance theorem allows concluding asymptotic stability. In this work, the symbols Om ^A` y OM ^A` stand for the minimum and maximum eigenvalues of a matrix respectively of a positive definite matrix A . The norm of vector x is defined as x

xT x and the term A

OM ^AT A` defines the induced norm of matrix A .

The rest of the paper describes further details of the proposed control scheme. Section 2 presents the dynamic model of the redundant parallel robot. Section 3 shows the vision system model. Section 4 deals with the proposed control law and the closed loop stability study. Section 5 exhibits some experimental results to show the feasibility of the approach. The paper ends with some comments and future work.

2. PARALLEL ROBOT DYNAMIC MODEL WITH REDUNDANT ACTUATORS In accordance with [2], modelling of a planar parallel-overactuated robot with rotational joints is attained using an equivalent open chain mechanism. Thus, the robot is decomposed into three planar two-link arms having each a well-known dynamic model (Fig. 1).

Fig. 1. Planar parallel-overactuated robot and its equivalent open chain mechanism.

Supposing that the robot moves in the horizontal plane, the equivalent mechanism has the following set of equations: º º ª4 ª4 M i « i » Ci « i » N «¬ Mi ¼» ¬« Mi ¼»

ªW ai º « »; i ¬«W pi ¼»

(1)

1, 2, 3

Combining these equations gives the dynamic model of the mechanism: M q Cq N Ĳ

(2)

where M is the inertia matrix, C correspond to the Coriolis and centripetal forces matrix, N is a vector that comprises constant disturbances. The robot joint and torque variables are: q

ªqTa ¬

qTp º¼

Ĳ

ª ĲTa ¬

ĲTp º¼

T

T

ª¬41 42

43 M1 M2 M3 º¼

T

ªW a1 W a 2 W a 3 W p1 W p 2 W p 3 º ¬ ¼

T

Variable q a is the active joint position vector and q p denotes the passive joint position vector. Vectors Ĳ a and Ĳ p correspond to the active and passive joint torques respectively. It should be noted that if friction at the passive joints is ignored, then, Ĳ and Ĳ a are related by: T

W Ĳ

T

S Ĳa

(3)

where the Jacobian matrices W and S relate end effector velocities with velocities of the active joints:

q

wq X wX

ª wq a º « wX » « »X « wq p » ¬« wX ¼»

(4)

with ª wq a º « wX » wq » W « « wq p » wX «¬ wX »¼ wq a S wX

(5)

(6)

Substituting (2) into (3) gives: T

Cq N ) W ( Mq

T

(7)

S Ĳa

Taking the time derivate of (4) yields: WX WX q

(8)

Consequently, using (4) and (8) gives the dynamic model of the redundant parallel robot in terms of the end effector position X is: CX N ST Ĳ MX (9) a with M

T

W MW

WT CW C WT MW

N

T

W N

Where the dynamic model in (9) satisfies the follow properties : Property 1: The matrix M is symmetric and defined positive. 2C is skew symmetric. Property 2: The matrix M

Property 3: There exists a positive constant N C such that CȦ d N C1 Ȧ

2

Ȧ\

2

3. MODEL OF THE VISION SYSTEM The camera optics maps the robot workspace to the camera screen coordinate system [9], the Fig. 2 shows this map. Hence, the position X of the end effector in the robot coordinated frame X i

ª¬ xi

° ª x º ª Ox º ½° ª Cx º ªx º Xi « i » K hR ( E ) ® « » « » ¾ « » y ¬ i¼ ¯° ¬ y ¼ ¬O y ¼ ¿° ¬C y ¼

T

yi º¼ is given by:

(10)

T

Vector ª¬Cx C y º¼ is the centre of the image in the screen of the camera, K is the camera scaling factor given in pixels/m and is assumed negative and h is the magnification factor defined by: h

f 0 f z

(11)

where the focal distance of the camera is f , z is the distance between the camera and the robot workspace. The T

intersection between the optical axis and the robot workspace is ª¬Ox Oy º¼ . The term R ( E ) is the orientation matrix of the camera defined as: ªcos( E ) sin( E ) º R(E ) « » ¬ sin( E ) cos( E ) ¼

(12)

This matrix represents a rotation angle of E radians in the clockwise direction around the camera optical axis. From (10), it is clear that the desired position of the robot end effector X *

Xi

ª x* º « i» * ¬« yi ¼»

*

* ¬ª x

* ½ ° ª x º ª Ox º °

T

y* ¼º in image coordinates is given by: ªCx º

(13)

K hR ( E ) ® « » « » ¾ « » * O C ¯° «¬ y »¼ ¬ y ¼ ¿° ¬« y ¼»

We assume that desired position is strictly located within the robot workspace. The image position error X i is the distance between the end effector position and a constant desired position (Fig. 2): * X* X ª« xi º» ª xi º X i i i « » * ¬ yi ¼ ¬ yi ¼

(14)

The time derivate of the image position error is: X i

X i

(15)

K hR ( E ) X

(16)

Using the time derivate of (10), we can rewrite (15) as: X i

The proposed approach assumes that the joint position q and velocity q are available from measurements and that h , T

K and O ª¬Ox O y º¼ are unknown. The control problem is to find the actuator torques Ĳ a such that the robot end

effector reaches a desired image position within the robot workspace so that lim t of X i ( t )

Fig.2. Coordinate frames and position error definition.

0

2

\ .

4. PI-VISUAL-D TASK CONTROL LAW Definitions of the employed saturation function will be useful for showing stability of the proposed control law. Definition 1. F m,H ,x , 1tm !0, H !0 and x\ n , represents the set of continuous increasing differentiable functions f ( x)

> f ( x1 )

f ( x2 ) "

T

f ( xn )@ such that:

x

x t f ( x ) t m x , x\: x H ;

x

H t f ( x ) t mH , x\: x tH ;

x

1t

df ( x ) t 0; dx

The symbol stands for the absolute value. Fig. 3 shows the allowed region for functions belonging to the set F m,H ,x .

Fig. 3. Function F m,H ,x .

Two important properties of f ( x ) belonging to F m, H , x are: Property 4: The Euclidian norm of f ( x ) for all x\ n satisfies: °m x , if x H f ( x) t® °¯ mH if x tH

and ° x , if x H f (x) d® °¯ nH , if x tH

,

Property 5: Function f ( x )T x for all x\ n satisfies: ° m x 2 , if x H f ( x )T x t ® °¯mH x , if x tH

,

Using the de Moore-Penrose pseudoinverse ST allows proposing the following control law: Ĳa

ST

†

ªK R ( E )T X K t f R ( E )T X (V ) dV K X º i I ³0 i D ¼ ¬ P

(17)

Matrices K P , K D y K I are diagonal positive corresponding to the proportional, derivate and integral gains respectively, ST

†

S ST S

1

and f R ( E ) X i F m,H ,X i .

Substituting S S T

T †

the

S S S S T

T

1

control

law

(17)

into

the

robot

dynamic

model

(9)

and

using

the

fact

that

I yields the following closed loop system equation: T K ³ t f R ( E )T X (V ) d V K X K P R(E ) X i i I 0 D

CX N MX

(18)

Defining the new state: t

Z (t )

³0

(V ) d V K 1 N f R ( E )T X i I

(19)

Allows rewriting the closed loop system (18) as: MX

T K ZK X K P R(E ) X i I D CX

(20)

Defining y R ( E )T X i , we have the closed loop system rewritten as: ª º K hX « 1 » CX `» «M ^K P y K I Z K D X « » f y ¬« ¼»

ªy º d «» X dt « » «¬ Z »¼

(21)

Consider the following Lyapunov function candidate: ,Z V y ,X

1 2

T

T ª 1 º ª 1 º 1 1 « X K h f ( y ) » M « XK h f ( y ) » 2K h ª¬ Z y º¼ K I ª¬ Z y º¼ K 2 h 2 ¬ ¼ ¬ ¼

y

³0

f (Ȧ )T K D dȦ

T 1 y ª¬K P K I º¼ y 12 2 f ( y ) Mf ( y ) 2K h 2K h

(22)

The first tem is a non-negative function of y and X , while the second term is a non-negative function of y and Z . The third term satisfies: 1

K 2h2

y

³0

z0 f (Ȧ )T K D dȦ ! 0, X i

(23)

Because K D is a positive definite diagonal matrix, f (0) 0 and the entries of f (Ȧ ) are increasing function. Hence, the term (23) is definite positive with respect to y. Using the Rayleigh-Ritz inequality allows obtaining: 1 T 1 1 2 y > K P K I @y t Om ^K P ` y OM ^K I ` y 2K h 2K h 2K h

2

Using Property 4 and the Rayleigh-Ritz inequality yields: 1 O ^M` y 2 , if y H 2 2 M 2 ° 2K h T 1 1 2 2 f ( y ) Mf ( y )t 2 2 OM ^M` f ( y ) t ® 2K h 2K h ° 21 2 OM ^M`H 2 , if y tH ¯ K h

Then 1 ª 2 Om ^K P `OM ^K I ` 1 OM ^M`»º y ° Kh ¼ 1 y T ªK K º y 1 f ( y )T Mf ( y ) t ° 2K h «¬ ® ¬ P I¼ 2K h 2K 2 h 2 ° 1 ªO ^K `O ^K ` 1 O ^M`ºH 2 m P M I Kh M »¼ ¯° K h «¬

is a definite positive function with respect to y if Om ^K P `!OM ^K I ` 1 OM ^M` holds. Kh

if y H if y tH

After some simplifications and using property 2, the time derivative of (22) along the closed loop system (21) trajectories is: T 1 T T ,Z X TK X 1 T 1 V y , X D K h f ( y ) MX K h f ( y ) > K P K I @y K h X C f ( y )

(24)

wf ( y ) wf ( y ) f ( y ) y K h X wy wy

(25)

with

The entries of

wf j ( y ) wf ( y ) fulfils 0 wy d 1 and this matrix is diagonal. Let F ( y ) wy j

,Z V y , X

X

T

> K D F (y )M @ X K1h f (y )

T

wf ( y ) , then (24) is rewritten as: wy

T ª¬K P K I º¼ y 1 f ( y ) CX Kh

(26)

Using properties 1 and 2 to obtain the norm of the third term yields: T d 1 N H 2 X 1 f ( y ) CX Kh K h C1

2

d NC 2 X

2

) in the first term produces: On the other hand, using the Rayleigh-Ritz inequality and the definition of F ( X i TK X T X D X MF ( y ) X d Om ^K D ` X

2

OM ^M` X

2

The second term in (26) is bounded by: T T 1 f ( y ) ª¬K P K I º¼ y d 1 ª¬Om ^K P `OM ^K I `º¼ f ( y ) y Kh Kh

because K P and K I are diagonal positive definite matrices. The above inequalities yields: ,Z d ªO ^K `O ^M`N º X 2 1 ªO ^K `O ^K `º f ( y )T y V y , X M C2¼ M I ¼ ¬ m D Kh ¬ m P

(27)

Since f ( y )T y !0 then (27) is a negative semidefinite function if Om ^K D `!OM ^M`N C 2 and Om ^K P `!OM ^K I ` . The fact that the Lyapunov candidate function (22) is a positive definite function and its time derivative a semidefinite negative function allows concluding that equilibrium point of the closed loop system (21) is stable. In order to conclude Asymptotic stability, it should be noted that since f ( y ) f ( R ( E ) X i ) 0 if X i 0, then ,Z V y , X X

0 if X

0. Then MX

0 and X i

0. From the closed loop system MX

CX 0 , this implies that K P y K I Z K D X

full rank. Then the invariant set is :

^ X i ,X ,Z |X i

CX , if X K Py K I Z K DX

0 . Thus K I Z

0 follows that Z

0 then

0 if K I is

`

0, Z 0 . The following proposition resumes the stability 0, X

analysis described above applying the LaSalle’s invariance theorem. Proposition 1: Consider the Parallel robot dynamic system (9) together with control law (17) where )F ( m,H , X ). If f (X i i Om ^K I ` ! 0 Om ^K D `!OM ^M`N C 2 Om ^K P ` ! OM ^K I ` 1 OM ^M` Kh

then, the equilibrium ª¬ X iT

T X

ZT º¼

T

6

0 \ of (21) is asymptotically stable.

5. EXPERIMENTAL RESULTS To show the feasibility of the proposed approach the control law (17) was implemented on a laboratory prototype. The corresponding block diagram is show in Fig. 4. The platform employs two personal computers integrated under the Matlab/Simulink/Wincon software environment. The parallel robot has 3 incremental encoders for the active or actuated joints and 3 absolute encoders for the passive joints. Three power amplifiers drive the DC motors. A digital Dalsa camera model CA-D1-128 having a 128x128 pixel resolution, measures the position of the robot end effector. This camera is connected to a third personal computer through a special data acquisition board. This computer executes image acquisition and processing and sends the end effector position to the control computer trough an RS232 link. Reference [10] gives further details in this experimental setup. The active joint incremental encoders were sampled at 0.5 ms, absolute encoders were sampled at 15 ms and the end effector position given by the vision system was sampled at 4ms. The gain matrixes values were set to K I diag^0.176,0.156` , K D diag^0.004,0.004` and K P diag^0.22,0.22` . The desired position X*i is a filtered square *

function having amplitude of 16 pixels and a frequency of 0.2 Hz to the xi* component and 0.4 Hz to the yi

component. Fig. 5 shows the experimental control results without integral action and with integral action. The upper part in figure 5 corresponds to the x coordinate whereas the bottom part corresponds to the y coordinate. Fig. 6 shows the position errors; note that when the reference changes the position error settles around 0.5 pixels by the integral action.

Fig. 4. Block diagram of the law control PI-Visual-D- Task Space.

(a)

(b) *

Fig. 5. Positions X i and X i , (a) without integral action, (b) with integral action.

(a) (b) Fig. 6. Position errors Xi , (a) without integral action, (b) with integral action

6. CONCLUSIONS The use of a vision system to measure directly the position of the robot end effector precludes the use of the robot forward kinematics for closing the control loop. This approach avoids errors introduced by the uncertainties in the robot kinematics. An imaged-based PID control law with a saturation function in the integral term compensates for constant disturbances. Global asymptotic stability of the closed loop system is obtained using the Lyapunov method together with the LaSalle’s invariance principle. Experiments using a laboratory prototype of a redundant parallel robot show the feasibility of the proposed approach. Future work will focus upon a comparison of the visual approach against the use of calibration and direct kinematics. ACKNOWLEDGMENT Authors would like to thank Gerardo Castro, Jesús Meza and Roberto Lagunes for their support in setting up the experimental facilities.

REFERENCES [1]

[2] [3] [4] [5] [6] [7] [8] [9] [10]

Kock, S., Schumacher W., “A Parallel x-y Manipulator with Actuation Redundancy for High-Speed and ActiveStiffness Applications”, Proc. IEEE Int. Conf. on Robotics and Automation Leuven, Belgium, 2295-2300 (1998). Cheng, H., Yiu, Y. K. & Li, Z. X., “Dynamics and Control of Redundantly Actuated Parallel Manipulator”,IEEE/ASME Trans. on Mechatronics, 8(4),483-491 (December 2003). Cheng, H., Liu, G. F., Yiu, Y. K., Xiong, Z. H. and Li, Z. X., “Advantages and Dynamics of Parallel Manipulators with Redundant Actuation”, Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, (2001). Jaagersand, M., Fuentes, O., Nelson, R., “Experimental Evaluation of Uncalibrated Visual Servoing for Precision Manipulator”, in Proc. IEEE Int. Conf. Robotics and Automation, 2254-2260 (1991). Hutchinson, S., Hager, G. D. & Corke, P. I., “A Tutorial on Visual Servo Control” IEEE Trans. on Robotics and Automation, 12(5), 651-664 (1996). Wen, J. T. & Murphy, S., “PID control for robot manipulators”, CIRSSE Document # 54, Rensselaer Polytechnic Institute(1990). Kelly, R., “Global positioning of robot manipulators via PD control plus a class of nonlinear integral actions”, IEEE Trans. on Automatic Control,43(7), 934-936 (1998). Santibañez, V., Kelly, R., “A Class of Nonlinear PID Global Regulators for Robot Manipulators”, Proc. IEEE Int. Conf. on Robotics and Automation,3601-3606 (1998). Kelly, R., “Robust Asymptotically Stable Visual Servoing of Planar Robots” IEEE Trans. on Robotics and Automation, 12(5),759-766 (1996). Soria, A., Garrido, R., Vásquez, I. & Vázquez, R., “Architecture for Rapid Prototyping of Visual Controllers”. Robotics & Autonomous Systems. 54( 6),486-495 (2006).