Persistent Localization of Autonomous Underwater Vehicles Using Visual Perception of Artificial Landmarks

Abstract

Persistent localization is a critical requirement for autonomous underwater vehicles (AUVs) engaged in long-term missions. Conventional dead-reckoning (DR) methods for estimating the position and orientation of AUVs often suffer from drift, necessitating additional information to correct accumulating errors. In this paper, we propose a visual artificial landmarks-based simultaneous localization and mapping (SLAM) system for AUVs. This system utilizes two types of underwater artificial landmarks that are observed using forward and downward-looking cameras. The information obtained from these detected landmarks, along with incremental DR estimates, is integrated within a framework based on the extended Kalman filter (EKF) SLAM approach, allowing for the recursive estimation of both the robot and the landmark states. We implemented the proposed visual SLAM method using our yShark II AUV and conducted experiments in an engineering basin to validate its effectiveness. A ceiling vision-based reference pose acquisition system was installed, facilitating a comparison between the pose estimation results obtained from DR and those derived from the SLAM method.

1. Introduction

Precise localization of autonomous underwater vehicles (AUVs) in an underwater environment is challenging but essential for the successful accomplishment of various tasks assigned to AUVs. Typical sensor modalities used in AUV localization include the absolute heading reference system (AHRS), the Doppler velocity log (DVL), and depth sensors [1,2]. With these sensors, the AUV can estimate its pose state through the dead reckoning (DR); where vehicle kinematic or dynamic models are typically exploited [3]. However, localization with DR suffers from accumulating errors because it integrates derivative measurements that contain random and systematic noise [4]. In particular, heading estimation by integrating gyroscope measurements is inherently prone to drift errors, which become more severe in nonlinear motions, such as rotations with varying angular velocities. This accumulation of drift can lead to significant deviations between the estimated trajectory and the actual path. To ensure the successful long-term operation of AUVs, it is essential to mitigate this error.

To improve positioning accuracy, additional aids must be integrated with the vehicles. A common approach involves using exterior sensors such as ultrashort baseline (USBL) [5,6] or long baseline (LBL) [7,8] systems. However, the USBL method requires support from the mother ship for the USBL transceiver, and there is an acoustic delay in the position information received. In contrast, the LBL method requires the placement of acoustic beacons on the seafloor, which can limit coverage to fixed areas. Therefore, these approaches are not adequate for the long-term operations of AUVs in deep-sea environments.

AUVs are typically equipped with one or more optical cameras to obtain visual information about underwater environments. By integrating an active lighting system, visual observations can also be employed to determine the pose information of the AUV. In our previous studies [9,10,11], we validated that observing artificial landmarks can improve the accuracy of the AUV localization. However, the assumption that the location of the landmarks should be provided prior to the vehicle deployment limits the generality of the method. The AUV should be capable of creating a map from scratch and identifying the locations of various landmarks, while simultaneously using this information to correct its position. This process is known as simultaneous localization and mapping (SLAM), a well-defined topic extensively studied in robotics [12,13,14] and successfully applied in AUV navigation [15,16,17]. The extended Kalman filter (EKF), unscented Kalman filter (UKF) [18], particle filter [19], and graph optimization [20] are commonly used methods for addressing SLAM (Simultaneous Localization and Mapping) problems. Among these, the EKF has the advantage of being computationally less demanding, especially when dealing with a finite number of landmarks, as is the case in our study. In this paper, we present an EKF-based SLAM method that incorporates visual measurements from three types of artificial landmarks captured by forward- and downward-looking cameras. We designed our SLAM framework to incorporate the observations from each landmark independently, along with a kinematics-based DR estimate. The highlights of this paper are as follows.

• We proposed a visual navigation system that uses artificial landmarks detected by forward- and downward-looking cameras.
• Visual information was integrated into an EKF-based SLAM framework.
• Experiments conducted with our AUV in an engineering basin demonstrated improved pose estimation compared to dead reckoning methods.
• Ground truth pose data were obtained using a ceiling-mounted vision-based reference system.

