Study of 3 dimension trajectory tracking of

Ocean Engineering 105 (2015) 270–274

Contents lists available at ScienceDirect

Ocean Engineering journal homepage: www.elsevier.com/locate/oceaneng

Study of 3 dimension trajectory tracking of underactuated autonomous underwater vehicle Ye Li, Cong Wei n, Qi Wu, Pengyun Chen, Yanqing Jiang, Yiming Li College of Shipbuilding Engineering, Harbin Engineering University, Harbin, Heilongjiang 150001, China

art ic l e i nf o

a b s t r a c t

Article history: Received 11 September 2014 Accepted 23 June 2015 Available online 15 July 2015

This paper addresses the problem of trajectory tracking control for underactuated autonomous underwater vehicles (AUVs) in 3 dimensions space. Given a smooth, inertial, 3D reference trajectory, in order to make the position error to stay in the neighborhood of zero, the control algorithm uses vehicle's kinematic equation and linear system stability theory to compute the desired body-fixed velocities. Using these methods, the velocity error dynamics are obtained. Backstepping techniques are used, forcing the tracking error to an arbitrarily small neighborhood of zero. Simulation of a spiral trajectory is performed. The result shows that the controller can realize 3 dimensions trajectory tracking effectively. & 2015 Elsevier Ltd. All rights reserved.

Keywords: AUV Underactuated Trajectory tracking Backstepping

1. Introduction Autonomous underwater vehicles (AUVs) are now routinely employed in a wide range of civilian and military applications. For example, AUVs perform long-distance, long-duration oceanographic sampling missions (Bellingham and Rajan, 2007), detect and localize pollutant sources (Farrell et al., 2003), and detect, locate, and neutralize undersea mines. Underactuated AUVs are a kind of AUVs having fewer actuators than its degrees of freedom. Many today's AUVs are underactuted ones because there are some potential benefits over full actuated AUVs. In case of actuator failures, a fully actuated AUV becomes an underactuated one which might be still controlled with a good control scheme for underactuated AUVs. Besides that advantage, with the underactuated property, an AUV can be designed to be more streamlined with the reduction of water resistance. The design of underactuated AUVs can also reduce the weight and cost, at the same time increase the reliability of AUVs (Mehmet Selcuk et al., 2009). Two main motion control objective of AUVs are path following control and trajectory tracking control respectively. Path following control is to follow a predefined path independent of time (no temporal constraints). Moreover, no restrictions are placed on the temporal propagation along the path. Trajectory tracking is that the position and velocity of the AUV should track desired time varying position and velocity reference signals. Sometimes the trajectory tracking control is based on way-point tracking, since a time varying position can be treated as a moving points.

n

Corresponding author. Tel.: þ 86 15846332743. E-mail addresses: [email protected], [email protected] (C. Wei).

http://dx.doi.org/10.1016/j.oceaneng.2015.06.034 0029-8018/& 2015 Elsevier Ltd. All rights reserved.

Way-point tracking has been studied by numerous authors in recent years. For a vehicle with movement in six degrees of freedom, the motion is highly coupled and nonlinear. Thus decoupling the altitude control and the planar way-point tracking control may result in poor performance. For systems experiencing quick maneuvers and high speed movement in three dimensional space, such as autonomous underwater vehicles and unmanned aerial vehicles, a 3-dimensional way-point tracking controller is preferable. Borhaug and Pettersen (2005) presented a control strategy for global k-exponential way-point tracking control of a class of underactuated mechanical systems with movement in six degrees of freedom. They use a cascaded approach and a backstepping-based method for synthesizing controllers that satisfy the developed dynamical constraints. Do et al. (2004) designed a controller using a Lyapunov's direct method, the popular backstepping and parameter projection techniques. The closed loop path following errors can be made arbitrarily small. It is shown that they developed control strategy is easily extendible to situations of practical importance such as parking and point-to-point navigation. Alessandretti (2013) addressed the design of Model Predictive Control (MPC) laws to solve the trajectory-tracking problem and the path-following problem for constrained under-actuated vehicles. By allowing an arbitrarily small asymptotic tracking error, they derived MPC laws where the size of the terminal set is only limited by the size of the system constraints. The resulting MPC controllers provide a global solution to the addressed constrained motion control problems. This MPC controllers can be applied to 2-D and to 3-D moving vehicles. One of the main difficulties in designing a motion controller is that many assumptions are made about mass and damping matrices. For example, some studies assume that the off-

