A Time-Varying Lookahead Distance Guidance Law for Path Following

Transcription

1 A Time-Varying Lookahead Distance Guidance Law for Path Following Anastasios M. Lekkas Thor I. Fossen Centre for Ships and Ocean Structures Norwegian University of Science and Technology, NO-7491, Trondheim, Norway ( Department of Engineering Cybernetics Norwegian University of Science and Technology, NO-7491, Trondheim, Norway ( Abstract: This paper presents a modified version of a well known line-of-sight (LOS) guidance algorithm for path-following. The proposed method employs a time-varying equation for the lookahead distance which depends on the cross-track error. A sliding mode controller is designed in order to stabilize the vehicle heading angle to the desired value specified by the guidance system. The guidance system along with the controller form a cascaded structure which is shown to be globally κ-exponentially stable when the control task is to converge to a straight line. The effectiveness of the proposed strategy is demonstrated by simulations indicating that the variable lookahead distance algorithm can contribute to obtaining a diminished oscillatory behavior around the desired path. Keywords: Nonlinear systems, interconnected systems, line-of-sight guidance, path following 1. INTRODUCTION Guidance systems are concerned with the transient motion behavior associated with the achievement of motion control objectives (see Breivik and Fossen 009) and consequently are of critical importance for the overall performance of both marine and flight vehicles. As mentioned in Fossen (011), path-following is one of the typical control scenarios in the control literature and it pertains to following a predefined path independent of time, i.e., without placing any restrictions on the temporal propagation along the path. Readers interested in motion control scenarios and guidance laws may refer to Yanushevsky (011) and Breivik (010). The problem of guiding an underwater vehicle with a LOS algorithm and designing a sliding mode controller for stabilizing the combined speed, steering and diving response was addressed in Healey and Lienard (1993). In Breivik and Fossen (004), a guidance-based pathfollowing scheme which is singularity-free for all regular paths was presented and Uniform Global Asymptotic Stability (UGAS) and Uniformly Local Exponential Stability (ULES) was proven by using cascade theory. UGAS and ULES is equivalent to global κ-exponential stability, a concept which was presented in Sørdalen and Egeland (1995). A cascaded systems approach was also developed by Børhaug and Pettersen (005) where the guidance system is interconnected with a sliding mode controller in order to achieve global κ-exponential stability to straight lines in 3-D space. All the aforementioned approaches considered a constant lookahead distance. The importance of using a variable lookahead distance was demonstrated in the κ-exponentially stable guidance laws which were developed at a kinematics level and proposed in Breivik and Fossen (005). The work of Pavlov et al. (009) presented a UGAS nonlinear Model Predictive Control (MPC) approach where the lookahead distance,, was optimized in order to achieve a combination of fast convergence and small overshoot compared to the constant lookahead LOS algorithms. In Oh and Sun (010), the LOS guidance parameter was embedded in the linear MPC controller design as as additional decision variable and the simulations indicated a smoother convergence to a straight line compared to the linear MPC controller with a fixed lookahead distance. The present work points toward a different direction by proposing a time-varying lookahead distance dependent on the cross-track error. This results in lower values for (and thus a more agile and aggressive response) when the vehicle is far from the desired path, and greater values when the vehicle is closer to the path and less abrupt behavior is needed so as to avoid oscillating around the path. The minimum and maximum values for can be determined by the user and should depend on the maneuvering characteristics of the vehicle. A globally exponentially stable (GES) sliding mode controller is designed for stabilizing the yaw angle of the vehicle and acts as the perturbing system in the cascade system that it forms along with a well-known line-of-sight guidance law (Papoulias, 1991) which now incorporates the time-varying lookahead distance equation. The cascade is shown to be globally κ-exponentially stable when the task is to converge to a straight line. Simulations of the proposed method indicate that smooth convergence, without oscillations around the desired path, is achieved. Similarly to Pavlov et al. (009) and Børhaug

2 p k x e y e k U x b LOS vector (x los,y los ) p k+1 p n we are dealing with only the path-following task, which does not impose temporal restrictions, and therefore the speed control problem will not be addressed. It should be noted that the kinematics in (1) (3) inherently assumes a scenario where no external disturbances are involved, hence the linear velocity is aligned with the heading vector. Furthermore, the goal of this paper is to compare the performance of a time-varying lookahead distance LOS guidance with respect to the conventional (constant lookahead distance) LOS guidance, consequently issues like measurement noise and observer design will not be considered here. x n y n p Fig. 1. Depiction of the basic geometry of LOS guidance and some of the main variables that are involved in the problem. and Pettersen (005), the combination of UGAS and ULES stability results in a robust behavior against modeling uncertainties or unmeasured external forces and disturbances. Moreover, the method does not increase the demand for computational power, contrary to what might be the case with an MPC approach. The rest of this paper is organized as follows: In Section, the kinematics of the vehicle as well as the control objective are given. Section 3 presents the time-varying equation and gives the stability proof of the LOS algorithm. A sliding mode controller for stabilizing the heading angle is designed in Section 4. The systems interconnection and the cascade structure is studied in Section 5. In Section 6, the proposed method is tested via simulations and, finally, Section 7 gives an overview of the paper and discusses future work.. VEHICLE MODEL AND CONTROL OBJECTIVE.1 Vehicle model In this paper we consider a marine craft moving at a constant forward speed 0 < U U max such that the surge velocity u U and the sway velocity υ 0 (for more details see Fossen 011). The three degrees-of-freedom (DOF) differential kinematics is described by the following equations: ẋ = U cos ψ, (1) ẏ = U sin ψ, () ψ = r, (3) where x and y represent the craft position, ψ is the yaw relative to an Earth-fixed reference frame and r the yaw rate of the craft. The craft is underactuated since two out of three DOF s can be controlled independently, namely the yaw angle and the surge velocity. In this work y b. Control objective Similarly to Breivik and Fossen (004), we define a local reference frame at p d (x, y) and name it the Path Parallel (PP) frame. The PP frame is rotated an angle α k relative to the inertial frame. In fact, since the desired trajectory is a straight line and not a curved path we have that α k is a constant. The PP frame is tangent to desired geometrical path and therefore p d is the desired vehicle position. The error vector between the vehicle position p and the desired position p d expressed in the PP frame is given by: where R T p = ε = R T p (α k )(p p d ), (4) [ ] cos(αk ) sin (α k ) sin (α k ) cos (α k ) SO() (5) is the rotation matrix from the inertial frame to the PP frame. The error vector ε = [x e, y e ] T includes the along-track error x e and the cross-track error y e, these variables are depicted in Fig. 1. We will investigate the stabilization of the cross-track error which represents the lateral distance to the path-tangent: y e (t) = [x(t) x k ] sin(a k ) + [y(t) y k ] cos(α k ), (6) with the associated control objective for straight-line path following being: lim y e(t) = 0. (7) t + Note that in the case where temporal constraints are needed (for example in a path-tracking scenario) then it is necessary to include the along-track error dynamics in our study as well. 3. TIME-VARYING LOOKAHEAD DISTANCE GUIDANCE LAW The LOS-based guidance (Papoulias, 1991) is employed in order to solve the cross-track error minimization problem. This is achieved by choosing the desired heading as ψ d (y e ) = ψ p + ψ r (y e ), (8) where ψ p = α k, (9) is the path-tangential angle, and ψ r (y e ) = arctan ( y e / ) (10) is the velocity-path relative angle, which ensures that the velocity is directed toward a point on the path that is located a lookahead distance > 0 ahead of the direct projection of p(t) on to the path.

3 We propose to use the following equation for the lookahead distance: (y e ) = ( max min )e γ ye + min. (11) where min and max are the minimum and maximum allowed values for respectively and, along with the convergence rate γ > 0, are important design parameters. The idea behind (11) is rather simple and it can be summarized by the fact that it assigns a small value when the craft is far from the desired path, (thus resulting in a more aggressive behavior that tends to decrease the cross-track error faster) and a large value for when the craft is close to the path and overshooting needs to be avoided. The concepts far and close with respect to the desired path are relative and several constraints should be taken into account when determining min, max and γ, such as the maneuvering characteristics of the vehicle. For the sake of notational brevity we assign r := max min and := (y e ) for the rest of this paper. The time derivative of (6) is ẏ e = ẋ sin (α k ) + ẏ cos (α k ), (1) = U cos (ψ) sin (α k ) + U sin (ψ) cos (α k ), (13) = U sin (ψ α k ). (14) Assuming that the desired heading is perfectly tracked at all times and choosing ψ d = ψ r + α k gives ẏ e = Uy e y e +. (15) The aforementioned assumption is not an oversimplification because the overall system (i.e. the guidance system and the heading controller) will be analyzed as a cascade structure where (14) constitutes the nominal system Σ 1 and the heading error dynamics constitutes the perturbing (or driving) system Σ. As a consequence, the stability analysis will show whether the time that the controller needs in order to converge can have a destabilizing effect on the guidance system. This will be further explained later on. We choose the positive definite and radially unbounded Lyapunov function candidate (LFC) V 1 = (1/)y e and differentiate it with respect to time along the trajectories of the cross-track error: V 1 = Uy e y e +, (16) which is negative definite for non-zero speeds. Hence, the origin y e = 0 is a UGAS equilibrium of the nominal system Σ 1. Moreover, on the ball D = {y e R y e φ}, φ > 0, we have that V 1 = Uy e φ + ky e, (17) for some 0 < U/( φ + ), which entails that the origin is a ULES equilibrium. The combination of UGAS and ULES implies global κ-exponential stability, according to Lefeber (000). 4. AUTOPILOT DESIGN In this section, the stability of the heading dynamics with a sliding mode controller is investigated. The nonlinear extension of Nomoto s 1st-order model (Norrbin, 1963) is considered: T ṙ + H N (r) = Kδ, (18) H N (r) = n 3 r 3 + n r + n 1 r + n 0, (19) where r is the yaw rate, δ the rudder control input, T and K the Nomoto time and gain constants respectively, and H N (r) is a nonlinear function describing the maneuvering characteristics of the ship. For a course stable ship n 1 > 0, T, K > 0 and assuming symmetry in the hull implies n = 0. The bias term n 0 can be treated as an additional rudder offset in the case where a constant rudder angle is required to compensate for constant steady-state wind, wave drift and current forces. Consequently, a large number of ships can be described by the equation: ṙ + α 1 r 3 + α r = δ, (0) where α 1 = n 3 /T > 0, α = n 1 /T > 0 and K/T = b > 0. Equivalently, in state space form: ṙ = α 1 r 3 α r + bδ, (1) ψ = r. () This is a simplification compared to a real ship. In practice, the rudder input often does not affect the dynamics linearly and the Nomoto coefficients change as a function of the forward ship speed. Equations (1) () constitute the perturbing system Σ. In order to control the system (1) (), we will construct a sliding mode controller. The goal is to stabilize the heading at the desired value ψ d = ψ r + α k. Consequently, r d = ψ d, = ( ye ), + ye y e = ( + ye). (3) γ r y e e γ ye We define the sliding surface s := r + λ ψ. (4) Next, we propose the LFC V = (1/)s and by differentiating along the trajectories of s, we get V = s( α 1 r 3 α r + bδ ṙ d + λ r). (5) Consequently, the control law δ = (1/b)(α 1 r 3 + α r + ṙ d λ r k d s) (6) with k d > 0 gives V = k d s and therefore the equilibrium point s = 0 is GES. From this result it follows that ψ = 0, r = 0 are also GES equilibria, see Utkin (1977), DeCarlo et al. (1988). This result inherently assumes an unconstrained rudder input, which is not possible in practice. 5. INTERCONNECTED SYSTEM AND STABILITY OF THE CASCADE As it was mentioned previously, the two nonlinear systems (15) and (1) () are interconnected and form a cascade structure. The driving system is the sliding mode controller since the convergence to the desired heading ψ d affects the stability of the guidance system via the state ψ = ψ ψ d. However, the guidance system perturbs the yaw control system as well, not only via the desired heading ψ d but also due to the fact that the cross-track error appears in the desired heading rate equation. This implies that apart from the three assumptions that need to be satisfied in order to infer upon the stability of the

4 s y e ṡ = f c (s, t) ẏ e = f g (t, y e )+g(y e,s) s ṡ = f c (s, y e ) ẏ e = f g (t, y e )+g(y e,s) Fig.. Equivalence between the closed-loop system and the cascade structure. cascade system (for the theoretical background and the proofs of the related theorems the reader is referred to Panteley and Loría 1998), it is necessary to prove that the system is forward complete. A system is called forward complete if for every initial condition ξ and every input signal u, the corresponding solution is defined for all t 0, i.e. σ max ξ,u = + (Angeli and Sontag, 1999). We achieve this in a similar manner as in Loría et al. (000), but before proceeding with the proof we compute the interconnecting term: g(y e, s) = U[sin (ψ α k ) sin (ψ d α k )] = U sin ( ψ/) cos (α k (ψ + ψ d )/). (7) This equation shows how the heading error dynamics acts and prevents U sin (ψ α k ) from becoming equal to U sin (ψ d α k ) instantaneously (perfect heading tracking). 5.1 Forward completeness of the closed-loop system Equation (7) allows the two systems (15) and (1) () to be rewritten as follows: Σ 1 : ẏ e = f g (t, y e ) + g(y e, s), (8) Σ : ṡ = f c (s, y e ), (9) where f g (t, y e ) = U sin (ψ d α k ). By proving that (8) (9) is forward complete we can consider f c (s, y e ) to be a time-varying function so as f c (s, y e ) = f c (s, t): Σ 1 : ẏ e = f g (t, y e ) + g(y e, s), (30) Σ : ṡ = f c (s, t). (31) Then it is possible to use the theorems from Panteley and Loría (1998). This equivalence is depicted in Fig.. According to Angeli and Sontag (1999), if a system is forward complete, then there exist a nonnegative, radially unbounded, smooth function V : R n R + and a class- K function σ such that: V (x) f(x, u) V (x) + σ( u ) (3) x x R n and u R. In order to show that the system (8) (9) is forward complete we employ the LFC: V fc = (1/)ye + (1/)s, (33) which gives V fc f(x, u) = Uy e x y e + + g(y e, s)y e k d s. (34) y e The first and third term of the right-hand side of the equation are negative. Regarding g(y e, s), from (7) we can write g(y e, s) U max, (35) consequently (34) becomes V fc x f(x, u) V fc + U max y e, (36) therefore the system is forward complete. 5. Stability of the cascade structure We choose the state vector that contains the error states of both the control and guidance systems that form the cascade: x = [y e, ψ, r] T. (37) Hence we continue by stating the following theorem: Theorem 1. The origin x = 0 of the cascade structure (8)-(9) (formed by the perturbing system (1) () and the perturbed system (15)) is globally κ-exponentially stable if the control law is given by (6), and the desired yaw angle is described by (8). Proof. The proof consists of showing that the three assumptions of Theorem 1 in Panteley and Loría (1998) are satisfied. In this paper, however, we will also use the formulation given in Panteley et al. (1998) in order to prove global κ-exponential stability. Assumption A1: We already showed that the equilibrium point y e = 0 is globally κ-exponentially stable when the heading is perfectly tracked. We also have that V 1 = (1/)y e, and: V 1 y e y e = y e y e V 1 y e y e c 1 V 1 (y e ) for c 1 and y e 0. (38) The condition V 1 y e c y e µ (39) is also satisfied y e µ, µ > 0. Assumption A: because of (35). This condition is apparently satisfied Assumption A3: It has already been proved that the equilibrium point s = 0 is GES. This means that if we rewrite the time derivative of the LFC as: V k d s s, (40) then the solutions will satisfy: s(t) λ o s(t o ) e (t to) (41) and therefore by choosing ν( s(t o ) ) = (λ o /) s(t o ) the integrability condition is satisfied. Since all three assumptions are satisfied and, in addition to this, the nominal system Σ 1 has a globally κ-exponentially stable equilibrium and the system Σ has a GES equilibrium, we conclude from Lemma 8 in Panteley et al. (1998) that the cascade system has a globally κ-exponentially stable equilibrium at x = 0.

5 Optimal (m) Speed (m/s) Lookahead distance (m) Fig. 3. Optimal constant lookahead distance for different speeds. Cross track error y e (m ) 3 x Fig. 4. Squared cross-track error comparison between the constant LOS algorithm with 7,opt = 300 m (dashed line) and the proposed method (solid line). 6. SIMULATIONS The proposed guidance law was simulated in Matlab on a 3-DOF model of a Mariner class vessel with length L = m which can be found in the MSS toolbox (Perez et al. 006). We chose the parameters in (11) as min = 00 m, max = m and γ = 1/300. In order to obtain a criterion with respect to which the effectiveness of the proposed method could be tested, we implemented Monte Carlo (MC) simulations where the ship was guided by the LOS algorithm with a constant. The range of the MC simulations was m with a step of 50 m and 5 U 9 m/s with a step of 1 m/s. For each speed, the optimal lookahead was defined as the distance which induced the lowest value for the expression y e dt, the results are presented in Fig. 3. It is reasonable that for higher speeds a greater lookahead distance value will be the optimal one due to the fact that the craft is approaching the target path faster and small lookahead distances will result in a greater overshoot. For U = 7 m/s the optimal lookahead distance was 7,opt = 300 m. This result fits nicely with the fact that in practice is often chosen as = L. The square of the cross track error induced by the two approaches can be seen in Fig. 4. The plot indicates that the constant lookahead method causes the vessel to reach the target line earlier than the proposed method. However, 7,opt = 300 m is responsible for an oscillatory behavior around the path before converging which does not appear in the time-varying case. The suggested method leads the craft in a way such that it meets the path with a small delay, but without overshooting. The explanation for this can be found by observing the time-varying lookahead distance plot in Fig. 5. When the simulation commences, the craft is far away from the target line, hence (11) indicates that a low lookahead distance value will be used. Fig. 5. Time-varying lookahead distance according to the proposed method. ψ d vs ψ (rad) Fig. 6. Actual ship heading (solid line) vs desired (dashed line) for 7,opt = 300 m. ψ d vs ψ (rad) Fig. 7. Actual ship heading (solid line) vs desired (dashed line) for time-varying. The closer the craft gets to the target line, the higher the lookahead distance becomes since a less agile behavior is needed in order to avoid overshooting. The abrupt drop of that occurs at around 140 sec is a corrective action against a small increase of the cross-track error. The reason for this increase is that the heading controller has not converged entirely to the desired angle by the time the vessel meets the desired path (see Fig. 7) and as a result the ship tends to deviate from the path. This corrective behavior indicates that the method is robust with respect to tracking errors. It is worth noting that parameter γ must be chosen carefully in order to get a more efficient. This happens due to the nature of (11), the function will give values only close to min if γ = 1. Figs. 6 and 7 illustrate the desired versus the true craft heading for the constant lookahead distance and the suggested method respectively. As expected, avoiding overshooting results in a smoother heading plot, whereas the conventional method requires that more heading adjustments are to be commanded before the craft has converged to the desired line and, thus, to a constant heading (ψ = ψ d = α k ). The reason for the large difference between the desired and actual heading is that a vessel of this size (and thus yaw inertia) cannot turn instantaneously to the heading indicated by the guidance system. Moreover, the rudder input is saturated and therefore the heading values

6 that are required by the guidance system and are outside the feasible range of the ship will be taken up (or down) to the minimum (or maximum) allowed values. 7. CONCLUSIONS This work dealt with the modification of a well-studied LOS-based guidance law by proposing a time-varying lookahead distance which depends on the cross-track error and a few tuning constants. This contributed to obtaining a more flexible approach since different lookahead distances result in different maneuvering behaviors. It was shown that the two interconnected systems (i.e. the guidance system and the heading autopilot) can be analyzed using cascaded systems theory as long as the overall system is forward complete. The equilibrium point of the overall system was proven to be globally κ-exponentially stable, when the task is to converge to a straight line, and simulations performed on a 3-DOF mariner class vessel demonstrated the effectiveness of the new approach. It is worth noting that although the heading controller of the vessel used in the simulations cannot converge to the desired value exponentially fast (as it was assumed in Section 4) due to, among other things, rudder input constraints, the proposed method gave better results compared to the constant lookahead distance approach. On this first step we used a few simplifying assumptions in order to present a fundamental proof of concept. Future work includes testing and extending the method to curved paths, such as interpolating splines, or paths consisting of concatenations of straight lines and arc segments. Furthermore, path-tracking tasks (where time constraints enter the problem as well and the speed control issue has to be addressed) are a natural continuation of the path-following problem which was tackled in the present paper. Compensating for the effect of environmental forces is also of crucial importance in practically almost every application, and in that case the integral LOS algorithm from Børhaug et al. (008) appears as a good candidate for a satisfactory performance. It is also necessary to develop a method that will help the user decide upon what values to choose for the tuning parameters. Moreover, different functions for have to be tested and their performance be compared to that of the equation proposed in the present paper. REFERENCES Angeli, D. and Sontag, E.D. (1999). Forward completeness, unboundedness observability, and their Lyapunov characterizations. Systems & Control Letters, 38(4), Børhaug, E., Pavlov, A., and Pettersen, K.Y. (008). Integral LOS control for path following of underactuated marine surface vessels in the presence of constant ocean currents. In 47th IEEE Conference on Decision and Control, Cancun, Mexico. Børhaug, E. and Pettersen, K.Y. (005). Cross-track control for underactuated autonomous vehicles. In Proceedings of the 44th IEEE Conference on Decision and Control, and the European Control Conference, Seville, Spain. Breivik, M. (010). Topics in Guided Motion Control of Marine Vehicles. Ph.D. thesis, Norwegian University of Science and Technology. Breivik, M. and Fossen, T.I. (004). Path following for marine surface vessels. In Proceedings of the OTO 04, Kobe, Japan. Breivik, M. and Fossen, T.I. (005). Principles of guidancebased path following in D and 3D. In Proceedings of the 44th IEEE Conference on Decision and Control, and the European Control Conference, Seville, Spain. Breivik, M. and Fossen, T.I. (009). Guidance laws for autonomous underwater vehicles, In A.V. Inzartsev (Ed.), Underwater Vehicles, INTECH Education and Publishing. DeCarlo, R.A., Zak, S.H., and Matthews, G.P. (1988). Variable structure control of nonlinear multivariable systems: A tutorial. Proceedings of the IEEE, 76(3), 1 3. Fossen, T.I. (011). Handbook of Marine Craft Hydrodynamics and Motion Control. John Wiley and Sons Ltd. Healey, A.J. and Lienard, D. (1993). Multivariable sliding mode control for autonomous diving and steering of unmanned underwater vehicles. IEEE Journal of Oceanic Engineering, 18(3), Lefeber, E. (000). Tracking control of nonlinear mechanical systems. Ph.D. thesis, University of Twente. Loría, A., Fossen, T.I., and Panteley, E. (000). A separation principle for dynamic positioning of ships: Theoretical and experimental results. IEEE Transactions on Control Systems Technology, 8(), Norrbin, N. (1963). On the design and analysis of the zig-zag test on base of quasi-linear frequency response. Technical Report B 104-3, The Swedish State Shipbuilding Experimental Tank (SSPA), Gothenburg, Sweden. Oh, S.R. and Sun, J. (010). Path following of underactuated marine surface vessels using line-of-sight based model predictive control. Ocean Engineering, 37(-3), Panteley, E., Lefeber, E., Loría, A., and Nijmeijer, H. (1998). Exponential tracking control of a mobile car using a cascaded approach. In Proceedings of the IFAC Workshop on Motion Control, 1 6. Grenoble, France. Panteley, E. and Loría, A. (1998). On global uniform asymptotic stability of nonlinear time-varying systems in cascade. Systems & Control Letters, 33(), Papoulias, F. (1991). Bifurcation analysis of line of sight vehicle guidance using sliding modes. International Journal of Bifurcation and Chaos, 1(4), Pavlov, A., Nordahl, H., and Breivik, M. (009). MPCbased optimal path following for underactuated vessels. In 8th IFAC International Conference on Manoeuvring and Control of Marine Craft, Guaruja, Brazil. Perez, T., Smogeli, Ø.N., Fossen, T.I., and Sørensen, A.J. (006). An overview of the marine systems simulator (MSS): A Simulink toolbox for marine control systems. Modeling, Identification and Control, 7(4), Sørdalen, O.J. and Egeland, O. (1995). Exponential stabilization of nonholonomic chained systems. IEEE Transactions on Automatic Control, 40(1), Utkin, V. (1977). Variable structure systems with sliding modes. IEEE Transactions on Automatic Control, (), 1. Yanushevsky, R. (011). Guidance of Unmanned Aerial Vehicles. CRC Press.