Improving the Formation-Keeping Performance of Multiple Autonomous Underwater Robotic Vehicles

Transcription

1 Proceedings of the IEEE International Conference on Mechatronics & Automation Niagara Falls, Canada July 25 Improving the Formation-Keeping Performance of Multiple Autonomous Underwater Robotic Vehicles Erfu Yang, Dongbing Gu, and Huosheng Hu Department of Computer Science University of Essex Wivenhoe Park, Colchester CO4 3SQ, United Kingdom {eyang, dgu, MAIN MENU AUTHOR INDEX Abstract This paper presents the application of the successive Galerkin approximation (SGA) approach to the nonlinear optimal and robust formation control of multiple autonomous underwater robotic vehicles (AURVs). A nonlinear change of coordinates and feedback is made such that the SGA algorithm developed for time-invariant nonlinear systems can be implemented to the formation control system under consideration in this paper. The formation-keeping performance is significantly improved by solving the associated Hamilton-Jacobi-Isaacs (HJI) equation with the SGA algorithm. The synthesized formationkeeping controller also has optimal and robust properties in comparison with the original control law designed for the formation system by using Lyapunov s direct method. Simulation results are presented to demonstrate the improved formationkeeping performance of a leader-follower formation of AURVs in nonholonomic chained form. Index Terms Successive Galerkin approximation, Hamilton- Jacobi-Isaacs (HJI) equation, formation control, autonomous underwater vehicles (AUVs), performance improvement. I. INTRODUCTION The formation control of multiple autonomous vehicles has received considerable attention during the last decade [] [5]. Application areas of formation control include unmanned aerial vehicles (UAVs), mobile robots, marine craft, and autonomous underwater vehicles (AUVs), etc. Among these applications formation control of multiple AUVs including AURVs has attracted special research interests in recent years. Multiple AURV systems have many scientific, military, and commercial applications due to their long-endurance, tolerant, and cooperative capabilities. The significant examples for demonstrating the applications of multiple AURV systems include distributed wide-area ocean exploration, large-scale multi-sensor survey, cooperatively handling of large object, and multi-site inspections. Although many new design methods for formation control of multiple autonomous vehicles have been developed in recent years, the formation control performance under the controllers designed via current design methods cannot be guaranteed in practice, due to the lack of efficient approaches to improving the performance of formation system. For example, the aim of most of control design methods is to achieve asymptotical stability. Hence, the transient error and oscillation of state variables will always be present. This phenomenon could make the formation system under a higher risk of collisions between the participating vehicles during their regulations. In [6] an iterative learning strategy was proposed for the transient performance improvement of model reference adaptive control. However, it was developed for continuous-time single-input single-output (SISO) linear time-invariant systems. There is no answer on how this learning strategy can be extended to multiple-input multiple-output (MIMO) nonlinear systems. The SGA approach has been applied to a wide variety of optimal control problems of individual vehicle systems including missiles and underwater robotic vehicles [7] [9]. However, it is still not clear whether the SGA approach can be directly extended to nonlinear formation control of multiple AURVs or not. The objective of this paper is to present the application of the SGA approach to the nonlinear formation control of a class of AURVs which can be described by a driftless chained form. By solving the generalized HJI equation the performance of the formation control system is expected to be improved with respect to its original control laws. Currently, the SGA approach only applies to time-invariant, nonlinear affine control systems. Since most of the formation systems are essentially time-varying, there is a difficulty if the SGA approach is directly applied to these applications. To solve this problem, a nonlinear change of coordinates and feedback for the original formation system under consideration is adopted in this study such that the popular SGA approach can be applied to improving the formation performance. II. PRELIMINARIES A. L 2 Gain Index Consider the nonlinear system ẋ = f(x) + g(x)u + k(x)w () y = h(x), x() = x where x R n and u R m are the state and control respectively. y R q is the output and w R p is the disturbance. Let L 2 (,T ) represent the set of measurable functions (x) from (,T ) to R such that T (x) 2 dx < +. If for all T and w L 2 (,T ) the following inequation T ( y(t) 2 + u(t) 2 R ) dt 2 T w(t) 2 P dt (2) is satisfied, then it is said that system () has L 2 gain less than or equal to. In (2) u(t) 2 R and w(t) 2 P are defined by X/5/$2. 25 IEEE 89

2 u(t) T Ru(t) and w(t) T Pw(t), respectively. The matrices R and P are positive definite. B. HJI Equation The Hamilton-Jacobi-Isaacs (HJI) equation is defined by V T f + ht h + V T 4 ( kp k T gr g T ) V 2 2 = (3) with the boundary condition V () =. (3) is a first order, nonlinear partial differential equation (PDE). Like the Hamilton-Jacobi-Bellman (HJB) equation, the HJI equation is also extremely difficult to solve in general. C. Generalized HJI Equation To reduce the HJI equation to an infinite sequence of linear partial differential equations, the Generalized Hamilton- Jacobi-Isaacs (GHJI) equation is usually used. The GHJI equation is formulated as follows V T (f + gu + kw) +ht h + u 2 R 2 w 2 P = (4) where u and w are known functions of x. Like the HJI equation, the GHJI equation is also very hard to solve analytically for a general nonlinear control system. III. STATEMENT OF THE CONTROL PROBLEM Consider a leader-follower formation of a pair of AURVs. It is assumed that the motion of the follower is governed by the following four-input driftless nonlinear control system ż f = u f, ż 2f = u 2f ż 3f = z 2f u f, ż 4f = u 3f ż 5f = z 4f u f, ż 6f = u 4f (5) where z f =(z f,,z 6f ) is the state, u f,u 2f,u 3f, and u 4f are the associated control inputs and denoted by u f. The trajectory of the leader z l =(z l,,z 6l ) is assumed to be generated by the following equations ż l = u l +ũ l, ż 2l = u 2l +ũ 2l ż 3l = z 2l (u l +ũ l ), ż 4l = u 3l +ũ 3l ż 5l = z 4l (u l +ũ l ), ż 6l = u 4lf +ũ 4l (6) where u l = (u l,u 2l,u 3l,u 4l ) is the measured or estimated control input of the leader, ũ l = (ũ l,ũ 2l,ũ 3l,ũ 4l ) is the disturbance arising from the measurement or estimation of the leader s motion in practice. The relative formation-keeping error of trajectories between the follower and the leader is denoted by z e := z f z l d, where d R 6 is the desired constant separation. By noting that d and ż e = ż f ż l, the formation dynamics can be directly derived as follows ż e = u f u l ũ l, ż 2e = u 2f u 2l ũ 2l ż 3e = z 2e u l + z 2f (u f u l ũ l ) ż 4e = u 3f u 3l ũ 3l ż 5e = z 4e (u l ũ l )+z 4f (u f u l ũ l ) ż 6e = u 4f u 4l (7) The problem on improving the formation-keeping performance of the leader-follower formation system (7) can be stated as: Given an initial, asymptotically stabilizing, feedback control law u () f for the nominal formation system of (7) (i.e., ũ l =), how can the formation-keeping performance of this control be significantly improved with respect to a specified formation performance index? In this study the specified performance index relates to find, if it exists, the smallest and the associated control law u f, such that system (7) has L 2 gain less or equal to for any >. IV. SGA APPROACH TO SOLVING THE HJI EQUATION To solve the HJB and HJI equations Galerkin-based approximations have been widely adopted. In [] Beard and his colleagues proposed a SGA approach to solving the HJI equation. It consists of two basic steps. First, Bellman s idea of iteration in policy space is used to reduce the HJI equation to a sequence of linear PDEs termed by the generalized HJI (GHJI) equations. Then, the SGA method with a proper selection of basis functions is applied to approximate each GHJI equation. System () will have L 2 gain less than or equal to ( > ) if u (x) = 2 R g T (x) V (8) x where V > is a smooth solution to the HJI equation (3). It has been shown [] that for > > the HJI equation has a continuously differentiable solution V, where is some lower bound of >. However there does not exist any solution if <. This fact was exploited by Beard in [] to develop his SGA algorithm. Beard s SGA algorithm starts with a known control u () that is asymptotically stable for the nominal system of () over a bounded domain Ω of state space. There are two simultaneous iterations of successive approximation in this algorithm. The first successive approximation is to compute the worst-case disturbance corresponding to the initial control. The second successive approximation is then used to find the control which gives the best response to the worst-case disturbance. Combining these two successive approximations yields the following algorithm for approximating the HJI equation []: ) Let Ω be the stability region of the initial control u (). Start the successive approximations from the initial control. 2) For i= to a) Set w (i,) =. b) For j =to i) Solve for V (i,j) from V (i,j)t + u (i) (f + gu(i) + kw(i,j) ) + h T h 2 R 2 w (i,j) 2 P = (9) 89

