- Email: [email protected]
Altitude Estimation and Control of ROV by use of DVL
9th IFAC Conference on Manoeuvring and Control of Marine Craft, 2012 The International Federation of Automatic Control September 19-21, 2012. Arenzano, Italy
Altitude Estimation and Control of ROV by use of DVL Fredrik Dukan ∗ Asgeir J. Sørensen ∗∗ ∗ Norwegian University of Science and Technology (NTNU), Department of Marine Technology and Centre for Ships and Ocean Structures (CeSOS), Norway (e-mail: [email protected]). ∗∗ Norwegian University of Science and Technology (NTNU), Department of Marine Technology and Centre for Ships and Ocean Structures (CeSOS), Norway (e-mail: [email protected]).
Abstract: A new method for altitude estimation and control of a remotely operated vehicle (ROV) by use of a Doppler velocity log (DVL) is presented in this paper. Altitude measurements from the DVL are used to approximate the gradient of the sea floor. The altitude rate of change is found from the sea floor gradient and ROV motions. This is used in an observer for altitude estimation. A guidance law for altitude control is also proposed. The altitude observer and guidance law have been implemented in the control system of NTNU’s ROV Minerva. Results from sea trials demonstrating the performance of the proposed altitude estimation and controller are presented in the paper. Keywords: ROV, DVL, Altitude estimation, Terrain following, Bottom tracking, Guidance, Control, Underwater vehicles 1. INTRODUCTION A new method for altitude estimation and control in an unstructured and unknown environment is proposed in this paper. Many altitude sensors, including the DVL, are based on acoustics where the measurement noise can be significant. The altitude rate of change depends on the ROV motion and the shape of the sea floor. This splitting of the problem is exploited throughout the paper. Measurements of the ROV states such as North and East positions, attitude and depth may be done by an acoustic positioning reference system, an inertial measurement unit and a pressure sensor. Good state estimates are achieved by utilizing a dynamical model of the ROV in a vehicle observer. The other half of the problem is related to the shape of the sea floor. If the gradient of the sea floor directly below the ROV can be approximated from several altitude measurements, this can be combined with the estimated ROV motions to approximate the altitude rate of change. The noisy altitude and altitude rate approximations are input to an observer for altitude estimation. The proposed gradient based altitude estimation is inspired from Caccia et al. (2003b). Other work on altitude control for ROVs can be found in e.g. Caccia et al. (2003a), Caccia et al. (1999) and for AUVs in Silvestre et al. (2009) and Yoerger et al. (2000). The low altitude control problem is described in Maki et al. (2007) and use of DVL for terrain navigation can be found in Aandnonsen and Hagen (2010).
Fig. 1. Minerva at the sea floor for opening of the Applied Underwater Robotics Laboratory (AUR-Lab).
desired altitude is achieved by generating a depth reference to the depth controller. Full scale experiments of the altitude estimation and control strategy presented in this paper were conducted with NTNU’s ROV Minerva, which is seen in Figure 1. The ROV control system is presented in Dukan et al. (2011). An expression relating the sea floor gradient and the ROV motions to altitude rate of change is derived in Section 2. An approximation of the sea floor gradient and the ROV altitude by use of a DVL is given in Section 3. The observer for altitude estimation is given in Section 4. Section 5 is on guidance laws for altitude control via depth control. A conclusion is given in Section 6.
Depth measurements are more reliable than altitude measurements, Yoerger et al. (2007), and depth is a state of the ROV. The altitude is not a state of the ROV, but a property that can be measured from it. Therefore, the ©2012 IFAC
79
10.3182/20120919-3-IT-2046.00014
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
N
# " # xp xp yp a = ra − p = − yp f (xp , yp ) zp # " 0 0 , = f (xp , yp ) − zp "
E p D ra
where r a is the NED position of the point on the sea floor below the CO of the ROV. According to the proposed Definition 1, the altitude is the length of (3) and is written as a = |a| = f (xp , yp ) − zp = a(xp , yp , zp ) = a(p). (4)
p˙ a
F (x, y, z) = 0
(3)
It is seen from (4) that the altitude is completely given by the ROV position, p. Assuming that the ROV can not go through the sea floor, zp ≤ f (xp , yp ), the altitude a is always positive. Note that the function f in (4) is the same as in (2). These facts are used in finding an expression for the altitude rate of change based on (4). Fig. 2. ROV above the sea floor in NED frame.
2.2 Altitude Rate of Change
2. ALTITUDE AND SEA FLOOR GRADIENT
The altitude rate of change is given in Proposition 2 and is valid for motions in 6 degrees of freedom (DOFs). Proposition 2. a˙ = ∇F (p) · p, ˙ (5) where ∇F (p) = ∂f , ∂f , −1 is the gradient ∂x ∂y
Figure 2 shows the ROV above the sea floor. The ROV position in the earth-fixed North, East and Down (NED) reference frame is p = [xp yp zp ]T . The NED velocity vector is p˙ and can be expressed as
xp ,yp
p˙ =
Rnb (Θnb )v,
(1)
where Θnb = [φ θ ψ]T is the attitude vector, Rnb (Θnb ) is the transformation matrix from body to NED frame and v = [u v w]T is the velocity vector in body frame, Fossen (2011).
Proof. An expression for the altitude rate of change a˙ is given in (6) using partial derivatives of a and the chain rule. a˙ = Dt a(p(t)) = Dt [a (xp (t), yp (t), zp (t))] ∂a dxp ∂a dyp ∂a dzp = + + . (6) · · · ∂xp dt ∂yp dt ∂zp dt
The sea floor can be expressed as the surface given by (2).
F (x, y, z) = f (x, y) − z = 0,
∂F = 0, ∂t
xp ,yp
vector of (2) evaluated at the horizontal position of the ROV, (xp , yp ). Eq. (5) is a new result of this paper and the proof is given in the following.
(2)
By using (4) in (6), the altitude rate of change in (6) becomes ∂f dyp dzp ∂f dxp + − . (7) a˙ = · · ∂xp dt ∂yp dt dt
where F is a time invariant function with continuous first-order partial derivatives. That the sea floor is time invariant is normally satisfied unless there are underwater avalanches or volcanic activity etc. The second assumption will be violated if there are sharp edges, i.e. underwater rocks and cliffs. The effects of this violation on the altitude estimation is presented in section 3.3. F is an unknown function, but only the structure of F is important to be known for the method of altitude estimation in this paper.
According to Definition 1, x = xp and y = yp in the ∂f function f in (2) and (4). Then ∂x and = ∂f ∂x p xp ,yp ∂f ∂f , indicating that the partial derivatives are ∂yp = ∂y xp ,yp
evaluated at x = xp , y = yp . Using this, the altitude rate of change in (7) becomes " x˙ p # ∂f ∂f , , −1 · y˙ p a˙ = ∂x xp ,yp ∂y xp ,yp z˙
2.1 Altitude
p
= ∇F (p) · p, ˙
(8) i ∂f where it was used that ∇F = ∂f if F = ∂x , ∂y , −1 F (x, y, x) = f (x, y) − z = 0, Edwards and Penney (1997). This completes the proof of (5).
The altitude of the ROV is given by the following definition. Definition 1. The altitude is the length of the vector from the center of origin (CO) of the ROV to the point on the sea floor with the same horizontal coordinates as the CO.
h
Eq. (5) written out as in (9) shows how the altitude rate of change is related to the ROV attitude and velocity vector and to the sea floor from the gradient vector.
In this paper the altitude vector is expressed as
80
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
ROV
DVL
x DVL
y
rbDV L
x
γi z
z
rb,ROV i
L rb,DV i
L rb,DV i L aDV i
Fig. 4. DVL vectors to ROV frame.
βi
L r b,ROV = R(ΘDV L )r b,DV + r bDV L i i r ni = Rnb (Θnb )r b,ROV , i
where ΘDV L is the orientation of the DVL body frame relative to the ROV body frame. r bDV L is the vector from the CO of the ROV to the center of the DVL.
Fig. 3. DVL ith beam vector components. " # u ∂f ∂f , , −1 Rnb (Θnb ) v , a˙ = ∂x xp ,yp ∂y xp ,yp w
3.2 Sea Floor Normal Vector Approximation
(9)
The DVL beams expressed in the NED frame, r ni , are used to approximate the normal vector of the sea floor under the ROV, see Figure 5. At least 3 beams are needed to get an approximation of the normal vector. With 4 beams, better approximations may be obtained due to redundancy by applying some weighting techniques using e.g. minimum variance. Only the linear approximation is described in this paper. The validity of the linear assumption can be checked by testing if the 4 beam vectors have end points in approximately the same plane.
where Θnb and v are measured or estimated from a vehicle ˆ nb and v observer as Θ ˆ . The gradient vector, ∇F (p) = ∂f , ∂f , −1 is approximated from the DVL ∂x ∂y xp ,yp
xp ,yp
altitude measurements as shown in the next section. 3. DVL MEASUREMENTS
The normal vectors are given by (12). With 4 beams, the beams can be combined in 4 ways to get 4 approximations of the normal vector. If these vectors are different, the sea floor under the ROV is nonlinear. As the DVL beam angles are constant, the footprint of the DVL will increase with altitude, see Figure 6. The linear approximation of the normal vector may be good for low altitudes, even for a curved sea floor. At high altitudes, the linear approximation may be poor if the sea floor is nonlinear.
The DVL is used to measure velocity through water or over the sea floor and altitude. This section shows to use the DVL altitude measurements to approximate the ROV altitude and the sea floor normal (gradient) vector. 3.1 DVL Beams and Kinematics A typical DVL has 4 beams. The ith DVL beam with parameters is shown in Figure 3. The ith beam is represented L by the vector r b,DV (10) and goes from the DVL to the i sea floor. The superscript b,DV L is to tell that the vector is expressed in the body fixed frame of the DVL.
nni
" # L xDV tan(γi )cos(βi ) i DV L tan(γi )sin(βi ) , (10) = yiDV L = ai L 1 aDV i
L r b,DV i
(11a) (11b)
(r ni − r ni+1 ) × (r n4 − r ni+1 ) if i = 1, = (r ni − r ni+1 ) × (r ni−1 − r ni+1 ) if i = 2, 3 (r ni − r n1 ) × (r ni−1 − r n1 ) if i = 4.
(12)
E.g. nn1 is the cross product of the two vectors seen in Figure 6. 3.3 ROV Altitude Approximation from DVL Measurements
where γi is the angle of the ith beam from the DVL zL axis, and βi is the angle from the x-axis. aDV is the i b,DV L b,DV L vertical component of r i . ri is expressed in terms L L of aDV and the constant angles βi and γi because aDV i i is an output measurement from the DVL.
The normal plane to the approximated gradient vector is used to find an approximation of the ROV altitude, aCO i . The elements of the ith gradient vector and DVL beam vector are
L r b,DV is transformed to the ROV body frame in (11a) i and to NED frame in (11b). The first transformation is illustrated in Figure 4.
nni,1 nni = nni,2 , nni,3
81
" r ni
=
# xni n yi . ani
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
ROV
DVL Altitudes 18
CO
x z
14 12
150
200
250
300
t [s] ROV Altitudes 18
rni+1
aCO 1
aCO 2
aCO 3
aCO 4
16 a [m]
−
L aDV 4
8
aCO i nni
rni
L aDV 3
10
rni
rni+1
L aDV 2
16 a [m]
DVL
L aDV 1
14 12 10
Fig. 5. Sea floor normal vectors.
8 150
x
200
250
300
t [s]
Fig. 7. Altitude approximation from DVL.
4
From the tangent plane equation (13b), it is seen that the components of the ith gradient vector are as given by (14a). The ith approximated gradient vector is given by (14b). If an altimeter is available, this measurement may be used in the observer instead of the approximated altitude from DVL measurements. However, then the filter effect of the approximated altitude can not be exploited.
1 y rn1
−
rn2
rn4 − rn2
ˆ ˆ nni,1 nni,2 ∂f ∂f =− n , =− n ∂x i n ∂y i n " i,3 # i,3 ˆ ˆ ˆ = ∂f , ∂f , −1 ∇F ∂x i ∂y i i
2 Fig. 6. DVL footprint.
(14a) (14b)
The approximated gradient vector is used in the altitude estimation and in the guidance law.
The plane equations, in the NED frame with origin in the CO of the ROV, are given by (13a) and solved for z to give (13b). The approximated altitude of the ROV is given by (13c).
3.4 Altitude and Rate Approximation The inputs to the altitude observer are the approximated altitude, aCO ˙ CO i , and altitude rate of change, a i , which were derived in section 3.3 from the DVL altitude measurements. The observer inputs will be noted as am and a˙ m and are given by (15a) and (15b).
nni,1 (x − xni ) + nni,2 (y − yin ) + nni,3 (z − ani ) = 0 (13a) nni,1 nni,2 ⇒ z = ani − n (x − xni ) − n (y − yin ) (13b) ni,3 ni,3 nni,1 n nni,2 n n ⇒ aCO = a + x + n yi . (13c) i i nni,3 i ni,3
ˆ ˆ ∂f ∂f am = aCO = ani − (15a) xni − yn i ∂x i ∂y i i " # " # u ˆ ˆ ∂f ˆ ∂f n ˆ v ˆ (15b) , , −1 R ( Θ ) a˙ m = a˙ CO = nb i b ∂x i ∂y i w ˆ
Note that the sea floor, as seen from the ROV, with the approximation in (13c) is z = zp + aCO i . This implies that the approximation method is also a filter (imagine the DVL beams as the ROV goes off a cliff at constant depth). The filtering effect of the sea floor is desirable when the estimated altitude is used in a guidance law for altitude control as the ROV has limited bandwidth in heave.
Note that the expression in (15b) is strictly only valid for a sea floor with continuous first-order partial derivatives. However, for a rugged sea floor, where the assumption of continuous first-order partial derivatives is violated, the approximated altitude and sea floor gradient vector is still usable for altitude estimation and in a guidance law for altitude control.
Experimental result 1. An example result of equation (13c) for i = 1, 2, 3, 4, is seen in the bottom plot of Figure 7 and L is based on the DVL altitudes, aDV , in the top plot. The i example is from a test with ROV Minerva moving over the sea floor at constant depth. The approximation aCO 1 is based on DVL beams no. 1, 2 and 4. aCO is based on 2 DVL beams no. 1, 2 and 3. aCO is based on DVL beams 3 no. 2, 3 and 4. aCO is based on DVL beams no. 3, 4 and 4 1. It is seen that the 4 different approximations are almost the same.
4. ALTITUDE ESTIMATION The measurement equations for the altitude observer is given as 82
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
am (k) = a(k) + v(k) a˙ m (k) = a(k) ˙ + w(k),
ROV Trace 25
where w(k) and v(k) are noise processes. 20 North Position [m]
The discrete time control plant model (CPM) equations are given as a(k + 1) = a(k) + a(k)h ˙ = a(k) + a˙ m (k)h − w(k)h y(k) = am (k) = a(k) + v(k)
15
10
5
where h is the time step.
0
The CPM equations are reformulated to standard notation as x(k + 1) = Φx(k) + ∆u(k) + Γw(k) (16a) y(k) = Hx(k) + v(k), (16b)
−30
−25
−20
−15 −10 East Position [m]
−5
0
Fig. 8. Lawnmower pattern. " # ˆ ∂f ˆ ∂f ˆ ˆ u wd = , Rnb (ψ) − a˙ d vˆ ∂x ∂y Z t + kp (a − ad ) + ki (a − ad ) dτ 0 Z t zd = wd dτ + zic ,
where x = am , u = a˙ m , Φ = 1, ∆ = h, Γ = −h, H = 1. An observer defined in terms of (16a) and (16b) should be used to estimate the altitude. 5. GUIDANCE LAW FOR ALTITUDE CONTROL VIA DEPTH CONTROL
5
(19a) (19b)
0
Depth measurements are more reliable than altitude measurements. As depth is a state the ROV, altitude control should be obtained from a depth reference to a depth controller. This choice is also a stability concern as the depth of the ROV is always continuous but the altitude can be discontinuous and may change by several hundred meters from one time step to another if the ROV goes off a shelf. Therefore the altitude should not be input to a feedback controller. A guidance law for depth reference is presented in this section.
where ad is the desired altitude, a˙ d is the desired altitude rate and zic is the initial depth when altitude control is activated. For cases including roll and pitch, the desired heave velocity must be transformed to NED before (19b) is applied. The proportional and integral feedback terms in wd (19a) is from Caccia et al. (2003b) and the feedforward term is a new result of this paper. The feedback terms are needed to compensate for inaccuracies in the sea floor gradient estimation and vehicle state estimates. The feedforward term is very important as it assures a sufficient desired depth velocity for any horizontal velocity, heading and steepness of the sea floor. Without the feedforward term, the ROV can hit the sea floor while going uphill or overshoot downhill. This method of altitude control is well suited for ROVs in joystick operation as it works for any heading and speed direction.
The relationship between the ROV motions, sea floor gradient and altitude rate of change was given in (9). This can be exploited in a feedforward term in the guidance law by setting (9) equal to the desired altitude rate of change as seen in (17). The desired depth velocity, wd,f f , is found by solving (17) for w.
It should be noted that if the ROV moves fast over a steep or rugged sea floor with altitude control, the ROV may not be able to follow the desired depth generated by (19b). In that case a feedback to the horizontal speed guidance law will assure that the desired depth reference is within the ROV capabilities.
Rnb (Θnb )v
a˙ = ∇F (p) · " # u ∂f ∂f ! n = , , −1 Rb (Θnb ) v = a˙ d (17) ∂x xp ,yp ∂y xp ,yp w The desired depth velocity in the special case for zero roll and pitch is
Experimental result 2. Results from a test with altitude control according to the guidance law in (19a) and (19b) are shown in Figures 8, 9 and 10. The test was conducted in the Trondheimsfjord with the ROV Minerva. The task is to track a lawnmower pattern while maintaining a constant altitude of 5 m. The horizontal ROV trace and snapshots of the ROV outline is seen in Figure 8. The ROV moves in surge with 0.3 m/s while heading towards the next way point. The top plot in Figure 9 shows the desired, measured and estimated altitude and the bottom plot shows the desired depth as generated by (19b) and the estimated depth from the vehicle observer. In this case, measurements from an altimeter is used as input to
wd,f f
∂f ∂f u n = , Rb (ψ) − a˙ d . (18) v ∂x xp ,yp ∂y xp ,yp
When roll and pitch are included, the expression for wd,f f gets more complex. Eq. (18) may be used for a ROV which has only small deviations from zero roll and pitch. This leads to a new guidance law in (19a) and (19b) for desired depth.
83
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
of the sea floor gradient and ROV altitude with the vehicle observer, was presented. Finally, a guidance law for depth reference was given. The new contribution was a feedforward term based on the expression for altitude rate of change. This assures proper depth reference for any horizontal motion and sea floor steepness during altitude control. The presented methods have been successfully implemented and tested on the ROV Minerva.
ROV Altitude
6
a ˆCO avg
am
aCO d a [m]
5.5 5 4.5
0
500
1000
1500
t [s] ROV Depth
ACKNOWLEDGEMENTS
86 z [m]
84
This work has also been carried out at the Centre for Ships and Ocean Structures (CeSOS) and the Applied Underwater Robotics Laboratory (AUR-Lab). The Norwegian Research Council is acknowledged as the main sponsor of CeSOS.
82 80 78
zd 0
500
zˆ
1000
1500
t [s]
REFERENCES
Fig. 9. Guidance law; altitude estimation and depth reference.
Aandnonsen, K. and Hagen, O. (2010). An analysis of real-time terrain aided navigation results from a hugin auv. In OCEANS 2010, 1 –9. Caccia, M., Bono, R., Bruzzone, G., and Veruggio, G. (2003a). Bottom-following for remotely operated vehicles. Control Engineering Practice, 11(4), 461 – 470. Caccia, M., Bruzzone, G., and Veruggio, G. (1999). Active sonar-based bottom-following for unmanned underwater vehicles. Control Engineering Practice, 7(4), 459 – 468. Caccia, M., Bruzzone, G., and Veruggio, G. (2003b). Bottom-following for remotely operated vehicles: Algorithms and experiments. Autonomous Robots, 14, 17–32. Dukan, F., Ludvigsen, M., and Sørensen, A.J. (2011). Dynamic positioning system for a small size rov with experimental results. In OCEANS, 2011 IEEE - Spain, 1 –10. Edwards, C. and Penney, E. (1997). Edwards & Penney fifth edition Calculus with Analytical Geometry. Prentice Hall. Fossen, T.I. (2011). Handbook of Marine Craft Hydrodynamics and Motion Control. John Wiley and Sons Ltd. Maki, T., Ura, T., Mizushima, H., Kondo, H., Sakamaki, T., and Yanagisawa, M. (2007). Low altitude tracking of rugged seafloors for autonomous visual observation. In Underwater Technology and Workshop on Scientific Use of Submarine Cables and Related Technologies, 2007. Symposium on, 488 –494. Silvestre, C., Cunha, R., Paulino, N., and Pascoal, A. (2009). A bottom-following preview controller for autonomous underwater vehicles. Control Systems Technology, IEEE Transactions on, 17(2), 257 –266. Yoerger, D., Jakuba, M., Bradley, A., and Bingham, B. (2007). Techniques for deep sea near bottom survey using an autonomous underwater vehicle. In S. Thrun, R. Brooks, and H. Durrant-Whyte (eds.), Robotics Research, volume 28 of Springer Tracts in Advanced Robotics, 416–429. Springer Berlin / Heidelberg. Yoerger, D., Bradley, A., Walden, B., Cormier, M.H., and Ryan, W. (2000). Fine-scale seafloor survey in rugged deep-ocean terrain with an autonomous robot. In Robotics and Automation, 2000. Proceedings. ICRA ’00. IEEE International Conference on, volume 2, 1787 –1792 vol.2.
D [m]
−84 −86 −88 −90 0
25 −5
20 −10
15 −15
10 −20
E [m]
5 −25
0
N [m]
Fig. 10. Approximation of normal vectors of the sea floor under the ROV. the altitude observer. The estimated altitude from DVL measurements are seen as the blue line. The approximated gradient vectors (normal vectors) of the sea floor under the ROV from (14b) is seen in Figure 10. The tail of the gradient vector for each time step is placed in the ROV x and y position with z = zˆp + a ˆCO avg . This gives an approximation of the sea floor geometry. The parameters in the guidance law (19a) are set to kp = 0.2, ki = 0 in the test. The feedforward term wd,f f is used with sea floor gradient estimation from DVL beams no. 1, 2 and 4. 6. CONCLUSION A new method for altitude estimation was presented. The key point was to split the altitude problem into ROV motions and sea floor geometry. Because the altitude is given by the ROV position if the sea floor is time invariant, the altitude rate of change can be expressed from the sea floor gradient and ROV attitude and velocity vectors. A new method to approximate the sea floor gradient and ROV altitude from DVL altitude measurements was given. An altitude observer, which combines the approximations 84
Altitude Estimation and Control of ROV by use of DVL Fredrik Dukan ∗ Asgeir J. Sørensen ∗∗ ∗ Norwegian University of Science and Technology (NTNU), Department of Marine Technology and Centre for Ships and Ocean Structures (CeSOS), Norway (e-mail: [email protected]). ∗∗ Norwegian University of Science and Technology (NTNU), Department of Marine Technology and Centre for Ships and Ocean Structures (CeSOS), Norway (e-mail: [email protected]).
Abstract: A new method for altitude estimation and control of a remotely operated vehicle (ROV) by use of a Doppler velocity log (DVL) is presented in this paper. Altitude measurements from the DVL are used to approximate the gradient of the sea floor. The altitude rate of change is found from the sea floor gradient and ROV motions. This is used in an observer for altitude estimation. A guidance law for altitude control is also proposed. The altitude observer and guidance law have been implemented in the control system of NTNU’s ROV Minerva. Results from sea trials demonstrating the performance of the proposed altitude estimation and controller are presented in the paper. Keywords: ROV, DVL, Altitude estimation, Terrain following, Bottom tracking, Guidance, Control, Underwater vehicles 1. INTRODUCTION A new method for altitude estimation and control in an unstructured and unknown environment is proposed in this paper. Many altitude sensors, including the DVL, are based on acoustics where the measurement noise can be significant. The altitude rate of change depends on the ROV motion and the shape of the sea floor. This splitting of the problem is exploited throughout the paper. Measurements of the ROV states such as North and East positions, attitude and depth may be done by an acoustic positioning reference system, an inertial measurement unit and a pressure sensor. Good state estimates are achieved by utilizing a dynamical model of the ROV in a vehicle observer. The other half of the problem is related to the shape of the sea floor. If the gradient of the sea floor directly below the ROV can be approximated from several altitude measurements, this can be combined with the estimated ROV motions to approximate the altitude rate of change. The noisy altitude and altitude rate approximations are input to an observer for altitude estimation. The proposed gradient based altitude estimation is inspired from Caccia et al. (2003b). Other work on altitude control for ROVs can be found in e.g. Caccia et al. (2003a), Caccia et al. (1999) and for AUVs in Silvestre et al. (2009) and Yoerger et al. (2000). The low altitude control problem is described in Maki et al. (2007) and use of DVL for terrain navigation can be found in Aandnonsen and Hagen (2010).
Fig. 1. Minerva at the sea floor for opening of the Applied Underwater Robotics Laboratory (AUR-Lab).
desired altitude is achieved by generating a depth reference to the depth controller. Full scale experiments of the altitude estimation and control strategy presented in this paper were conducted with NTNU’s ROV Minerva, which is seen in Figure 1. The ROV control system is presented in Dukan et al. (2011). An expression relating the sea floor gradient and the ROV motions to altitude rate of change is derived in Section 2. An approximation of the sea floor gradient and the ROV altitude by use of a DVL is given in Section 3. The observer for altitude estimation is given in Section 4. Section 5 is on guidance laws for altitude control via depth control. A conclusion is given in Section 6.
Depth measurements are more reliable than altitude measurements, Yoerger et al. (2007), and depth is a state of the ROV. The altitude is not a state of the ROV, but a property that can be measured from it. Therefore, the ©2012 IFAC
79
10.3182/20120919-3-IT-2046.00014
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
N
# " # xp xp yp a = ra − p = − yp f (xp , yp ) zp # " 0 0 , = f (xp , yp ) − zp "
E p D ra
where r a is the NED position of the point on the sea floor below the CO of the ROV. According to the proposed Definition 1, the altitude is the length of (3) and is written as a = |a| = f (xp , yp ) − zp = a(xp , yp , zp ) = a(p). (4)
p˙ a
F (x, y, z) = 0
(3)
It is seen from (4) that the altitude is completely given by the ROV position, p. Assuming that the ROV can not go through the sea floor, zp ≤ f (xp , yp ), the altitude a is always positive. Note that the function f in (4) is the same as in (2). These facts are used in finding an expression for the altitude rate of change based on (4). Fig. 2. ROV above the sea floor in NED frame.
2.2 Altitude Rate of Change
2. ALTITUDE AND SEA FLOOR GRADIENT
The altitude rate of change is given in Proposition 2 and is valid for motions in 6 degrees of freedom (DOFs). Proposition 2. a˙ = ∇F (p) · p, ˙ (5) where ∇F (p) = ∂f , ∂f , −1 is the gradient ∂x ∂y
Figure 2 shows the ROV above the sea floor. The ROV position in the earth-fixed North, East and Down (NED) reference frame is p = [xp yp zp ]T . The NED velocity vector is p˙ and can be expressed as
xp ,yp
p˙ =
Rnb (Θnb )v,
(1)
where Θnb = [φ θ ψ]T is the attitude vector, Rnb (Θnb ) is the transformation matrix from body to NED frame and v = [u v w]T is the velocity vector in body frame, Fossen (2011).
Proof. An expression for the altitude rate of change a˙ is given in (6) using partial derivatives of a and the chain rule. a˙ = Dt a(p(t)) = Dt [a (xp (t), yp (t), zp (t))] ∂a dxp ∂a dyp ∂a dzp = + + . (6) · · · ∂xp dt ∂yp dt ∂zp dt
The sea floor can be expressed as the surface given by (2).
F (x, y, z) = f (x, y) − z = 0,
∂F = 0, ∂t
xp ,yp
vector of (2) evaluated at the horizontal position of the ROV, (xp , yp ). Eq. (5) is a new result of this paper and the proof is given in the following.
(2)
By using (4) in (6), the altitude rate of change in (6) becomes ∂f dyp dzp ∂f dxp + − . (7) a˙ = · · ∂xp dt ∂yp dt dt
where F is a time invariant function with continuous first-order partial derivatives. That the sea floor is time invariant is normally satisfied unless there are underwater avalanches or volcanic activity etc. The second assumption will be violated if there are sharp edges, i.e. underwater rocks and cliffs. The effects of this violation on the altitude estimation is presented in section 3.3. F is an unknown function, but only the structure of F is important to be known for the method of altitude estimation in this paper.
According to Definition 1, x = xp and y = yp in the ∂f function f in (2) and (4). Then ∂x and = ∂f ∂x p xp ,yp ∂f ∂f , indicating that the partial derivatives are ∂yp = ∂y xp ,yp
evaluated at x = xp , y = yp . Using this, the altitude rate of change in (7) becomes " x˙ p # ∂f ∂f , , −1 · y˙ p a˙ = ∂x xp ,yp ∂y xp ,yp z˙
2.1 Altitude
p
= ∇F (p) · p, ˙
(8) i ∂f where it was used that ∇F = ∂f if F = ∂x , ∂y , −1 F (x, y, x) = f (x, y) − z = 0, Edwards and Penney (1997). This completes the proof of (5).
The altitude of the ROV is given by the following definition. Definition 1. The altitude is the length of the vector from the center of origin (CO) of the ROV to the point on the sea floor with the same horizontal coordinates as the CO.
h
Eq. (5) written out as in (9) shows how the altitude rate of change is related to the ROV attitude and velocity vector and to the sea floor from the gradient vector.
In this paper the altitude vector is expressed as
80
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
ROV
DVL
x DVL
y
rbDV L
x
γi z
z
rb,ROV i
L rb,DV i
L rb,DV i L aDV i
Fig. 4. DVL vectors to ROV frame.
βi
L r b,ROV = R(ΘDV L )r b,DV + r bDV L i i r ni = Rnb (Θnb )r b,ROV , i
where ΘDV L is the orientation of the DVL body frame relative to the ROV body frame. r bDV L is the vector from the CO of the ROV to the center of the DVL.
Fig. 3. DVL ith beam vector components. " # u ∂f ∂f , , −1 Rnb (Θnb ) v , a˙ = ∂x xp ,yp ∂y xp ,yp w
3.2 Sea Floor Normal Vector Approximation
(9)
The DVL beams expressed in the NED frame, r ni , are used to approximate the normal vector of the sea floor under the ROV, see Figure 5. At least 3 beams are needed to get an approximation of the normal vector. With 4 beams, better approximations may be obtained due to redundancy by applying some weighting techniques using e.g. minimum variance. Only the linear approximation is described in this paper. The validity of the linear assumption can be checked by testing if the 4 beam vectors have end points in approximately the same plane.
where Θnb and v are measured or estimated from a vehicle ˆ nb and v observer as Θ ˆ . The gradient vector, ∇F (p) = ∂f , ∂f , −1 is approximated from the DVL ∂x ∂y xp ,yp
xp ,yp
altitude measurements as shown in the next section. 3. DVL MEASUREMENTS
The normal vectors are given by (12). With 4 beams, the beams can be combined in 4 ways to get 4 approximations of the normal vector. If these vectors are different, the sea floor under the ROV is nonlinear. As the DVL beam angles are constant, the footprint of the DVL will increase with altitude, see Figure 6. The linear approximation of the normal vector may be good for low altitudes, even for a curved sea floor. At high altitudes, the linear approximation may be poor if the sea floor is nonlinear.
The DVL is used to measure velocity through water or over the sea floor and altitude. This section shows to use the DVL altitude measurements to approximate the ROV altitude and the sea floor normal (gradient) vector. 3.1 DVL Beams and Kinematics A typical DVL has 4 beams. The ith DVL beam with parameters is shown in Figure 3. The ith beam is represented L by the vector r b,DV (10) and goes from the DVL to the i sea floor. The superscript b,DV L is to tell that the vector is expressed in the body fixed frame of the DVL.
nni
" # L xDV tan(γi )cos(βi ) i DV L tan(γi )sin(βi ) , (10) = yiDV L = ai L 1 aDV i
L r b,DV i
(11a) (11b)
(r ni − r ni+1 ) × (r n4 − r ni+1 ) if i = 1, = (r ni − r ni+1 ) × (r ni−1 − r ni+1 ) if i = 2, 3 (r ni − r n1 ) × (r ni−1 − r n1 ) if i = 4.
(12)
E.g. nn1 is the cross product of the two vectors seen in Figure 6. 3.3 ROV Altitude Approximation from DVL Measurements
where γi is the angle of the ith beam from the DVL zL axis, and βi is the angle from the x-axis. aDV is the i b,DV L b,DV L vertical component of r i . ri is expressed in terms L L of aDV and the constant angles βi and γi because aDV i i is an output measurement from the DVL.
The normal plane to the approximated gradient vector is used to find an approximation of the ROV altitude, aCO i . The elements of the ith gradient vector and DVL beam vector are
L r b,DV is transformed to the ROV body frame in (11a) i and to NED frame in (11b). The first transformation is illustrated in Figure 4.
nni,1 nni = nni,2 , nni,3
81
" r ni
=
# xni n yi . ani
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
ROV
DVL Altitudes 18
CO
x z
14 12
150
200
250
300
t [s] ROV Altitudes 18
rni+1
aCO 1
aCO 2
aCO 3
aCO 4
16 a [m]
−
L aDV 4
8
aCO i nni
rni
L aDV 3
10
rni
rni+1
L aDV 2
16 a [m]
DVL
L aDV 1
14 12 10
Fig. 5. Sea floor normal vectors.
8 150
x
200
250
300
t [s]
Fig. 7. Altitude approximation from DVL.
4
From the tangent plane equation (13b), it is seen that the components of the ith gradient vector are as given by (14a). The ith approximated gradient vector is given by (14b). If an altimeter is available, this measurement may be used in the observer instead of the approximated altitude from DVL measurements. However, then the filter effect of the approximated altitude can not be exploited.
1 y rn1
−
rn2
rn4 − rn2
ˆ ˆ nni,1 nni,2 ∂f ∂f =− n , =− n ∂x i n ∂y i n " i,3 # i,3 ˆ ˆ ˆ = ∂f , ∂f , −1 ∇F ∂x i ∂y i i
2 Fig. 6. DVL footprint.
(14a) (14b)
The approximated gradient vector is used in the altitude estimation and in the guidance law.
The plane equations, in the NED frame with origin in the CO of the ROV, are given by (13a) and solved for z to give (13b). The approximated altitude of the ROV is given by (13c).
3.4 Altitude and Rate Approximation The inputs to the altitude observer are the approximated altitude, aCO ˙ CO i , and altitude rate of change, a i , which were derived in section 3.3 from the DVL altitude measurements. The observer inputs will be noted as am and a˙ m and are given by (15a) and (15b).
nni,1 (x − xni ) + nni,2 (y − yin ) + nni,3 (z − ani ) = 0 (13a) nni,1 nni,2 ⇒ z = ani − n (x − xni ) − n (y − yin ) (13b) ni,3 ni,3 nni,1 n nni,2 n n ⇒ aCO = a + x + n yi . (13c) i i nni,3 i ni,3
ˆ ˆ ∂f ∂f am = aCO = ani − (15a) xni − yn i ∂x i ∂y i i " # " # u ˆ ˆ ∂f ˆ ∂f n ˆ v ˆ (15b) , , −1 R ( Θ ) a˙ m = a˙ CO = nb i b ∂x i ∂y i w ˆ
Note that the sea floor, as seen from the ROV, with the approximation in (13c) is z = zp + aCO i . This implies that the approximation method is also a filter (imagine the DVL beams as the ROV goes off a cliff at constant depth). The filtering effect of the sea floor is desirable when the estimated altitude is used in a guidance law for altitude control as the ROV has limited bandwidth in heave.
Note that the expression in (15b) is strictly only valid for a sea floor with continuous first-order partial derivatives. However, for a rugged sea floor, where the assumption of continuous first-order partial derivatives is violated, the approximated altitude and sea floor gradient vector is still usable for altitude estimation and in a guidance law for altitude control.
Experimental result 1. An example result of equation (13c) for i = 1, 2, 3, 4, is seen in the bottom plot of Figure 7 and L is based on the DVL altitudes, aDV , in the top plot. The i example is from a test with ROV Minerva moving over the sea floor at constant depth. The approximation aCO 1 is based on DVL beams no. 1, 2 and 4. aCO is based on 2 DVL beams no. 1, 2 and 3. aCO is based on DVL beams 3 no. 2, 3 and 4. aCO is based on DVL beams no. 3, 4 and 4 1. It is seen that the 4 different approximations are almost the same.
4. ALTITUDE ESTIMATION The measurement equations for the altitude observer is given as 82
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
am (k) = a(k) + v(k) a˙ m (k) = a(k) ˙ + w(k),
ROV Trace 25
where w(k) and v(k) are noise processes. 20 North Position [m]
The discrete time control plant model (CPM) equations are given as a(k + 1) = a(k) + a(k)h ˙ = a(k) + a˙ m (k)h − w(k)h y(k) = am (k) = a(k) + v(k)
15
10
5
where h is the time step.
0
The CPM equations are reformulated to standard notation as x(k + 1) = Φx(k) + ∆u(k) + Γw(k) (16a) y(k) = Hx(k) + v(k), (16b)
−30
−25
−20
−15 −10 East Position [m]
−5
0
Fig. 8. Lawnmower pattern. " # ˆ ∂f ˆ ∂f ˆ ˆ u wd = , Rnb (ψ) − a˙ d vˆ ∂x ∂y Z t + kp (a − ad ) + ki (a − ad ) dτ 0 Z t zd = wd dτ + zic ,
where x = am , u = a˙ m , Φ = 1, ∆ = h, Γ = −h, H = 1. An observer defined in terms of (16a) and (16b) should be used to estimate the altitude. 5. GUIDANCE LAW FOR ALTITUDE CONTROL VIA DEPTH CONTROL
5
(19a) (19b)
0
Depth measurements are more reliable than altitude measurements. As depth is a state the ROV, altitude control should be obtained from a depth reference to a depth controller. This choice is also a stability concern as the depth of the ROV is always continuous but the altitude can be discontinuous and may change by several hundred meters from one time step to another if the ROV goes off a shelf. Therefore the altitude should not be input to a feedback controller. A guidance law for depth reference is presented in this section.
where ad is the desired altitude, a˙ d is the desired altitude rate and zic is the initial depth when altitude control is activated. For cases including roll and pitch, the desired heave velocity must be transformed to NED before (19b) is applied. The proportional and integral feedback terms in wd (19a) is from Caccia et al. (2003b) and the feedforward term is a new result of this paper. The feedback terms are needed to compensate for inaccuracies in the sea floor gradient estimation and vehicle state estimates. The feedforward term is very important as it assures a sufficient desired depth velocity for any horizontal velocity, heading and steepness of the sea floor. Without the feedforward term, the ROV can hit the sea floor while going uphill or overshoot downhill. This method of altitude control is well suited for ROVs in joystick operation as it works for any heading and speed direction.
The relationship between the ROV motions, sea floor gradient and altitude rate of change was given in (9). This can be exploited in a feedforward term in the guidance law by setting (9) equal to the desired altitude rate of change as seen in (17). The desired depth velocity, wd,f f , is found by solving (17) for w.
It should be noted that if the ROV moves fast over a steep or rugged sea floor with altitude control, the ROV may not be able to follow the desired depth generated by (19b). In that case a feedback to the horizontal speed guidance law will assure that the desired depth reference is within the ROV capabilities.
Rnb (Θnb )v
a˙ = ∇F (p) · " # u ∂f ∂f ! n = , , −1 Rb (Θnb ) v = a˙ d (17) ∂x xp ,yp ∂y xp ,yp w The desired depth velocity in the special case for zero roll and pitch is
Experimental result 2. Results from a test with altitude control according to the guidance law in (19a) and (19b) are shown in Figures 8, 9 and 10. The test was conducted in the Trondheimsfjord with the ROV Minerva. The task is to track a lawnmower pattern while maintaining a constant altitude of 5 m. The horizontal ROV trace and snapshots of the ROV outline is seen in Figure 8. The ROV moves in surge with 0.3 m/s while heading towards the next way point. The top plot in Figure 9 shows the desired, measured and estimated altitude and the bottom plot shows the desired depth as generated by (19b) and the estimated depth from the vehicle observer. In this case, measurements from an altimeter is used as input to
wd,f f
∂f ∂f u n = , Rb (ψ) − a˙ d . (18) v ∂x xp ,yp ∂y xp ,yp
When roll and pitch are included, the expression for wd,f f gets more complex. Eq. (18) may be used for a ROV which has only small deviations from zero roll and pitch. This leads to a new guidance law in (19a) and (19b) for desired depth.
83
IFAC MCMC 2012 September 19-21, 2012. Arenzano, Italy
of the sea floor gradient and ROV altitude with the vehicle observer, was presented. Finally, a guidance law for depth reference was given. The new contribution was a feedforward term based on the expression for altitude rate of change. This assures proper depth reference for any horizontal motion and sea floor steepness during altitude control. The presented methods have been successfully implemented and tested on the ROV Minerva.
ROV Altitude
6
a ˆCO avg
am
aCO d a [m]
5.5 5 4.5
0
500
1000
1500
t [s] ROV Depth
ACKNOWLEDGEMENTS
86 z [m]
84
This work has also been carried out at the Centre for Ships and Ocean Structures (CeSOS) and the Applied Underwater Robotics Laboratory (AUR-Lab). The Norwegian Research Council is acknowledged as the main sponsor of CeSOS.
82 80 78
zd 0
500
zˆ
1000
1500
t [s]
REFERENCES
Fig. 9. Guidance law; altitude estimation and depth reference.
Aandnonsen, K. and Hagen, O. (2010). An analysis of real-time terrain aided navigation results from a hugin auv. In OCEANS 2010, 1 –9. Caccia, M., Bono, R., Bruzzone, G., and Veruggio, G. (2003a). Bottom-following for remotely operated vehicles. Control Engineering Practice, 11(4), 461 – 470. Caccia, M., Bruzzone, G., and Veruggio, G. (1999). Active sonar-based bottom-following for unmanned underwater vehicles. Control Engineering Practice, 7(4), 459 – 468. Caccia, M., Bruzzone, G., and Veruggio, G. (2003b). Bottom-following for remotely operated vehicles: Algorithms and experiments. Autonomous Robots, 14, 17–32. Dukan, F., Ludvigsen, M., and Sørensen, A.J. (2011). Dynamic positioning system for a small size rov with experimental results. In OCEANS, 2011 IEEE - Spain, 1 –10. Edwards, C. and Penney, E. (1997). Edwards & Penney fifth edition Calculus with Analytical Geometry. Prentice Hall. Fossen, T.I. (2011). Handbook of Marine Craft Hydrodynamics and Motion Control. John Wiley and Sons Ltd. Maki, T., Ura, T., Mizushima, H., Kondo, H., Sakamaki, T., and Yanagisawa, M. (2007). Low altitude tracking of rugged seafloors for autonomous visual observation. In Underwater Technology and Workshop on Scientific Use of Submarine Cables and Related Technologies, 2007. Symposium on, 488 –494. Silvestre, C., Cunha, R., Paulino, N., and Pascoal, A. (2009). A bottom-following preview controller for autonomous underwater vehicles. Control Systems Technology, IEEE Transactions on, 17(2), 257 –266. Yoerger, D., Jakuba, M., Bradley, A., and Bingham, B. (2007). Techniques for deep sea near bottom survey using an autonomous underwater vehicle. In S. Thrun, R. Brooks, and H. Durrant-Whyte (eds.), Robotics Research, volume 28 of Springer Tracts in Advanced Robotics, 416–429. Springer Berlin / Heidelberg. Yoerger, D., Bradley, A., Walden, B., Cormier, M.H., and Ryan, W. (2000). Fine-scale seafloor survey in rugged deep-ocean terrain with an autonomous robot. In Robotics and Automation, 2000. Proceedings. ICRA ’00. IEEE International Conference on, volume 2, 1787 –1792 vol.2.
D [m]
−84 −86 −88 −90 0
25 −5
20 −10
15 −15
10 −20
E [m]
5 −25
0
N [m]
Fig. 10. Approximation of normal vectors of the sea floor under the ROV. the altitude observer. The estimated altitude from DVL measurements are seen as the blue line. The approximated gradient vectors (normal vectors) of the sea floor under the ROV from (14b) is seen in Figure 10. The tail of the gradient vector for each time step is placed in the ROV x and y position with z = zˆp + a ˆCO avg . This gives an approximation of the sea floor geometry. The parameters in the guidance law (19a) are set to kp = 0.2, ki = 0 in the test. The feedforward term wd,f f is used with sea floor gradient estimation from DVL beams no. 1, 2 and 4. 6. CONCLUSION A new method for altitude estimation was presented. The key point was to split the altitude problem into ROV motions and sea floor geometry. Because the altitude is given by the ROV position if the sea floor is time invariant, the altitude rate of change can be expressed from the sea floor gradient and ROV attitude and velocity vectors. A new method to approximate the sea floor gradient and ROV altitude from DVL altitude measurements was given. An altitude observer, which combines the approximations 84