Trajectory Tracking Control of Underwater Vehicle
in Horizontal Motion
ZYGMUNT KITOWSKI, JERZY GARUS
Department of Mechanical and Electrical Engineering
Naval University
81-103 Gdynia, ul. Smidowicza 69
POLAND
Abstract: - The paper addresses sliding mode control for underwater vehicle and its relationship with dynamical inversion control. For the tracking of desired trajectory, the way-point line of sight scheme is incorporated and three independent controllers are used to generate command signals. Quality of control is concerned without and in presence of external disturbances. Some computer simulations are provided to demonstrate effectiveness, correctness and robustness of the approach.
Key-Words: - Underwater vehicle, autopilot, sliding mode control, dynamical inversion control
1Introduction
The steering and manoeuvring performance of underwater vehicles have been treated in various papers [1,3,4,5,7,8,12]. Modelling of vehicle dynamics with taking into consideration all real world conditions of operation is a complicated process due to problems with evaluation of coefficients of state equations, description of forces operating on the vehicle and environmental perturbations. The full model of motion of the marine vehicle in 6 degrees of freedom (DOF) is required only if a displacement in any direction and violent changes of a trajectory are considered. In case when stationary operating conditions are considered, such as motion and stabilisation of the vehicle on demanded depth, direction, constant distance to or above the target, etc. the simplified model may be applied.
The paper consists of the following six sections. A brief description of the equations of motion for underwater vehicles is presented in Section 2. In Section 3 the algorithm of control of the vehicle
in 3-dimensional space is presented and relationship between sliding mode control and dynamical inversion control is examined. Coordination systems and problem of tracking of desired trajectory are discussed in Section 4. Section 5 provides results of simulation study. The conclusions are given in Section 6.
2Dynamics of underwater vehicle
The general motion of marine vehicle in 6 DOF can be described by the following vectors [1,4]:
(1)
where:
– the position and orientation vector with coordinates in the earth-fixed frame;
v – the linear and angular velocity vector with coordinates in the body-fixed frame;
– describes the forces and moments acting on the vehicle in the body-fixed frame.
The nonlinear dynamic equations of motion can be expressed as [3]:
(2)
where:
M - inertia matrix (including added mass);
C(v) - matrix of Coriolis and centripetal terms (including added mass);
D(v) – hydrodynamic damping and lift matrix;
g() - vector of gravitational forces and moments;
J() – velocity transformation matrix between vehicle and earth fixed frames.
3Control of vehicle in six degrees of freedom
The algorithm of driving of the vehicle along the desired trajectory in 6 DOF has been worked out under assumptions:
- presented in (2) nonlinear model of dynamics can be divided into six non-coupling subsystems, everyone in form as follows [4,12]:
(3)
where , a ;
- vehicle’s position and orientation in the earth-fixed frame is defined by the reference trajectory ;
- the states of the model are measurable.
Let us denote by:
(4)
(5)
respectively a control error and a sliding surface with exponential dynamics . The problem of tracking can be regarded as a solved problem if the control error e remains on the surface s, or keeping the scalar quantity for [4,5,7].
Proposed control law is composed of two parts:
(6)
where:
- nominal part,
- discontinuous part.
By using the fact that the desired position and orientation of the vehicle is given a priori by the reference trajectory d, as the nominal part may be applied dynamical inversion control law [10,11]. The basic idea in dynamic inversion control is to calculate, on basis of the predefined trajectory, the command signals that assure proper behaviour of the vehicle. Under above assumption for vehicle’s model (3), the nominal part can be determined by:
(7)
The discontinuous part of control law (6) is obtained by using a Lyapunov-like function candidate:
(8)
According to Lyapunov stability theory
(9)
implies that the control system is asymptotically stable [4,7]. A sufficient condition to satisfy the above expression is to choose such that true is the following inequality:
(10)
Hence, differentiating (4)
(11)
and substituting to (10) yields:
(12)
Transforming (3) to the form , and using the above formulae takes the form:
(13)
When (7) is applied to (13) and dividing by s yields:
(14)
Since, after replacing by (7) the following expression for is obtained:
(15)
After differentiating (4) and combining with (15) the following relationship is determined:
(16)
Finally, denoted by and substituting (5) into (14) the following controller equation is obtained:
(17)
where and are constant values.
4Coordinate systems and tracking control law
For conventionally of underwater vehicles basic motion is movement in horizontal plane with some variation due to diving. Hence they operate in crab-wise manner in 4 DOF with small roll and pitch angles that can be neglected during normal operations. Therefore, it is purposeful to regard
3-dimensional motion of the vehicle as superposition of two displacements: motion in the horizontal plane and motion in the vertical plane.
The main task of the designed tracking control system is to minimize distance of attitude of the vehicle’s centre of gravity to desired trajectory under assumptions:
- vehicle can move at varying: linear velocities u, , w and angular velocity about z-axis r;
- trajectory is given by means of set of points .
It is convenient to define three coordinate systems when analysing a route tracking systems for the marine vehicle in horizontal plane (see Fig. 1) [6]:
- the global coordinate systemOXY (called also the earth-fixed frame);
- the local coordinate systemO0X0Y0 (fixed to the body of the vehicle);
- the reference coordinate systemOrXrYr (system is not fixed).
Fig.1. Coordinate systems used to description of track-keeping for the underwater vehicle in horizontal motion: OXY – earth-fixed system, O0X0Y0– body-fixed system, OrXrYr– reference system.
Let us assume that the vehicle’s route is composed of some straight lines defined by turning points A, B, C, etc., with coordinates , , , etc. The tracking error of the vehicle is defined in the reference coordinate system OrXrYr, (in Fig. 1 it is equal AXr1Yr1 due to the first waypoint is regarded), as the perpendicular distance l of the vehicle located in position to the predefined trajectory. According to this, the two components of the vehicle’s position in the global coordinate system can be expressed as:
(18)
where:
[xA,yA]T – global coordinates of the point A;
A – rotation of the reference coordinate system with respect to the global one:
; (19)
- local coordinate of the vehicle, determined by using expression:
(20)
where:
- local heading angle defined as the angle between the track reference line and the vehicle’s centreline;
- the vehicle’s heading;
– increment of the vehicle’s local coordinates in t=kt;
uk, k – linear speeds in t=kt;
t – quantization step;
k = 0,1,2,… .
Each time the vehicle location at the time t satisfies:
(21)
where 0 is a circle of acceptance, the next waypoint should be selected on a basis of the reference coordinate system (e.g. BXr2Yr2) and the vehicle’s position updated corresponding to the new reference coordinate system.
5Simulations
A simulation study of a track-keeping has been performed for a remotely operated vehicle called Ukwial designed and built for the Polish Navy. The ROV is an open frame robot controllable in 4DOF, being 1.5 m long and having a propulsion system consisting of six thrusters. Displacement in horizontal plane is done by means of four thrusters which generate force up to 750 N assuring speed up to 1.2 m/s and 0.6 m/s consequently in x and y direction. Control system consists of 3 independent controllers producing command signals ,andcalculated from the hybrid control law (17). The inertia and damping matrices of the simplified dynamics model (3) for the Ukwial are given in Appendix A.
Numerical simulations have been made to confirm validity of the proposed control algorithm for the following assumptions:
- the vehicle has to follow the desired trajectory beginning from (10m,10m), passing target waypoints (60m,10m), (100m,60m), (100m,80m), (40 m, 80 m), (10 m, 40 m) and coming back to the start;
- the turning point is reached when the vehicle is inside of the two-metre circle of acceptance 0;
- initial conditions are the same and equal .
Tracking control simulation result and the courses of command signals for no added environmental disturbances are depicted respectively in Fig. 2, Fig. 3 and Fig. 4. The real trajectory is almost totally as the desired one. Also a quality of course-keeping control is satisfactory. On about 750 seconds the last point is reached and all inputs become equal zero. Figures from 5 to 7 show tracking results and the courses of command signals for a sea current disturbance. From these figures there is seen the influence of external disturbances on the vehicle’s trajectory. Although the total time is longer and the errors of position and course are much bigger than in the previous case the autopilot is able to cope with external disturbances and reach the turning points with commanded orientation.
Fig. 2. Simulation results of track-keeping without environmental disturbances.
Fig. 3. A quality of course-keeping control without environmental disturbances: headings (upper plot) and error of heading (low plot).
Fig. 4. Time histories of command signals for track keeping without environmental disturbances.
The case study showed that the proposed autopilot consisting of three sliding mode controllers with nominal part based on dynamical inversion control enhanced good tracking control of the desired route. The main advantage of the approach is using the simple nonlinear model of the vehicle’s dynamics to design controllers and its high performance for relative large constant sea current disturbances (comparable with velocity of the vehicle).
Fig. 5. The influence of sea current on track-keeping (speed 0.5 m/s and direction 1350).
From the results it can be deduced that the quality of control can be improved, especially in presence of external disturbances. It may be achieved by using a such values of the parameters and in the control law (17) that assures minimum of the performance index J, e.g. in form:
(22)
where:
l - perpendicular distance of the vehicle to the desired trajectory.
Evaluation of the parameters may be done by means of numerous optimisation techniques, classical or modern ones, like Genetic Algorithms [2,9]. Therefore further investigations are required.
Fig. 6. A quality of course-keeping control under interaction of sea current (speed 0.5 m/s and direction 1350): headings (upper plot) and error of heading (low plot).
Fig. 7. Time histories of command signals for track keeping under interaction of sea current (speed 0.5 m/s and direction 1350).
5Conclusion
In this paper the nonlinear waypoint tracking autopilot combining slide mode control and dynamical inversion control for underwater vehicles has been described. The nonlinear model of the Polish ROV UKWIAL was applied for carried out computer simulations. The obtained results with the control system design method based on three decoupling controllers showed the presented autopilot to be simple and useful for a practical usage. One of the main advantages of the proposed solution is its flexibility with regard to the vehicle’s model and the performance index. Disturbances from sea current were added to verify the performance, correctness and robustness of the approach.
References:
[1]R. Bhattacharyya, Dynamics of Marine Vehicles, John Wiley and Sons Ltd., 1978.
[2]A. Boryson, Dynamic Optimization, Adison Wesley, 1999.
[3]J. Craven, R. Sutton, R.S. Burns, Control Strategies for Unmanned Underwater Vehicles, Journal of Navigation, No.51,1998, pp. 79-105.
[4]T.I.Fossen, Guidance and Control of Ocean Vehicles, John Wiley and Sons, 1994.
[5]J. Garus, Z. Kitowski, Sliding control of motion of underwater vehicle, Marine Technology III, Computational Mechanics Publications, Southampton, 1999, pp. 603-611.
[6]A.D. Hansen, Predictive Control and Identification: Application to Steering Dynamic, Ph.D. Dissertation, Technical University of Denmark, Department of Mathematical Modelling, 1996.
[7]A.J. Healey, D. Lienard, Multivariable Sliding Mode Control for Autonomous Diving and Steering of Unmanned Underwater Vehicles, IEEE Journal of Ocean Engineering, No. 18 (3),1993, pp. 327-339.
[8]Z. Kitowski, J. Garus, Control of Motion of Underwater Robot under Conditions of External Disturbances,Proc. of the 7th Int.. Conf. on Advanced Robotics ICAR’95, Sant Feliu de Guixols, 1995, pp. 278-282.
[9]Y. Li, D.J. Murray-Smith, G.J. Gray, K.C. Sharman, Genetic Algorithm Automated Approach to Design of Sliding Mode Controller Systems, International Journal of Control, No. 63 (4), 1996, pp. 721-739.
[10]J.E. Slotine, W. Li, Applied Nonlinear Control, Prentice–Hall Int., 1991.
[11]M.W. Spong, R. Ortega, On Adaptive Inverse Dynamics Control, IEEE Transaction of Automatic Control, No. 1 (35), 1990, pp. 92-95.
[12]D.R. Yoerger, J.E. Slotine, Robust Trajectory Control of Underwater Vehicles, IEEE Journal of Oceanic Engineering, No. 10 (4),1985, pp. 462-470.
APPENDIX A
Parameters of the ROV Simplified Model
The inertia and damping matrices of the simplified dynamics model (3) of the Remotely Operate Vehicle UKWIAL used in simulation study: