Optimization techniques for autonomous underwater vehicles: a practical point of view

Optimization techniques for autonomous underwater vehicles: a practical point of view M. Chyba, T. Haberkorn Department of Mathematics University of Hawaii, Honolulu, HI 96822 Email: mchyba@math.hawaii.edu, haberkor@math.hawaii.edu Abstract The main focus of this paper is to design time efficient trajectories for underwater vehicles. The equations of motion are derived from Lagrangian mechanics and we assume bounds on the controls. We develop a numerical algorithm based on the time optimal trajectories. Our goal is to design controls that are piecewise constants with a small number of switchings and such that the duration for the vehicle to achieve the trajectory differs from the time optimal one by a number negligible in terms of the application. I. INTRODUCTION In the exploration of our world s oceans, there comes a time when human controlled vehicles are not up to the task. Whether it be deep ocean survey, or long term monitoring projects, Autonomous Underwater Vehicles AUV s) are the natural alternative. One of the essential components for an underwater vehicle to be autonomous is the ability to design its own trajectories. It is known as the motion planning problem. Moreover, the goal is to find trajectories that are efficient with respect to some criteria. Indeed, we may want our vehicle to stay down longer for monitoring, or finish the search faster to save money. All of these questions can be addressed through optimal control. Much research has been allocated to AUV propulsion, robustness, sensors, and power sources. With the plethora of sizes and shapes combined with the youth of the AUV field, and the complexity of hydrodynamics, accurate models of AUV s are hard to come by. Thus, among all AUV research areas, the area of optimal control is one of the least developed. Almost all existing controllers have the disadvantage that they do not take into account minimization criteria such as time and energy. In this paper, we address the problem of finding the time minimal trajectories for underwater vehicles; it is an optimal control problem OCP). Previous results were obtained in [2], [3], [4], [], [6], [7] about the structure of time minimal trajectories and the role of the singular trajectories. However, due to the complexity of the equations we need to supplement our theoretical study with numeric computations. It is the subject of this paper. A first way to obtain numerically the desired trajectories is to discretized our OCP and to solve the resulting optimization problem whose unknowns are the discretized state and control of the system. However, the obtained solutions are Research supported in part by NSF grant DMS-3641 usually not feasible in terms of practical implementation since their control structures have a lot of switchings. Another drawback of this method is that the resulting optimization problem is time consuming to solve since it needs a large number of unknowns in order to have the desired accuracy. For these reasons, we propose another approach that consists in looking for control strategies that are time optimal with respect to a given switching structure. To accomplish that goal, we fix the number of switching times and rewrite the OCP as an optimization problem whose unknowns are the switching times and the value of the constant arcs for each component of the control. This method has two main advantages. The first one is that such optimization problem has a small dimension and that we obtain a much better accuracy since we can use high order integration scheme to integrate the dynamical system of the model. The second, and main, advantage is that the obtained control strategies are easier to implement than the ones resulting from the solving of the discretized OCP on a real model. Indeed, the former ones are just concatenation of constant arcs for the control and we can even fix a relatively small number of switching times so that real thrusters can follow these strategies more efficiently. Furthermore, it is also possible to add some smoothing at the switching times so that the junctions between constant thrust arcs become more regular. II. MODEL In this section, using Lagrangian mechanics, we derive the equations of motion for marine vehicles in 6 degrees of freedom in the body-fixed frame. Let us first introduce some notations. We denote by η = x, y, z, φ, θ, ψ) t the position and orientation of the vehicle with respect to the earth-fixed reference frame, the coordinates φ, θ, ψ being the Euler angles for the body frame. The coordinates corresponding to translational and rotational velocities in the body frame are ν = u, v, w, p, q, r) t. See Figure 1. We define η 1 = x, y, z) t, η 2 = φ, θ, ψ) t and ν 1 = u, v, w) t, ν 2 = p, q, r) t. We then have the following relation η = Jη)ν 1) where J represents the linear and angular velocity transformations. Let L = T V be the Lagrangian, with

