Proceedings of CONTROLO'2000 - 4th Portuguese Conference on Automatic Control, Guimar~aes, Portugal, October 2000.
3D PATH FOLLOWING CONTROL OF AUTONOMOUS UNDERWATER VEHICLES ? P. Encarnac~ao A. Pascoal Institute for Systems and Robotics and Dept. Electrical Eng. Instituto Superior Tecnico Av. Rovisco Pais, 1, 1049-001 Lisboa, Portugal E-mail:fpme,
[email protected]
Abstract: A new methodology is proposed for the design of path following systems for autonomous underwater vehicles. Global convergence to reference paths is achieved with a nonlinear control strategy that takes explicitly into account the dynamics of the vehicle. Formal convergence proofs are indicated. Simulation results with the model of a prototype autonomous underwater vehicle are presented to illustrate the performance of the path following system derived. Copyright CONTROLO 2000 Keywords: Autonomous Underwater Vehicles. Path Following Systems. Nonlinear Control. Backstepping Techniques. 1. INTRODUCTION
For simplicity of presentation, only paths of constant curvature and torsion (i.e., straight lines, circumferences or helices) are considered. This paper deals with AUVs that are only actuated in surge, pitch, and yaw. Thus, the system is not feedback linearizable and other nonlinear control approaches must be sought. The key ideas behind the development of the nonlinear control strategy can be simply explained as follows. An hypothetical vehicle is considered that has its main body axis aligned with the total velocity vector and a nonlinear "kinematic" controller is derived for that vehicle to steer it to a reference path. Asymptotic convergence to the reference path is proven for this control system. Then, the "kinematic" controller derived is extended to incorporate the dynamics. This is done by casting the vehicle dynamic equations into standard integrator form using nonlinear dynamic inversion (Isidori, 1989) and applying backstepping techniques (Krstic et al., 1995). It is assumed that the position and attitude of the underwater vehicle, as well as its angles of attack and sideslip, are accessible for measurement. Formal convergence proofs are indicated. Simulation results with the model of a prototype autonomous underwater vehicle are presented to illustrate the performance of the resulting path following controller. An introduction to the problem of path following for air and underwater vehicles can be found in (Kaminer et al., 1998), (Fryxell et al., 1996), where the authors proposed the use of linearization techniques for accurate vehicle control about trimming paths thus obtaining stability and performance results that are only local in nature. The reader will nd in (Krstic et al., 1995) a lucid presentation of backstepping techniques. See also (Fossen and Berge, 1997; Aamo et al., n.d.) for interesting applications to ship control.
This paper proposes a new methodology for the design of path following systems for autonomous underwater vehicles. This problem has been motivated by the practical need to develop such a control system for INFANTE, an autonomous underwater vehicle (AUV) developed at the Instituto Superior Tecnico. The reader is referred to (Samson, 1992; Micaelli and Samson, 1993) and the references therein for related work in the eld of land robots, where methods for path following have been devised that rely on Lyapunovbased nonlinear control. The techniques proposed essentially solve the problem of path following for land vehicles by taking into account the kinematic equations only. However, they cannot be simply extended to encompass air or underwater vehicles due to the fact that aero/hydrodynamics play a key role in the motion of the latter. For air or underwater vehicles it is necessary to develop methodologies for accurate path following that deal explicitly with the presence of complex aero/hydrodynamic terms in the vehicle dynamics. The work reported in (Micaelli and Samson, 1993) has recently been extended for the case of marine craft in the presence of constant but unknown ocean currents. In (Encarnac~ao et al., 2000) the authors describe a nonlinear path following control system that takes explicitly into account the dynamics of the vehicle as well as those of a current estimator. The same circle of ideas is explored here for the three dimensional case. This work was supported in part by the Commission of the European Communities under contract no. MAS3-CT970092, (ASIMOV) and the Portuguese PRAXIS programme under projects INFANTE and CARAVELA. The rst author bene ted from a PRAXIS XXI Graduate Fellowship. ?
1
X = 12 Vt2 L2 Xuu + Xvv 2 Y = 12 Vt2 L2 Yv + Yr rL Vt 1 2 2 Z = 2 Vt L Zw + Zq qL Vt 1 pL 2 3 K = 2 Vt L Kp V + zCB cos 0 sin 0 gV t M = 12 Vt2 L3 Mw + Mq qL + M + M b b s s Vt 0 +zCB sin gV N = 12 Vt2 L3 Nv + Nr rL + N r r V
2. KINEMATIC AND DYNAMIC MODEL OF AN AUV. THE PATH FOLLOWING PROBLEM.
q a b
lB
xB
y
Vt T
lN zB
Path
yB
t
{SF} xU
{U}
where is the uid density, L is a representative vehicle length, and X(), Y(), Z() , K() , M() , and N() are hydrodynamic coecients. In addition, zCB is the metacentric hight, V is the vehicle's volume, g is the gravity acceleration, 0 and 0 are the pitch and yaw angles from the body xed to the global frame, and b , s , and r are the bow and stern plane and the rudder de ections, respectively. In this model, it was assumed that the vehicle is neutrally buoyant. The forces due to the de ection of the control surfaces, as well as added mass and higher order hydrodynamical terms were neglected. In what follows it is convenient to express the translational motion of the vehicle in terms of its total linear speed Vt , angle of attack , and sideslip angle , which are related to u, v, and w as p Vt = u2 +v2 + w2 ; = arctan wu ;
N
yU
B
zU
Fig. 1. Vehicle following a 3D path. This section derives the dynamic model of an AUV and introduces the path following problem. See (Fryxell, 1994; Silvestre and Pascoal, 1997) for details on the construction, modelling, and control of a representative AUV prototype. Following standard practice, the equations of motion of an AUV can be developed using a global coordinate frame fU g and a body- xed coordinate frame fB g that moves with the vehicle. The following notation is required (Fryxell, 1994; Silvestre and Pascoal, 1997): Bv T is the velocity of the origin of Borg (u; v; w ) fB g relative to fU g, expressed in fB g (i.e., body xed linear velocity) and B !B (p; q; r)T is the angular velocity of fB g relative to fU g, expressed in fB g (i.e., body- xed angular velocity). From NewtonEuler equations, the translational and rotational motion of the AUV are described by
X +T Y Z K M N
= m (u_ + qw = m (v_ + ru = m (w_ + pv = Ix p_ + (Iz = Iy q_ + (Ix = Iz r_ + (Iy
= arcsin Vv ; t
respectively. Assuming the vehicle's speed is held constant, the Vt equation is discarded and the vehicle dynamic model can be written as L3 Z q q+ 2m cos cos 3 tan tan L sin tan + Yr r + 2m cos Vt L2 tan tan Y + Vt L2 Z v w 2m cos 2m cos cos 3 2 tL _ = sin p + cos + 2mLcos Yr r + 2V m cos Yv
_ =
rv) pw) qu) Iy ) qr Iz ) pr Ix ) pq
cos
tan p +
1+
(1) (2)
p_ = Iz I Iy qr + I1 12 Vt2 L3 Kp pL Vt + x x zCB cos sin gV (3) q_ = Ix I Iz pr + I1 21 Vt2 L3 Mw + Mq qL Vt + Mb b + y y Ms s + zCB sin gV (4) r_ = Iy I Ix pq + I1 21 Vt2 L3 Nv + Nr rL Vt + Nr r : (5) z z
where (X; Y; Z ) are the forces along the body- xed frame fB g, (K; M; N ) are the rolling, pitching and yawing moments about frame fB g, and Ix , Iy , and Iz are the moments of inertia about the same frame. In the equations T is the trust force along the xB axis generated by the back propellers and m is the vehicle mass. The external forces and moments include (restoring) terms caused by gravity and buoyancy, added mass terms, the e ects of control surface de ections, and centripetal, Coriolis, and friction as well as hydrodynamic damping forces and moments acting on the vehicle's body. A simpli ed model for the external forces and moments is given by (Fryxell, 1994; Fossen, 1994)
0
0
0
This paper addresses the problem of designing a feedback controller for an underwater vehicle with dynamics described by (1)-(5) to drive it along a desired path with constant speed. Intuitively, the path following controller should look at i) the distance from the vehicle to the path and ii) the angles between 2
2
the vehicle velocity and the tangent to the path, and reduce both to zero (Samson, 1992; Micaelli and Samson, 1993). This motivates the development of the vehicle kinematic model in terms of the so-called Serret-Frenet frame and the use of the above distance and angles as coordinates of the error space where the control problem will be formulated. The following additional notation is required. Let fW g be the frame that results from fB g by rotating it about yB through the angle of attack followed by a rotation about zB through the sideslip angle . Thus, fW g has its xW axis aligned with the total velocity vector; following standard nomenclature in aircraft control, fW g will be called the wind frame. The symbol W RB ( ; ) denotes the rotation matrix from fB g to fW g, parameterized by and . Consider also fSF g, the Serret-Frenet frame associated with each point of the path, with unity vectors T (tangent to the path), N (normal to the path), and B = T N. Let s be the distance along the path, k the path's curvature, and its torsion. According to the well-known Serret-Frenet formulas, the angular velocity of fSF g measured in fU g and expressed in fSF g is given by SF !SF = ( s;_ 0; ks_)T . Let lN and lB denote the distances between the vehicle center of mass and the origin of the Serret-Frenet frame (at the point on the path that is closest to the vehicle) along the N and B axes respectively, i.e., SF PorgW = (0; lN ; lB )T . With this notation, the relative velocity between the wind andT the Serret-Frenet frames is d SF PorgW = 0; l_N; l_B . Let SF RW ( ; ; ) be the dt rotation matrix from the Serret-Frenet frame to the wind frame that is parameterized locally by the roll ( ), pitch (), and yaw ( ) angles. Propagating the vehicle linear velocities from the wind frame to the Serret-Frenet frame yields (Craig, 1986)
3
" 0 cos sin 0 # " 0 # W r _ ; !W;B = 4 0 5 + sin cos 0 0 0 0 1 _ W !B = W RB ( ; ) B !B ; W !SF = W RSF ( ; ; ) SF !SF : Now, the derivatives of the roll, pitch and yaw angles can be computed using the Jacobian operator that relates the angular velocity of fW g with the angular rates (Craig, 1986). Performing all the computations described and using (1) and (2) yields the following "kinematic" model for the velocity vector 1
s_ = 1Vt cc lN k l_N = Vt cs + lB s_ l_B = Vt s lN s_ L3 t Z _ = c p + q c 2mc
(7) (8) (9)
L3 s t Z q + q 2mc 3 2 3 s L t (t ) Y L s tt t Y + L3 c t Y r + r r r c 2m 2m 2mc Vt L2 t (t )2 Y Vt L2 t Z Vt L2 s tt t Y v w v 2m 2mc 2m 2 2 Vt L s t Z + Vt L c t Y c s_ (10) w v 2mc 2mc c 3 3 c L3 s Zq q + L 2c t t _ = L 2mc m Yr 2mc Yr r + Vt L2 c t t Y Vt L2 c Z Vt L2 s Y + v w v 2m 2mc 2mc s s_ (11) 3 s t t 3 c 3 s L L L _ = Zq q + Yr + 2mcc Yr r + 2mcc 2mc
Vt L2 s t t Y Vt L2 s Z + Vt L2 c Y v w v 2mc 2mcc 2mcc tc s_ ks:_
(12)
3. KINEMATIC CONTROL DESIGN
d SF PorgW
SF RW ( ; ; ) W vB = SF vSF + + (6) dt SF !SF SF Porg W
To drive the vehicle to a reference geometric path, it is necessary to align the total velocity vector with the tangent to the path T. When those vectors are aligned, possible errors in roll do not a ect path following and should not be compensated. Moreover, in the AUV model there is no direct control over the angular velocity p. Therefore, the goal of the kinematic controller is to drive the distances lN and lB , as well as the angular displacements and to zero. To do this the kinematic controller uses the angular velocities q and r. From (11) and (12) it follows that the subsystem formed by and has vector relative degree (1; 1). Thus, a feedback linearization control law can be used to linearize the equations; namely, setting
where W vB = (Vt ; 0; 0)T is the vehicle linear velocity measured in fU g and expressed in fW g and SF vSF = (s;_ 0; 0)T is the Serret-Frenet frame linear velocity measured in fU g and expressed in fSF g. Expanding (6) gives s_ = Vt 1cos l cos Nk l_N = Vt cos sin + lB s_ l_B = Vt sin lN s:_ To derive the equations for the evolution of , , and , one can compute the wind frame angular velocity r relative to the Serret-Frenet frame W !W;SF as the W di erence between the angular velocity !W of fW g and the angular velocity W !SF of fSF g. Since W !W r is the sum of the angular velocity W !W;B of fW g W relative to fB g with the angular velocity !B of fB g, it follows that W r r !W;SF = W !W;B + W !B W !SF ; where
q =D 1 1 r
E1 + qr
;
(13)
where D1 and E1 are easily obtained by inspection of (11) and (12), gives The symbols s (), c (), and t () denote the trigonometric functions sin (), cos (), and tan (), respectively. 1
3
_ = q + s s_ _ = r tc s_ ks:_
V_ 1
(14) (15)
In order to have some control over the transient properties of the control system, it is important to shape the evolution of the vehicle trajectory as it approaches the desired path. This motivates the choice of the Lyapunov function candidate V1
2 = 21 lB2 + lN + a (
f (lB ))2
+b(
g (lN ))2
Using (16) V_ 1
f = laB Vt sin sin sin s_ fl0B Vt sin f 0 flB lN s_ k ( f ) and lN Vt cos sin sin g + cos tan s_ + ks_ (16) r = b g +gl0 Vt sin cos + gl0 lB s_ k cos ( g)
N
where k ; k > 0, f; g : < < ! < are C 1 functions, fl0B is the derivative of f with respect to lB , and gl0N is the derivative of g with respect to lN . If hypothesis H1: f (0) = g (0) = 0; H2: jf (lB )j < 2 ; 8lB ; H3: jg (lN )j < 2 ; 8lN ; H4: lB u sin f (lB ) 0; 8lB ; H5: lN u sin g (lN ) 0; 8lN ; hold, lN 6= 1=k, and , , and remain within 2 and 2 , then lN , lB , , and converge to zero. Proof Consider the partial Lyapunov function candidate 2 = 1 lB2 + lN + a ( 2
f (lB ))2
+ b(
g (lN ))2 ;
where a and b are positive scalars and f and g satisfy hipothesis H1 through H5. The time derivative of V along the closed loop system trajectories can be easily computed to yield V_ 1
lB Vt sin f + lN Vt sin g cos bk ( g )2 ;
ak (
f )2
The kinematic controller was designed assuming that the angular velocities q and r were the actual control signals. However, the real vehicle is actuated via control surface de ections. In this section a backstepping technique (Krstic et al., 1995) is used to modify the kinematic controller to accommodate the vehicle dynamics. To cast the system in standard integrator backstepping form, it is necessary to compute the derivatives of q and r . As the vehicle has three sets of control surfaces (bow and stern diving planes and rudders), the equations for _ q and _ r involve three control inputs, thus preventing the use of a nonlinear inversion technique. In this work, the approach is to consider a single equivalent diving plane. Accordingly, Mb b + Ms s in moment M is substituted by Meq eq , where Meq = Mb +2 Ms is a scale factor that makes eq of the same order of magnitude as b and s . Given an equivalent de ection, real control surface de ections are computed in such a way as to make them proportional to the control surface eciency, as evaluated by the moment coecients Mb and Ms . This yields b + Ms ) s = M2sM(M eq ; 2 2 b + 2Ms Mb : b = M s
q
V1
=
which, by hypothesis H4 and H5, is negative semide nite. Now, the use of Barbalat's lemma allows to conclude that lN , lB , , and converge to zero. 2 As suggested in (Micaelli and Samson, 1993), possible choices for f and g are the following sigmoid functions: 2kf lB f (lB ) = sign(Vt )d ee2kf lB + 11 and 2kg lN g (lN ) = sign(Vt ) d ee2kg lN + 11 ; where kf ; kg > 0, 0 d < 2 , 0 d < 2 , and sign () is the signal function. It is straight-forward to verify that these functions satisfy hypothesis H1 to H5. 4. BACKSTEPPING KINEMATICS INTO DYNAMICS
for kinematic control analysis and design, where f (lB ) and g (lN ) are nonlinear functions that impose transient behaviour for and , respectively. Following (Micaelli and Samson, 1993), f (lB ) can be chosen such that l lim f (lB ) = d and l lim f (lB ) = 0, B !1 B !0 where d is a design parameter. Intuitively, the function f (lB ) saturates to a value d when the vehicle is away from the path. Similar comments apply to g (lN ). Some restrictions to these functions can be found in Theorem 1. The positive constants a and b are instrumental balancing the competing goals of driving the position and orientation errors to zero and are additional design parameters to shape closed loop vehicle behaviour. The main result of this section follows. Theorem 1. Consider the closed loop system consisting of (7)-(10),(14)-(15) and the control law
N
= a ( f ) q + sin s_ + fl0B Vt sin 0 + fl0B lN s_ + b( g ) ( r cos tan s _ ks_ gl0N Vt sin cos gl0N lB s_ lB Vt sin f lB Vt (sin sin f ) + lN Vt sin g cos + lN Vt cos (sin sin g) :
s
Using this strategy, the derivatives of q and r can be cast in the form _ q = D2 eq + E2 ; (17) _ r r
= a ( f ) q + sin s_ + fl0B Vt sin + fl0B lN s_ + b( g ) ( r cos tan s _ ks_ gl0N Vt sin cos gl0N lB s_ lB Vt sin + lN Vt sin cos :
where2
2 Vt2 L6 c Z M q eq 4mIy c 4 D2 = 2 Vt2 L6 s Z M q eq 4mI cc
Adding and subtracting lB Vt sin f and lN Vt sin g cos gives
y
4
2 Vt2 L6 c t t + s Y N 3 4mIz c r r 2 Vt2 L6 s t t c Y N 5 : 4mIz c cc r r
The expression for E2 is omitted due to space limitations. It follows that (17) has vector relative degree (1; 1). Thus, the input transformation
eq = D 1 2 r
E2 + eq r
done for the cases where the vehicle motion is con ned to the horizontal and vertical planes. When the vehicle is restricted to navigate in the horizontal plane, the state variables are s, lN , , , and r (the other variables are set to zero). The zero dynamics is thus obtained from the _ equation. Since zr tends to zero, r ! r ; (16) and the fact that lN and converge to zero shows that r ! ks_ . Now, using equations (13) and (2), yields _ = Vt Yv + kV 2kmVt cos :
leads to the linear decoupled subsystem _ q = eq : _ r r
(18)
L Yr
Let q and r be the virtual control laws in (16). According to the backstepping method, de ne zq = q q ; (19) zr = r r
L3Yr
t
By resorting to the comparison lemma (Khalil, 1996), 1 2 ; 8tt0 where 1 and 2 satisfy the di erential equations _ = Vt Yv + kV + 2kmVt
as the di erences between the virtual control variables and the virtual control laws. Intuitively, if zq and zr go to zero the system approaches the manifold correspondent to the "kinematic system". This motivates the choice of the Lyapunov function V2 = V1 + 12 zq2 + zr2 : Di erentiating V2 along the closed loop system trajectories and performing the change of coordinates q = zq + q and r = zr + r yields V_2 = lN l_N + lBl_B + a ( f ) q + s s_ fl0B l_B + b ( g) r tc s_ ks_ gl0N l_N + zq eq _ q + a ( f ) + zr r _ r b ( g) : Setting eq = _ q a ( f ) kzq zq and (20) r = _ r b ( g) kzr zr
t L Yr 1 L3 Yr V Y 2 kmVt : t v _2 = L Y 2 + kVt L 3Y r r Thus, 1 , 2 , and consequently are bounded as long as YYvr < 0. In the vertical plane, the state variables are s, lB , , , and q, and the zero dynamics is obtained from the _ equation. In this case the model should be slightly 1
modi ed to accommodate for the necessary change of the reference path parameterization. Now, the path's curvature should be seen as a rotation about the ySF axis. The details are omitted here, but it can be proven (as for the horizontal plane) that the zero dynamics reduces to _ = Vt Zw kV 2kmVt cos :
L Zq
t
L3 Zq
Again using the comparison lemma, _ remains bounded if ZZwq > 0. It is interesting to point out that the conditions Yv Zw Yr < 0 and Zq > 0 require that the vehicle be stern dominant both in the lateral and vertical planes. For a de nition of bow and stern dominant vehicles, see (Lewis, 1989, Chapter 9, Section 4.2).
with kzq ; kzr 2