Y. Li et al. / Ocean Engineering 105 (2015) 270–274

271

Table 1 The main parameters of the test AUV. m Ix

183 3

λ5 λ6

86 86

X u_ Y v_

Iy

95

Xu

49

Iz λ1 λ2 λ3 λ4

95 13 255 255 2.6

Yv Zw Kp Mq Nr

243 230 0 140 160

13 257

Z jwjw K jpjp

422 0

Z w_

257

M jq jq

62

K p_ N r_ M q_ X juju Y jvjv

0 86 86 16 542

N jrjr

78

diagonal terms are zero (Pettersen and Nijmeijer, 2001; Jiang, 2002; Lefeber et al., 2003), however, because the bow and the stern are not always symmetric, the off-diagonal terms are not zero. So it is imperative to design a controller which is not very sensitive to off-diagonal terms of the mass and damping matrices. Bong Seok (2015) presented a formation controller for desired formation of underactuated autonomous underwater vehicles (AUVs). They designed the controller assuming that the mass and damping matrices are not diagonal and that hydrodynamic damping terms are unknown. At the same time, they introduced an additional control input and prove the stability using the Lyapunov stability theory. In this paper, the problem of trajectory tracking control for underactuated AUVs moving on the desired trajectory is addressed. Given a smooth 3D trajectory, the planning algorithm produces desired velocity of [ud, qd, rd] based on the error of position. Then the control forces and moments of [X, M, N] are generated from the controller. The algorithm is based on velocity error of the AUV. The error of position and velocity are connected into a cascaded system, and using the backstepping method, we are able to come up with the proper control forces and moment [X, M, N] in order to make these error converge to arbitrarily small neighborhood of 0. At last, the trajectory used for the illustration during simulation of the method is a spiral line. Simulation results that demonstrate the performance of the developed control design are presented and discussed. The main contribution of this work is: using the cascaded system and the backstepping method to design a controller for underactuated AUVs with nonlinear dynamic characteristic to perform trajectory tracking without decoupling and the controller is not sensitive to the term arrangement of the damping and mass matrices. The remainder of this paper is organized as follows: AUV's kinematics and kinetics equation is derived in Section 2. In Section 3, a controller for trajectory tracking is presented based on the linear system stability theory and a backstepping technique 2 Rnb ðΘnb Þ ¼

cos ψ cos θ 6 sin ψ cos θ 4 sin θ

sin ψ cos ϕ þ cos ψ sin θ sin ϕ cos ψ cos ϕ þ sin ϕ sin θ sin ϕ

Fig. 1. The underactuated AUV model in plane motion.

applications, for the representation of rotations, it is customary to use the xyz (roll-pitch-yaw) convention defined in terms of Euler angles. To study the motion, we define an inertial reference frame {I} and a body-fixed frame {B} (Fig. 1). The origin of the {B} frame coincides with the AUV center of mass (CM) while its axes are along the principal axes of inertia of the vehicle assuming three planes of symmetry: xb is the longitudinal axis, yb is the transverse axis, and zb is the normal axis. The kinematic equations of motion for an AUV in 3 dimension space can be written as

η_ ¼ JðηÞv

ð1Þ

h

iT

T . peb=n ¼ x y z A ℝ3 represents the h iT inertial coordinates of the CM of the vehicle, Θnb ¼ ϕ θ ψ A S3 is the orientation of {B} with respect to {I} frame in terms of Euler angles. h iT b T ωbb=n T . vbb=n ¼ u v w T A ℝ3 are the surge v ¼ vb=n sway and heave velocities, T respectively, defined in the body fixed frame. ωbb=n ¼ p q r A ℝ3 are the body-fixed angular velocities. T η ¼ peb=n T Θnb

" JðηÞ ¼

Rnb ðΘnb Þ 033

033

# ð2Þ

T Θ ðΘnb Þ

where

3 sin ψ sin ϕ þ cos ψ cos ϕ sin θ cos ψ sin ϕ þ sin θ sin ψ cos ϕ 7 5

cos θ sin ϕ

