Isobath Following using an Altimeter as a Unique Exteroceptive Sensor Luc Jaulin ENSTA Bretagne LabSticc lucjaulin@gmail.com Abstract We consider an underwater robot equipped with an altimeter (a simple echo-sounder oriented downward), a barometer and a low cost gyro- scope. The robot has no compass. We show that using vector field control combined with a Kalman observer, it is possible to follow an isobath (which is a curve that connects all points having the same depth). The robustness of the controller is validated on a simulation. 1 Introduction This paper deals with the difficult problem for a robot to explore an unknown environment, without any localiza- tion system and without being lost. By being lost, we mean not being able to reach a target set, or equivalently not being able to come back home. Under the water, we can easily know the depth using a barometer and the problem of finding a path can be considered in the horizontal 2D plane. If we are able to measure some quantities such as the altitude, the temperature, the salinity, etc, we can find a reliable path which allows us not being lost. This the case of underwater animals such as marine turtles, or whales which follow isotherms (Lohmann and Lohmann, 1996) with the help of an internal compass. These underwater animals do not know where they are but they know from the evolutionary process that if they follow a sequence of iso-potentials they will perform a cycle which is stable. Here, to perform the exploration we will rely on a bathymetric approach. This is motivated by the thesis of Rohou (Rohou, 2017) who has shown that once some underwater exploration has been done, a bathymetric localization can be performed if we know the map. Better than that, an efficient and reliable bathymetric simultaneous localization and mapping (SLAM) could be done using the tube approach of Rohou (Rohou, 2017). Now, Rohou assumed that the mission was already performed and an external localization system had to be used for this purpose. This motivates the need to explore an unknown underwater environment using an altimeter as the single exteroceptive sensor. The paper proposes a solution for this exploration following an isobath in a simple manner, with simple low cost sensors, without any compass and without surfacing to use the GPS. Copyright c by the paper’s authors. Copying permitted for private and academic purposes. In: S. M. Schillai, N. Townsend (eds.): Proceedings of the International Robotic Sailing Conference 2018, Southampton, United Kingdom, 31-08-2018 105 ROBOTIC SAILING 2018 Figure 1: Underwater robot that has to follow an isobath 2 Problem An isobath is an imaginary curve that connects all points having the same depth h(x, y) below the surface, i.e., an underwater level curve. Consider an underwater robot (Jaulin, 2015a) described by the state equation:    ẋ = cos ψ  ẏ = sin ψ (1)   ż = u1  ψ̇ = u2 where (x, y, z) corresponds to the position of the robot and ψ is its heading angle. The robot is able to measure its altitude y1 with an echo sounder which is a simple and low-cost sonar transmitting a sound pulse. The time interval between emission and return of a pulse is recorded and provides the distance to the seafloor. In the first part of the paper, we assume that we measure the angle y2 of the gradient of h in its own frame. This assumption which is not always realistic will be lifted later in Section 4. Moreover the robot is able to know its depth y3 using a pressure sensor. The observation function of our system is thus   y1 = z − h(x, y) y2 = angle(∇h(x, y)) − ψ (2)  y3 = −z  where ∇h(x, y) the gradient of h. For instance, if y = 7, − π2 , 2 , the robot knows that it is following an isobath corresponding to −y1 − y3 = −7 − 2 = −9m, at a depth of 2m. This is illustrated by Figure 1. 3 Controller In this section, we propose a controller of the form u = r (y), which makes the robot follows an isobath corre- sponding to h0 = −9m at a depth y 3 = 2m. For the control of the depth, we can take a proportional control of the form u1 = y3 − y 3 . (3) For the heading, assume that we first want to follow the isobath just below the robot. Equivalently we want to the robot be perpendicular to ∇h(x, y), for instance, y2 = ± π2 , depending if we want the gradient on our right or on our left. Take for instance e1 = y2 + π2 as an error. This means that we want to have an angle y 2 = − π2 with ∇h, or equivalently, we want ∇h on the right, as in Figure 1. If e1 = 0, we follow an isobath, but is may not be the right one. We thus have to consider another error corresponding to e2 = −y3 − y1 − h0 = 0. It e2 = 0, we are just above the right isobath but maybe not parallel to it. If both e1 = 0 and e2 = 0, we are on the right isobath (e1 = 0) but we also go on the right direction (e2 = 0). For the heading, we may take the following controller π u2 = tanh(e2 ) + sawtooth(e1 ) = −tanh(h0 + y3 + y1 ) + sawtooth(y2 + ), (4) 2 106 Manoeuvre & Route Planning where tanh creates a saturation (Jaulin, 2015b). The sawtooth function is given by:   e = 2atan tan θe = mod(θe + π, 2π) − π sawtooth(θ) (5) 2 As illustrated by Figure 2, the function corresponds to an error in heading. The interest in taking an error θe filtered by the sawtooth function is to avoid the problem of the 2kπ modulus: we would like a 2kπ to be considered non-zero. Figure 2: Sawtooth function used to avoid the jumps in the heading control The controller may thus be given by   y3 − y 3 u= . (6) −tanh(h0 + y3 + y1 ) + sawtooth(y2 + π2 ) The coefficients for the controller (all taken here equal to ±1) should be tuned in order to have the stability and correct time constants. Remark. For the heading, the controller is close to a proportional and derivative control, where tanh(h0 + y3 + y1 ) corresponds to the proportional term and sawtooth(y2 + π2 ) to the derivative term. For the heading control, we could take a proportional and derivative control of the form   cos ψ u2 = (y 1 − y1 ) + ẏ1 = (y 1 − y1 ) + ż − ∇h(x, y) · , (7) sin ψ   cos ψ where y 1 = −y 3 − h0 , ż can be assumed to be zero and ∇h(x, y) · is assimilated to sawtooth(y2 + π2 ). sin ψ Recall that x, y, ψ are not measured and cannot be used by our controller. Test-case We consider a seafloor described by (x+2)2 +(y+2)2 (x−2)2 +(y−2)2 h (x, y) = 2 · e− 10 + 2 · e− 10 − 10. (8) For the initial condition, we take x = 2, y = −1, z = −2, ψ = 0 we obtain the trajectory depicted on Figure 3. 107 ROBOTIC SAILING 2018 Figure 3: Simulation of underwater robot (blue) following an isobath. The surface shadow (gray) and the seafloor shadow are also painted. 4 Using an observer to get the gradient of the seafloor We assumed previously that the robot was able to measure the vertical distance y1 to the seafloor using an echo- sounder. Now, we have also assumed that the robot was also able to measure the angle y2 of the underneath isobath which is not realistic with a low-cost sonar. The angle y2 = angle(∇h(x, y))−ψ should thus be estimated using an observer such as a Kalman filter. The idea is similar to what is proposed in (Jaulin, 2015a) in the context where a car has to follow a wall at a given distance from measuring the distance to the wall only. In the local frame of the robot projected onto the surface, the underneath plane satisfies the equation z1 = p1 x1 + p2 y1 + p3 (9) where (p1 , p2 ) corresponds to the gradient and p3 to h (x, y). Let us note that • (i) Since the robot has no compass, it has no idea of its orientation and can thus have an estimation of its neighborhood only in its own frame. This is why we have chosen to express the plane in the robot frame. • (ii) Without limitation of the method, we have chosen a linear model. Now, for more accuracy, we could have taken a model of the seafloor with more parameters, such as a quadratic model z1 = p1 x21 + p2 y12 + p3 x1 y1 + p4 x1 + p5 y1 + p5 . This quadratic model is particularly interesting if the seafloor is smoothly curved. Prediction. We assumed that the underneath seafloor is locally planar. We thus have:   0 ψ̇ 0 ṗ =  −ψ̇ 0 0  p. (10) 1 0 0 This equation can be understood by the fact that the gradient (p1 , p2 ) turns with the robot and that the variable p3 increases when the robot moves with the gradient (i.e., with p1 ). As a consequence, we can assume the following prediction equation for the underneath plane: 108 Manoeuvre & Route Planning controller Kalman filter Figure 4: Use of a Kalman filter to estimate the seafloor   1 dt · u2 (k) 0 p (k + 1) =  −dt · u2 (k) 1 0  p (k) + α (k) . (11) dt 0 1 where α (k) is a white Gaussian noise, the covariance matrix of which depends on how planar is the seafloor. Correction. Since we measure both the altitude y1 and the depth y3 , we have an indirect measurement of p3 . We thus have the correction equation:  −y1 − y3 = 0 0 1 p (k) + β (k) . (12) where β (k) is a white Gaussian noise. The unknown gradient can thus be estimated by a Kalman filter which returns an estimation p̂ of p. This is illustrated by Figure 4. In the Kalman filter box, we have represented a plane which what is actually estimated by the Kalman observer. The controller (6) may thus be given by   y3 − y 3 u= . (13) −tanh(h0 − p̂3 ) + sawtooth(atan2(p̂2 , p̂1 ) + π2 ) As illustrated by Figure 5, the trajectory oscillates and the isobath following is less accurate. If we observe the covariance matrix for p, we observe that when the seafloor becomes planar for a while, the estimation is good at the beginning and becomes slowly very bad. This is due to the fact that a straight trajectory corresponds to a singularity. Now, in such a case, the robot goes ahead and performs the isobath following. Later, when the seafloor changes its orientation, the estimation of p becomes more accurate and the robot is able to change its orientation accordingly. 109 ROBOTIC SAILING 2018 Figure 5: The underwater robot follows an isobath, without compass and with a low-cost echo-sounder 5 Conclusion In this paper, we have shown that it was possible to follow an isobath with low-cost sensors that are not greedy in energy. Now, once we are able to follow such an isobath, it should be possible to detect the existence of a cycle using proprioceptive sensors (motors for instance) (Aubry et al., 2013). Such a cycle could thus lead us to a localization and also to the possibility to perform a bathymetric SLAM. Since the control is bathymetric, we have all elements to solve a pure bathymetric explore and return problem (Newman et al., 2002) which has not been solved yet, to our knowledge. References Aubry, C., Desmare, R., and Jaulin, L. (2013). Loop detection of mobile robots using interval analysis. Auto- matica, 49(2):463–470. Jaulin, L. (2015a). Automation for robotics. John Wiley & Sons. Jaulin, L. (2015b). Mobile robotics. Elsevier. Lohmann, K. and Lohmann, C. (1996). Orientation and open-sea navigation in sea turtles. Journal of Experi- mental Biology, 199(1):73–81. Newman, P., Leonard, J., Tardós, J. D., and Neira, J. (2002). Explore and return: Experimental validation of real-time concurrent mapping and localization. In Robotics and Automation, 2002. Proceedings. ICRA’02. IEEE International Conference on, volume 2, pages 1802–1809. IEEE. Rohou, S. (2017). Reliable robot localization: a constraint programming approach over dynamical systems. PhD thesis, Brest. 110