Paper proposes a simultaneous localisation and mapping (SLAM) scheme which is applicable to small military unmanned underwater vehicles (UUVs). The SLAM is a process which enables concurrent estimation of the position of UUV and landmarks in the environment through which the vehicle is passing. An unscented Kalman filter (UKF) is utilised to develop a SLAM suitable to nonlinear motion of UUV. A range sonar is used as a sensor to collect the relative position information of the landmark in the environment in which the UUV is navigating. The proposed SLAM scheme was validated through towing tank experiments about two degrees of freedom motion with UUV motion simulator and real range sonar system for small UUV. The results of these experiments showed that proposed SLAM scheme is capable of estimating the position of the UUV and the surrounding objects under real underwater environment

Keywords:    Simultaneous localisation and mapping,  range sonar,  unmanned underwater vehicle,  unscented Kalman filter,  towing tank experiment

Autonomy of an unmanned underwater vehicle (UUV) allows the UUVs to be used in the many missions in military area¹. A precise navigation system of UUV is mandatory for mission success, because navigational error affects the quality of recorded data during the mission and the recovery of UUV. Inertial navigation system (INS), which is composed of the inertial measure unit (IMU) and auxiliary sensor such as Doppler velocity log (DVL) to prevent the growth of the navigational drift from IMU² has been widely used as navigation system of UUV for a long time. However, since navigational error is still accumulate, as mission time increases even if using DVL, it is necessary to reset the navigational drift by determining UUV’s position to external reference point³. The reset can usually be done by resurfacing for GPS, but this can be impossible if the mission of UUV is covert. One of the alternatives for resetting the drift is simultaneous localisation and mapping (SLAM) because SLAM can deduce the position of UUV from the map of underwater environment which is built up simultaneously during the mission⁴. Although any Bayesian estimation theory can be used in SLAM, many researches apply Kalman filter and its variations to SLAM⁵,⁶ because of low computational burden and optimality under Gaussian assumption. When the dynamics system is nonlinear, extended Kalman filter (EKF) can be used by using first Taylor series to approximate nonlinear system under assumption that system is locally linear. However, if dynamic system is highly nonlinear as like a lawn mowing motion of UUV in the mine countermeasure (MCM) mission, EKF can be unstable by broken locally linear assumption while unscented Kalman filter can be consistent through statistical linearisation instead of analytical linearisation⁷, ⁸.

The aim of this work is to implement the suitable SLAM into small UUV for MCM and other military missions. Thus SLAM in this work adopted UKF as an estimation method and range sonar system as a sensory system considering the nonlinear motion and payload of small UUV. The proposed SLAM was verified through experiments under towing tank conditions and the experimental results are presented.

.

The SLAM in this work is composed of 3 steps that are: state estimation, data association, and map management. The position of UUV and landmark is estimated simultaneously in the state estimation step. The decision about changing the system state by adding newly detected objects and deleting not detected landmark from system state is performed through data association step. Map management step manages the size of system state vector for efficient computation.

2.1 State Estimation

If the UUV is assumed to move in three degrees of freedom in the ocean with multiple objects located adjacent to where the vehicle is operating, the state of the vehicle can be written as:

$x_v={\left[\begin{array}{cccccc}{x}_v& y_v& {\text{z}}_v& {\psi }_v& {\theta }_v& V_v\end{array}\right]}^T$     (1)

where $x_v,y_v,z_v$ denote the positions in 3-D space; ${\psi }_v,{\theta }_v$ denote the heading and pitch angle; and $V_v$ denotes the total velocity. The state of M objects acquired by the sensor equipped with UUV are given by their positions:

$x_o={\left[\begin{array}{ccccccc}{x}_{o1}& y_{o1}& z_{o1}& \cdots & x_{oM}& y_{oM}& z_{oM}\end{array}\right]}^T$     (2)

Since system state vector in SLAM is defined as the combination of a vehicle state and an object’s state, the system state vector in authors work is as follows:

$x={\left[\begin{array}{cc}{x}_v^T& x_o^T\end{array}\right]}^T$     (3)

The motion of UUV is modelled with second kinematics model by a constant velocity with white noise acceleration⁹. The discrete time state equation at time k and k+1 with sampling time $\Delta T$ is described as:

$\begin{array}{l}{x}_v(k+1)=x_v(k)+V_v\cos ({\psi }_v+r)\cos ({\theta }_v+q)\Delta T+q_x(k)\\ y_v(k+1)=y_v(k)+V_v\sin ({\psi }_v+r)\cos ({\theta }_v+q)\Delta T+q_y(k)\\ z_v(k+1)=z_v(k)+V_v\sin ({\theta }_v+q)\Delta T+q_z(k)\\ {\psi }_v(k+1)={\psi }_v(k)+r+q_{\psi }(k)\\ {\theta }_v(k+1)={\theta }_v(k)+q+q_{\theta }(k)\\ V_v(k+1)=V_v(k)+q_V(k)\\ x_o(k+1)=x_o(k)\end{array}$     (4)

where $q_x(k),q_y(k),q_z(k),q_{\psi }(k),q_{\theta }(k),q_V(k)$ are the process noise.

The range sonar system with four channels is considered as a sensor system for taking a measurement of the relative location between landmark and UUV. Fig 1. shows the concepts of measurement model.

The measurement model for four channel range sonar system at time k+1 is described as:

$z(k+1)=\left[\begin{array}{c}{z}_1(k+1)\\ z_2(k+1)\\ z_3(k+1)\\ z_4(k+1)\end{array}\right]+\left[\begin{array}{c}{w}_1(k+1)\\ w_2(k+1)\\ w_3(k+1)\\ w_4(k+1)\end{array}\right]$      (5)

The subscript in Eqn (5) denotes the corresponding direction in the range sonar system as shown in Fig 1.. The measurement model for single objects detected by i^th range sonar at time k+1 is described as:

$z_i(k+1)=\left[\begin{array}{c}{r}_i(k+1)\\ {\alpha }_i(k+1)\\ {\beta }_i(k+1)\end{array}\right]+w_i(k+1)=h_i(x(k+1))+w_i(k+1)$      (6)

where $w_i(k+1)$ represents the measurement noises of the sonar, which are assumed to have a zero mean while Gaussian noise with the covariance dependent on the sonar specifications. $z_i$ is composed of range $r_i(k+1)$ , angle rotated from z axis in ${\alpha }_i(k+1)$ , and angle rotated from y axis in in the ${\beta }_i(k+1)$ Fig 1.. The $h_i(k+1)$ in Eqn (6) can be described as a function of the UUV state and objects state as:

$h_i(x(k+1))=\left[\begin{array}{c}\sqrt{\left(\begin{array}{l}{(}^{x_v}+(y_v(k+1)-\\ y_{om}(k+1){)}^2+{(}^{z_v}\end{array}\right)}\\ \arctan \frac{y_v(k+1)-y_{om}(k+1)}{x_v(k+1)-x_{om}(k+1)}-{\psi }_v\\ \arctan \frac{z_v(k+1)-z_{om}(k+1)}{x_v(k+1)-x_{om}(k+1)}-{\varphi }_v\end{array}\right]$     (7)

Unscented Kalman filter estimates the system state described in Eqn (3) using Eqns (4) ~ (7). The UKF follows the process of Kalman filter to estimate using the sigma points and the unscented transform. The detailed process of UKF is found in reference⁷.

2.2 Data Association and Map Management

Even though data association and map management are important problem in SLAM, the established methods about data association and map management were used in this work because purpose of this work is implementation of the SLAM into small military UUVs. In data association step, the detection of new object determines the nearest neighbourhood standard filter (NNSF)⁹. Measurements that do not correspond to existing objects through NNSF are considered as new objects, and a new object is added to the existing system state vector and covariance matrix using the stochastic map. As a number of newly detected objects are added, size of system state vector and its covariance matrix increases and the computational burden grows. In this work, the local submap method¹⁰ is considered as map management method and divided the environment in which the UUV is navigating into several local submaps with the full covariance method to estimate the positions of the UUV and landmark in a single submap.

Several experiments under towing tank conditions with a length of 120 m, breadth of 8 m, and depth of 3.5 m using an instrument (Fig 2.) to simulate two degrees of freedom motion and real range sonar system for small UUV (Fig. 2(b)) are carried out for the verification of proposed method.

Specifications of the range sonar system are summarised in Table 1. and experimental conditions are shown in Table 2.. Two cylindrical and two cubic shaped steel objects were placed as a discrete landmark in towing tank for the verification of data association function.

Experimental results of mapping of wall of the tank and localising of UUV are presented in Fig. 3(a) and the comparisons between locations were found with proposed method. The real location of UUV is shown inFig. 3(b) . Comparing the position of the mapped wall shown in Fig. 3(a) and the towing tank dimension, it is clearly shown that proposed method can map detected objects properly. The data in Fig. 3(b) show that the proposed method can accurately localise the UUV position under two degrees of freedom motion.

Fig. 4 presented location error and one wall position mapping error about the y-axis for experimental results. Since a mapping error is influenced by localisation error from characteristics of SLAM, the positive localisation error induces the positive mapping error for velocity 0.1 m/s and the negative localisation error does the negative mapping error for 0.3 m/s as shown in Fig. 4. The location and mapping error in Fig. 4 can be increased as heading measurement increase even if heading error is small because of the combination of heading and velocity in Eqn (4). From the RMSEs in Table 3. and the fact that errors in Fig. 4 were not diverging, it is shown that proposed method can be implemented in the system to map the wall of tank and localise the real UUV position and be expected to provide the reference position for resetting navigational drift.

Paper presents a SLAM method which is suitable for a small military UUV. The proposed method adopts an unscented Kalman filter in estimation step for a nonlinear motion model of UUV, the nearest neighbourhood standard filter in data association step and the local submap method in map management step. The proposed method was tested through the towing tank experiments. Experiment results presents that the proposed method can map the wall of tank and locate the position of UUV under two degrees of freedom motion wrt the estimation function for nonlinear motion and real time calculation capability. Future works will validate the capability of proposed SLAM through experiments with real UUV under three degrees of freedom motion.

1. Christopher von A. REMUS 100 transportable mine countermeasure package. In Proceedings of Oceans 2003. San Diego, California, USA, September 2003. pp. 1925-930.

2. Stutters, L.; Liu, H.; Tiltman, C.; & Brown, D.J. Navigation technologies for autonomous underwater vehicles. IEEE Trans. Syst., Man, Cybernetics-Pt C: Appli. Rev., 2008, 38(4), 581-89.

3. Lee, C.; Lee, P.; Hong, S.; Kim, S. & Seong, W. Underwater navigation system based on inertial sensor and Doppler velocity log using indirect feedback Kalman filter. Int. J. Offshore Polar Eng., 2005, 15(2), 88-95.

4. Aulinas, J.; Petillot, Y.; Salvi, J. & Llado, X. The SLAM problem: survey. In Artificial intelligence and application : Proceedings of the 11th International Conference of the Catalan Association for Artificial Intelligence, edited by Teresa Alsinet, Josep Puyol - Gruart, and Catme Torras, IOS Press, 2008, 184, pp. 363-71.

5. Ribas, D.; Ridao, P.; Tardos, J.D. & Neira, J. Underwater SLAM in man-made structured environments. J. Field Robotics, 2008, 25(11-12), 898-921.

6. Walter, M.; Hover, F. & Leonard, J. SLAM for ship hull inspection using exactly sparse extended information filters. In Proceeding of IEEE ICRA 2008, Pasadena, California, US, May 2008. pp. 1463-1470.

7. Julier, S.J. & Uhlmann, J.K. Unscented filtering and nonlinear estimation. Proceedings IEEE, 2004, 92(3), 401-22.

8. Zhou, W.; Zhao, C. & Guo, J. The study of improving Kalman filter family for nonlinear SLAM. J. Intelligent Robotic Syst., 2009, 56(5), 542-64.

9. Bar-Shalom, Y. & Fortmann, T. Tracking and data Association. Academic Press, Orlando, Florida, USA, 1988. 353 p.

10. Kim, S.J. Efficient simultaneous localisation and mapping algorithm using submap networks. MIT, Cambridge, Massachusetts, USA, 2004. PhD Thesis