LTR Control of an Autonomous Underwater Vehicle Using a Hybrid Guidance Law

LTR Control of an Autonomous Underwater Vehicle Using a Hybrid Guidance Law

IFAC Copyright Cl' IFAC Guidance and Conlrol of Underwater Vehicles, Wales, UK, 2003 ~ Publications www.e1sevier.comiloeatelifae LQG/LTR Control o...

660KB Sizes 10 Downloads 44 Views

IFAC

Copyright Cl' IFAC Guidance and Conlrol of Underwater Vehicles, Wales, UK, 2003

~

Publications www.e1sevier.comiloeatelifae

LQG/LTR Control of an Autonomous Underwater Vehicle Using a Hybrid Guidance Law

w. Naeem, R

Sutton and S. M. Ahmad

{w.naeem, Lsutton, s.ahmad}@plymouth.ac.uk

Marine and Industrial Dynamic Analysis Group Department ofMechanical and Marine Engineering The University ofP~vmouth, PL4 8AA, UK

Abstract: This paper addresses the issue of guidance and control of an autonomous underwater vehicle (AUV) for a cable tracking problem. A linear quadratic Gaussian controller with loop transfer recovery (LQG/LTR) is developed because of its strong robustness properties. The vehicle is guided towards the target using a combination of different guidance algorithms. The vehicle speed is used to formulate the guidance problem. Simulation results are presented and a comparison is made between fix and variable AUV speeds. Copyright © 2003 IFAC Keywords: Guidance, control, LQG/LTR, cable tracking, autonomous underwater vehicle.

I. INTRODUCTION Guidance and control of AUVs have seen a tremendous growth and development in the last few years and there have been significant applications of guidance and control systems for missions such as cable/pipeline tracking, mines clearing operation, deep sea exploration, feature tracking etc. For an AUV to work effectively, a well-integrated navigation, guidance and control (NGC) system is imperative. A simple block diagram of an NGC system is depicted in Figure I. The navigation system generates information about the vehicle position, velocity, heading etc. using various onboard sensors such as a compass, global positioning system (GPS), pressure sensor etc. The guidance system manipulates the navigation information and generates suitable references to be followed by the AUV. The control system is then responsible for keeping the vehicle on course as specified by the guidance system.

Fig. I. Navigation, guidance and control of a vehicle this regard, the guidance system plays the key role in bringing autonomy to the vehicle. The purpose of this paper is to develop an integrated guidance and control algorithm for an AUV test model, which will eventually be developed and tested in real time in an actual AUV. A plethora of control systems is available to be implemented on an AUV. A good account of various control systems is presented by Craven (1999), while Naeem et al. (2003), recently documented a review on various guidance laws for underwater vehicles.

The main difference between an AUV and ROY (remotely operated vehicle) is that the ROY is controlled by a trained human operator while the AUV is steered by an onboard guidance system. In

Todate optimal control theory has been extensively used to solve various control engineering problems. Especially with the advent of powerful digital

31

computers, the computation time is curtailed to a considerable extent. The optimal control is simply a minimisation or maximisation problem for which an objective function is defined that could involve different design parameters or states to optimise.

where the coefficients a, b, c and d are given in terms of vehicle speed v in knots.

Linear quadratic Gaussian (LQG) is an optimal controller whose name is derived from the fact that it assumes a linear system, quadratic cost function and Gaussian noise. Unlike pole placement method, where the designer must know the exact pole locations, LQG places the poles at some arbitrary points within the unit circle so that the resulting system is optimal in some sense. A linear quadratic state feedback regulator (LQR) problem is solved which assumes that all states are available for feedback. However, this is not always true because either there is no available sensor to measure that state or the measurement is very noisy. A Kalman filter can be designed to estimate the unmeasured states. The LQR and Kalman filter can be designed independently and then combined to form an LQG controller, a fact known as the separation principle. Individually the LQR and Kalman filter have strong robustness properties with gain margin up to infinity and over 60° phase margin, (Burl, 1999). Unfortunately, the LQG has relatively poor stability margins which can be circumvented by using loop transfer recovery (LTR). A discrete time LQG/LTR design is presented in this paper motivated from the work of Maciejowski (1985). The LTR works by adding fictitious noise to the process input which effectively cancels some of the plant zeros and possibly some of the stable poles, and inserts the estimator's zeros (Maciejowski, 1985; Skogestad and Postlethwaite, 1996). The hybrid guidance law developed utilises vehicle speed as a means to formulate the guidance problem that was first proposed by Naeem et al., (2003) and is simulated in this paper.

