Model predictive control for autonomous underwater vehicle

0 downloads 0 Views 342KB Size Report
Model predictive control for autonomous underwater vehicle ... Second method uses the nonlinear ... nonlinear optimization techniques are used as tool for.
Indian Journal of Geo-Marine Sciences Vol. 40(2), April 2011, pp. 191-199

Model predictive control for autonomous underwater vehicle Agus Budiyono Department of Aerospace Information Engineering, Konkuk University 1 Hwayang-dong, Gwangjin-gu, Seoul 143-701, South Korea [E-mail:[email protected]] Received 23 March 2011; revised 28 April 2011 Research on the autonomous underwater vehicles (AUVs) has been gaining more interest in the recent past. AUVs have been envisioned as a cost effective and safe solution for various underwater missions including but are not limited to underwater scientific test-bed, deep oceanic surveillance, environmental monitoring and underwater structures inspection. The control for such autonomous vehicles, however, continues to pose a serious challenge due to its complex dynamics and uncertain environment. As an alternative of direct tuning PID controllers which has several limitations, Model Predictive Control is proposed in this work. Characteristic of AUV where the dynamics is sluggish and linear around the region of operation makes the application of MPC amenable. In addition, the ability of the method to handle constraints inherent in the control inputs of AUV is appropriately exploited. Overall effectiveness of the new approach is evaluated based on the tracking performance, disturbance rejection and robustness. [Keywords: MPC, AUV, underwater robotics, Autonomous, Dynamics]

Introduction Autonomous underwater vehicles have been known to be characterized not only by high degree of nonlinearity but also time-varying dynamic behaviour. Uncertainties also present both in hydrodynamic coefficients of the vehicle dynamics model as well as the unstructured operation environment including the changing ocean current. Additionally, when robotics manipulators exist, their motion will significantly shift the center of gravity and buoyancy. All these factors add up and result in the difficulty in designing control for autonomous underwater vehicles. Control for AUV has been proposed by using numerous approaches and techniques. When categorized based on whether and how the dynamic model is used, all can simply be grouped into three different methods. The first method relies on an in situ gain tuning of PID single-input single-output controller. While the successful use of this method has been reported for some cases, the general design for AUV has been proven to warrant a more advanced multivariable controller accommodating complex performance criteria. Second method uses the nonlinear dynamics model as the guide for control design where nonlinear optimization techniques are used as tool for system identification. Appropriate nonlinear control can then be designed using the model. Third method in centered on the extraction of a linear model associated

with AUV operating condition. Linearization procedure is conducted based on the principle of small perturbation around trimmed points. Linear control synthesis can then be applied to each linear model. To cover the entire AUV flight envelope, a gain-scheduled controller is applied based on the interpolation between families of linear models. Present study is focused on the development of controller for AUV Squid which was designed and developed at the Center for Unmanned System Studies, Institut Teknologi Bandung, Indonesia1. AUV first prototype is described in Fig. 1. Control for the first

Fig. 1—AUV Squid first prototype [8]

INDIAN J. MAR. SCI., VOL. 40, NO. 2, APRIL 2011

192

prototype of AUV Squid was designed by using PID with a direct gain tuning. PID controller was successfully implemented for designing heading hold system as well as trajectory following system. Due to the changes in the payload configuration, however, it turned out that the direct tuning PID was tedious and not effective in finding the new gains for the new prototype. Experimental test showed that the AUV suffered from an un-damped phugoid oscillation2. Subsequent efforts were directed at addressing the problem by model-based control synthesis. The algebraic approach using CDM was proposed as a practical control design as reported in the author’s previous work3. Stability for such a controller is discussed in a relevant AUV application4. The feasibility of applying a robust LPV controller is studied by Sutarto and Budiyono5. Present study envisages to investigate the viability of MPC for AUV Squid. The paper is organized as follows. In Section 2, basic of MPC is presented. In Section 3, dynamics of AUV Squid is summarized and the MPC controller is proposed. Control design results are presented in Section 4. The paper is ended by concluding remarks. Materials and Methods

