Voronoi Diagram-based USBL Outlier Rejection for AUV Localization

Hyeonmin Sim; Hangil Joe

doi:10.26748/KSOE.2024.049

Abstract

USBL systems are essential for providing accurate positions of autonomous underwater vehicles (AUVs). On the other hand, the accuracy can be degraded by outliers because of the environmental conditions. A failure to address these outliers can significantly impact the reliability of underwater localization and navigation systems. This paper proposes a novel outlier rejection algorithm for AUV localization using Voronoi diagrams and query point calculation. The Voronoi diagram divides data space into Voronoi cells that center on ultra-short baseline (USBL) data, and the calculated query point determines if the corresponding USBL data is an inlier. This study conducted experiments acquiring GPS and USBL data simultaneously and optimized the algorithm empirically based on the acquired data. In addition, the proposed method was applied to a sensor fusion algorithm to verify its effectiveness, resulting in improved pose estimations. The proposed method can be applied to various sensor fusion algorithms as a preprocess and could be used for outlier rejection for other 2D-based location sensors.

1. Introduction

Autonomous underwater vehicles (AUVs) have been deployed for various missions in underwater environments, making the accurate localization of AUVs crucial for successful mission execution. The ultra-short baseline (USBL) and Doppler velocity log (DVL) are the most commonly used sensors for tracking AUV positions. DVL, as one of the primary sensors for AUVs, emits acoustic waves from four transducers towards the seabed to measure velocity, and the measured velocity was integrated to estimate the AUV position. On the other hand, the integration constant accumulates with time, resulting in drift error, which can decrease accuracy when applied to the AUV navigation system. USBL, however, analyzes acoustic signals transmitted from an array to obtain the relative position of the AUV. Although USBL can secure stable position data without Drift Error over long operational periods, the data acquisition cycle may be extended depending on the distance between the transducer and transponder. In addition, outliers can occur due to multipath effects under varying environmental conditions. Missing data may occur in severe cases (Fig. 1). Sensor fusion-based approaches have been studied widely to overcome these drawbacks while integrating the advantages of different sensor types. More accurate and reliable localization data can be provided by combining information obtained from various sensors, enhancing the navigation and operational performance of AUVs.

Sensor fusion technology to improve AUV localization performance has been investigated in various ways. Studies include sensor fusion between the DVL and USBL based on particle filters (Rigby et al., 2006) and underwater navigation systems combining DVL and inertial measurement units (IMU) based on the unscented Kalman filter (UKF) (Krishnamurthy and Khorrami, 2013). Nevertheless, there is a high occurrence of outliers due to the USBL characteristics. Using data containing outliers as input values for the sensor fusion filter can cause critical flaws in the accuracy and reliability of underwater navigation systems. Therefore, improving the technology for removing outliers from USBL data significantly enhances the AUV navigation performance.

Previous studies on removing USBL outliers include using a median filter to calculate the median, measuring the distance between the median and current data, and considering it an outlier if it exceeds a set threshold (Morgada et al., 2015). Another study modeled the measurement errors and removed outliers based on the Mahalanobis distance (Lee et al., 2022). Furthermore, a moving average filter was proposed to consider the last N data points of a time series in a sliding window for support vector regression (SVR) training to remove the USBL outliers (Liu et al., 2019). Although these methods can be compatible with various systems, they involve many parameters for optimization and lack a structured method for selecting global parameter values, making parameter selection for the proposed method challenging.

Currently, commercially available underwater acoustic sensors often include outliers in their output because of their inherent characteristics. These outliers can distort the correlation between sensor data, making the estimated position or state inaccurate, degrading system performance, and causing unexpected errors and instability. Sensor data outliers must be identified and filtered out to ensure that sensor fusion algorithms receive accurate and reliable inputs because accurate localization is crucial in underwater environments.

This paper proposes a novel algorithm that utilizes Voronoi diagrams to detect and remove outliers from USBL data. The structure of this paper is as follows. Section 2 explains the AUV navigation system modeling, the proposed outlier removal approach, and the localization algorithm. Section 3 validates the proposed algorithm using Sea trial data. In addition, it outlines the design and application of a sensor fusion model to analyze the impact of outlier removal on sensor fusion results and presents comparative analyses.

2. Methodology

2.1 System Modeling

