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

Transcription

1 LQG/LR Control o an Autonomous Underwater Vehicle Using a Hybrid Guidance Law W. Naeem, R. Sutton and S. M. Ahmad {w.naeem, r.sutton, s.ahmad}@plymouth.ac.uk Marine and Industrial Dynamic Analysis Group Department o Mechanical and Marine Engineering he University o Plymouth, PL4 8AA, UK Abstract: his paper addresses the issue o guidance and control o an autonomous underwater vehicle (AUV) or a cable tracking problem. A linear quadratic Gaussian controller with loop transer recovery (LQG/LR) is developed because o its strong robustness properties. he vehicle is guided towards the target using a combination o dierent guidance algorithms. he vehicle speed is used to ormulate the guidance problem. Simulation results are presented and a comparison is made between ix and variable AUV speeds. Copyright 23 IFAC Keywords: Guidance, control, LQG/LR, cable tracking, autonomous underwater vehicle. 1. INRODUCION Guidance and control o AUVs have seen a tremendous growth and development in the last ew years and there have been signiicant applications o guidance and control systems or missions such as cable/pipeline tracking, mines clearing operation, deep sea exploration, eature tracking etc. For an AUV to work eectively, a wellintegrated navigation, guidance and control (NGC) system is imperative. A simple block diagram o an NGC system is depicted in Figure 1. he navigation system generates inormation about the vehicle position, velocity, heading etc. using various onboard sensors such as a compass, global positioning system (GPS), pressure sensor etc. he guidance system manipulates the navigation inormation and generates suitable reerences to be ollowed by the AUV. he control system is then responsible or keeping the vehicle on course as speciied by the guidance system. he main dierence between an AUV and ROV (remotely operated vehicle) is that the ROV is controlled by a trained human operator while the AUV is steered by an onboard guidance system. In Position Coordinates Guidance System Set Point Sensors Controller Vehicle Dynamics Vehicle Position Fig. 1. Navigation, guidance and control o a vehicle this regard, the guidance system plays the key role in bringing autonomy to the vehicle. he purpose o this paper is to develop an integrated guidance and control algorithm or an AUV test model, which will eventually be developed and tested in real time in an actual AUV. A plethora o control systems is available to be implemented on an AUV. A good account o various control systems is presented by Craven (1999), while Naeem et al. (23), recently documented a review on various guidance laws or underwater vehicles. odate optimal control theory has been extensively used to solve various control engineering problems. Especially with the advent o powerul digital

2 computers, the computation time is curtailed to a considerable extent. he optimal control is simply a minimisation or maximisation problem or which an objective unction is deined that could involve dierent design parameters or states to optimise. Linear quadratic Gaussian (LQG) is an optimal controller whose name is derived rom the act that it assumes a linear system, quadratic cost unction 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 eedback regulator (LQR) problem is solved which assumes that all states are available or eedback. 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 ilter can be designed to estimate the unmeasured states. he LQR and Kalman ilter can be designed independently and then combined to orm an LQG controller, a act known as the separation principle. Individually the LQR and Kalman ilter have strong robustness properties with gain margin up to ininity and over 6 o phase margin, (Burl, 1999). Unortunately, the LQG has relatively poor stability margins which can be circumvented by using loop transer recovery (LR). A discrete time LQG/LR design is presented in this paper motivated rom the work o Maciejowski (1985). he LR works by adding ictitious noise to the process input which eectively cancels some o the plant zeros and possibly some o the stable poles, and inserts the estimator s zeros (Maciejowski, 1985; Skogestad and Postlethwaite, 1996). he hybrid guidance law developed utilises vehicle speed as a means to ormulate the guidance problem that was irst proposed by Naeem et al., (23) and is simulated in this paper. he paper is organised as ollows. he next section describes the AUV model while Section 3 explains the LQG/LR control system design. Section 4 states the guidance law ormulation and simulation results are presented in Section 5. Finally, concluding remarks are made in Section AUV MODELLING he AUV test model is that used by Kwiesielewicz et al. (21), where the model parameters are given in terms o vehicle speed. Since the guidance law require dierent vehicle speeds, thereore this model has been chosen or demonstration o the proposed algorithm. he singleinput singleoutput (SISO) vehicle model or a given vehicle speed can be described by the ollowing transer unction, as b G( s) = (1) 2 s( s cs d) where the coeicients a, b, c and d are given in terms o vehicle speed v in knots. a =.583 v 2, b =.449 v 3 c = v, d =.856 v 2 he input to the AUV are the rudder delections while the output is the heading o the vehicle. he model parameters are calculated or three dierent vehicle speeds and the resulting continuous time model is discretised at a sampling rate o 1 Hz. he discretized models are then converted into state space controllable canonical orms owing to the LQG controller requirements. he LQG/LR controller requires the model to be minimum phase and should be controllable and observable. he model is tested or these requirements and ound to be good or said purposes. he model AUV is assumed to have a turning radius o 25 m and the constraints on the rudder actuator are maximum 25 degrees in either let or right direction. 3. CONROL SYSEM DESIGN LQG/LR control o an unmanned underwater vehicle (UUV) has been reported by Juul et al., (1994), and riantayllou and Grosenbaugh, (1991). However, both these papers deal with multivariable continuous LQG/LR control o an underwater vehicle assuming that the guidance commands are available. In this paper, a discretetime LQG/LR controller is developed which is more realistic or practical purposes. A guidance law is also developed which generates suitable commands to be ollowed by the vehicle and is the subject o next section. In the ollowing, the LQG/LR controller is developed or the AUV model shown in Section Design Speciications wo types o design speciications are usually given prior to any controller design which are closely related. he time domain speciications involve the maximum overshoot, settling time etc. while the requency domain speciications provides the bandwidth, gain margin (GM), phase margin (PM) etc. o the system. hey can be evaluated by generating the step response and Bode plot o the system respectively. However, in an LQG/LR design, requency tuning is usually desired. A Bode plot o the open loop system (Equation 1), suggests ininite gain at zero requency thereore, gain crossover requency (gc) is used as a measure o the bandwidth o the system. he desired gc o the open loop system or all vehicle speeds is 1 Hz. An acceptable nominal design usually is one that attains both a GM 3 db and PM 3 o, (Wolovich, 1994). he desired GM in this case is set at 1 db while the PM at 53 o, well above the nominal values. 3.2 Kalman Filter Design Since the heading o the AUV corrupted by noise is the only measured variable, the remaining states have to be measured through a state estimator prior to

