Autonomous Navigation Based on SEIF with Consistency Constraint for C-Ranger AUV

Shen, Yue; Zhang, Hao; He, Bo; Yan, Tianhong; Liu, Yang

doi:https://doi.org/10.1155/2015/752360

Mathematical Problems in Engineering

On this page

Abstract Introduction Conclusion Acknowledgments References Copyright Related Articles

Research Article | Open Access

Volume 2015 | Article ID 752360 | https://doi.org/10.1155/2015/752360

Autonomous Navigation Based on SEIF with Consistency Constraint for C-Ranger AUV

Yue Shen,¹Hao Zhang,²Bo He,¹Tianhong Yan,³and Yang Liu¹

Academic Editor: Oscar Reinoso

Received21 Apr 2015

Revised06 Aug 2015

Accepted16 Aug 2015

Published10 Sept 2015

Abstract

An autonomous underwater vehicle (AUV) has to solve two essential problems in underwater environment, namely, localization and mapping. SLAM is one novel solution to estimate locations and maps simultaneously based on motion models and sensor measurements. Sparse extended information filter (SEIF) is an effective algorithm to reduce storage and computational costs of large-scale maps in the SLAM problem. However, there exists the inconsistency in the SEIF since the rank of the observability matrix of linearized error-state model in SLAM system is higher than that of the nonlinear SLAM system. By analyzing the consistency of the SEIF-based SLAM from the perspective of observability, a SLAM based on SEIF with consistency constraint (SEIF-CC SLAM) is developed to improve the estimator’s consistency. The proposed algorithm uses the first-ever available estimates to calculate SEIF Jacobians for each of the state variables, called the First Estimates Jacobian (FEJ). Then, the linearized error-state model can keep the same observability as the underlying nonlinear SLAM system. The capability of autonomous navigation with the proposed algorithm is validated through simulations experiments and sea trials for a C-Ranger AUV. Experimental results show that the proposed SEIF-CC SLAM algorithm yields more consistent and accurate estimates compared with the SEIF-based SLAM.

1. Introduction

The inconsistency problem in linearization-based autonomous navigation system has attracted significant concern. The reason of inconsistency is analyzed from the perspective of observability. A consistency-constrained method is adopted to guarantee the consistent observability and to minimize the linear errors caused by the filter.

As a key technique of truly autonomous navigation, simultaneous localization and mapping (SLAM) is the process by which a mobile robot can incrementally build consistent maps in an unknown environment and use the maps to determine its own location at the same time [1]. It has played an important role in different domains, such as autonomous underwater hull inspection [2], autonomous underwater vehicle (AUV) in deep sea regions [3], unmanned aerial vehicle (UAV), and miniature aerial vehicle (MAV) in the sky [4], as well as autonomous driver-assistance-systems in cars [5]. The SLAM problem involves an unknown and uncertain environment description and sensors noise [6–13]. There are many algorithms developed to address this SLAM problem in various forms, such as extended Kalman filter (EKF) [9, 10], particle filter [11], and extended information filter (EIF) [12, 13] based SLAM algorithms.

EKF-based SLAM has been proved to be widely popular as a standard nonlinear state estimator for its simplicity [6, 7]. This algorithm uses the observable features to update robot’s pose and estimate map. However, the quadratic complexity of computational capacity is one shortcoming of EKF-based SLAM in large-scale maps [14–16]. Extended information filter (EIF) [17] is the dual form of EKF and is parameterized by information matrix and information vector. It is found that the information matrix is dominated by a small number of diagonal elements while most off-diagonal elements are close to zero [18]. So a sparse approximated representation of information matrix is obtained by ignoring the conditional dependence relationships between the mobile robot and a subset of the map, which is developed into a sparse variant of the EIF, called the sparse extended information filter (SEIF) [12, 19]. Due to the sparsification of SLAM information matrix, SEIF can avoid EKF’s quadratic cost in computation and memory. Therefore, SEIF has been successfully implemented with real-world datasets and has been demonstrated to be more scalable [20, 21].

