The information received from the mechanical scanning sonar is innate noisy because these active acoustic devices estimate the distance using the time-of-flight principle. Echoes from sonar are affected by different sources of uncertainty that can seriously degrade the distance estimation accuracy. Some examples are the wide opening angle of acoustic signals presented by most sonar sensors and the multi-path reflections. One solution to filter out noisy distance estimation readings is to check the coherence of data received at different times. We deal with this problem by building the spatio-temporal relations between the sonar echoes. Maintaining the sonar buffer implies a series of operations:

We can view the sonar buffer as a vehicle-centered consistent local map along time, at least up to a certain degree depending on how the vehicle’s velocity is measured or estimated. We can apply multiple feature extraction algorithms when a map is available. The next sections present the techniques adopted to perceive circumference arcs and line segments in structured environments. The perception of such features is useful for improving the accuracy of the navigation system.

#### 3.2.1. Circular Model-Fitting

The recognition of circumference arcs is useful when the underwater vehicle operates in a structured environment with these features. We can mention circular swimming pools, fish farms, and tanks, to name but a few. The recognition of these features provides us information that allows us to locate the vehicle to the distance estimated from the center of the circumference with a known location in the workplace. We can adopt different alternatives for the perception of circumference arcs [

21] from the sonar scanner reading. We can mention algebraic-fitting methods and geometric-fitting techniques. The former is quite fast, but they lack robustness in the presence of outliers. The latter are iterative and tend to be robust in the presence of outliers. Since the sonar scanner readings tend to be quite noisy, algebraic fitting techniques produce very few fittings, always when the robot is static, and thus we conclude that geometric fitting methods seem more appropriate for circumference arc extraction. We follow the circle-fitting approach [

22], which is detailed as follows.

Let

$\mathcal{P}={\left\{\phantom{\rule{0.166667em}{0ex}}(\phantom{\rule{0.166667em}{0ex}}{x}_{i}\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}{y}_{i}\phantom{\rule{0.166667em}{0ex}})\phantom{\rule{0.166667em}{0ex}}\right\}}_{i}$ be a set of points with a distribution approximately circular. We can use the following circle equation to model their position

where

$(\phantom{\rule{0.166667em}{0ex}}a\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}b\phantom{\rule{0.166667em}{0ex}})$ is the center of the circle and

R its radius. Each point

$(\phantom{\rule{0.166667em}{0ex}}{x}_{i}\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}{y}_{i}\phantom{\rule{0.166667em}{0ex}})$ in our set

$\mathcal{P}$ will approximately satisfy this equation. We can rewrite this approximate equation factorizing in terms that contain the model parameters

$\{a,b,R\}$ , and terms that contain the position of each point

$(\phantom{\rule{0.166667em}{0ex}}{x}_{i}\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}{y}_{i}\phantom{\rule{0.166667em}{0ex}})$ as follows

We can then transform our model-fitting problem into a

linear least-squares problem. Note that, since we have three unknown variables (

a,

b, and

R), we need three (or more)

${n}_{e}$ equations to obtain a determined (or overdetermined) system of linear equations; at least three points to determine the parameters of our model. Then, building the matrices

we can compute the matrix

$\mathbf{X}$ that minimizes the distance

${\parallel \phantom{\rule{0.166667em}{0ex}}\mathbf{A}\phantom{\rule{0.166667em}{0ex}}\mathbf{X}\phantom{\rule{4pt}{0ex}}-\phantom{\rule{4pt}{0ex}}\mathbf{b}\phantom{\rule{0.166667em}{0ex}}\parallel}^{2}$ through

Finally, we can obtain the model parameters using the following relations:

where {

${X}_{1}$,

${X}_{2}$,

${X}_{3}$} is the solution of Equation (

6).

However, we often obtain measurements that do not fit with a distribution approximately circular. We should detect and filter out these measures to achieve a robust detection of circumference arcs in the structured environment. The random sample consensus (RANSAC) method [

23] and its variants are fundamental tools for outlier rejection. In particular, we rely on the RANSAC variant maximum likelihood estimation sample and consensus method MLESAC [

24] to design the outlier rejection method in the circular model-fitting approach. For this purpose, we define the cost function for the model parameters

$a,\phantom{\rule{0.166667em}{0ex}}b,\phantom{\rule{0.166667em}{0ex}}$ and

R as follows:

Algorithm 1 presents the circular model-fitting with outlier rejection. The algorithm requires the set of $\mathcal{P}$ points received from the mechanical scanning sonar (Blue Robotics Ping 360), the threshold T used to compute the cost function, the probability of not finding a correct model, and the proportion of inliers in data. The output of the process is the best model parameters for $a,\phantom{\rule{0.166667em}{0ex}}b,\phantom{\rule{0.166667em}{0ex}}$ and R.