time instants t= kT where k =0,1,2,.. and T denotes the sampling time interval. The MPC algorithm consists of three steps: prediction, optimization and control. In essence, MPC represents a control strategy in which control input at each sampling time is calculated based on (generally constrained) optimization over a certain horizon based on a model prediction. Prediction horizon means that future system output is predicted at every sampling time for a predetermined sampling interval ahead. The predicted outputs is obtained by iterating the model for each mode yˆ ( k + i | k ) = C d xˆ ( k + i | k ) i

= C d Adi x ( k ) + ∑ C d Adj −1 Bd uˆ ( k + i − j | k ) j =1

… (2) The predicted outputs can be grouped to form a vector as follows  yˆ (k + 1)   C d Ad   yˆ (k + 2)   2   =  C d Ad  x(k )           Hp   yˆ (k + H p ) C d Ad 

Basic of Model Predictive Control

A. Basic motivation MPC controller has been successfully used for different applications ranging from chemical plants to aerospace systems. Some key features that motivate the underwater applications include6:    

It naturally handles multivariable control problem Actuator limits are taken into account It allows operation closer to constraints Control update rates are low

B. Structure of MPC control To understand the concept of MPC, the system model is given in the discrete time representation as follows:

x(k + 1) = Ad x(k ) + Bd u (k ) y(k ) = Cd x(k ) z ( k ) = C z x( k )

… (1)

where y(k), z(k), u(k) and x(k) represent system outputs, controlled outputs, inputs, and states respectively at

 C d Bd  C A B d d d +    H p −1 Bd C d Ad

0 C d Bd





 

H −2 C d Ad p Bd

uˆ ( k )    uˆ ( k + 1)   ×      uˆ ( k + H p − 1) 

      C d Bd  0 

… (3)

The input prediction uˆ (k + i) can be expressed in term of input increment ∆uˆ (k + i) , where ˆ ˆ ˆ ∆u (k + i) = u (k + i) − u (k + i − 1) . The predicted input vector thus become uˆ ( k )   u ( k − 1)   uˆ ( k + 1)      = u ( k − 1)            uˆ ( k + H p − 1)  u ( k − 1) 

AGUS: MODEL PREDICITIVE CONTROL FOR AUTONOMOUS UNDER WTER VEHICLE

I I +   I

0 I I



 

∆uˆ ( k )  0   ∆uˆ (k + 1)            ˆ I  ∆u (k + H p − 1)

Υˆ = Φ x( k ) + γu (k − 1) + G Uˆ     past

If the measured disturbance is brought to the calculation, equation (1) become

H u ≤ i ≤ H p − 1 , uˆ (k + i) = uˆ (k + H u − 1) . Following the derivation given by Joelianto et.al7, the structure of MPC can be elaborated as follows. The predicted outputs now can be writen as:

… (8)

B  Bm =  dm  .  0 

With this expression, the

 yˆ (k + 1)   CA   yˆ (k + 2)   CA 2   = x +      k     Hp   yˆ (k + Hp ) CA   CB  CAB     Hp −1 B CA

∑ ∑



x(k + 1) = Ax(k + 1) + B∆u(k ) + Bm d (k )

predicted outputs can then be given by





… (7)

where Bdm is a measured disturbance matrix and d(k) is the measured disturbance. In MPC design, it is more convenient to express the model with an increment input. One form of expression is given as follows6:

where

 C d Bd     H u −1    i C d Ad Bd    i =0  +  Hu  u (k − 1) i  C d Ad B d   i =0      Hp   C d Adi B d   i =0 



x(k + 1) = Ad x(k ) + Bd u(k ) + Bdmd (k )

y(k ) = Cx(k )

 yˆ (k + 1)   C d Ad             yˆ (k + H u )   C d AdH u   = H u +1  x ( k )  yˆ (k + H u + 1) C d Ad           Hp   yˆ (k + H p )   C d Ad 



... (6)

future

… (4)

It is assumed that the input only changes at time such that for instants k , k + 1 ,  , k + H u − 1

Cd Bd  C A B + C B z d  d d d    H u −1  ∑ C Ai B d d d  +  iH=0  ∑u C Ai B  i =0 d d d    H p  i  ∑ Cd Ad Bd  i =0

193

     ∆uˆ ( k )   Cd Bd    ∆uˆ ( k + 1)     Cd Ad Bd + C z Bd     ∆uˆ (k + H u − 1)     H p −Hu  i Cd Ad Bd  ∑ i =0  0 0 

