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

摘要

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.