The input to the AUV are the rudder deflections while the output is the heading of the vehicle. The model parameters are calculated for three different vehicle speeds and the resulting continuous time model is discretised at a sampling rate of 10Hz. The discretized models are then converted into state space controllable canonical forms owing to the LQG controller requirements. The LQG/LTR controller requires the model to be minimum phase and should be controllable and observable. The model is tested for these requirements and found to be good for said purposes. The model AUV is assumed to have a turning radius of 25 m and the constraints on the rudder actuator are maximum 25 degrees in either left or right direction.

2

a = 0.05803 v , b = 0.00449 v 3 c = 0.25963 v, d = 0.00856 i

3. CONTROL SYSTEM DESIGN LQG/LTR control of an unmanned underwater vehicle (UUV) has been reported by luul et al., (1994), and Triantafyllou and Grosenbaugh, (1991). However, both these papers deal with multivariable continuous LQG/LTR control of an underwater vehicle assuming that the guidance commands are available. In this paper, a discrete-time LQG/LTR controller is developed which is more realistic for practical purposes. A guidance law is also developed which generates suitable commands to be followed by the vehicle and is the subject of next section. In the following, the LQG/LTR controller is developed for the AUV model shown in Section 2. 3.1 Design Spec!fications Two types of design specifications are usually given prior to any controller design which are closely related. The time domain specifications involve the maximum overshoot, settling time etc. while the frequency domain specifications provides the bandwidth, gain margin (GM), phase margin (PM) etc. of the system. They can be evaluated by generating the step response and Bode plot of the system respectively. However, in an LQG/LTR design, frequency tuning is usually desired. A Bode plot of the open loop system (Equation I), suggests infinite gain at zero frequency therefore, gain crossover frequency (gct) is used as a measure of the bandwidth of the system. The desired gcf of the open loop system for all vehicle speeds is I Hz. An acceptable nominal design usually is one that attains both a GM ~ 3 dB and PM ~ 30°, (Wolovich, 1994). The desired GM in this case is set at 10 dB while the PM at 53°, well above the nominal values.

The paper is organised as follows. The next section describes the AUV model while Section 3 explains the LQG/LTR control system design. Section 4 states the guidance law formulation and simulation results are presented in Section 5. Finally, concluding remarks are made in Section 6. 2. AUV MODELLING The AUV test model is that used by Kwiesielewicz et al. (200 I), where the model parameters are given in terms of vehicle speed. Since the guidance law require different vehicle speeds, therefore this model has been chosen for demonstration of the proposed algorithm. The single-input single-output (SISO) vehicle model for a given vehicle speed can be described by the following transfer function,

3.2 Kalman Filter Design as +b

G(s) = - - ,- - -

Since the heading of the AUV corrupted by noise is the only measured variable, the remaining states have to be measured through a state estimator prior to

(I)

s(s-+cs+d)

32

control calculations. A current estimator is used because the estimate is based on the current measurement. This is done because the processing time required to compute each control signal is small in contrast to the sampling time. In addition, this scheme gives more accurate results as compared to a prediction estimator (Franklin et al., 1998). Let the plant to be controlled is modelled in state space form as x(k

The above values provide asymptotic recovery of the stability margins, given that the plant obeys some specific characteristics. The state feedback matrix Kc is obtained by solving equations dual to Equations 3 and 4, and is used to generate the control according to lI(k)

(8)

x

where is the estimate of the state x given by Equation 2, and u is the control action. The closed form solution of K for the values of Q and R in Equation 7, is given by Maciejowski (1985) as

+ I) = Ax(k) + Bu(k) (2) y(k)

= -Kcx(k)

= Cx(k)

Kc= (CB)') CA

The design objective is to find the Kalman gain Kt such that the estimate ofx(k) is optimal. The solution to this problem is given by the discrete steady state Kalman filter gain equation given by (Franklin et aI., 1998; Maciejowski, 1985)