Figure 6a shows an example of a fit to a circle with noisy data using Algorithm 1. We represent the set of points P to fit using black crosses. The continuous green circumference is the target fit with the center at the green cross point. We show the best circle-fitting using three points with a red dotted circle with the center at the red cross point. Finally, we depict the best fit using all the inliers with a dashed blue circumference with the center at the blue cross point. We can observe that the best fit using all the inliers is closer to the target solution than the fit using three points.

Figure 6b shows the resulting circumference using the mechanical scanning sonar readings while the underwater vehicle navigates in the swimming pool.

**Algorithm 1** Circular model-fitting with outlier rejection |

**Input:** | |

`points` | ▹ Set of points to be fitted |

T | ▹ Threshold used to compute the cost function |

FAILURE_PROBABILITY | ▹ Probability of not finding a correct model |

INLIER_PROPORTION | ▹ Proportion of inliers in data |

**Output:** | |

$best2\_a\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}best2\_b\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}best2\_R$ | ▹ Best model parameters found |

Initialization | |

1: $best\_C\phantom{\rule{4pt}{0ex}}\leftarrow \phantom{\rule{4pt}{0ex}}{10}^{12}$ | ▹ Initialize to a large number |

2: $N\phantom{\rule{4pt}{0ex}}\leftarrow \phantom{\rule{4pt}{0ex}}log(FAILURE\_PROBABILITY)/log(1-INLIER\_PROPORTIO{N}^{3})$ |

Find model | |

3: **for**$i=1$ to N **do** | |

Find possible model | |

4: Take 3 points randomly | |

5: Build matrices $\mathbf{A}$ and $\mathbf{b}$ using equations (4) and (5) and the 3 sampled points |

6: Find model parameters $a\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}b\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}R$ using equations (6) and (7) |

7: Compute the cost function C using equations (8) to () |

If this possible model is better than the previous one, we keep it |

8: **if** ($C<best\_C$) **then** | |

9: $best\_C\phantom{\rule{4pt}{0ex}}\leftarrow \phantom{\rule{4pt}{0ex}}C$ | |

10: $best\_a\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}best\_b\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}best\_R\phantom{\rule{0.222222em}{0ex}}\phantom{\rule{4pt}{0ex}}\leftarrow \phantom{\rule{0.222222em}{0ex}}\phantom{\rule{4pt}{0ex}}a\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}b\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}R$ |

11: **end if** | |

12: **end for** | |

Refine the model using inliers |

13: Select the points $(\phantom{\rule{0.166667em}{0ex}}{x}_{i}\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}{y}_{i}\phantom{\rule{0.166667em}{0ex}})$ such that ${e}_{i}<T$ using (8) |

14: Build matrices $\mathbf{A}$ and $\mathbf{b}$ using equations (4) and (5) and the selected inliers |

15: Find model parameters $best2\_a\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}best2\_b\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}best2\_R$ using equations (6) to (7) |

16: **return**$best2\_a\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}best2\_b\phantom{\rule{0.166667em}{0ex}},\phantom{\rule{0.166667em}{0ex}}best2\_R$ |

#### 3.2.2. Line Segment Model-Fitting

The perception of line segments in structured environments is a more complicated task than the circle fitting presented above. This fact is because we can obtain several features of this type from the sonar scanning sensor. We have to deal with the uncertainty of such perceptions to maintain a coherent representation of the environment surrounding the underwater vehicle. We adopt a fuzzy segment framework [

25,

26] to represent and deal with the location uncertainty using line segments. These features include a representation of their uncertain location. The fuzzy segment framework represents the uncertainty using a fuzzy set whose degree of membership reflects how much the location could be occupied. This fuzzy segment framework provides power tools, based on similarity interpretation of fuzzy logic [

27], to match the degree of similarity of information expressed as fuzzy segments. We use such tools to fuse and manage formally the uncertainty of the observations represented by fuzzy sets [

28].

Let a line segment S be defined as a tuple as

where

$\theta $ and

$\rho $ are the parameters of the line equation

$xcos\left(\theta \right)+ysin\left(\theta \right)=\rho $ obtained by fitting

k collinear range sensor observations, and

$({x}_{i},{y}_{i})$ and

$({x}_{j},{y}_{j})$ are the end-points of the line segment calculated as the projection of the sensor observations on the fitted line using the

k collinear sensor observations.

We need to extract the set of

k collinear sensor scanner readings to perform the eigenvector line fitting mentioned above. We have adopted an optimized algorithm that only split sets from consecutive reading. The main reason is the performance constraints of our application. In particular, we use the Iterative End Point Fit (IEPF) algorithm [

29,

30], which requires the initial definition of the minimum number of points

${k}_{min}$ of a set of collinear observations and the maximum distance

${\rho}_{max}$ of the scatter sensor readings to the fitted line segment. We have to remark that this algorithm requires a set of ordered observations. For a set

s of continuous sensor scanner readings, the algorithm is as follows:

**Initialization**. We initialize the algorithm with a set s containing all the ordered observations.

**Step 1**. If the set s is composed of more than ${k}_{min}$ observations, draw a line segment between the first and last data (end-points), otherwise reject the set s.

**Step 2**. Detect the point P with maximum distance ${\rho}_{P}$ to the fitted line segment between the end-points.

**Step 3**. If ${\rho}_{P}\ge {\rho}_{max}$ splits the set s at P into two subsets ${s}_{1}$ and ${s}_{2}$ and **goes to Step 1** for both subsets. Otherwise, the set s is a candidate to be a line segment.

**Stopping criteria**. We finalize the search when all the subsets are a candidate to be a line segment satisfying the condition ${\rho}_{P}\le {\rho}_{max}$ or are rejected because they have fewer than ${k}_{min}$ observations.

Figure 7 shows an example of the Iterative End Point Fit recursive (IEPF) method. We can observe that the algorithm operates with a set

s of

k continuous sensor scanner readings. The initial step consists of drawing a line segment (represented as a red dotted line) between the end-points of the initial set

s. Since there are numerous sensor readings further away from the confidence interval defined by the

${\rho}_{max}$ parameter (represented as a blue dotted line), the initial set is divided by the point

P at the furthest distance

${\rho}_{P}$ from the line segment between end-points (red dotted line) into two subsets. If all the sensor readings of the corresponding set (with more than

${k}_{min}$ observations) are inside the confidence interval, we consider such a set of sensor readings as a candidate to be fitted as a line segment. We repeat this procedure until there is not any set candidate to form a line segment. We have to remark that this approach does not provide a set of line segments but groups of sensor readings candidate to be fitted as line-segments.

We represent the uncertainty on the location of the line segment

S using the trapezoidal fuzzy set

${t}_{\rho}$. This set represents the uncertainty in the

$\rho $ parameter. Different factors can affect the location uncertainty of the

$\rho $ parameter, such as the line segment fitting from scattering sonar scanner readings, the aging of the fuzzy segment building, and the motion of the underwater vehicle, to name but a few. Assuming the independence of all these factors, we can define the trapezoidal fuzzy set

${t}_{\rho}$ in the

$\Omega $ domain as the addition of the representation of all the sources of uncertainty that affect the

$\rho $ parameter as

where

$t{p}_{\rho}^{i}$ with

$i=1,\cdots ,n$ are the trapezoidal fuzzy sets representing the

i factors that influence the

$\rho $ parameter, ⊕ is the bounded sum operator, (

$-{\rho}_{0}$,

${\rho}_{0}$) is the

$\alpha $-cut in the fuzzy membership

$\mu =0$, and (

$-{\rho}_{1}$,

${\rho}_{1}$) is the

$\alpha $-cut in fuzzy membership

$\mu =1$. These

$\alpha $-cuts define the regions within fuzzy segments are considered within the degree of similarity

$\alpha $. We can use this criterion to address matching problems taking into account the location uncertainty.

Figure 8 shows the scatter points fitted to a fuzzy segment with the trapezoidal fuzzy set

$t{p}_{\rho}$, defined as an ordered tuple

$(-{\rho}_{0},-{\rho}_{1},{\rho}_{1},{\rho}_{0})$, including the different factors affecting the location uncertainty.

Thus, we define a fuzzy segment as a line segment

S including its associated location uncertainty represented as a trapezoidal fuzzy set

${t}_{\rho}$ as

where

$t{p}_{\rho}$ is the trapezoidal fuzzy set representing the uncertainty in

$\rho $. We build this fuzzy set from the sonar scanner readings candidate to be fitted as a line segment. In particular, we assign the interval with confidence level

$0.68$ to the

$\alpha $-cut in

$\mu =1$, and the interval with a confidence level of

$0.95$ to the

$\alpha $-cut in

$\mu =0$. These intervals are the values of one and two standard deviations for a Gaussian distribution of the observations. The confidence interval for the Gaussian distribution with known variance is given by

$\rho \pm |{t}_{k-1;1-\frac{\alpha}{2}}|\xb7{\sigma}_{\rho}$, where