This paper expands on the conference paper previously presented [21].

The remainder of this paper is organized as follows. The design of artificial landmarks is presented in Section 2. In Section 3, we present the formulation of the proposed visual SLAM, followed by the experimental setup and results in Section 4. Finally, the conclusions are provided in Section 5.

2. Design of Landmarks

We designed three types of artificial landmarks, as shown in Figure 1. The first type is cylindrical landmarks. This one is designed for viewpoint invariant observations, as shown in Figure 2a. When the robot and landmarks are in a similar depth, basic geometric shapes (circle, triangle, etc.) can be seen in any direction from the forward-looking camera. Supervised learning with the random forest was used in the detection of the landmarks. Underwater images of these landmarks under various lighting conditions were used for the training of the ensemble of trees [9]. When the landmarks are detected in the current forward-looking image, conventional template matching with adaptive thresholding is performed to classify the shape of the landmarks, which outputs identification (ID) of the landmark.

The second one is tag-type landmarks, as shown in Figure 2b. This one is detected by the down-looking camera. A predefined 2D bar code is printed on these landmarks, allowing a unique ID for each tag. In this paper, AprilTag [22] was used. The advantage of this landmark is that we can obtain a 6 degree-of-freedom (DOF) pose of the camera from a single image. For underwater usage, the tags were coated with water-proof film.

As a third type of landmark, we installed steel panels cluttered with various objects such as rope, plastic pipe, small buoy, etc. In order to detect visual features of these objects, SURF [23] was used in this study. Any other feature extraction algorithms [24,25,26,27,28,29] can be used without the loss of generality.

3. SLAM Framework

Figure 3 illustrates the overall framework of the proposed visual SLAM system. We have modules for AUV dead-reckoning, image processing, data association, and the SLAM core. In this work, EKF-SLAM [12] is utilized as the SLAM core. Although we have chosen the EKF, other SLAM methods could also be applied without loss of generality, as the same motion and observation models can be utilized. For instance, when implementing graph optimization-based SLAM, information from the dead-reckoning and image processing blocks can create constraints in the front end, while optimization in the back end can update the entire trajectories.

We first define the state variables related to the localization and mapping problems. The robot state {${\mathbf{x}}_t$} is defined as SE(3) pose as

$${\mathbf{x}}_t={\left[\begin{array}{c}x,y,z,q_x,q_y,q_z,q_w\end{array}\right]}^{\top },t=0,1,\dots$$

(1)

where $\mathbf{q}={[q_x,q_y,q_z,q_w]}^{\top }$ is a quaternion that represents the vehicle’s orientation. The map is then defined as a vector describing the state of each landmarks as follows:

$$\begin{array}{ccc}\hfill \mathbf{m}& =& \left[\begin{array}{c}{\mathbf{m}}^{\mathrm{tag}}\\ {\mathbf{m}}^{\mathrm{cyl}}\\ {\mathbf{m}}^{\mathrm{obj}}\end{array}\right]\hfill \end{array}$$

(2)

$$\begin{array}{ccc}& =& {\left[\begin{array}{c}{\mathbf{m}}_1^{\mathrm{tag}\top },\dots ,{\mathbf{m}}_{N_t}^{\mathrm{tag}\top }|{\mathbf{m}}_1^{\mathrm{cyl}\top },\dots ,{\mathbf{m}}_{N_c}^{\mathrm{cyl}\top }|{\mathbf{m}}_1^{\mathrm{obj}\top },\dots ,{\mathbf{m}}_{N_f}^{\mathrm{obj}\top }\end{array}\right]}^{\top }\hfill \end{array}$$

(3)

