Table of Contents Author Guidelines Submit a Manuscript
Scientific Programming
Volume 2017 (2017), Article ID 7136702, 10 pages
Research Article

Underwater Matching Correction Navigation Based on Geometric Features Using Sonar Point Cloud Data

1Robotics Institute, Beijing University of Aeronautics and Astronautics, XueYuan Road No. 37, HaiDian District, Beijing 100191, China
2State Key Lab of Intelligent Technology and Systems, Tsinghua National Laboratory for Information Science and Technology, Department of Computer Science and Technology, Tsinghua University, HaiDian District, Beijing 100083, China

Correspondence should be addressed to Mingjie Dong

Received 11 September 2016; Accepted 1 December 2016; Published 4 January 2017

Academic Editor: Wenbing Zhao

Copyright © 2017 Mingjie Dong et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.


In order to localize the Remotely Operated Vehicle (ROV) accurately in the reactor pool of the nuclear power plant, an underwater matching correction navigation algorithm based on geometric features using sonar point cloud data is proposed. At first, an Extended Kalman Filter (EKF) is used to compensate the motion induced distortion after the preprocessing of the sonar point cloud data. Then, the adjacent scanning point cloud data are fitted to be four different straight lines using Hough Transform and least square method. After that, the adjacent straight line is modified based on geometric features to get a standard rectangle. Since the working environment of the ROV is a rectangular shape with all dimensions known, it is used as a priori map. The matching rectangle is then used to compare with the a priori map to calculate the accurate position and orientation of the ROV. The obtained result is then applied as the measurement for the second EKF to obtain better localization accuracy. Experiments have been conducted in man-made water tank which is similar to the reactor pool of the nuclear power plant, and the results successfully verify the effectiveness of the proposed algorithm.

1. Introduction

Nuclear energy has become an important energy source worldwide and the uncertainty of human factors must be a minimum to ensure the safety. That demands high precision robots to substitute human to detect the nuclear power plant and complete the corresponding operation which was done by human workers before. Many robots for nuclear environment have been developed during the last decades. ROV is just one of them. However, ROV needs to have higher reliability as it is used in the reactor pool of the nuclear power plants and often works for more than two hours. So, it is of vital importance to have the ability to get absolute navigation with higher accuracy, especially when it works for a longer time in the nuclear environment.

However, navigation of underwater environment is challenging because GPS is not available due to the rapid attenuation of electromagnetic waves in water [1]. Active acoustic positioning systems such as long baseline (LBL), short baseline (SBL), and ultra-short baseline (USBL) are good alternatives to GPS for correcting dead reckoning errors [2]. The disadvantage of such a technique is that the deployment, calibration, recovery of the transponders, and the relatively small area of coverage consumes costly ship time and complicates the operations [3, 4]. Many researchers have used vision sensors to carry out underwater visual simultaneous localization and mapping (SLAM). The movement of underwater vehicle can be estimated from the displacement of features in the images grabbed by vision sensors and the registered images are combined to produce photomosaic of the traveled area at the same time [5, 6]. The disadvantage is that vision is limited to a few meters under the water and can be easily disturbed by turbulence, floating sediment, or lighting conditions. On the other hand, the multimodality fusion has attracted much attention from the academics and industry [710].

In recent years, sonar has been a very popular tool for underwater navigation. The acoustic sonar frequencies can penetrate further the water column and are not prone to turbidity; thus sonar can provide information even in bad visibility conditions [11]. Many underwater SLAM navigation methods with respect to sonar have been proposed during the last decades. Paper [12] proposed a method that is based on the probabilistic iterative correspondence (pIC) algorithm but takes into account the distortions in the acoustic image to deal with data gathered by an underwater vehicle utilizing mechanically scanned imaging sonar (MSIS). The underwater sonar probabilistic iterative correspondence () is proposed in [13] to deal with the significant uncertainty in the measurements or large scan time through adopting probabilistic scan matching strategy and defining a method to strongly alleviate the motion induced distortion. The result shows some improvements in the pose estimate. Meanwhile, the modified-FastSLAM algorithm is proposed and used in the navigation for an open-frame AUV research platform [14]. A novel localization algorithm for an AUV equipped with MSIS is proposed which incrementally constructs a pose graph and conducts graph optimization to correct the robot poses and, especially, the data association algorithm based on Mahalanobis distance and shape matching is deployed to determine loop closures, leading to associated scan pairs used for calculating constraints of the pose graph. The experimental results show that the algorithm outperforms traditional algorithms such as dead reckoning and in terms of both localization and mapping accuracy [15].

