Speed Control of Autonomous Underwater Vehicle with Constraints Using Model Predictive Control

Transcription

1 INTERNATIONAL JOURNAL OF COASTAL & OFFSHORE ENGINEERING IJCOE No. 2/Summer 2016 (27-33) Speed Control of Autonomous Underwater Vehicle with Constraints Using Model Predictive Control Seyyed Hossein Ghenaati 1, Shahram Aghaei 2 * 1 Electrical and Computer Department, Yazd University, Yazd, Iran; ghenaati@stu.yazd.ac.ir 2 Assistant Professor, Electrical and Computer Department, Yazd University, Yazd, Iran; aghaei@yazd.ac.ir ARTICLE INFO Article History: Received: 8 Feb Accepted: 15 Sep Keywords: Autonomous Underwater Vehicle Model Predictive Control Optimization Constraint Satisfaction ABSTRACT 1. Introduction Autonomous Underwater Robotic Vehicles have become an important tool to explore the secret life undersea. Then researches on AUV systems have been important since two decades and deals with wide variety of fields such as research, inspection, recovery, construction, rescue maneuvering and etc. [1-3]. The first commercial use of underwater vehicles commenced with the discovery of offshore oil and gas fields in North Sea then due to required complex maneuvering, autonomous type of underwater vehicle is becoming more popular as regards it can operate and explore in extreme depth especially is useful for offshore industries. Nonlinear dynamic AUV model is introduced in several references [4-7]. Many scientists have focused on control, stability analysis and optimization concepts. Valentinis et al. [8] used energy based control design strategy and designed feedback control based on port Hamilton theory of motion control. Yushida et al. [9] implemented two air cored electromagnets to maintain the pitching stability. Elnashr [10] used phase plane analysis to study stability of AUV for two main decoupled systems of depth and heading control. Bagnia et al. [11] applied MPC on AUV depth control. Fernandez [12] Nowadays Autonomous Underwater Vehicles (AUVs) are an unavoidable part of marine industries. One of the most important parts of any autonomous vehicle is the control issue to achieve the desired performance. This paper is concerned with speed control of an AUV model respecting the state and control constraints. According to the Newton-Euler method, the 6 DOF kinematic and dynamic models of the AUV are established. A well-defined performance index and constrained finite horizon optimization program in the form of Model Predictive Control (MPC) strategy is proposed to regulate the horizontal speed of AUV to its desired value while the constraints on the states like depth and control signals are considered in finite time horizon optimization program to be satisfied. The main problem for such a situation is the interaction between speed control and depth deviation then quadratic programing technique managed responses to avoid state and control signal constraints. Simulation results show a reliable performance of proposed MPC strategy to control the horizontal speed of AUV while all the constraints on state, control signal and also the variation of the control signal are satisfied. presented an MPC approach to reducing under water robot position error under the influence of water waves performed in two-dimensional coordination. Johansen et al. [13] developed a collision avoidance system using MPC algorithm. Zhao et al. [14] introduced a new approach to real time collision avoidance that complies with COLREGS rules for underwater surface vehicle. Akçakaya et al. [15] designed a real time MPC schedule for set tracking control of variable speed AUV. Franze and lucia [16-17] described discrete time linear time invariant (LTI) model and applied receding horizon control strategy to solve obstacle avoidance motion planning algorithm. In this paper, a well-defined performance index along with a constrained finite horizon optimization program propose an MPC strategy to regulate the horizontal speed of AUV to its desired value while the constraints on the states and control signals are considered in finite time horizon optimization program to be satisfied. The benefit of this work is about analysis of whole model of AUV and lack of necessity for decoupled model brings about all undesirable motions to be considered as feasible constraints therefore this process decreases interactions. In order to reduce modeling error, we 27