3 ii) Update the disturbance: c) End d) Update the control: w (i,j+) = 2 2 P k u (i+) = 2 R g (i,j) T V (i, ) T V () () 3) End It has been proven in [] that V (i,j) (x) V (i,j+) (x) V (i, ) (x), and V (i, ) (x) V pointwise on Ω. The key to the successive approximations outlined above is to find an efficient numerical solution which repeatedly solves the GHJI equation for each iteration. Toward this end, a computation Galerkin method is used by Beard and McLain []. Let { j (x)} N j denote the set of basis functions. In the stability region Ω the value function V (i,j) is approximated by V (i,j),n (x) = N Substituting (2) into (9) gives ( ) N e (i,j) (x) = k c(i,j) k ψ T j k c (i,j) k j(x) (2) V (i,j)t [f + gu(i) + kw (i,j) ] + h T h + u (i) 2 R 2 w (i,j) 2 P (3) where e (i,j) is the error resulted from approximating V (i,j) with V (i,j),n. The unknown coefficients c(i,j) k are found by solving the following equations: <e (i,j) (x), j (x) > =, k=,,n (4) where < > denotes the inner product of two functions and defined by Ω e(i,j) (x) j(x)dx. On the details of Beard s SGA approach, see []. For this approach the following advantages are particularly highlighted: It is iterated from a known initial stabilizing control until to reach a satisfactory performance. Thus, there is a strong relation between the design methods and the synthesized optimal control laws. The stability region Ω of the initial control explicitly determines the region of convergence for the approximate control. Moreover, the stability region of the approximate control is equal to the region of convergence. Therefore, the SGA algorithm has guaranteed stability for the solution obtained through successive approximations. The synthesized control laws resulted from the finite truncations can approximate the true optimal and robust solution of the HJI equation arbitrarily closely. The on-line computational burden only consists of assembling the linear combinations of state-dependent basis functions, though a large number of off-line computations are needed. Although there are many advantages as pointed out above, it is still hard to directly apply the SGA algorithm to formation system (7). On the one hand, the SGA algorithm requires an initial, asymptotically stabilizing control law. Generally speaking, it is not an easy task to design such a control law for specific nonlinear control systems, especially for nonholonomic systems [], [2]. On the other hand, the SGA algorithm only applies to time-invariant, nonlinear affine control systems with f() = and h() =. However, the formation system (7) is a time-varying, non-affine control system in essence. When x =it does not necessarily result in f() =. V. ASYMPTOTICALLY STABILIZING CONTROL LAW FOR FORMATION-KEEPING To apply the SGA approach mentioned in the last section to the formation system (7), the first thing is to make the system applicable to the SGA algorithm. Then, an asymptotically stabilizing control law needs to be designed for the nominal system of (7). This section first presents how system (7) can meet the requirements of the aforementioned SGA approach by exploiting a nonlinear change of coordinates and feedback, and then gives an initial stabilizing control law by taking advantage of Lyapunov s direct method. A. Model Transformation Denote x= (x,,x 6 ) R 6. A change of coordinates is defined by the mapping (z e ):R 6 R 6 x = z 5e (z 4e + z 4l )z e,x 2 = z 3e (z 2e + z 2l )z e x 3 = z 6e, x 4 = z 4e x 5 = z 2e, x 6 = z e (5) In the new coordinates x= (x,,x 6 ), system (7) is transformed into the following convenient form ẋ = u l x 4 u 3l x 6 + w x 4 u 3 x 6 ẋ 2 = u l x 5 u 2l x 6 + w x 5 u 2 x 6 ẋ 3 = u 4 w 4, ẋ 4 = u 3 w 3 ẋ 5 = u 2 w 2, ẋ 6 = u w (6) where u = u f u l, u 2 = u 2f u 2l, u 3 = u 3f u 3l, and u 4 = u 4f u 4l. The disturbance w= (w,w 2,w 3,w 4 ) denotes (ũ l,ũ 2l,ũ 3l,ũ 4l ). By comparing (6) with (), the definitions of f(x), g(x), and k(x) can be easily inferred. B. Initial Controller Design The new form (6) of system (7) greatly facilitates the design of control. Particularly, the Lyapunov s direct method can be directly applied to the controller design of the nominal system of (6) (namely, w =). Toward this end, consider a candidate Lyapunov function as follows V (x) = 2 x x x x x x2 6 (7) in which > and 2 >. If it is assumed that z 2l, z 4l, and u l are bounded over [,+ ], then under the continuous, 892