3 control calculations. A current estimator is used because the estimate is based on the current measurement. his 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 orm as x( k 1) = Ax( Bu( y( = Cx( (2) he design objective is to ind the Kalman gain K such that the estimate o x( is optimal. he solution to this problem is given by the discrete steady state Kalman ilter gain equation given by (Franklin et al., 1998; Maciejowski, 1985) K 1 = PC ( CPC V) (3) where V is the measurement noise spectral density matrix and P is the steady state error covariance matrix given by the solution o a discrete steady state Riccatti equation, (Maciejowski, 1985) 1 P = APA APC ( CPC V) CPA W (4) where W is the process noise spectral density matrix. he parameters W and V are tuned until the desired ilter s openloop return ratio (z) speciications are met which is shown below 3.3 LQR Design 1 ( z) = C ( zi A) A * K (5) Once the Kalman gain is evaluated or the desired speciications or all models, the LQR state eedback gains are calculated. An objective unction is minimised given by [ ( Qx( u ( Ru( ] 1 N J = x (6) 2 k = where the weighting matrices Q and R are chosen according to Maciejowski (1985) as Set Point N e Q = C C, R LQR Saturation Nonlinearity u x^ AUV Kalman State Estimator y Vehicle Position (7) Fig. 2. LQG controller showing LQR gain and state estimator he above values provide asymptotic recovery o the stability margins, given that the plant obeys some speciic characteristics. he state eedback matrix K c is obtained by solving equations dual to Equations 3 and 4, and is used to generate the control according to u( = K cxˆ( (8) where xˆ is the estimate o the state x given by Equation 2, and u is the control action. he closed orm solution o K c or the values o Q and R in Equation 7, is given by Maciejowski (1985) as K c = (CB) 1 CA (9) A eedback compensator is inally synthesised as a series connection o the Kalman ilter and the optimal stateeedback controller as depicted in Figure 2 given by (Maciejowski, 1985) xˆ( k 1) = ( A BKc K CA K CBKc )ˆ( x L ( A BK c ) K e( (1) u( = K ( I K C)ˆ( x K K e( c where e = r x, is the error between a reerence signal r and desired state x. Let G(z) is the transer unction o the system deined by Equation 2 and H(z) is the compensator transer unction. I the plant G(z) is minimum phase and det (CB), then ull recovery is achieved i G ( z) H( z) = ( z) (11) where G(z)H(z)is called the loop transer unction. 4. GUIDANCE LAW he objective o any guidance law is to steer the AUV so that it intercepts the target in minimum time and maximum accuracy. he guidance law used in this paper utilises AUV speed as a means to ormulate the problem. he complete mission is classiied into our dierent phases utilising dierent guidance laws. hese are i) launch phase, ii) midcourse phase, iii) terminal phase, and iv) tracking phase as shown in Figure 3. In the irst phase called the launch phase or the boost phase, the vehicle is launched rom a vessel and guided in the direction o the line o 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 ollows the LOS angle with maximum speed using the way point guidance, (Healey and Lienard, 1993). During this part o the light, changes may be required to bring the vehicle onto the desired course and to make certain that it stays on that course. he midcourse guidance system is used to place the vehicle near the target area, where the system to be used in the inal phase o guidance can take over. It should be noted that there is no need or the vehicle to submerge at this stage, as the objective is to approach the target area with maximum accuracy regardless o the orientation o c

4 the vehicle with respect to the cable. When the vehicle reaches within the circle o 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. he circle o acceptance in this case as opposed to Healey and Lienard (1993), should be taken at least the minimum turning radius o the vehicle in order to avoid overshoot. Finally, when the vehicle enters the waypoint, the ourth 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 ollow the cable. I the cable to be ollowed is an electrical/ communication cable, then magnetometers could be used to detect the radiation rom the cable and guide the vehicle in the appropriate direction (Naeem et al., 23). o implement the guidance law, it is necessary to compute the LOS angle λ. his requires relative positions o the AUV and target in both the coordinates i.e., he kinematic equations o the AUV are stated below and represents the components o the velocity in the (x, y) plane V V = x V p = y V p cosψ p sinψ p where ψ p and V p represents the actual heading and velocity o the AUV respectively. he speed o the AUV is regulated at three dierent values used in dierent phases o the mission as mentioned above. he components (x v, y v ) o the AUV position can be evaluated by integrating the velocities (V x, V y ), respectively. In addition to the LOS angle rom the vehicle to the target, the guidance system also generates the range (distance) o the AUV rom the target. he range measure is used to switch between dierent pretuned controllers. he guidance subsystem block diagram is shown in Figure 4 and is implemented in Simulink environment. 5. SIMULAION RESULS thereore, h = yt yv r = xt xv 1 λ = tan erminal h r circle o acceptance cable racking he proposed integrated guidance and control algorithm is implemented on the AUV model shown in Section 2 in Matlab/Simulink environment. he ollowing assumptions are taken or the simulations: i) he AUV and target are in the same plane. ii) Complete navigational inormation is available through onboard sensors. iii) A complete knowledge o the target s motion is available to the AUV. iv) he AUV is equipped with a vision system that generates the coordinates o the points on the cable to be tracked. v) he initial target coordinates (one end o the cable) are known prior to the mission. AUV Launch LOS Midcourse AUV Waypoint Fig. 3. Planar view o the our phases o light or cable tracking problem o an AUV. V p ψ p Polar to Cartesian. V x = x v. V y = y v x v y v x t y t Cartesian to Polar Fig. 4. Guidance subsystem block diagram Range LOS angle he irst step in any LQG/LR control problem is to design a target ilter s openloop return ratio given by Equation 5, which requires the Kalman gain to be evaluated. By manipulating the spectral density matrices W and V in Equations 3 and 4, the Kalman ilter can be designed and hence the target ilter s openloop return ratio. In this paper, the procedure adopted by Weerasooriya and Phan (1995) is ollowed. Keeping the measurement noise spectral density ixed at unity and tuning the process noise spectral density matrix, until the desired requency domain speciications are met. he Bode plot o the desired ilter s openloop return ratio or the 1 knots speed model o the vehicle is shown in Figure 5. he GM, PM and gc can be readily evaluated rom the plot. he next step is to calculate the eedback gains using the optimal Q and R in Equation 7 and develop the LQG/LR compensator using Equation 1. he loop transer unction G(z)H(z) is also evaluated and the Bode plot superimposed on the Bode plot o the openloop return ratio in Figure 6 shows the amount o recovery achieved. In this case, ull recovery is

5 achieved as the two plots overlap each other. Figure 7 presents the step response o the closed loop eedback system showing a large overshoot. his can be reduced by adding more damping to the system by introducing a weighting actor on the diagonal term o Q corresponding to the velocity state. his is equivalent to using rate eedback or improving damping rom a conventional sense (Weerasooriya and Phan, 1995). Figure 8 depicts the step response o the closed loop system with modiied Q and Figure 9 presents the Bode plot o the loop transer unction. Although overshoot has subsided but at the cost o reduced stability margins due to the deviation rom the optimal values. he 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 1 or a cable tracking mission, which clearly shows good tracking behaviour using the proposed guidance algorithm. he control surace delections generated by the controller is depicted in Figures 11 and 12 or the case o optimal and modiied Q respectively. Clearly, the modiied Q with additional damping causes less variation in the control input as compared to optimal Q but at the cost o reduced stability margins. However, both igures suggest that the delections are within the constrained actuator limits. 6. CONCLUSION his paper demonstrates an integrated guidance and control system approach using an LQG/LR controller and a hybrid guidance law. he LQR/LR controller is synthesised in discretetime and a hybrid guidance law is proposed which uses dierent vehicle speeds in dierent phases o the mission. Simulation results are presented to show the robustness properties o the proposed integrated system. Results or a cableollowing mission also depicts good tracking perormance. A SISO system is used or the simulations, however, a multivariable LQG/LR integrated with the proposed guidance system is an area o ongoing research. Magnitude (db) (deg) Bode Plots o the OpenLoop Return Ration and Recovered Loop ranser Function Frequency (rad/s) Desired Open Loop Return Ratio Recovered Loop ranser Function Fig. 6. Bode plots o the ilter s openloop return ratio and recovered loop transer unction or nominal Q (ull recovery) Step Response Step Response o the Closed Loop System Using Nominal Q ime (in samples) Fig. 7. Step response o the closed loop system or Q=C C and R. Step Response Step Response o the Closed Loop System with Modiied Q ime (in samples) Fig. 8. Step response o the closed loop dystem or modiied Q & R 8 Bode Plot o the Desired ilter s OpenLoop Return Ratio 8 Superimposed Bode Plot o the OpenLoop Return Ratio and Recovered Loop ranser Function or Modiied Q Magnitude (db) Magnitude (db) (deg) Frequency (rad/s) Fig. 5. Bode plot o the target s ilter openloop return ratio (deg) Frequency (rad/s) Fig. 9. Bode plots o the ilter s openloop return ratio (solid line) and recovered loop transer unction (dashed line) with added damping, (reduced stability margins)

6 Vehicle YCoordinates launching position AUV Cable racking Mission, Fix Speed vs. Variable Speed LOS waypoint ix speed vehicle coordinates circle o acceptance Vehicle XCoordinates variable speed vehicle coordinates Fig. 1. Cable tracking mission rom launching to tracking, variable speed vs. ixed speed Control Surace Delections Cable Rudder Delections Generated by the Controller or Nominal Q ime (Samples) Fig. 11. Rudder delections generated by the LQG/LR controller or Q=C C & R. Control Surace Delections Rudder Delections Generated by the Controller or Modiied Q Healey, A. J., and D. Lienard, (1993). Multivariable Sliding Model Control or Autonomous Diving and Steering o Unmanned Underwater Vehicles. IEEE Journal o Oceanic Engineering, vol. 18, no. 3, pp , July. Juul, D. L., M. McDermott, E. L. Nelson, D. M. Barnett and G. N. Williams (1994). Submersible Control Using the Linear Quadratic Gaussian with Loop ranser Recovery Method. In: Proceedings o the Symposium on Autonomous Underwater echnology, pp , July, Cambridge, MA, USA. Kwiesielewicz, M., W. Piotrowski and R. Sutton (21). Predictive Versus Fuzzy Control o Autonomous Underwater Vehicle. IEEE International Conerence on Methods and Models in Automation and Robotics, pp 69612, 2831 August, Miedzyzdroje, Poland. Maciejowski, J. M., (1985). Asymptotic Recovery or Discreteime Systems. IEEE ransactions on Automatic Control, vol. AC3, no. 6, pp. 6265, June. Naeem, W., R. Sutton, S. M. Ahmad, and R. S. Burns, (23). A Review o Guidance Laws Applicable to Unmanned Underwater Vehicles. o be published in he Journal o Navigation. Vol. 56, no., pp 115. Skogestad, S. and I. Postlethwaite, (1996). Multivariable Feedback Control: Analysis and Design using Frequencydomain Methods. John Wiley and Sons Ltd. riantayllou, M. S., and M. A. Grosenbaugh, (1991). Robust Control or Underwater Vehicle Systems with ime Delays. IEEE Journal o Oceanic Engineering, vol. 16, no. 1, pp , January. Weerasooriya, S. and D.. Phan, (1995). Discrete ime LQG/LR Design and Modelling o a Disk Drive Actuator racking Servo System. IEEE ransactions on Industrial Electronics, vol. 42, no. 3, pp 24247,June. Wolovich, W. A., (1994). Automatic Control Systems, Basic Analysis and Design. International Edition, Saunders College Publishing ime (Samples) Fig. 12. Rudder delections generated by the LQG/LR controller or modiied Q & R REFERENCES Burl, J. B., (1999). Linear Optimal Control, H 2 and H Methods. AddisonWesley Longman Inc. Craven, P. J. (1999). Intelligent control strategies or an autonomous underwater vehicle. PhD hesis, University o Plymouth, UK Franklin, G. F., J. D. Powell and M. Workman (1998). Digital Control o Dynamic Systems, 3 rd ed. AddisonWesley Longman Inc.