z u p r w q v and with the following control vector fields: Gχ) = M 1 ) 1) In the sequel we will call χ the state of the system while the variable η represents a configuration of the system. y x Fig. 1. Earth-fixed and body-fixed reference frame. T = T RB + T A ; respectively the rigid-body kinetic energy and the fluid kinetic energy. The potential V is defined implicitly by V η = J 1 η)gη) 2) where gη) represents the restoring forces gravitational forces and moments). Let us recall first the quasi-lagrange equations a generalization of Kirchoff s equations when considering a non zero potential): d dt L ) + ν 2 L J T 1 η 2 ) L η 1 = τ 1 3) d dt L ) + ν 2 L + ν 1 L J2 T η 2 ) L = τ 2 4) ν 2 ν 2 η 2 where τ = τ 1, τ 2 ) is the vector of control inputs. Let us determine more precisely these equations. Since T η =, we have by construction that Jη) L η = gη). Moreover, we have that L ν = Mν, d L dt ν = M ν. Defining the Coriolis and Centripetal matrix as ν 2 T ) Cν)ν = ν 2 T ν 2 + ν 1 T we can rewrite equations 3) and 4) as M ν + Cν)ν + Dν)ν + gη) = τ ) where the term Dν)ν accounts for the damping forces. We obtained: ν = M 1 C + D)ν M 1 g + M 1 τ 6) η = Jη)ν 7) where M is the inertia matrix, C the coriolis and centripetal matrix, D the damping forces and the variable τ represents the control. See [8] for instance for more details. We can rewrite equations 6) and 7) as an affine control system: χt) = fχt)) + Gχt))ut), χt) IR 12. 8) where χ = η, ν), the drift is ) Jη)ν fχ) = M 1 C + D)ν M 1 g 9) Constraints on the control The controls represent the thrust, and we assume the following bounds: U = {τ IR 6 ; α i τ i β i, α i < < β i, i = 1,, 6}. 11) A control is said to be admissible if it is a measurable bounded function τ such that τt) U for a.e. t. Let us now describe the parameters used for the numerical computations. The location of the center of buoyancy and gravity are respectively: x B, y B, z B ) =,, 8.1e 4) meters) and x G, y G, z G ) =,,.64e 4) meters). The inertia matrix, M, takes the following form: 3/2 m mz G 3/2 m mz G 3/2 m mz G I x I xy I xz mz G I xy I y I yz I xz I yz I z 12) where m = 12.991 kg is the mass of the AUV, and I. and I.. are inertia factors I x = 6.8 kg.m, I y = 7.2 kg.m, I z = 9.1 kg.m, I yz = I xz = I yz = ). The restoring forces and moment vector gη) is taken to be: gη) = W g B) sin θ W g + B) cos θ sin φ W g + B) cos θ cos φ z G W g z B B) cos θ sin φ z G W g z B B) sin θ where W g is the weight of the vehicle, and B = W g + 38 Newton) is the buoyancy force on the vehicle. The damping forces Dν) are of the form diagx u + X uu u, Y v + Y vv v, Z w + Z ww w, K p + K pp p, M q + M qq q, N r + N rr r ) where X u, X uu ) =, 1)), Y v, Y vv ) =, 1)), and Z w, Z ww ) =, 1)) are the drag coefficients for pure surge, sway, and heave, respectively. And, K p, K pp ) =, 1)), M q, M qq ) =, 1)), N r, N rr ) =, 1)) are the drag coefficients for pure roll, pitch, and yaw, respectively. Finally, the coriolis matrix C is:

m z G r m w Z wd w C 4,3 m w + Z wd w m z G r C,3 m v Y vd v m u + X ud u m m z G r) m w) Z wd w C 1,6 Z wd w m w m z G r m u X ud u C 3,4 C 3, m C 4, C 4,6 C,4 C,6 C 6,4 C 6, Where X ud = Y vd = Z wd = m/2 and K pd = M qd = N rd = and the coefficients C.,. are: C 1,6 = m z G p v) + Y vd v C 3,4 = m v z G p) Y vd v C 3, = X ud u m u + z G q) C 4,3 = m z G p v) + Y vd v C 4, = I z r I yz q I xz p N rd r, C 4,6 = I yz r + I xy p I y q + M qd q C,3 = m u + z G q) X ud u C,4 = I yz q + I xz p I z r + N rd r, C,6 = I xz r I xy q + I x p K pd p C 6,4 = I y q I yz r I xy p M qd q, C 6, = I xy q I x p I xz r + K pd p The domain of control is assumed to be U = [ 2, 2] 2 [ 2, ] [, ] 3 Newton. III. MAXIMUM PRINCIPLE Let χ, χ T be initial and final states. Assume that there exists an admissible time-optimal control τ : [, T ] U such that the corresponding trajectory χ.) solution of 8) steers the system from χ to χ T. The maximum principle, [1], implies that there exists an absolutely continuous vector λ : [, T ] IR 12, λt) for all t, such that the following conditions hold almost everywhere: χ j = H λ j χ, λ, τ), for j = 1,, 12, where Hχ, λ, τ) = λ t fχ) + λj = H χ j χ, λ, τ) 13) 6 λ t g i χ)τ i i=1 is the Hamiltonian function where the g i are the column vectors of G. Furthermore, the maximum condition holds: Hχt), λt), τt)) = max γ U Hχt), λt), γ) 14) Moreover, the maximum of the Hamiltonian is constant along the solutions of 13) and must satisfy Hχt), λt), τt)) = λ, λ. A triple χ, λ, τ) that satisfies the maximum principle is called an extremal, and the vector function λ.) is called the adjoint vector. The maximum condition 14), along with the control domain 11), is equivalent to almost everywhere: τ i t) = α i if ϕ i t) < and τ i t) = β i if ϕ i t) > 1) for i = 1,, 6, where ϕ i.) are called the switching functions. The switching functions are absolutely continuous functions defined as follows i = 1,, 6): ϕ i : [, T ] IR, ϕ i t) = λ t t)g i χt)) 16) According to 1), the zeroes of the switching functions define the structure of the time-optimal control τ.). If ϕ i t) is zero on a nontrivial subinterval of [, T ], the corresponding extremal is called τ i -singular. The component τ i is then called singular on the corresponding subinterval. The maximum principle implies that if ϕ i t) for almost all t [, T ] the component τ i of the control is bang-bang, which means that τ i only takes values in {α i, β i } for almost every t [, T ]. A time t s, T ) connecting a singular and a bang arc, or two bang arcs with different values) is called a switching time. To study the zeroes of the switching functions and the existence of singular extremals we analyze their derivatives. It follows from ϕ i t) = λ t t)g i, that almost everywhere in t the following is verified: n ϕ i = λ t [f, g i ]χ) λ t [g j, g i ]χ)τ j. 17) j=1 Since the control vector fields are given by the inverse of the inertia matrix, their are constant. A consequence is that ϕ i is an absolutely continuous function given by: ϕ i t) = λ t t)[f, g i ]χt)). 18) Notice that it is a property shared by the set of controlled mechanical system of the form kinetic minus potential energy. Using equations 9) and 1) we have that [f, g i ] = Jη)M 1 i ν M 1 C + D)ν)M 1 i ) 19) where M 1 i denotes the ith column of the inverse of the inertia matrix. The second derivative is given by: n ϕ i = λ t ad 2 f g i + λ t [g j, [f, g i ]]χ)τ j 2) j=1 where ad 2 f g i stands for [f, [f, g i ]]. Notice that since the first n components of the vector field [f, g i ] do not depend on ν but only on η, we have that the first n components of [g j, [f, g i ]] are zeroes. As a consequence, in the case we make the assumption that the damping matrix is linear in ν, we have that the vector fields [g j, [f, g i ]] can be written as a linear combination of the g i. Unless we make additional assumptions on the matrices involved in the equations of motion of the vehicle, which