In this paper, we propose the underwater matching correction navigation based on geometric features using sonar point cloud data. The designed and manufactured ROV prototype is shown in Figure 1. Just as described in our previous paper [16], the ROV is radiation proof under the certain dose rate, and the basic components contain control cabinet, buoyancy module, propeller, camera, manipulator, sonar, and so on. The main body frame is made of aluminum alloys to provide sufficient strength and resist the acidic corrosion of the reactor pool of the nuclear power plant. The control cabinet which is made of thick stainless steel is installed inside the main body frame and the double seal processing had been done to protect the control system inside from water leakage. The ROV is designed neutral buoyancy and can move in the direction of surge, sway, heave, and yaw. Besides, the ROV is equipped with a variety of sensors, such as sonar, depth gauge, three-axis accelerometer, three-axis gyroscope, and three-axis magnetometer, which are used to get the position and attitude of the ROV. A fuzzy PID controller is used to realize the depth control of the ROV, and field experiment shows that the ROV can suspend at any user-specified depth under the water, which means that the ROV can work efficiently and stably in the nuclear reactor pool. In the latter paper, the ROV is assumed to be operating solely in a planar field based on the depth control.

Figure 1: Prototype of the ROV.

The paper is organized as follows. Section 2 demonstrates the introduction of mechanical scanning sonar. The motion compensation based on EKF is presented in Section 3. Section 4 is a fine description of the matching correction navigation based on geometric features. Field experiment is displayed and discussed in Section 5, while conclusions are drawn and discussed in Section 6.

2. Instruction of Mechanical Scanning Sonar

The Tritech Micron DST Sonar used in our ROV is a small compact mechanical scanning sonar with digital Compressed High Intensity Radar Pulse (CHIRP) system designed for underwater applications, such as obstacle avoidance and target recognition for both AUVs and ROVs. This sonar can be programmed to cover variable length sectors from a few degrees to full 360° scans. A characteristic fan-shaped beam with vertical aperture angle of 35° and narrow horizontal aperture of 3° allows a sonar image to be formed with enough information about the surrounding environment to recognize sizes, shapes, and surface reflecting characteristics of a target at distances of up to 75 meters. The sensor is mounted on the upper front part of the ROV to provide a clear view and avoid occlusions in the resulting data. Its capacity to sense the environment in which the vehicle is operated makes the DST sonar one of the most important sensors aboard the ROV.

The sonar performs scans in a horizontal 2D plane by rotating a mechanically actuated transducer head at pre-set angular increments [17]. For each emitted beam, an echo intensity profile is returned from the environment and discretized into a set of bins (distance versus echo-amplitude values) [18]. Since the mechanical scanning sonar needs a considerable period of time to obtain a complete scan, the ROV’s motion induces a distortion in the acoustic image when ROV moves. To deal with this, it is necessary to know the ROV’s pose at the beam reception time [19]. The EKF algorithm is used to estimate the position as well as its uncertainty while the sonar performs the scan and to correct the distortions induced by the motion of ROV, which will be described in the next section and the expected correction result is as in Figure 2.

Figure 2: The distortion induced by movement of the ROV can be corrected through motion compensation.

