A Control System for Automated Docking of an Unmanned Underwater Vehicle

(1)

A Control System for Automated Docking of an Unmanned Underwater Vehicle

E R I K R U N D Q V I S T

Master's Degree Project Stockholm, Sweden 2005

(2)

ABSTRACT

Unmanned underwater vehicles (UUV) are receiving increased attention for both military and civilian applications. For example, UUVs were deployed in the war against Iraq for mine counter measure missions and are becoming a necessary tool in the deep sea mining industry. If UUVs were able to dock while submerged, it would greatly increase their efficiency, as well as reduce the cost involved in their deployment and recovery. The ability to both operate and dock in a submerged state would also provide the UUV a degree of stealth. A future application where UUVs would prove a valuable asset is for harbor control where stealth is an essential quality. A submerged docking bay would enable the UUV to both upload completed mission data and download new mission objectives while recharging its batteries. To succeed in this endeavor, a control system capable of guiding the UUV safely into an underwater docking bay is required. This thesis describes the development of two different control algorithms, a fuzzy controller and a Linear Quadratic Regulator (LQR). For simulation purposes, a 3D model of a small UUV is generated using ADAMS/view software. Another model is generated in Simulink/MATLAB using the equations of motions for the UUV, yielding faster and more numerically stable simulations. Both models are including the effects of water drag and have vertical and horizontal rudders and thruster inputs. The controllers are built using Simulink/MATLAB and the simulations are run either using Simulink/MATLAB entirely or in co-simulation mode with ADAMS, enabling a more graphic representation of the results. The UUV chosen for this thesis is called REMUS and is developed by Woods Hole Oceanographic Institute. It measures 1.6 m and weighs 37 kg. Included in this work is a thorough analysis of both controllers including key results enabling a comparison of the two controllers performance. A number of requirements were set up for the controllers and except for water current disturbances, both controllers met their requirements. When off course the presented LQR is able to steer the UUV back on course more quickly and smoothly than the Fuzzy controller. The conclusion of this thesis when combining all characteristics of the different controllers’ performance is that the LQR is the natural choice of controller for autonomous underwater docking of an UUV. The research described in this thesis has the potential to further increase the efficiency of UUVs by enabling underwater docking. The ultimate future work would naturally be to test the described controllers on the REMUS in a test tank to verify the results presented in this thesis.

(3)

Acknowledgments

The following people have given important assistance and support concerning this work, and without any of them this work would never have been possible to accomplish.

At the AMAS department at the University of Florida, I would first like to thank Dr.

Gloria J. Wiens for taking me in, for giving me the opportunity to work with such an interesting project, and for all the assistance along the way. I would like to thank Dr.

Norman G. Fitz-Coy for his assistance, and his ability to find solutions when the work seemed to be stuck in a dead end. I would also like to thank all the people working in the AMAS lab and especially the SAMM group for their eagerness to answer any questions.

At KTH, I would like to thank Dr. Bo Wahlberg for accepting me and for his help reading and finalizing this thesis.

I would also like to thank Theresia Jonsson for many fruitful conversations and the solution to many problems that arose during the project. Finally I would like to thank Dr.

Rune E. Lindgren for making it possible for me to perform my thesis project in the US and for supplying me with a place to stay in Gainesville.

(4)

LIST OF FIGURES

Fig 1: UUV and docking bay ... 8

Fig 2: The REMUS ... 9

Fig 3: Handover error tolerance. Worst accepted starting position and orientation ... 13

Fig 4: UUV at desired destination position, observed from the side ... 14

Fig 5: UUV at desired destination position, observed from the front... 14

Fig 6: Closed loop system... 15

Fig 7: The model of the REMUS, as it looks in ADAMS/View ... 17

Fig 8: Fin and fin joint attaching fin to UUV body ... 18

Fig 9: Lift and drag forces ... 22

Fig 10: Fuzzy states for the controller inputs, N=negative, P=positive... 28

Fig 11: Virtual docking funnel... 33

Fig 12: Feedback loop system presentation ... 40

Fig 13: Step in x using initial versus final LQR parameters... 43

Fig 14: Step response in y using Fuzzy versus LQR control... 45

Fig 15: Step response in x using Fuzzy resp. LQR controller ... 46

Fig 16: Worst case scenario, y-offset... 48

Fig 17: Worst case scenario, z-offset ... 48

Fig 18: Worst case scenario, pitch angle ... 48

Fig 19: Worst case scenario, yaw angle... 49

Fig 20: Worst case scenario, x-offset... 49

Fig 21: Worst case scenario, roll angle... 50

Fig 22: Worst case scenario, Thruster force ... 50

Fig 23: Current affecting the UUV ... 52

Fig 24: Relevant plots of docking scenario with abort function... 55

Fig 25: Non zero plant step responses for Input 1 – Thruster force ... 58

Fig 26: Non zero plant step responses for Input 2 – vertical rudder angle ... 59

Fig 27: Non zero plant step responses for Input 3 – horizontal rudder angle ... 60

Fig 28: Non zero plant step responses for Input 4 – keel torque ... 61

Fig 29: Worst case scenario, vertical rudder angle ... 63

Fig 30: Worst case scenario, horizontal rudder angle... 63

Fig 31: Water current applied ... 64

Fig 32: Noise on input to system,ω_u ... 65

Fig 33: Noise on output from system ( )^G ^,ω ... 66

Fig 34: Disturbance used to trigger abort function ... 66