using Lyapunov stability theory. In order to demonstrate the effectiveness of the proposed control scheme, certain simulation results are presented in Section 4. Finally, we make a brief conclusion of the paper in Section 5.

2. Underwater vehicle equations of motion and control objective The AUV is modeled as a neutrally buoyant, rigid body of mass m. The vehicle is equipped with a single propeller, aligned with the axis of symmetry, and with moment actuators that provide independent control in pitch and yaw. In guidance and control

cos θ cos ϕ and 2

1 6 T Θ ðΘnb Þ ¼ 4 0 0

sin ϕ tan θ

cos ϕ tan θ

sin ϕ= cos θ

cos ϕ= cos θ

cos ϕ

sin ϕ

3 7 5

It follows that the kinetic equations of a rigid body (Fossen, 2011) M v_ þ CðvÞv þ DðvÞv þ gðηÞ ¼ τ

ð3Þ

τ ¼ ½X; Y; Z; K; M; NT

ð4Þ

where M accounts for the mass and added mass coefficients, CðvÞ accounts for the body moments of inertia and added moments of

272


inertia coefficients. The matrix DðvÞ represents the drag force and moment. gðηÞ accounts for forces and moments caused by gravitation (and possibly buoyancy). The objective of this paper is to design a control system that force the underactuated vehicle to converge to the desired trajectory. In particular, we seek to develop a state feedback control law τ ¼ τðη; vÞ such that ðx; y; zÞ-ðxd ; yd ; zd Þ and vðtÞ-vd ðtÞ as t-1 where ðxd ; yd ; zd Þ is the position of the desired trajectory and vd ðtÞ is the desired velocity vector.

AAℝ

;

x_ ðtÞ ¼ AðtÞxðtÞ;

xðt 0 Þ ¼ x0

ð5Þ

Here is the theorem and corollary Theorem 1. (Rugh, 1996). For the linear state equation, denotes the largest and smallest pointwise eigenvalues of A(t) þAT(t)by λmax ðtÞ and λmin ðtÞ. Then for any x0 and t0, the solution of (5) satisfies Rt Rt 1=2 λmin ðσ Þdσ 1=2 λmax ðσ Þdσ t0 t0 ‖x0 ‖e r ‖xðtÞ‖ r‖x0 ‖e t Z t0 ð6Þ Corollary 1. (Rugh, 1996). The linear state Eq. (5) is uniformly exponential stable if there exist finite, positive constants γ and λ such that the largest pointwise eigenvalue of A(t) þAT(t)satisfies Z t λmax ðσ Þdσ r λðt τÞ þ γ ; f or all t; τ such that t Z τ ð7Þ τ

To realize tracking control, firstly the position of the vehicle's center of mass is able to converge to the trajectory. Position error is expressed as follows: ε ¼ Rnb ðθnb Þ 1 ðp pd Þ

ð8Þ

where ε is the position error in body fixed coordinate. The derivative of error ε with respect to time is ε̇ ¼

Ṙnb ðθnb Þ 1 ðp pd Þ þ Rnb ðθnb Þ 1 ðp_ p_ d Þ

ð9Þ

Let

σ ¼ ερ

ð11Þ

We differentiate z1 with respect to time ż1 ¼ S ωbb=n z1 S ωbb=n ρ þ vbb=n Rnb ðθnb Þ 1 p_ d 2 3 2 3 0 r q u 6 r 7 b 0 p 5ρþ 4 v 5 Rnb ðθnb Þ 1 p_ d ¼ S ωb=n z1 4 w q p 0 2 32 3 2 3 1 0 0 u 0 6 76 7 6 7 ¼ S ωbb=n z1 þ 4 0 0 δ 54 q 5 þ 4 v 5 Rnb ðθnb Þ 1 p_ d 0 δ 0 r w 0 1 2 3 u 0 B C 6 7 ¼ S ωbb=n z1 þ P @ q A þ 4 v 5 Rnb ðθnb Þ 1 p_ d r w 20 1 0 13 2 3 ud ue 0 6B C B C7 6 7 ¼ Sðωbb=n Þz1 þ P 4@ qe A þ @ qd A5 þ 4 v 5 Rnb ðθnb Þ 1 p_ d rd re w

r

0

B Sðωbb=n Þ ¼ @ r q

q

1

p C A

0 p

0