However, in practical application, every ping received will contain noise and interference except for useful information, especially within the limited space like water tank or swimming pool. As shown in Figure 2, only the innermost rectangle is the boundary of the water tank. When the acoustic pulse encounters the wall, the propagation of the acoustic pulse is blocked and part of the mechanical energy is reflected back in the opposite direction depending on the nature of the obstacle. Likewise, as the reflected pulse moves across the environment and finds other objects, part of its energy is also returned, ricocheting again with the wall and returning to the sonar head where it is interpreted as if the reflection has never taken place. In other words, the wall acts as a mirror for the acoustic pulse and, as a result, phantoms and reflections not corresponding with real objects can appear [17]. The sonar signal of one ping is as in Figure 3, where -axis represents the echo intensity of the sonar signal, and has no unit, while -axis represents the resulting target distance deduced by raw sonar data, from which we can see that the peak echo intensity after the first peak is the phantoms and reflections and must be excluded during the preprocessing.

Figure 3: Sonar signal of a ping.

To deal with this phenomenon, we need to preprocess the sonar signal before motion compensation using EKF, including threshold denoising and the sampling distance limit, as shown in our previous paper [20]. The effect of the preprocessing on the distortion in Figure 2 is as in Figure 4. Only when the sonar signal is being preprocessed and motion compensated through EKF, the matching correction navigation based on geometric features can be conducted.

Figure 4: Preprocessing of sonar signal before motion compensation.

3. Motion Compensation Based on EKF

The way to compensate motion induced distortion depends on the EKF algorithm, which estimates the state vector containing the position and velocity information of the vehicle. In this system, three-axis accelerometer, three-axis gyroscope, and three-axis magnetometer are used as inertial measurement unit (IMU) to estimate the position and attitude of the ROV. For the reason that the ROV can suspend at any depth under the water based the depth control algorithm and can move stably, as shown in our previous work [16], we assume that it moves only in the planar field with roll and pitch negligible. In other words, the depth value is invariable, and roll and pitch are zero, respectively.

The different reference frames involved in the system are shown in Figure 5, where , , and inside the rectangle represent earth fixed reference frame, the base reference frame (the orientation of the experimental water tank), and the body reference frame, respectively.

Figure 5: Different reference frame involved in the system.
3.1. Nonlinear Process Model

The state vector of the ROV contains the information of the position and velocity at time k.where the vector represents the position and orientation of the ROV in the base reference frame , while represents the corresponding linear and angular velocities in the body reference frame . The initial value of the state vector and its covariance matrix should be estimated before starting the EKF. For the reason that the ROV is set to known location with velocities set to zero at the beginning of the experiment, the state vector at time 0 isHere, a simple constant velocity kinematics model is introduced to predict the state.where is the white Gaussian acceleration noises with zero mean. The covariance of is represented by the noise matrix .

Prediction. The estimate of the state is obtained asThe covariance matrix iswhere and are the Jacobian matrices of partial derivatives of the function with respect to the state and the noise , respectively.

3.2. Nonlinear Measurement Model

The nonlinear measurement model is represented aswhere is the measurement vector and is the white Gaussian noises with zero mean with its covariance matrix .

Update Using Three-Axis Accelerometer. The model prediction is updated by the EKF equations each time a new measurement from the three-axis accelerometer arrives and finishes the first and second integrals.whereUpdate Using Three-Axis Magnetometer. The model prediction is updated by the EKF equations each time a new measurement from the three-axis magnetometer arrives. What calls for special attention is that the angle from the magnetometer is ; the yaw angle in the state vector is .where

Update Using Three-Axis Gyroscope. The model prediction is updated by the EKF equations each time a new measurement from the three-axis gyroscope arrives and finishes the first-order integrals.where

After finishing the EKF based motion compensation, the sonar image we see is coherent and smooth; one of the experiments from the water tank (length width depth = ) is as in Figure 6.

Figure 6: Sonar image after motion compensation.

However, in spite of the effectiveness of the motion compensation, the boundary of the water tank is not straight enough for us to localize the ROV. To deal with this problem, we proposed the matching correction navigation algorithm based on geometric features which will be introduced in the next section in detail.

4. Matching Correction Navigation Algorithm Based on Geometric Features