However, the estimate of SEIF is prone to inconsistency for two reasons. First and foremost, SEIF may easily become overconfident due to the sparsification strategy [22]. Secondly, SEIF is based on single-point linearization as well as EKF, which may cause the accumulation of errors and lead to the estimated inconsistency [23–25]. Then, some approaches have been developed to improve the inconsistency of SEIF-based SLAM through the theoretical analysis. One method is exactly sparse extended information filter (ESEIF) proposed by Walter et al. [26], which is a typical approach referred to as full SLAM [27, 28]. ESEIF keeps the exactly sparse information matrix through marginalizing out the robot pose and relocating the robot, which is both locally and globally consistent [29]. But ESEIF is no longer computationally tractable and also faces the same nonlinear problems as EKF. Another method to improve the inconsistency is Huang’s sparse local submap joining filter (SLSJF) [30], where a sequence of small sized local submaps is represented in the global coordinate frame and EIF is used to ensure that the information matrix is exactly sparse. However, small inconsistency in lots of local maps may result in large inconsistency in the global map. Additional work in the SLSJF is required to complete the local maps joining. In short, ESEIF and SLSJF cannot concurrently have advantages of low computational cost and consistency.

The observability of the nonlinear SLAM system was analyzed by Lee et al. [31]. The nonlinear SLAM system has three degrees of freedom, while the linearized error-state system is a two-dimensional subspace. So the obtained spurious information of linear SLAM system is the primary cause of inconsistency. Huang et al. [32] proved that the rank of the observation model’s Jacobians in the linearized error-state system is higher than that of the nonlinear SLAM system. The filter Jacobians are calculated using the first-ever available estimates for each state variable, called the First Estimates Jacobian (FEJ), which ensures that the observability of error-state model is the same as the underlying nonlinear SLAM systems. Simulations have also been demonstrated that the FEJ-based EKF algorithm is superior to the standard EKF in terms of accuracy and consistency [33].

In view of the quadratic complexity and inconsistency of the EKF-based SLAM algorithm in the large-scale environments, a novel SLAM algorithm based on SEIF with the consistency constraint (SEIF-CC SLAM) is proposed to improve the autonomous navigation capability of AUV in this paper. SEIF is adopted to maintain the sparsification of information matrix according to the low computational cost. Meanwhile, the estimator’s consistency is constrained by the FEJ algorithm so as to obtain the same dimension between the linearized error-state system and the nonlinear SLAM system. And the SEIF-CC SLAM will be compared with the SEIF-based SLAM through Monte Carlo simulations in terms of accuracy and consistency.

Moreover, autonomous navigation experiment of the C-Ranger AUV is employed to validate the reliability of the proposed SEIF-CC SLAM algorithm in this paper. The C-Ranger AUV is developed in Underwater Vehicle Laboratory at Ocean University of China, which is equipped with a number of sensors, such as DVL, gyro, digital compass, AHRS, and GPS, as well as multibeam imaging sonar for active sensing in underwater environment [34–36]. The experimental results will show that the performance of the proposed algorithm is superior to the SEIF-based algorithm in terms of the consistency and accuracy based on the C-Ranger AUV. Furthermore, SEIF-CC SLAM also preserves the SEIF’s low computational cost and storage requirements.

The remainder of the paper is organized as follows. The SEIF-based SLAM algorithm is presented briefly in Section 2. And Section 3 will analyze the consistency of SEIF-based SLAM. In Section 4, the proposed SEIF-CC SLAM algorithm is described and its performance is compared with SEIF-based SLAM through Monte Carlo simulations. Section 5 introduces the basic framework of the C-Ranger AUV platform as well as the onboard sensors. Then, results of sea trials will be presented to validate the performance of the proposed SEIF-CC SLAM algorithm. Finally, we summarize the results and give the future work.

2. The SEIF-Based SLAM

2.1. Information Form

The system model of SLAM includes the robot poses and a series of features’ or landmarks’ locations in the global coordinate system. The robot pose is determined by its position and heading , while the location of a feature is expressed as . So at time , the state vector can be expressed as follows:where the superscript “” refers to the transpose of a vector.

The robot aims to find a probabilistic estimate of the state vector in SLAM. The estimation problem transforms to calculate a posterior distribution based on the Bayesian rule, where and are past sensor measurements and control inputs at time , respectively. Reviewing the EKF solution to SLAM problem, the posterior is obeying a multivariate Gaussian distribution:where and are the mean vector and covariance matrix and denotes the Gaussian distribution.

Fundamental representation of SEIF is the information vector and information matrix which substitute the state vector and covariance matrix of EKF. According to the dual representation for the Gaussian distribution [12], the posterior probability distribution of is also described by the information matrix and information vector:In particular, the transformation between the state representations in EKF and the information form in EIF is got via the following formulas:

2.2. The Motion and Observation Model

According to the system state at time , the motion model of the robot at time is as follows:where is the independent noise variable that obeyed the white Gaussian distribution, . So the first-order Taylor expansion of the motion model is as follows:where is the Jacobian matrix of the motion function with respect to .