where ${\mathbf{m}}_i^{\mathrm{tag}}\in \mathbf{SE}\left(3\right)$, ${\mathbf{m}}_i^{\mathrm{cyl}}\in {\mathbb{R}}^3$, and ${\mathbf{m}}_i^{\mathrm{obj}}\in {\mathbb{R}}^3$ denote the state of the tag, cylindrical, and feature landmarks, respectively, and $N_t$, $N_c$, and $N_f$ denote the total number of the corresponding landmarks registered in a state vector at time step t. We assume that the landmarks are static during the AUV’s operation. The measurement ${\mathbf{z}}_t$ then can be defined as follows:

$$\begin{array}{ccc}\hfill {\mathbf{z}}_t& =& \left[\begin{array}{c}{\mathbf{z}}^{\mathrm{tag}}\\ {\mathbf{z}}^{\mathrm{cyl}}\\ {\mathbf{z}}^{\mathrm{obj}}\end{array}\right]\hfill \end{array}$$

(4)

$$\begin{array}{ccc}\hfill {\mathbf{z}}_t^{\mathrm{tag}}& =& {\left[\begin{array}{c}ID,x,y,z,q_x,q_y,q_z,q_w\end{array}\right]}^{\top }\hfill \end{array}$$

(5)

$$\begin{array}{ccc}\hfill {\mathbf{z}}_t^{\mathrm{cyl}}& =& {\left[\begin{array}{c}ID,d,\theta \end{array}\right]}^{\top }\hfill \end{array}$$

(6)

$$\begin{array}{ccc}\hfill {\mathbf{z}}_t^{\mathrm{obj}}& =& {\left[\begin{array}{c}ID,\theta \end{array}\right]}^{\top }\hfill \end{array}$$

(7)

where ${\mathbf{z}}_t^{\mathrm{tag}}$, ${\mathbf{z}}_t^{\mathrm{cyl}}$, and ${\mathbf{z}}_t^{\mathrm{obj}}$ denote the measurement for the tag, cylindrical, and feature landmarks, respectively, and $\theta$ denotes the bearing of the detected landmark. d is the distance from the forward camera to the cylindrical landmark, which is calculated as

$$d=f\sqrt{{\displaystyle \frac{A_c}{A_p}}}$$

(8)

where f denotes the focal length of the forward looking camera, $A_c$ is the actual cross section area of the observed cylindrical landmark, and $A_p$ is the pixel area of the landmark in the camera image. Finally, the augmented state variable ${\mathbf{y}}_t$ is defined as follows.

$${\mathbf{y}}_t=\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{m}}_t\end{array}\right].$$

(9)

3.1. SLAM Formulation

In the EKF SLAM framework, the SLAM procedure consists of two main steps: prediction and measurement update. These steps are executed recursively after the state initialization.

3.1.1. Prediction

In the prediction step, the state of the previous timestep is transformed into the next timestep using the state transition model $\mathbf{f}({\mathbf{y}}_{t-1},{\mathbf{u}}_t)$. We employed the AUV’s kinematics model to describe the transition as follows.

$$\begin{array}{ccc}\hfill {\overline{\mathbf{y}}}_t& =& \mathbf{f}({\mathbf{y}}_{t-1},{\mathbf{u}}_t)\hfill \\ & =& \left[\begin{array}{c}{x}_{t-1}+({\nu }_{x,t}^{\mathrm{DVL}}cos{\psi }_t-{\nu }_{y,t}^{\mathrm{DVL}}sin{\psi }_t)\Delta t\\ y_{t-1}+({\nu }_{x,t}^{\mathrm{DVL}}sin{\psi }_t+{\nu }_{y,t}^{\mathrm{DVL}}cos{\psi }_t)\Delta t\\ z_t^{\mathrm{d}}\\ q_{x,t}^{\mathrm{AHRS}}\\ q_{y,t}^{\mathrm{AHRS}}\\ q_{z,t}^{\mathrm{AHRS}}\\ q_{w,t}^{\mathrm{AHRS}}\\ {\mathbf{m}}_{t-1}\end{array}\right]\hfill \end{array}$$