The bins we obtain from the mechanical scanning sonar are a set of fixed-bearing temporal signals with different intensity. Every intensity point can be represented in polar coordinate, where and represent range and bearing of the acoustic ping. So it is necessary to perform coordinate transformation for the measurement data. Assume that the position of point in the scan plane is . Then we have

That is what we see in Figure 6 with sonar point cloud data displayed in Cartesian coordinate, in which the origin is the center of the sonar.

4.1. Corner and Boundary Line Detection

To process the match correction navigation algorithm, we need to eliminate the influence of the noise and interference and find the four intersection points of the four water tank boundaries. The result of obtaining the four corners using Hough Transform [21] is as in Figure 7.

Figure 7: Four corners found using Hough Transform.

Then, the data sets between two adjacent corners of the four corners are used for line fitting using least square method. Let us assume that each of the four lines , ,  , and  isWith all the data between two adjacent corners substituted into (17), we can getAfter solving the four equations, the four straight lines we get in sonar image are as in Figure 8.

Figure 8: Sonar image after line fitting.
4.2. Rectangle Modification Using Geometric Features

However, as shown in Figure 8, the geometric figure surrounded by the four lines is not a standard rectangle. So the adjacent straight lines are modified based on geometric features to get a standard rectangle. The line with most data as we define line is set to be the base line, which is calculated using least square method to fit the line at first, then the two adjacent lines are modified with right angle modification relative to the base line to get the slope of the line, respectively, while the fourth line is modified to be parallel to the base line. When all the other three lines’ slope is known, we can get these three lines fitted using least squared method again. When the final rectangle is built, it will be compared with the a priori map to calculate the location and orientation of the ROV in the water tank with its uncertainty, respectively. The whole matching correction algorithm based on geometric features is shown in Algorithm 1 in detail.

Algorithm 1: The map matching correction based on geometric features.

The result after geometric modification is as in Figure 9. The read “” represents the position of the sonar. The boundary of the water tank is the base reference frame as shown in Figure 5.

Figure 9: Rectangle after geometric modification.
4.3. Key Edges Identification Based on Corner Matching

After obtaining the standard rectangle, we can calculate the position of the sonar through measuring the distance between sonar and the straight line in the base reference frame; then the position of the ROV can be deduced according to the relative position between the sonar and the ROV’s center of gravity. Meanwhile, we can also deduce the orientation of the ROV by comparing the generated rectangle with the a priori map using the slope of the line. However, it is hard for us to recognize the two edges of the water tank which is used as the base reference frame at the beginning. For this reason, we use the lines and as the directions and in the base reference frame. Every time the ROV moves and the sonar finishes a complete scan, we use Hough Transform to find the four corners, and the adjacent corner between two scans is identified as the same corner, so the lines and can be identified, and then we calculate the position and the orientation of the ROV.

4.4. Position and Orientation Correction Using the Second EKF

In order to obtain the better localization accuracy of the ROV, the position and orientation deduced from the matching correction algorithm are then used as new measurement to proceed the second EKF. The whole process of the entire recursive process is as in Figure 10.

Figure 10: The entire recursive process.

5. Field Experiment

To verify the effectiveness of the proposed navigation algorithm, many experiments have been carried out in man-made water tank which is similar to the reactor pool of the nuclear power plant. The water tank is 6 m × 3 m × 1.5 m (length × width × depth, where depth means the depth of water) as shown in Figure 11, and the ROV ran slowly.

Figure 11: Experiment in water tank.

All the data from sensors were obtained to calculate the localization and orientation of the ROV precisely, and the tick mark of the water tank is used as the ground truth to evaluate the localization accuracy of the proposed algorithm, while the orientation calculated from the magnetometer after calibration is used as the ground truth to assess the orientation accuracy of the algorithm. The value of in the base reference is positive when the ROV rotates in a clockwise direction. The results of the experiments are as Figures 12 and 13.

Figure 12: The first experiment.
Figure 13: The second experiment.

The fitting straight lines of the first and the second experiments is as in (19), in which the order is , , , and .