Fig 35: Definition of alignment distance ... 68

Fig 36: Step response with definitions... 68

(7)

LIST OF TABLES

Table 1: Controller output notations from first set of rules ... 29

Table 2: Controller output values: thruster force ... 29

Table 3: Controller output notation and values for vertical rudder angle... 31

Table 4: Controller output states and values for horizontal rudder angle... 32

Table 5: Abort function boundaries ... 33

Table 6: Couplings between inputs and outputs ... 41

Table 7: Results from docking scenario using initial Q and R values ... 42

Table 8: Results from docking scenario using final Q and R values ... 43

Table 9: Step response in y (step size=1m) ... 45

Table 10: Step response in x (step size=40 m) ... 46

Table 11: Docking Scenario from worst case starting point... 47

Table 12: Simulation with 1 m/s current added to the system along the y-axis direction 52 Table 13: Amplitude of noise applied to the closed loop system ... 53

Table 14: Docking Scenario from worst case starting point with noise applied ... 53

Table 16: Coefficients... 69

Table 17: Notations for input signals to the system... 71

Table 18: Notations for the states of the system ... 71

(8)

1 INTRODUCTION

1.1 Background

Today’s need for increased surveillance to protect our harbors from terrorist operations heavily relies on the ability of Unmanned Underwater Vehicles (UUVs) to maneuver and gather reconnaissance undetected. This ability is also crucial to our ability to locate and disable underwater mines in foreign waters. The first cable-controlled UUVs were developed in the 1950’s. Today, the UUV is used for deep sea mapping and exploration, environmental surveys and monitoring, military reconnaissance and surveillance, and numerous applications in the oil industry. In their use, the UUV has been operated either as an Autonomous Underwater Vehicle (AUV) or a Remotely Operated Vehicle (ROV).

The anticipated impact of autonomy in UUVs is reflected by statement of Bob Barton [1]:

“Autonomous underwater vehicles (AUVs) will soon dictate the design of deepwater oilfield development, production and IRM facilities, from simple wellheads to subsea processing facilities.” Thus the two major areas of current UUV deployment is in the oil industry and the military. Presently, most military applications involve mine reconnaissance and counter measure operations for harbor control.

As the UUV becomes more reliable and advanced, the range of applications will increase.

The UUV holds many advantages compared to Manned Underwater Vehicles (MOV).

The UUVs can work in harsh conditions that no human could bear, they can be operated continuously and the risk involved in operating the vehicle is minimal. The size of the UUV can also be significantly decreased compared to MOVs with decreased fuel consumption, heightened agility and easier deployment and recovery process as result.

The ROVs however have the inherited limitations associated with range limits due to cable length or communication range. When making use of acoustic modems, the communication speed is also limiting the overall performance of the UUV and the ability to remain undetected. Hence, the main limitation for UUVs (both AUVs and ROVs) in terms of range and length of operations is the battery performance. Other than that, the guidance, navigation and control (GNC) system is what determines for what applications the UUV can be used.

Today, most UUVs are deployed and recovered from surface vessels or ground bases.

This is a costly process associated with considerable dangers both for the UUV and the personnel involved in the process. This especially applies when deployment or recovery is performed in harsh conditions. For some applications such as environmental studies these extreme conditions are sometimes the exact conditions the UUV is to examine. As stated at the Marcus Evans Conference [2], “The ability to recover and re-launch vehicles will be an essential, cost saving feature of operational efficiency.” Therefore, to be able to dock autonomously and underwater would greatly reduce the cost of operating the UUV and the risks involved. In addition, an underwater docking bay for the UUV

(9)

could also allow mission data uploads and downloads of new assignments as the UUV recharges. When used for military applications the ability to dock underwater also supplies the UUV with a level of stealth by not having to surface between its missions.

Furthermore, the time that can be spent carrying out missions is increased due to the time lost when deploying and recovering the UUV has been reduced. The underwater docking bay is usually tethered to a surface vessel or base. Docking bays tethered to a buoy would in the future enable UUVs to be used without the immediate presence of humans.

1.2 Problem Overview

This thesis focuses on the control system required for autonomous docking of an UUV with an underwater docking bay. The scenario considered is as follows: a Guidance, Navigation and Control (GNC) system takes the UUV to a waypoint 40 m from the end- wall of the docking bay where the docking control system seizes control of the UUV and guides it safely into the docking bay, see Fig 1. In this thesis, the research focuses on the docking control design and analysis. The docking control system should be able to safely guide the UUV into the docking bay. Requirements on the docking control system apply to the ability to compensate for errors in position and orientation when handed over from GNC, ability to coupe with disturbances, and achieving an acceptable position and orientation when entering the docking bay. Two control methods, Fuzzy Logic and LQR are explored and compared in this thesis.

Fig 1: UUV and docking bay 40 m

Docking bay UUV at waypoint

Desired path

offset

x

(10)

The UUV considered as a model for this thesis is called REMUS (Remote Environmental Monitoring Units). The REMUS is a UUV developed by Woods Hole Oceanographic Institution [3] and is manufactured by Hydroid Inc [4].

Fig 2: The REMUS

The REMUS is 1.6 m long and weights 37 kg [4]. While many commercially deployed UUVs are both larger and heavier, a smaller unit suits the purpose better for experimental and testing applications. In addition, in-depth physical details on the REMUS are readily available in the literature [4]. Although in this thesis no experimental tests on the real UUV will be performed, using a rather small UUV such as the REMUS as model will simplify future work involving system testing.