(10)

where ${\mathbf{u}}_t={[{\nu }_{x,t}^{\mathrm{DVL}},{\nu }_{y,t}^{\mathrm{DVL}},z_t^{\mathrm{d}},q_{x,t}^{\mathrm{AHRS}},q_{y,t}^{\mathrm{AHRS}},q_{z,t}^{\mathrm{AHRS}},q_{w,t}^{\mathrm{AHRS}}]}^{\top }$ denotes the control input from the DVL, depth sensor, and AHRS with a noise covariance ${\mathbf{Q}}_t$, $\Delta t$ denotes the time interval between the current and previous time step, and ${\psi }_t$ denotes the heading angle of the AUV. In (10), only the robot state undergoes the state transition and thus is updated. The map states remain unchanged in the prediction step. The error state covariance ${\mathbf{P}}_t$ is also updated in the prediction step as follows.

$$\begin{array}{c}\hfill {\overline{\mathbf{P}}}_t={\mathbf{F}}_t{\mathbf{P}}_{t-1}{\mathbf{F}}_t+{\mathbf{Q}}_t\end{array}$$

(11)

where ${\mathbf{F}}_t$ is the Jacobian of the state transition model.

3.1.2. Measurement Update

In the measurement update step, both the robot state and the map states are updated according to the measurement model $\mathbf{h}\left({\mathbf{y}}_t\right)$. As explained in the (5)–(7), the measurement ${\mathbf{z}}_t$ is represented in a sensor-fixed frame. The predicted measurement ${\widehat{\mathbf{z}}}_t$ can be modeled as follows.

$$\begin{array}{ccc}\hfill {\widehat{\mathbf{z}}}_t& =& \mathbf{h}\left({\overline{\mathbf{y}}}_t\right)\hfill \end{array}$$

(12)

$$\begin{array}{ccc}& =& \left[\begin{array}{c}{\widehat{\mathbf{z}}}_t^{\mathrm{tag}}\\ {\widehat{\mathbf{z}}}_t^{\mathrm{cyl}}\\ {\widehat{\mathbf{z}}}_t^{\mathrm{obj}}\end{array}\right]\hfill \end{array}$$

(13)

$$\begin{array}{ccc}\hfill {\widehat{\mathbf{z}}}_t^{\mathrm{tag}}& =& \mathbf{h}({\overline{\mathbf{x}}}_t,{\overline{\mathbf{m}}}_t^{\mathrm{tag}})\hfill \end{array}$$

(14)

$$\begin{array}{ccc}\hfill {\widehat{\mathbf{z}}}_t^{\mathrm{cyl}}& =& \mathbf{h}({\overline{\mathbf{x}}}_t,{\overline{\mathbf{m}}}_t^{\mathrm{cyl}})\hfill \end{array}$$

(15)

$$\begin{array}{ccc}\hfill {\widehat{\mathbf{z}}}_t^{\mathrm{obj}}& =& \mathbf{h}({\overline{\mathbf{x}}}_t,{\overline{\mathbf{m}}}_t^{\mathrm{obj}})\hfill \end{array}$$

(16)

Update of the vehicle and map states are performed by the Kalman gain calculated from the Jacobian ${\mathbf{H}}_{\mathbf{m}}$ of the observation models (14)–(16) as follows.

$$\begin{array}{ccc}\hfill {\mathbf{H}}_{\mathbf{m}}& =& {\displaystyle \frac{\partial \mathbf{h}\left({\mathbf{y}}_t\right)}{\partial \mathbf{m}}}\hfill \\ & =& \left[\begin{array}{ccc}{\mathbf{H}}_{\mathbf{m}}^{\mathrm{tag}}& {\mathbf{H}}_{\mathbf{m}}^{\mathrm{cyl}}& {\mathbf{H}}_{\mathbf{m}}^{\mathrm{obj}}\end{array}\right]\hfill \end{array}$$