If we choose 0 1 0 1 2 3 ud 0 Bq C C 6 7 1B n @ d A ¼ P @ Kz1 4 v 5 þ Rb ðθnb Þ 1 p_ d A rd w 0 1 ue h i B C z_ 1 ¼ S ωbb=n þ K z1 þ P @ qe A; K A ℝ33 re

ð13Þ

ð14Þ

T r e is uniformly exponentially converges to 0 and h i matrix z_ 1 ¼ S ωbb=n þK z1 ; K A ℝ33 is uniformly exponential If

ue

qe

stable, then the trajectory tracking is realized. In this paper, matrices K is designed to be K ¼ kI 33 where k A ℝ. The selection of matrix K is based on Theorem 1, Corollary 1. The eigenvalue of S ωbb=n þ K ST ωbb=n þ K T is 2k. So if there exist 0 o λ r 2k and γ 4 0 then because corollary 1, h i z_ 1 ¼ S ωbb=n þ K z1 is uniformly exponential stable. Apparently, k 40 is enough for this condition. In order to make velocities converge to the desired ones, we now take advantage of a method developed by Borhaug and Pettersen (2005), and we make a change about it. The second error equation is z2 ¼ v v d ¼ v e where h v d ¼ ud

ð15Þ

α 2 α 3 α 4 qd r d

iT

ð16Þ

We then differentiate z2 with respect to time and pre-multiply the result by M M z_ 2 ¼ Mðv_ v_ d Þ ¼ CðvÞv DðvÞv gðηÞ þ τ M v_ d We take V ¼ ð1=2Þ

ð10Þ

where ρ is the radius of the error space, ρ ¼ ½δ; 0; 0T . It is permitted when δ is relatively small compared with length of AUV. z1 ¼ σ

0

Then

3. Controller design and stability analysis nn

where

zT2 Mz2

ð17Þ

as our first LFC and differentiate V

V_ ¼ zT2 ð CðvÞv DðvÞv gðηÞ þ τ M v_ d Þ

ð18Þ

The independent controls τ1 , τ5 and τ6 are chosen to stabilize the surge, pitch and yaw modes respectively

τ1 ¼ n1 ðη; vÞ þeT1 Mv_ d c1 z2;1

ð19aÞ


ð19bÞ


ð19cÞ

where N η; v 9C ðvÞv þDðvÞv þ gðηÞ ¼ ni η; v

ð20Þ

ci 4 0; i A f1; 2; 3; 4; 5; 6g

ð12Þ

And eT1 ¼ 1

0

0

0

0

0 ;

eT5 ¼ 0

0

0

0

1

0 ;

eT6 ¼ 0

0

0

0

0

1

However, α2 α4 have not been selected, but according toEq. (18) α2 α4 can be chosen as following to render V


semi-negative. 0 10 1 0 1 c2 z2;2 α_ 2 m22 m23 m24 Bm CB _ C B c z C @ 32 m33 m34 A@ α 3 A ¼ @ 3 2;3 A c4 z2;4 m42 m43 m44 α_ 4 1 0 0 10 1 n2 η ; v u_ d m21 m25 m26 B n η; v C B m CB C @ 31 m35 m36 A@ q_ d A @ 3 A n4 η ; v r_ d m41 m45 m46

273

desired trajectory. Fig. 3 shows the forward speed u of the AUV. The AUV was given the initial position ðx; y; zÞ ¼ ð0; 0; 0Þ½m and the initial orientation ðφ; θ; ψ Þ ¼ ð0 3 ; 0 3 ; 0 3 Þ. The surge speed varied with time in order to make the position error to be

ð21Þ

Finally V_ ¼ zT2 Cz2 r 0

ð22Þ

where C ¼ C 66 . V is a Lyapunov Function for the z1, z2-system and is bounded from above by a quadratic and negative definite function. Because V is quadratic and positive definite, (z1, z2) ¼(0, 0) is definitely a globally uniformly exponentially stable (GUES) equilibrium point of (10) and (14). The closed loop system is 1 i ! 0 h ! z z_ 1 PF Sðωbb=n Þ þ K @ A 1 ¼ ð23Þ z2 M z_ 2 C 063 where 0 1 B F ¼@0

0

0

0

0

0

0

0

1

C 0A

0

0

0

0

0

1

0