The localization and orientation deduced from the two experiments are as in Table 1. The localization coordinate refers to the base reference in Figure 5.

Table 1: Results of the experiment.

From Table 1, we can see that the error is very low and the accuracy of localization and orientation is very high. The biggest advantage compared with other underwater navigation algorithms is that the proposed algorithm is based on the geometric features and the a priori map matching. In other words, the algorithm is not interfered with by the running time and the accuracy error will not accumulate as time goes on just like GPS works on land, which is very important in practical application.

6. Conclusion

The paper proposes a new underwater navigation algorithm based on geometric features and map matching using mechanical scanning sonar in known man-made structured environment. The motion induced distortion of the sonar point cloud data is modified and compensated for using EKF with all the sensor data fused. Then the boundary of the water tank is carried out using Hough Transform combined with the least square fitting. To modify the error of the line fitting, the line with most sonar data is used as the base line with all the three other lines modified based on geometric features. After calculating the rectangle figure of the water tank, we can easily deduce the localization and orientation of the ROV. Water tank environment verifies the validation of the proposed algorithm.

The ROV we use as the platform is designed to help monitor underwater environment and salvage small parts like bolts and nuts in the reactor pool and other water-filled infrastructure of the nuclear power plants, and it has been experimented in the reactor simulation pool of the Daya Bay Nuclear Power Plant many times to test the leaking tightness and movement stability as we introduce in paper [12]. In the current time, this is sufficient for us to know the vehicle’s accurate localization and orientation when the ROV moves slowly or even suspends under the water in specific time to guarantee the safety of the nuclear equipment. More experiments will be carried out in the reactor simulation pool of the nuclear power plant and the algorithm will get further validation.

Competing Interests

The authors declare that they have no competing interests.