… (5) Therefore, the predicted outputs can be written in more compact equation

0



CB  CA Hp −2 B



       Hp − Hu CA B 0

∆uˆ (k )    ∆uˆ (k + 1)   ×       ∆uˆ (k + H u − 1)  CBm  CAB m +    Hp−1 CA Bm 

0



CBm  Hp−2 CA Bm

 

0  d (k)    dˆ(k + 1)         ˆ CBmd (k + Hp −1) 

or written in a more compact equation: Υˆ ( k ) = Φ x ( k ) + Θ∆ Uˆ ( k ) + Ξ Dˆ ( k )     past future

… (9)

INDIAN J. MAR. SCI., VOL. 40, NO. 2, APRIL 2011

194

The general aim of MPC synthesis is to minimize the quadratic cost function Hp 2 Hu 2 J = ∑ y(k + 1) − w(k + i) Q + ∑ ∆u (k + i − 1) R i =0 i =1

… (10)

w is the reference, H p and H u are the

where

∆Uˆ T H∆Uˆ − ∆Uˆ T G which is a quadratic function. In this equation the matrix H is the Hessian matrix, thus the function can be changed to ∆Uˆ T H∆Uˆ − G T ∆Uˆ . The cost function is subject to linear inequality in the inputs, input increments, and outputs as follows:

maximum prediction horizon and the control horizon respectively, and the weighting matrices Q and R are defined by

u min ≤ u(k + i) ≤ u max , i = 0,1,..., H u − 1

Q (1)  0 Q=     0

y min ≤ y ( k + i) ≤ y max , i = 1,2,..., H p

 R (1)  0 R=     0

0 Q ( 2)







0

0

0



R ( 2)  0

 0

      Q ( H p )  0 0

J MPC

2

  0     R ( H u − 1)  0

Q

− ∆Uˆ

2

ΓΘ∆Uˆ ≤ −Γ[Ψx(k ) + Υu (k − 1)] − g

… (11)

R

Defining the error prediction as

ε = Γ(k ) − Φx(k ) − ΞD(k )

Q

+ ∆Uˆ

for suitable matrices ψ, Y and Θ And where F = F1 ,..., FHu and F, Γ and W are matrices with appropriate size. All inequalities in the equation (15) can be arranged together into a single set vectormatrix equation as follows:

[

]

... (16) ,

the cost function in the MPC problem becomes a quadratic programming (QP) problem as follows7: 2

W∆Uˆ ≤ w

Ω∆Uˆ ≤ ω

r (k + i) = w(k + i)− ∈ (k + i) = w(k + i) − e −iTs / Tref ∈ (k ) and ∈ (k ) = w(k ) − y (k ) .

J MPC = Θ∆Uˆ − ε

… (15)

… (12)

 r (k + 1)    Γ(k ) =    r ( k + H p )   

Where

... (14)

In a more general form the constraints can be written as F∆Uˆ ≤ −F1u(k − 1) − f

The cost function can then be simplified in a matrix form as = Yˆ − W

∆u min ≤ ∆u (k + i) ≤ ∆u max , i = 0,1,..., H u − 1

2 R

J MPC = constant − ∆Uˆ T G + ∆Uˆ T H∆Uˆ

… (13)

where G = 2ΘT Qε and H = ΘT QΘ + R . The minimization the cost function J MPC is equivalent to the minimization of the objective function

where, − F1u (k − 1) − f  F       Ω = ΓΘ  and ω =  −Γ[Ψx(k ) + Υu (k − 1)] − g   W    w … (17)

In the MPC control design, the inequality matrix needs to be composed only once to be used for each optimization process since constraints are constant. Control of AUV Squid using MPC

A. AUV Squid dynamic model This section presents the implementation of the MPC synthesis approach to longitudinal control the AUV Squid. Only longitudinal equation of motion is considered.

AGUS: MODEL PREDICITIVE CONTROL FOR AUTONOMOUS UNDER WTER VEHICLE

The model of AUV squid has been derived by Budiyono et. al8. The linearization is conducted for predefined operating conditions to extract the linear model. To be workable for stability analysis and control synthesis, the linearized equations of motion are expressed in state-space form. First, from the Newton-Euler equation, the matrix equations of motion can be given as  m − X u

  0   mzG  0   a 00  0 M  uw  0