1 Fig. 3. Actual and desired surge velocity in 300 s.

4. Simulation test In this section we present some simulation results for the proposed control strategy applied to a model of a slender AUV equipped with rudders for steering and diving, and a propeller to provide forward thrust, that is, the AUV has three independent controls, and the control vector τ is given by τ ¼ ½X; 0; 0; 0; M; NT . And the controller gain matrices were chosen as K ¼ I 33

C ¼ 50dI 66

The parameters of this AUV is shown in Table 1 as follows: The mathematical expression of the desired spiral line is shown below: 8 > < xd ¼ 40 cos ð0:02π tÞ½m yd ¼ 40 sin ð0:02π tÞ½m > : z ¼ 0:1t½m d

The simulation results for trajectory tracking scenario is shown in Figs. 2–7. Fig. 2 shows the 3-D trajectory of AUV when tracking a

Fig. 4. Sway velocity in 6000 s.

Fig. 2. Spiral trajectory tracking. AUV desired and actual trajectory in 300 s.

274


ðxe ; ye ; ze Þ ¼ ð0; 0; 0Þ½m. Figs. 4–7 shows the other speed of AUV when tracking the desired trajectory. 5. Conclusion

Fig. 5. Heave velocity in 6000 s.

For the trajectory tracking control of AUV, an output feedback controller based on linear stability theory and backstepping technique was proposed and shown to track the desired trajectory in the earth-fixed frame. The original problem is divided into two parts, one is the linear guidance problem which can come up with the desired velocity, and the other is a nonlinear control problem which can output the control force and moments. Position error is considered in the first place, and in order to make position error to be zero, the desired velocity is presented. Based on the advantage of a backstepping technique, the velocity error function is constructed to obtain the proper control force and moments. This was helpful for improving the accuracy of trajectory tracking. A focus of this paper was taken on an attempt to use some simple methods so as to fulfill the task of 3-D trajectory tracking. The simulations result show the effectiveness of the proposed trajectory tracking scheme. The AUV is able to track the trajectory closely and the surge speed quickly converge to the desired value. Acknowledgments This work is supported by National Natural Science Foundation of China, No. 51179035 and No. 51279221, and is also supported by Natural Science Foundation of Heilongjiang Province (E201121) References

Fig. 6. Actual and desired pitch velocity in 6000 s.

Fig. 7. Actual and desired yaw velocity in 300 s.

Alessandretti, A., 2013. Trajectory-tracking and path-following controllers for constrained underactuated vehicles using model predictive control. In: Proceedings of European Control Conference. pp. 1371–1376. Bellingham, J.G., Rajan, K., 2007. Robotics in remote and hostile environments. Science 318, 1098–1102. Bong Seok, Park, 2015. Adaptive formation control of underactuated autonomous underwater vehicles. Ocean Eng. 96, 1–7. Farrell, J. A., et al., 2003. Chemical plume tracing experimental results with REMUS AUV. In: Proceedings of OCEANS. 2, pp. 962–968. Jiang, Z.P., 2002. Global tracking control of underactuated ships by Lyapunov's direct method. Automatica 38 (1), 301–309. Lefeber, E., Pettersen, K.Y., Nijmeijer, H., 2003. Tracking control of an underactuated ship. IEEE Trans. Control Syst. Technol. 11 (1), 52–61. Mehmet Selcuk, Arslan, et al., 2009. Optimal Control of underactuated Underwater Vehicle with single Actuator. InTech, Rijeka, Croatia. Pettersen, K.Y., Nijmeijer, H., 2001. Underactuated ship tracking control: theory and experiments. Int. J. Control 74 (14), 1435–1446. Rugh, W.J., 1996. Linear System Theory, Second ed. Prentice Hall, Inc., Upper Saddle River New Jersey. Fossen, T., 2011. Handbook of Marine Craft Hydrodynamics and Motion Control. Jhon Wiley & Sons Ltd., United Kingdom. Borhaug. E. and Pettersen. K. Y., 2005. Adaptive way-point tracking control for underactuated autonomous vehicles. In: Proceedings of the 44th IEEE Conference on Decision and Control, and the European Control Conference. pp. 4038-4034. Do, K.D., et al., 2004. Robust and adaptive path following for underactuated autonomous underwater vehicles. Ocean Eng. 31, 1967–1997.