TM-3-6

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

Abstract

Position and attitude set-point regulation of au- tonomous underwater vehicles in 6 degrees of freedom is discussed. Euler parameters are used in the rep- resentation of global attitude. A class of non-linear PD-control laws is derived by using a general Lya- punov 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 vehi- cles which carry their own energy source and au- tomatically 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 free- dom (DOF) local autonomy is required to some ex- tent. 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 no- tation. 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 manipula- tors 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 mo- tion are usually separated into translational and rota- tional motion. Position is specified by a three vector while various representations of attitude have been discussed in the literature. The most frequently ap- plied 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. How- ever, there are no sensors which can measure the Eu- ler 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 representa- tions and therefore they must contain singular points [SI. The Euler angles are defined by three succes- sive 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 Or- thogonal group of order 3, implies numerous computa- tions with trigonometric functions. Consequently, it

0-7803- 1872-2/94/$4.00 0 1994 IEEE 857

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. 857-862.

cannot be claimed that Euler angles are better suited than other attitude representations in control appli- cations.

To increase the flight envelope of an UUV, the vehicle should be allowed to operate a t 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 p a r a m e t e r s , see e.g. [4]. Euler parameters, or unit quaternions, have been used in different con- texts of attitude control. Control of spacecraft, satel- lites. aircraft and helicopter are well known applica- tions. More recently the use of Euler parameters has been reported in the robot literature. Quaternion- based 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 be- tween 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 con- sidered 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 3.

2. Mat hematical modelling

The kinematic model describes the geometrical rela- tionship between the earth-fixed and the vehicle-fixed motion.

Kinematic equations of motion The transformation matrix J ( q ) relates the body- fixed reference frame (B-frame) to the inertial ref- erence 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 an- gular velocities of the vehicle in the B-frame. The elements of the unit quaternion q E Sc'(2), that is the Special U n z t a y group of order 2. are called Euler parameters and they satisfy:

7 2 + E'€ = 72 - E ; + €; + <; = 1 (2)

The rotation matrix R from I to B in terms of Euler parameters is written as:

q 2 + E: - f; - €; R(q) = 2 ( € 1 € 2 + T € 3 )

2 (€1€2 - T ( 2 )

j (3)

i 2 ( € i f 2 - 77C3) 2 ( € 1 ( 3 + T C 2 )

q2 - C y + E ; - f; 2 (€2 C 3 - 7€]) 2(€2€3 + ' )€ 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:

(4)

Accordingly, the inverse rotation matrix can be writ- ten:

(5) R-'(q) = RT(q) = R(G)

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 oper- ator S(a) = -ST(a) defined as (a E R3):

0 -a3 +a? +a3 0 -a1 ] E SS(3) (8) - 0 2 +a1 0

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 U T ( q ) q = 0. whereas T ( q ) e = T E

858

Rigid-body dynamics Newton's equations of motion for a rigid-body with respect to the B-frame are usually written:

Io&+, x ( I o w ) + m T G x (G+w x v ) = f ? (11)

where T G = [ t ~ , Y G , Z G ] ~ 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 f l 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:

M R B ~ + C R B ( v ) v = r R B

where rRB = [ f f F]' and

- m S ( v ) - m S ( S ( w ) r G ) -S ( Iow) + mS(S(v)rc)

Notice that the zero term mS(v)v = 0 is added to make C R B ( ~ ) skew-symmetric.

Added inertia For a completely submerged vehicle a t great depth the hydrodynamic added inertia matrix MA is posi- tive 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 K q Mu M; M; I M+ Mq M, Nu Nv N; I N? Nq N i

where A l l = A:l, A12 = A l l , and A22 = A;2. The concept of added mass introduces Coriolis and cen- trifugal terms. These extra terms can be represented by, cf (13) and (14):

which is skew-symmetrical.

Hydrodynamic damping/lift For an underwater vehicle, the hydrodynamic damp- ing/lift matrix D ( v ) should at least include laminar skin friction and viscous damping due to vortex shed- ding. The matrix D ( v ) will be strictly positive, that is:

D ( v ) > 0 6 x 6 (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 = [ X G , YG, ZG]' and the centre of buoyancy T B = [zg, YB, Z B ] ' , respec- tively. They can be transformed to the B-frame by:

where W = m g and B denote the weight and buoy- ancy 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 vec- tor g(q) according to:

Dynamic equations of motion The rigid-body dynamics combined with added iner- tia, hydrodynamic damping/lift and restoring forces and moments yields the total dynamic model:

Mb + C(v)v + D(v)v + g(q) = r (20)

where

and r is a vector of actuator control forces and m e ments. Notice that M = MT > 0 6 x 6 is constant and positive definite, and that C ( v ) = -CT(v) is skew- symmetrical. These properties will be exploited in the Lyapunov analysis of the proposed control laws.

859

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 = R i l R = RZR and the control objective there- fore transforms to i? = 1 6 x 6 . If we applies the Euler parameter representation we obtain R = R(4) where:

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 W d = 0 . Hence, Lz, = w - w d = W . Notice that the attitude error itself has group structure, that is i j E S U ( 2 ) .

3. Main results

In this section we propose a class of set-point regula- tors for the UUV model in Section 2 . The controllers are formulated in a general framework exploiting the 6 DOF nonlinear model properties.

General Lyapunov function approach A Lyapunov function candidate for (20 ) is:

1 2 V -(vTMv + STKz%) + 2~ H(i j ) ( 2 6 )

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 i j = 1. H(ij) also satisfies the Lipschitz condition on the interval

i j E [-1,1] except at singular points where H ( f ) is not defined.

Differentiating V with respect to time yields (assum- ing i d = 0 and G d = 0):

where U = [vT> wTIT and

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:

= - K d V - Kp(q)z + g ( q ) (28 )

with K d , = Ki > 0 6 x 6 . This finally yields a non- positive V , that is:

v = - -VT[Kd + D(Y)] Y 5 0 (29)

Notice that i- = 0 if Y = 0 . Hence asymptotic sta- bility cannot be guaranteed by applying Lyapunov's direct method. Suppose Y = 0 : then the closed loop dynamics yields:

and the equilibrium points are given by Z = 0 and E = 0 e i j = f l or a H / a f = 0. Only equilibrium points corresponding to i j = A1 represent the desired attitude of the vehicle.

Different choices for the function H(i j ) and corre- sponding feedback z are discussed in the subsequent sections.

Vector quaternion feedback control law Feedback from the vector quaternion E will first be discussed in terms of PD control. Defining H ( i j ) as:

H ( i j ) = 1 - 161 (31)

yields the feedback control law:

860

where the signum function is defined as

The function H(i j ) vanishes a t i j = kl, and both equilibrium points are asymptotically stable accord- ing to the invariant set theorem of LaSalle [3]. Notice that the signum function is non-zero by definition in order t o avoid an extra (unstable) equilibrium point at ;i = 0.

Alternatively, the function H(i j ) can be defined as:

H(i j ) = 1 - ij (34)

to give the control law:

-r = - - K P - -Kp(q)[ ; ] + S(Q) (35)

Now, the equilibrium point ii = 0, i j = 1 is asymptot- ically stable according to LaSalle’s theorem, whereas 5 = 0; i j = -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 i j = - 1 + 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 f 2 a 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 ro- tation about an arbitrary axis.

It is straightforward to show that choosing the func- tion H(i j ) = 1 + i j gives the same results except that z2 = -E and i j = -1 is asymptotically stable whereas i j = 1 is unstable.

Alternativ feedback control laws From the previous sections it follows that asymptot- ically convergence is obtained for the feedback laws given by (28) where L is computed from the class of functions H( i j ) . The properties and performance of the closed loop system is changed by simply shaping

H(ij) . Two classical approaches are the Euler rota- tion feedback and the Rodrigues parameter feedback. The former is obtained by choosing:

H(i j ) = 1 - i j 2 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 distin- guished between asymptotic stable equlibrium points (a.s.e.p.), unstable equlibrium points (u.e.p.) and sin- gular points (s.P.).

a.s.e.p.

ij = i l

i j = 1

i j = - 1

? = * l

ij = *l

ij = *l

i j = *1

i j = 1

? / = - I

u.e.p.

i j = -1

i j =1

i j = O

i j = O

s.p.

i j = O

i j = O

? 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 ( i j ) . 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 underwa- ter 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

2

7 1

The vehicle is assumed to be neutrally buoyant with W = B = 185 t 9.8 ( N ) . with the B-frame ori- gin at the center of gravity. i.e. T G = 0 . The control law parameters were set t o Kd = 1 6 x 6 .

K , = 30 . 1 3 x 3 and c = 200 Thz initial vai- ues were t ( 0 ) = [IO, 10, 10, 0.5. 0.5. 0.5, 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 sys- tem performance is excellent.

U -6000 10 20 30

t (sec)

4.5; 10 20 ,b

-=ti /’ 1

I

10 20 30

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 gravi- tational 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 possi- bly 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 sen- sor. The linear velocity is assumed estimated using standard observer techniques together with an LBL

system and three linear accelerometers (strap-down navigation system).

The control law was simulated for an underwater ve- hicle 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 Insti- tute of Technology.

References [l] T. I. Fossen. Guidance and Control of Ocean I.khi-

cles. John Wiley 8i Sons Ltd., 1994.

121 D. E. Koditschek. h’atural Motion of Robot Arms. In Proceedings of the 23rd ZEEE Conference on Decision and Control, pages 733-735. Las Vegas, SV, 1984.

[3] J. LaSalle and S. Lefschetz. Stability by Lyapunovs Direct Method. Acadamic Press, 1961.

[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 249- 251. 1984.

[SI G. Meyer. Design and Global .Analysis of Spacecraft Attitude Control Systems. Technical Report N.4S.4 TR R-361, National Aeronautics and Space Admin- istration, Washington D.C., 19i1.

A Globally Stable Linear .4tti- tude Regulator. International Journal of Control,

161 R. E. Mortensen.

8( 3):297-302, 1968.

[i] S. V. Salehi and E. P. Ryans. A Non-Linear Feed- back Attitude Regulator. International Journal of Control, 41(1):281-287. 1985.

[S] J . Stueipnagel. On the Parametrization of the Three-Dimensional Rotation Group. SIAM Review, 6(4):422-430, 1964.

[9] M. Tagegaki and S . Arimoto. A New Feed- back Method for Dynamic Control of Manipulators. ASME Journal of Dynamic Systems. Measurement and Control. 102:119-125. June 1981.

[lo] B. Wie, H. Weks, and A. Arapostathis. Quaternion Feedback Regulator for Spacecraft Eigenaxis Rota- tions. AIAA Joumal of Guzdance. Control and Dy- namics. 12(3):375-380, 1989.

[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):393- 400, 1986.

862