(17)

where ${\mathbf{H}}_{\mathbf{m}}^{\mathrm{tag}}$, ${\mathbf{H}}_{\mathbf{m}}^{\mathrm{cyl}}$, and ${\mathbf{H}}_{\mathbf{m}}^{\mathrm{obj}}$ denote the Jacobian matrix corresponding to the each landmark state. The Kalman update is performed as follows:

$$\begin{array}{ccc}\hfill {\mathbf{S}}_t& =& {\mathbf{H}}_{\mathbf{m}}{\overline{\mathbf{P}}}_t{\mathbf{H}}_{\mathbf{m}}^T+{\mathbf{R}}_t\hfill \end{array}$$

(18)

$$\begin{array}{ccc}\hfill {\mathbf{K}}_t& =& {\overline{\mathbf{P}}}_t{\mathbf{H}}_{\mathbf{m}}^T{\mathbf{S}}_t^{-1}\hfill \end{array}$$

(19)

$$\begin{array}{ccc}\hfill {\mathbf{y}}_t& =& {\overline{\mathbf{y}}}_t+{\mathbf{K}}_t({\mathbf{z}}_t-{\widehat{\mathbf{z}}}_t)\hfill \end{array}$$

(20)

$$\begin{array}{ccc}\hfill {\mathbf{P}}_t& =& {\overline{\mathbf{P}}}_t-{\mathbf{K}}_t{\mathbf{S}}_t{\mathbf{K}}_t^T\hfill \end{array}$$

(21)

where ${\mathbf{R}}_t$ denotes the sensor noise covariance, ${\mathbf{S}}_t$ denotes the innovation covariance, and ${\mathbf{K}}_t$ denotes the Kalman gain.

4. Experiments

4.1. Experimental Setup

Experiments were conducted in a basin environment to validate the proposed SLAM method. Figure 4 illustrates the setup of the landmarks and path plans for the robot during the tests. We installed 4 tag-type landmarks and 3 cylindrical landmarks within the basin. Additionally, we placed two panels with various objects attached to serve as visual features. The yShark II, an AUV developed by the Korea Research Institute of Ships and Ocean Engineering (KRISO), was utilized in this experiment. The robot moved along the square (5 m by 5 m) path 4 times. Tag-type landmarks were observed from a down-looking camera (PointGrey Grasshopper3), and object panels and cylinder-type landmarks were observed from a forward-looking camera with the same model. Navigation sensors including the AHRS (Microstrain 3DM-GX3-25), DVL (LinkQuest NavQuest600 Micro), and depth sensor (Paroscientific Digiquartz) were used in the calculation of the DR estimation and SLAM prediction. All software for data acquisition, landmark detection, DR, SLAM, and vehicle control was developed in-house in C++.

Figure 5 illustrates our testing setup, located in an engineering basin at Changwon National University.

We implemented a ceiling-mounted vision-based reference acquisition system (VRS) equipped with a wide field of view ($f=2.8$ mm) camera and attached one large AprilTag on the AUV’s back. In the ground station, remote connections to the AUV’s internal PC and the VRS were set through the fiber-optic communication and power-over-ethernet, respectively. The DR and SLAM algorithms were run online and all the sensor data were also saved for the offline evaluations. In the offline evaluation, To generate extreme navigation scenarios, we added synthetic random noises to the DVL and AHRS measurements during the 3rd and 4th loops. Gaussian noise with standard deviation $\sigma$ = [0.5 m, 10°] were periodically (once per 100 visual measurements) added to the translation and rotation states of the vehicle, respectively. Total 5 offline datasets were generated in this way.

4.2. Results

