- Email: [email protected]
SYMBOLIC SYNTHESIS OF CONTROL LAWS FOR UNDERWATER VEHICLE AND ROBOT MANIPULATOR
SYMBOLIC SYNTHESIS OF CONTROL LAWS FOR UNDERWATER VEHICLE AND ROBOT MANIPULATOR A.A. Dyda, E.V. Lubimov and A.K. Sheykhot Abstract: This paper deals with the problem of the automation of design procedures for nonlinear control systems. As an example the procedure has been realized for the well-known technique consisting of the exact linearization via nonlinear feedback. The developed software achieves the synthesis of control laws in symbolical form that is convenient for further analysis, modeling, and practical implementation of the system. Examples of the software application to underwater vehicle and robot manipulator are given and discussed. Copyright © 2007 IFAC Keywords: Nonlinear control, algorithms, software engineering, linearization, manipulators.
1. INTRODUCTION
N
ONLINEAR control techniques typically involve a certain degree of difficulties due to the calculations which are needed. In fact, when the dimensions of system state, control, and output vectors are large, the control designer faces huge difficulties associated with complicated computations. Indeed, these aspects reduce the possibility of comparing the various control methodologies available for a certain system. A software which automatically generates the various control laws and simulates the controlled system can help the designer to choose the most appropriate control action for a given system. Motivated by these reasons, this work presents a software which can be used for the automatic generation of a code implementing a certain control strategy. More precisely, we concentrate on the well– known technique of exact nonlinear feedback linearization (NFL). In fact, a number of modern methods used to solve the control synthesis problem for nonlinear systems exploit the idea of linearization. Among them, the exact linearization of the controlled dynamics via nonlinear feedback is one of the most popular techniques. This approach has been applied to various systems and has been extended also in the context of adaptive control, variable structure control, etc. (Isidori, 1995; Nijmeijer and Van der Schaft, 1990; Byrnes, et al., 1997; Kokotovic, et al., 1991). The main advantages of the NFL method consist of the
possibility to decouple the controlled subsystems and to provide desired dynamics. To overcome these aforementioned difficulties to carry out the desired control law, in this work we use the available software to derive procedures for symbolic synthesis and verification, on the basis of the NFL method taken as example. The paper is organized as follows. In Section II the necessary background on the NFL method is given. Then, we present the main features of the algorithms and the program of symbolic synthesis developed. In Section III the examples of control synthesis for underwater vehicle robot manipulator are presented. The results are derived as symbolical expressions describing the nonlinear control laws. Final comments conclude the paper.
2. THE NFL METHOD AND ITS COMPUTER SYMBOLICAL IMPLEMENTATION To begin with, following (Isidori, 1995; Nijmeijer and Van der Schaft, 1990) we briefly review the necessary concepts of feedback linearization. The mathematical model of dynamical control object is supposed to be described by the equations ⋅
x = f ( x) + g ( x)u , y = h( x )
(1)
where A. A. Dyda is with the Department of Automatic and Information Systems, Maritime State University, Vladivostok, 690059 Russia and partially with the Institute for Automation and Control Processes, RAS, Vladivostok (phone:+7-(4232)-28-41-67; e-mail: [email protected]). E. V. Lubimov is with the Department of Automatic and Information Systems, Maritime State University, Vladivostok, Russia. (e-mail: [email protected]). A.K. Sheykhot is with the Department of Information Systems, Far-Eastern State Technical University, Vladivostok, Russia.
⋅
x = ( x1 ,...xn )T , ⋅
u = (u1 ,...um )T , ⋅
y = ( y1 ,... ym )T are the state, control, and output vectors, respectively.
The control law −1
u = A ( x )( w − Г ( x )) (2) ensures the feedback decoupling and linearization. The decoupling matrix A ( x ) and the vector Г ( x ) are calculated by formulas ⎡ Lg1 Lr1f −1h1 ( x) ⎢ Lg1 Lrf2 −1h2 ( x) ⎢ A( x) = ⎢ .. ⎢ rm −1 ⎣⎢ Lg1 L f hm ( x)
.. .. .. ..
.. Lgm Lr1f −1h1 ( x) ⎤ ⎥ .. Lgm Lrf2 −1h2 ( x) ⎥ , ⎥ .. .. ⎥ .. Lgm Lrfm −1hm ( x)⎦⎥ (3)
⎡ Lrf1 h1 ( x) ⎤ ⎢ r2 ⎥ L f h2 ( x) ⎥ ⎢ Г ( x) = ⎢ ... ⎥ ⎢ rm ⎥ ⎢⎣ L f hm ( x)⎥⎦ ⋅
where r = (r1 ,...rm )
T
(4)
inputs and outputs, the computational complexity soon increases. Hence, symbolic expressions of control laws are convenient for further analysis, modelling, and practical realization. The complicated work for deriving the symbolic control law based on the NFL method can be essentially facilitated by means of modern software systems such as Maple, MATEMATICA, MATLAB, MATHCAD etc. They have built–in functions for symbolic differentiation, vector and matrix operations, and so on. In particular we used the environment of Maple and MATLAB to derive the NFL–based symbolic synthesis of nonlinear control. The main modules of the software are following: – the description of variables (state, control and output vectors); – the description of the desirable system trajectory yd (t ) ; – –
g (x) ;
is the relative degree vector,
and w = ( w1 ,...wm ) is the new control input, where T
the Lie derivative of a function h j (x) along the vector
– – –
the determination of the relative degree vector; symbolic calculations of the decoupling matrix A ( x ) and the vector Г ( x ) ; calculations of the symbolic expressions for the
–
control u and the new control w ; closed–loop system model and simulation.
⋅
field f (x ) is defined as follows
L f h j ( x) =
n
∑
i =1
∂h j ( x ) ∂xi
fi (x) .
(5)
The feedback control (2) transforms a nonlinear system (1) into m decoupled chains of ri cascade– connected integrators and, as consequence, the input– output behavior of the system satisfies the linear equations
yi
( ri )
= wi .
(6)
Obviously, choosing as new controls
wi = ydi
( ri )
+ k1ei
( ri −1)
the input of dynamical object model in symbolic form; the derivation of expressions for f (x ) and
+ ... + k ri ei ,
(7)
where yd (t ) is the desirable trajectory vector of the system, with e(t ) = y d (t ) − y (t ) the error, provides asymptotically stable tracking. If the output is such that r1 + ... + rm = n , the system is full–state linearizable. Otherwise, it has an unobservable subsystem, whose dynamics constrained to the zero output is the so–called zero dynamics (for more details see (Isidori, 1995; Nijmeijer and Van der Schaft, 1990)). For systems of relatively low dimension, the use of the previous formulas is rather simple. But with the increase of the system dimension, of the number of
3. EXAMPLE OF SYMBOLICAL CONTROL SYNTHESIS To demonstrate the effectiveness of the approach and of the developed software, let us consider the following examples, for whom the symbolic control synthesis and simulation have been performed. The Robot Manipulator The first example of dynamical object to which we will apply the developed software is a robot manipulator UMS–2 (Vucobratovic and Kircansky, 1982). Its dynamics are described by the equation
D(q)q + B(q , q)q + G (q) = U where U ∈ R 3 , q ∈ R 6 are the control and the generalized coordinates vectors. The matrices are determined as follow
⎡I1 + I2 + I3 + m3(q3 + l3)2 0 0⎤ ⎢ ⎥ D(q) = ⎢ 0 m2 + m3 0 ⎥ , ⎢ 0 0 m3 ⎥⎦ ⎣
3
⎡ 2m3 (q3 + l3 )q3 0 m3 (q3 + l3 )q1 ⎤ ⎥, B(q, q) = ⎢ 0 0 0 ⎥ ⎢ ⎥⎦ ⎢⎣− m3 (q3 + l3 )q1 0 0
(m2 + m3 ) g
2 m ,rad,m /sec ,rad/s ec
G ( q ) = [0
x1 x2 x3 x4 x5 x6
0] . T
1
0
-1
The order of the system is n = 6, the number of inputs is m = 3. The variables are redefined as
-2
x1 = q1 , x2 = q1 , x3 = q2 , x4 = q2 , x5 = q3 , x6 = q3 :
0
1
2
3
4
5 sec
6
7
8
9
10
Fig. 1. The evolution of the state vector components. The parameters (see (Dyda and Di Gennaro, 1994)) for their definition) are supposed to be constant. The outputs of the system are the coordinates of the manipulator grasp in the task space
U1 U2 U3
150
100 n/m
h1 = ( x 5 + l 3 ) sin( x1 ),
200
50
h 2 = ( x 5 + l 3 ) cos( x1 ), h3 = x 3 .
0
-50
The desired trajectory yd (t ) is chosen to provide the
0
1
2
3
4
5 sec
6
7
8
9
10
Fig. 2. The control vector components.
movement of the grasp along the spatial circle 1.5
y d 1 = R sin( α t ), y d 2 = c1 ,
0.5 m
y d 3 = c 2 − R cos( α t )
w1 w2 w3
1
0
The program generates the following symbolic expressions for the control
-0.5
-1
1
2
3
4 sec
5
6
7
Fig. 3. The new control vector components. 3 2
Refrence Output Error
1 m
⎡ w1 m3 x52 cos x1 + ( 2 m3 x5 l3 + m3 l32 )( w1 cos x1 − w2 sin x1 ) ⎤ +⎥ ⎢ x5 + l3 ⎥ ⎢ ⎥ ⎢ ( w1 cos x1 − 2 x 2 x 6 − w2 sin x1 ) I ⎥ ⎢+ x5 + l3 ⎥ ⎢ U =⎢ ( m 2 + m3 )( w3 + G ) ⎥ ⎥ ⎢ m3 ( x 22 + w2 x5 cos x1 + w2 l3 cos x1 + 2 x 22 x5 l3 ⎥ ⎢ + x5 + l3 ⎥ ⎢ ⎥ ⎢ 2 2 2 2 x l + x 2 x5 + x5 w1 sin x1 + l3 w1 sin x1 ) ⎥ ⎢ + 2 3 ⎥⎦ ⎢⎣ x5 + l3
0 -1 -2 -3
0 1 2 3 4 5 6 7 8 9 10 ⎡ (R sinαt − ( x5 + l 3 ) sin x1 )k11 + (Rα cosαt − x6 sin x1 − ⎤ sec ⎢ ⎥ 2 Fig. 4. First output, reference, and error. ⎢ − ( x5 + l 3 ) x2 cos x1 )k12 − Rα sinαt ⎥ w = ⎢(c1 − ( x5 + l3 ) cos x1 )k 21 + ((x5 + l 3 ) x2 sin x1 − x6 cos x1 )k 22 ⎥ ⎢ (c − R cosαt − x )k + (Rα sinαt − x )k + Rα 2 cosαt ⎥ Due to the space available, only the first output, the 3 31 4 32 ⎢ 2 ⎥ reference and the error are given (Fig.4). Clearly, the ⎣⎢ ⎦⎥ simulations confirm that the derived control forces the Figs. 1 – 4 show a simulation of the whole control tracking errors to tend asymptotically to zero. system.
The Underwater Vehicle To finish the illustration of the developed software for symbolic synthesis of control system, consider the model of underwater vehicle (UV) (Dyda and Di
Gennaro, 1994). It consists of 12 differential equations that describe the UV kinematics and dynamics:
5 Refrence Output Error
4
(ω y cosθ sin ζ + ω z sin ψ sin θ ) dθ = ωx − dt cosψ dϕ (ω y cosθ + ω z sin θ ) = dt cosψ dψ = ω z cosθ + ω y sin θ dt
3 2 1 0 -1
dz = −Vx cosψ sinϕ + Vy cosϕ sinθ + Vy cosθ sinϕ sinψ + dt + Vz cosϕ cosθ − Vz cosϕ sinψ sinθ
(M + λ11)Vx − Mycωz + (M + λ33)Vzωy − (M + λ22 )Vyωz + + Mycωxωy + Pl sinψ = Fx
(M + λ22 )Vy + (M + λ11)Vxωz − (M + λ33 )Vzωx − 2
− Myc (ωz + ωx ) + Pl cosψ cosθ = Fy
− ( M + λ11 )Vxω z + + Mycω yω z − Pl cosψ sin θ = Fz (J x + λ44 )ω x + ((J z + λ66 ) − (J y + λ55))ωzωy + + Myc g cosψ sinθ = Mx ( J y + λ55 )ω y + ((J x + λ44 ) − ( J z + λ66 ))ω xω z +
+ (λ11 − λ33 )VxVz = M y ( J z + λ 66 )ω z + (( J y + λ55 ) − ( J x + λ 44 ))ω x ω y − − My V + (λ − λ )V V + My g sin ψ − 11
x
y
θ , ϕ ,ψ
( x , y , z )T
is
the coordinates vector, (ω x , ω y , ω z ) T , (Vx ,Vy ,Vz )T are angular
12
14
16
18
20
u 2 = Fy =
x8*x7*l11-
x11*x10*l33+x8*x7*Mm-x11*x10*Mmyc*x7^2*Mm-yc*x10^2*Mmcos(x3)*sin(x2)*w3*l22cos(x3)*sin(x2)*w3*Mm+ cos(x3)*cos(x2)*w1*l22+ cos(x3)*cos(x2)*w1*Mm+ sin(x3)*w2*Mm+sin(x3)*w2*l22Mm*x11*x12+x9*x7*l22+ Mm*x9*x7x11*x12*l22+PL*cos(x3)*cos(x1),
As seen from simulation results, asymptotically stable tracking of reference trajectory of UV is achieved with derived control law.
c
are Euler’s angles;
10 sec
y d 3 = 4 + 0 . 3 cos( 0 . 4 t ) − 0 . 2 t
− My c (V z ω y − V y ω z ) = M z where
8
where Xi are the components of state space vector (linear and angular coordinates and velocities) of underwater vehicle. It was demonstrated that even very long expressions for control derived can be optimized to the form acceptable for real time implementation. The simulation of the control system had confirmed correctness of control laws synthesis. As illustration, on Fig. 8 the third output of the system is shown with desirable trajectory
+ MycVz + (λ33 − λ22 )VzVy − Myc (Vxωy −Vyωx ) +
22
6
w2 =(R2*sin(omg2*t)x(5))*ki21+(R2*cos(omg2*t)*omg2sin(x(3))*x(8)cos(x(3))*cos(x(1))*x(9)+ cos(x(3))*sin(x(1))*x(11))*ki22R2*sin(omg2*t)*omg2^2.
( M + λ33 )Vz + ( M + λ22 )Vyω x + Mycω x −
x
4
The control law generated by the program is very large. Examples of shortest control elements for (2) and (7) are the following:
dy = Vx cosψ + V y cosψ cosθ − Vz cosψ sinθ dt
c
2
Fig. 8. Third output, reference, and error.
dx = Vx cosϕ cosψ − Vy cosϕ cosθ sinψ + Vy sinϕ sinθ + dt + Vz cosϕ sinψ sinθ + Vz cosθ sinϕ
2
0
and
linear velocities vectors; Fx , Fy , Fz , Mx , M y , Mz are components of the control
vector (forces and torques); other parameters are constant.
4. FINAL COMMENTS The software developed in this work can effectively support the procedure of control system design in symbolic form. Clearly, the bigger the dimension of the object mode is, the longer and more complicated symbolic expressions for control will be obtained. For
example, one can expect that the program will generate very large expressions describing the control laws for different flexible structures (such as given in (Gennaro and Dyda, 1993)) whose dynamics are approximated by a finite multi–dimensional model. The software helps to designer to solve the problem of the synthesis of nonlinear control systems and to simulate them easily. The NFL has been taken as an example to show the effectiveness of the symbolic computations. Sure, other methodologies can be considered for computer symbolic synthesis, such as adaptive and robust control.
REFERENCES A. Isidori (1995) Nonlinear Control Systems. Berlin: Springer-Verlag. H. Nijmeijer, A. Van der Schaft (1990) Nonlinear Adaptive Feedback Linearization of Systems. New York: Springer-Verlag. C.I. Byrnes, F.D. Priscoli, A. Isidori (1997). Output Regulation of Uncertain Nonlinear Systems. Boston: Birkhauser. P.V. Kokotovic, I. Kanellakopoulos, A.S. Morse (1991) Adaptive Feedback Linearization of Nonlinear Systems, P.V. Kokotovic (ed.), Foundation of Adaptive Control. Berlin: SpringerVerlag. M. Vucobratovic, N. Kircansky (1982). Scietific Fundamentals of Robotics Vol.1-3 . Berlin: Springer-Verlag. A. A. Dyda, S. Di Gennaro (1994). Adaptive trajectory control for underwater robot. Proc.OCEANS-94 Osates, Brest. S. Di Gennaro, A. A. Dyda (1993). Attitude control of a satellite with damping compensation of the flexible beam. Proc. European Control Conf., Groningem, pp. 1656-1661.
1. INTRODUCTION
N
ONLINEAR control techniques typically involve a certain degree of difficulties due to the calculations which are needed. In fact, when the dimensions of system state, control, and output vectors are large, the control designer faces huge difficulties associated with complicated computations. Indeed, these aspects reduce the possibility of comparing the various control methodologies available for a certain system. A software which automatically generates the various control laws and simulates the controlled system can help the designer to choose the most appropriate control action for a given system. Motivated by these reasons, this work presents a software which can be used for the automatic generation of a code implementing a certain control strategy. More precisely, we concentrate on the well– known technique of exact nonlinear feedback linearization (NFL). In fact, a number of modern methods used to solve the control synthesis problem for nonlinear systems exploit the idea of linearization. Among them, the exact linearization of the controlled dynamics via nonlinear feedback is one of the most popular techniques. This approach has been applied to various systems and has been extended also in the context of adaptive control, variable structure control, etc. (Isidori, 1995; Nijmeijer and Van der Schaft, 1990; Byrnes, et al., 1997; Kokotovic, et al., 1991). The main advantages of the NFL method consist of the
possibility to decouple the controlled subsystems and to provide desired dynamics. To overcome these aforementioned difficulties to carry out the desired control law, in this work we use the available software to derive procedures for symbolic synthesis and verification, on the basis of the NFL method taken as example. The paper is organized as follows. In Section II the necessary background on the NFL method is given. Then, we present the main features of the algorithms and the program of symbolic synthesis developed. In Section III the examples of control synthesis for underwater vehicle robot manipulator are presented. The results are derived as symbolical expressions describing the nonlinear control laws. Final comments conclude the paper.
2. THE NFL METHOD AND ITS COMPUTER SYMBOLICAL IMPLEMENTATION To begin with, following (Isidori, 1995; Nijmeijer and Van der Schaft, 1990) we briefly review the necessary concepts of feedback linearization. The mathematical model of dynamical control object is supposed to be described by the equations ⋅
x = f ( x) + g ( x)u , y = h( x )
(1)
where A. A. Dyda is with the Department of Automatic and Information Systems, Maritime State University, Vladivostok, 690059 Russia and partially with the Institute for Automation and Control Processes, RAS, Vladivostok (phone:+7-(4232)-28-41-67; e-mail: [email protected]). E. V. Lubimov is with the Department of Automatic and Information Systems, Maritime State University, Vladivostok, Russia. (e-mail: [email protected]). A.K. Sheykhot is with the Department of Information Systems, Far-Eastern State Technical University, Vladivostok, Russia.
⋅
x = ( x1 ,...xn )T , ⋅
u = (u1 ,...um )T , ⋅
y = ( y1 ,... ym )T are the state, control, and output vectors, respectively.
The control law −1
u = A ( x )( w − Г ( x )) (2) ensures the feedback decoupling and linearization. The decoupling matrix A ( x ) and the vector Г ( x ) are calculated by formulas ⎡ Lg1 Lr1f −1h1 ( x) ⎢ Lg1 Lrf2 −1h2 ( x) ⎢ A( x) = ⎢ .. ⎢ rm −1 ⎣⎢ Lg1 L f hm ( x)
.. .. .. ..
.. Lgm Lr1f −1h1 ( x) ⎤ ⎥ .. Lgm Lrf2 −1h2 ( x) ⎥ , ⎥ .. .. ⎥ .. Lgm Lrfm −1hm ( x)⎦⎥ (3)
⎡ Lrf1 h1 ( x) ⎤ ⎢ r2 ⎥ L f h2 ( x) ⎥ ⎢ Г ( x) = ⎢ ... ⎥ ⎢ rm ⎥ ⎢⎣ L f hm ( x)⎥⎦ ⋅
where r = (r1 ,...rm )
T
(4)
inputs and outputs, the computational complexity soon increases. Hence, symbolic expressions of control laws are convenient for further analysis, modelling, and practical realization. The complicated work for deriving the symbolic control law based on the NFL method can be essentially facilitated by means of modern software systems such as Maple, MATEMATICA, MATLAB, MATHCAD etc. They have built–in functions for symbolic differentiation, vector and matrix operations, and so on. In particular we used the environment of Maple and MATLAB to derive the NFL–based symbolic synthesis of nonlinear control. The main modules of the software are following: – the description of variables (state, control and output vectors); – the description of the desirable system trajectory yd (t ) ; – –
g (x) ;
is the relative degree vector,
and w = ( w1 ,...wm ) is the new control input, where T
the Lie derivative of a function h j (x) along the vector
– – –
the determination of the relative degree vector; symbolic calculations of the decoupling matrix A ( x ) and the vector Г ( x ) ; calculations of the symbolic expressions for the
–
control u and the new control w ; closed–loop system model and simulation.
⋅
field f (x ) is defined as follows
L f h j ( x) =
n
∑
i =1
∂h j ( x ) ∂xi
fi (x) .
(5)
The feedback control (2) transforms a nonlinear system (1) into m decoupled chains of ri cascade– connected integrators and, as consequence, the input– output behavior of the system satisfies the linear equations
yi
( ri )
= wi .
(6)
Obviously, choosing as new controls
wi = ydi
( ri )
+ k1ei
( ri −1)
the input of dynamical object model in symbolic form; the derivation of expressions for f (x ) and
+ ... + k ri ei ,
(7)
where yd (t ) is the desirable trajectory vector of the system, with e(t ) = y d (t ) − y (t ) the error, provides asymptotically stable tracking. If the output is such that r1 + ... + rm = n , the system is full–state linearizable. Otherwise, it has an unobservable subsystem, whose dynamics constrained to the zero output is the so–called zero dynamics (for more details see (Isidori, 1995; Nijmeijer and Van der Schaft, 1990)). For systems of relatively low dimension, the use of the previous formulas is rather simple. But with the increase of the system dimension, of the number of
3. EXAMPLE OF SYMBOLICAL CONTROL SYNTHESIS To demonstrate the effectiveness of the approach and of the developed software, let us consider the following examples, for whom the symbolic control synthesis and simulation have been performed. The Robot Manipulator The first example of dynamical object to which we will apply the developed software is a robot manipulator UMS–2 (Vucobratovic and Kircansky, 1982). Its dynamics are described by the equation
D(q)q + B(q , q)q + G (q) = U where U ∈ R 3 , q ∈ R 6 are the control and the generalized coordinates vectors. The matrices are determined as follow
⎡I1 + I2 + I3 + m3(q3 + l3)2 0 0⎤ ⎢ ⎥ D(q) = ⎢ 0 m2 + m3 0 ⎥ , ⎢ 0 0 m3 ⎥⎦ ⎣
3
⎡ 2m3 (q3 + l3 )q3 0 m3 (q3 + l3 )q1 ⎤ ⎥, B(q, q) = ⎢ 0 0 0 ⎥ ⎢ ⎥⎦ ⎢⎣− m3 (q3 + l3 )q1 0 0
(m2 + m3 ) g
2 m ,rad,m /sec ,rad/s ec
G ( q ) = [0
x1 x2 x3 x4 x5 x6
0] . T
1
0
-1
The order of the system is n = 6, the number of inputs is m = 3. The variables are redefined as
-2
x1 = q1 , x2 = q1 , x3 = q2 , x4 = q2 , x5 = q3 , x6 = q3 :
0
1
2
3
4
5 sec
6
7
8
9
10
Fig. 1. The evolution of the state vector components. The parameters (see (Dyda and Di Gennaro, 1994)) for their definition) are supposed to be constant. The outputs of the system are the coordinates of the manipulator grasp in the task space
U1 U2 U3
150
100 n/m
h1 = ( x 5 + l 3 ) sin( x1 ),
200
50
h 2 = ( x 5 + l 3 ) cos( x1 ), h3 = x 3 .
0
-50
The desired trajectory yd (t ) is chosen to provide the
0
1
2
3
4
5 sec
6
7
8
9
10
Fig. 2. The control vector components.
movement of the grasp along the spatial circle 1.5
y d 1 = R sin( α t ), y d 2 = c1 ,
0.5 m
y d 3 = c 2 − R cos( α t )
w1 w2 w3
1
0
The program generates the following symbolic expressions for the control
-0.5
-1
1
2
3
4 sec
5
6
7
Fig. 3. The new control vector components. 3 2
Refrence Output Error
1 m
⎡ w1 m3 x52 cos x1 + ( 2 m3 x5 l3 + m3 l32 )( w1 cos x1 − w2 sin x1 ) ⎤ +⎥ ⎢ x5 + l3 ⎥ ⎢ ⎥ ⎢ ( w1 cos x1 − 2 x 2 x 6 − w2 sin x1 ) I ⎥ ⎢+ x5 + l3 ⎥ ⎢ U =⎢ ( m 2 + m3 )( w3 + G ) ⎥ ⎥ ⎢ m3 ( x 22 + w2 x5 cos x1 + w2 l3 cos x1 + 2 x 22 x5 l3 ⎥ ⎢ + x5 + l3 ⎥ ⎢ ⎥ ⎢ 2 2 2 2 x l + x 2 x5 + x5 w1 sin x1 + l3 w1 sin x1 ) ⎥ ⎢ + 2 3 ⎥⎦ ⎢⎣ x5 + l3
0 -1 -2 -3
0 1 2 3 4 5 6 7 8 9 10 ⎡ (R sinαt − ( x5 + l 3 ) sin x1 )k11 + (Rα cosαt − x6 sin x1 − ⎤ sec ⎢ ⎥ 2 Fig. 4. First output, reference, and error. ⎢ − ( x5 + l 3 ) x2 cos x1 )k12 − Rα sinαt ⎥ w = ⎢(c1 − ( x5 + l3 ) cos x1 )k 21 + ((x5 + l 3 ) x2 sin x1 − x6 cos x1 )k 22 ⎥ ⎢ (c − R cosαt − x )k + (Rα sinαt − x )k + Rα 2 cosαt ⎥ Due to the space available, only the first output, the 3 31 4 32 ⎢ 2 ⎥ reference and the error are given (Fig.4). Clearly, the ⎣⎢ ⎦⎥ simulations confirm that the derived control forces the Figs. 1 – 4 show a simulation of the whole control tracking errors to tend asymptotically to zero. system.
The Underwater Vehicle To finish the illustration of the developed software for symbolic synthesis of control system, consider the model of underwater vehicle (UV) (Dyda and Di
Gennaro, 1994). It consists of 12 differential equations that describe the UV kinematics and dynamics:
5 Refrence Output Error
4
(ω y cosθ sin ζ + ω z sin ψ sin θ ) dθ = ωx − dt cosψ dϕ (ω y cosθ + ω z sin θ ) = dt cosψ dψ = ω z cosθ + ω y sin θ dt
3 2 1 0 -1
dz = −Vx cosψ sinϕ + Vy cosϕ sinθ + Vy cosθ sinϕ sinψ + dt + Vz cosϕ cosθ − Vz cosϕ sinψ sinθ
(M + λ11)Vx − Mycωz + (M + λ33)Vzωy − (M + λ22 )Vyωz + + Mycωxωy + Pl sinψ = Fx
(M + λ22 )Vy + (M + λ11)Vxωz − (M + λ33 )Vzωx − 2
− Myc (ωz + ωx ) + Pl cosψ cosθ = Fy
− ( M + λ11 )Vxω z + + Mycω yω z − Pl cosψ sin θ = Fz (J x + λ44 )ω x + ((J z + λ66 ) − (J y + λ55))ωzωy + + Myc g cosψ sinθ = Mx ( J y + λ55 )ω y + ((J x + λ44 ) − ( J z + λ66 ))ω xω z +
+ (λ11 − λ33 )VxVz = M y ( J z + λ 66 )ω z + (( J y + λ55 ) − ( J x + λ 44 ))ω x ω y − − My V + (λ − λ )V V + My g sin ψ − 11
x
y
θ , ϕ ,ψ
( x , y , z )T
is
the coordinates vector, (ω x , ω y , ω z ) T , (Vx ,Vy ,Vz )T are angular
12
14
16
18
20
u 2 = Fy =
x8*x7*l11-
x11*x10*l33+x8*x7*Mm-x11*x10*Mmyc*x7^2*Mm-yc*x10^2*Mmcos(x3)*sin(x2)*w3*l22cos(x3)*sin(x2)*w3*Mm+ cos(x3)*cos(x2)*w1*l22+ cos(x3)*cos(x2)*w1*Mm+ sin(x3)*w2*Mm+sin(x3)*w2*l22Mm*x11*x12+x9*x7*l22+ Mm*x9*x7x11*x12*l22+PL*cos(x3)*cos(x1),
As seen from simulation results, asymptotically stable tracking of reference trajectory of UV is achieved with derived control law.
c
are Euler’s angles;
10 sec
y d 3 = 4 + 0 . 3 cos( 0 . 4 t ) − 0 . 2 t
− My c (V z ω y − V y ω z ) = M z where
8
where Xi are the components of state space vector (linear and angular coordinates and velocities) of underwater vehicle. It was demonstrated that even very long expressions for control derived can be optimized to the form acceptable for real time implementation. The simulation of the control system had confirmed correctness of control laws synthesis. As illustration, on Fig. 8 the third output of the system is shown with desirable trajectory
+ MycVz + (λ33 − λ22 )VzVy − Myc (Vxωy −Vyωx ) +
22
6
w2 =(R2*sin(omg2*t)x(5))*ki21+(R2*cos(omg2*t)*omg2sin(x(3))*x(8)cos(x(3))*cos(x(1))*x(9)+ cos(x(3))*sin(x(1))*x(11))*ki22R2*sin(omg2*t)*omg2^2.
( M + λ33 )Vz + ( M + λ22 )Vyω x + Mycω x −
x
4
The control law generated by the program is very large. Examples of shortest control elements for (2) and (7) are the following:
dy = Vx cosψ + V y cosψ cosθ − Vz cosψ sinθ dt
c
2
Fig. 8. Third output, reference, and error.
dx = Vx cosϕ cosψ − Vy cosϕ cosθ sinψ + Vy sinϕ sinθ + dt + Vz cosϕ sinψ sinθ + Vz cosθ sinϕ
2
0
and
linear velocities vectors; Fx , Fy , Fz , Mx , M y , Mz are components of the control
vector (forces and torques); other parameters are constant.
4. FINAL COMMENTS The software developed in this work can effectively support the procedure of control system design in symbolic form. Clearly, the bigger the dimension of the object mode is, the longer and more complicated symbolic expressions for control will be obtained. For
example, one can expect that the program will generate very large expressions describing the control laws for different flexible structures (such as given in (Gennaro and Dyda, 1993)) whose dynamics are approximated by a finite multi–dimensional model. The software helps to designer to solve the problem of the synthesis of nonlinear control systems and to simulate them easily. The NFL has been taken as an example to show the effectiveness of the symbolic computations. Sure, other methodologies can be considered for computer symbolic synthesis, such as adaptive and robust control.
REFERENCES A. Isidori (1995) Nonlinear Control Systems. Berlin: Springer-Verlag. H. Nijmeijer, A. Van der Schaft (1990) Nonlinear Adaptive Feedback Linearization of Systems. New York: Springer-Verlag. C.I. Byrnes, F.D. Priscoli, A. Isidori (1997). Output Regulation of Uncertain Nonlinear Systems. Boston: Birkhauser. P.V. Kokotovic, I. Kanellakopoulos, A.S. Morse (1991) Adaptive Feedback Linearization of Nonlinear Systems, P.V. Kokotovic (ed.), Foundation of Adaptive Control. Berlin: SpringerVerlag. M. Vucobratovic, N. Kircansky (1982). Scietific Fundamentals of Robotics Vol.1-3 . Berlin: Springer-Verlag. A. A. Dyda, S. Di Gennaro (1994). Adaptive trajectory control for underwater robot. Proc.OCEANS-94 Osates, Brest. S. Di Gennaro, A. A. Dyda (1993). Attitude control of a satellite with damping compensation of the flexible beam. Proc. European Control Conf., Groningem, pp. 1656-1661.