would result in an unrealistic model, the complexity of the equations is such that an analytical study is very difficult. Partial results can be derived but we cannot expect general results such as a uniform bounds on the number of switchings. Moreover, our major goal is the implementation of our techniques to an existing vessel. We then have to take into account practical constraints such as the capacity of the thrusters to follow a prescribed control and the ability of the vehicle to realize a given path. It is a well known fact that optimal trajectories can involve singular trajectories or have infinitively many switchings. In our problem, the complexity of the optimal structure has been outlined already on a simplified model where we constructed a semi-camonical form for our system in a neigborhood of singular trajectories and deduce the existence of chattering extremals, see [2]. It is a consequence of the fact that a τ i -singular extremal, with i = 4, or 6 is of order 2. It means that the lowest order derivative d2q dt ϕ 2q i in which τ i appears explicitely with a nonzero coefficient is given by 2q = 4. Hence, to compute a singular control τ i, we must differentiate the corresponding switching function ϕ i 4 times and solve this final equation for the control. Such a control τ i is then computed as a feedback of the state χ, of the adjoint vector λ and of the other component of the control assuming they are constants). For this reason, we developed a numerical methods taking into account the criterion to minimize as well as the ability of the vehicle and the thrusters to follow a prescribed path. IV. OPTIMIZATION METHOD They are two kind of methods to solve numerically an optimal control problem: the indirect and the direct. Due to the form of our problem, the use of indirect methods is very complicated without knowing the structure of the solution in advance. We then use a direct method. It consists in discretizing OCP ) in order to transform it into a nonlinear optimization problem. We call this new problem N LP ). The unknowns of NLP ) are the discretized states and controls. Its nonlinear constraints come from the discretization according to a fixed step integration scheme, here a Heun one, of the dynamic 8). Additional constraints are the ones on the final state and the upper and lower bounds on the controls. To solve NLP ), we write it using the AMPL modeling language, see [9], and we apply the large-scale nonlinear solver IpOpt, see [13]. The solving usually takes around 3 min, counting refining steps, for a time grid of 4 points so approximatively 72 unknowns and 48 nonlinear constraints). Notice that a solution of N LP ) is only an approximation of a solution of OCP ) since the fixed step integration scheme used to discretized the dynamic introduces inaccuracy but mostly because the control is piecewise constant due to the discretization of the problem. To increase the accuracy, we need to refine the time discretization grid which also increases the number of unknowns of NLP ). A direct effect of a larger number of unknowns is an increase in the execution time needed to solve NLP ), and ultimately a failure to solve it without important computational resources. Figure 2 shows an example of a solution of NLP ) for χ =,,, ) and χ T =, 3,,, ) for the model described previously. The evolution of the corresponding controls are represented on Figure 3. The time for this τ 1 τ 2 τ 3 z m) θ rad) ψ rad) τ 4 τ τ 6 x m) 2 4 6 8 1 12 2 2 4 6 8 1 12. y m) 4. 2 4 6 8 1 12 1 φ rad) 1 2 4 6 8 1 12.. 2 4 6 8 1 12. 1 2 4 6 8 1 12 2 u m/s) v m/s) w m/s) p rad/s) q rad/s) r rad/s). 2 4 6 8 1 12 1. 2 4 6 8 1 12.. 2 4 6 8 1 12.1.1 2 4 6 8 1 12.1.1 2 4 6 8 1 12.1.1 2 4 6 8 1 12 Fig. 2. Time optimal trajectory for NLP ) 2 2 4 6 8 1 12 2 2 2 4 6 8 1 12 2 4 6 8 1 12 2 4 6 8 1 12 2 4 6 8 1 12 2 4 6 8 1 12 Fig. 3. Time optimal control strategy for NLP ) trajectory is t f min 12.441 s. Looking at the Figure 3, it is obvious that the control strategy is inappropriate