(9)

A feedback compensator is finally synthesised as a series connection of the Kalman filter and the optimal state-feedback controller as depicted in Figure 2 given by (Maciejowski, 1985) x(k

+ I) = (A - BK c

- KfCA + K rCBKc )x(k)

+ ...

(A - BK c )Kre(k)

(3)

( 10)

where V is the measurement noise spectral density matrix and P is the steady state error covariance matrix given by the solution of a discrete steady state Riccatti equation, (Maciejowski, 1985)

u(k) = Kc(I - KrC)x(k) + KcKre(k)

where e = r - x, is the error between a reference signal r and desired state x. Let G(z) is the transfer function of the system defined by Equation 2 and H(z) is the compensator transfer function. If the plant G(z) is minimum phase and det (CB) 7= 0, then full recovery is achieved if

where W is the process noise spectral density matrix. The parameters Wand V are tuned until the desired filter's open-loop return ratio cJ) (z) specifications are met which is shown below cJ)(z)=C(zI-A)

-I

A*K

G(z)H(z) = cJ)(z)

where G(z)H(z)is called the loop transfer function.

(5)

f

4. GUIDANCE LAW The objective of any guidance law is to steer the AUV so that it intercepts the target in minimum time and maximum accuracy. The guidance law used in this paper utilises AUV speed as a means to formulate the problem. The complete mission is classified into four different phases utilising different guidance laws. These are i) launch phase, ii) midcourse phase, iii) terminal phase, and iv) tracking phase as shown in Figure 3. In the first phase called the launch phase or the boost phase, the vehicle is launched from a vessel and guided in the direction of the line of sight (LOS) with maximum speed, using the LOS guidance only. Once the vehicle approaches the LOS, midcourse guidance could be invoked. In midcourse phase, the vehicle follows the LOS angle with maximum speed using the way point guidance, (Healey and Lienard, 1993). During this part of the flight, changes may be required to bring the vehicle onto the desired course and to make certain that it stays on that course. The midcourse guidance system is used to place the vehicle near the target area, where the system to be used in the final phase of guidance can take over. It should be noted that there is no need for the vehicle to submerge at this stage, as the objective is to approach the target area with maximum accuracy regardless of the orientation of

3.3 LQR Design

Once the Kalman gain is evaluated for the desired specifications for all models, the LQR state feedback gains are calculated. An objective function is minimised given by

N[T J = -I I. x (k)Qx(k) + u T(k)Ru(k) ] 2 k=O

(6)

where the weighting matrices Q and R are chosen according to Maciejowski (1985) as Q

= C T C,

(7)

R '" 0

(\ I)

Vehicle I----1-----,P..,..ition

y

Fig. 2. LQG controller showing LQR gain and state estimator

33

the vehicle with respect to the cable. When the vehicle reaches within the circle of acceptance, the third phase called the terminal phase is invoked. During this phase the vehicle must be slowed down and submerged in order to line up with the cable/pipeline as shown in Figure 3. The circle of acceptance in this case as opposed to Healey and Lienard (1993), should be taken at least the minimum turning radius of the vehicle in order to avoid overshoot. Finally, when the vehicle enters the waypoint, the fourth phase called the tracking phase is called up utilising any existing guidance law with the vehicle speed reduced to its minimum value. For example, the vehicle could use vision based guidance system to follow the cable. If the cable to be followed is an electrical/ communication cable, then magnetometers could be used to detect the radiation from the cable and guide the vehicle in the appropriate direction (Naeem et al., 2003).

The kinematic equations of the AUV are stated below and represents the components of the velocity in the (x, y) plane

v, V"

where 1fIt. and ~> represents the actual heading and velocity of the AUV respectively. The speed of the AUV is regulated at three different values used in different phases of the mission as mentioned above. The components (x,., y,.) of the AUV position can be evaluated by integrating the velocities (Vx , V,.), respectively. In addition to the LOS angle from the vehicle to the target, the guidance system also generates the range (distance) of the AUV from the target. The range measure is used to switch between different pre-tuned controllers. The guidance subsystem block diagram is shown in Figure 4 and is implemented in Simulink environment.