${t}_{k-1;1-\frac{\alpha}{2}}$ is the value of a t-student distribution with

$k-1$ degrees of freedom with probability

$\frac{\alpha}{2}$ and

${\sigma}_{\rho}$ is the standard deviation of the fitted

$\rho $ parameter. Thus, the fuzzy set that represents the uncertainty of the fitted line is given by

We can calculate the uncertainty in the

$\theta $ parameter of the line equation

$xcos\left(\theta \right)+ysin\left(\theta \right)=\rho $ obtained by fitting

k collinear range sensor observations as

where

l is line segment length.

We can maintain a coherent representation surrounding the underwater vehicle with the fuzzy segments using a similar approach to the Spatio-temporal relations between the different sonar echoes presented above. We can update the location and uncertainty of such features using the time elapsed from their generation. We can also update their position using the motion estimation of the underwater vehicle. The bounded sum operator ⊕ allows us to fuse the uncertainty of the fuzzy segment $t{p}_{\rho}$ with the motion estimation and the elapsed time from the generation of the features representing the world around the vehicle.

We can also fuse the detection of new features to the local perception representing the environment surrounding the underwater vehicle using the degree of similarity between fuzzy segments. We merge similar features by detecting their collinearity and fusing their uncertainty. Two segments

$F{S}_{a}$ and

$F{S}_{b}$ are considered collinear if they satisfy

where

$f(x,y)$ function is the matching degree between two trapezoidal fuzzy sets defined in the same universe

$\Omega $ as follows

where

${A}_{x}$ and

${A}_{y}$ denote the area enclosed by the fuzzy sets

x and

y, respectively, and

${A}_{xy}$ denotes the area of the intersection of

x and

y.

We combine new fuzzy segment perceptions with the ones contained in the buffer representing the environment around the underwater vehicle that satisfies the collinear condition (

16). This procedure allows us to enrich the local representation and remove redundant information, which reduces the uncertainty of old and imprecise feature representations. The combined fuzzy segment

$F{S}_{r}$ from two collinear ones is calculated by

where (

${x}_{ir}$,

${y}_{ir}$) and (

${x}_{jr}$,

${y}_{jr}$) are the end-point perpendicular projections of (

${x}_{ia}$,

${y}_{ia}$), (

${x}_{ja}$,

${y}_{ja}$), (

${x}_{ib}$,

${y}_{ib}$), and (

${x}_{jb}$,

${y}_{jb}$) on the line with (

${\theta}_{r}$,

${\rho}_{r}$) parameters calculated as

where

$t{p}_{{\rho}_{r}}$ is the trapezoidal fuzzy set representing the uncertainty of the fusedfuzzy segment.

Figure 9a shows an example of the buffer data using the range observations from the mechanical sonar scanner sensor. We extract the set of candidate points to be considered a line segment using the IEPF method. We fit these candidates to form a line using an eigenvector line fitting method. We represent these line segments using green lines.

Figure 9b depicts the local representation of the environment around the vehicle using

fuzzy segments. We can observe that such features include the location uncertainty using the corresponding trapezoidal fuzzy set

$t{p}_{\rho}$. This local representation using fuzzy segments allows us to add and remove perceptions using a formal model, maintaining a coherent representation around the vehicle.

Figure 10 shows the flowchart of the procedure for building and maintaining a local representation of the environment using fuzzy segments. We group the sonar scanner readings into

n sets with

$\{{k}_{1},\cdots ,{k}_{n}\}$ observations using the IEPF method described above. We then fit such groups of consecutive sensor readings using some eigenvector line fitting method to obtain the set of line segments

$\{{S}_{1},\cdots ,{S}_{n}\}$. We use the confidence interval of the line-fitting algorithm to build the trapezoidal fuzzy sets

$\{t{p}_{\rho 1},\cdots ,t{p}_{\rho n}\}$ representing the uncertainty of the fuzzy segment with (

14). Once we have calculated the set of

n fuzzy segments detected from the observations, they are fused with the set of

m fuzzy segments representing the environment around the underwater vehicle using the (

16) criteria, or they are incorporated into such a representation. We update periodically the set of fuzzy segments

$\{LF{S}_{1},\cdots ,LF{S}_{m}\}$ representing the local environment around the vehicle introducing the different sources of uncertainty affecting them. We model the uncertainty of the vehicle motion and the aging of the representation using trapezoidal fuzzy sets. We incorporate these sources of uncertainty into the fuzzy segment representation using the bounded sum operator of (

12). We also remove these uncertain features when the area enclosed by the trapezoidal fuzzy set

$t{p}_{\rho}$ of the fuzzy segment is higher than a prescribed threshold.