AUVs are generally described by six degrees of freedom motion, and the sensors primarily used for AUV localization considered in this study are the USBL, DVL, and IMU. The USBL measures the position in the global frame, while the DVL measures the velocity in the body-fixed frame. The motion of an AUV can be modeled based on the position, velocity, and attitude values obtained from sensors attached to the AUV (Lee et al., 2017). Considering a box-type hovering AUV used for precise surveys, the sway and heave motions are assumed to be negligible. The motion of the AUV can be expressed using the velocity obtained from the DVL and the attitude values from the IMU, as expressed in Eq. (1).

(1)

xk+1=[xyzuvwφ]k+1=[x+(uk⋅cosφk−vk⋅sinφk)⋅dty+(uk⋅sinφk+vk⋅cosφk)⋅dtz+wk⋅dtukvkwkφk]

where x, y, z, and ϕ represent the position and azimuth of the AUV in the global frame, while u, v, and w represent the velocity of the AUV in the body-fixed frame. Using the velocity values in the body-fixed frame and ϕ, the velocity components ẋ, ẏ, and ż of the AUV in the global frame can be expressed as Eq. (2) (Caccia and Veruggio, 2000), and the motion in the global frame can be represented over time.

(2)

x˙=uk⋅cos(φk)−vk⋅sin(φk)y˙=uk⋅sin(φk)+vk⋅cos(φk)z˙=wk

A hovering-type AUV can be described with three degrees of freedom, assuming it moves at a constant depth underwater. The system model can be simplified to include the velocity values in the global frame using Eq. (3).

(3)

[xyx˙y˙uvφ]=[x+x˙⋅dty+y˙⋅dtuk⋅cosφk−vk⋅sinφkuk⋅sinφk+vk⋅cosφkukvkφk]

The observation matrix measures all values in the state variables, and the characteristics of each sensor can be modeled through the measurement noise v_k introduced from the sensors using Eq. (4).

(4)

yk=Hkxk+vkHk=[I]7×7

In addition, the system model noise is modeled by adding ω_k using Eq. (5).

(5)

xk+1=f(xk)+ωk

2.2 Voronoi Diagram

The proposed algorithm consists of three main steps. First, USBL data is divided into regions using Voronoi diagrams, defining an area centered around each data point and structuring the data spatially. Second, a query point is calculated using a moving average filter. Finally, the presence of the query point in the divided regions is examined. The data in that area are treated as an inlier if a query point exists in the structured area. The data are classified as an outlier if the query point is not found.

The process of producing Voronoi diagrams involves connecting the closest feature points to a single feature point with line segments using Euclidean distance and then drawing perpendicular bisectors at the center of these segments to separate the areas of the feature points. Performing this process for all feature points results in the production of polygonal regions (Voronoi cells) (Fig. 2) (Sack and Urrutia, 1999).

A Voronoi cell represents an area on a plane centered around each feature point (S_i), with each feature point forming a cell. The shape and size of the cells vary according to the positions of the feature points. The cell size decreases if the feature points are in close proximity and vice versa. A Voronoi edge is a line segment that forms the boundary of each cell, located at the midpoint between the feature point of the cell and the closest feature point of the neighboring cell. A Voronoi vertex is the point where the Voronoi edges meet, formed at the intersection of three or more cells, determining the structure of the diagram (Fig. 3).

When a query point x̂_k is given, the closest Voronoi cell location Ø_S is found based on the distance between x̂_k and the feature points of the Voronoi cells (Eq. 6) (Edelsbrunner and Seidel, 1985).

(6)

∅S=argminSi∈S‖x^k−Si‖

Eq. 6 is used to find the Voronoi cell that minimizes the distance between the query point and the feature points with Ø_S indicating the location of the Voronoi cell containing the query point. Query points belonging to the same Voronoi cell can be expressed as follows (Eq. 7).

(7)

Ri={x^k∈Rd|∅S(x^k)=Si}

This characteristic of Voronoi diagrams effectively distinguishes the regions for all feature points and identifies to which Voronoi cell the query point belongs. Therefore, they are often used in algorithms for classification or clustering. On the other hand, this property can be used to determine the outliers using query points that effectively represent data trends. Query points are calculated by averaging observations from time-series data and moving forward to calculate the average at the next time point (Eq. 8).

(8)

x^k=Sk−n+1+Sk−n+2+⋯+Skn