1.3 Literature Overview

There are a number of different factors involved in an autonomous underwater docking process of an AUV. First a GNC system must be able to guide the AUV to a distance from which docking is possible. See Naeem et al [5] for an overview of different guidance techniques. Next, both when in mission mode and docking mode, the sensor capabilities sets boundaries for what can be accomplished. As a consequence, the accuracy of the sensors as well as their functionality can potentially be the limiting factors of the overall performance of the controller. These capabilities can be expressed in terms of what tasks that can be performed and what inputs are available for the control systems.

There are a number of available underwater sensors including optical, acoustic and electromagnetic. For long range sensors, sonar and hydrophones are most common.

(11)

proximity of the dock. Evans et al. [6] reports that sonar can be used to guide the UUV to within a few meters from the docking bay. For the final part of the docking sequence the need for high accuracy is essential to guarantee a safe docking of the UUV. The most common sensors used for this purpose are optical-, or vision-based sensors, see Evans et al. [6]. Vision based sensors impose limitations on the number of applications for which the system can be utilized. In dark or murky waters the vision based sensors accuracy may be significantly reduced. To account for the reduced visibility, lights on the docking bay acting as homing beacons may be used, see Lee et al [7]. When utilizing electromagnetic sensors, the docking bay generates a magnetic field used by sensor coils within the AUV to measure the bearing and orientation relative to the docking bay [8].

The controller used for the docking sequence can be designed in different ways. The typically considered control approaches are Proportional-Integral-Derivative Control (PID), Gain-scheduling Control, Linear Quadratic Gaussian (LQG) Control, Adaptive Control, H-Infinity Control

( )

^H∞ , Fuzzy Logic approaches and Neural Network Methods (Craven et al [9]). The following subsections summarize each of these methods.

1.3.1 PID-control

The PID controller has been implemented for a long time with various results, see for example Lots et. al [10]. It is highly dependent of an accurate linearized model of the vehicle. When the system is operating far from the operating point used for the linearization, the performance of the PID controller decreases significantly. The ability to tune the PID controller also limits how well it may perform.

1.3.2 Gain-scheduling and adaptive control

Gain-scheduling is a method which makes the controller less vulnerable to errors caused by operation far away from the operating point. Using this approach a number of linearizations are made about different operating points. A scheduler is then used to determine which linearization best describes the system at any given time, and uses a set of gains associated with that linearization. A problem with this method is designing the scheduler in a way that prevents the controller outputs from exhibiting “jumps” when the scheduler switches from one set of gains to another. This is a method usually used in combination with other control methods, such as PID and LQG. The use of adaptive control is another way to account for the vehicle operating under different circumstances.

As with gain-scheduling, the gains are changed to adjust the controller to the new operating point. This is done by using a gain adjusting algorithm to recalculate the gains at every step. Designing this algorithm without causing the system to become unstable at any point can be very difficult. See Sarkar et al. [11] for an example of adaptive control.

(12)

1.3.3 LQG (Linear Quadratic Gaussian) control

Robust control design using LQG control theory generates an optimized controller. The control strategy requires a linearized representation of the plant. A trade off between minimizing the input signals to the plant and the outputs from the plant is set manually.

This setting should be based on the type of application where the controller is implemented. The LQG technique then derives the optimal gains for the operating point used in the linearization. When applying to a UUV which is a highly nonlinear and coupled system, the success of a LQG controller, also known as a LQR (Linear Quadratic Regulator) depends heavily on the choice of operating point and the ability to derive a linearization of the plant with high accuracy. Naeem et al [12] successfully used a LQR though they only tried it on a single input single output (SISO) system.

1.3.4 H_∞-control

The H_∞based control is another approach for designing a robust controller capable of handling the differences between the physical plant and the model of the plant used for controller design. Designing the weighting matrices requires careful consideration for optimal performance. Common problems involve controller demanded inputs that are higher than what is possible due to saturation.

1.3.5 Fuzzy logic control

Fuzzy Logic based controllers is designed to mimic the way humans control complex nonlinear systems. The range of all input variables is split up into states and then rules are established for every combination of these states. An output signal is then assigned to each rule and these outputs are weighted together to generate the output signal from the controller. A more detailed description of the technique can be found in Section 3:

FUZZY CONTROL. Fuzzy logic provides a controller which is well suited to handle nonlinear, time dependent and complex systems. This type of controller provides an extremely robust controller. The ability to design rules and assign outputs limits how well this controller performs. The fuzzy approach is well suited for pitch control of UUVs normal GNC, see DeBitetto [13], and docking control, see White et al [14]. White used a virtual funnel using fuzzy logic for docking of an UUV.

1.4 Problem Definition

As stated previously, this thesis focuses on the design and analysis of the docking control system required for autonomous docking of an UUV with an underwater docking bay.

The docking controller is assumed to commence control of the UUV at its initial waypoint 40 m in front of the docking bay and for safely guiding the UUV into the docking bay. An abort control is also considered and implemented to enable the UUV to

(13)

abort the docking sequence if it is too far away from its desired course. In the abort scenario, the UUV turns around returns to the initial waypoint where it restarts the docking sequence.

1.5 Boundaries of thesis

The research in this thesis is limited to only the design and analysis of the docking control strategies of the UUV. It does not include a thorough analysis of GNC systems used for general maneuvering of the UUV outside the docking mode. No tests of the controllers on the real UUV are performed, instead the model developed in ADAMS/View is used to represent the real UUV and all test results are derived from the ADAMS/View model. Weight distributions and change of pay load on the UUV are also disregarded and the UUV is considered to have a homogenous weight distribution. The docking bay itself was designed by Berglund [15] and will only briefly be considered in this thesis.