Examples of the landmark detection results are shown in Figure 6. For cylindrical landmarks, the detected bounding box and its center position are used to construct the measurement vector; $A_p$ in (8) is calculated as the area of the bounding box; the bearing $\theta$ in (6) is calculated with the center position. All three types of cylindrical landmarks were successfully detected in our test. Related to the tag landmarks, only 3 to 5 subsequent frames contained the tag images when the AUV moved over the landmark; however, this was sufficient for the update and convergence of the vehicle and map states. Lastly, feature-based landmarks showed that the extracted visual features were dominant around the frame structures, rather than the small objects. This is due to the reflection of the steel frame by the AUV’s lighting system.

Localization results from one of the noisy datasets are shown in Figure 7. During the first two loops, the SLAM registered every new landmark detected from the forward and down-looking cameras. Because the error covariance increases during the prediction step (11), the covariance ellipse of the first detected landmark (tag-type) had the smallest value; and this contributed to the significant update whenever a large loop-closure occurred. The covariance for the cylindrical landmarks had an elliptical shape at first (Figure 7b), but converged to the circular shape after a few observations (Figure 7c), as the depth estimation described in (8) are available for this type of landmarks. In contrast, the covariance ellipse for the feature landmarks always had a narrow shape as the bearing information was only available. As the DR also has a good short-term performance, its trajectory is similar to the SLAM results until the second loop (Figure 7b). However, when synthetic noise was added from the 3rd loop, the DR estimate started to diverge with accumulating errors (Figure 7c); while the SLAM trajectory recovered from the erroneous state owing to the information from artificial landmarks.

Results for all 5 datasets are shown in Figure 8. We applied different random noises for each dataset. Estimated trajectories and the absolute trajectory errors in Figure 8f support the superiority of the proposed SLAM method, which maintained the bounded errors throughout the entire trajectories.

Table 1. Summary of Absolute Trajectory Errors.

Dataset	RMSE (m)		Max. Error (m)
	DR-Only	SLAM	DR-Only	SLAM
1	1.26	0.68	2.52	1.21
2	1.11	0.66	2.22	1.30
3	1.02	0.67	2.04	1.26
4	1.23	0.64	2.04	1.07
5	1.07	0.69	1.85	1.19
Mean	1.14	0.67	2.13	1.20

summarizes absolute trajectory errors for DR and SLAM. The RMSE of the SLAM trajectory was 0.67 m for 92 m paths. This improved the positioning error of DR by 41.2%. Similarly, the maximum error was reduced by 43.6% when we used the SLAM.

4.3. Discussions

We can verify how each type of landmark contributes to the reduction of position error by examining the ’jumps’ in the AUV trajectories. This analysis is possible because the layout of the landmarks in the experimental setup typically allows for the detection of one type of landmark at a time. It is clear that tag-type landmarks contribute significantly to pose correction, as illustrated in Figure 7c. The influence of a cylindrical landmark is also evident in Figure 7e, where the green circle in the lower right subplot indicates the detected landmark, causing the SLAM pose to jump onto the correct path. The contribution of object-clustered landmarks is harder to explain explicitly; however, we can infer their impact from the fact that the SLAM paths near the object panels exhibit the smallest errors in all cases, as shown in Figure 8.

While the experimental results are promising, there are limitations to consider. The validation was conducted in a controlled basin environment, meaning that the visibility of landmarks may be more challenging in real-world field conditions. To address this issue, deep learning-based image enhancement techniques [30] or multi-modal markers [31] that can be detected by both optical and sonar cameras could be utilized.

5. Conclusions

In this study, visual SLAM for AUVs using artificial landmarks has been investigated. Three types of artificial landmarks were proposed to be used with forward and down-looking cameras available in our AUV. We implemented a conventional EKF-based SLAM framework incorporating a kinematic model of the AUV and landmark observation models. We obtained experimental data from the tests conducted in a basin and validated the performance of the proposed method for the persistent localization scenario. We expect our method to be applied in the area of long-term AUV operations, such as valve-turning in deep-sea platforms or manipulation of nuclear waste in nuclear power plants.