Quaternion Feedback Regulation of Underwater Vehicles

Transcription

1 Fjellstad, O.-E. and T. I. Fossen (1994). Quaternion Feedback Regulation of Underwater Vehicles, Proceedings of the 3rd IEEE Conference on Control Applications (CCA'94), Glasgow, August 24-26, 1994, pp Quaternion Feedback Regulation of Underwater Vehicles Ola-Erik Fjellstad and Thor I. Fossen Department of Engineering Cybernetics The Norwegian Institute of Technology University of Trondheim N-7034 Trondheim, Norway Ola- Erik. Fj ellst ad Qi tk.uni t. no TM-3-6 Abstract Position and attitude set-point regulation of autonomous underwater vehicles in 6 degrees of freedom is discussed. Euler parameters are used in the representation of global attitude. A class of non-linear PD-control laws is derived by using a general Lyapunov function for the 6 degrees of freedom dynamic model of the vehicle. 1. Introduction Unmanned underwater vehicles (UUV) have become an important tool in diverless subsea operations. Most UUVs are remotely operated vehicles (ROV) connected to a surface ship by a tether. Autonomous underwater vehicles (AUV) are free-swimming vehicles which carry their own energy source and automatically interact with the environment. Typical applications for UUVs are visual survey, inspection, maintenance, welding and equipment retriveal. For ROVs which are controlled in 6 degrees of freedom (DOF) local autonomy is required to some extent. 6 DOF station-keeping or tracking of swimming devices are difficult tasks for human operators. Also, if the communication channel is narrow-banded, such as an accoustic link, the need for local intelligence is increased. Supervisory control is an important aid for high level teleoperation of both the vehicle and the robot manipulator [ll]. For rigid bodies in 6 DOF the nonlinear dynamic equations of motion have a systematic structure which becomes apparent when applying vector notation. This is exploited in the control literature, particularly in the control of mechanical systems like vehicles and robot manipulators. A PD-control law exploiting the passivity property of robot manipulators was first derived by [3], and later reformulated by [2]. The control law was formulated in both the joint-space and the task-space. For 6 DOF vehicles the dynamic equations of motion are usually separated into translational and rotational motion. Position is specified by a three vector while various representations of attitude have been discussed in the literature. The most frequently applied representations are the Euler angle conventions, which are minimal 3-parameter representations. The roll, pitch and yaw (RPY) convention dominates in the context of mobile vehicles. The popularity of the Euler angle conventions can probably be explained by their easily understood physical interpretation. However, there are no sensors which can measure the Euler angles directly. Therefore some transformation between the measurement and the parameters must be carried out. Similarly, the desired Euler angles must be generated from some desired attitude signal. These properties are shared by all known attitude representations. Hence nothing is actually gained from knowing the physical interpretation of the Euler angles. There are also some obvious disadvantages in terms of the Euler angle attitude representations. As earlier mentioned, they are 3-parameter representations and therefore they must contain singular points [SI. The Euler angles are defined by three successive rotations about three axes in a certain sequence. This rotation sequence is not exploited in the control design. Application of Euler angles to parameterize rotation matrices R E S0(3), that is the Speczal Orthogonal group of order 3, implies numerous computations with trigonometric functions. Consequently, it /94/$ IEEE 857

2 cannot be claimed that Euler angles are better suited than other attitude representations in control applications. To increase the flight envelope of an UUV, the vehicle should be allowed to operate at any global attitude. This can be done by applying two 3-parameter chart representations with singularities at different points. Switching between the charts, however. will introduce discontinuities in the control law. A better approach is to choose a singularity-free representation such as the Euler parameters, see e.g. [4]. Euler parameters, or unit quaternions, have been used in different contexts of attitude control. Control of spacecraft, satellites. aircraft and helicopter are well known applications. More recently the use of Euler parameters has been reported in the robot literature. Quaternionbased attitude set-point regulation has been discussed by [5]: [6]. [$I and [lo] for instance. However, the translational motion aspects have not been addressed b?; these authors. For 6 DOF control problems like underwater vehicles there are significant couplings between the rotational and translational motion. For instance. hydrodynamic added mass will introduce additional couplings due to Coriolis and centrifugal forces. In addition to this. hydrodynamic damping will be strongly coupled. These effects must be considered in the design of a 6 DOF controller. In this paper we discuss automatic station-keeping, or dynamic positioning, in 6 DOF for an UUV. The vehicle is shown to have a dynamic model structure similar to standard robot manipulator equations of motion: Section 2. The UUV model is written in terms of Euler parameters to represent attitude. A class of non-linear 'PD'-control laws for position and attitude regulation is presented in Section Mat hematical modelling The kinematic model describes the geometrical relationship between the earth-fixed and the vehicle-fixed motion. Kinematic equations of motion The transformation matrix J(q) relates the bodyfixed reference frame (B-frame) to the inertial reference frame (I-frame) according to: where z = [z, y, z]' is the I-frame position of the vehicle, q = [q, et.it = [q, 1, 2, c3it is the unit quaternion representing the attitude, and v = Iu, U, w]' and w = Ip: q, r]' are the linear and angular velocities of the vehicle in the B-frame. The elements of the unit quaternion q E Sc'(2), that is the Special Unztay group of order 2. are called Euler parameters and they satisfy: 72 + E' = 72 - E; + ; + <; = 1 (2) The rotation matrix R from I to B in terms of Euler parameters is written as: i q2 + E: - f; - ; R(q) = 2( T 3) 2( T (2) 2( if2-77c3) 2( 1(3 + TC2) q2 - Cy + E ; - f; 2( 2C3-7 ]) j (3) 2( ') I) 72 - E: - c: + 3 The quaternion q can be interpreted as a complex number with 77 being the real part and E the complex part. Hence the complex conjugate of q is defined as: Accordingly, the inverse rotation matrix can be written: R-'(q) = RT(q) = R(G) (4) (5) Successive rotations involves multiplication between two rotation matrices. It can be shown that: where quaternion multiplication q1 qz is defined as: Here we have used the skew-symmetric matrix operator S(a) = -ST(a) defined as (a E R3): 0 -a3 +a? +a3 0 -a1 ] -02 +a1 0 E SS(3) (8) such that for an arbitrary vector b E [R3 we have a x b E S(a)b. LVith this notation the coordinate transformation matrix U(q) can be written as: Notice that UT(q)q = 0. whereas T(q)e = TE 858