1.6 Requirements

The requirements on the system can be divided into five parts: handover error tolerance, robustness, sensitivity, speed and docking accuracy.

1.6.1 Handover error tolerance

Handover error tolerance is a measure of the controller’s ability to handle errors involved with the handover process. These errors are introduced by the GNC system that takes the UUV into proximity of the dock, i.e. to the waypoint. The position and orientation of the UUV will always differ from the desired values when the handover is executed. How large these errors are depends on the accuracy of the GNC system.

For this thesis, the handover error tolerance is set to a 2 m offset in every direction, a maximal roll angle of±π 8 rad and a maximal pitch angle of ±π 4 rad and no limit on the yaw angle, i.e. − < ≤ , see Fig 3. This means the GNC must be accurate enough π ψ π to deliver the UUV to within a 4 4 4× × m square with the waypoint in the middle and under the stated constraints to orientation, see Fig 7 for coordinate definitions.

(14)

Fig 3: Handover error tolerance. UUV placed at worst accepted starting position and orientation (Red cube represents boundaries of accepted starting position)

1.6.2 Robustness

Robustness is an assessment of the controller’s capability to account for errors in the model used for controller derivation. No specific requirements are set for the robustness of the control system.

1.6.3 Speed

The speed is a measure of how fast the controller can adjust the UUV’s location and orientation to the desired values. The only requirement concerning speed is that the UUV must be able to align its course to the desired path before entering the docking bay. This means within 38 m for this thesis since the waypoint is placed 40 m from the dock and maximal accepted error is 2 m as stated in Section 1.6.1: Handover error tolerance.

1.6.4 Docking accuracy

Docking accuracy is a measure of how accurately the controller must be able to control the UUV’s motions in order to guarantee a safe docking sequence. Described in Fig 4 and Fig 5, the docking bay has an inner radius of 0.25 m and the UUV has a maximal radius of 0.095 m. Hence, upon entering the docking bay the UUV has an error margin of 0.155 m. The desired destination is 0.1 m from the end wall of the docking bay.

Desired path

x y

z

Waypoint

(15)

Fig 4: UUV at desired destination position, observed from the side

Fig 5: UUV at desired destination position, observed from the front 0.1 m

0.095m

0.155m

0.25m

(16)

From these values the following allowed error margins were determined. Upon entering the docking bay the UUV is limited to a maximum displacement of 0.05 m in y and z direction and it is considered settled when within 0.05 m from reference value for x.

Orientation offset is also limited for the docking phase to a maximum pitch and yaw angle of ±π 32rad.

1.6.5 Sensitivity

The sensitivity of a system indicates how well the controller can handle disturbances. For the UUV, a realistic disturbance that will be evaluated is water current. Normal values for currents range from 0 m/s to 1 m/s. For this thesis, the controller is required to be able to account for currents up to 1 m/s since this is also the maximum current tolerated for operating the UUV, see [16]. Waves will not be considered at this time. Noise will also be considered, for example due to sensor input errors. White noise is applied to the input to the system and to the output from the system, i.e. the input to the controller in order to test the sensitivity of the system, G, see Fig 6. On the input signals, the noise is defined to have a magnitude of 10% of the maximal input value. On the output signals, the noise has a magnitude equal to two times the accepted error margin for docking accuracy of the corresponding state, see Section 1.6.4. Therefore, the magnitude of the noise on the position variables is 0.1 and π 16 on the orientation variables.

Fig 6: Closed loop system

1.7 Assumptions

For this thesis, a number of assumptions have been made in order to keep the focus of the thesis on the docking control system of the UUV. The assumptions that are made do not compromise the models authenticity or overall behavior in a way that renders it not useful for the purposes of this thesis. However, due to the assumptions, the model becomes considerably less complex yet still behaves reasonably well compared to the expected behavior of a real system.

Reference signal u_in

Disturbance, ω

Output of the system, y Disturbance, ω_u

-K

K G

Plant

(17)

The model of the body assumes the center of rotation is located at the center of mass, the origin of the body fixed coordinate frame

(

^B^x^,^B^y^,^B^z

)

, see Fig 7. It is further assumed that the center of buoyancy remains unchanged and is coincident with the center of mass.

As shown in Fig 7, the system’s six degrees of freedom are defined in coordinates x, y, z for displacement or

(

^B^x^,^B^y^,^B^z

)

in body fixed coordinates, and roll-pitch-yaw

(

^{φ θ ψ}^{, ,}

)

^.

The origin of the body-fixed frame

(

^B^x^,^B^y^,^B^z

)

is located at the center of mass. The corresponding rates (p, q, r) and moments

(

^B^M^x^,^B^M^y^,^B^M^z

)

are defined about the body- fixed axes.

The fins of the UUV are coupled in pairs preventing any active control of the roll angle of the vehicle. The weight distribution of the UUV may be such that it gives the effect of a keel which in that case would stabilize the roll angle at zero degrees. For completeness as well as simplicity, the keel is implemented as a torque opposing the roll angle and with a magnitude defined as a function of the roll angle.