Using these features, USBL data is segmented into polygonal regions with feature points, and the moving average filter is used as the query point to detect USBL outliers based on the constraint conditions of Eq. (9). x̂_k is the moving average value as the query point, and R_i is the Voronoi cell generated by the feature point S_i. The feature point is determined to be an inlier if the query point is located in the Voronoi cell of it. Fig. 4 shows the step-by-step process of the Voronoi diagram-based USBL outlier removal.

(9)

{Si=Inlierif x^k∈RiSi=Outlierif x^k∉Ri

2.3 Advanced Query Point Calculation

The moving average filter calculates the output by averaging the previous input data. As the window size increases, the output is computed as the average of the earlier values of the input data, which can introduce a delay. When concurrency is required, the time delay issue can be addressed by proposing an alternative calculation for the query point.

For example, the time delay of the query point can be mitigated using an exponentially weighted moving average (EWMA) filter (Eq. (10)).

(10)

x^′k=α⋅Sk+(1−α)⋅x^′k−1

The weight α can be adaptively applied by considering the difference between the current USBL measurement and the previous measurements (Eq. (11)).

(11)

α=1−‖Sk−Sk−1‖Kk

where, K_k is a value adaptively calculated by considering the UUV speed u_k, the USBL acquisition period T, and the expected error range ϵ of the USBL, as expressed in Eq. (12).

(12)

Kk=uk⋅T+∊

2.4 Sensor Fusion

A sensor fusion structure was designed to compare the results and verify the effectiveness of the proposed algorithm. The localization algorithm is based on the Kalman filter, which fuses sensor data to estimate the position of the AUV. A Federated Kalman filter structure was adopted and divided into a master filter and a local filter to ensure the convenience of sensor data fusion and the robustness of the DVL and IMU position data (Fig. 5) (Carlson, 1988). The local filter uses the values from the DVL and IMU to estimate the velocity in the global frame, which is then used by the master filter to estimate the final position. The proposed algorithm is applied for the USBL data filtering (Eq. (13)).

(13)

[x˙y˙uvφ]=[uk⋅cosφk−vk⋅sinφkuk⋅sinφk+vk⋅cosφkukvkφk]

The measurements in the local filter include the velocities u, v obtained from the DVL and the yaw angle ϕ obtained from the IMU. The state variables estimate the velocities ẋ, ẏ ̇in the global frame, the velocities u, v in the body-fixed frame, and the yaw angle ϕ. The observation variables are the velocities u, v from the DVL and the yaw angle from the IMU (Eq. (14)).

(14)

xIMU,DVLL=[x˙,y˙,u,v,φ]TZIMU,DVLL=[u,v,φ]T

The system model is expressed as Eq. (15), where wKL and vKL are assumed to follow a Gaussian distribution for the system noise and measurement noise, respectively (Eq. (15)).

System equation

xKL=g(xK-1)+wKL,wKL~N(0,QL2)

Measurement equation

(15)

ZKL=h(xK)+vKL,vKL~N(0,RL2)

The Jacobians of g(x_K₋₁) and h(x_K) are expressed in Eq. (16) because the system model of the local filter is nonlinear.

(16)

G=∂g∂X|xK−1|K−1HKL=∂h∂X|xK|K−1G=[02×2cosφk−sinφ2−(uk⋅sinφk+vk⋅cosφk)03×2sinφkcosφkuk⋅cosφk−vk⋅sinφkI3×3]HKL(IMU,DVL)=[03×2I3×3]vKL~N(0,RL2)

The state prediction and update stages are defined by Eq. (17).

Prediction step

xK|K-1L¯=g(xK-1)PK|K-1L¯=GK,K-1PK|KL^(GK,K-1)T+QKL

Correction step

(17)

KKL=PK|K−1L¯(HKL)T[HKLPK|K−1L¯(HKL)T+RL]−1xKL^=xK|K−1L¯KKL(ZKL−h(xk))PKL^=[I−KKLHKL]PK|K−1L¯

In the master filter, the USBL data and the results from the local filter are used as input to estimate the AUV position. The USBL data is preprocessed using the proposed algorithm to remove outliers before being input into the master filter, and the system model is expressed linearly. The state variables x, y, ẋ and ẏ were obtained from the USBL and the local filter, respectively, and the system model of the master filter is given by Eq. (18).

[xyx˙y˙]=[x+x˙⋅dty+y˙⋅dtx˙y˙]xUSBL,LoutputM=[x,y,x˙,y˙]TFKM=[I2×2I2×2⋅dt02×2I2×2]

System equation

xKM=FKxKM+wKM,wKM~N(0,QM2)

Measurement equation

(18)

ZKM=HKMxKM+vKM,vKM~N(0,RM2)

The acquisition period of the USBL and the output period of the local filter are different. Hence, the observation matrix is classified into two cases, as shown in Eq. (19) (Lee et al., 2017):

(a) Updating with preprocessed USBL data and local filter output
(b) Updating with only the local filter output

(19)

ZKM(USBL,Loutput)=[x,y,x˙,y˙]THKM(USBL,Loutput)=[I4×4]ZKM(Loutput)=[x˙,y˙]THKM(Loutput)=[02×2I2×2]

When the USBL measurement is not input to the master filter, the Kalman filter updates only with the velocity values from the local filter to estimate the AUV position. The results of the Kalman filter are updated when relatively accurate USBL data is input (Eq. (20)).

Prediction step

xK|K-1M¯=FK,K-1xK-1M¯PK|K-1M¯=FK,K-1PK-1M^(FK,K-1)T+QKM

Correction step

(20)

3. Experimental Result

3.1 Parameter Analysis

Experiments were conducted to verify the performance of the proposed algorithm and analyze the characteristics of the user parameters. The experimental data were obtained from the coastal waters of Jangil-ri in Pohang, with a depth of approximately three meters. A transceiver was installed on a coastal pontoon structure. A transponder was mounted on a vessel to acquire underwater sensor data along the planned route of the vessel (Fig. 6). The USBL used in the experiment was the S2C R 18/34 from Evologics, with main specifications including a frequency of 18–34 kHz, a distance error of 0.01m, and an azimuth error of 0.1 degrees.

Fig. 7 presents the USBL data obtained from the experiment. The blue, red, and green points represent the USBL data identified as inliers, outliers, and GPS data for comparison, respectively. The GPS and USBL data were compared using root mean square error (RMSE). The USBL data were upsampled and interpolated based on the GPS data to calculate the RMSE using GPS and USBL signals with different periods.

The algorithm was applied using the moving average to the query points of the Voronoi diagram, and the results were compared to find the optimal window size, which is a user parameter. The RMSE was smallest when the window size was 15 (Table 1). The number of outliers increased as the window size increased, and a delay in measurement values occurred. In contrast, the delay in measurement values decreased when the window size decreased, but the robustness against large errors, such as spike noise, tended to decline.

In addition, the results were compared when the algorithm was applied using the weighted moving average for the query points of the Voronoi diagram (Table 2). The weights used in the weighted moving average were calculated using Eq. (11), and the gain K_k in Eq. (11) was determined based on the expected error range ϵ of the USBL, which is a user parameter. The performance change of the algorithm according to variations in ϵ was analyzed, as shown in Table 2.

The experimental results indicated that the RMSE decreased as ϵ decreased. The impact of ϵ was analyzed by comparing the changes in weight α and the latest USBL values when ϵ was 0.2 (Fig. 8). The results showed that the calculated weight was small when there was a large difference between the latest USBL value and the previous value, and the calculated weight was large when there was a small difference, confirming that the weight was adaptively calculated. Hence, K_k increased as ϵ increased, which increased the weight of the latest USBL value, making it more affected by outliers with a large difference from the previous value, such as spike noise. On the other hand, smaller weights were assigned to large outliers as ϵ decreased, resulting in more robust results. In contrast, data that fell within the normal range were sometimes classified as outliers when ϵ was too small. When ϵ=0.2, the number of normal values improved compared to the case with a window size of 15 lists in Table 1.

The performance of the proposed algorithm was evaluated by comparing it with other outlier removal and noise reduction algorithms. The comparison groups included raw USBL data, Mahalanobis distance, moving average, and minimum covariance determinant (MCD) (Hubert and Debruyne, 2010). For meaningful outlier detection, the threshold for the Mahalanobis distance and MCD was set to apply the three-sigma rule, defining values more than three times the standard deviation as outliers.

The comparison results with the data obtained from the experiments showed that the RMSE improved by 4.05% for the Mahalanobis distance, 10.5%, 26.57%, and 29.84% for the sliding window, MCD, and the proposed method, respectively, compared to the raw USBL data (Table 3).

3.2 Performance Evaluation of Outlier Removal

This study used real-world data from the AUV Cyclops, developed by Pohang University of Science and Technology (POSTECH), to verify the effectiveness of the proposed algorithm on underwater navigation systems (Fig. 9). The AUV for data acquisition is a hovering type AUV, with its main specifications listed in Table 4 (Joe et al., 2021; Pyo et al., 2015). The data acquisition site was the coastal area of Jangil-ri in Guryongpo, Pohang.

The position acquisition sensors were an S2C/R 18/34 USBL from Evologics and a Workhorse Navigator DVL from Teledyne. The attitude data were obtained using the HG1700 from Honeywell.

The AUV Cyclops was operated in an area of 20 m × 15 m to acquire real-world data. The acquired data was plotted over the time axis in an offline environment using MATLAB (Fig. 10). The DR Nav of the DVL and IMU showed deviations from the USBL over time due to drift errors. Although the USBL data was free of drift errors, outliers were included in the sensor output.

The proposed method was used to remove the outliers from the USBL data. The results were compared with the raw USBL data and the DVL results (Fig. 11). The blue dotted line represents the raw USBL data, the black dashed line represents the dead reckoning (DR) results using DVL and IMU, and the red dotted line represents the results processed according to the proposed algorithm. Significant outliers, around 6m and 12m, occurred at points a and b. The proposed algorithm effectively detected and removed these outliers, resulting in improved USBL data.

Figs. 12 and 13 compare the performance of sensor fusion with and without the USBL outlier removal algorithm, respectively. The blue line represents the raw USBL data, and the black dashed line represents the dead reckoning results using DVL and IMU. The pink dots in Fig. 12 show the estimated position of the AUV when sensor fusion was performed without outlier removal. Fig. 13 shows the results using the USBL data after outlier removal using the proposed algorithm. When outliers were not removed, there were local path errors due to outliers (points (a), (b), and (c)), as shown in Fig. 12.

The outliers were removed using the proposed algorithm, as shown in Fig. 13. As a result, the extended Kalman filter (EKF) did not experience local path errors.

4. Conclusion

This study proposes a method for removing outliers from USBL data using a Voronoi diagram-based approach. The USBL data was segmented into regions based on the Voronoi diagram, and the outliers were identified by analyzing whether the weighted moving average query points, using adaptive weights, were within the Voronoi cell. Real-world data were collected using GPS and USBL to validate the performance of the proposed method, and the results were compared based on the adaptive weight parameters. In addition, the effectiveness of the proposed algorithm was verified by applying it to field test data obtained from an real AUV.

An AUV localization algorithm was designed with a federated Kalman filter structure to verify the improvement of results using the proposed method as a preprocessing step for sensor data fusion in AUV localization. This algorithm performed sensor fusion of USBL, DVL, and IMU data. The application to real-world data showed that removing outliers from USBL data using the proposed algorithm enabled more robust AUV localization. This approach enhances the reliability of USBL data and emphasizes the importance of raw data processing techniques in underwater navigation systems. Future research will focus on refining USBL-based AUV localization by analyzing and correcting the error caused by the oscillating transceiver on the ship and on applying the findings to develop cooperative systems between unmanned surface vehicles and AUVs.

Window size	Inlier	Outlier	Outlier proportion	RMSE (m)
5	43	16	27%	3.2655
10	35	24	40%	2.8592
15	36	23	39%	2.3236
20	34	25	42%	4.4192
25	29	30	51%	3.8119

∊	Inlier	Outlier	Outlier proportion	RMSE (m)
0.1	43	16	27%	3.0784
0.2	43	16	27%	2.5408
0.3	42	17	29%	2.7521
0.4	44	15	25%	3.4191
0.5	41	18	30%	3.3720
0.6	44	15	25%	3.6757

Method	RMSE (m)	Improvement rate (%)
Raw data	3.6217	-
Mahalanobis	3.4806	4.05
Moving Avg.	3.2775	10.5
MCD	2.6594	26.57
Ours	2.5408	29.84

Type	Hovering
Weight (kg)	210 (air)
Dimension (m)	0.9 × 1.5 × 0.9
Depth rating (m)	100
Propulsion (W)	475 (8 thrusters)
Max. speed (m/s)	1.029
Sensors	Doppler Velocity log
	Dual Frequency Imaging Sonar
	Ultra-short baseline
	Digital pressure transducer
	Video camera
	Fiber-optic gyro