To implement the guidance law, it is necessary to compute the LOS angle A. This requires relative positions of the AUV and target in both the coordinates i.e., h r

= yl = XI -

= VI' cos IJII' = VI' sin IJII'

5. SIMULATION RESULTS The proposed integrated guidance and control algorithm is implemented on the AUV model shown in Section 2 in Matlab/Simulink environment. The following assumptions are taken for the simulations:

yv Xv

therefore,

A. = tan

i) The AUV and target are in the same plane. ii) Complete navigational information is available through onboard sensors. iii) A complete knowledge of the target's motion is available to the AUV. iv) The AUV is equipped with a vision system that generates the co-ordinates of the points on the cable to be tracked. v) The initial target co-ordinates (one end of the cable) are known prior to the mission.

-I(h) -;

cirCle of acceptance

c:abl.

The first step in any LQG/LTR control problem is to design a target filter's open-loop return ratio given by Equation 5, which requires the Kalman gain to be evaluated. By manipulating the spectral density matrices Wand V in Equations 3 and 4, the Kalman filter can be designed and hence the target filter's open-loop return ratio. In this paper, the procedure adopted by Weerasooriya and Phan (1995) is followed. Keeping the measurement noise spectral density fixed at unity and tuning the process noise spectral density matrix, until the desired frequency domain specifications are met. The Bode plot of the desired filter's open-loop return ratio for the 10 knots speed model of the vehicle is shown in Figure 5. The GM, PM and gcf can be readily evaluated from the plot. The next step is to calculate the feedback gains using the optimal Q and R in Equation 7 and develop the LQG/LTR compensator using Equation 10. The loop transfer function G(z)H(z) is also evaluated and the Bode plot superimposed on the Bode plot of the open-loop return ratio in Figure 6 shows the amount of recovery achieved. In this case, full recovery is

Launc:tl Phase

Fig. 3. Planar view of the four phases of flight for cable tracking problem of an AUV.

Range

Polar to

Cartesian

Cartesian

to Polar LO ange

Fig. 4. Guidance subsystem block diagram

34

achieved as the two plots overlap each other. Figure 7 presents the step response of the closed loop feedback system showing a large overshoot. This can be reduced by adding more damping to the system by introducing a weighting factor on the diagonal term of Q corresponding to the velocity state. This is equivalent to using rate feedback for improving damping from a conventional sense (Weerasooriya and Phan, 1995). Figure 8 depicts the step response of the closed loop system with modified Q and Figure 9 presents the Bode plot of the loop transfer function. Although overshoot has subsided but at the cost of reduced stability margins due to the deviation from the optimal values. The same procedure has been applied to all vehicle models at various speeds and compensators are developed. Finally guidance and control system integration is done and the simulation results are shown in Figure 10 for a cable tracking mission, which clearly shows good tracking behaviour using the proposed guidance algorithm. The control surface deflections generated by the controller is depicted in Figures I I and 12 for the case of optimal and modified Q respectively. Clearly, the modified Q with additional damping causes less variation in the control input as compared to optimal Q but at the cost of reduced stability margins. However, both figures suggest that the deflections are within the constrained actuator limits.

Bode Plots of the Open-Loop Return Ratio and Recovered Loop Transfer Function

Frequency (rad/s)

Fig. 6. Bode plots of the filter's open-loop return ratio and recovered loop transfer function for nominal Q (full recovery)

Step Response of the Closed Loop System Using Nominal Q

'L ~

~_.~~----~~-----

6. CONCLUSION This paper demonstrates an integrated guidance and control system approach using an LQG/LTR controller and a hybrid guidance law. The LQRlLTR controller is synthesised in discrete-time and a hybrid guidance law is proposed which uses different vehicle speeds in different phases of the mission. Simulation results are presented to show the robustness properties of the proposed integrated system. Results for a cable-following mission also depicts good tracking performance. A SISO system is used for the simulations, however, a multivariable LQG/LTR integrated with the proposed guidance system is an area of ongoing research.

Time (in samples)

Fig. 7. Step response of the closed loop system for Q=CTC and R",O.

Step Response of the Closed Loop System with Modified Q

Bode Plot of the Desired fIlter's Open-Loop Return Ratio