2 Seyyed Hossein Ghenaati, Shahram Aghaei / Speed control of Autonomous Underwater Vehicle with constraints using Model Predictive Control linearized system in every discretization step time frequently. The obtained results show that applied MPC strategy on the linearized model of AUV can control the system such a manner that all states achieve desired steady state values while initial states have unfavorable values. The simulation results of the above mentioned idea are presented in this paper while some conditions and properties of the idea such as designing proper terminal constraint and terminal set leading to stability and feasibility are left to next papers, when the research to be completed. This paper has been organized as follows: In the following section, a discussion on modeling the AUV together with constraint structure for speed control program is presented. The next two sections provide details of the proposed MPC scheme and simulation results, respectively. The final section concludes the presented work. 2-Kinematic and dynamic modeling and requisite constraints As shown in "Figure 1", AUV contains sail plane in addition to rudder and elevator planes to comprehensive navigation and main propeller motor motivates vehicle and "Figure 2" represents global and body fixed coordinate with OXYZ and oxyz respectively. Figure 2: Body frame and Global frame illustration [7] According to charted parameters in the "Table 1", 12 nonlinear states contain locations and Euler angles in global frame besides linear and angular velocities from body frame in the form of position and velocity vectors are defined as follows: n 1 = (x y z) T, n 2 = (φ. θ. ψ) T, n = (n 1. n 2 ) T, V = (u. v. w) T, ω = (p. q. r) T, v = (V. ω) T then effective force and torque vectors are defined as: τ 1 = (Fx. Fy. Fz) T, τ 2 = (Mx. My. Mz) T, τ = (τ 1 τ 2 ) T and the center of gravity is r G = (x G. y G. z G ) T. Transformation of position and velocity vectors from body frame to global coordinate is calculated with the following formulation where J is known as coordinate transformation matrix. There are two coordination transform matrices to calculate nonlinear state velocity of positions and angles in global frame therefore equations (1)-(2) depict coordinate transform matrices and equation (3) calculates nonlinear state velocities, based on coordinate transformation matrix. Surge, sway and lift velocities explain first order derivation of vehicle motions and derivation of Euler angles is depending on coordinate transformation matrix and angular velocities in body fixed frame. Figure 1: Submarine body and actuators [7] cos cos sin cos cos sin sin sin sin cos cos sin J1( n2) sin cos cos cos sin sin sin cos sin sin sin cos sin cos sin cos cos 1 sin tan cos tan J2( n2) 0 cos sin 0 cos sec sec cos { n 1 = J 1 (n 2 ) V n 2 = J 2 (n 2 ) ω (1) (2) (3) 28

3 Seyyed Hossein Ghenaati, Shahram Aghaei / IJCOE 2016, No. 3; Degree of freedom Table 1: Motion modes, states and effective forces of AUV Linear Location/Euler Force/Torque Velocity/angular angles (in the Motion (in the body Velocity (in the global frame modes frame body frame coordinate) coordinate) coordinate) 1 Surge F x u x 2 Sway F y v y 3 Lift F z w z 4 Roll M x p φ 5 Pitch M y q θ 6 Yaw M z r ψ We discussed vehicle kinematic relationship between body and global frames before and so as to dynamic model. Based on Euler-Newton theorem which describes Euler two laws of motion [18], we wrote the rigid body dynamic equations in six degrees of freedom in equations (4)-(9). All forces and torques in the right section of equations (4)-(9) definitely are produced by propeller speed and plane deflections as well as hydrodynamic forces constructed from fluid inertia force, fluid viscous force and restoring force. Brief of constructions and influence of mentioned forces are given in reference [19]. Equations (4)-(6) explain dynamic behavior of body forces and derivation of linear and angular velocities while body torque affluence affects dynamic model in equations (7)-(9). As mentioned before MPC optimization process considers constraints inclusive signal bounds and interactions then this ability together with prediction increases robustness, applicability and softness for outputs and actuators. Regarding to singularity, stability and operational issues, "Table 2" charted all requisite constraints to solve this problem. m[u vr + wq x G (q 2 + r 2 ) + y G (pq r ) + z G (pr + q )] = F x (4) m[v wq + ur + x G (qp + r ) y G (p 2 + r 2 ) + z G (qr p )] = F y (5) m[w uq + vp + x G (rp q ) + y G (rq + p ) + z G (p 2 + q 2 )] = F z (6) I x p + (I z I y )qr + m[y G (w + vp uq) z G (v + ur wq)] = M x (7) I y q + (I x I z )rp + m[z G (u + wq vr) x G (w + vp uq)] = M y (8) I z r + (I y I x )pq + m[x G (v + ur wp) y G (u + wq vr)] = M z (9) Table 2. State and actuator constraints Rudder angle ( low limit) -30 Rudder angle variation per second ( low limit) -4 Rudder angle ( high limit) +30 Rudder angle variation per second ( high limit) +4 Propeller speed ( low limit) 0 RPM Propeller speed variation per second ( low limit) -40 RPM Propeller speed ( high limit) 1800 RPM Propeller speed variation per second ( high limit) +40 RPM Pitch angle ( low limit) -30 Pitch angle ( high limit) +30 Depth interaction MPC scheme for speed control MPC is an optimizing method which predict future behavior of known LTI models by discrete time slots and assumes discrete constrained control signals such a predictive way that a performance index with weighted coefficients reflecting to relative importance of states and control signals, can be reduced. The main advantage of MPC is the fact that this strategy allows the current time slot to be optimized while keeping future timeslots in account. This approach considers optimizing a finite time horizon but just implementing the current timeslot and due to quadratic programing method, MPC has capability of produce instant feasible trajectories respected to various kind of input and output restrictions. Extending of MPC in industrial problems is extremely appealing thanks to these inherent features which makes smooth response in actuators. The design of speed control in the right direction is of interest both from the view of motion stabilization as well as tracking performance. We linearized then discretized system at certain operating point then MPC according to quadratic programing method [20] tries to find optimized control signals for prediction horizons based on linearized model. Defined effective weighted coefficients in performance index makes all motions and rotations nearby zero except surge velocity in other word output control signals hold non desired motions to zero. Due to regulate velocity just in x direction hence horizontal direction and improve 29

4 Seyyed Hossein Ghenaati, Shahram Aghaei / Speed control of Autonomous Underwater Vehicle with constraints using Model Predictive Control quadratic programing schedule we just consider propeller speed and rudder deflection in account then prevent stern and elevator from deflection while speed control scheme. Applying tools from linear system theory fundamentally needs transformation of nonlinear system to linearized form so that be approximately equal to original system therefore MPC requires a linear discrete time model for optimizing process. Linearization and discretizing operations will be applied every 250 milliseconds as discretizing moment alternatively due mainly to decrease the modeling error inasmuch as variation of actual model in different positions. Regarding to this fact very high number of Np causes infeasible and wrong responses. Neglecting the disturbance and ocean current, equation (10) illustrates AUV model in state space form. x = f(x, u) { y = h(x) (10) Where comprehensive description of f(. ) exists in previous section and h(. ) possesses all states separately. We can achieve linear approximation as equation (11) whereas high order derivations of basic model in Taylor series expansion is omitted. { x = f x 0 x + f u 0 u = A x + B u y = h x 0 x = C x (11) Where (x, u, y ) denote deviation of states and outputs and subscript 0 means Jacobean matrices is evaluated at the operating point. Equation (12) describes conversion from continuous to discrete time version and equation (13) approximates linear discrete time model for MPC optimization procedure as t is discretization step size. A t F = e { G = e A t t 0 e A τ dτ B x(k + 1) = F x(k) + G u(k) { y(k + 1) = C x(k) (12) (13) Equation (14) summarizes state prediction where state vector X contains states of Np timeslots with relative control horizon variations. X 0, X M and U 1 are initial and desired states and previous control signals respectively and length of U which calculates control signal variations is considered as long as prediction horizon. X 1 F G X 2 F GF G 0 X 0 X Np Np 1 Np 2 Np F GF GF G U U I 0 U 1 I I 0 U 1 U U 1 I I I Np 1 (14) After definition of prediction behavior of model based on states and control signal variations in Np timeslots, the performance index includes state difference from desired values and control signal variation as equation (15). Np 1 T PI ( X X ) Q ( X X ) i 0 i M i M T T ( U ) R ( U ) ( X X ) P ( X X ) i i Np M Np M (15) Then quadratic programing method described in reference [20] optimizes well-defined performance index in equation (15) regarding to restrictions and Q = [400,100,10,1,400,500] is relevant weighting state gain, R = [ ] is responding control gain and P describes terminal constraint coefficients for optimizing speed control solution. Both equation (15) and mentioned constraints for speed control scheme in "Table 2", consist a finite horizon optimal control program that means we defined a MPC structure for horizontal speed control of AUV. Complete knowledge about steady state value of states ( X M ) is necessary to achieve desired performance and initial state situation may have effects too. Regarding to our problem and initial situation, steady state vector of 12 states is defined as X M = [200,0,10,1,0,0,0,0,0,0,0,0] which describes final behavior of AUV. MPC executes optimization process to decrease error between actual state values and desired ones. The last section of equation (15) is terminal state constraint to achieve infinite horizon control that guarantees closed-loop stability and P as terminal weight is calculated in such a way that optimal value of performance index roles as a Lyapunov function [20-22]. The other objective which is necessary to investigate for respecting the feasibility issue of online optimization is constructing a terminal constraint. Research about this issue and achieving a powerful formulation to calculate the proper terminal set -as considered [21-23] and references in therecorresponding to local linearized model or original nonlinear system is in hand and will be reported in next papers Results and Discussion Some states like Y and φ are absolutely invariant since not only drag force and ocean current are

5 Seyyed Hossein Ghenaati, Shahram Aghaei / IJCOE 2016, No. 3; 1-7 neglected but also rigid body and symmetric design assumption in addition to zero initial states and zero elevator angle are considered. Whereas some deviations exist in practice while ignoring presumptions. Speed regulation at 1 m/s in x direction is demanded and "Figure 3" illustrates speed control and "Figure 4" depicts pitch angle solution with different number of prediction horizon. speed as control signals is depicted in "Figure 5"and "Figure 6". Figure 3: Speed regulation with different number of prediction horizon Figure 4: Pitch angle stabilization with different number of prediction horizon A low quantity of prediction horizon makes uneven signals and increases tracking error but in other hand high quantity for prediction horizon may have ruined speed regulation respected to wrong estimation based on instant linearized model. Whereas vehicle achieve desired linear velocity, pitch angle must vary how much that decreases motion interactions on sway and lift directions as much as possible so pith, yaw and roll angles are expected to reach zero. If body frame matches global coordinate frame, vehicle linear velocity motivates just in x direction of vertical plane and coordinate transformation matrix must be identity. In order to satisfy favorites and respected to all restrictions, MPC moves pitch angle from initial value to zero and accelerates vehicle till desired speed concurrently. Rudder deflection and propeller motor Figure 5: Rudder angle deflections and operating constraint bound for different number of prediction horizon Figure 6: Propeller speed and high limit constraint for different number of prediction horizon MPC operates such a model based manner that faced with nonlinear systems, prediction may be incorrect especially in chaotic models because of nonlinear states. This fact as well as interactions caused inability of whole vehicle motion control simulations show MPC succeed when AUV reaches desired speed with corresponding pitch angle as mentioned before but according to "Figure 7" there are some interactions which is arisen from coupled states but all constraints are fulfilled. 31

6 Seyyed Hossein Ghenaati, Shahram Aghaei / Speed control of Autonomous Underwater Vehicle with constraints using Model Predictive Control Figure 7: Depth interaction while speed regulation and constraint depth 5. Summary and conclusions Since enormous exciting plant restrictions on inputs, states and outputs such as low and high ranges, variation rate, saturations and etc., MPC is known as powerful strategy for industrial control. In this paper we introduced an AUV nonlinear dynamic model and after linearization we applied MPC to solve speed regulation problem in horizontal plane. Our approach found response so feasible signals that all restrictions on control signals contain high and low limit bound, variation rate of rudder deflection and propeller speed is considered. Besides other state restrictions or interactions like depth of vehicle are satisfied too. Indeed, researchers can focus on using of the MPC to decoupling AUV dynamic model in future works or implementing it to multitask motion system or improve reliability. In addition, using the offset-free control strategy [24] can involve ocean current as disturbance and in order to guarantee stability, reference governor approach [22] leads to convergence and avoids infeasibility issues. References [1] Blidberg, D. R., The development of autonomous underwater vehicles (AUV); a brief summary, IEEE ICRA, vol. 4, 2001 [2] Wernli, R.L., AUVs-a technology whose time has come, Underwater Technology, Proceedings of the 2002 International Symposium on. IEEE, [3] Danson, E. F., AUV tasks in the offshore industry, Technology and Applications of Autonomous Underwater Vehicles, Griffiths, G.(ed). Taylor and Francis, London, pp , [4] Vervoort, J. H. A. M., Modeling and control of an unmanned underwater vehicle, [5] Fossen, T. I., Handbook of marine craft hydrodynamics and motion control, John Wiley & Sons, [6] Tang, S. C., Modeling and simulation of the autonomous underwater vehicle, Autolycus (Doctoral dissertation, Massachusetts Institute of Technology), [7] Wadoo, S., Kachroo, P., Autonomous underwater vehicles: modeling, control design and simulation, CRC Press (2016). [8] Valentinis, F., Donaire, A., Perez, T., Energybased motion control of a slender hull unmanned underwater vehicle, Ocean Engineering, vol. 104, pp , [9] Yoshida, K., El-Nemr, M., Yamshita, Y., Pitching stability analysis and control for underwater maglev linear motor vehicle ME02, Power Electronics and Motion Control Conference, EPE- PEMC th International. IEEE, [10] Elnashr, G. A., Performance and stability analysis of an autonomous underwater vehicle guidance and Control, proceding international conference of modeling, identification and control (ICMIC), pp , [11] Bagnia, S.,Aggarwal, N., Depth control of autonomous underwater vehicle using MPC, international journal of research, vol. 2, no. 8, [12] Fernández, D. C., Model Predictive Control for Underwater Robots in Ocean Waves (master s thesis), Oregon State University, [13] Johansen, T. A., Perez, T., Cristofaro, A., Ship collision avoidance and COLREGS compliance using Simulation-based control behavior selection with predictive hazard assessment, [14] Zhao, Y., Li, W., Shi, P., A real-time collision avoidance learning system for Unmanned Surface Vessels, Neurocomputing, vol. 182, pp , [15] Akçakaya, H., Gören Sümer, L., Robust control of variable speed autonomous underwater vehicle, Advanced Robotics, vol. 28, no. 9, pp , [16] Franze, G., Lucia, W., A Receding Horizon Control Strategy for Autonomous Vehicles in Dynamic Environments, IEEE Transactions on Control Systems Technology 24, no. 2, pp , [17] Franzè, G., Lucia, W., The obstacle avoidance motion planning problem for autonomous vehicles: A low-demanding receding horizon control scheme, Systems & Control Letters, vol. 77, pp. 1-10, [18] Kasdin, N. J., Dynamics of Particles and Rigid Bodies [Book Review], IEEE Control Systems, 26, no. 6, pp , [19] Healey, A. J., Lienard, D., Multivariable sliding mode control for autonomous diving and steering of unmanned underwater vehicles, IEEE journal of Oceanic Engineering, vol. 18, no. 3, pp , [20] Wang, L., Model predictive control system design and implementation using MATLAB, Springer Science & Business Media, [21] D.Q. Mayne, J.B. Rawlings, C.V. Rao, and P.O.M. Scokaert., Constrained model predictive 32

7 Seyyed Hossein Ghenaati, Shahram Aghaei / IJCOE 2016, No. 3; 1-7 control: Stability and optimality Automatica, 36(6): , [22] Aghaei, S., Sheikholeslam, F., Farina, M., Scattolini, R., An MPC-based reference governor approach for offset-free control of constrained linear systems, International Journal of Control, vol. 86, no. 9, pp , [23] Gilbert, E., &Tan, K. (1991). Linear systems with state and control constraints: The theory and application of maximal output admissible sets. IEEE Transactions on Automatic Control, 36, [24] Aghaee, S., Zakeri, Y., Sheikholeslam, F., Offset-free control of constrained linear systems using model predictive control, 2008 IEEE International Symposium on Industrial Electronics. IEEE,