Control of an underwater vehicle with nonholonomic acceleration constraints

Control of an underwater vehicle with nonholonomic acceleration constraints

Preprints of the Fourth IFAC Symposium on Robot Control September 19-21, 1994, Capri, Italy Control of an Underwater Vehicle with Nonholonomic Accele...

1MB Sizes 0 Downloads 29 Views

Preprints of the Fourth IFAC Symposium on Robot Control September 19-21, 1994, Capri, Italy

Control of an Underwater Vehicle with Nonholonomic Acceleration Constraints Olav Egeland and Einar Berglund Department of Engineering Cybernetics The Norwegian Institute of Technology N-7034 Trondheim, Norway Keywords: Nonholonomic robotic systems, nonlinear control, mobile robots, underwater vehicles Abstract: In this article we present a feedback control law for an idealized dynamic model of a nonholonomic underwater vehicle. The nonholonomic constraints are formulated as acceleration constraints on the vehicle so that only acceleration in the forward direction and angular acceleration are allowed. The control law gives asymptotic stability with exponential convergence to a constant desired configuration. This is achieved by transforming the dynamic model into a chained form , and then deriving a controller for the chained system based on previous work on feedback control of chained forms.

1

Introduction

If the acceleration of an underwater vehicle is constrained so that only the angular accelerations and the forward translational acceleration component can be nonzero, the vehicle is said to have two nonholonomic motion constraints. The dynamic model of this nonholonomic underwater vehicle belongs to the class of systems which cannot be stabilized by a smooth pure feedback law (Brockett 1983). Asymptotic stabilization of a class of nonholonomic systems was achieved with time-varying smooth feedback laws in (Samson 1991 , Pomet 1992, Teel 1992). Exponential convergence was achieved with non-smooth timevarying feedback in (M'Closkey and Murray 1993, Sordalen 1993) and piecewise smooth pure feedback in (Canudas de Wit and Sordalen 1992). Nonholonomic acceleration constraints and conditions for integrability of the constraints to the velocity and position level were discussed in (Ori010 and Nakamura 1991). In (Bloch, Reyhanoglu and McClamroch 1992) control of nonholonomic systems with velocity constraints were investigated in term of the equations of motion . Feedback control of a nonholonomic underwater vehicle with velocity constraints was studied in (Nakamura and Savant 1992, 1992) for a time-varying desired trajectory. The same system was investigated in (Sordalen , Dalsmo and Egeland 1993) where a discontinuous, piecewise smooth control law was proposed based on the controller proposed in (Canudas de Wit and Sordalen 1992) for a unicycle. In (Egeland, Berglund and Sordalen 1994) the kinematic model of the nonholonomic underwater vehicle was trans-

formed into a chained form , and the feedback controller of (Sordalen 1993, Sordalen and Egeland 1993) was applied to the system. In the present paper a controller for an idealized underwater vehicle with nonholonomic acceleration constraints is proposed. The controller is derived by transforming the dynamic model into a chained form. Then , based on similar ideas as the ones used in (Sordalen 1993, Sordalen and Egeland 1993) , a feedback controller is designed which results in asymptotic stability with exponential convergence for the closed loop system. A simulations study is included .

2

Control scheme

In this section a controller is developed for the chained form ul

(1)

Xl

(2)

X3

U2

(3)

X4

X3

(4)

Xs

X4 U l

(5)

X6

Xs

(6)

Xl X2

=

with acceleration inputs Ul and U2 and state vector x = (Xl .. . X6)T . This is done by further developing the ideas presented in (Sordalen 1993, Sordalen and Egeland 1993) where a controller was proposed for the chained form Xl = Ul, X2

= U2,

Xi

= Xi-lUl'

A sequence {to, t J , t2 ... } of time instants is defined by tk = kT for all k E {a, 1,2 ... } where

959

First the prescribed dynamics of z(t) is preT is a positive constant. Let the control variable sented in the form of a Lemma. UI (t) be given by

UI(t)

= a[x(tk)]a(t),

t E [tk, tk+d.

(7)

Lemma 1 Let z(t) be the solution of the system

for all k E {O, 1,2 ... } where a(t) = sin 2 ':j: . Here a[x(tk)] is a constant in each time interval [tk, tk+d, and the subsystem described by Xl and X2 is given by

Z3 Z4 Z5 Z6

->'3 Z3 ->'4Z4 + Z3 - >'5a4 Z5 + aaZ4

(18)

->'6a5z6 + Z5·

(21)

(19) (20)

(8)

(9) in each time interval [tk, tk+l), while the subsystem described by ~ = (X3,X4, ... ,X6)T is given by

X3 X4 X5 X6

U2 X3 a[x(td]a(t)x4

(10)

X5

(13)

(11) (12)

which is a linear time-varying system in each time interval [tk, tk+d. Define

Then there exist positive constants rand c so that for all tk = hk, k = 0,1,2, ...

This can be shown by applying Lemma 3 in Appendix A to each of the equations (18-21) as in (S0rdalen 1993). Next the state transformation and control law are given. Let z(t) be the solution of (18-21), and, in addition, let (23) From (13,21) it is found that

2/1; [ T2 3 x t{td+ 3 ] T p X2(td (14)

~

[;2XI(td + ;2X2(td] , (15)

and let a[x(tk)] be given by -sat{alk

a[x(tk)] = {

+ sgn[alk]II~(tdllt, K} ,

Differentiation with respect to time and (12,20) gives (25) where

-(>'6 a4 + >'5 a3 ) >'~a9 - >'6 a3 a.

k odd 1 sat{a2k + sgn[a2dll~(tdllt, K}, k even. (16)

where /I; is a positive constant, 1I~lh = L~=3Ixil is the one-norm of ~ and sat(-, G) is the saturation function defined by sat(r, K) = r when Irl ::; K and sat(r, K) = sgn(r)K when Irl > K. Except for the saturation, this is a deadbeat controller with an additional term proportional to II~( td II

(24)

aX3 = aZ3 + aC34Z4 + C35Z5 + C36Z6

. E {3 ... 6 } .

2

(28)

where ->'4

+ c45 a

(:45 - C45>'5 a4 + C46 (:46 - C46>'6 a5 .

r

IXi(tdl ::; V ./ IXi(tdl, a [x ( tk )]

(27)

Differentiating once more and using (11,19) gives

1

The saturation function is used to ensure that g[X(tk)] is bounded in the analysis of the subsystem described by f We note that whenever g[X(tk)] ::; K

(26)

(29) (30) (31)

Finally, differentiation and (10,18) gives the control law which is required for z to satisfy the prescribed dynamics (18-21).

(17)

To derive the control law for U2 a state trans- where the time-varying feedback gains are formation ~ = L(t , a[x(td])z is introduced where (33) g23 >'3 + C34 z = (Z3, Z4, Z5, Z6)T, and z(t) is the solution of (34) (:34 - C34 >'4 + C35 aa g24 a linear time varying system with prescribed dya4 namics so that z = 0 is an exponentially stable (35) (:35 - C35>'5 + C36 g25 equilibrium. Then it will be shown that this ima5 (36) (:36 - C36>'6 . g26 plies that ~ = 0 is an asymptotically stable equilibrium of (10-13), and that the solution ~(t) It will be shown in the following that U2 is boundconverges exponentially to zero as t --+ 00. ed whenever lIelh is bounded.

960

Lemma 2 Consider the system (10-13) with the contrallaws (7), (16) and (32). Then there exists a class K function (Khalil 1992) (3d·) and a positive constant >'( so that for all tk

where (3dllz(O)lId may be written in the form

Since Xl (t) and X2 (t) are continuous the result follows from Lemma 2. Boundedness of Ul then follows because of the saturation function, while (17) and Lemma 2 and boundedness of x implies that U2 is bounded . 0 It is noted that the saturation constant K can be selected arbitrarily large as it is only needed to prove Lemma 1.

Modeling

3

for some positive constants rl and r2.

The equation of motion for an underwater vehiProof: Inserting (23) and (24) into (25) and (28) , cle can be written (Fossen 1994) it is seen that whenever Cl =f. 0, Mv + C(v)v + D(v)v = T (46)

X3

where v = (v w)T is given in vehicle coordinates, is the velocity vector and w = (w x Wy wz) T is the angular velocity vec tor. The kinematic differential equations are v

1 5 = Z3 + c34 Z4 + -[C35(X5 + >'6a X6) + c36 X61 Cl

(40) where a(t) is a factor in C34, C35, C36, C45 and C45· Rv (47) P From (17) ,23,24 ,39,40) and a(td = 0 it follows RS(w). (48) R that x6(td = z6(td, x5(td = z5(td, x4(td = z4(td and x3(td = z3(td - >'4Z4(td· Lemma 1 Here R = (Tl T2 T 3) = {rij} is the rotation mathen implies that there exists a positive constant trix from the inertial fram e to th e vehicle frame , r z so that for all tk and S(w) is the skew-symmetric form of w. The Ilz(t)lll :S rzll~(tdlhe- c( t-tk ), Vt 2: tk. (41) angle/axis representation of R is

R

It follows from (23) , (24) and (41) that

IX5(t)1

+ IX6(t)1 :S

r56[11~(0)lld e->'56 t

(42 )

for some positive constants r56 and >'56, while (17), (25), (28) and (41) implies that

IX3(t)1

I/>

p

I

P34~+r6~

for some positive constants r34, P34 and >'34. The result then follows from (41 ,42). 0 We can now state the main result of this sect ion. Theorem 1 Consider th e system (l - 6) with the control law (7), (16) and (32). Th en the controls Ul and U2 are bounded, and th e equilibrium x = 0 of the closed loop system is asymptotically stab{e. Moreover, there exists a class K function (3 (.) and a positive constant>. so that whenever Ilx(O)lll < (3- 1(K), the solution x(t) satisfies

Il x (t) ll l :S (3(lI x(O) lld e->' t, Vt 2: O.

(43)

Proof: Assume that Ilx(O)lll < 3- 1(K). Th en g[x(tdl :S K for all tk, and 2K

1

< hl l ~(tdllf

(-14 )

< 2KII~(tk)llt

(45 )

cos 1/» . (49)

where k is a unit vector and I/> is an angle of rotation around k. The Euler-Rodriguez parameter vector p = (P x Py p;)T can be written

+ IX4(t)1 < r34[l z 3( t)1 + Z4(t)1l +

= I cos I/> + S(k) sin I/> + kk T (1 -

1

= k tan 2" = TraceR + 1

(

r32 -

r 23

r13 - r 31 r21 - r1 2

)

,

(50) while the associated kinematic differential equation is is

p=

Tl R(R + I )w . 1 + race

(51)

Details are given in (Egeland, Berglund and Sordalen 199-1) . In the underactuated case one or more of the elements of T are identical to zero. To obtain a nonh olonomic system we make the simplifi cation of assuming that underactuation can be modelled as acceleration constraints. We believe that this somewhat coarse simplification will sen·e as a first step to\\·ards handling the complete dynamics and kinemati cs for the system. Th e model is written

961

P

Ra

(52)

R

RS(w)

(53)

0"

(54)

where p = (pz PII pz)T is the position of the vehicle, a = (a z OO)T is the acceleration vector and a = (oz 011 oz)T is the angular acceleration vector. The nonholonomic constraints are P = p = O. The inputs to the system are a z , Oz, 011 and Oz· The desired values are Pd = 0 , Pd = 0, Rd = I and Wd = O.

rI

rT

4

Chained form

The contribution of this section is a transformation of the system given by (52- 54) into a chained form . This is done using a transformation (R,p,w,v) f-t x where x E R12 is a vector of local coordinates. Then the desired value for x is Xd = O. We make the assumptions AI: 381 > 0 so that Tll 2: 81 > O. A2: 382 > 0 so that 1 + TraceR 2: 82 > O. The physical interpretation of Al is that the angle between the x axis of the vehicle fram e and the x axis of the desired fram e is less that 90° , while A2 means that the angle rP between the vehicle frame and the desired frame is less than 180°. In the analysis part of the paper it is shown that these assumptions hold for the controller which is proposed in the following. Let x = (Xl'" X12)T where (55) X3

=

X4 ,

X7

=

xS ,

T21 =-, Tll T31 Xs= , Tll

X4

Xs

=

X6,

Xg

=

XlO ,

X6

= Py

(56)

XlO = Po , (57) (58)

Xll = X12, X12 = Px·

From (52) we find that Xl = Tlla x , X3 = T21ax and Xs = T31 a x . Defin e the control variables

= Tlla x ,

= X3,

= XII· (59) Then, according to (55- 59), the system can be written in the chained form

Ul

U2

=

U3

X7,

X3

=

U2

X7

=

= X3

Xs

= X7

= =

= XSUI XlO = Xg

Xs X6

X4Ul Xs

Xll X12

Xg

= U4 = Xli

5

U3

(6 1)

(62)

Next , the inputs a x , Ox, Dy and Oz to the physical system must be found as fun ct ions of the control variables Ul , U2, U3 and U4 of the chained

Control of chained form

Inspection of the chained form (60,61,62) reveals that the system can be considered to consist of the two chained forms Xl - X6 and Xl, X2, X7 -XlO which both have the same structure as the chained form (1-6). In addition, the system has the double integrator (62). The controller for each of the subsystems XI-X6 and Xl ,X2 ,X7 - XlO can then be designed as in Section 2, with the only modification that both X3-X6 and X7-XlO enter in the control law for Ul. The control law is

Xs = -923 X 3 -924 X 4 -92So

U2

u3

X6

-9260

Xg XlO 92S- - 926o 0 = -).llXll - A12X12

= -923 X 7 u4

924 X S -

(64) (65) (66)

where a(t) is defined in connection with (7), and o[x(tdJ is defined in (16) with ~ = (X3" .XlO)T . The time-varying gains 923, 924 , 92S and 926 are defined in (36- 36) , while All and A1 2 are positive constants. Then it can be shown using essentially the same proofs as in Section 2 that for the system (60- 62) with the control law (63- 66) the controls UI - U4 are bounded, and the equilibrium x = 0 of the closed loop system is asymptotically stable. :\10reover, there exists a class K fun ction (3( .) and a positive constant A so that whenever Ilx (O)1 11 < (3 -1 (K) , the solution x (t) satisfies Ilx(t)111 :s:: (3(llx(O)lh)e-)'t,

U4

(60)

X4

form . The expressions are derived in the Appendix, where it is shown that a z , Oz, 011 and Oz can be found from Ul, U2, U3 and U4 whenever Assumptions Al and A2 are satisfied.

6

'It 2: O.

(67)

Stability in SE(3)

P appears in the state vector x. The rotation matrix R is orthogonal which implies Trl + rr2 + rf3 = 1. Then from (55, 56) it follows that Tr I = 1+ X ~ + X 2' Since X2 and X4 are bounded, it 2 4 foll ows that Assumption Al will hold whenever rl dO) > O. From (Egeland, Berglund and Sordalen 1994) we have 1 + TraceR = 2(1 + rll )( l + p;)-l. We see that 1 + TraceR(t) > 0 wheneyer Tll (t ) > 0, and hence, since Px = XI2 is bounded, Assumption A2 holds whenever Assumption Al holds. Finally, from (Berglund(1993) and (Egeland, Berglund and Sordalen 1994) we have Il wll~ :s:: 3x5 + 3x~ + 16xL ,

p and

962

Ilpll~ :::; X~2 + (3/2)(x~ + x~)(1 + X~2) and IIpll~ :::; X~l + (1/2)(7x~ + 7x~ + 16x~1 )(1 + X~2)· Thus, (pT, VT, pT, pT) = 0 is asymptotically stable with exponential convergence to zero. Recalling that IIpl12 = I tan(I/>/2)1, we see that an arbitrary small bound on IIwl12 and Il/>l can be achieved by appropriately selecting the initial conditions of the system .

7

"i~"" o

20

25

30

35

40

45

,·,:tS=: : : : , : : 5

'0

'5

20

25

30

35

40

45

. :rs=' , : : : : : o

f

'00

80

10

'5

20

25

30

35

40

45

Figure 2: Position coordinates Pr, Py and pz '.6r---.-""""T"--.--....---.-----.-""""T"--.--....1.4

1.2

0.8

0 .6

0.4

0.2

VJ

80

5

tid [.]

~

40

'5

tid[s]

The system was simulated with T = 7r, K, = 10, >'3 = A4 = A5 = A12 = 1 and All = 2. First a simulation in the plane was performed with initial conditions R(O) = I, p(O) = (0 100 O)T, V = 0 and w = 0 while A6 = 0.5. A plot of the motion in the xy plane is shown in Figure fig1. Then the system was simulated in SE(3) . Initial conditions were k = (10 O)T , I/> = 7r/2 in the angle/axis representation (49) and p(O) = (1 23)T , v = 0 and w = 0, while A6 = 1. The results are shown in Figures 2 and 3. It is seen that exponential convergence was obtained .

p,

'0

tid[.]

o

Simulations

5

I

XJ \J~~IQ'Cl . .

'5

20

25

30

35

40

45

tid[.]

Figure 3: Rotation angle I/>

-() -o-~ ~ ~O~

under the following assumptions: 1) f[x(t) , t] has the property that for all solutions .';-;,00;;:--.=80----:-60:---.40~---::20,---"---..:'-20--40~ x(t) and all t ~ 0 ---------~----

------

'1t

P.

Figure 1: Position coordinates Pr and Py and

orientation

{f[X(T) , T] - Af }dTI :::; F

(69)

where Af and F are positive constants . 2) The signal d[x(t) , t] is bounded for all solutions x(t) and all t ~ 0 by

Acknowledgments

Id[x(t) , t]1 :::; De- Adt

This research was supported by the Center of Maritime Control Systems at NTH / SINTEF .

(70)

for some positive constants D and Ad · Then x(t) is uniformly bounded and converges exponentially to zero as t -+ 00 . Moreover ,

Appendix A Lemma 3 Consider the nonlinear , non-autonomous system

x(t)

= - f[x(t) , t]x(t) + d[x(t) , t]

(68)

where

* is

the convolution operator.

Proof: The result follows easily using an integrating factor F(t) = expU; {f[X(T), T] - Af }dT

963

which leads to

d dt [F(t)x(t)J

= ->'fF(t)x(t) + F(t)d[x(t) , tJ

Then the result follows since for all t F(t) ~ e P and F-1(t) ~ e P .

~

(72) 0 we have 0

Appendix B From (Egeland , Berglund and Sordalen 1994) we have

Brockett, RW. (1983). "Asymptotic Stability and Feedback Stabilization", in RW. Brockett, R.S . Millman and H .J. Sussman (Eds) , Differential Geometric Control Theory , pp. 181-208, Birkhauser. Canudas de Wit, C. and O. J. S0rdalen (1992). "Exponential Stabilization of Mobile Robots with Nonholonomic Constraints", IEEE Transactions on Automatic Control, pp. 1791- 1797, Vo!. 37, No. 11, Nov. 1992. Egeland , 0., E. Berglund and O.J. S0rdalen (1994). "Exponential stabilization of a nonholonomic underwater vehicle with constant desired configuration', Proc . 1994 IEEE Conf. on Robotics and Automation, San Diego , California. Fossen, T.!. (1994), Guidance and control of ocean vehicles, John Wiley, New York.

Then

)(~~ ) where

+

W2

r

+2T13T32W~ and W3

[Tll T3 3WxW y

-

Tll T32W xW:

ll

- 2(T12T32 - Tl3T33 )wyw: - 2Tl2 T 33 W +[-TIIT2 3WxWy r 11

+

n

TIIT22W xW:

-2TI 3 T22W~ +2(TI2T22 -TI3T23)WyW: +2TI2T23W;J.

Solving for

(

ay ) a:

ay

= Tll

and

a:

( - T 23 T2 2

gives -T33) ( T32

U2 - W2 U3 - W3

)

.

(73) It is seen from (50) that X6 = Px, and from (51) we find that U4 = Px = (1 + TraceR) - 1(1 + Tll )a x + W4 where W4 = (1 + TraceR) -1 (Tl2ay + TI3a:) - 2pT wiYx , and p and Px can b e expressed by the laments of Rand W according to (50) and (51). This gives

o:x

= - -1[ ( 1 + TraceR)u4 1 + Tll

-

W4J

(74)

Khalil , H.K (1992). Nonlinear systems , Macmillan, New York. :\! 'Closkey, RT . and R .M. Murray (1993). "Convergence rates for nonholonomic systems in power form ", Proc. American Control Conference, San Francisco, 1993. Nakamura, Y. and S. Savant (1991). "Nonholonomic ~!otion Control of an Autonomous Underwater Vehicle", IROS '91 , pp . 1254-1259, Osaka, Japan , Nov. 1991. );akamura, Y. and S. Savant (1992). "Nonlinear Tracking Control of Autonomous Underwater Vehicles" , Proc. 1992 IEEE Conf. on Robotics and Automation , pp. A4-A9, Nice, France , May 1992. Oriolo , G. and Y . Nakamura (1991). "Control of mechanical systems with second order nonholonomic constraints: Underactuated manipulators", Proc. 30th CDC Brighton , England . Pomet, J .-B. (1992) . " Explicit Design of Time-varying Stabilizing Control Laws for a Class of Controllable Systems without Drift" , Syst. Control Letters. Publications , 1991. Samson, C. (1991). " Velocity and Torque Feedback Control of a Nonholonomic Cart" , in C. Canudas de \\-it (Ed ), Advanced Robot Control, Springer-Verlag .

Finally, from (59) we find that

(75) We note that the inputs a I • O:I' O:y and 0:: can be computed from (75) , (73 ) and (74) wh enever Assumption Al is satisfied.

Sordalen , O.J. (1993). Feedback contro l of nonholonomic mobile robots , doctoral dissertation, Department of Engineering Cybernetics , The :\orwegian Institute of Technology.

References

Sordalen , O.J. and O. Egeland (1993) , "Exponential Stabilization of Chained Nonholonomic Systems", Proc. European Control Conference, Groningen , The :\etherlands , pp. 1438- 1443 .

Berglund , E. (1993). Stabilization of ocean vehicles with constraints on ve lo city and acceleration. :\ISc Thesis. Department of Engineering Cybernetics, The :\orwegian Institut e of Technology (in :\orwegian ).

Sordalen, O.J., :\1. Dalsmo and O. Egeland (1993). "An exponentially convergent control law for a nonholonomic underwater Yehicle" . 1993 IEEE Int. Con£. on Robotics and Automation , Atlanta, Georgia, :\Iay 2- 7 1993 .

Bioch , A.:\1.. :\. H . :\IcClamroch and :\1. Reyhanoglu (1990). '"Control and Stabilization of :\onholonomic dynamic systems" , IEEE Transa ctions on Automatic Control, pp . 1746- 1757, Vo!. 37, :\0. 11 , :\ov . 1992 .

Teel, A.R. , R :\1. :\!urray and G. \\-alsh (1992). ":\onholonomic Control Systems: From Steering to Stabilization with Sinusoids", Proc. 31st CDC, pp . 1603-1609. Tucson , Arizona .

964