The system state of the robot is estimated according to the velocity and the attitude information , which is provided by the velocity sensor and attitude sensor individually. The Jacobian matrix can be presented (in this paper, the subscripts and refer to the state’s update and prediction step at time , resp.):where denotes the 2 × 2 rotation matrix and is the velocity-based increment of the robot’s position estimate between time steps and :

The observation model of the mobile robot is analyzed in the following. The approximation of the nonlinear observation function is derived from its first-order Taylor expansion. Assuming that the global position of the th landmark is , is the Jacobian matrix of the observation function with respect to , and is also subject to the white Gaussian noise, , the linearized observation model is given by

The linearized observation model describes the direct measurement model of relative-position between features and robot. The relative-position is obtained by imaging sonar.

2.3. Some Related Steps of SEIF-Based SLAM

This paper mainly aims to improve the consistency of SEIF from the perspective of observability without changing the sparsification strategy; so the motion, update, and augmentation step related to the consistency analysis are introduced briefly.

2.3.1. Motion Step

Motion step is to predict the robot’s state at time according to one at time . Given information vector and information matrix at time , the information vector and information matrix can be predicted:where

2.3.2. Update Step

The state estimation will be updated when a feature is repeatedly observed. The updated information matrix and information vector can be expressed:where expresses the Jacobian of corresponding to the landmark position relative to robot and elements of are nonzero only at positions associated with and .

2.3.3. Augmentation Step

The state augmentation step will be implemented if a feature is observed for the first time. In the process of state augmentation, the robot’s observation model and its first-order Taylor expansion form are as follows:where is the white Gaussian noise and is Jacobian matrix of the observation function with respect to . The augmented information matrix and information vector are as follows:with

The mean of state is employed in update and sparsification steps; so the mean is recovered after update step by information vector and information matrix: .

3. Consistency Analysis of SEIF-Based SLAM

In general, the system model of the real SLAM is higher dimension, highly nonlinear, and coupled. SEIF-based SLAM algorithm is significantly inconsistent because of the pruning strategy and linearization. Consistency problem of the system model should be reanalyzed from the observable perspective. Comparing the observability of the SEIF system with the nonlinear system, it is evident to discern a fundamental deficiency caused by the first-order Taylor expansion of the motion and observation model. The observability of the real and linearized SLAM system is analyzed based on the observability rank condition [33]. They do not possess the same observable rank so as to lead to the inconsistency.

3.1. Nonlinear Observability Analysis for SLAM

The Lie derivative is employed to deduce the observability matrix with a single landmark in order to analyze the observability of the nonlinear SLAM system.

Based on the kinematic model of the robot, the increment of system state in time interval is given bywhere and are the linear velocity and rotational velocity, respectively. They are the control inputs.

Considering the robot-to-landmark measurement as the relative-position measurement, the position of the landmark relative to the robot at time is

The Lie derivative of a function along an analytic vector field is defined asSo the first-order Lie derivative of and along and can be calculated:Then, the second-order Lie derivatives are as follows:

The second-order Lie derivatives of other functions are equal to zeros. Obviously, the second-order Lie derivatives are the linear forms of the first-order ones. It is easy to find that the higher-order Lie derivatives can also express the linear form of the first-order ones; so the space spanned can be denoted:

Therefore, the observability matrix with one landmark in the underlying nonlinear SLAM system is got aswhere and , respectively. The rank of the observability matrix of the nonlinear SLAM system is 2. So the rank of the observability matrix with -dimensional landmarks is in the nonlinear SLAM system.

3.2. Local Observability of SEIF-Based SLAM

SEIF and EIF are the information form of EKF based on single-point linearization in the SLAM framework. So they also have the same observability matrix. Assuming one landmark is observed between time steps and , the local observability matrix during time intervals can be defined: Here, and have been given in the formulas (7), (16), and (17), respectively. Formula (29) can be expanded:where expresses the block-diagonal matrix. It is nonsingular; so the rank of is equivalent to the rank of , .

3.2.1. Ideal SEIF-Based SLAM

As known, the evaluated filter Jacobian is derived by using the linearized error-state model. In studying the observability of ideal SEIF, the filter Jacobian is evaluated using the true state variables to substitute the mean .

According to formula (7), is got as follows: So the following formula (32) can be induced:

Formula (33) can be deduced by means of formulas (17) and (32) (the superscript “” is used to denote the true state values):Now, the matrix can be derived:

The rank of the above matrix is 2, , which is equivalent to the rank of the actual nonlinear SLAM system. Therefore, the ideal SEIF-based SLAM and the actual nonlinear SLAM system have the similar observability.

3.2.2. Actual SEIF-Based SLAM

The actual state variables are usually unknown in the observability analysis of SEIF-based SLAM; so the filter Jacobians are calculated by means of linearized error-state model; that is, Jacobians are evaluated at the mean of the state after update. Similar to formulas (32) and (33), the expression can be derived:

Now, the matrix can be written:

The rank of the linearized error-state model’s observability matrix is equal to 3, . But the nonlinear observability matrix is two-dimensional according to formula (30). So the SEIF-based SLAM whose Jacobians are evaluated at the mean has one spurious information, which leads to the inconsistency. It is necessary to select special linearization points in order to guarantee the correct rank of observability matrix for each state variable.

4. A SLAM Algorithm Based on SEIF with Consistency Constraint

4.1. The Consistency Constraint

The FEJ-based EKF estimator can improve the estimator’s consistency [33]. Its main idea is to choose the first-ever available estimates as the linearization points in terms of all the state variables. The modified measure in EKF can analogously be used in SEIF since SEIF is an information filter form of the EKF. The consistency of SEIF is constrained by FEJ so as to obtain the same dimensionality between the state and observation model.

The updated estimate is replaced by the position estimate in the Jacobian matrix of the state. So formula (7) can be written asThe mean will be recovered after motion step for .

Assuming that a landmark was first observed at time , the measurement Jacobian can be written as

4.2. Algorithm Process

The proposed SLAM based on SEIF with consistency constraint (SEIF-CC SLAM) with pseudocode is provided in Algorithm 1. And main differences are the motion and measurement step compared with SEIF-based SLAM.

Require: waypoints
Global //global variables
= Initialization (0); //initialize global variables
Threshold = 8; //The limited number of active features

WHILE waypoints not 0DO //main loop
, , , ); //new vehicle pose
Add random noise to nominal control values:
//Motion model
Predict_FEJ(, , , ) //prediction
Predict state
Compute Equation (37) //estimate at
Compute Ψ Equation (12) with
Predict information matrix Equation (10)
Predict information vector Equation (11)
Mean recovery:
Get observations : set of rang-bearing observation
Add random measurement noise
//Data association
//: index tags for each landmark;
;
//Update step
IF not empty THEN // is not a new feature
Observe_Update(, , )
FOR from 1 to size(, 2) DO
Compute Equation (38)
Compute Equation (15)
Compute Equation (13)
Compute Equation (14)
END FOR
END IF
//Augmentation step
IF not empty THEN //new feature
SEIF_Augment()
Compute at
Compute Equation (21)
Compute Equation (19)
Compute Equation (20)
END IF
Recover mean
IF the number of active features > Threshold THEN
SEIF_Sparse(); //sparsification
END IF
END WHILE

4.3. Observability of SEIF-CC SLAM

To verify the consistency of the proposed algorithm, the rank of the observability matrix is calculated. FEJ is employed to estimate the robot position in the filter Jacobians. And the landmark estimate observed at the first time is utilized to compute the measurement Jacobian matrix at the evaluated point. As a result, the observability matrix of this new filter can be presented:

It can be easily proven that the rank of is 2, . Thus, the dimension of the linearized error-state system model is the same as that of the nonlinear observable system by means of the SEIF-CC SLAM algorithm. Similarly, it can be shown that the observability matrix is of rank when there are landmarks included in the state system. FEJ is used in SEIF to ensure the consistency without changing the sparse formation of links, which plays the role of consistency constraint.

4.4. Simulation Experiment

The performance of SEIF-CC SLAM is compared with SEIF-based SLAM through Monte Carlo simulations. 50 Monte Carlo simulations with both SEIF and SEIF-CC are conducted, respectively, under the same environments and the same basic parameters. The normalized estimation error squared (NEES) and the root mean square (RMS) are used to evaluate the filters consistency and accuracy, respectively [37], which is defined as follows:where is the ground truth for the state vector, and are the estimated state and covariance matrix, represents the dimension of , and is the times of Monte Carlo simulations.

The simulation path and environment are shown in the Figure 1, where 134 point features are randomly distributed in the environment. The robot moves at a constant velocity of 4 m/s and is able to observe the limited number of neighboring features. All simulations apply with individual compatibility nearest-neighbour (ICNN) data association. The rejected gate is 5 and the augmented gate is 30. And the number of active features in SEIF is limited to 8. Both control inputs and sensor measurements are added by additive white Gaussian noise.