to a practical implementation on real vehicle for at least two reasons. The first one comes from the existence of singular arcs for the component τ 6. Along these singular arcs, the control varies continuously which is impossible to implement. The second reason is that there is a large number of switching times. In practice we observe delays with the thrusters to respond to a given command, as a consequence we want to keep the number of switchings small. Moreover, the fact that the trajectory is computed through a fixed step integration make it less accurate to the theoretical model than a proper high-order, adaptative step integration. REMARK 4.1: The reason to use a high order scheme to improve the accuracy of the solution of the N LP ) problem is that we already have a poor knowledge of the hydrodynamics coefficients which introduces uncertainties in the experiments, hence we would like to minimize any other source of potential difference between the theoretical computations and the experiments. Inspired from the work in [1], [12], we develop another approach to overcome the main issues of the previous solving method. In [12], the authors use the discretized solution of an optimal control problem to extract the switching structure of the optimal control. The next step is to rewrite the optimal control problem as a nonlinear optimization problem whose unknowns are the switching times or more precisely the time length between two switching times). They are then able to integrate their dynamical system with a high order integrator. The motivation for their approach is the verification of second order sufficient conditions for optimality. This method cannot be applied to our problem. The reason is that along time minimal trajectories we have the existence of singular arcs. As mentioned before, along these arcs the singular control can be computed as a feedback of the state and of the adjoint vector, contrary to [12] where the singular control is a feedback of the state only. Moreover, the switching structure of the solutions of NLP ) is not always easy to extract, see Figure 3. Our motivation is different, we want to develop an algorithm to compute trajectories that are efficient in time but mostly feasible by the vehicle. Hence, our goal is, based on our previous computations for the N LP ) problem, compute trajectories that are close in time from the optimal one but that are such that their implementation on a vessel is possible. To reach our goal, we rewrite OCP ) as an optimization problem but without taking into account the switching structure given by the solution of NLP ). We fix the number of switching times, preferably to a small number. The new optimization problem, called ST P P ) p for Switching Time Parameterization Problem), has then for unknowns the time arclengths between two switching times plus the time arclength between the last switch and the final time) but also the values of the constant thrust arcs. ST P P ) p has the following form: min t p+1 z D t = t i+1 = t i + ξ i, i = 1,, p χ i+1 = χ i + t i+1 t i χt, τ i )dt 21) χ p+1 = χ T z = ξ 1,, ξ p+1, τ 1,, τ p+1 ) D = IR np+1) + U p+1 where ξ i, i = 1,, p + 1 are the time arclengths and τ i U, i = 1,, p + 1 are the values of the constant thrust arcs. χt, τ i ) is the right hand side of the dynamical system 8) with the constant control τ i. The integration of the dynamical system of ST P P ) p which is simply 8)) can be done with a high order adaptative step integrator. We use DOP83, see [11]. ST P P ) p has a much smaller number of unknowns than NLP ) and so, even if the integration takes more time because it is more accurate, the computational resources needed to solve ST P P ) p are drastically reduced. To solve it we use once more IpOpt, see [13], which yields very good results. V. NUMERICAL RESULTS In this section we describe the results using the method described in the previous section. One of the main advantage of ST P P ) p is that we are usually able to fix a small p, that is a small number of switching times. However, notice that a solution of ST P P ) p will only be optimal with respect to the fixed number of switching times. So it won t be a solution of OCP ), but only an admissible control and trajectory that steers the system from χ to χ T. Figure shows a control strategy solution of ST P P ) 3 with the same terminal configurations as for Figure 2, i.e. χ =,,, ) and χ T =, 3,,, ). We impose the total number of switching times along the trajectory to be 3. The final time corresponding to the solution shown on Figure is t f 12.814 s. This final time has to be compared to the one of a solution of NLP ), Figure 2, which is t f min 12.441 s. We see that the difference is neglectable providing that the obtained longer control strategy is easier than the time-optimal one. A solution of ST P P ) 1 yields a t f 17.44 s and a solution of ST P P ) 2 yields t f 13.28 s. Note that along the solution of ST P P ) 3 the control strategy is not bang-bang. Indeed, since the values of the control between two switching times belong to the unknowns of the problem, there is no guarantee that the thurst of the vehicle will be saturating all the time, i.e. take their values in {α i, β i }. VI. CONCLUSION In this paper, we focus on a practical implementation of time optimal trajectories for an underwater vehicle. Time optimal trajectories are often difficult to implement due to their structure, moreover in practice we are not interested