3 Rigid-body dynamics Newton's equations of motion for a rigid-body with respect to the B-frame are usually written: Io&+, x (Iow)+mTG x (G+w x v)= f? (11) where TG = [ t~, YG, ZG]~ is the center of gravity, m is the constant mass, IO is the constant inertia matrix of the vehicle with respect to the B-frame origin, and fl and f are vectors of external applied forces and moments, respectively. To exploit the structure of the dynamic equations in the control design, we write (10) and (11) in a more compact form as: MRB~ + CRB(v)v = rrb where rrb = [ f f F]' and Hydrodynamic damping/lift For an underwater vehicle, the hydrodynamic damping/lift matrix D(v) should at least include laminar skin friction and viscous damping due to vortex shedding. The matrix D(v) will be strictly positive, that is: D(v) > 06x6 (17) such that vtd(v)v > 0 V v # 0. This reflects the dissipative nature of the hydrodynamic damping forces. Restoring forces and moments The gravitational and buoyant forces f and f act through the centre of gravity TG = [XG, YG, ZG]' and the centre of buoyancy TB = [zg, YB, ZB]', respectively. They can be transformed to the B-frame by: -ms(v) - m S(S(w)rG) -S(Iow) + ms(s(v)rc) Notice that the zero term ms(v)v = 0 is added to make CRB(~) skew-symmetric. where W = m g and B denote the weight and buoyancy of the underwater vehicle. Notice that the I- frame z-axis is taken to be positive downwards. The restoring forces and moments are collected in the vector g(q) according to: Added inertia For a completely submerged vehicle at great depth the hydrodynamic added inertia matrix MA is positive definite and constant [l]: xu xu xw I xp xq xi Y, Yv Yw I Yp Yq Yi z, z;, zw 1 zp zq z; =-[ Kv K; I h;j Kq Mu M; M; I M+ Mq M, Nu Nv N; I N? Nq Ni where All = A:l, A12 = All, and A22 = A;2. The concept of added mass introduces Coriolis and centrifugal terms. These extra terms can be represented by, cf (13) and (14): which is skew-symmetrical. Dynamic equations of motion The rigid-body dynamics combined with added inertia, hydrodynamic damping/lift and restoring forces and moments yields the total dynamic model: where Mb + C(v)v + D(v)v + g(q) = r (20) and r is a vector of actuator control forces and m e ments. Notice that M = MT > 06x6 is constant and positive definite, and that C(v) = -CT(v) is skewsymmetrical. These properties will be exploited in the Lyapunov analysis of the proposed control laws. 859