The performance of each algorithm is evaluated by the average NEES and RMS errors of robot position, as well as the heading’s RMS errors, respectively. The RMS errors show the accuracy of a given estimator, and NEES is used to characterize the filter consistency. Specifically, NEES follows a Chi-square distribution with degrees of freedom [37], where is the dimension of . So the average NEES for robot position must have an upper bound indicated by the horizontal line 2.6 and a lower bound by 1.5 if a filter is consistent with the 95% probability density in 50 Monte Carlo runs [37]. All filters are used to process the same data and set the same parameters. The selected filters in the simulation are (1) the ideal SEIF, (2) the SEIF, and (3) the SEIF-CC.

From Figure 2, it can be seen that the average NEES error of robot position for the ideal SEIF is always between the intervals [2.4, 2.6], and SEIF-CC’s NEES is a little higher than the ideal SEIF’s NEES and much lower than the SEIF’s NEES. This result indicates that the observability of the SEIF-CC’s error-state system in SLAM has a significant effect on consistency.

Figures 3 and 4 present the RMS errors of robot position and heading, respectively. The RMS errors position and heading in ideal SEIF are always low of 8.0 and 0.1, while their RMS errors in SEIF are high of 8.0 and 0.095, respectively. But the SEIF-CC’s RMS errors are superior to their SEIF’s RMS and inferior to ideal SEIF’s RMS errors. The comparative results of RMS indicate that the proposed SEIF-CC SLAM performs more accurate estimations than the SEIF-based SLAM.

5. C-Ranger AUV and Sea Trial

5.1. C-Ranger AUV

As Figure 5 shown, the C-Ranger is an open-frame-structure vehicle with the size of 1.6 m long, 1.3 m wide, and 1.1 m high, the gross drainage tonnage of 208 L, and the maximum work depth of 100 m. The total weight of C-Ranger is about 206 kg when it works at full load. The maximal speed is close to 3 knots (1.5 m/s). The continuous running time can reach 8 hours when C-Ranger travels at the speed of 1 knot and is fully charged.

The AUV system is composed of two electronic cabins and five underwater propeller thrusters. The top cabin is referred to as instrument cabin to host sensors, including two PC104 industrial computer units, the inner-hull parameter monitor module, the communication module, and peripherals for data processing. The bottom cabin is the battery cabin which contains li-ion battery packs, battery management module, and motion driven module. C-Ranger has five degrees of freedom (DOF), including yaw, pitch, roll, heave, and surge, which ensures the good maneuverability. In addition, there are five propeller thrusters in the C-Ranger. The two horizontal thrusters are equipped in the belly of C-Ranger and parallel to the bow direction, which are used to supply the horizontal propulsion and dominate the freedoms of yaw and surge. Meanwhile, the other three vertical ones are applied for offering the vertical propulsion and regulating the freedom of roll, pitch, and heave.

5.2. Onboard Sensors

There are some internal sensors installed on C-Ranger, such as gyroscope, digital compass, pressure sensor, and AHRS (Attitude and Heading Reference System). In addition, there are also some external sensors, including sonar, altimeter, GPS, CCD camera, and DVL (Doppler Velocity Log).

A multibeam imaging sonar (Gemini 720i) is installed at the front top of C-Ranger, working at the frequency of 720 kHz. It is the principal sensor of C-Ranger to provide active sensing of environment features for its real-time, crisp imagery of the underwater scene ahead.

In the navigation course of C-Ranger AUV, the vehicle velocity relative to the seabed is acquired by the NavQuest600 DVL installed at the rear bottom of vehicle; the DVL’s work frequency and maximum velocity are 600 kHz and 20 knots, respectively. The attitude and angular velocity information are acquired from the AHRS and gyroscope. In particular, the AHRS’s ranges of heading angle and acceleration are 0°~360° and ±2 g, respectively.

A high-precision GPS is installed at the top postmedian of C-Ranger with 1.1 m (CEP) accuracy and 20 Hz maximum data update rate. The GPS system is regarded as a perfect benchmark for the purpose of algorithm comparison.

5.3. SLAM for C-Ranger

The vehicle position can be expressed by its position and heading at time , and the feature is described by its and coordinate in the global coordinate system.

At time , the control inputs and observations of AUV are as follows:Assuming that the variables , , , denote vehicle length, vehicle velocity, rotation angle, and sampling interval, respectively, the motion model of AUV is as follows:

Assuming that the global position of the th feature is for the measurements of sonar, the observation model of AUV can be formed by the range and bearing from the vehicle to feature:where and are the observation Gaussian noise, , .