z m) φ rad) x m) y m) 1 4 2 1 1. 1.. 1.4 θ rad) ψ rad) τ 1 τ 2 τ 3 τ 4 τ τ 6.2 1. 1 1 2 u m/s) v m/s). 1.. 1.4 w m/s) φ rad/s) θ rad/s) ψ m/s).2 1.1.1 1.1.1 1.1.1 1 Fig. 4. Trajectory corresponding to ST P P ) 3. 2 2 4 6 8 1 12 2 2 2 4 6 8 1 12 2 4 6 8 1 12 2 4 6 8 1 12 2 4 6 8 1 12 2 4 6 8 1 12 Fig.. Control strategy corresponding to ST P P ) 3. in the time optimal trajectory but in a trajectory efficient in time and easily feasible by the vehicle. Our method allows us to restrict the control strategy to piecewise constant functions with a small total number of switching times. The essential idea for the algorithm to work was to introduce the value of constant arcs for the control has unknowns in our optimization problem. The results exceeded our expectations. Computations on the model presented in this paper have showned that usually no more than switching times are necessary to obtain a solution with a final time very close to the optimal one. We are currently testing our algorithms on an underwater vehicle, the results will be presented in a forthcoming article. Notice that some additional issues may have to be taken into account. Indeed, underwater propeller, like any other engines, are not able to accomplish instantaneous switchings. However, since the structure of ST P P ) p allows flexibility to additional constraints, we can also adapt the switching time parameterization in order to impose continuous junctions between the constant arcs for the control. For instance, one can introduce a linear junction. REFERENCES [1] B. Bonnard and M. Chyba. Singular Trajectories and their Role in Control Theory. Springer-Verlag, 23. [2] M. Chyba and T. Haberkorn. Autonomous Underwater Vehicles: Singular extremals and chattering. Proceedings of the 22nd IFIP TC 7 Conference on System Modeling and Optimization, Italy, 18-22 July 2. [3] M. Chyba and T. Haberkorn. Designing efficient trajectories for underwater vehicles using geometric control theory. Proceedings of the 24rd International Conference on Offshore Mechanics and Arctic Engineering, Greece, 12-17 June 2. [4] M. Chyba, N.E. Leonard and E.D. Sontag. Singular trajectories in the multi-input time-optimal problem: Application to controlled mechanical systems. Journal on Dynamical and Control Systems 91):73-88, 23. [] M. Chyba, N.E. Leonard and E.D. Sontag. Optimality for underwater vehicles. In Proceedings of the 4th IEEE Conf. on Decision and Control, Orlando, 21. [6] M. Chyba. Underwater vehicles: a surprising non time-optimal path.in 42th IEEE Conf. on Decision and Control, Maui 23. [7] M.Chyba, H. Maurer, H.J. Sussmann and G. Vossen. Underwater Vehicles: The Minimum Time Problem. In Proceedings of the 43th IEEE Conf. on Decision and Control, Bahamas, 24. [8] T.I.Fossen. Guidance and control of ocean vehicles. Wiley, New York, 1994 [9] R. Fourer, D.M. Gay and B.W. Kernighan. AMPL: A Modeling Language for Mathematical Programming. Duxbury Press, Brooks- Cole Publishing Company,1993. [1] C.Y. Kaya and J.L. Noakes. Computation method for time-optimal switching control. Journal of Optimization Theory and Applications, 1171):69-92, 23. [11] E. Hairer, S.P. Norsett and G. Wanner. Solving ordinary differential equations I. Nonstiff problems. 2nd edition.springer series in computational mathematics, Springer Verlag, 1993. [12] H. Maurer, C. Büskens, J-H.R. Kim and I.C. Kaya. Optimization methods for numerical verification of second order sufficient conditions for bang-bang controls. Optimal Control Application and Methods, 26:129-16, 2. [13] A. Waechter and L. T. Biegler. On the Implementation of an Interior- Point Filter-Line Search Algorithm for Large-Scale Nonlinear Programming. Research Report RC 23149, IBM T.J. Watson Research Center, Yorktown, New-York.