We consider an underwater robot equipped with an altimeter (a simple echo-sounder oriented downward), a barometer and a low cost gyroscope. 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.
This paper deals with the difficult problem for a robot to explore an unknown environment, without any
localization 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
Here, to perform the exploration we will rely on a bathymetric approach. This is motivated by the thesis
of Rohou
y2 y3 = z − h(x, y) = angle(∇h(x, y)) − ψ = −z 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 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
In this section, we propose a controller of the form u = r (y), which makes the robot follows an isobath corresponding to h0 = −9m at a depth y3 = 2m. For the control of the depth, we can take a proportional control of the form
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 y2 = − π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 + ),
2
where tanh creates a saturation
sawtooth(θe) = 2atan tan θ2e = mod(θe + π, 2π) − π (5) For the initial condition, we take x = 2, y = −1, z = −2, ψ = 0 we obtain the trajectory depicted on Figure 3. h (x, y) = 2 · e− (x+2)21+0(y+2)2 + 2 · e− (x−2)21+0(y−2)2 − 10.
u =
y3 − y3 −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 u2 = (y1 − y1) + y˙1 = (y1 − y1) + z˙ − ∇h(x, y) · cos ψ sin ψ where y1 = −y3 − h0 , z˙ can be assumed to be zero and ∇h(x, y) · csoins ψψ Recall that x, y, ψ are not measured and cannot be used by our controller.
is assimilated to sawtooth(y2 + π2 ). (6) (7) (8)
We assumed previously that the robot was able to measure the vertical distance y1 to the seafloor using an
echosounder. 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
In the local frame of the robot projected onto the surface, the underneath plane satisfies the equation (9) (10) z1 = p1x1 + p2y1 + p3 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 = p1x12 + p2y12 + p3x1y1 + p4x1 + p5y1 + 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 p˙ = −ψ˙ 0 0 p.
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: controller
Kalman filter
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 = 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.
u =
y3 − y3 −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. (11) (12) (13)
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)