An assumption regarding the fins is that they move symmetrically in pairs and their controlled motion is instantaneous. Each pair of fins is called a rudder. Fins that fail to move simultaneously can be considered as a noise in the system which the controller is supposed to handle. When applying a controller to the physical UUV, sudden changes in fin angles are undesired. The aim is to design the controller in such a way that it generates a continuous motion of the fins. As long as no “jumps” in fin angles are made, the model does not differ significantly from the physical system. This also applies to changes in propeller thrust force. Since the aim is to slowly place the UUV inside the docking bay, no jumps in propeller thrust input should occur.

To maintain its altitude under water, the UUV uses buoyancy control. In this thesis, the buoyancy force is assumed to be equal to the gravitational force, resulting in a form of weightlessness. Skin friction acting on the body of the UUV plays a minor part to the dynamics of the UUV compared to the water drag forces and are therefore assumed to be zero, except for the model of the keel where it will be implemented due to the oscillations in roll angle that arise otherwise.

The implementation of drag is essential for authenticity of the dynamics of the UUV. For the physical system, the drag is distributed evenly over the whole body, opposing motion.

For this thesis, the cross flow body drag is divided into two parts, one as a result of water drag from the front half of the body, and one as a result of water drag from the aft half of the body, see Fig 7. This approach results in a drag torque opposing the pitch and yaw rates of the UUV. The torque will stabilize these rates and without other forces acting on the UUV will eventually bring them to zero, similar to the scenario of the real system. If the total drag force affecting the UUV would be applied at the center of mass, a pure

(18)

rotational motion about the center of mass would never settle. The body drag force along the longitudinal axis of the UUV is applied only as one force, since it has no stabilizing effects on any angular velocities of the UUV. The drag forces introduced by the fins are neglected since it is considered small in comparison to the drag of the body. While applying drag uniformly over the body and adding drag forces from the fins would give a more authentic model, the model used in this thesis is considered to capture the most important features and effects of the water drag acting on the UUV.

The signals that are used as inputs to the controller are made available by sensors on the UUV. It is assumed that from the sensors signals position, orientation, velocities and angular rates can be calculated.

Fig 7: The model of the REMUS, as it looks in ADAMS/View Thruster Force

Front Drag Forces -^BFdf

Aft Drag forces -^BFda

Lift Forces from fins - ^BFl

Drag Force in ^Bx

direction -^BFd_x

, x

φ

, y

θ

,

z

ψ

q_,^BM_y

Inertial coordinate system Body-fixed coordinate system

, , ^Bx u ^BF_x

, ,

B B

y v Fy

, ,

B B

z w Fz

, ^B _x

p M

, ^B _z

r M

(19)

2 MODEL

A dynamics model of the AUV was generated using a software program called ADAMS/View. ADAMS/View is a sophisticated program capable of handling complex dynamics and presenting the data visually. All these capabilities are available in 3D. The ADAMS model of the UUV was developed with reference to the REMUS. The dimensions of the model are therefore similar to the dimensions of the REMUS. The model consists of five parts, the main body and the four fins. The fins are attached to the body by revolute joints that rotate about an axis formed by the front edge of the fins, see Fig 8. The fin angles are controlled by imposing motion in these joints. The propulsion system is modeled by a force in the^Bxdirection applied at the center of the fins. A keel is implemented by adding a Torque about the^Bx-axis that is negatively proportional to the roll angle and the roll rate. The change in mass distribution due to the keel and the fins is disregarded. In addition, the products of inertia (I_xy,I_xz,I_yz) are small compared to the moments of inertia (I_xx,I_yy,I_zz).With the inertia tensor defined with respect to the body- fixed origin at the center of mass, see Fig 9. Therefore, they are assumed to be zero.

Fig 8: Fin and fin joint attaching fin to UUV body

In order to fully describe the UUV’s motion, Euler Angles are used to describe the orientation of the vehicle’s body-fixed frame

(

^B^x^,^B^y^,^B^z

)

with respect to the inertial coordinate system

(

x y z . There are twelve different Euler Angle coordinate system ^{, ,}

)

definitions but for naval applications the 321 convention is standard. It is also called Roll

( )

^φ ^-Pitch

( )

^θ ^-Yaw

( )

^ψ . To position the vehicle using Euler Angles a sequence of

Fin Fin joint

(20)

rotations about the inertia coordinates x, y and z is performed. Since the sequence is 321, the first rotation is about the z-axis

[ ]

,

( ) ^{( )} ( ) ^{( )}

cos sin 0

sin cos 0

0 0 1

R z_ψ

ψ ψ

−

⎡ ⎤

⎢ ⎥

= ⎢ ⎥

⎢ ⎥

⎣ ⎦

(1)

The second rotation is about the new y-axis produced by the previous rotation

[ ] ^{( )} ^{( )}

( ) ( )

,

cos 0 sin

0 1 0

sin 0 cos R y_θ

θ θ

⎡ ⎤

⎢ ⎥

= ⎢ ⎥

⎢− ⎥

⎣ ⎦

(2)

and the final rotation is about the new x-axis, produced by the two previous rotations.

[ ] ( ) ( )

( ) ( )

,

1 0 0

0 cos sin

0 sin cos

R x_φ φ φ

φ φ

⎡ ⎤

⎢ ⎥

=⎢ − ⎥

⎢ ⎥

⎣ ⎦

(3)

By multiplying the

[ ]

R -matrices, a transformation matrix defining the body-fixed coordinate system with respect to the inertial coordinate system is generated.

[ ] [ ] [ ] [ ]

R R z, R y, R x,

ψ θ φ

= (4)

