Institutionen för systemteknik
Department of Electrical Engineering
Examensarbete
Advanced control of a remotely operated underwater
vehicle
Examensarbete utfört i Reglerteknik vid Tekniska högskolan vid Linköpings universitet
av
Patrik Johansson and Jacob Bernhard LiTH-ISY-EX--12/4599--SE
Linköping 2012
Department of Electrical Engineering Linköpings tekniska högskola
Linköpings universitet Linköpings universitet
Advanced control of a remotely operated underwater
vehicle
Examensarbete utfört i Reglerteknik
vid Tekniska högskolan vid Linköpings universitet
av
Patrik Johansson and Jacob Bernhard LiTH-ISY-EX--12/4599--SE
Handledare: Tohid Ardeshiri
isy, Linköpings universitet Examinator: Daniel Axehill
isy, Linköpings universitet
Avdelning, Institution Division, Department
Automatic Control
Department of Electrical Engineering SE-581 83 Linköping Datum Date 2012-06-15 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport
URL för elektronisk version
http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-XXXXX
ISBN — ISRN
LiTH-ISY-EX--12/4599--SE
Serietitel och serienummer Title of series, numbering
ISSN —
Titel Title
Avancerad reglering av en fjärrstyrd undervattensrobot. Advanced control of a remotely operated underwater vehicle
Författare Author
Patrik Johansson and Jacob Bernhard
Sammanfattning Abstract
Remotely Operated underwater Vehicles (ROVs) are getting more and more advanced with every new model. As new functionality is added, the price increases. This thesis is one part of a larger project, where the goal is to develop a low-budget ROV. The ROV should later be made autonomous and entered into a competition.
This thesis have focused on the modeling and stabilizing control of an ROV that was designed by mechanical engineering students at Linköping University. The only sensor used was an Inertial Measurement Unit (IMU) and the ROV has a torpedo-like design. The modeling was done using identification in Matlab with the grey box and black box methods. Water trials and simulations show that the model is estimated sufficiently good to be used as the basis of different model based controllers.
Two different control strategies were implemented; a linear quadratic controller (LQ) and a model predictive controller (MPC). Both controllers worked desirably in simulations. Only the LQ controller was evaluated in real world tests. Due to problems with the implementa-tion environment chosen for the MPC, the MPC could not be tested.
The thesis also uses decision matrices as a mean to motivate the important decisions that have been made.
Nyckelord
Abstract
Remotely Operated underwater Vehicles (ROVs) are getting more and more ad-vanced with every new model. As new functionality is added, the price increases. This thesis is one part of a larger project, where the goal is to develop a low-budget ROV. The ROV should later be made autonomous and entered into a com-petition.
This thesis have focused on the modeling and stabilizing control of an ROV that was designed by mechanical engineering students at Linköping University. The only sensor used was an Inertial Measurement Unit (IMU) and the ROV has a torpedo-like design. The modeling was done using identification in Matlab with the grey box and black box methods. Water trials and simulations show that the model is estimated sufficiently good to be used as the basis of different model based controllers.
Two different control strategies were implemented; a linear quadratic controller (LQ) and a model predictive controller (MPC). Both controllers worked desirably in simulations. Only the LQ controller was evaluated in real world tests. Due to problems with the implementation environment chosen for the MPC, the MPC could not be tested.
The thesis also uses decision matrices as a mean to motivate the important deci-sions that have been made.
Acknowledgments
This semester has been an exciting journey. We could not have done the ROV project without help from some amazing people.
We want to start by thanking our examiner Daniel Axehill and our supervisor Tohid Ardeshiri for taking a special interest in our project and always being available for discussion. Your commitment and drive have been an inspiration throughout the project.
Marcus Eriksson will have a special place in our hearts. We will always share this success with you. We hope you never have to tighten a screw-nut again, "Mutter Mutter".
Our thanks also go out to the staff at IEI for founding this project and offering us this chance to have an exciting practical Master Thesis. Especially we would like to thank Lars Andersson for helping with the electronics and Magnus Carlsson for discussing solutions and ideas.
Special thanks to the staff at Ljungsbro Simhall for changing the appointments when we had a last minute abortion, borrowing swim goggles and a monkey wrench when things were tough.
Also thanks to our friends and families that have listened to our problems in our time of need. Especially Karl-Johan Barsk, for his company at fika-time and the project discussions we had.
We would also like to thank the ISY support staff; Greger Karlströms and Jean-Jaques Moulis for supplying us with computers and hard-drives.
As a response to our opponents’ remark in their acknowledgments, we would like to thank Daniel Karlsson and Marcus Arvidsson for showing good sports-manship and not mentioning our defeat in badminton. The rematch will be held in Rydskogen with a set of frisbees. Are you ready?
Lastly we send our thanks to Joachim Ferreau, David Törnqvist and Manon Kok for their help. And lastly we thank Niklas Wahlström, for borrowing us the IMU. Quod qui non supernatet, mergi
Linköping, Juni 2012 Patrik Johansson och Jacob Bernhard
Contents
Notation 9 1 Introduction 11 1.1 Background . . . 11 1.2 ROVs . . . 12 1.3 Motivation . . . 12 2 System description 15 2.1 The motors . . . 16 2.2 The electronics . . . 16 2.2.1 Arduino board . . . 17 2.2.2 Control cards . . . 17 2.3 The sensors . . . 17 2.3.1 IMU . . . 17 2.3.2 Web camera . . . 18 2.3.3 Leakage sensor . . . 18 2.3.4 Pressure sensor . . . 19 2.3.5 Tachometer . . . 192.3.6 Acoustic measurement devices . . . 19
3 Modeling 21 3.1 First principles modeling . . . 21
3.1.1 Estimating the thruster force . . . 22
3.1.2 Free body diagrams . . . 22
3.1.3 Model linearization and discretization . . . 30
3.1.4 The model . . . 31
3.1.5 Future work . . . 34
3.2 Parameter estimation . . . 34
3.2.1 Black box modeling . . . 34
3.2.2 Grey box modeling . . . 36
3.3 Observer . . . 38
3.3.1 Kalman filter . . . 38
3.3.2 Extended Kalman filter . . . 40 5
6 CONTENTS 3.3.3 Results . . . 41 3.4 Conclusion . . . 44 3.4.1 Parameter estimation . . . 44 3.4.2 Observer . . . 44 4 Controller 45 4.1 Linear quadratic controller . . . 45
4.2 Model predictive control . . . 46
4.2.1 Linear MPC . . . 46
4.2.2 Nonlinear MPC . . . 48
5 Implementation 51 5.1 Decisions . . . 51
5.1.1 Decision on operating system . . . 52
5.1.2 Decision on MPC implementation environment . . . 53
5.2 Software . . . 54
5.2.1 Software overview . . . 55
5.2.2 Robotic Operating System (ROS) . . . 56
5.2.3 Steering the ROV . . . 57
5.2.4 Control modes . . . 58 5.2.5 CUSUM . . . 59 5.3 Controller implementation . . . 61 5.3.1 LQ . . . 62 5.3.2 MPC . . . 63 6 System evaluation 65 6.1 Simulations . . . 65 6.1.1 Simulation scenarios . . . 68 6.1.2 Conclusions . . . 70 6.2 Dry tests . . . 71 6.2.1 DT1 - Magnetometer test . . . 71
6.2.2 DT2 - Leakage sensor test . . . 72
6.3 Static wet tests . . . 72
6.3.1 SWT1, Controller test . . . 72
6.3.2 SWT2, Stress testing . . . 73
6.4 Dynamic wet tests . . . 73
6.4.1 DWT120217, Identification . . . 74
6.4.2 DWT120425, Identification and verification . . . 74
6.4.3 DWT120525, Identification and verification . . . 74
6.5 Magnetometer disturbance . . . 74
6.6 Recommendations . . . 75
7 Discussion 77
8 Conclusions 79
CONTENTS 7
A.1 Model disclaimers . . . 81
A.2 Future work . . . 81
B Test procedure 83 B.1 Data collection needed . . . 83
B.1.1 Important things to note . . . 84
B.1.2 Handling the collected data sets . . . 84
B.2 How to use the estimation files . . . 84
B.2.1 Troubleshooting . . . 85
B.3 How to use the newly created model . . . 85
Notation
Abbreviations
armax Auto-regressive moving average with exogenous input arx Auto-regressive with exogenous input
mpc Model Predictive Controller
nmpc nonlinear Model Predictive Controller lq Linear Quadratic
rov Remotely Operated underwater Vehicle auv Autonomous Underwater Vehicle
ekf Extended Kalman Filter imu Inertial Measurement Unit rpm Revolution per minute
isy Department of Electrical Engineering
iei Department of Management and Engineering sitb System Identification Toolbox
dt Dry Test
swt Static Wet Test
dwt Dynamic Wet Test
1
Introduction
This thesis is a part of a chain of projects using the ROV as a platform. This chapter will describe the origin of the project, the long-term goal and which goals this specific part of the project has achieved.
1.1
Background
In the fall of 2010, eleven students at IEI (Department of Management and En-gineering) at the University of Linköping (LiU) started designing a remotely op-erated underwater vehicle (ROV). When the project was finished, it was decided that the robustness of the system had to be increased and that the design should be evaluated. One year later another group of students implemented the current design (see Figure 1.1) and it was tested under water.
Based on these real-world experiments it was decided that the current design was a good basis for future projects. In the fall of 2011, the authors of this thesis via ISY (Department of Electrical Engineering) at LiU were assigned the task to investigate the controllability of the system and to model the dynamics of the ROV. Two control schemes were planned to be evaluated; firstly an LQ controller and later an MPC controller. The LQ controller was meant to be more of a proof of concept, whereas the MPC was supposed to be the advanced controller. Parallel to this thesis, a student at IEI (Marcus Eriksson) also has been working on the ROV. The goal of that thesis is to improve the mechanical implementation of the ROV and install the new functionality that this thesis and the future projects require.
12 1 Introduction
Figure 1.1:The ROV seen from the right-hand side.
1.2
ROVs
Remotely operated underwater vehicles (ROVs) have since first developed in the mid 1950s by the Royal Navy, become more and more common. At first they were used by different authorities to recover torpedoes on the ocean floor and to clear mines. In the mid 1980s ROVs became an essential part of the offshore oil and gas industry, since the pipes were placed deeper and further into the ocean. As we enter the 21:st century, ROVs are a commercial product that can be used for anything from exploration and surveillance to rescue missions and maintenance. As the ROVs become more advanced, their price increases.
1.3
Motivation
The aim of the long-term project is to develop a robust and practical cost-efficient ROV that in a couple of years can be turned into an Autonomous Underwater Vehicle (AUV) and enter the SAUC-E competition, [Sau, 2012].
The SAUC-E competition is a yearly competition for AUVs from all over the world. A variety of tasks have to be completed, which demand that the AUV can move autonomously and carry out a predefined mission using only its own sensor data. With this long-term goal in mind, the objective of this thesis has been to take the first step towards autonomy by closing the control-loop, developing an advanced controller and creating a robust software foundation for future projects.
1.3 Motivation 13 design is the MARES-project, [Ferreira et al., 2009], where they offer a method of modeling an underwater vehicle. The fact that the overall design of their AUV is similar to our ROV, made their report a good starting reference for our work.
2
System description
This chapter will describe the design of the ROV and its implications. The equip-ment that is being used and possible future equipequip-ment is also discussed. To sum-marize the mechanical design, the following observations are made:
Advantages of the current design:
• It has a streamline design that do not suffer of water damping effects as most other ROV designs do.
• The design is both durable and easily repaired, since most parts of the ROV are made at IEI.
• It has specially designed propellers, which makes it possible to generate the same amount of force in both directions of the tunnel thrusters.
• The weight is placed so that the ROV always strives to stabilize itself. Disadvantages of the current design:
• Due to Newtons third law, acceleration with the back engine will result in an unwanted roll rotation. This can cause problems, especially if the user is using the on board camera. Fortunately, due to the pendulum stability of the ROV, it will not turn over.
• It is known that tunnel thrusters loose performance at higher speeds. • The design makes it difficult to remove the ROV computer and recharge
the batteries. If the user wants to recharge the batteries, the ROV have to be opened and the electronics taken out.
16 2 System description • The design of the propellers makes them less effective than a normal
pro-peller in one direction.
2.1
The motors
The ROV has a torpedo-like design which was mainly chosen by the previous projects to be able to move fast in the surge direction, [IEI, 2012]. Two schematic models of the ROV can be seen in Figure 2.1. To control the vessel, the ROV is equipped with five thrusters; one main thruster at the back that controls the surge motion ( ˙x), two horizontally aligned thrusters to control the yaw rate ( ˙ψ) and the sway ( ˙y), and two vertically aligned thrusters to control the pitch rate ( ˙θ) and the heave (− ˙z). The roll rate, ˙φ , is not independently controllable, which results in five degrees of freedom.
(a) x y B&G (b) x z G B
Figure 2.1: A principal physical model of the ROV seen from; (a) an above and (b) a frontal view.
2.2
The electronics
To be able to control a robot, a variety of electric components are needed. In this project the electronics mainly consist of a main computer, a hard drive, a measurement board (Arduino board), control cards and a set of sensors to diag-nose and observe the system. The energy source consists of 18 Li-Po-batteries (Lithium-ion polymer batteries) that together provide 37.8 Ah. The most impor-tant devices will be described below. More information about the electronics can be read in the user manual,[IEI, 2012].
Since electronics usually are sensitive to water, a water-proof container is needed inside the underwater vehicle. In this project, the main electronics are assembled on a plastic structure inside an aluminum hull at the center of the ROV.
2.3 The sensors 17
2.2.1
Arduino board
The Arduino board consists of an open-source micro controller designed to make the electronics more accessible in software projects. The board has a set of ana-logue input pins and digital ports that can read and send signals to other compo-nents. The Arduino board will distribute most signals used by the system and is therefore one of the most important parts of the system.
Since the micro controller has its limitations regarding memory, this board is a possible bottle-neck in the system.
2.2.2
Control cards
Each control card transforms a voltage signal from the Arduino board to a PWM signal (Pulse-Width Modulation) that is sent to each motor. A PWM signal is a signal that always has a fixed amplitude, but vary the pulse-width. Since the motors have different dimensions, the current needed to drive them is different depending on the motor. Therefore it is important to have a reasonable current-limitation.
2.3
The sensors
The only sensors that were installed beforehand were a leakage sensor and a web camera. To be able to control the robot, information about the orientation, posi-tion or moposi-tion must be added. Therefore an IMU (Inertial Measurement Unit) from Xsens was installed, see Figure 2.2. One additional aspect of the project was to find out which sensors are needed to turn the ROV into an AUV in future projects. Some suggestions are discussed in this section.
2.3.1
IMU
An IMU is a measurement device that combines three different sensors into one measuring board. The IMU consists of a gyroscope that measures angular veloc-ities, three accelerometers that measures the acceleration in each direction and a magnetometer that measures the magnetic field. With this information, it is straight-forward to use the Kalman filter to estimate the orientation of the de-vice. In this way we can attain knowledge of the orientation, rotation and linear motion of the ROV. Since the IMU came with a built in configured Kalman filter there was no need to estimate the orientation with the help of the accelerome-ter and the magnetomeaccelerome-ter, therefore it was decided to use the IMUs orientation estimate.
The IMU had to be placed inside the water proof hull. It is usually a good de-cision to place the IMU close to the mass center, but in this case it is not the best placement since the IMU could be disturbed by the electronics. Strong cur-rents in the electronics that control the motors generate a magnetic field, which could potentially cause a problem with the magnetometer-measurement. Since
18 2 System description
Figure 2.2:The IMU from Xsens that is being used in the project.
the magnetometer data is only used by the Kalman filter to estimate the angles, it is only the angle-estimates that could be corrupted by this effect.
2.3.2
Web camera
The web camera is currently placed in the front of the ROV. It was previously used as a mean to see how the ROV was oriented, but due to great delays in the previous system it was not very useful. The plan for the future is to use the information from the web camera to obtain a visual feedback to the control-system. By interpreting how the particles in the water move across the screen, it is possible to obtain information about how the ROV is moving relative to the water. The web camera is not used in this thesis since it will not be useful until the autonomous stage.
2.3.3
Leakage sensor
The leakage sensor was constructed by the previous project and can be seen in Figure 2.3. It consists of two racks of metallic pins that are connected to the Arduino board via a resistor and placed at the bottom of the board of electronics. One part of each rack is connected to ground and the other is connected to the resistor. When no water is present, the racks are short-circuited and the Arduino board reads a voltage of 4.8 V. When water or moisture enters the metallic tube, the water on the rack connects the pins causing the ground and resistor to be connected. This causes a voltage drop, that is easily discovered by the software. The racks are placed at opposite ends of the tube so that the rotation does not effect the sensor-readings.
One drawback with this type of leakage sensor is that it requires a conductive fluid, which distilled water is not. This means that different water types will conduct electricity differently and therefore give different or no reading. This is something that needs to be checked carefully before every new water trial.
2.3 The sensors 19
Figure 2.3:One of the racks that are a part of the leakage sensor.
2.3.4
Pressure sensor
Pressure is the effect of a force applied to a surface. It is defined as the amount of force acting per unit area. The standard unit for pressure is the Pascal, which is a Newton per square meter, [pre, 2012]. The most common pressure sensor is the differential pressure sensor that measures the difference between two pressures. Since we know that the difference in pressure grows with the depth, we receive a dead-reckoning measurement of the z-position. This could greatly improve the estimate of the velocities and angular velocities. Knowing the z-position would also open up the possibility of changing modes between being completely sub-merged and being close to the surface. The pressure sensor installed in the ROV have a set reference value and compares that fixed pressure to the pressure out-side the ROV. Since the sensor has a set reference value, it has to be calibrated before each water trial.
2.3.5
Tachometer
Tachometers are used for measuring the rotational speed of the propellers. Since the propellers are custom made by previous students at IEI, the relation between the control signal and the force generated by the propeller is unknown. Models that describe the relation between the rotational speed and force generated exist, but if the relation between control signal and the rotational speed is nonlinear that relation must be estimated. Then the rotational speed must be measured and the tachometer is needed. This year’s project did not have time to install tachometers.
2.3.6
Acoustic measurement devices
To be able to take the step from an ROV to an AUV, the robot must have a position estimate. A pressure sensor can help with estimating the depth, but to be able to perform more complicated tasks, a three dimensional position is needed. The way that most AUVs solve this is by using some sort of sonar or other acoustic measurement device.
propaga-20 2 System description
tion in the water to navigate and locate surrounding objects. There are two types of sonar; active and passive sonar. An active Sonar, Figure 2.4, sends out a sound pulse and listens for the echo. By measuring the time it takes for the pulse to be received at the transmitter, the distance to the nearest object in the current direc-tion can be found. If the environment is known (e.q. with a map), the posidirec-tion of the ROV can be estimated. By comparing the distance at one time instance with the distance at another time instance, relative motion can be estimated. A passive sonar only listens for a sound wave and can therefore only be used for detecting objects and not for navigation in this framework.
Figure 2.4:A schematic image of the workings of an active sonar, [wik]. A sonar was not added during our project, but has to be considered for upcoming projects. One down-side with sonar is that it is an expensive piece of equipment.
3
Modeling
In this chapter the modeling aspects of the project will be discussed. Here we list the motivation behind modeling.
• In order to make it possible to estimate the states of the ROV and filter out some measurement noise from the sensors, an observer is needed. This in turn requires a model.
• With a model, it is possible to simulate different behaviors of the ROV. This makes it possible to suggest changes in the ROV design.
• The model can be used to create model based controllers.
The first part of this chapter will go through the procedure of deriving the model and the second part will show a direct usage for the model.
3.1
First principles modeling
Creating an exact model is often difficult and requires a lot of tests and physical analysis. By stating the free body diagram and estimating the most significant parameters, it is possible to create the first model. This has been done in this section, where a model has been derived for the ROV with the help of free body diagrams and a physical interpretation.
22 3 Modeling
3.1.1
Estimating the thruster force
Since the propeller speed cannot be measured, a thruster model had to be esti-mated. According to Palmer et al. [2008], a good first modeling approach is to estimate the generated thruster force as quadratic to the turning rate of the motor. Palmer et al. [2008] also says that it in most cases is reasonable to assume a linear behavior between the input voltage to a motor and the generated revolution per minute (RPM) for the propeller. Using that assumption, we derive the expression
F = C · u|u|. (3.1)
3.1.2
Free body diagrams
In this section, the free body diagrams will be presented with the resulting equa-tions and the estimated variables. For more information about the estimation process go to section 3.2.
Note that a few effects, like wind and wave, have been neglected due to the as-sumption that the ROV’s main operating environment is under water.
3.1 First principles modeling 23
In Table 3.1, there is a short description of which variables that is going to be used in this section.
Variable Description
G Center of mass
B Center of buoyancy
C Center of camera
m Mass of the ROV
dmx, dmy, dmz Added mass generated from displacing fluid due to
acceleration of the ROV Fzr, Fzf, Fyr, Fyf, FT Force generated by thrusters
Tzr, Tzf, Tyr, Tyf, TT Torque generated from accelerations with thrusters
xzr, xzf, xyr, xyf Distance from thrusters to G
zB Distance in z-direction from the center of buoyancy
to the center of mass
zC Distance in z-direction from center of mass to the
middle of the ROV where the camera is located Cp, Cy Linear damping coefficient for pitch and yaw rate
Cpd, Cyd Quadratic damping coefficient for pitch and yaw rate
Dx, Dy, Dz Drag and damping forces.
TDr, TDp, TDy Torque generated by drag and damping forces in roll,
pitch and yaw
Ixx Rotational inertia in roll direction
Iyy Rotational inertia in pitch direction
Izz Rotational inertia in yaw direction
FB Force generated from buoyancy
Trest,r, Trest,p Restoring torque in roll and pitch direction.
Czr, Czf, Cyr, Cyf, CT Thruster coefficient.
uzr, uzf, uyr, uyf, uT Control signals to thrusters.
V Water tight volume of the ROV.
ρ Water’s density.
L Length of the ROV from the main thruster to the cam-era.
d Maximum diameter of the ROV.
b Length between rear vertical thruster and rear for-ward thruster.
24 3 Modeling
To be able to obtain a clear picture of the size of the ROV, Figure 3.1 is made to display the dimensions for the ROV.
(a) xzr xzf L b x z G B (b) y z G B C d zC zB (c) x y xyr xyf B&G (d)
Parameter Measured value
L 1.400m b 0.350m d 0.250m xyr 0.310m xyf 0.350m xzr 0.420m xzf 0.460m m 40kg (e)
Parameter Estimated value
zB ≈0.1m
zC ≈0.05m
xB 0.0m
yB 0.0m
V ≈0.023m3
Figure 3.1: The dimensions of ROV shown in three schematics; (a) frontal view, (b) side view ,(c) top view, (d) measured parameters, (e) estimated pa-rameters .The papa-rameters in (e) have been estimated through assumptions and measurements.
3.1 First principles modeling 25 Tyr Tyf x z X Z Fzr Fzf FT G B mg FB Dx L Dz TDp x z θ
Figure 3.2:The free body diagram of the ROV, seen from a frontal view
The free body diagram in Figure 3.2 is used to extract the torques in
X Fz = −Dz−L + Fzr+ Fzf + mg cos θ − FB, (3.2) X Fx= −Dx+ FT −mg sin θ, (3.3) X My= Tyr+ Tyf + Fzrxzr−Fzfxzf −TDp−(FT −Dx)zC−zBFBsin θ, (3.4) where, FB= ρV g, FT = CTuT|uT|, TDp= Cpθ + C˙ pdθ| ˙˙θ|, Fzr = Czruzr|uzr|, Fzf = Czfuzf|uzf|. (3.5) Combining (3.4) and (3.5) with Newton’s second law of motion, we obtain
(m + dmz) ¨z = −Dz−L + Czruzr|uzr|+ Czfuzr|uzr|+ mg cos θ − ρV g, (3.6)
(m + dmx) ¨x = −Dx+ CTuT|uT| −mg sin θ, (3.7)
Iyyθ =C¨ zrxzruzr|uzr| −Czfxzfuzf|uzf| −Cpθ − C˙ pdθ| ˙˙θ|
+ Tyr+ Tyf −(FT −Dx)zC−zBFBsin θ. (3.8)
The last term in Equation (3.4) is also called the restoring torque and is denoted by Trest,p. It is the torque that brings the system back to equilibrium and therefore
26 3 Modeling
Trest,p= −zBFBsin θ. (3.9)
In Table 3.2, there is a list of the unknown parameters included in the pitch equations. They were estimated using a grey box method according to Section 3.2.
Parameters Estimated value
Cp Iyy 0.50 Cpd Iyy 0.10 Tyr+Tyf Iyy 0.00 FBzB Iyy 0.22 Czr Iyy 0.11 Czf Iyy 0.07 (FT−Dx)zC Iyy 0.00
Table 3.2:A table describing the parameters that need to be estimated. Here, the torque generated by Tyr+ Tyf and (FT −Dx)zCare known to be small.
3.1 First principles modeling 27 Tzr x Tzf y X Y Fyr Fyf B&G Dy TDy x y ψ
Figure 3.3:The free body diagram of the ROV seen from a top view
The free body diagram in Figure 3.3 is used to extract the equations X Fy= Fyr+ Fyf −Dy, (3.10) X Mz= Tzr+ Tzf + Fyfxyf −Fyrxyr−TDy, (3.11) where, TDy= Cyψ + C˙ ydψ| ˙˙ψ|, Fyr= Cyruyr|uyr|, Fyf = Cyfuyf|uyf|. (3.12) Using Newton’s second law of motion as before, the following equations can be derived
(m + dmy) ¨y = Cyruyr|uyr|+ Cyfuyf|uyf| −Dy, (3.13)
¨
28 3 Modeling
In Table 3.3, there is a list of the unknown parameters included in the yaw equa-tions. They were estimated according to Section 3.2.
Parameters Estimated value
Cy Izz 0.27 Cyd Izz 0.67 Tzr+Tzf Izz 0.00 Cyr Izz 0.10 Cyf Izz 0.11
Table 3.3:A table describing the parameters that needed to be estimated. Here, the torque generated by Tzr+ Tzf is known to be a small value. Since it
3.1 First principles modeling 29 y z G B FB TDr φ mg TT Y Z
Figure 3.4:The ROV seen from a side view
The free body diagram in Figure 3.4 is used to extract the equation
X
Mx= TT −TDr−zBFBsin φ, (3.15)
where,
TDr= Crφ + C˙ rdφ| ˙˙φ|.
(3.16)
Using Newton’s second law of motion in angular form, the rotational acceleration can be derived as
¨
φIxx= CTuT|uT| −Crφ − C˙ rdφ| ˙˙ φ| − zBFBsin φ. (3.17)
In (3.17) the term TT is approximated by CTuT|uT|. The last term in the equation
is also called restoring torque in roll direction and is denoted by Trest,r and is
calculated according to
Trest,r= ~rBG×~Frest= −zBFBsin φ. (3.18)
In Table 3.4, there is a list of the unknown parameters included in the roll equa-tions. They were estimated according to Section 3.2.
30 3 Modeling Parameters Estimated value
Cr Ixx 0.65 Crd Ixx 5.11 CT Ixx 0.66 zBFB Ixx 7.74
Table 3.4:A table describing the parameters that needed to be estimated.
3.1.3
Model linearization and discretization
Linearization
Since the main goal of this thesis is to use the nonlinear model in both the ob-server and the controller, only a linearization around the origin was used. Ac-cording to Glad and Ljung [2008], if there is a stationary point x0, u0, y0, it is
possible to Taylor expand ˙x into what is displayed in Equation (3.20) by studying small deviations from x0as seen in
x =x0+ ∆x,
u =u0+ ∆u. (3.19)
˙x = f (x0+ ∆x, u0+ ∆u)
≈f (x0, u0) + fx(x0, u0)∆x + fu(x0, u0)∆u (3.20)
This will cause the model to be fairly exact around its origin, but have some particularity around high rate values or larger absolute values on the roll and pitch angle (since the sine function is equal to zero if we linearize around the origin). This due to the fact that the more states diverge from the linearization point, the bigger the linearization error grows.
Using the linearization above, the model’s nonlinear terms can be approximated according to
x|x| ≈ 0,
sin(φ), sin(θ) ≈ 0. (3.21)
Discretization
In order to make a discretization of the model, an Euler forward method with a given sample time Tswere used according to
3.1 First principles modeling 31
˙y = f (x) ⇒
yn+1= yn+ Tsf (x). (3.22)
Using this on a linear state space model in continuous time according to
˙x = Ax(t) + Bu(t), (3.23)
will be written according to
xk+1= (I + ATs)xk+ TsBuk. (3.24)
3.1.4
The model
In the following section the model used in this thesis is described and compared to the so called "Shark-model", [Campa, 2001]. The current model is derived as described previously by this chapter and it turns out to be equivalent to the part of the Shark-model that describes the orientation. The plan was to use the com-plete Shark-model (including position estimates) to be able to integrate the depth sensor, but due to lack of time, it was not possible to implement the complete Shark-model.
Current model
The current model only describes the orientation and rotation of the ROV. To be able to compare the current model with the Shark-model, we use the same notation as by Campa [2001]. Using Equation (3.8),(3.14) and (3.17), while using a matrix notation, the orientation model will be given on the form
I ¨x = −D ˙x − Dd˙x2+ τrest(x) + τb, (3.25) where, x = φ θ ψ , τb= CTuT|uT| Czfuzf|uzf|+ Czruzr|uzr| Cyfuyf|uyf|+ Cyruyr|uyr| , I = Ixx 0 0 0 Iyy 0 0 0 Izz ,
32 3 Modeling D = Cr 0 0 0 Cp 0 0 0 Cy , Dd= Crd 0 0 0 Cpd 0 0 0 Cyd , τrest= −zBFBsin φ −zBFBsin θ 0 .
Complete Shark model
For understanding and learning how a professional ROV model is expressed, the Shark-model Campa [2001] has been used as an example. This section will de-scribe and explain the complete Shark-model.
To help ease the calculations and notations, a few utility matrices are defined. There will be two different frame coordinate systems defined.
1. The body frame, b, is a coordinate system that is fixed in the body of the ROV.
2. The world frame, w, is a coordinate system that is fixed in the earth. To define the state variables we need two sets of variables; one defining the ROV in the world frame and one defining it in the body frame. The state variables in the world frame are called η and the ones in the body frame υ according to
η =hη1η2 i =hx y z φ θ ψiT ; η1= [xyz]T; η2= [φθψ]T, (3.26) υ =hυ1υ2 iT =hu v w p q riT; υ1= [uvw]T; υ2= [pqr]T. (3.27)
A rotation matrix is defined, which takes three angles φ, θ and ψ as seen in
bR e=
cos θ cos ψ cos θ sin ψ −sin θ
sin φ sin θ cos ψ − cos φ sin ψ sin φ sin θ sin ψ + cos φ cos ψ sin φ cos θ cos φ sin θ cos ψ + sin φ sin ψ cos φ sin θ sin ψ − sin φ cos ψ cos φ cos θ . (3.28) It is then possible to use this to obtain ˙η, as seen in
˙ η = J(η2)υ (3.29) where, J(η2) = "b RTe 03x3 03x3 Jb(η2) # , (3.30)
3.1 First principles modeling 33 Jb(η2) = 1 0 0
sin φ tan θ cos φ sin φcos θ cos φ tan θ −sin φ cos φ
cos θ . (3.31)
To define the cross product, a matrix S, is defined so that x × y = S(x)y, where
S(λ) = 0 −λ3 λ2 λ3 0 −λ1 −λ2 λ1 0 , (3.32)
for any 3 × 1 or 1 × 3 vector λ.
The force and moment vector acting on the body is defined as τ. The motion of a body in a 3D-space can be expressed as
MBυ + C˙ B(υ)υ = τ, (3.33) where MB=" mI3x3 −mS(rG) mS(rG) I0 # , (3.34) and CB= " 03x3 −mS(υ1) − mS(υ2)S(rG) −mS(υ1) + mS(υ2)S(rG) −S(I0υ2) # . (3.35)
where I0 is the body’s inertia matrix and rG is the distance from the center of
gravity to the body fixed coordinate system.
To keep things simple at first, only those forces that give the biggest impact on the modeling will be estimated. Since all tests have been done in a pool, the effect of sea currents have been neglected.
τB = The body’s internal forces and moments, e.g. thrusters. This will be our
model control signal.
τRest = The restoring forces and moments generated from the weight and
buoy-ancy.
τAdd= The added mass forces and moments due to the inertia of the surrounding
water.
τDamp = The hydrodynamic drag and damping forces. This force is one of the
most important to obtain a good parameter estimation of. The Shark model states the restoring force as
τRest= τgrav(η) + τbouy(η) =
" b Remg S(rG)bRemg # − " b ReV ρg S(bReV ρg)rB # , (3.36)
where g is the gravity vector, V the volume of the ROV, rBthe center of buoyancy
and ρ the density of the water. Then the added mass force is calculated as
34 3 Modeling
and the damping force as
τDamp= Dq(|υ|)υ + Dlυ, (3.38)
where both Dqand Dl are a 6 × 6 dimensional matrix that needs to be estimated.
Combining these equations finally result in an expression for the derivate of the states in the body frame according to
˙
υ = (MB+ MA)
−1
(−CB(υ)υ − CA(υ)υ − Dq(|υ|)υ − Dlυ + τRest(η) + τB). (3.39)
Comparison between the current model and the complete Shark model
Due to the fact that there has been no time to investigate any Coriolis effects be-tween e.g. a thruster effecting pitch motion and its effect on a yaw motion, this has been neglected. So by assuming CB = 0 in the Shark model and letting rG
be placed at the center of gravity (rG =
0 0 0T), it can be seen that the cur-rent model is basically the same as the orientation part of the Shark model. This is encouraging, since it allows future projects to compare and make use of sim-ilar projects’ documentation and ideas. It also allows for matrix representation, which may reduce computation times in e.g. controllers or parameter estimation.
3.1.5
Future work
One major improvement that needs to be done is to upgrade the ROV-model to include and estimate a real world position. With the accelerometer and the current orientation model it is possible to dead reckon a position, but since there is no way to measure the current position the calculation error quickly grows. If it would be possible to measure a real world position, it would also be possible to create a complete model for both motions regarding position and orientation. During this thesis, position estimation has been looked into, although it is not yet implemented due to the lack of sensors.
3.2
Parameter estimation
To obtain and estimate each of the unknown parameters given by the equations in Chapter 3.1, a greybox model was used. To see if a first order model is sufficient to obtain a decent model estimation some comparisons were made with a blackbox model, namely a first order autoregressive with exogenous terms (ARX) model.
3.2.1
Black box modeling
A black box model means, a model where you only study the correlation between the input signal and the output signal, without caring about the underlying phys-ical process. According to Glad and Ljung [2004], this can either be expressed with a set of mathematical equations or more illustratively with a plot or a table. The main purpose for this black box model is to verify that the second order model in Section 3.1, is sufficient to estimate the model and that any higher model
3.2 Parameter estimation 35
orders are redundant. It is also used to prove that the statement in Equation (3.1), that the force generated from the thrusters is proportional to the input signal squared, holds.
In the black box tests, only an ARX model will be used. An ARX model can be stated according to
A(z)y(k) = B(z)u(k − nk) + e(k). (3.40)
In Figure 3.5, the basic concept of the ARX model is described.
Figure 3.5:A basic picture of an ARX system, where u is the input signal, e is the disturbance and y is the output.
There are three parameters that describe the order of the ARX model that has been estimated. nadefines the number of poles, nb defines the number of b
pa-rameters and nk describes the pure time delay of the system. For example, a
model order ofhna= 1 nb= 1 nk = 1
i
would generate
y(k) = b1u(k − 1) + e(k). (3.41)
Using the System Identification Toolbox (SITB) in Matlab, it is possible for a given input and output signal to estimate a variety of ARX- and ARMAX models. In Fig-ure 3.6 and 3.7 two different ARX models are presented and fed with a squared respectively non-squared input signal. Since the fit of the ARX models are very good for the squared input signal ( 74%) and since that fit is much better than for the non-squared input signal, the conclusion can be drawn that the transfer function from the voltage sent to the motors to the rotation of the ROV is more likely to be quadratic then linear.
36 3 Modeling
Figure 3.6: Two different black box models are estimated using a step as in-put. The input is the squared voltage sent to one of the horizontally aligned motors. The output is the estimated yaw rate. Since the increased model order does not improve the fit noticeably, the low order ARX should be suf-ficient.
The theory in Section 3.1 states that the pitch rate is dependent on a sine term. Since this knowledge should be taken taken into consideration, black box model-ing for the pitch rate model should be avoided. Due to this, all black box models have been done for the purpose of modeling the yaw rate. Their main purpose are to find and prove theories that exists in both pitch and yaw rate, but since the dynamics are simpler in the yaw rate it should prove more sufficient to study this case.
Using SITB the ARX models have been compared to a variety of ARMAX models. Since the fit was not noticeably improved, the ARMAX models are not further investigated.
3.2.2
Grey box modeling
A grey box model is a good tool for parameter estimation and model validation. The basic structure is illustrated in Figure 3.8. It takes a model, with specified constants and unknown parameters and then tries to estimate the unknown pa-rameters such that the estimated output signal ˆy is as close to the measured signal y as possible. This is a common way for estimating parameters since it allows the combining of physical insight with real world measurements.
If the model is y = Au2+ √
u − B, the grey box model will take this model, the input signal u and the output signal y and estimate A and B as well as possible.
3.2 Parameter estimation 37
Figure 3.7: Two different black box models are estimated using a step as input. The input is the voltage sent to one of the horizontally aligned motors. The output is the estimated yaw rate. When comparing the result to that of Figure 3.6, it is clear that the fit is worse.
One drawback is that the solver might get stuck in a local minimum without being able to make any further progress towards the optimal solution. To avoid this it is important to give the solver a good starting guess on the parameters that should be estimated. It is also important to constrain the parameters as much as possible, to minimize the number of local minima that the solver might get stuck into.
38 3 Modeling
Grey box model Input signals
Grey box structure
Measurements
Estimated unknowns
Figure 3.8:The basic structure of a grey box model.
3.3
Observer
The observer is a great tool to filter noise and obtain a good estimation of the true orientation of the ROV. For this thesis, the goal was to create an observer, that could handle the nonlinearities of the ROV but still have a low computation time. The first implementation was a regular Kalman filter just to obtain a decent, simple observer running early in the thesis. Later in project, it was decided to aim at implementing an extended Kalman filter (EKF).
Benefits with an observer:
• With a decent model it is possible to obtain a good noise filtering.
• Due to less measurement noise it is easier for the controller to maintain a stable system and be more aggressive.
• Cheaper than the alternative of measuring all the states. Drawbacks with an observer:
• Requires a model of the system.
• Might estimate the states wrong if not calibrated correctly.
3.3.1
Kalman filter
If a model is linear with white noise as disturbance, the Kalman filter is the best possible linear filter, [Gustafsson, 2010]. Even though the ROV dynamics is far from linear, it was still possible to linearize the model and create a Kalman filter with good performance.
Through tests in Matlab (see Section 3.2.1) it was concluded that the transfer function from the voltage sent to the motors ,u, to the generated rotation is ap-proximately squared. To capture this effect in the observer as well, the voltage u was squared with sign before given to the observer, according to Section 3.2. The model derived in Chapter 3 was linearized around the origin. This will cause some problems e.g. for large pitch angles and that is one off the reasons for
3.3 Observer 39
implementing an EKF.
Algorithm
To be able to use a Kalman filter, the continuous time model first needs to be lin-earized and discretized according to (3.22) and (3.20) in order to obtain a discrete model. With the model in
xk+1= Fkxk+ Gu,kuk+ Gv,kvk where Cov(vk) = Qk,
y = Hkxk+ Dkuk+ ek where Cov(ek) = Rk,
E(x0) = ˆx1|0,
Cov(x0) = P1|0. (3.42)
a linear Kalman filter is created by the recursion formula in Measurement update. ˆ xk|k= ˆxk|k−1+ Pk|k−1HkT(HkPk|k−1HkT + Rk) −1 (yk−Hkxˆk|k−1−Dkuk), Pk|k= Pk|k−1−Pk|k−1HkT(HkPk|k−1HkT + Rk) −1 HkPk|k−1, (3.43)
and Time update
ˆ
xk+1|k= Fkxˆk|k+ Gu,kuk,
Pk+1|k= FkPk|kFkT + Gv,kQkGTv,k, (3.44)
initialized with E(x0) = ˆx1|0and Cov(x0) = P1|0, [Gustafsson, 2010].
Implementation
With the help of the linear model, the Kalman filter parameters in (3.42) was estimated to the matrices seen in
A = 0 1 0 0 0 0 0 −Cr Ixx 0 0 0 0 0 0 1 0 0 0 0 0 0 −ICp yy 0 0 0 0 0 0 0 1 0 0 0 0 0 −ICy zz , (3.45a) B = 0 0 0 0 0 0 0 0 0 CT Ixx 0 0 0 0 0 Czr Iyy Czf Iyy 0 0 0 0 0 0 0 0 0 0 0 CIyr zz Cyf Izz 0 , (3.45b) F = I6+ A · T s, (3.45c)
40 3 Modeling
G = (I6· T s + 0.5 · A · T s2)B, (3.45d)
H = I6, (3.45e)
Gv= I6, (3.45f)
R = 100 · diag(0.1, 0.2, 0.1, 0.1, 1, 0.1, 1), Q = I6, (3.45g)
where the matrices in (3.45g) are chosen such that the Kalman filter estimates the states good enough and still filters out some of the measurement noise. It is important to note a few things about the choice of design parameters in (3.45g):
• During the configurations, the uncertainty in the roll model was so high that the certainty parameter R was tuned down.
• The integration of the IMU was not complete at DWT1 and therefore the IMU could not generate proper angle estimates, this forced lower values in the R matrix.
3.3.2
Extended Kalman filter
There are two standard ways of writing an EKF. Either do a first order Taylor ex-pansion (TT1) or a second order Taylor exex-pansion (TT2) on the nonlinear state equations. Usually it is sufficient with an EKF based on a TT1 (EKF1), but some-times the extra second order term from the TT2 is needed to improve the EKF (EKF2).
Algorithm
If there is a nonlinear system according to
xk+1= f (xk, uk, vk), (3.46a)
yk = h(xk, uk, ek). (3.46b)
Then the EKF2 algorithm will be according to Sk = Rk+ h 0 x( ˆxk|k−1)Pk|k−1(h 0 x( ˆxk|k−1))T −1 2htr(h 00 i( ˆxk|k−1)Pk|k−1h 00 j( ˆxk|k−1)Pk|k−1) i i,j, (3.47a) Kk = Pk|k−1(h 0 x( ˆxk|k−1))TS −1 k , (3.47b) k = yk−h( ˆxk|k−1) − 1 2 h tr(h00i( ˆxk|k−1)Pk|k−1) i i, (3.47c) ˆ xk|k= ˆxk|k−1+ Kkk, (3.47d) Pk|k= Pk|k−1−Pk|k−1(h 0 x( ˆxk|k−1))TS −1 k h 0 ( ˆxk|k−1)Pk|k−1, (3.47e) ˆ xk+1|k = f ( ˆxk|k) + 1 2 h tr(fi00Pk|k) i i, (3.47f) Pk+1|k = Qk+ f 0 x( ˆxk|k)Pk|k(f 0 x( ˆxk|k))T+ 1 2htr(f 00 i,x( ˆxk|k)Pk|kf 00 j,x( ˆxk|k)Pk|k) i ij, (3.47g)
3.3 Observer 41
where the EKF1 algorithm is obtained by letting fx00and h
00
xbe zero.
Implementation
With help of the model, the matrices in (3.48) are derived. The f ( ˆxk|k) is given by
inserting ˆx into Gv= I6, (3.48a) h = ˆx, (3.48b) fx= I6+ Ts 0 1 0 0 0 0 −zBFB Ixx cos( ˆx1) −Cr−2Cdr|xˆ2| Ir 0 0 0 0 0 0 0 1 0 0 0 0 −zBFB Iyy cos( ˆx3) −Cp−2Cdp|xˆ4| Iyy 0 0 0 0 0 0 0 1 0 0 0 0 0 −Cy−2CI dy|xˆ6| zz , (3.48c) R = 30I6 Q = I6. (3.48d)
3.3.3
Results
By taking a telegraph signal and comparing the measured data to the estimations from the different Kalman filters, the results could be used to view different ob-servers’ performance. Note that the Q and R matrices were chosen so that the Kalman filters would cancel out a decent amount of noise without making the estimation error too large.
The Kalman filter
In Figure 3.9a and 3.9b the linear Kalman filter is fed with the plotted data drawn against the estimated pitch and yaw rate.
EKF1
Figure 3.9 and Figure 3.10 displays the Kalman filter and the EKF1 fed with the same telegraph signal.
42 3 Modeling (a) 0 200 400 600 800 1000 1200 1400 1600 1800 2000 −0.5 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 Samples
Pitch rate (rad/s)
Viewing of the performance for the Kalman filter
Real data Kalman filter (b) 0 200 400 600 800 1000 1200 1400 1600 1800 2000 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 Samples
Yaw rate (rad/s)
Viewing of the performance for the Kalman filter
Real data Kalman filter
Figure 3.9: A Kalman filter used on a telegraph signal, showing estimated; (a) pitch rate and (b) yaw rate. Studying the plots, it is clear that there are more nonlinearities in the system than the Kalman filter can estimate. It is especially clear in (a), where the lack of the sine term greatly reduces the filter’s chance of estimating a proper behavior. Since the yaw rate can be estimated fairly well and the Kalman filter never overestimates the pitch rate against the measured data, the Kalman filter still would work as a state estimator for the ROV.
3.3 Observer 43 (a) 0 200 400 600 800 1000 1200 1400 1600 1800 2000 −0.5 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 Samples
Pitch rate (rad/s)
Viewing of the performance for the EKF1
Real data EKF1 (b) 0 200 400 600 800 1000 1200 1400 1600 1800 2000 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 Samples
Yaw rate (rad/s)
Viewing of the performance for the EKF1
Real data EKF1
Figure 3.10: EKF1 used on a telegraph signal; (a) pitch rate and (b) yaw rate. Comparing to the Kalman filter, it is clear that the EKF1 handles the nonlinearities in of the ROV a lot better then the Kalman filter, as expected. It basically gives a smaller estimation error with no worse effects for the noise filtration. This is best seen when watching the steps in the data. The EKF1 follows the peaks much better than the Kalman filter.
44 3 Modeling
EKF2
Due to the satisfying results of the EKF1 and that a lot of focus was spent on other, higher priority tasks, the EKF2 was discarded. This is mainly because the implementation and configuration in Matlab never got good enough to compete with the EKF1 and therefore we took the decision to focus on other tasks at hand.
3.4
Conclusion
In this section, the conclusions that was made in Chapter 3 will be summarized.
3.4.1
Parameter estimation
By comparing Figure 3.6 and 3.7, it is possible to see that a squared input signal helps in estimating an ARX model, which indicates that it is better to estimate a model with the input signal squared than with the input signal left unchanged. To summarize the nonlinear greybox modeling tool integrated in Matlab the fol-lowing observations where made:
Strengths
• May take highly nonlinear models. • Easy to integrate and test model changes. Drawbacks
• High computation time for large data sets. • May get stuck in local minimums.
3.4.2
Observer
By comparing the results of the Kalman filter and the EKFs, it is possible to ob-serve that the EKF1 has better state estimates and a better performance in general than the Kalman filter. This was expected, since it is widely known that an EKF can handle nonlinearities better than a Kalman filter. The decision was therefore to use the EKF1 as a state estimator in the software. It should; provide good enough state estimations, have a low computation time and have the possibility for easy configurations for coming projects.
The EKF2 was mainly discarded due to the good results of the EKF1. If future projects wants to create a more exact observer it is possible to configure the EKF2 for this purpose. Future projects might discover a more nonlinear model than the current model stated in Section 3.1. If so, then the EKF2 is the natural choice for upgrading the observer.
4
Controller
In order to be able to steer the system as desired, a stabilizing controller needs to be implemented. The controller is based on the results in Chapter 3. For this the-sis, three different controllers were developed; a linear quadratic (LQ) controller, a linear model predictive controller (MPC) and a nonlinear model predictive con-troller (NMPC). The LQ concon-troller is mainly created as a proof of concept and as something to compare the MPC with.
The main tool used to control the ROV is the IMU (see Section 2.3.1). This com-bined with the placement of the motors gives the following control problem:
• Control the ROV in pitch and yaw angle and angle rate, with help of the angle and angle rate estimations from the EKF.
• Since it is rather straight forward to integrate the roll model into the MPC, it is possible to add a constraint on the roll angle, telling the MPC never to use the back thruster more than that the absolute roll angle stays under a certain |φ| ≤ φmax. Due to technical issues with the MPC and the
mechani-cal parts this was not implemented during the time of the project.
The lack of sensors that can be used to estimate the absolute position, e.g. a sonar, makes it to hard to model the ROV in surge, sway and heave. Therefore, there have been no attempt of creating a controller that is controlling these three states.
4.1
Linear quadratic controller
A linear quadratic controller calculates an optimal feedback from the states by solving a linear optimal control problem. It is a very simple controller that can
46 4 Controller
be used on most systems thanks to the existence of reliable numerical software for solving the algebraic Riccati Equations, [Glad and Ljung, 2008].
To create an LQ controller, the first thing needed is a linear state space model. The linearization and discretization of the model were done according to Chapter 3 in Equation (3.22) and (3.20).
An LQ controller tries to solve a linear optimal control problem. With the help of Svensson [2004] it is possible to state the problem as
min( kxk2Q1+ kuk2Q2) = X xTQ1x + uTQ2u, (4.1) subject to x(k + 1) =Fx(k) + Gu(k), y(k) =Cx(k) + Du(k),
where x are the current state variables, u the control signals and y the measure-ments. The LQ algorithm generates a controller according to
u(k) = −Lx(k) + l0r(k), (4.2)
where r is the reference trajectory and l0is the prefiltering of the reference signal.
According to Glad and Ljung [2003] it is possible to calculate this l0as
l0= [C(BL − A)
−1
B]−1. (4.3)
4.2
Model predictive control
MPC is one of the few advanced modern control strategies that has had a break-through in the industry the last few decades. The reason for this is that it is one of the few controllers that explicitly can take care of constraints on states and inputs, [Enqvist et al., 2010].
For this project, two MPCs where developed; one linear MPC using qpOASES in Matlab and one nonlinear MPC using ACADO in ROS. The linear one serves as a backup and as a test controller, used to try different theories and configurations. It is also possible to, in case there is too much problem with ACADO, to add the linear MPC in ROS. This is fairly straightforward since the setup for qpOASES is basically the same in both Matlab and ROS.
4.2.1
Linear MPC
A linear MPC is by far the most common way to solve an MPC problem. It basi-cally does the same thing as an LQ controller, but instead of just solving it in one time step it uses the model to predict the outcome of the current movements and
4.2 Model predictive control 47
control signals in a number of time steps ahead of itself. A normal rule of thumb is that the length of the time step horizon should cover all the normal transients. With the help of this and the constraints, an optimal control signal u can be cal-culated for a linear system. To solve this, the standard way is to use the fact that a quadratic optimization problem have been solved many times before and that there are advanced computation algorithms for this. There are two main ways of formulating an MPC problem, either using a sparse or a dense formulation. What will be used here is the so called dense formulation.
Theory
To create a linear model, the system equations firstly need to be written on a discrete state space model according to
x(k + 1) = Fx(k) + Gu(k), (4.4a)
y(k) = Cx(k), (4.4b)
z(k) = Mx(k), (4.4c)
where the equations can be generated through the linearization and discretiza-tion equadiscretiza-tions (3.20) and (3.22).
By iterating this model over several time steps, for example as in
x(k + 2) = Fx(k + 1) + Gu(k + 1) = F2x(k) + FGu(k) + Gu(k + 1), (4.5) it is possible to predict how the system will move in the future. This can be used by the optimization algorithm to find an optimal u for the current time step. By using the result in (4.5) it is then possible to obtain a matrix notation according to X = Sxx(k) + SuU , (4.6) where Sx= I F .. . FN , Su = 0 0 0 . . . 0 G 0 0 . . . 0 FG 0 0 . . . 0 .. . . .. . .. ... ... FN −1G FN −2G . . . FG G , (4.7)
where N is the prediction horizon.
By then using the result in (4.7) it is possible to formulate the MPC problem stated in
48 4 Controller N −1 X j=0 kx(k + j)k2Q 1+ ku(k + j)k 2 Q2= X TQ 1X + UTQ2U =(Sxx(k) + SuU )TQ1(Sxx(k) + SuU ) + UTQ2U . (4.8)
The goal is then to reformulate (4.8) into the form of a QP
min w 1 2w TH w + fTw (4.9) subject to Aw ≤ b,
using the fact that the states can be expressed as input signals. Then the MPC problem can be formulated as
min u 1 2u T(ST uMTQ1MSu + Q2)u + (SuTMTQ1(MSxx − r))Tu subject to Au ≤ b, (4.10)
where r is the reference signal from the Xbox-joypad, x are the current states estimated by the observer and M is the matrix that decides what states to control. As seen in this section, the MPC controller depends a lot on the model and is therefore sensitive to errors in the model.
4.2.2
Nonlinear MPC
There are several ways to solve a nonlinear MPC problem and in this section, a short note will be made on how ACADO [ACA, 2012] specifies the problem. ACADO solves the continuous time optimal control problem
min x(·),u(·),p t0+T Z t0 kh(t, x(t), u(t), p) − η(t)k2 Qdt + km(x(t0+ T ), p, t0+ T ) − µk2P, subject to : ∀t ∈ht0, t0+ Ti: 0 = f (t, x(t), ˙x(t), u(t), p), ∀t ∈ht0, t0+ Ti: 0 ≥ s(t, x(t), u(t), p), x(t0) = x0, 0 = r(x(t0+ T ), p, t0+ T ), (4.11)
where f ( · ) is the model equations, s the path constraints and r the terminal con-straints. Instead of using the normal ACADO problem solver to solve the
prob-4.2 Model predictive control 49
lem, a toolbox named Code generation has been used. It basically lets you spec-ify the problem according the 4.11 and generates code that solves the problem, given a measurement and state reference. This has some promising benefits for the project:
• It is easy to specify a nonlinear problem and constraints on the system. The Code generation toolbox handles the more error prone parts internally. • Automatically defines and creates an optimized NMPC problem for the
equations defined in Code generation file, which gives lower computation time.
• No need to include the entire ACADO library to the software. Only the generated files are needed.
There are a lot of different options in ACADO to solve this problem. For more information about this see ACA [2012].
5
Implementation
This chapter will describe the way we chose to implement the software oriented parts of the thesis. This includes the controllers, operating system, basic struc-ture of the software and how the ROV is controlled with an Xbox-joypad. Firstly we describe the way we made important decisions in this thesis.
5.1
Decisions
Since this thesis is a part of a chain of projects, all major decisions which affect the whole project should be studied carefully before making such decisions. While a certain choice might be optimal for this thesis alone, the best choice for the entire project might be something completely different. To help making these kinds of decisions, a decision matrix is used.
In this section, two of the most important choices which we have made will be de-scribed and motivated. The first and most important choice, was which platform that should be used. In previous projects the software was run on a Windows computer with a Matlab program located on the ROV. The user used the remote desktop function to control the on-board computer and sent video stream and control signals to the master on land. One could easily see that this system prob-ably is not optimal in terms of performance and alternatives where investigated. The second choice was regarding how to implement the advanced controller. It can be time consuming and error prone to try to structure the MPC by hand. For that reason two frameworks that can help with the structuring of the problem were considered; ACADO, [ACA, 2012] and CVXGEN, [CVX, 2012].