The authors would like to acknowledge the support of Natural National Key Basic Research Program of China under Grant no. 2013CB035503 and the National High Technology Research and Development Program of China under Grant no. 2011AA040201.


  1. Y. Shim, J. Park, and J. Kim, “Relative navigation with passive underwater acoustic sensing,” in Proceedings of the 12th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI '15), pp. 214–217, IEEE, Goyang, South Korea, October 2015. View at Publisher · View at Google Scholar
  2. D. Ribas, P. Ridao, A. Mallios, and N. Palomeras, “Delayed state information filter for USBL-Aided AUV navigation,” in Proceedings of the IEEE International Conference on Robotics and Automation, pp. 4898–4903, May 2012. View at Publisher · View at Google Scholar · View at Scopus
  3. J. C. Kinsey, R. M. Eustice, and L. L. Whitcomb, “A survey of underwater vehicle navigation: recent advances and new challenges,” in Proceedings of the IFAC Conference of Manoeuvering and Control of Marine Craft, p. 88, Lisbon, Portugal, September 2006.
  4. L. Stutters, H. Liu, C. Tiltman, and D. J. Brown, “Navigation technologies for autonomous underwater vehicles,” IEEE Transactions on Systems, Man and Cybernetics Part C: Applications and Reviews, vol. 38, no. 4, pp. 581–589, 2008. View at Publisher · View at Google Scholar · View at Scopus
  5. I. Mahon, S. B. Williams, O. Pizarro, and M. Johnson-Roberson, “Efficient view-based SLAM using visual loop closures,” IEEE Transactions on Robotics, vol. 24, no. 5, pp. 1002–1014, 2008. View at Publisher · View at Google Scholar · View at Scopus
  6. A. Elibol, N. Gracias, and R. Garcia, “Augmented state–extended Kalman filter combined framework for topology estimation in large-area underwater mapping,” Journal of Field Robotics, vol. 27, no. 5, pp. 656–674, 2010. View at Publisher · View at Google Scholar · View at Scopus
  7. H. Liu, J. Qin, F. Sun, and D. Guo, “Extreme kernel sparse learning for tactile object recognition,” IEEE Transactions on Cybernetics, no. 99, pp. 1–12, 2016. View at Publisher · View at Google Scholar
  8. H. Liu, Y. Yu, F. Sun, and J. Gu, “Visual-Tactile Fusion for Object Recognition,” IEEE Transactions on Automation Science and Engineering, vol. PP, no. 99, pp. 1–13, 2016. View at Publisher · View at Google Scholar
  9. H. Liu, D. Guo, and F. Sun, “Object recognition using tactile measurements: kernel sparse coding methods,” IEEE Transactions on Instrumentation and Measurement, vol. 65, no. 3, pp. 656–665, 2016. View at Publisher · View at Google Scholar
  10. H. Liu, Y. Liu, and F. Sun, “Robust exemplar extraction using structured sparse coding,” IEEE Transactions on Neural Networks and Learning Systems, vol. 26, no. 8, pp. 1816–1821, 2015. View at Publisher · View at Google Scholar · View at MathSciNet · View at Scopus
  11. A. Mallios, P. Ridao, D. Ribas, and E. Hernández, “Scan matching SLAM in underwater environments,” Autonomous Robots, vol. 36, no. 3, pp. 181–198, 2014. View at Publisher · View at Google Scholar · View at Scopus
  12. E. Hernández, P. Ridao, D. Ribas, and J. Batlle, “MSISpIC: a probabilistic scan matching algorithm using a mechanical scanned imaging sonar,” Journal of Physical Agents, vol. 3, no. 1, pp. 3–12, 2009. View at Google Scholar · View at Scopus
  13. A. Burguera, Y. González, and G. Oliver, “The UspIC: performing scan matching localization using an imaging sonar,” Sensors, vol. 12, no. 6, pp. 7855–7885, 2012. View at Publisher · View at Google Scholar · View at Scopus
  14. B. He, Y. Liang, X. Feng et al., “AUV SLAM and experiments using a mechanical scanning forward-looking sonar,” Sensors, vol. 12, no. 7, pp. 9386–9410, 2012. View at Publisher · View at Google Scholar · View at Scopus
  15. L. Chen, S. Wang, H. Hu, D. Gu, and L. Liao, “Improving localization accuracy for an underwater robot with a slow-sampling sonar through graph optimization,” IEEE Sensors Journal, vol. 15, no. 9, pp. 5024–5035, 2015. View at Publisher · View at Google Scholar · View at Scopus
  16. M. Dong, W. Chou, B. Fang, G. Yao, and Q. Liu, “Implementation of remotely operated vehicle for direct inspection of reactor pressure vessel and other water-filled infrastructure,” Journal of Nuclear Science and Technology, vol. 53, no. 8, pp. 1086–1096, 2016. View at Publisher · View at Google Scholar · View at Scopus
  17. D. Ribas, P. Ridao, and J. Neira, Underwater SLAM for Structured Environments Using an Imaging Sonar, Springer, Berlin, Germany, 2010.
  18. D. Ribas, P. Ridao, J. D. Tardós, and J. Neira, “Underwater SLAM in man-made structured environments,” Journal of Field Robotics, vol. 25, no. 11-12, pp. 898–921, 2008. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at Scopus
  19. A. Mallios, P. Ridao, M. Carreras, and E. Hernández, “Navigating and mapping with the SPARUS AUV in a natural and unstructured underwater environment,” in Proceedings of the MTS/IEEE Kona Conference (OCEANS '11), pp. 1–7, Kona, Hawaii, USA, September 2011. View at Scopus
  20. B. Fang, W.-S. Chou, M.-J. Dong, X. Ma, and X.-Q. Guo, “Location algorithm of underwater robot based on the probabilistic iterative correspondence,” Journal of Electronics & Information Technology, vol. 36, no. 4, pp. 993–997, 2014. View at Publisher · View at Google Scholar · View at Scopus
  21. W. A. Barrett and K. D. Petersen, “Houghing the hough: peak collection for detection of corners, junctions and line intersections,” in Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR '01), vol. 2, Kauai, Hawaii, USA, December 2001. View at Publisher · View at Google Scholar