[ ]

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )

cos cos sin sin cos cos sin cos sin cos sin sin

cos sin sin sin sin cos cos cos sin sin sin cos

sin sin cos cos cos

R

θ ψ φ θ ψ φ ψ φ θ ψ φ ψ

θ φ θ φ θ

− +

= + −

−

⎡ ⎤

⎢ ⎥

⎣ ⎦

(5)

Using the Yaw, Pitch and Roll angles and the translational velocities of the vehicle in xyz-coordinates,x , y and z, the motion can be described in terms of surge (u), sway (v) and heave (w). Surge is the velocity along the body fixed x-axis,^Bx pointing forward, sway is the velocity along the body fixed y-axis,^By , pointing out of the right hand side of the vehicle and heave is the velocity along the body fixed z-axis,^Bz , pointing downward.

(21)

[ ]

^and

[ ]

^T

x u u x

y R v v R y

z w w z

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥= ⎢ ⎥ ⎢ ⎥= ⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

(6)

Using the rotation matrices described in Equations (1), (2) and (3), the angular velocities of the UUV

(

p q r can be described in terms of ^{, ,}

)

ψ , θ and φ , and their time derivatives. I.e.,

[ ] [ ] [ ] ( ) ( ) ( ) ^{( )}

( ) ( ) ( )

, , ,

0 0 1 0 sin

0 0 0 cos sin cos

0 0 0 sin cos cos

T T T

x y x

p

q R R R

r

φ θ φ

θ φ φ θ θ

ψ φ φ θ ψ

⎡ ⎤ ⎡ − ⎤⎡ ⎤

⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ ⎥⎢ ⎥

⎢ ⎥=⎢ ⎥+ ⎢ ⎥+ ⎢ ⎥=⎢ ⎥⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ −

⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎣ ⎦

(7)

[ ]

p

q S

r

φ θ ψ

⎡ ⎤ ⎡ ⎤

⎢ ⎥ = ⎢ ⎥⎢ ⎥

⎢ ⎥ ⎢ ⎥

⎢ ⎥⎣ ⎦ ⎣ ⎦

(8)

In addition, it is noted that

[ ] [ ] [ ] ^{( ) ( )} ( ) ^{( ) ( )} ( ) ( ) ( ) ( ) ( ) [ ]

1 1

1 sin tan cos tan

and S 0 cos sin

0 sin sec cos sec

T T

R R S

φ θ φ θ

φ φ

φ θ φ θ

− − ⎡ ⎤

⎢ ⎥

= =⎢ − ⎥≠

⎢ ⎥

⎣ ⎦

(9)

2.1 Forces and Moments Affecting the UUV

For this thesis four different forces affecting the UUV are implemented: thruster force, lift forces from the fins (rudder lift), keel imposed torque and drag forces acting on the body. The thruster force and the rudder lift forces are input signals while the drag and keel torque are reaction forces. In the modeling, the thruster force is applied as a concentrated force at the center of the fins, acting along the ^Bx-axis. The drag introduced to the system by the fins is proportionally small and is neglected in this thesis. However there are lift forces induced by the fins that play a vital part in the ability to control the motion of the UUV. The lift force is proportional to the angle of attack of the passing water. By turning the fins the angle of attack is altered, and hence the lift force changes, providing the UUV maneuverability.

(22)

The lift forces in body coordinates

(

^B^F^l ^{= ⎣}^⎡^0,^B^F^ly^,^B^F^lz^⎤^⎦^T

)

are defined by Equation (10).

The A_rudder is the combined projected area of the two fins constrained to move together.

Hence, there are two rudders: vertical pair of fins and horizontal pair of fins shown in Fig 7.

2 2 2

2 2

lift rudder water B

ly fins fins fins fins v

lift rudder water B

lz fins fins fins fins h

F c A u u v w

ρ ϕ

= + +

(10)

and

2

lift rudder water liftforces

c c A ρ

= (11)

where c_lift is the lift coefficients, ϕ_vis the vertical rudder angle, ϕ_h is the horizontal rudder angle and l_fins, l_aft and l_frontare the moment arm distances from the center of mass for lift and drag forces ^BF_l,^BF_da and ^BF , respectively. The localized fin and drag force _df velocities

(

û^fins^,ûâft^,û^front^,^v^fins^,^vâft^,^v^front^,^w^fins^,^wâft^,^w^front

)

are defined in terms of the center of mass velocities

(

^{u v w}^cm^, ^cm^, ^cm

)

, angular velocities

(

p q r and corresponding moment ^{, ,}

)

arms. These velocities and rates are all expressed in body coordinates. I.e.,

fins front aft cm

front cm front

aft cm aft

fins cm fins

u u u u

v v l r

w w l q

v v l r

w w l q

v v l r

w w l q

= = =

= + ⋅

= − ⋅

= + ⋅

= − ⋅

= + ⋅

(12)

see Fig 9 for definition of lengths

(23)

When the terms

(

^{u v w}^{, ,}

)

are used without subscript, they refer to

(

^u^cm^,^v^cm^,^w^cm

)

^{in this}

thesis. The ‘aft’ and ‘front’ are used to denote the terms related to the aft section of the UUV and front section relative to the center of mass, respectively. These and the remaining terms in these equations are defined in the Nomenclature section of this thesis.

Using the new definitions for the velocities, Equation (10) can be rewritten as

( ) ( )

2 2

2

2 2

2 B

ly liftforces cm cm cm fins cm fins v