5.4. Sea Trial

The real-world experiment was performed at Tuandao Bay (Qingdao, China). The onboard sensors provided speed and heading angle information to predict the AUV position. At the same time, the multibeam imaging sonar perceived the environment, so that the appropriate environment feature points can be extracted and used for building an environmental global map. AUV traveled underwater with a certain depth; so the bidimensional vehicle-landmark model is adopted in underwater environment.

Figure 6 shows the trajectory (the red line) of C-Ranger measured by GPS, and the starting point is marked by a yellow five-pointed star. The trajectory is regarded as ground truth used to compare the effectiveness of algorithms. The C-Ranger’s sailing speed was about 1 knot in the experiment.

The imaging sonar was configured to field of view with scanning range of 100 meters. The pretreatment of the raw data measured by sonar includes denoising and sparsifying redundancy features; so the remaining features were not affecting the positioning accuracy. At the same time, the image distorted problem caused by the vehicle’s motion was also considered in the real-world application. For the C-Ranger AUV, an effectual position feedback technique was adopted to correct the distorted image [36].

Figure 7 shows clear path comparison results for GPS (the red line), SEIF (the green line), and the SEIF-CC (the blue line). It can be seen that the path deviation of the proposed SEIF-CC SLAM relative to GPS is smaller than that of the SEIF-based SLAM.

Figures 8 and 9 present the average NEES and RMS errors of SEIF and SEIF-CC relative to GPS data, respectively. It can be seen that the NEES error of SEIF-CC is almost low of 10, but the SEIF’s NEES is far higher than the SEIF-CC’s NEES. So the consistency of the SEIF-CC SLAM algorithm is superior to the SEIF-based SLAM in sea trial. According to Figure 9, the RMS errors of SEIF-CC and SEIF are rising at the beginning, but the general trend of RMS error of SEIF is also upward while the SEIF-CC’s error curve is dropt after a period of time. Therefore, the SEIF-CC SLAM algorithm is an efficient one with better consistency and accuracy in real-world environments.

6. Conclusion and Future Work

SEIF-based SLAM has been proposed to build large-scale maps. However, the state estimation will suffer from global inconsistency due to SEIF’s pruning strategy. What is more, the SEIF is also subject to the linearized error. So a SLAM algorithm based on SEIF with consistency constraint is proposed to improve the autonomous navigation capability of AUV in this paper. The “First Estimates Jacobian” is adopted to constrain the SEIF’s consistency. SEIF-CC SLAM performs better in terms of consistency and accuracy, because the observable matrix of this error-state system has the same rank as that of the nonlinear system. The NEES and RMS errors of SEIF-CC are compared with those of SEIF through Monte Carlo simulations and sea trials of C-Ranger AUV navigation in Tuandao Bay. The experimental results demonstrated that the SEIF-CC SLAM applied to AUV navigation can improve the consistency and accuracy compared with SEIF-based SLAM.

More work is required to further improve the consistency of SEIF in underwater large-scale environment implementations. There are two sources that caused estimation errors. One is that the approximation is linearized; the other is that the Jacobians are evaluated at estimates, which are not at the true values. Moreover, information-based compact pose SLAM is one method to address the first error source [38]. This method can reduce computational cost and delay inconsistency by means of keeping nonredundant poses and highly informative links. In addition, square root cubature information filter (SRCIF) was proposed for nonlinear state estimation [39], which provided a more accurate filtering to solve a wider range of nonlinear systems. Further methods will be used to improve the navigation accuracy of the C-Ranger AUV.

Conflict of Interests

The authors declared that they have no conflict of interests regarding this work.

Acknowledgments

This work is partially supported by the National Science Foundation of China (41176076, 51075377, and 51379198) and The High Technology Research and Development Program of China (2014AA093410).

References