4 .5 x x 2.5 x 4 x 5 Z z l ω zl ω yl y l x l ω xl v xl x 3 x 6 Leader State State c l Control u u 2 Control u 3 u 4 O Y z f ω zf ω yf c f v xf x f ω xf Follower X Fig.. Illustrating the performance issues of control (8) state-feedback controller of the following form u = [ 2 (u 2 + u 2l ) x 2 + (u 3 + u 3l x ] k x 6 u 2 = 2 u l x 2 k 2 x 5 u 3 = u l x k 3 x 4 u 4 = k 4 x 3 (8) from any initial error x ()= (z e ()), all the solutions of the closed-loop system (6) and (8) are uniformly bounded. Where, >, 2 >,k >,k 2 >,k 3 >, and k 4 >. It should be noted that V (x) is only a positive semidefinite function under (8). However, the asymptotical convergence of control law (8) can be guaranteed by Barbălat s lemma and its extension [3] if u l does not converge to zero. C. Performance Issues Although the control law in (8) is asymptotically stable for the nominal system of (6), it does not necessarily result in the guaranteed robustness for any disturbance w. Since any optimal control problem has not been addressed during the design of control (8), the optimal performance of closed-loop control system cannot be guaranteed. Moreover, the control becomes more difficult to tune because there are many control parameters to be chosen. Tuning these parameters may result in unexpected effects on the states and outputs of the formation system. Another issue is that the control in (8) only focuses on the asymptotical stability of the closed-loop system. As a result, the transient responses of the system are not good. In particular there is always an oscillating error, as shown in Fig.. Generally speaking, this oscillating phenomenon cannot be eliminated by manually tuning the control parameters of (8). Therefore, an effectively tuning approach is expected to improve the overall performance of the closed-loop formation system. VI. APPLICATION OF THE SGA APPROACH TO FORMATION-KEEPING CONTROL In this section we present the application of the SGA approach to the formation-keeping control of AURVs where Fig. 2. Schematic drawing of the Leader-Follower formation of AURVs each AURV has two nonholonomic motion constraints [3]. The nonholonomic systems are those with nonintegrable constraints. According to the famous Brockett theorem, controlling a nonholonomic system is an extremely challenging issue [2]. For the formation control of multiple nonholonomic vehicles there have been a large number of novel nonlinear methods. However, there has been no systematic approach to its performance improvement. To the best knowledge of the authors, this study is the first time that the SGA approach is used to improve the formation performance of nonholonomic AURVs. A schematic representation of the leader-follower formation of AURVs is depicted in Fig. 2. The inertial coordinate system I is denoted by {O,X,Y,Z}, and the body coordinate system B i (i = l,f) is given by {c i,x i,y i,z i }. The kinematic motion of the AURV iis described by R i = R i S i (ω i ), p i = R i v i (9) where R i = {r i,jk } =(n i,s i,a i ) SO(3) (j,k =,2,3) represents the orthogonal rotation matrix from frame I to B i, n i,s i,a i R 3 are the orthogonal column vectors in R i. S i ( ) is the associated skew-symmetric matrix defined by a i b i =S i (a i )b i for any vectors a i,b i R 3. ω i = ( xi, yi, zi ) T is the angular velocity of AURV iin frame B i. p i = (x i,y i,z i ) T denotes the position of AURV i in frame I. v i =(v xi,,) T is the velocity in frame B i. We choose a unit quaternion vector q i =( i,ɛ i ) to parameterize the rotation matrix R i SO(3). The unit quaternion vector q i is defined by i =( i, 2i, 3i ) T = k i sin( i 2 ), i =cos( i 2 ) (2) with 2 i + 2 2i + 2 3i + 2 i = (2) It is directly checked that system (9) can be transformed into the chained form like (5) by the following local change of coordinates and feedback: z i = x i, z 2i = r i,2, z 3i = y i r i, z 4i = r i,3, z 5i = z i, z 6i = r i,32 r i,23 (22) r i, +tr(r i ) u i = r i, v xi, u 2i =ż 2i, u 3i =ż 4i, u 4i =ż 6i 893

5 If i > and r i, (,) (,] hold true, the actual inputs v xi, xi, yi, and zi can be computed in terms of the interim control variables u i, u 2i, u 3i, and u 4i as follows: v xi = u i r i, xi = [( + tr(r i )) u 4i r i,2 +r i, yi r i,3 zi ] yi = r i, r i,23 u 2i r i, r i,33 u 3i zi = r i, r i,22 u 2i + r i, r i,32 u 3i (23) For more details on the above model and the transformation of its coordinates, see [3] and references therein. The nonlinear and robust controller synthesis presented here by using the SGA approach considers uncertainties in control inputs u l, u 2l, u 3l, and u 4l. In practice these disturbances may be resulted from the measurement and/or estimation of the motion of the leader AURV. Observing (22) and (5) we can choose x,x 2, and x 6 as the outputs of interest for the formation system. The weighting matrices R and P in the HJI and GHJI equation can be freely determined by the designer. Another problem remains unsolved is that how to make a right choice of the basis functions of Galerkin approximation. Indeed, this is a very critical part in the applications of the SGA approach. The basis functions used in the approximation not only determine the accuracy of the SGA approach, but also the computation cost since the computational burden is about (nm N 3 ) [], [4], where N is the number of basis functions, n is the size of state space, and M is the mesh size of each axis. To make a tradeoff between the approximation accuracy and computational burden, in this study we first select the least set of basis functions by observing the initial control law (8) and the structure of system (6) such that the basis functions are capable of capturing the nonlinearities of the system and approximating the Lyapunov function (7) and its derivative. Then, the size of basis functions is increased slowly in the late trials by analyzing the approximation results of previous computations. This process is repeated until a satisfactory performance is reached. Having made a proper selection of basis functions, we can now apply the SGA algorithm to formation system (6) with the model described above for improving the performance of an given initial control law thereafter. VII. SIMULATION RESULTS In this section, we carried out several simulations to illustrate the improved performance resulted from the application of the SGA approach to the formation control of nonholonomic AURVs in nonlinear chained form. The velocity of the leader AURV, u l, was set to be (.5,.,.,.2). The disturbance ũ l was simulated by generating the normally distributed noise with a zero mean and a standard deviation =.5. The design parameters of the initial control and other initial conditions were picked as k =.6,k 2 =.5,k 3 =.4,k 4 =.3, =. 2 =.,d =(5.,.,5.,.,5.,.)m (24) z e () =(.5,.6,.6,.8,.5,.5) TABLE I BASIS FUNCTIONS AND COEFFICIENTS j ψ j c j j ψ j c j j ψ j c j x x x 2 x 2 4 x x x x 3 x x 3 4 x x x 2 x 2 x x 2 x 5 x x x x x 2 2 x x x 2 x 5 x x 2 x x 3 2 x x 2 2 x 5 x x x 2 x 4 x x x 4 x 5 x x x x x 2 x 4 x x x 4 x 5 x x 2 x x 2 2 x 4 x x 2 4 x 5 x x 4 x x x 2 4 x x x 2 5 x x x 2 x 2 4 x x 2 x 2 5 x x x x 3 4 x x 4 x 2 5 x x 2 x x 2 x2 5 3 x 4 x x x 2 x x 5 x x 2 2 x2 5 5 x x x 4 x x x 2 x 4 x x 3 x x 2 4 x2 5 8 x 2 x x x x x x 2 x x 3 5 x x 2 x x x 2 x x 2 2 x x x 4 x x 2 x 4 x x 2 4 x x x 5 x x x 4 x x 2 x 5 x x 3 x x x 4 x 5 x x 2 x 2 x x 3 x x 2 5 x x x 2 2 x x 2 x 2 x x x x 3 x x x 2 2 x x 2 x x 2 x x 3 x x 4 x x x 2 x x 2 x 4 x x 5 x x 2 2 x x x 2 x 4 x x x x x 2 2 x 4 x x x 2 x x x 2 4 x x The stability region Ω was set to be [ x i ](i =, 6) for reducing the off-line computational burden. Both the weighting matrices Q and R were taken as identical matrices with appropriate dimensions. Table I lists the sets of basis functions used in this paper and the coefficients resulted from the SGA algorithm. The resulting control is determined by ( u,n = N 2 gt (x) j= ψ j ) (25) where g(x) can be easily inferred from (6). For solving the associated HJI equation we mainly utilized the Matlab toolbox provided in [5]. However, we made a strong revision to enhance its computation efficiency. The formation trajectory in x coordinates was plotted in Fig. 3. Fig. 4 shows the formation trajectory in z e coordinates. Both Fig. 3 and 4 indicate that the transient responses of the controls obtained from the SGA approach are significantly improved in comparison with the original control law. The original control just guaranteed that the responses of the system were asymptotically stable, but it did not say anything about how the behavior of the system would be as time increased. The L 2 gain index N of the closed-loop system is.95. The robustness of the closed-loop system under the SGA-based control is also enhanced, which can be particularly observed from Fig. 4. The time histories of the control variables are shown in Fig. 5, indicating that the controls obtained from the SGA approach are more robust than the original controls. Additionally, the system displays an exponential-like behavior which is particularly interesting for the close formation of AURVs and other autonomous vehicles because the exponential-like stability can efficiently reduce the risk of the collisions between the participating followers. 894

6 x x u u x x x 5 z e z 3e z 5e (e) Fig. 3. x (f) Formation trajectory in x coordinates (e) Fig. 4. z 2e z 4e z 6e (f) Formation trajectory in z e coordinates VIII. CONCLUSIONS In this paper the SGA approach has been applied to the nonlinear formation control for a class of AURVs which can be modeled by four-input driftless nonlinear chained systems. To make the SGA approach, which is developed for time-invariant nonlinear control systems, applicable to the essentially timevarying formation control problem, a nonlinear change of coordinates and feedback has been introduced first. The nonlinear optimal and robust controls are then synthesized by solving the associated HJI equation with the SGA algorithm. There are several advantages of the SGA applications in the formation control of AURVs. First, the performance, particularly the transient behavior of the system under the synthesized control has been significantly improved. Second, an exponential-like asymptotical stability can be approximately achieved if the order of approximation is large enough. It is very advantageous to the close formation of AURVs for reducing the risk of the collisions between the participating followers. Third, the resulting controls are still in feedback closed-loop form and easily implemented to on-line applications. Finally, the controls achieved by solving the HJI equation are robust in essence. For demonstrating the wonderfully improved performance of the formation control of AURVs, several simulation results have also been provided in this paper. u Fig. 5. u Formation control inputs ACKNOWLEDGMENT This research is funded by the Engineering and Physical Sciences Research Council (EPSRC) under grant GR/S4558/. REFERENCES [] H. G. Tanner, G. J. Pappas, and V. Kumar, Leader-to-Formation stability, IEEE Transactions on Robotics and Automation, vol. 2, no. 3, pp , June 24. [2] J. P. Desai, J. P. Ostrowski, and V. Kumar, Modeling and control of formations of nonholonomic mobile robots, IEEE Transactions on Robotics and Automation, vol. 7, no. 6, pp , December 2. [3] M. Egerstedt and X. Hu, Formation constrained multi-agent control, IEEE Transactions on Robotics and Automation, vol. 7, no. 6, pp , December 2. [4] I. F. Ihle, R. Skjetne, and T. I. Fossen, Nonlinear formation control of marine craft with experimental results, in Proceedings of the 43rd IEEE Conference on Decision and Control, Atlantis, Paradise Island, Bahamas, December , pp [5] P. Ögren, M. Egerstedt, and X. Hu, A control Lyapunov function approach to multiagent coordination, IEEE Transactions on Robotics and Automation, vol. 8, no. 5, pp , October 22. [6] A. Tayebi, Transient performance improvement in model reference adaptive control via iterative learning, in Proceedings of the 43rd IEEE Conference on Decision and Control, Atlantis, Paradise Island, Bahamas, December , pp [7] J. Lawton and R. W. Beard, Successive Galerkin approximation of nonlinear optimal attitude control, in Proceedings of the 999 American Control Conference, San Diego, June 999, pp [8] T. W. McLain and R. W. Beard, Nonlinear robust missile autopilot design using successive Galerkin approximation, in Proceedings of the AIAA Guidance, Navigation, and Control Conference, Portland, OR, 999, pp , AIAA [9], Successive Galerkin approximations to the nonlinear optimal control of an underwater robotic vehicle, in Proceedings of the 998 International Conference on Robotics and Automation, Leuven, Belgium, May 998, pp [] R. W. Beard and T. W. McLain, Successive Galerkin approximation algorithms for nonlinear optimal and robust control, International Journal of Control, vol. 7, no. 5, pp , 998. [] H. K. Khalil, Nonlinear Systems, 2nd ed. New Jersey: Prentice-Hall, Inc., 996. [2] I. Kolmanovsky and N. H. McClamroch, Developments in nonholonomic control problems, IEEE Control Systems Magazine, vol. 5, no. 6, pp. 2 36, 995. [3] E. Yang, T. Ikeda, and T. Mita, Nonlinear tracking control of a nonholonomic fish robot in chained form, in Proceedings of the SICE Annual Conference 23 (SICE 23), Fukui, Japan, August , pp [4] R. W. Beard, G. N. Saridis, and J. T. Wen, Improving the performance of stabilizing controls for nonlinear systems, IEEE Control Systems Magazine, vol. 6, no. 5, pp , 996. [5] R. Beard, HJtools: a Matlab toolbox. [Online]. Available: http: // docs/research sga.html 895