B

lz liftforces cm cm cm fins cm fins h

F c u u v l r w l q

ϕ ϕ

= + − ⋅ + + ⋅

(13)

For an underwater vehicle, the interaction between the water and the motion of the vehicle creates a drag force which is a function of the vehicle velocity and direction as well as the UUV’s exterior shape. The drag force is a distributed force over the UUV’s exterior. In the ADAMS/View model, the drag force effect is generated using five concentrated one dimensional forces, ^BF_dx, ^BF_dfy, ^BF_dfz,^BF_dayand ^BF_daz, see Fig 7 and Fig 9.

Fig 9: Lift and drag forces B

F

da ^B

F

_df

l

aft

l

_front

B

F

l

fins

cm

(24)

The drag model, mathematically, can be expressed in body coordinates as follows,

2 2 2

B

dx frontal cm cm cm cm

B

dfy frontside front front front front

B

dfz frontside front front front front

B

day aftside aft aft aft aft

B

daz aftside aft aft aft aft

F c u u v w

F c v u v w

F c w u v w

F c v u v w

F c w u v w

= − + +

(14)

2 2 2

front frontal water frontal

side frontside water fronside

side aftside water aftside

c A c

ρ ρ ρ

=

(15)

Where the subscript ‘d’ indicates that it is a drag force and ‘df’ and ‘da’ denote the drag forces due to the projection of the front and aft part respectively. A represents the projected area of the specific part. The drag coefficientc_frontrelates to the shape of the front exterior of the AUV and c_side relates to the exterior of the side of the AUV. From Prestero [18] the c_front is 0.27 and c_side is 1.1. Using Equation (12), Equation (14) can be written using only velocities measured at center of mass.

( ) ( ) ( )

( ) ( )

2 2 2

2 2

2

2 2

2

2 2 B

dx frontal cm cm cm cm

B

dfy frontside cm front cm cm front cm front

B

dfz frontside cm front cm cm front cm front

B

day aftside cm aft cm cm aft cm

F c u u v w

F c v l r u v l r w l q

F c w l q u v l r w l q

F c v l r u v l r w l

= − + +

= − + ⋅ + + ⋅ + − ⋅

= − − ⋅ + + ⋅ + − ⋅

= − − ⋅ + − ⋅ +

(

+

)

( ) ( ) ( )

2

2 2

2

aft

B

daz aftside cm aft cm cm aft cm aft

q

F c w l q u v l r w l q

⋅

= − + ⋅ + − ⋅ + + ⋅

(16)

(25)

In accordance with the assumptions, the effects of the keel are modeled as an applied torque proportional to the roll angle (φ) and the roll rate (p). The roll angle represents the effect of the gravity acting on the UUV and the roll rate represents the effect of the skin friction affecting the hull of the UUV.

0.05 0.1

B

Tk = − φ− p (17)

Combining the above equation with the lift and drag forces yields the following equations of the applied forces

(

^B^F^{= ⎣}^⎡^B^F^x^,^B^F^y^,^B^F^z^⎤^⎦^T

)

and the moments about the center of mass

(

^B^M = ⎣^⎡^B^M^x^,^B^M^y^,^B^M^z^⎤⎦ acting on the UUV, all expressed in body coordinates. The

)

combined drag and thruster forces

(

^B^F ^{= ⎣}^⎡^B^F^x^,^B^F^y^,^B^F^z^⎤^⎦^T

)

are defined by the following equations and are applied as concentrated loads as illustrated in Fig 9.

B B

x thruster dx

B B B B

y ly dfy day

B B B B

z lz dfz daz

B B

x keel

B B B B

y lz fins dfz front daz aft

B B B B

z ly fins dfy front day aft

F F F

F F F F

M T

M F l F l F l

M F l F l F l

= +

= + +

=

= ⋅ − ⋅ + ⋅

= − ⋅ + ⋅ − ⋅

(18)

Per the literature [4], the nominal operating speed of the REMUS is 1.5 m/s. From Equation (14), this yields a drag force of 8.7 N which requires a thruster force of 8.7 N for maintaining a constant operating speed. The turning radius of the REMUS is 3.83 m [18]. Hence, in the ADAMS/View model, a rudder angle of ±π 4rad results in a turning radius of 4 m when a thruster force of 8.7 N is applied. To stay within the REMUS motion constraints, the limitations on the input signals are set lower than the maximal values. For the rudder angles, a limit is set at ±π 4 rad and for the thruster force the limit is set to 8 N. This maximal thruster force yields a forward velocity of 1.45 m/s which will be assumed as operating speed for this thesis.

For the analysis in this thesis, the handover situation where the UUV arrives at the waypoint is assumed to be at a forward velocity ( u ) of 1 m/s. This is due to the assumption that the normal GNC will slow down the UUV slightly so as to increase the accuracy when approaching the waypoint. The handover velocity of 1 m/s will therefore

(26)

be the initial velocity of the UUV for the docking scenario. When applying an initial velocity in ADAMS/View, this can only be done in x y z , , . Using Equations (5) and (6), the following expression for the initial velocities can be calculated

( ) ( ) ( ) ( ) ( )

( / ) 1 cos cos ( / ) 1 cos sin ( / ) 1 sin

init

x m s y m s z m s

θ ψ

θ

= ⋅

(19)

2.2 Equations of Motion

Building a nonlinear representation of the plant enables testing in MATLAB without using ADAMS/View to calculate the motions of the UUV. It also supplies a nonlinear plant that is used to derive a linearized model of the plant for the LQG controller design.

