Visual PID Control of a redundant Parallel Robot Rubén Garrido, Alberto Soria, Miguel Trujano Department of Automatic Control, CINVESTAV-IPN, Mexico D.F., Mexico Phone (5255) 5747-3744
Fax (5255) 5747-3982
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 damping using joint level measurements. The Lyapunov method and the LaSalle invariance principle allow assessing asymptotic closed loop stability. Experiments show the performance of the proposed approach. Keywords –– Parallel Robots, Visual Servoing, Robot Control, PID Control.
I. 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 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 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]. There exist two Visual Servoing schemes, namely, 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 mounts 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
E-mail:
[email protected]
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. Reference [7] proposes a class of global regulators having a linear PD feedback term plus a non-linear integral term depending on the position error. Reference [8] studies a class of non-linear global PID regulator for robot manipulator employing a nonlinear proportional term produced by a non-quadratic artificial potential. The control law proposed here has a similar structure as in [7] and [8]. However, in the present case the aim is to solve the regulation problem for redundant parallel planar robots 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 stiction. The Lyapunov method together with the LaSalle’s invariance theorem allows concluding asymptotic stability. 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.
II. Parallel Robot Dynamic Model with Redundant Actuators In this work, the symbols λm { A} y λM { A} stand for
the minimum and maximum eigenvalues of a matrix respectively of a positive definite matrix A . The norm of vector x is
A =
defined
as x =
T
x x and
the
term
λM { AT A} defines the induced norm of matrix A .
In accordance with [2], modelling of a planar paralleloveractuated 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. Supposing that the robot moves in the horizontal plane, the equivalent mechanism has the following set of equations º ªτ a º ªΘ ªΘ i º i Mi (1) + Ci «¬ ϕi »¼ «¬ ϕi »¼ + N = «¬τ p »¼ ; i = 1, 2, 3 Combining these equations gives the dynamic model of the mechanism (2) Mq + Cq + N = τ 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=ª ¬ qa
q p º¼ = [ Θ
τ = ª¬τ aT
τ Tp º¼ = [τ a1
T
T
T
1
T
Θ2
Θ3
ϕ1
τ a2
τ a3
τ p1
ϕ2
τ p2
τ p3 ]
T
τ 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 W τ = S τa
(3)
where the Jacobian matrices W and S relate end effector velocities with velocities of the active joints ª ∂qa º q =
∂q « ∂X » X =« »X ∂ q ∂X p « »
«¬ ∂X »¼ ª ∂qa º « ∂X » ∂q W =« »= ∂ q p « » ∂X «¬ ∂X »¼ S=
∂qa ∂X
M = W MW T T C = W MW + W CW
N =W N T
III. Model of the Vision system The camera optics maps the robot workspace to the camera screen coordinate system [9]. Hence, the position X of the end effector in the robot coordinated frame
X i = [ xi
(4)
yi ] is given by T
ª x º ªOx º½ ª C x º ª xi º X i = « » = η hR ( β ) ® « » − « » ¾ + « » (10) ¬ yi ¼ ¯ ¬ y ¼ ¬Oy ¼¿ ¬C y ¼
T
q p denotes the passive joint position vector. Vectors τ a and
T
a
T
ϕ3 ]
Variable qa is the active joint position vector and
T
Consequently, using (4) and (8) gives the dynamic model of the redundant parallel robot in terms of the end effector position X T MX + CX + N = S τ (9)
Vector [ Cx
C y ] is the centre of the image in the screen of T
the camera, η is the camera scaling factor given in pixels/m and is assumed negative and h is the magnification factor defined by f (11) h= <0 f −z Where the focal distance of the camera is f , z is the distance between the camera and the robot workspace. The intersection between the optical axis and the robot
[
workspace is Ox
Oy
]
T
. The term R ( β ) is the orientation
matrix of the camera defined as: ªcos( β ) − sin( β ) º R(β ) = « » ¬ sin( β ) cos( β ) ¼
(12)
This matrix represents a rotation angle of β radians in the clockwise direction around the camera optical axis. From (10), it is clear that the desired position of the robot
(5)
end effector X = ª¬ x *
*
*
T
y º¼ in image coordinates is given
by
ª xi* º ª x* º ªOx º ½ ª C x º X i = « * » = η hR ( β ) ® « * » − « » ¾ + « » (13) ¯¬ y ¼ ¬Oy ¼ ¿ ¬C y ¼ ¬ yi ¼ *
(6)
Substituting (2) into (3) gives T T W ( Mq + Cq + N ) = S τ a .
(7)
Taking the time derivate of (4) yields q = WX + WX
(8)
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. ª xi* º ª xi º * X i = X i − X i = « * » − « » (14) ¬ yi ¼ ¬ yi ¼
The direct kinematics gives the position of the end effector with respect to the robot coordinate frame in terms
The symbol ⋅ stands for the absolute value. Figure 1 shows
of joint position q ∈ \
F ( m, ε , x ) .
6
X = f (q)
(15)
the allowed region for functions belonging to the set
Using (10), (13) and (15), it is possible to define the image position error X i as
X i = η hR ( β ) [ f ( qd ) − f ( q ) ] .
(16)
In addition, its time derivate corresponds to
∂ {η h ⋅ R ( β ) [ f ( qd ) − f ( q ) ]} X i = q a ∂qa
(17)
= −η h ⋅ R ( β ) S q a †
Figure 1. Function F ( m, ε , x ) .
Using q a = SX , (17) allows rewriting (17) as
Two
important
dt = − X i = −η h ⋅ R ( β ) X
(18)
Property 1: The Euclidian norm of
where S denotes the More-Penrose pseudoinverse of S . The proposed approach assumes that the joint position q and velocity q are available from measurements Oy
]
T
f ( x) ≥
m x , if x < ε ® if x ≥ ε ¯ mε
f ( x) ≤
x , if x < ε ® ¯ nε , if x ≥ ε
and
and h are unknown. Moreover, the
term R ( β ) is equal to the identity matrix. The control problem is to find the actuator torques τ a such that the robot end effector reaches a desired image position within the 2 robot workspace so that lim X (t ) = 0 ∈ \ . t →∞
i
IV. PI-Visual-D task control law
, Property 2: satisfies
1.
F ( m, ε , x )
T
n
differentiable function f ( x ) = [ f ( x1 ) such that
x ≥ f ( x) ≥ m x ,
ε ≥ f ( x ) ≥ mε , 1≥
df ( x ) dx
≥ 0; ,
f ( x2 )
∀ x ∈ \ : x < ε; ∀x ∈ \ : x ≥ ε ;
"
T f ( xn ) ]
x∈\
for all
n
m x 2 , if x < ε ® ¯mε x , if x ≥ ε
,
, 1 ≥ m > 0, ε > 0
and x ∈ \ , represents the set of continuous increasing
T
f ( x) x
Function
f ( x) x ≥
The following definitions about saturate functions will be useful for showing stability of the proposed control law.
Definition
f ( x ) for all
x ∈ \ n satisfies:
†
[
to
F ( m, ε , x ) are
d X i = (η h ⋅ R ( β ) [ f (qd ) − f (q )])
and that O = Ox
of f ( x ) belonging
properties
T
Using the de Moore-Penrose pseudoinverse S allows proposing the following control law
IJa = (S
T
) {R ( β ) †
T
ªK ¬
P
+K X i I
³ f [ X (σ ) ]dσ º¼ − K t
i
0
D
} (19)
X
Matrices K P , K D y K I are diagonal positive corresponding to the proportional, derivate and integral gains respectively,
(S ) T
†
(
=S S S T
)
−1
) ∈ F ( m, ε , X ) . Substituting and f ( X i i
the control law (19) into the robot dynamic model (9) and using the fact that S
T
(S ) T
†
(
=S S S S T
following closed loop system equation
T
)
−1
= I yields the
+ CX + N = R(β ) MX
ª¬ K
T
³ f [ X (σ ) ]d σ º¼ − K t
+K X
P
i
I
i
0
D
X
(20)
Defining the new state:
³ f [ X (σ )]dσ − K t
Z(t ) =
i
0
−1 I
R(β )N
−
1 2 2
2η h
≥−
(21)
Allows rewriting the closed loop system (20) as
ª X i º
ª « X » = «M dt « » « ¬« Z ¼» ¬« d
−X i −1
{
R(β )
[K
T
º }»» (22) ¼»
Consider the following Lyapunov function candidate 1 ,X , Z) = ªX − V (X « 2¬ ηh 1
i
+ + + −
1
)º f (X »¼ i
[ Z + X ]
T
2η h
T
i
ª − 1 M X «¬ η h
η h 2
³
2
1 2η h
1
i
2η h
] K I [Z + X i
0
[
T
i
(23)
f (Ȧ ) K D dȦ
° 1 Q X ®2 °¯ Qε
f
2 2
2η h 2
( X )
T
i
<ε if X i
i
≥ε if X i
where
[
2
2
1
]
T K −K X − X i p I i
)≥ ⋅ Mf ( X i
]
T K −K X X i p I i
2η h 2
X
0
1
³
2η h
λM {M} f ( X i )
2
1
X i
2 2
Then:
)º f (X »¼
Q=
)T Mf ( X ) f (X i i
, and X The first tem is a non-negative function of X i while the second term is a non-negative function of and Z . The third term satisfies: X i
1 2 2 η h
1
− 1 λ {M} X 2 , if X < ε i i °° 2η 2h 2 M ≥® ≥ε ° − 1 λ {M}ε 2 , if X i °¯ η 2h 2 M
+ K Z] − K X − CX X P i I D ) f (X i
) T Mf ( X ) f (X i i
≠0 f (Ȧ) K D dȦ > 0, ∀ X i T
(24)
1 ª º λm {K P } − λM {K I } − λM {M}» ; « ηh ¬ ηh ¼ 1
as long as is a definite positive function with respect to X i the following inequality holds
λm {K P } > λM {K I } +
−
i
1 2η h
λM {K I } X i
λM {M}
(25)
,X , Z) = −X T [ K − MF ( X ) R( β )] X V ( X i D i
and the entries of f (Ȧ ) are increasing function. Hence, the . Using the term (24) is definite positive with respect to X
−
ηh
Therefore, the Lyapunov candidate function (23) is positive definite if inequality (25) holds. After some simplifications and using property 2, the time derivative of (23) along the closed loop system (22) trajectories is
because K D is a positive definite diagonal matrix, f ( 0) = 0
Rayleigh-Ritz inequality allows obtaining 1 T ≥ 1 λ {K } X Xi [K P − K I ] X m i P i 2η h 2η h
1
− 2
− 2
−
Using Property 4 and the Rayleigh-Ritz inequality yields
1
ηh 1
ηh 1
ηh 1
ηh
[
]
)T R ( β ) T K − K X f (X i P I i ) f (X i
T
[ R(β )
T
]
− I KI Z
(26)
)T K [ R ( β ) − I ] X f (X i D T CT f ( X ) X i
with ) = −F (X )X , f ( X i i i
(27)
) is a diagonal matrix with entries having In addition, F ( X i
the form 0 <
) ∂f ( X i ∂X i
≤1.
The fourth and fifth terms of (26) are equal to zero since R ( β ) = I . As a consequence, the time derivate of the Lyapunov candidate function (26) becomes ,X , Z ) = −X T [ K − MF ( X )] X V ( X i D i − −
1
ηh 1
ηh
)T [ K − K ] X f (X i P I i
(28)
T CT f ( X ) X i
1
T CT f ( X )≤ 1 κ ε 2 X X − i C1 ηh ηh
2
2 2 °¯ −γ X − δε X i , si X i ≥ ε
The fact that the Lyapunov candidate function (23) is a positive definite function and its time derivative a semidefinite negative function allows concluding that equilibrium point of the closed loop system (22) is stable. In ,X , Z) ≡ 0 . order to establish asymptotic stability let V ( X i Therefore, from (22) it follows that R ( β ) K I Z = 0 . Since T
R ( β ) K I of full rank then Z = 0 . The result follows applying the LaSalle’s invariance theorem. The following proposition resumes the stability analysis described above. T
2
On the other hand, using the Rayleigh-Ritz inequality ) produces and the definition of F ( X i T MF ( X )X ≤ λ {M F ( X )} X X i M i ≤ λM {M} X
2
Proposition 1: Parallel robot dynamic system (9) and control law (19) has a unique equilibrium point at and f ( X ) ∈ F ( m, ε , X ). [ X X Z ] = 0 ∈ \ T
T
T
2
i
i
λm {K I } > 0
λm {K D } > λM {M} + κ C 2 2
λm {K P } > λM {K I } +
Therefore, the time derivate of (28) satisfies:
,X , Z ) ≤ −γ X 2 − 1 f (X )T [ K − K ] X (29) V ( X i i P I i ηh where γ = λm {K D } − λM {M} − κ C 2 is a positive constant if
λm {K D } > λM {M} + κ C 2 .
(30)
The second term of equation (29) is bounded by 1
ηh
)T [ K − K ] X ≤− f (X i P I i
1
ηh
[λ {K } P
m
(31)
)T X − λ M {K I } f ( X i i
]
)T X > 0 . Making use of Property 2 in (31) because f ( X i i leads to −
1
ηh
Where δ =
[ λ {K } − λ {K }] f ( X ) m
1
ηh
P
M
I
≤
−δ X i ® ¯ −δε X i
2
i
T
X i
<ε if X i
(32)
≥ε if X i
[ λm {K P } − λM {K I }] m.
i
provided that
2
TK X ≤ − λ {K } X −X D D m
−
<ε si X i
= 0 and X = 0 . Thus X = 0 . This is true if and only if X i
Using properties 1 and 2 to obtain the norm of the third term yields
≤ κC2 X
−γ X 2 − δ X i 2 ,
,X , Z) ≤ ° V ( X ® i
K P satisfies
(25) then δ > 0 . Consequently, taking into account (31) and (32) in (29) allows obtaining the following upper bound
ηh
λM {M}
V. Experimental Results To show the feasibility of the proposed approach the control law (19) was implemented on an experimental test bed. 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 6 absolute encoders for all the joints. Three power amplifiers drive the DC motors. A digital camera from Dalsa, model CA-D1-128 having a 128x128 pixel resolution, measures the position of the robot end effector. This camera connects 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 RS-232 link. Reference [10] gives further details in this experimental setup. The active joint incremental encoders were sampled at 0.1 ms, absolute encoders were sampled at 15 ms and the end effector position given by the vision system was sampled at 3.7 ms. The gain matrixes values were set K = diag {0.004, 0.004} and to K = diag {0.176, 0.156} , I
If
1
D
K P = diag {0.22, 0.22} . The desired position X i is a filtered *
square
[16 pixels
function
having
amplitude
16 pixels ] and a frequency of [ 0.2 Hz T
of 0.4 Hz ] . T
Figure 2 shows the experimental control results. The upper
part in figure 2 corresponds to the x coordinate whereas the bottom part corresponds to the y coordinate. Figure 3 shows the position errors; note that when the reference changes the position error settles around 0.5 pixels by the integral action.
together with the LaSalle’s invariance principle. Experiments using an experimental platform verify 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. VII. References [1] S. Kock, W. Schumacher, “A Parallel x-y Manipulator with Actuation Redundancy for High-Speed and Active-Stiffness Applications”, Proceedings of the IEEE International Conference on Robotics and Automation, pp. 2295-2300, Leuven, Belgium, 1998. [2] H. Cheng, Y. K. Yiu and Z. X. Li, “Dynamics and Control of Redundantly Actuated Parallel Manipulator” IEEE/ASME Transactions on Mechatronics, Vol. 8, No. 4, pp. 483-491, December 2003.
Figure 2. Desired and actual position control.
[3] H. Cheng, G. F. Liu, Y. K. Yiu, Z. H. Xiong and Z. X. Li, “Advantages and Dynamics of Parallel Manipulators with Redundant Actuation”, Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Maui, Hawaii, USA, Oct. 29 – Nov. 03, 2001. [4] M. Jaagersand, O. Fuentes, R. Nelson, “Experimental Evaluation of Uncalibrated Visual Servoing for Precision Manipulator”, in Proc. IEEE Int. Conf. Robotics and Automation, pp. 2254-2260, 1991. [5] S. Hutchinson, G. D. Hager and P. I. Corke, “A Tutorial on Visual Servo Control” IEEE Transactions on Robotics and Automation, Vol. 12 No. 5, pp. 651-664, October 1996. [6] J. T. Wen and S. Murphy, “PID control for robot manipulators”, CIRSSE Document # 54, Rensselaer Polytechnic Institute, 1990. [7] R. Kelly, “Global positioning of robot manipulators via PD control plus a class of nonlinear integral actions”, IEEE Transactions on Automatic Control, Vol. 43, No. 7, July 1998. [8] V. Santibañez, R. Kelly, “A Class of Nonlinear PID Global Regulators for Robot Manipulators”, Proceedings of the IEEE International Conference on Robotics and Automation, pp. 3601-3606, Leuven, Belgium May 1998. [9] R. Kelly, “Robust Asymptotically Stable Visual Servoing of Planar Robots” IEEE Transactions on Robotics and Automation, Vol.12, No. 5, pp. 759-766, October 1996
Figure 3. Position errors.
VI. 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
[10] SORIA, A.; GARRIDO, R.; VÁSQUEZ, I. & VÁZQUEZ, R.“Architecture for Rapid Prototyping of Visual Controllers”. Robotics & Autonomous Systems. Vol. 54, No. 6, 2006. pp. 486-495.