0

(

m − Z w

− ( mxG + M w ) 0 0 c1 − (Z w − X u )U o − e1 0

[

 u   XδT  w  0  =  q   M δ T1    θ   0

0   u   − mxG + Z q

0   w   −  I yy − M q

0   q    0 1  θ  Xθ  0 [U o (m + X u ) + c 2 ] 0   U o mx G − Z q + e 2 Mθ  1 0  mzG

]

[ (

)

) ]

XδT 0

M δ T2 0

B. Control synthesis The synthesis of MPC was carried out in the following steps. First, the parameter of MPC is chosen considering the given linearized model. The prediction horizon and control horizon are chosen to be Hp = 10 and Hu = 4, respectively. The time elapsed between control moves (or sampling time) is 1 s. The constraints for the input and state variables are given as: For the input variable, the differential thrust dT1 (Newton) for longitudinal control is limited by −2 ≤ dT1 ≤ 2 For the state variables, the pitch angle θ (rad) is constrained to

   δT 1   δ  M δ T3   T 2   δT 3  0 

XδT 0

195

−0.2 ≤ θ ≤ 0.2 … (18)

The forward velocity u (m/s) is limited by

This matrix equation can be simply rewritten as

−5 ≤ u ≤ 5

M x − Cd x = D u

Meanwhile, the heave velocity w and the pitch rate q are not constrained. The second step is determining the input and output weight parameters. For the input variable dT1 no weight is assigned meaning that dT1 is allowed to vary freely between its minimum value and maximum value. However the rate weight of dT1 must be assigned non-zero value since in reality the rate of change of the differential thrust is limited. The rate weight of dT1 is chosen to be 0.3. The output weight of 1 is assigned to the pitch angle θ. No weight is assigned to the other state variables. Overall, the choice of weight is guided by the trade-off between the robustness and the combined disturbance rejection and set point tracking. The control synthesis is performed using MPC design tool in MATLAB®.

… (19)

and finally the standard state-space can be expressed as x = A x + B u

where A = M − 1C d

… (20) and

B = M − 1D

The state x = {u , q, w,θ } is state variable vector and the input vector is u = {dT1} .The values of the A and B matrices content are function of flight parameters, primarily the forward speed and depth. The state space associated with speed U0= 2 m/s and depth D = 50 m is taken as a design example. The state space equation can be written as:

Results and Discussion As a case study the AUV is to follow the pitch angle θ setpoint defined as a pulse at t = 10 s with the amplitude of 0.1 rad and period of 100 s. Two cases are taken as examples to emphasize the inherent

196

INDIAN J. MAR. SCI., VOL. 40, NO. 2, APRIL 2011

Fig. 2—AUV state variables response due to increasing disturbance (case A)

trade-off between robustness in one side and tracking performance combined with disturbance rejection on the other side. For both cases, the unmeasured disturbance is introduced with increasing magnitude as step at t = 150 for input dT1 and as pulse with period of 1 s at t = 250 s for pitch angle θ. Case A : The weight is tuned in order to decrease input rate penalties relative to setpoint penalties. In this setting, disturbance rejection and setpoint tracking is expected to be quicker with the sacrifice of some level of robustness. Case B : The weight is tuned in order to increase input rate penalties relative to setpoint penalties. In this setting, it is expected that the robustness is

increased with the consequence of more sluggish disturbance rejection and setpoint tracking. Fig. 2 shows the AUV Squid state variables response due to increasing disturbance for Case A. Fig. 3 indicates the corresponding control input with increasing step disturbance. It is evident that the disturbance both in the control input and state variable is effectively rejected. The setpoint for the pitch angle θ is followed while respecting all the constraints both in control input and state variables u and θ . Fig. 4 and 5 shows the corresponding results from Case B for the state variable response and control input, respectively. When comparing Fig. 2 and Figs. 4, it is evident that for Case A where the weight is tuned to decrease

AGUS: MODEL PREDICITIVE CONTROL FOR AUTONOMOUS UNDER WTER VEHICLE

Fig. 3—AUV control input with increasing step disturbance (case A)

Fig. 4—AUV state variables response due to increasing disturbance (case B)

197

198