The following section details the derivation of the nonlinear equations of motion.

Let

, , and

x u p

r y v v q

z w r

φ

θ θ ω

ψ

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

=⎢ ⎥ =⎢ ⎥ =⎢ ⎥ =⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

(20)

Using these notations, the following equations of motion can be written.

B x B

y B

z B

x B

y Inertia Inertia

B z

F u p u

F m v m q v

F w r w

M p p p

M I q q I q

M r r r

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥= ⎢ ⎥+ ⎢ ⎥ ⎢ ⎥×

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦

⎣ ⎦

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎛ ⎡ ⎤⎞

⎢ ⎥= ⎢ ⎥ ⎢ ⎥+ ×⎜ ⎢ ⎥⎟

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎜ ⎢ ⎥⎟

⎜ ⎟

⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎝ ⎢ ⎥⎣ ⎦⎠

⎣ ⎦

(21)

(27)

The states are chosen as

[ ]

^T

x = x y z φ θ ψ u v w p r q (22)

The states are used to describe the motion of the UUV. Using these states together with Equations (6), (7) and (21) the state equations of motion take the form.

[ ] [ ]

¹

1 1

1

B x B

y B

z B

x B

Inertia y Inertia

B z

x u

y R v

z w

p

S q

r

u F p u

v F q v

w m F r w

p M p

q I M I q

r M r

φ θ ψ

−

− −

⎡ ⎤ ⎡ ⎤

⎢ ⎥= ⎢ ⎥

⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦

⎡ ⎤ ⎡ ⎤

⎢ ⎥= ⎢ ⎥

⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥⎣ ⎦

⎣ ⎦

⎡ ⎤

⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥

⎢ ⎥= ⎢ ⎥−⎢ ⎥ ⎢ ⎥×

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

⎡ ⎤

⎡ ⎤ ⎡ ⎤

⎢ ⎥

⎢ ⎥= ⎢ ⎥− ⎢ ⎥

⎢ ⎥ ⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦

Inertia

p

I q

r

⎛ ⎛ ⎡ ⎤⎞⎞

⎜ ×⎜ ⎢ ⎥⎟⎟

⎜ ⎜ ⎢ ⎥⎟⎟

⎜ ⎟

⎜ ⎝ ⎢ ⎥⎣ ⎦⎠⎟

⎝ ⎠

(23)

Where

0.177 0 0

0 3.45 0

0 0 3.45

xx xy xz

Inertia yx yy yz

zx zy zz

I I I

I I I I

I I I

⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ ⎥

=⎢ ⎥ ⎢= ⎥

⎢ ⎥ ⎢⎣ ⎥⎦

⎣ ⎦

(24)

The cross terms are approximated to zero per the previously stated assumptions.

3 FUZZY CONTROL

Fuzzy control is based on the utilization of fuzzy logic. In the work of this thesis, the implementation of a fuzzy controller uses six inputs: x_offset , u , y_offset , z_offset , pitch

andyaw. Thex_offset is defined as the distance along the x-axis from the nose of the UUV

to the end wall of the docking bay, see Fig 1. They_offset and z_offset are defined as the

(28)

distance along the y- and z-axis respectively, measured from the center of mass of the UUV to the desired path location. The outputs from the controller are thruster force (F_thruster), vertical rudder angle (ϕ_v) and horizontal rudder angle (ϕ_h^).

Fuzzy logic is a method used for dividing the range of a variable into a number of states, see Fig 10. For example, the yaw angle of the UUV can be divided into states such as:

small negative, (N1), negative (N2), big negative (N3) and so on. For this thesis, N is used for negative states and P for positive states. By letting these states overlap each other, the boundaries become “fuzzy”, hence fuzzy states. The raw value of the signal considered is the input to the fuzzy process. In standard fuzzy-logic nomenclature this value is commonly referred to as the “truth value”. As shown in Fig 10, the overlap of the states is denoted by assignment of a nominal value ( mu ) to each state between 0 and 1.

mu of the different states is directly related to the truth value of the signal considered. For this thesis, these nominal values are defined using linear weighting functions, shown in Fig 10. When approaching the docking bay, the UUV will almost always simultaneously be within two neighboring states, such as near and moderate. The only situation when only one state applies is when the UUV is positioned exactly at the peak of one of the states, for example when u is 0 m/s, 0.2 m/s, or 0.4 m/s. When u is 0.35 for example, P1 becomes 0.25 and P2 becomes 0.75. This means the UUV is closer to the center of the P2 state than the center of the P1 state. Applying this method to all relevant input variables yields a number of fuzzy sets.

Rules may then be implemented using combinations of these fuzzy sets. For example, a rule combining x_offset and u would be “if x_offset is N1 AND u is OK THEN apply a small positive thruster force P3”, see Table 1. Similar rules are set up for all the coupled states:

offset

x - u , y_offset-yaw and z_offset-pitch. The resulting controller output variables of these rules are: thruster force (F_thruster), vertical rudder angle (ϕ_v) and horizontal rudder angle (ϕ_h), see Equation (25). All possible outcomes of the rules are coded in the same manner as the states with P for positive and N for negative outputs, see Table 1. These outputs are then assigned numerical output values given in

Table 2.

and and and

offset

offset v

offset h

x u thruster force

y yaw

z pitch

ϕ ϕ

⇒

(25)

A Control System for Automated Docking of an Unmanned Underwater Vehicle