., I/)

c

8.

I/)

~

.,

Q,

in

Time (in samples)

Frequency (rad/s)

Fig. 8. Step response of the closed loop dystem for modified Q & R",O

Fig. 5. Bode plot of the target's filter open-loop return ratio

35

Superimposed Bode Plot of the Open-loop Return Ratio and Recovered loop Transler Function for Modified Q

Rudder Deflections Generated by the Controller for Modified Q

Frequency (rad/s)

Time (Samples)

25

Fig.

Fig. 9. Bode plots of the filter's open-loop return ratio (solid line) and recovered loop transfer function (dashed line) with added damping, (reduced stability margins)

Healey, A. 1., and D. Lienard, (1993). Multivariable Sliding Model Control for Autonomous Diving and Steering of Unmanned Underwater Vehicles. IEEE Journal o(Oceanic Engineering, voI. 18, no. 3, pp. 327-339, July. Juul, D. L., M. McDermott, E. L. Nelson, D. M. Bamett and G. N. Williams (1994). Submersible Control Using the Linear Quadratic Gaussian with Loop Transfer Recovery Method. In: Proceedings ~f the Symposium on Autonomous Underwater Technology, pp. 417-425, July, Cambridge, MA, USA. Kwiesielewicz, M., W. Piotrowski and R. Sutton (200 I). Predictive Versus Fuzzy Control of Autonomous Underwater Vehicle. IEEE International Co'!(erence on Methods and Models in Automation and Robotics, pp 609-612, 28-31 August, Miedzyzdroje, Poland. Maciejowski, J. M., (1985). Asymptotic Recovery for Discrete-Time Systems. IEEE Transactions on Automatic Control, vol. AC-30, no. 6, pp. 602-605, June. Naeem, W., R. Sutton, S. M. Ahmad, and R. S. Bums, (2003). A Review of Guidance Laws Applicable to Unmanned Underwater Vehicles. To be published in The Journal of Navigation. VoI. 56, no. 00, pp 1-15. Skogestad, S. and I. Postlethwaite, (1996). Multivariable Feedback Control: Analysis and Design using Frequency-domain Methods. John Wiley and Sons Ltd. Triantafyllou, M. S., and M. A. Grosenbaugh, (1991). Robust Control for Underwater Vehicle Systems with Time Delays. IEEE Journal of Oceanic Engineering, voI. 16, no. 1, pp 146-151, January. Weerasooriya, S. and D. T. Phan, (1995). DiscreteTime LQG/LTR Design and Modelling of a Disk Drive Actuator Tracking Servo System. IEEE Transactions on Industrial Electronics, vol. 42, no. 3, pp 240-247,June. Wolovich, W. A., (1994). Automatic Control Systems, Basic Analysis and Design. International Edition, Saunders College Publishing.

AUV Cable Tracking Mission, Fix Speed vs. Variable Speed 14(1--

.. ~'~ 120~

~ ro~

8

\

:

(.)

launchll19 60:- posltton

~

4(}>

>-

:c

~

3)~

Or

---::'soc;;----=,;,o-o---;;;:;,,,,,---

·2i.~oo~-

300

,so

Vehicle X-Coordinates

Fig. IO. Cable tracking mission from launching to tracking, variable speed vs. fixed speed Rudder Deflections Generated by the Controller for Nominal Q 25,

I

I~I J ~

I,

~

U

('

I

Jl Qt

e8

-'of

u

lsl.

I

I

-"'t

i

J

_"'oL-~~;u-~-~~-~-"'ro-~~)--:':-~

Time (Samples)

Fig.

12. Rudder deflections generated by the LQG/LTR controller for modified Q & R",O

11. Rudder deflections generated by the LQG/LTR controller for Q=eC & R==O

REFERENCES Burl, J. 8., (1999). Linear Optimal Control, H} and H ~ Methods. Addison-Wesley Longman Inc. Craven, P. 1. (1999). Intelligent control strategies for an autonomous underwater vehicle. PhD Thesis, University ~f Plymouth, UK. Franklin, G. F., J. D. Powell and M. Workman (1998). Digital Control of Dynamic Systems, 3,d ed. Addison-Wesley Longman Inc. 36