4 Attitude error dynamics The rotation matrix R E SO(3) from the I-frame to the B-frame represents the actual attitude of the vehicle. Thus the Euler parameters can be seen as a parameterization of S0(3), that is R = R(q). Let R d denote the desired attitude, that is the rc+ tation matrix from the inertial frame to a desired frame (D-frame). The quaternion parameterization is given by Rd = R(Qd). The control objective is to make the B-frame coincide with the D-frame such that R = Rd. The attitude error is defined as k = RilR = RZR and the control objective therefore transforms to i? = 1 6x6. If we applies the Euler parameter representation we obtain R = R(4) where: ij E [-1,1] except at singular points where H(f) is not defined. Differentiating V with respect to time yields (assuming i d = 0 and Gd = 0): where U = [vt> wtit and This expression is obtained by combining (5), (6) and (7). Perfect set-point regulation is expressed in quaternion notation as: (24) The attitude error differential equations follows from (1) and (9), that is: where the desired angular velocity Wd = 0. Hence, Lz, = w - wd = W. Notice that the attitude error itself has group structure, that is ij E SU(2). Here we have used the fact that M is constant and symmetrical and that C(Y) is skew-symmetrical. The control law is chosen as: = - KdV - Kp(q)z + g(q) (28) with Kd,= Ki > 06x6. This finally yields a nonpositive V, that is: v = --VT[Kd + D(Y)] Y 5 0 (29) Notice that i- = 0 if Y = 0. Hence asymptotic stability cannot be guaranteed by applying Lyapunov's direct method. Suppose Y = 0: then the closed loop dynamics yields: 3. Main results In this section we propose a class of set-point regulators for the UUV model in Section 2. The controllers are formulated in a general framework exploiting the 6 DOF nonlinear model properties. and the equilibrium points are given by Z = 0 and E = 0 e ij = f l or ah/af = 0. Only equilibrium points corresponding to ij = A1 represent the desired attitude of the vehicle. Different choices for the function H(ij) and corresponding feedback z are discussed in the subsequent sections. General Lyapunov function approach A Lyapunov function candidate for (20) is: 1 V -(vtmv + STKz%) + 2~ H(ij) (26) 2 where c > 0, K, = KT > 0 is positive definite and the position error is % = z - Z d. The scalar function H(?) is non-negative on the interval 6 E [-I, 11 and it vanishes only at ij = -1 and/or ij = 1. H(ij) also satisfies the Lipschitz condition on the interval Vector quaternion feedback control law Feedback from the vector quaternion E will first be discussed in terms of PD control. Defining H(ij) as: yields the feedback control law: H(ij) = (31) 860

5 where the signum function is defined as The function H(ij) vanishes at ij = kl, and both equilibrium points are asymptotically stable according to the invariant set theorem of LaSalle [3]. Notice that the signum function is non-zero by definition in order to avoid an extra (unstable) equilibrium point at ;i = 0. Alternatively, the function H(ij) can be defined as: H(ij) = 1 - ij (34) to give the control law: -r = --KP - -Kp(q)[;] + S(Q) (35) Now, the equilibrium point ii = 0, ij = 1 is asymptotically stable according to LaSalle s theorem, whereas 5 = 0; ij = -1 is unstable. This can be seen from the following discussion. Suppose ij = -1 and 5 = 0. The steady-state value of the Lyapunov function is then: v,, = 2c (36) If the. system is perturbed to ij = E where E > 0 it can be shown that V takes the value: v = 2c - E < v,, (37) Since V decreases monotonically for ij # +l, the system can never return to the unstable equilibrium point. If q represent one certain attitude, then -q is the same attitude after a f2a rotation about an axis. Physically these two points are indistinguishable, but mathematically they are distinct, as demonstrated in case of the pure vector quaternion feedback case above. Notice that the identity on SU(2) is a 4a rotation about an arbitrary axis. It is straightforward to show that choosing the function H(ij) = 1 + ij gives the same results except that z2 = -E and ij = -1 is asymptotically stable whereas ij = 1 is unstable. Alternativ feedback control laws From the previous sections it follows that asymptotically convergence is obtained for the feedback laws given by (28) where L is computed from the class of functions H( ij). The properties and performance of the closed loop system is changed by simply shaping H(ij). Two classical approaches are the Euler rotation feedback and the Rodrigues parameter feedback. The former is obtained by choosing: H(ij) = 1 - ij2 3 L = [ ] (38) while the latter comes from: A summary of the rotational part of the presented feedback control laws and also some alternatives to them, are given in Table 1. In the table it is distinguished between asymptotic stable equlibrium points (a.s.e.p.), unstable equlibrium points (u.e.p.) and singular points (s.p.). a.s.e.p. ij = i l ij=1 ij=-1?=*l ij = *l ij = *l ij = *1 ij=1?/=-i u.e.p. ij= -1 ij=1 s.p.?j=1 Table 1: Alternativ choices of H(ij). p is a positive integer. In this paper we have not discussed the optimal choice of H(ij). Hence further investigations should be performed in order to decide how H(ij) should be chosen to obtain best performance. 4. Simulation study The control law (32) were simulated for an underwater vehicle given by the following set of parameters: M = diag(215, 265, 265, 40, 80, 80) D(Y) = diag(70, 100, 100, 30, 50, 50}+ 86 1

6 2 The vehicle is assumed to be neutrally buoyant with W = B = 185 t 9.8 (N). with the B-frame origin at the center of gravity. i.e. TG = 0. The control law parameters were set to Kd = 16x6. K, = x3 and c = 200 Thz initial vaiues were t(0) = [IO, 10, 10, , 0.5]T and u(0) = 0, and the regulation set-point was chosen as Id = [0, 0, 0, 1, 0. 0, OIT We used Runge-Kutta s 4th-order method with sampling time 0.25 (sec) in the simulations. The results are shown in Figure 1. The simulation study indicates that the overall system performance is excellent. system and three linear accelerometers (strap-down navigation system). The control law was simulated for an underwater vehicle in 6 DOF. The simulation study indicates that the overall system performance is excellent. 6. Acknowledgments This work was supported by the Royal Norwegian Council for Scientific and Industrial Research through the MOBATEL Programme at the Norwegian Institute of Technology. References 7 1 U t (sec) 4.5; 10 20,b -=ti / 1 I f (sec) Figure 1 : Step response: vector quaternion feedback. 5. Conclusions We have derived a class of 6 DOF underwater vehicle control laws for set-point regulation. Furthermore, we have shown that these control laws can be written in a unified framework according to: T = -KdY - Kp(q)z f dq) (40) This simply is a nonlinear PD-control law with gravitational compensation. To implement the proposed control law we need to know both global position and attitude. and linear and angular velocities in the B-frame. Position is typically measured by a hydro-accoustic long base-line (LBL) system possibly combined with a pressure gauge measurement for depth. Attitude can be measured with a standard compass and two inclinometers, whereas body-fixed angular velocity is measured with a 3-axes rate sensor. The linear velocity is assumed estimated using standard observer techniques together with an LBL [l] T. I. Fossen. Guidance and Control of Ocean I.khicles. John Wiley 8i Sons Ltd., D. E. Koditschek. h atural Motion of Robot Arms. In Proceedings of the 23rd ZEEE Conference on Decision and Control, pages Las Vegas, SV, [3] J. LaSalle and S. Lefschetz. Stability by Lyapunovs Direct Method. Acadamic Press, [4] D. 3. Lewis, J. M. Lipscombe, and P. C. Thomasson. The Simulation of Remotely Operated Vehicles. In Proceedings of the ROV 84 Conference, pages [SI G. Meyer. Design and Global.Analysis of Spacecraft Attitude Control Systems. Technical Report N.4S.4 TR R-361, National Aeronautics and Space Administration, Washington D.C., 19i R. E. Mortensen. A Globally Stable Linear.4ttitude Regulator. International Journal of Control, 8( 3): , [i] S. V. Salehi and E. P. Ryans. A Non-Linear Feedback Attitude Regulator. International Journal of Control, 41(1): [S] J. Stueipnagel. On the Parametrization of the Three-Dimensional Rotation Group. SIAM Review, 6(4): , [9] M. Tagegaki and S. Arimoto. A New Feedback Method for Dynamic Control of Manipulators. ASME Journal of Dynamic Systems. Measurement and Control. 102: June [lo] B. Wie, H. Weks, and A. Arapostathis. Quaternion Feedback Regulator for Spacecraft Eigenaxis Rotations. AIAA Joumal of Guzdance. Control and Dynamics. 12(3): , [ll] D. R. Yoerger, J. B. Kewman, and J. J. E. Slotine. Supervisory Control System for the JASO?! ROV. ZEEE Joumal of Oceanic Engineering, 11(3): ,