H. Durant-Whyte and T. Bailey, “Simultaneous localization and mapping (SLAM) part I: the essential algorithms,” IEEE Robotics and Automation Magazine, vol. 13, no. 3, pp. 99–110, 2006.
View at: Google Scholar
F. S. Hover, R. M. Eustice, A. Kim et al., “Advanced perception, navigation and planning for autonomous in-water ship hull inspection,” International Journal of Robotics Research, vol. 31, no. 12, pp. 1445–1464, 2012.
View at: Publisher Site | Google Scholar
L. Paull, S. Saeedi, M. Seto, and H. Li, “AUV navigation and localization: a review,” IEEE Journal of Oceanic Engineering, vol. 39, no. 1, pp. 131–149, 2014.
View at: Publisher Site | Google Scholar
G. M. Hoffmann, H. Huang, S. L. Waslander, and C. J. Tomlin, “Precision flight control for a multi-vehicle quadrotor helicopter testbed,” Control Engineering Practice, vol. 19, no. 9, pp. 1023–1036, 2011.
View at: Publisher Site | Google Scholar
C. Rernandez, R. Dominguez, D. Fernandez-Liorca et al., “Autonomous navigation and obstacle avoidance of a micro-bus,” International Journal of Advanced Robotic Systems, vol. 10, no. 212, 2013.
View at: Google Scholar
M. W. M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrant-Whyte, and M. Csorba, “A solution to the simultaneous localization and map building (SLAM) problem,” IEEE Transactions on Robotics and Automation, vol. 17, no. 3, pp. 229–241, 2001.
View at: Publisher Site | Google Scholar
S.-J. Lee, D.-W. Cho, and J.-B. Song, “Novel sonar salient feature structure for extended kalman filter-based simultaneous localization and mapping of mobile robots,” Advanced Robotics, vol. 26, no. 8-9, pp. 1055–1074, 2012.
View at: Publisher Site | Google Scholar
J. Fuentes-Pacheco, J. Ruiz-Ascencio, and J. M. Rendón-Mancha, “Visual simultaneous localization and mapping: a survey,” Artificial Intelligence Review, vol. 43, no. 1, pp. 55–81, 2012.
View at: Publisher Site | Google Scholar
F. F. Yadkuri and M. J. Khosrowjerdi, “Methods for improving the linearization problem of extended Kalman filter,” Journal of Intelligent & Robotic Systems, vol. 78, no. 3-4, pp. 485–497, 2015.
View at: Publisher Site | Google Scholar
G. Klancar, L. Teslic, and I. Škrjanc, “Mobile-robot pose estimation and environment mapping using an extended Kalman filter,” International Journal of Systems Science, vol. 45, no. 12, pp. 2603–2618, 2014.
View at: Publisher Site | Google Scholar | MathSciNet
R. Havangi, M. A. Nekoui, H. D. Taghirad, and M. Teshnehlab, “An intelligent UFastSLAM with MCMC move step,” Advanced Robotics, vol. 27, no. 5, pp. 311–324, 2013.
View at: Publisher Site | Google Scholar
S. Thrun, Y. Liu, D. Koller, A. Y. Ng, Z. Ghahramani, and H. Durrant-Whyte, “Simultaneous localization and mapping with sparse extended information filters,” International Journal of Robotics Research, vol. 23, no. 7-8, pp. 693–716, 2004.
View at: Publisher Site | Google Scholar
E. Brekke and M. Chitre, “A multi-hypothesis solution to data association for the two-frame SLAM problem,” International Journal of Robotics Research, vol. 34, no. 1, pp. 43–63, 2015.
View at: Publisher Site | Google Scholar
J. Leonard and P. Newman, “Consistent, convergent, and constant-time SLAM,” in Proceedings of the 18th International Joint Conference on Artificial Intelligence (IJCAI '03), pp. 1143–1150, Acapulco, Mexico, August 2003.
View at: Google Scholar
J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, “Consistency analysis and improvement of vision-aided inertial navigation,” IEEE Transactions on Robotics, vol. 30, no. 1, pp. 158–176, 2014.
View at: Publisher Site | Google Scholar
M. Bosse, P. Newman, J. Leonard, and S. Teller, “Simultaneous localization and map building in large-scale cyclic environments using the atlas framework,” International Journal of Robotics Research, vol. 23, no. 12, pp. 1113–1139, 2004.
View at: Publisher Site | Google Scholar
A. G. O. Mutambara, “Information based estimation for both linear and nonlinear systems,” in Proceedings of the American Control Conference, pp. 1329–1333, IEEE, San Diego, Calif, USA, June 1999.
View at: Publisher Site | Google Scholar
U. Frese, “A proof for the approximate sparsity of SLAM information matrices,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '05), pp. 329–335, April 2005.
View at: Publisher Site | Google Scholar
S. Thrun, Y. Liu, and D. Koller, “Simultaneous mapping and localization with sparse extended information filters: theory and initial results,” in Algorithmic Foundations of Robotics V, vol. 7 of Springer Tracts in Advanced Robotics, pp. 363–380, Springer, Berlin, Germany, 2004.
View at: Publisher Site | Google Scholar
Y. Liu and S. Thrun, “Results for outdoor-SLAM using sparse extended information filters,” in Proceedings of the IEEE International Conference on Robotics and Automation, vol. 1, pp. 1227–1233, September 2003.
View at: Google Scholar
X. Wang and L. Ma, “Sparse extended information filter for feature-based SLAM,” in Proceedings of the International Conference on Electronics, Communications and Control (ICECC '11), pp. 2290–2293, 2011.
View at: Google Scholar
R. Eustice, M. Walter, and J. Leonard, “Sparse extended information filters: insights into sparsification,” in IEEE IRS/RSJ International Conference on Intelligent Robots and Systems (IROS '05), pp. 3281–3288, Alberta, Canada, August 2005.
View at: Publisher Site | Google Scholar
T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, “Consistency of the EKF-SLAM algorithm,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '06), pp. 3562–3568, Beijing, China, October 2006.
View at: Publisher Site | Google Scholar
S. D. Huang and G. Dissanayake, “Convergence and consistency analysis for extended Kalman filter based SLAM,” IEEE Transactions on Robotics, vol. 23, no. 5, pp. 1036–1049, 2007.
View at: Publisher Site | Google Scholar
G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “Observability-based rules for designing consistent EKF SLAM estimators,” The International Journal of Robotics Research, vol. 29, no. 5, pp. 502–528, 2010.
View at: Publisher Site | Google Scholar
M. R. Walter, R. M. Eustice, and J. J. Leonard, “Exactly sparse extended information filters for feature-based SLAM,” International Journal of Robotics Research, vol. 26, no. 4, pp. 335–359, 2007.
View at: Publisher Site | Google Scholar
M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: incremental smoothing and mapping,” IEEE Transactions on Robotics, vol. 24, no. 6, pp. 1365–1378, 2008.
View at: Publisher Site | Google Scholar
M. Montemerlo and S. Thrun, FastSLAM: A Scalable Method for the Simultaneous Localization and Mapping Problem in Robotics, Springer Tracts in Advanced Robotics, Springer, 2007.
M. Walter, F. Hover, and J. Leonard, “SLAM for ship hull inspection using exactly sparse extended information filters,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '08), pp. 1463–1470, May 2008.
View at: Publisher Site | Google Scholar
S. Huang, Z. Wang, and G. Dissanayake, “Sparse local submap joining filter for building large-scale maps,” IEEE Transactions on Robotics, vol. 24, no. 5, pp. 1121–1130, 2008.
View at: Publisher Site | Google Scholar
K. W. Lee, W. S. Wijesoma, and I. G. Javier, “On the observability and observability analysis of SLAM,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '06), pp. 3569–3574, Beijing, China, October 2006.
View at: Publisher Site | Google Scholar
G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “Analysis and improvement of the consistency of extended Kalman filter based SLAM,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '08), pp. 473–479, Passadena, Calif, USA, May 2008.
View at: Publisher Site | Google Scholar
G. Huang, A. I. Mourikis, and S. I. Roumeliotis, “A first-estimates Jacobian EKF for improving SLAM consistency,” in Experimental Robotics, O. Khatib, V. Kumar, and G. Pappas, Eds., vol. 54 of Springer Tracts in Advanced Robotics, pp. 373–382, Springer, 2009.
View at: Google Scholar
B. He, B. R. Wang, T. H. Yan, and Y. Y. Han, “A distributed parallel motion control for the multi-thruster autonomous underwater vehicle,” Mechanics Based Design of Structures and Machines, vol. 41, no. 2, pp. 236–257, 2013.
View at: Publisher Site | Google Scholar
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 Site | Google Scholar
B. He, H. Zhang, C. Li, S. Zhang, Y. Liang, and T. Yan, “Autonomous navigation for autonomous underwater vehicles based on information filters and active sensing,” Sensors, vol. 11, no. 11, pp. 10958–10980, 2011.
View at: Publisher Site | Google Scholar
Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software, Wiley-Interscience, 2004.
V. Ila, J. M. Porta, and J. Andrade-Cetto, “Information-based compact pose SLAM,” IEEE Transactions on Robotics, vol. 26, no. 1, pp. 78–93, 2010.
View at: Publisher Site | Google Scholar
K. P. B. Chandra, D.-W. Gu, and I. Postlethwaite, “Square root cubature information filter,” IEEE Sensors Journal, vol. 13, no. 2, pp. 750–758, 2013.
View at: Publisher Site | Google Scholar

Copyright

Copyright © 2015 Yue Shen 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.

PDF Download Citation

Download other formats

Order printed copies

Views

928

Downloads

949

Citations