Abstract
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 [7–10].
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.
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.
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.
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.
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.
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.
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.
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.
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.
| ||||||||||||||||||||||||||||||||||||||||
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.
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.
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.
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.
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.
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.
Acknowledgments
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.