INDIAN J. MAR. SCI., VOL. 40, NO. 2, APRIL 2011

Fig. 5—AUV control input with increasing step disturbance (case B)

input rate penalties relative to setpoint penalties, the setpoint tracking and disturbance rejection is quicker than those of Case B. In Case A, the disturbance is completely rejected after 25 s and the setpoint is achieved in 20 s. For Case B, the corresponding values are 70 s and 50 s, respectively. The same trend appears upon the comparison of Fig. 3 and Fig. 5. However, from the control input comparison, the suppression of disturbance is relatively more pronounced for Case B indicating the higher level of robustness. Overall, the two cases demonstrate the inherent trade-off in MPC where robustness should be in balance with setpoint tracking and disturbance rejection. For the anticipation of further work, it is interesting to explore the viable way of the real-time application of the proposed technique. The question is whether there is enough onboard computing power to handle the optimization problem (13) under constraint (14) that must be solved in each step (prediction horizon). The work by Truong et.al9 proposed the fusion of the continuous time MPC algorithm using orthogonal functions and the algorithm of Intermittent Predictive

Control capable to tolerate slower computational time and longer sampling interval. Meanwhile more realistic simulation needs to consider the unpredicted AUV environment. Xu and Zhang10 proposed the combination of neural network modeling and generalized predictive control to achieve online learning capability. The online adaptive mechanism enables the control system to handle uncertainties within AUV operating environment. Conclusion This article presented a method for using MPC based controller for an autonomous underwater vehicle. The optimization of the control input is done considering the weighted tracking error between the predicted output and reference trajectory and the consumption of control energy subject the constraint in the ranges of actuators and their slew rates. The choice of weight for the input and its rate as well as for the state variable is governed by the trade-off between robustness and setpoint tracking as well as disturbance rejection. The proposed MPC controller is shown to be robust against disturbance while

AGUS: MODEL PREDICITIVE CONTROL FOR AUTONOMOUS UNDER WTER VEHICLE

maintaining an acceptable setpoint tracking performance. Overall performance indicates the feasibility of MPC for autonomous underwater vehicle application.

3

Acknowledgement The work was supported by the MKE(The Ministry of Knowledge Economy), Korea, under the ITRC(Information Technology Research Center) support program supervised by the NIPA(National IT Industry Promotion Agency) (NIPA-2011-C10901131-0003).

5

References 1 Muljowidodo, Jenie S.D., Budiyono A and Adinugroho S, Design, Development and Testing of Underwater Vehicles: ITB Experience, paper presented at The International Conference on Underwater System Technology: Theory and Application, Penang, Malaysia, 2006 2 Muljowidodo and Budiyono A, Recent Progress in the Research on Unmanned Underwater Vehicles at ITB, paper presented at the Workshop on Underwater System Technology, Kuala Lumpur, Malaysia, 2007

4

6 7

8

9

10

199

Budiyono A, Muljowidodo and Sugama A, Coefficient Diagram Method for the Control of an Unmanned Underwater Vehicle, Indian J Mar Sci., 3(2009) 316-323 Budiyono A, Robust Control Synthesis for an Unmanned Underwater Vehicle, World Academy of Science, Engineering and Technology, 2009, 60: 48-55. Budiyono A and Sutarto H Y, Development of Linear Parameter Varying Control System for Autonomous Underwater Vehicle, Indian J Mar Sci., (press) 2011 Maciejowski J M, Predictive Control with Constraints, (Pearson Education), 2002 Joelianto E, Sumarjono E M, Budiyono A, and Penggalih D R, Model Predictive Control System Design for a Small Scale Autonomous Helicopter, Aircraft Engineering and Aerospace Technology, (in review ) 2011 Budiyono A, Sugama A and Muljowidodo, Dynamics Analysis of AUV Sotong, paper presented at the 2nd International Conference on Underwater System Technology: Theory and Applications 2008 (USYS’08), Bali, Indonesia, 2008 Truong Q, Wang L P, and Gawthrop P, Intermittent Model Predictive Control of an Autonomous Underwater Vehicle, paper presented at the ICARCV, 2006 Xu J and Zhang M, Neural Network Modeling and Generalized Model Predictive Control of Autonomous Underwater Vehicle, paper presented at the IEEE Industrial Informatics, 2008