a control system for automated docking of an unmanned...
TRANSCRIPT
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 ProjectStockholm, Sweden 2005
IR-RT-EX-0516
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.
2
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.
3
TABLE OF CONTENTS
1 INTRODUCTION............................................................................................................................... 7
1.1 BACKGROUND.............................................................................................................................. 71.2 PROBLEM OVERVIEW ................................................................................................................... 81.3 LITERATURE OVERVIEW .............................................................................................................. 9
1.3.1 PID-control........................................................................................................................... 101.3.2 Gain-scheduling and adaptive control ................................................................................. 101.3.3 LQG (Linear Quadratic Gaussian) control .......................................................................... 11
1.3.4 H∞ -control.......................................................................................................................... 11
1.3.5 Fuzzy logic control ............................................................................................................... 111.4 PROBLEM DEFINITION ................................................................................................................ 111.5 BOUNDARIES OF THESIS ............................................................................................................. 121.6 REQUIREMENTS.......................................................................................................................... 12
1.6.1 Handover error tolerance..................................................................................................... 121.6.2 Robustness ............................................................................................................................ 131.6.3 Speed..................................................................................................................................... 131.6.4 Docking accuracy ................................................................................................................. 131.6.5 Sensitivity.............................................................................................................................. 15
1.7 ASSUMPTIONS ............................................................................................................................ 15
2 MODEL ............................................................................................................................................. 18
2.1 FORCES AND MOMENTS AFFECTING THE UUV .......................................................................... 202.2 EQUATIONS OF MOTION ............................................................................................................. 25
3 FUZZY CONTROL.......................................................................................................................... 26
4 LQG ................................................................................................................................................... 33
4.1 LINEARIZATION.......................................................................................................................... 344.1.1 Controllability & observability............................................................................................. 414.1.2 Step response ........................................................................................................................ 41
4.2 LQR........................................................................................................................................... 42
5 RESULTS .......................................................................................................................................... 44
5.1 STEP RESPONSE.......................................................................................................................... 445.2 WORST CASE SCENARIO ............................................................................................................ 465.3 DISTURBANCES .......................................................................................................................... 515.4 ABORT ....................................................................................................................................... 54
6 CONCLUSIONS ............................................................................................................................... 56
7 FUTURE WORK .............................................................................................................................. 57
8 APPENDICES................................................................................................................................... 58
8.1 APPENDIX A............................................................................................................................... 588.1.1 Additional worst case scenario plots .................................................................................... 638.1.2 Simulations with current....................................................................................................... 64
8.1.3 Worst case scenario with uω and ω applied ..................................................................... 65
4
8.2 APPENDIX B ............................................................................................................................... 678.3 APPENDIX C ............................................................................................................................... 69
9 NOTATION....................................................................................................................................... 71
10 REFERENCES.................................................................................................................................. 73
5
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 ......... 13Fig 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
6
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
7
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
8
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 LQRare explored and compared in this thesis.
Fig 1: UUV and docking bay
40 m
Docking bay UUV at waypoint
Desired path
offsetx
9
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. During a docking sequence these acoustic sensors may be used to bring the UUV into
10
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.
11
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
12
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.
13
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
14
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
15
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 32π± rad.
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
16
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 B Bx y 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 B Bx y z in body fixed coordinates, and roll-pitch-yaw ( ), ,φ θ ψ .
The origin of the body-fixed frame ( ), ,B B Bx y z is located at the center of mass. The
corresponding rates (p, q, r) and moments ( ), ,B B Bx y zM M M 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
17
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 B xdirection - B
xFd
, x φ
, y θ
, z ψ , Byq M
Inertial coordinate system Body-fixed coordinate system
, , B Bxx u F
, , B Byy v F
, , B Bzz w F
, Bxp M
, Bzr M
18
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 B x direction applied at the center of the fins. A keel is implemented by adding a Torque about the B x -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 ( , ,xy xz yzI I I ) are small compared to the
moments of inertia ( , ,xx yy zzI I I ).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 B Bx y 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
19
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 1z
Rψ
ψ ψ
ψ ψ
−⎡ ⎤⎢ ⎥
= ⎢ ⎥⎢ ⎥⎣ ⎦
(1)
The second rotation is about the new y-axis produced by the previous rotation
[ ]( ) ( )
( ) ( ),
cos 0 sin
0 1 0
sin 0 cosy
Rθ
θ θ
θ θ
⎡ ⎤⎢ ⎥
= ⎢ ⎥⎢ ⎥−⎣ ⎦
(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 cosx
Rφ
φ φ
φ φ
⎡ ⎤⎢ ⎥
= −⎢ ⎥⎢ ⎥⎣ ⎦
(3)
By multiplying the [ ]R -matrices, a transformation matrix defining the body-fixed
coordinate system with respect to the inertial coordinate system is generated.
[ ] [ ] [ ] [ ], , ,z y xR R R R
ψ θ φ= (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, , y zx and� � � , 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, B x pointing forward, sway is the velocity along the body fixed y-axis, B y , pointing out of the right hand side of
the vehicle and heave is the velocity along the body fixed z-axis, B z , pointing downward.
20
[ ] [ ] 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 TR 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 B x -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.
21
The lift forces in body coordinates ( )0, ,TB B B
l ly lzF F F⎡ ⎤= ⎣ ⎦ are defined by Equation (10).
The rudderA 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 2
2
2
lift rudder waterBly fins fins fins fins v
lift rudder waterBlz fins fins fins fins h
c AF u u v w
c AF u u v w
ρϕ
ρϕ
= + +
= + +
(10)
and
2lift rudder water
liftforces
c Ac
ρ= (11)
where liftc is the lift coefficients, vϕ is the vertical rudder angle, hϕ is the horizontal
rudder angle and finsl , aftl and frontl are the moment arm distances from the center of mass
for lift and drag forces BlF , B
daF and BdfF , respectively. The localized fin and drag force
velocities ( ), , , , , , , ,fins aft front fins aft front fins aft frontu u u v v v w w w are defined in terms of the center
of mass velocities ( ), ,cm cm cmu v w , 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
front cm front
aft cm aft
aft cm aft
fins cm fins
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
22
When the terms ( ), ,u v w are used without subscript, they refer to ( ), ,cm cm cmu v w 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 22
2 22
Bly liftforces cm cm cm fins cm fins v
Blz liftforces cm cm cm fins cm fins h
F c u u v l r w l q
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, B
dxF , BdfyF , B
dfzF , BdayF and B
dazF , see Fig 7 and Fig
9.
Fig 9: Lift and drag forces
BdaF B
dfF
aftl frontl
BlF
finsl
cm
23
The drag model, mathematically, can be expressed in body coordinates as follows,
2 2 2
2 2 2
2 2 2
2 2 2
2 2 2
Bdx frontal cm cm cm cm
Bdfy frontside front front front front
Bdfz frontside front front front front
Bday aftside aft aft aft aft
Bdaz 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 waterfrontal
side frontside waterfronside
side aftside wateraftside
c Ac
c Ac
c Ac
ρ
ρ
ρ
=
=
=
(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 coefficient frontc relates to the shape of the
front exterior of the AUV and sidec relates to the exterior of the side of the AUV. From
Prestero [18] the frontc is 0.27 and sidec is 1.1. Using Equation (12), Equation (14) can be
written using only velocities measured at center of mass.
( ) ( ) ( )
( ) ( ) ( )
( ) ( )
2 2 2
2 22
2 22
22
Bdx frontal cm cm cm cm
Bdfy frontside cm front cm cm front cm front
Bdfz frontside cm front cm cm front cm front
Bday 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 22
aft
Bdaz aftside cm aft cm cm aft cm aft
q
F c w l q u v l r w l q
⋅
= − + ⋅ + − ⋅ + + ⋅
(16)
24
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.1BkT pφ= − − (17)
Combining the above equation with the lift and drag forces yields the following equations
of the applied forces ( ), ,TB B B B
x y zF F F F⎡ ⎤= ⎣ ⎦ and the moments about the center of mass
( ), ,B B B Bx y zM M M M⎡ ⎤= ⎣ ⎦ acting on the UUV, all expressed in body coordinates. The
combined drag and thruster forces ( ), ,TB B B B
x y zF F F F⎡ ⎤= ⎣ ⎦ are defined by the following
equations and are applied as concentrated loads as illustrated in Fig 9.
B Bx thruster dx
B B B By ly dfy day
B B B Bz lz dfz daz
B Bx keel
B B B By lz fins dfz front daz aft
B B B Bz ly fins dfy front day aft
F F F
F 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 4π± rad 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
25
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
init
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.
Bx
By
Bz
Bx
By Inertia Inertia
Bz
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)
26
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
1B
xB
yB
z
Bx
BInertia y Inertia
Bz
x u
y R v
z w
p
S q
r
u F p u
v F q vm
w 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: offsetx , u , offsety , offsetz , pitch
and .yaw The offsetx 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. The offsety and offsetz are defined as the
27
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 ( thrusterF ), 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 offsetx and u would be “if offsetx 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:
offsetx -u , offsety - yaw and offsetz - pitch . The resulting controller output variables of these
rules are: thruster force ( thrusterF ), 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)
28
Fig 10: Fuzzy states for the controller inputs, N=negative, P=positive
Per the physics, the dynamics of the vehicle indicates that the vertical rudder angle is closely linked to the offsety and yaw and that the horizontal rudder angle to the offsetz
and pitch , as denoted above by Equation (25). The offsetx andu will only be relevant when
the UUV is close to the docking bay and by then the UUV should be aligned with the desired course, e.g., along the x-axis. Therefore, the offsetx and the u are only linked to
thruster force. If for some reason the UUV should not be on course when approaching the docking bay, an abort function will seize control of the UUV and the offsetx and
theu become irrelevant. The following is an example illustrating how the fuzzy controller determines the inputs to the system.
N1N2N3 OK P1 N1 OK P1 P2 P3
N3 OK P1 OK
OKN8-N1 P1-P8 OKN8-N1 P1-P8
N2 N1 P2 P3 P3P2N3 P1N2 N1
29
Table 1: Controller output notations from first set of rules: coupled states offsetx -u
x_offset (states)
N3 N2 N1 OK P1
N1 P8 P8 P8 P8 N8
OK P8 P5 P3 0K N8
P1 P8 P2 0K N3 N8
P2 P8 N2 N4 N6 N8
u (states)
P3 P8 N4 N6 N8 N8
Table 2: Controller output values: thruster force
N8 N7 N6 N5 N4 N3 N2 N1 OK P1 P2 P3 P4 P5 P6 P7 P8 -8 -7 -6 -5 -4 -3 -2 -1 0 1 2 3 4 5 6 7 8
Example Consider the case when offsetx is -1.5 m and u is 0.3 m/s. Referring to Fig 10, this
indicates that offsetx is 0.75 N1 and 0.25 OK and u is 0.5 P1 and 0.5 P2. Normally mu is
used to signify these values, e.g. mu for N1 is 0.75. For this case, the following rulesfrom Table 1 apply.
• Rule 1: If offsetx is N1 and u is P1 then Thruster force is OK
• Rule 2: If offsetx is N1 and u is P2 then Thruster force is N4
• Rule 3: If offsetx is OK and u is P1 then Thruster force is N3
• Rule 4: If offsetx is OK and u is P2 then Thruster force is N6
The centroid computation makes use of the lowest nominal value of each rule and multiplies the output with it. For this thesis, the centroid computation is used to determine the weight of each rule as a function of minimal mu of the rule’s states.
30
1
1
min( )
min( )
N
i ii
N
ii
mu OutputNet Output Value
mu
=
=
⋅
=∑
∑ (26)
Looking at rule 1, the nominal value of N1 for offsetx is 0.75 and 0.5 of u for P1. The 0.5 is
lower than 0.75 which yields that the output of rule 1, and therefore the thruster force, is multiplied by 0.5. The following summarizes the outcome of the above four rules (N=4).
• Rule 1: 0.5*OK • Rule 2: 0.5*N4 • Rule 3: 0.25*N3 • Rule 4: 0.25*N6
Table 2 shows the output values, which in this case are equal to the numbers, OK = 0, N3=-3, N4=-4, N6=-6. The centroid computation then yields that the crisp value sent to the system is given by:
( ) ( ) ( )0.5 0 0.5 4 0.25 3 0.25 62.83 N
0.5 0.5 0.25 0.25thrusterF⋅ + ⋅ − + ⋅ − + ⋅ −
= = −+ + +
(27)
The maximum thruster force allowed in this thesis is 8 N, which corresponds to a forward velocity of 1.43 m/s. When traveling at 1.43 m/s, the stopping distance of the REMUS is 3.1 m using a thruster force equal to -8 N. Hence, the thruster force is kept at 8 N for all values of offsetx less than -5 m, regardless ofu . In successful docking sequences, offsetx can
never become positive since that represents the UUV going through the end wall of the docking bay. The states for the remaining variables are chosen in order to fit the range within which they are relevant, see
Table 3 and Table 4. The first four variables in Fig 10 ( ), , ,offset offset offsetx u y z have no
lower or upper boundaries since they could theoretically approach infinity. The pitchand yaw on the other hand have fixed boundaries at π± rad. This boundary is used to set
the states for both angles even though the pitch angle can never exceed 2π± rad in
reality. A pitch or yaw value that exceeds 2π rad or is less than 2π− rad represents a motion in the direction away from the docking bay. When being this far off course, the rudder angle should always be maximized regardless of the pitch and yaw angles and respective offset distances. This is the reason for the N8 and P8 states for the pitch
and yaw angles to cover the whole range from 2π± to π± . Utilizing these fuzzy sets, a
31
series of rules are set up. The outcome of these rules forms the states of the outputs. Crisp output values are then assigned to all the different output states. The vehicle control by the keel is implemented through application of a keel torque as described in Equation (17). Hence, the keel is a passive control element and is not considered in the set of controller output variables for the controller.
Table 3: Controller output notation and values for vertical rudder angle: coupled states offsety - yaw
y-offset (states) N3 N2 N1 OK P1 P2 P3
N8 N5 N4 N4 N4 N4 N3 N3 N7 N5 N4 N4 N4 N3 N3 N2 N6 N4 N4 N4 N3 N3 N3 N2 N5 N4 N4 N3 N3 N3 N2 N1 N4 N4 N3 N3 N3 N2 N2 0K N3 N4 N3 N3 N2 N2 N1 P1 N2 N3 N3 N2 N2 N1 0K P2
Yaw angle
(states) N1 N3 N2 N2 N1 0K P1 P2 OK N3 N2 N1 0K P1 P2 P3 P1 N2 N1 0K P1 P2 P2 P3 P2 N2 0K P1 P2 P2 P3 P3 P3 N1 P1 P2 P2 P3 P3 P4 P4 0K P2 P2 P3 P3 P3 P4 P5 P1 P2 P3 P3 P3 P4 P4 P6 P2 P3 P3 P3 P4 P4 P4 P7 P2 P3 P3 P4 P4 P4 P5 P8 P3 P3 P4 P4 P4 P4 P5
Output values for vertical rudder angles ( 16π rad)N5 N4 N3 N2 N1 OK P1 P2 P3 P4 P5 -5 -4 -3 -2 -1 0 1 2 3 4 5
An abort feature is added to the fuzzy controller. The abort function is to seize control of the UUV if it strays too far from the desired path. How far depends on the distance to the docking bay. This creates something like a virtual funnel where the abort function is triggered if the UUV is located outside of the funnel, see Fig 11. If the UUV at any time during docking has an offset that exceeds the limit for the zone which the UUV is currently in, the abort function guides the UUV back to the waypoint from which a new docking attempt is performed. Once the abort function is triggered it will maintain a constant ThrusterF of 8 N and gradually turn the desired path around the z-axis until turned
180 degrees, pointing in the opposite direction of the original desired path. The turn is performed using “the right hand rule” or counter “right hand rule” , depending on which is closest. Using the fuzzy controller, the UUV will therefore try to return to the waypoint again. When the UUV passes the x-coordinate of the waypoint, -40 m, the desired path will gradually turn back around the z-axis until restored to normal. When the desired path
32
is pointing towards the docking bay again the abort function is disengaged and
ThrusterF will again be operated by the fuzzy controller. The reason for the desired path to
be turned around the z-axis is that then the turn is performed by changing the yaw angle instead of the pitch angle which is limited to 2 2π θ π− < < .
Table 4: Controller output states and values for horizontal rudder angle: coupled states offsetz - pitch
z-offset (states) N3 N2 N1 OK P1 P2 P3
N8 P3 P3 P4 P4 P4 P4 P5 N7 P2 P3 P4 P4 P4 P4 P5 N6 P2 P3 P3 P3 P4 P4 P4 N5 P1 P2 P3 P3 P3 P4 P4 N4 OK P2 P2 P3 P3 P3 P4 N3 N1 P1 P2 P2 P3 P3 P4 N2 N2 OK P1 P2 P2 P3 P3 N1 N2 N1 OK P1 P2 P2 P3 OK N3 N2 N1 OK P1 P2 P3 P1 N3 N2 N2 N1 OK P1 P2 P2 N3 N3 N2 N2 N1 OK P2 P3 N4 N3 N3 N2 N2 N1 P1 P4 N4 N3 N3 N3 N2 N2 OK P5 N4 N4 N3 N3 N3 N2 N1 P6 N4 N4 N4 N3 N3 N3 N2 P7 N5 N4 N4 N4 N3 N3 N2
Pitch angle
(states)
P8 N5 N4 N4 N4 N4 N3 N3
Output values for horizontal rudder angles ( 16π rad)N5 N4 N3 N2 N1 OK P1 P2 P3 P4 P5 -5 -4 -3 -2 -1 0 1 2 3 4 5
33
Fig 11: Virtual docking funnel
Table 5: Abort function boundaries
Abort zone 1 2 3 4 Max offset y,z (m) 2 1 0.4 0.1
Max offset pitch, yaw (rad) 4π 8π 12π 16π
4 LQG When utilizing Linear Quadratic Gaussian (LQG) control techniques, a controller that is optimized for a specific operation point is derived. This controller is called the Linear Quadratic Regulator (LQR). The controller is derived by minimization of a cost function containing two parameter matrices, Q and R. The Q relates to the relative significance of the states of the system and similarly R relates to the input signals to the system. Normally, both these matrices are initially set to be identity matrices. They can then be altered in order to enhance the performance of the LQR or fulfill some application specific requirements. To use the LQG control approach, a linearization of the plant must be derived. The linearized system also needs to be both controllable and observable in order to derive a LQR.
-40 -1 -4 -10 -0.1
2
2
-20
3
1
3
1
Abort zones
4
4
Abort zones
Abort funnel
x (m)
Max offset
34
4.1 Linearization For the linearization of the system, the states [ ], , , , , , , , , , ,x x y z u v w p q rδ δ δ δφ δθ δψ δ δ δ δ δ δ=
are used. To derive a linear state space representation of the plant, Equations (23) are linearized using the perturbation method and dropping higher order terms (H.O.T.).
( )0 0 0
0 0 0
0 0 0
0 0 0, , ( , , ) . . .a a a a a ab b b b b bc c c c c c
F F FF a b c F a b c a b c H O T
a b cδ δ δ= = =
= = == = =
∂ ∂ ∂≅ + + + +
∂ ∂ ∂
0≈ (28)
The following equations of the states and inputs to the system, expressed as functions of the nominal states and the perturbation variables ( )δ ⋅ , are substituted into Equations(23).
0
0
0
0
0
00
0
0
0
0
0
0
x x x
y y y
z z z
x x xu u u
v v v
w w w
p p p
q q q
r r r
δ
δ
δ
φ φ δφ
θ θ δθ
ψ ψ δψδ
δ
δ
δ
δ
δ
δ
= +⎡ ⎤⎢ ⎥
= +⎢ ⎥⎢ ⎥= +⎢ ⎥
= +⎢ ⎥⎢ ⎥= +⎢ ⎥
= +⎢ ⎥= + = ⎢ ⎥
= +⎢ ⎥⎢ ⎥= +⎢ ⎥
= +⎢ ⎥⎢ ⎥= +⎢ ⎥
= +⎢ ⎥⎢ ⎥= +⎣ ⎦
(29)
0
0
0
0
0
thrusterthruster
v vin in in
hh
keelkeel
F F
u u u
TT
δ
ϕ δϕδ
δϕϕ
δ
⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥= + = +⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣ ⎦⎣ ⎦
(30)
The naught terms denote the nominal states of the operation point chosen for the linearization. The linearization point should be chosen to reflect the states’ range of operation, see Equation (31) for values selected. Considering the docking scenario of the UUV, an acceptable nominal location can vary greatly so at the moment no numerical
35
values are assigned to 0 0 0, and x y z . The major objective is to keep the angles and
angular velocities as small as possible during the docking scenario. Hence, the corresponding naught terms ( )0 0 0 0 0 0, , , , ,p q rφ θ ψ are set to zero. The velocities sideways
( ),v w are expected to be low so their naught terms ( )0 0,v w are also set to 0. For the first
linearization test, 0u was also set to zero. However, this resulted in a linear representation
of the system that lacked most of the characteristics of the nonlinear system and proved to be completely useless. By utilizing a linearization point which includes the forward motion of the UUV, the important characteristics were maintained in the linear representation of the system. For this thesis, 0u was chosen as 1 m/s. The values for the
linearization point are listed below.
0
0
0
0
0
0 0
0 0
0 0
0
0
00
0
0
0
0
0
0
0
00
00 and
01
00
0
0
0
0
thruster
v
inh
keel
x x
y y
z z
F
x uu
v Tw
p
q
r
φ
θ
ϕψ
ϕ
⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎡ ⎤⎢ ⎥ ⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥= = = =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣ ⎦⎣ ⎦⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦⎣ ⎦
(31)
Using this linearization point, the linear system and the nonlinear system will behave in approximately the same manner within proximity of the linearization point. The further away from the linearization point the system operates, the less accurate is the linearized system will be compared to the nonlinear system. To succeed in deriving a linearized presentation of the system, some assumptions are made to eliminate nonlinear terms. Described in Equation (32) are the assumptions regarding the thi and thj perturbation
variables, ( )iδ ⋅ and ( )jδ ⋅ .
( ) ( ) ( ) ( )1 0i j i jδ δ δ δ⋅ ⋅ ⋅ ⇒ ⋅ ⋅ ⋅ ≅� (32)
36
Applying Equations (29)-(32) to the nonlinear model, a linearized presentation of the equations of motions can be derived. Thus, Equations (5) and (7) defining the rotation and orientation matrices, R and S, reduce to
[ ] [ ]1
1
1
1
1 0
and 0 1
0 1
R S
δψ δθ
δψ δφ
δθ δφ
δθ
δφ
δφ
−
−
−
−
⎡ ⎤⎡ ⎤⎢ ⎥⎢ ⎥≅ ≅ −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ ⎣ ⎦
(33)
Linearizing the nonlinear terms in Equation (23), the following expressions can be written.
[ ] [ ]
1 1
R R
u u u
v v v
w w w
δ δ
δ δψ δ
δ δθ δ
+ +⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥≅ = +⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥− +⎣ ⎦ ⎣ ⎦ ⎣ ⎦
(34)
[ ] [ ]1 1
p p p
S q S q q
r r r
δ δ
δ δ
δ δ
− −
⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥≅ =⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦
(35)
The cross terms in the later expressions of Equation (23) exhibit the following linear form.
1 01 1 1
p u p u
q v q v rm m m
r w r w q
δ δ
δ δ δ
δ δ δ
+⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥× ≅ × =⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦
(36)
1 1
0
0
0Inertia Inertia Inertia Inertia
p p p p
I q I q I q I q
r r r r
δ δ
δ δ
δ δ
− −
⎛ ⎞ ⎛ ⎞⎛ ⎞ ⎛ ⎞⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥× ≅ × =⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎝ ⎠ ⎝ ⎠⎝ ⎠ ⎝ ⎠
(37)
To obtain the complete linearized expression for Equation (23), the forces
( ), ,B B Bx y zF F F and moments ( ), ,B B B
x y zM M M also need to be linearized. The
equations for the forces are as follows.
37
2 2 2( ) 2B B
thruster frontalx thruster dx thrusterF c u u v wF F F F u
m m m m
δ δ− + ++ −= = ≅ (38)
( ) ( )
( ) ( ) ( )
( )
2 22
2 22
2
B B B By ly dfy day
liftforcescm cm cm fins cm fins v
frontsidecm front cm cm front cm front
aftsidecm aft cm cm aft
F F F F
m mc
u u v l r w l qm
cv l r u v l r w l q
mc
v l r u v lm
ϕ
+ +=
= + − ⋅ + + ⋅
− + ⋅ + + ⋅ + − ⋅
− − ⋅ + −( ) ( )
( )
( ) ( )( )
2 2
1
1
cm aft
liftforces v frontside frontside front aftside aftside aft
liftforces v frontside aftside frontside front aftside aft
r w l q
c c v c l r c v c l rm
c c c v c l c l rm
δϕ δ δ δ δ
δϕ δ δ
⋅ + + ⋅
≅ − − ⋅ − + ⋅
= − + − ⋅ − ⋅
(39)
( ) ( )
( ) ( ) ( )
( ) ( )
2 22
2 22
22
B B BBlz dfz dazz
liftforcescm cm cm fins cm fins h
frontsidecm front cm cm front cm front
aftsidecm aft cm cm aft c
F F FF
m mc
u u v l r w l qm
cw l q u v l r w l q
mc
w l q u v l r wm
ϕ
+ +=
= + − ⋅ + + ⋅
− − ⋅ + + ⋅ + − ⋅
− + ⋅ + − ⋅ + ( )
( )
( ) ( )( )
2
1
1
m aft
liftforces h frontside frontside front aftside aftside aft
liftforces h frontside aftside frontside front aftside aft
l q
c c w c l q c w c l qm
c c c w c l c l qm
δϕ δ δ δ δ
δϕ δ δ
+ ⋅
≅ − + − −
= − + + −
(40)
Using the linearized expressions for the forces ( ), ,B B Bx y zF F F , the following equations
provide the linearized moments affecting the UUV ( ), ,B B Bx y zM M M .
( ) ( ) ( )1 1 11,1 1,1 1,1B B BInertia x Inertia keel Inertia keelI M I T I Tδ− − −= ≅ (41)
38
( ) ( )
( ) ( )
1 1
1 2 2
2,2 2,2
2,2
B B B BInertia y Inertia lz fins dfz front daz aft
Inertia liftforces fins h frontside fins frontside front aftside aft aftside aft
I M I F l F l F l
I c l c l w c l q c l w c l qδϕ δ δ δ δ
− −
−
= ⋅ − ⋅ + ⋅
≅ + − − −(42)
( ) ( )
( )( )
1 1
1 2 2
3,3 3,3
3,3
B B B BInertia z Inertia ly fins dfy front day aft
Inertia liftforces fins v frontside fins frontside front aftside aft aftside aft
I M I F l F l F l
I c l c l v c l r c l v c l rδϕ δ δ δ δ
− −
−
= − ⋅ + ⋅ − ⋅
≅ − − − + −(43)
Using Equation (23) as reference and substituting in Equations (35) through (43), the following linear representation of the time derivative of the states x� is generated.
( )
( ) ( )( )
1
1
1
1
2Thruster
liftforces v frontside aftside frontside front aftside aft
liftforce
uxvywz
p
q
rx m F uu
m c c c v c l c l rv
m cw
p
q
r
δδδψ δδδθ δδ
δδφδδθδδψ
δ δδδϕ δ δδ
δ
δ
δ
δ
−
−
−
+
+
− +
= = −
− + − −
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
�
�
�
�
�
�
�
�
�
�
�
�
�
( ) ( )( )( )
( ) ( )( )
1
1 2 2
1
1,1
2, 2
3,3
s h frontside aftside frontside front aftside aft
B
Inertia keel
Inertia liftforces fins h frontside fins frontside front aftside aft aftside aft
Inertia liftfo
c c w c l c l q
I T
I c l c l w c l q c l w c l q
I c
δϕ δ δ
δ
δϕ δ δ δ δ
−
−
−
− + − −
+ − − −
−( )2 2
rces fins v frontside fins frontside front aftside aft aftside aftl c l v c l r c l v c l rδϕ δ δ δ δ− − + −
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(44)
At this point, the linear state space representation can be written on the form
x Ax Bu
y Cx Du
= +
= +
�
(45)
When substituting in the numerical values for the coefficients and lengths listed in Appendix A, Equation (44) becomes
39
1
0.03 0.21
0.55 3.36 0.62
0.55 3.36 0.62
5.65
3.29 6.66 6
Thruster
v
h
h
x
y
z
xu
v
w
p
q
r
u
v
w
p
q
r
F u
v r r
w q q
T
w
δ
δ
δ
δφ
δθ
δψ
δ
δ
δ
δ
δ
δ
δ
δψ δ
δθ δ
δ
δ
δ
δ δ
δϕ δ δ δ
δϕ δ δ δ
δϕ δ
=
+⎡ ⎤⎢ ⎥ +⎢ ⎥
− +⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥ =⎢ ⎥ ⋅ − ⋅⎢ ⎥
⋅ − ⋅ + ⋅ −⎢ ⎥⎢ ⎥ ⋅ − ⋅ − ⋅ +⎢ ⎥
⋅⎢ ⎥⎢ ⎥
⋅ − ⋅ −⎢ ⎥⎢ ⎥⎣ ⎦
�
�
�
�
�
�
�
�
�
�
�
�
�
.73
3.29 6.66 6.73v
q
v r
δ
δϕ δ δ
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥− ⋅ + ⋅ −⎣ ⎦
(46)
The A, B, C and D matrices in the linear state space representation are extracted from Equation (46) and are shown below in Equations (47) and (48).
0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 1 0 1 0 0 0 0
0 0 0 0 1 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 0 0 0 0 1
0 0 0 0 0 0 0.21 0 0 0 0 0
0 0 0 0 0 0 0 3.36 0 0 0 0.38
0 0 0 0 0 0 0 0 3.36 0 0.38 0
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 6.66 0 6.73 0
0 0 0 0 0 0 0 6.66 0 0 0 6.73
A
−
−
−
− −
− −
−
=
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦
(47)
40
and
[ ]
[ ] [ ]
6 4
12 12 12 4
0
0.03 0 0 0
0 0.55 0 0
, and 00 0 0.55 0
0 0 0 5.65
0 0 3.29 0
0 3.29 0 0
B C I D
×
× ×
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥
= = =⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥−⎣ ⎦
(48)
The constant value of 1 that appears for xδ � in Equation (46) is treated as a part of the initial conditions for the states ( )0x , see Fig 12, and is not included in the following
equations.
Fig 12: Feedback loop system presentation
The nonlinear plant was also linearized using MATLAB linearization tools which gave the exact same result.
GK
Reference signal
R
-K
Initial conditions for x , ( )0xInitial conditions for inu , ( )
0inu
Disturbance, uω Disturbance, ω
r inu y
System, Plant
Controller for y
Controller for r
uδ
41
4.1.1 Controllability & observability To be able to derive a LQR based on the linearized state space system, it must be both controllable and observable. To determine if a system is controllable and observable the controllability matrix ( , )contS A B and observability matrix ( , )obsO A C are used. For this
thesis, the sensors on the UUV are assumed to provide measures of all 12 states. This is represented by the C-matrix which is a 12 12× identity matrix.
( ) ( ) ( ) ( )
2 1
2 1
( , ) ...
( , ) ...
ncont
TT TT T nobs
S A B B AB A B A B
O A C C CA CA CA
−
−
⎡ ⎤= ⎣ ⎦
⎡ ⎤= ⎢ ⎥⎣ ⎦
(49)
The system is controllable when the controllability matrix has full rank and observable when the observability matrix has full rank. For this state space system, the observability matrix has a size of 144 12× and a rank of 12 and the controllability matrix has a size of 12 48× and a rank of 12. Hence, the system is both observable and controllable.
4.1.2 Step response The figures on page 58-61 in Appendix A show step responses for the system G, defined in Fig 12. Only the step responses that are not zero are presented. The gathered information regarding how the inputs affect the outputs is described in Table 6.
Table 6: Couplings between inputs and outputs
Input (number, name) Outputs affected (number, name)[1,Thruster force] [1,x] and [7,u] [2,Vertical rudder angle] [2,y], [6,yaw], [8,v] and [12,r] [3,Horizontal rudder angle] [3,z], [5,pitch], [9,w] and [11,q] [4,Keel torque] [4,roll] and [10,p]
Considering the A, B, C and D matrices in Equations (47) and (48) the same result can be derived. For example, looking at input 2 (vertical rudder angle) it affects states 8 and 12 (v and r) as shown in the B matrix. In the A matrix state 8 affect states 2, 8 and 12 and state 12 affect states 6, 8 and 12. This shows that a change in input 2 affects states 8 and 12 directly but also that states 2 and 6 are affected indirectly. Since the C matrix is an identity matrix and the D matrix is zero, the affected outputs are equal to the affected states. Hence, a change in input 2 affect outputs 2, 6, 8 and 12 as stated in Table 6.
42
4.2 LQR The objective when applying LQG is to minimize the following cost function
( ) ( ) ( ) ( )T Tx t Qx t u t Ru t dt+∫ (50)
Where Q is a symmetric nonnegative definite matrix and R is a symmetric positive definite matrix. The Q sets the priority of minimization for the different states, x. The R sets the priority of minimization for the different parameters of u . The resulting state feedback controller is derived through the LQR equations.
inu K xδ δ= (51)
where the gain matrix is defined as
1 TK R B P−= (52)
The positive definite P matrix is determined from the Algebraic Riccati Equation
1 0T TA P PA Q PBR B P−+ + − = (53)
As starting point both Q and R are chosen to be identity matrices. The resulting P and K matrices are shown in Appendix A. Implementing this controller as shown in Fig 12 into a MATLAB model, this K yields the following significant results.
Table 7: Results from docking scenario using initial Q and R values
Controller parameters Q=I, R=I Initial conditions for x [m,rad,m/s,rad/s] [-40 0 0 0 0 0 1 0 0 0 0 0]
Reference for x [m,rad,m/s,rad/s] [-0.l 0 0 0 0 0 1 0 0 0 0 0] Overshoot, x-axis direction (m) 1.739 (4.35%)
Time until settled at destination (s) 91.332 Rise time (s) 23.347
(See Appendix B for definitions of overshoot etc.)
The overshoot of 1.74 m indicates that the UUV will collide with the back wall of the docking bay which is strongly undesired. The reference value for x is set to -0.1 due to the fact that it is desirable that the UUV stops 0.1 m meter from the end wall of the dock. This will position it far enough inside the dock to enable docking and still give the controller some error margin. This error margin is set to 0.05 m for this thesis as stated in Section 1.6.4: Docking accuracy.
43
To reduce the overshoot, the value for Q(7,7), which corresponds to u , was increased. This leads to an increased priority to minimizing error refu u u= − . After some tuning, the
nominal value 200 was chosen for Q(7,7). Applying the K derived using the new Q, the following results were obtained
Fig 13: Step in x using initial versus final LQR parameters
Table 8: Results from docking scenario using final Q and R values
Controller parameters Q=described above, R=I Initial conditions for x [m,rad,m/s,rad/s] [-40 0 0 0 0 0 1 0 0 0 0 0]
Reference for x [m,rad,m/s,rad/s] [-0.l 0 0 0 0 0 1 0 0 0 0 0] Overshoot, x-axis direction (m) 0.009 (0.25%)
Time until settled at destination (s) 60.718 Rise time (s) 29.533
(See Appendix B for definitions of overshoot etc.)
Using a higher nominal value for minimizing error refu u u= − will result in a lower overall
speed of the UUV since the desired value for error refu u u= − is 0. Even though the rise
time increases slightly, the docking accuracy and settling time both show much more desirable results and since the overshoot is less than 0.1 m, no impact between docking bay and UUV occurs.
44
5 RESULTS
To analyze the two different controllers considered in this thesis, the results from the Fuzzy controller will be compared to those of the LQR. There are two different approaches available for running simulations on the nonlinear plant. Both approaches use MATLAB/Simulink to run the simulations but they use different methods to generate the nonlinear plant. The first method is to use the model built in ADAMS/View to generate the nonlinear plant block in co-simulation mode with Simulink. This utilizes ADAMS/Solver to calculate the dynamics of the UUV and Simulink to handle control aspects. It is an approach that enables presentation of the scenario graphically in 3D in ADAMS. It also allows the docking bay to be included in the simulations which is an important feature for presenting a more comprehensive scenario. Unfortunately discontinuities arise for some scenarios using this approach. The other approach is to use the Equation (23) to generate a MATLAB based nonlinear plant. This rids us of the discontinuities associated to the tests run using ADAMS/Solver. For comparisons of the two implemented controllers, the fully MATLAB based plant block is used to obtain the most reliable results. When simulating the abort control function, described in Section 5.4: Abort below, the ADAMS based nonlinear plant block is used. The reason for this is that this case is the most interesting scenario to present in 3D graphics, only possible using ADAMS. As discussed in the previous section the Q and R matrices will be set to be identity matrices except for Q(7,7) which will be set to 200. These settings will apply to the whole results section.
5.1 Step Response First test is to apply a step to the reference signal in both models. The step is applied to the reference signal for the second state, y. Fig 14 shows the resulting plot for the step response.
45
Fig 14: Step response in y using Fuzzy versus LQR control
Three key values are used to analyze and compare the results from the step response. These are: overshoot, rise time and settling time. See Fig 36 in Appendix A for definitions of these values. The overshoot is a measure of by how much the signal exceeds the new reference signal, the rise time shows how fast the system responds to the step in the reference signal and the settling time is the overall time it takes for the system to settle about a new reference value. The most important step responses for this thesis are in x , y and z since they describe the position of the UUV. The result from a step response in y is presented in Table 9. Due to the dynamics of the UUV, it can only be controlled when moving forward. Therefore, in order to keep a forward velocity, the reference signal for x is set to a value high enough that the UUV will not reach it during the simulation. For these tests, the reference value for x was chosen as 100.
Table 9: Step response in y (step size=1m)
Controller Fuzzy LQR Initial conditions for x [m,rad,m/s,rad/s] [0 0 0 0 0 0 1 0 0 0 0 0] [0 0 0 0 0 0 1 0 0 0 0 0]
Reference for x [m,rad,m/s,rad/s] [100 1 0 0 0 0 0 0 0 0 0 0] [100 1 0 0 0 0 0 0 0 0 0 0] Overshoot, y-axis direction (m) 0.097 (9.7%) 0.021 (2.1%)
Rise time (s) 3.919 2.881 Settling time (s) 5.503 4.049
Maximum vertical rudder angle (rad) 0.070 0.158 (See Appendix B for definitions of overshoot etc.)
These results clearly show that the LQR controller performs much better than the fuzzy controller in all aspects of the step response. The LQR utilizes a higher rudder angle to perform the step which could potentially be a negative effect for some applications. A
46
step in z will yield the exact same results. When simulating a step in x the following results were obtained, see Table 10 and Fig 15.
Table 10: Step response in x (step size=40 m)
Controller Fuzzy LQR Initial conditions for x [m,rad,m/s,rad/s] [0 0 0 0 0 0 1 0 0 0 0 0] [0 0 0 0 0 0 1 0 0 0 0 0]
Reference for x [m,rad,m/s,rad/s] [40 0 0 0 0 0 0 0 0 0 0 0] [40 0 0 0 0 0 0 0 0 0 0 0] Overshoot, x-axis direction (m) 0.082 (0.82%) 0.009 (0.09%)
Rise time (s) 23.894 30.201 Settling time (s) 26.079 32.386
Maximum vertical rudder angle (rad) 0 0 (See Appendix B for definitions of overshoot etc.)
The values for rise and settling time show that the fuzzy controller is faster at taking the UUV to the dock and coming to rest. However, the overshoot is nearly ten times as large which yields that the risk of bouncing into the end wall of the docking bay is significantly higher. Again, since the primary objective is to design a controller that can deliver the UUV safely into the docking bay with no time constraints, the overshoot is much more important than the speed.
Fig 15: Step response in x using Fuzzy resp. LQR controller
5.2 Worst Case Scenario An essential quality for the controllers is for the UUV to be able to dock when starting from the worst acceptable initial conditions as stated in Section 1.6.1: Handover error tolerance. Since the accepted handover space is a box-shaped space, the worst case scenario looking at position is when the UUV is handed over in one of the corners of the box. The four worst corners are the ones closest to the docking bay. This is due to the fact
47
that starting from one of these corners will reduce the distance available for the controller to align the UUV to the desired path. For the orientation, the roll should be 4π rad in
either direction, pitch 4π rad tilted away from desired path and yaw turned π rad, see Fig 3.
The results from the simulations using this worst case starting point are as follows. The alignment time is a measure of the time from control handover from GNC (start of simulation) until the UUV is aligned within the desired path’s relative error constraints as described in Section 1.6.4: Docking accuracy. Alignment distance describes the distance traveled along the x-axis during the alignment time. See Fig 35 in Appendix B for a visual description of alignment distance.
Table 11: Docking Scenario from worst case starting point
Controller Fuzzy LQR Initial conditions for x [m,rad,m/s,rad/s] [-38 2 2 π/8 –π/4 π 1 0 0 0 0 0] [-38 2 2 π/8 –π/4 π 1 0 0 0 0 0]
Reference for x [m,rad,m/s,rad/s] [-0.1 0 0 0 0 0 0 0 0 0 0 0] [-0.1 0 0 0 0 0 0 0 0 0 0 0] Overshoot in x (m) 0.077 0.009 Alignment time (s) 25.642 18.297
Alignment distance (m) 19.386 8.131 Time until settled at destination (s) 45.018 70.060
Max. vertical rudder angle (rad) 0.785 0.785 Max. horizontal rudder angle (rad) 0.745 0.785
See Appendix B for definitions of alignment time etc.
These results agree well to what could be expected considering the step responses from the previous chapter. Alignment time and distance corresponds to the time and distance the UUV must travel before it is aligned to the desired path. These characteristics relate to the step response in y for which the step responses of the LQR are much faster. The time it takes for the UUV to settle at destination is highly dependent on the rise time for x . From the step response in x , it is known that the Fuzzy controller is the faster as seen by comparing the values for ‘time until settled at destination’ in Table 11.
Shown in Fig 16 through Fig 19 are the y , z , pitch and yaw responses. The alignment time depends on all four of these responses and the UUV is not considered aligned until all four variables are within their error margins described in Section 1.6.4: Docking accuracy. The plots below clearly show that the values for alignment time and distance are higher using the Fuzzy controller than when using the LQR. For the LQR both y and z stabilize at 0 much earlier than for the fuzzy.
48
Fig 16: Worst case scenario, y-offset
Fig 17: Worst case scenario, z-offset
Fig 18: Worst case scenario, pitch angle
Alignment time
Alignment time Alignment time
Alignment time
Alignment time Alignment time
49
Fig 19: Worst case scenario, yaw angle
The plot for x shows the resulting alignment distance.
Fig 20: Worst case scenario, x-offset
Fig 21 shows how the controllers are able to coupe with the offset in roll angle for the worst case scenario. For the fuzzy controller the keel torque, B
keelT , is proportional to the
roll angle and angular rate ,p see Equation (17). For the LQR the keel angle is actively controlled by the LQR.
Alignment time Alignment time
Alignment time Alignment time
Alignment distance
Alignment distance
50
Fig 21: Worst case scenario, roll angle
Considering the real situation, the keel would stabilize the roll angle around zero during the docking scenario. Hence, both controllers in Fig 21 show realistic scenarios for the roll angle. Plots of the rudder angles can be found in Appendix A, Section 8.1.1. In Fig 20, depicting the offset in x, it is shown that the UUV first travels away from the dock due to the turned initial orientation before starting to head towards the docking bay. Fig 22 of the thruster force delineates as to why the fuzzy controller is faster than the LQR at reaching the destination. As seen, the fuzzy controller keeps the thruster at maximal thrust longer than the LQR and then uses negative thrust to slow it down. The LQR on the other hand barely uses negative thrust and keeps the thruster force lower than the fuzzy controller when slowing down the UUV.
Fig 22: Worst case scenario, Thruster force
51
5.3 Disturbances To test the sensitivity of the closed loop system, disturbances are introduced to the system in different locations. Many different aspects contribute to disturbances, for example: change of payload, sensor errors, model errors, model assumptions and outside influences.
A common outside influence for underwater vehicles is current. The direction of the current that is hardest for the controller to coupe with is from the side, hence the current is assumed to flow orthogonally to the desired path, i.e. along the y -axis. For this thesis, the disturbance due to current will be implemented in the drag model as modifications to the velocities of the UUV. This is done by introducing the following substitutions to Equation (14).
( ) ( )( )
( ) ( )( )( ) ( )( )
sin cos
cos cos
cos cos
cm cm current
front front current
aft aft current
u u v
v v v
v v v
ψ ψ
ψ ψ
ψ ψ
= −
= −
= −
(54)
where currentv represents the magnitude of the current. The first trigonometric term is to
account for the angle of attack of the current compared to the direction of the UUV’s movement. The ( )cos ψ in the end is to make up for a decreased projected area when the
orientation of the UUV is turned. The current’s effect on the UUV’s frontal area will not be considered since it is small compared to the effect of the side area. Due to the dynamics of the UUV, it is impossible to eliminate all effects caused by the current. A sideway current will always give an error in y-position and/or yaw angle. If the yaw angle is kept at zero, the UUV will slide away sideways with the current, and if the y-position is kept at zero, the yaw angle must be altered to counter the effect of the current. To maintain a y-position at a specific current magnitude a certain amount of error in yaw angle is required, independent of which y-position. Hence, the most favorable scenario when a current is acting on the system is to keep the UUV’s y-position aligned to the y-position of the desired path and compensate the current by altering the yaw angle. Due to the way the controllers are designed, they will allow a certain amount of error in y-position before adjusting the yaw angle in order to reach the equilibrium necessary for maintaining a certain y-position, see Fig 23.
52
Fig 23: Current affecting the UUV
The maximum current disturbance for which the controller is designed to handle is 1 m/s, see Section 1.6.5: Sensitivity. By applying 1currentv = m/s, the following results are
generated.
Table 12: Simulation with 1 m/s current added to the system along the y-axis direction
Controller Fuzzy LQR Initial conditions for x [m,rad,m/s,rad/s] [-40 0 0 0 0 0 1 0 0 0 0 0] [-40 0 0 0 0 0 1 0 0 0 0 0]
Reference for x [m,rad,m/s,rad/s] [∞ 0 0 0 0 0 0 0 0 0 0 0] [∞ 0 0 0 0 0 0 0 0 0 0 0] y-offset when stabilized (s) 2.17 1.21
Yaw-offset when stabilized (rad) -0.63 -0.63
Table 12 shows that for the UUV to compensate for a 1 m/s current, a yaw angle of 0.63 radians is required. This is the value of the yaw angle that both controllers reach when equilibrium for y is reached. The Fuzzy controller is able to make the UUV reach equilibrium at 2.17 m of y-offset while the LQR is able to do so at 1.21 m of y-offset. The performance of the LQR can be enhanced by increasing Q(2,2). This will heighten the priority of keeping the y-offset near zero. Increasing Q(2,2) to 200 results in 0.36 m of y-offset required for equilibrium which is a significant improvement, yet still too far from the desired path for a successful docking. Increasing Q(2,2) further will reduce the offset but not to less than the required 0.1 m. Also, the increase will eventually lead to extreme oscillations for the associated input, in this case the vertical rudder angle. None of the controllers are therefore able to handle water current with a magnitude of 1 m/s. For a water current of 0.5 m/s with Q(2,2) set to 5000 the controller is able to reach equilibrium at a y-offset of 0.085 m which is within the accepted range of error. However, the large Q(2,2) value will significantly aggravate the overall performance of the controller. The plots representing the simulation with current applied can be found in Appendix A, Section 8.1.2.
Desired path
⇑Current
⇒UUV’s direction of motion
y-offset
y
Yaw offset
53
Noise, uω and ω , is introduced to the plant as shown in Fig 6 and Fig 12. The uω
represents white noise on the input to the system and is often caused by payload disturbances. As described in Section 1.6.5: Sensitivity, the amplitude of the noise on the input to the system, uω , is defined as 10% of the maximal input signals. The amplitude of
the noise on the output of the system,ω , is defined to be half the error margin of the corresponding state. No maximal error margins are available for states 7 through 12 so they are set to the same values as states 1 through 6. Table 13 provides a list of the values used. Fig 32 and Fig 33 in Appendix A, are plots of the noise levels. Applying these noise levels to the system and simulating a worst case scenario stating point, the following results are generated, Tables 14 (fuzzy control) and 15 (LQR control).
Table 13: Amplitude of noise applied to the closed loop system
thrustF
vϕ
hϕ x , u y , v z , w φ , p θ , q ψ , r
Maximal noise
amplitude 0.8 0.079 0.079 0.1 0.1 0.1 0.020 0.020 0.020
Table 14: Docking Scenario from worst case starting point with noise applied using Fuzzy controller
Controller Fuzzy Initial conditions for x [m,rad,m/s,rad/s] [-38 2 2 π/8 –π/4 π 1 0 0 0 0 0]
Reference for x [m,rad,m/s,rad/s] [-0.1 0 0 0 0 0 0 0 0 0 0 0] Value without noise Value with noise change
Overshoot in x (m) 0.077 (0.2%) 0.080 (0.21%) 0.003 3.9% Alignment time (s) 25.291 34.500 9.209 36.4%
Alignment distance (m) 19.285 32.328 13.043 67.6% Time until settled at destination (s) 44.279 44.600 0.321 0.7%
Max. vertical rudder angle (rad) 0.785 0.785 0 0 Max. horizontal rudder angle (rad) 0.745 0.734 -0.011 -1.5%
See Appendix B for definitions of alignment time etc.
Table 15: Docking Scenario from worst case starting point with noise applied using LQR
Controller LQR Initial conditions for x [m,rad,m/s,rad/s] [-38 2 2 pi/8 –pi/4 pi 1 0 0 0 0 0]
Reference for x [m,rad,m/s,rad/s] [-0.1 0 0 0 0 0 0 0 0 0 0 0] Value without noise Value with noise change
Overshoot in x (m) 0.009 (0.02%) 0.021 (0.06%) 0.012 133.3% Alignment time (s) 18.297 23.900 5.603 30.6%
Alignment distance (m) 8.131 15.992 7.861 96.7% Time until settled at destination (s) 70.060 69.100 -0.960 -1.4%
Max. vertical rudder angle (rad) 0.785 0.785 0 0 Max. horizontal rudder angle (rad) 0.785 0.785 0 0
See Appendix B for definitions of alignment time etc.
54
These results show that the alignment time and distance deteriorates when the noise is applied while the time until settled at destination stays basically the same. The overshoot is almost unchanged for the fuzzy controller but for the LQR the overshoot is more than doubled the value without noise. Considering the absolute change in alignment time and destination, it is shown that these values deteriorate more for the fuzzy controller than for the LQR. Despite the higher relative increase in overshoot for the LQR controller, it still performs better in every aspect than the fuzzy controller.
5.4 Abort
The abort function is only implemented for the fuzzy controller, see end of Section 3. To trigger the abort function a disturbance force is applied at center of mass of the UUV in the y-axis direction. The disturbance force applied is presented in Fig 34 in Appendix A, Section 8.1.3. By applying this disturbance, the abort function is initiated after 15.5 s and the UUV has traveled 22.8 m from the waypoint in the x-axis direction, see Fig 24 for the corresponding plots. As described earlier the abort function will guide the UUV back to the way point and commence a new docking scenario. Since the UUV at this point has a yaw angle of 0.1 rad, the closest way to turn the UUV to point it towards the waypoint
( )40 , 0, 0x m y z= − = = is by using a “right hand rule” turn around the z-axis. Therefore,
the yaw angle will increase until it reaches π rad. About this value the UUV’s yaw angle will then start to stabilize. The steps in vertical rudder angle at 15.5 s and 32.6 s corresponds to the engaging and disengaging of the abort function. The plot of y shows that the UUV needs a little less than 15 m of space off of the desired path to perform the turn. This value can be used to set the constraints on the environmental surroundings of the docking bay.
55
Fig 24: Relevant plots of docking scenario with abort function
Docking control
Docking control
Abort control
Required space in y-axis direction (almost 15 m)
56
6 CONCLUSIONS
The main objective of this thesis is to develop a controller capable of safely guiding a UUV from a waypoint 40 m away from a docking bay to rest inside the docking bay under specified requirements. Two controllers have been developed for this purpose. The first one is a rule based controller derived using Fuzzy Logic and the second one is a LQR using LQG control. When starting from a worst possible position, it has been shown that both these controllers are able to compensate the UUV sufficiently well before reaching the dock. Another requirement on the controllers was to be able to handle water current effects of at most 1 m/s. It was shown that neither of the controllers fulfilled this requirement. The LQR was the closest to meeting the requirements and could be improved even more but with a lower overall performance as a result. The controllers were also required to be able to handle noise on both inputs to the system, uω , and in
system output signals,ω . Both controllers were able to compensate for both of these disturbances and successfully dock the UUV when starting from the worst case scenario starting point with white noise applied to the system. This means that apart from the water current, both controllers were able to perform their objectives under stated requirements.
Also essential to this thesis is a comparison between the two controllers. Using response simulations, it was shown that the LQR demonstrated better control capabilities for all aspects of control of y and z . In control of x , the fuzzy controller proved to be faster but less accurate, i.e. higher overshoot. Since the speed in the x -axis direction is considered to be of less significance, all results indicate that LQR is the better controller for the purpose of safely docking the UUV. When the UUV is handed over from the GNC with an error in position or orientation, the same performance characteristics appear. The presented LQR is able to steer the UUV back on course more quickly and smoothly than the Fuzzy controller which tends to cause the UUV to oscillate even though it reaches the docking bay faster. Again, the time it takes to complete the docking is not as significant at this stage of the research development, so the LQR is again the controller that shows best performance. For the docking scenario with disturbance, starting from the worst possible initial state, the results are again more favorable for the LQR controller. The deterioration concerning alignment time and distance is greater for the fuzzy controller than for the LQR. Even though the overshoot performance deteriorated far more for the LQR than for the fuzzy controller, the LQR still shows much better overall performance.
Furthermore, the LQR responds faster to displacements in all directions except x. The LQR also handles disorientation faster than the fuzzy controller. Additionally, the LQR handled both water current and noise in a far better way than the fuzzy controller. In conclusion, these combined characteristics yield that the LQR is the natural choice of controller for autonomous underwater docking of an UUV.
57
7 FUTURE WORK
The ultimate future work is naturally to implement the described controllers, at least the LQR which performed far better than the fuzzy controller. Before that, there are several aspects that would need consideration. Concerning the position and orientation of the UUV, an investigation into available sensors is needed to further quantify what accuracy can be expected upon implementation.. For example, this information could then be used to assess what output errors would be expected in the feedback signals to the controller. It would also supply information regarding which states would be observed and fed into the controller. To only measure the first six states ( ), , , , ,x y z φ θ ψ would be enough to
maintain observability which is required for the LQR design. These inputs could then be used to generate the rest of the states required by the controller.
None of the controllers were very efficient in meeting the requirements concerning the water current. This static disturbance generates a static error for the LQR and fuzzy controlled system rendering them incapable of docking. Finding a solution that would improve the UUV performance when subjected to this condition would be an important aspect of the future work. A potential way to handle the static disturbance of the water drag would be to implement an integrating controller. Integrating controllers are especially effective at eliminating static errors and should therefore be able to handle the disturbance of the current.
Depending on the depth of the docking bay, waves may be a source of disturbance that needs to be considered. Also, an abort function for the LQR similar to the one developed for the fuzzy controller would have to be developed to complete the LQR docking control system.
To change the center of rotation and weight distributions of the model would allow one to see how well the controllers are able to handle controlling variations in the UUV. Further studies of UUV performance operating under these new circumstances would also yield additional information regarding the robustness of the controllers.
58
8 APPENDICES
8.1 Appendix A -Additional plots and results-
Fig 25: Non zero plant step responses for Input 1 – Thruster force
( ) 1 , 7Output x Output u= =
59
Fig 26: Non zero plant step responses for Input 2 – vertical rudder angle
( ) 2 , 6 , 8 , 12Output y Output Output v Output rψ= = = =
60
Fig 27: Non zero plant step responses for Input 3 – horizontal rudder angle
( ) 3 , 5 , 9 , 11Output z Output Output w Output qθ= = = =
61
Fig 28: Non zero plant step responses for Input 4 – keel torque
( ) 4 , 10Output Output pφ= =
62
11.58 0 0 0 0 0 37.00 0 0 0 0 0
0 2.79 0 0 0 3.29 0 1.26 0 0 0 0.34
0 0 2.79 0 3.39 0 0 0 1.26 0 0.34 0
0 0 0 1.16 0 0 0 0 0 0.18 0 0
0 0 3.39 0 7.76 0 0 0 2.32 0 0.82 0
0 3.39 0 0 0 7.76 0 2.32 0 0 0 0.82
37.00 0 0 0 0 0 144.02 0 0 0 0 0
0 1.26 0 0 0 2.32 0 0.97 0 0 0 0.28
0 0 1.26 0 2.32 0 0 0
P
− −
− −
=
− 0.97 0 0.28 0
0 0 0 0.18 0 0 0 0 0 0.21 0 0
0 0 0.34 0 0.82 0 0 0 0.28 0 0.15 0
0 0.34 0 0 0 0.82 0 0.28 0 0 0 0.15
−
− −
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(55)
The matrix P is symmetric and positive definite.
1 0 0 0 0 0 10.56 0 0 0 0 0
0 1 0 0 0 2.79 0 0.84 0 0 0 0.61
0 0 1 0 2.79 0 0 0 0.84 0 0.61 0
0 0 0 1 0 0 0 0 0 1.16 0 0
K− − − −
=− −
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
(56)
63
8.1.1 Additional worst case scenario plots
Fig 29: Worst case scenario, vertical rudder angle
Fig 30: Worst case scenario, horizontal rudder angle
Alignment time
Alignment time Alignment time
Alignment time
64
8.1.2 Simulations with current
Fig 31: Water current applied
65
8.1.3 Worst case scenario with uω and ω applied
Fig 32: Noise on input to system, uω
66
Fig 33: Noise on output from system ( )G , ω
Fig 34: Disturbance used to trigger abort function
67
8.2 Appendix B -Definitions-
Controller parameters - Indicates how the Q and R where chosen.
Initial conditions - Shows at what position and orientation the UUV is handed over from the GNC to the docking control system.
Alignment time The alignment time is a measure of the time from control handover from GNC (start of simulation) until the UUV is aligned within the error constraints of the desired path, meeting the requirements for docking as described in Section 1.6.4, Docking accuracy.
Alignment distance Alignment distance describes how far along the x-axis the UUV travels during the alignment time. See Fig 35 for a visual description of alignment distance.
Time until settled at destination - Time from handover of UUV control until the UUV has settled at a offsetx less than
0.01 m. Overshoot - See (Fig 36) Rise time -
See (Fig 36) Settling time -
See (Fig 36)
68
Fig 35: Definition of alignment distance
Fig 36: Step response with definitions
1.1
0.9
1
0.1
Overshoot
Rise time
y
t
Settling time
Alignment distance
Starting position (point of handover)
Position when ready to enter the docking bay (within docking accuracy requirements)
x
Desired path
69
8.3 Appendix C -Coefficients-
Table 15: Coefficients
Symbol Value Units Description
UUVr 9.50e-02 m Radius of UUV
l 1.60e+00 m Total length
m 3.70e+01 kg Total weight
frontl 3.00e-01 m Length from cm to front drag forces
aftl 5.00e-01 m Length from cm to aft drag forces
finsl 8.40e-01 m Length from cm to centre of fins
liftc 3.12e00 n a Lift coefficient for UUV fins
frontc 2.70e-01 n a Drag coefficient for UUV front
sidec 1.10e00 n a Drag coefficient for side of body
waterρ 9.99e+02 3/kg m Density of water
rudderA 1.33e-02 2m Projected rudder area (2*fin area)
frontalA 2.85e-02 2m Projected frontal area
frontsideA 8.90e-02 2m Projected side area in front of cm
aftsideA 1.37e-01 2m Projected side area behind cm
70
liftforcesc 20.72e00 n a Combined coefficient for lift forces
frontalc 3.84e00 n a Combined coefficient for frontal drag
frontsidec 48.90e00 n a Combined coefficient for front drag forces
aftsidec 75.27e00 n a Combined coefficient for aft drag forces
maxFt 8.00e00 N Maximum thruster force
maxvϕ 7.85e-01 rad Maximum vertical rudder angle ( )4π
maxhϕ 7.85e-01 rad Maximum horizontal rudder angle ( )4π
71
9 NOTATION
Table 16: Notations for input signals to the system
Input signals to the system
thrusterF Thruster force
vϕ Vertical rudder angle
hϕ Horizontal rudder angle
keelT Keel torque
inu [ ], , ,T
thruster v h keelF Tϕ ϕ
Table 17: Notations for the states of the system
States
φ Roll angle
θ Pitch angle
ψ Yaw angle
x x-coordinate in inertial coordinate system
y y-coordinate in inertial coordinate system
z z-coordinate in inertial coordinate system
B x x-coordinate in body fixed coordinate system
B y y-coordinate in body fixed coordinate system
B z z-coordinate in body fixed coordinate system
72
u Velocity along B x , Bu x= �
v Velocity along B y ,Bv y= �
w Velocity along B z ,Bw z= �
p Angular velocity about B x
q Angular velocity about B y
r Angular velocity about B z
x [ ]T
x y z u v w p q rφ θ ψ
r [ ]T
x y z
θ [ ]T
φ θ ψ
v[ ]
Tu v w
ω[ ]
Tp q r
73
10 REFERENCES
1. Bob Barton, Offshore Engineer, 14 Jan 2003
2. Unmanned Underwater Vehicles, Marcus Evans Conferences, March 2005
3. Woods Hole Oceanographic Institution, http://www.whoi.edu
4. Hydroid inc. http://www.hydroidinc.com
5. W. Naeem, R. Sutton, S. M. Ahmad and R. S. Burns, A Review of Guidance Laws Applicable to Unmanned Underwater Vehicles. The Journal of Navigation (2003), 56, 15-29
6. J. Evans, P. Redmond, C. Plakas, K. Hamilton and D. Lane. Autonomous Docking for Intervention-AUVs using sonar and video-based Real-time3D Pose Estimation. 2003
7. P. Lee, B. Jeon and S. Kim. Visual Servoing for Underwater Docking of a Autonomous Underwater Vehicle with One Camera. 2003
8. M. Feezor, F. Sorrell, P. Blankinship and J. Bellingham. Autonomous Underwater Vehicle Homing/Docking via Electromagnetic Guidance. IEEE JOURNAL OF OCEANIC ENGINEERING, VOL. 26, NO. 4, OCTOBER 2001
9. P. Craven, R. Sutton and R. Burns. Control Strategies for Unmanned Underwater Vehicles. 1998
10. J. Lots, D. Lane, E. Trucco, F. Chaumette. A 2-D visual Servoing for Underwater Station Keeping. Proceedings of the 2001 IEEE International Conference on Robotics & Automation Seoul, Korea – May 21-26, 2001
11. N. Sarkar, J. Yuh and T. Podder. Adaptive Control of Underwater Vehicle-Manipulator Systems Subject to Joint Limits. Proceedings of the 1999 IEEE/RSJ International Conference on Intelligent Robots and Systems.
12. W. Naeem, R. Sutton and S. Ahmad. LQG/LTR Control of an Autonomous Underwater Vehicle Using a Hybrid Control Law. 2003
13. P. DeBitetto. Fuzzy Logic for Depth Control of Unmanned Undersea Vehicles. IEEE JOURNAL OF OCEANIC ENGINEERING, VOL. 20, NO. 3, JULY 1995
74
14. K. White, S. Smith, K. Ganesan, D. Kronen, G. Rae and R. Langenbach. Performance Results of a Fuzzy Behavioral Altitude Flight Controller and Rendezvous & Docking of an Autonomous Underwater Vehicles with Fuzzy Logic. 1996
15. O. Berglund. Design of a Capturing Mechanism for Intelligent Docking of UUVs. 2004
16. Institute of Marine & Coastal Sciences. Rutgers University. REMUS Autonomous Underwater Vehicle. http://marine.rutgers.edu/rumfs/MarineOps/Marineopsremus.html
17. T. Prestero. Verification of a Six-Degree of Freedom Simulation Model for the REMUS Autonomous Underwater Vehicle. 2001
18. F. Fish, G. Lauder, R. Mittal, A. Techet, M. Triantafyllou, J. Walker and P. Webb. Conceptual Design for the Construction of a Biorobotic AUV Based on Biological Hydrodynamics.