Autonomous navigation method of AUV (Autonomous Underwater Vehicle) based on Unscented FastSLAM (Simultaneous Localization and Mapping) algorithm

Abstract

The invention discloses an autonomous navigation method of an AUV (Autonomous Underwater Vehicle) based on a FastSLAM (Simultaneous Localization and Mapping) algorithm. The autonomous navigation method comprises the steps that 1) the AUV acquires initial pose and position information through the GPS and a navigation sensor on the water surface; 2) predicting the pose and position and an environmental road sign of the AUV by adopting unscented particle filtering according to latest control variables inputted into the AUV and observation variables of the sensor; 3) generating a proposal distribution function for parameter adaptive adjustment by adopting fading adaptive unscented particle filtering, and sampling in the proposal distribution function; 4) associating the latest observation environment information according to each particle, and updating estimation for each characteristic by adopting unscented Kalman filtering; 5) performing resampling on a particle set by adopting an adaptive partial system resampling method; and 6) performing AUV positioning and map building. The autonomous navigation method can improve the particle sampling efficiency of the Unscented FastSLAM algorithm and reduce the degradation degree of the particles through improving the proposal distribution function and the resampling process of the Unscented FastSLAM algorithm, thereby enabling the consistency of AUV pose and position estimation and the accuracy of autonomous navigation to be greatly improved.

Description

technical field [0001] The invention relates to an AUV autonomous navigation method, in particular to an AUV autonomous navigation method based on the Unscented FastSLAM algorithm, which belongs to the technical field of robot navigation. Background technique [0002] An important topic of AUV (underwater robot) research is the study of autonomous navigation. Autonomous navigation ensures that the robot moves and performs tasks in a known environment, and the prior map at this time is known. However, the seabed map information in the actual situation is unknown, so to truly realize AUV autonomous navigation, the robot must have the ability of simultaneous positioning and map construction. [0003] Simultaneous localization and mapping (SLAM) can enable AUV to move in an unknown environment, and carry sensors, such as sonar, cameras, etc., to continuously collect the surrounding environmental information and realize its own pose. , while incrementally constructing a global e...

AI Technical Summary

Problems solved by technology

[0005] Although FastSLAM solves the complexity and data association problems of the EKF-SLAM algorithm very well, in the standard FastSLAM process, due to the particle filter resampling, the samples with larger weights will be selected multiple times, resulting in the sampling results. The decrease in sample diversity makes particle poverty appear in the particle filter, which may eventually lead to a decrease in the accuracy of SLAM

At the same time, EKF estimates highly nonlinear motion models and measurement models, which may cause divergence and inconsistency in estimates

On the other hand, EKF needs to calculate the Jacobian matrix, which has a certain complexity

The patent document with the application number "200910100962.3" discloses a "FastSLAM algorithm based on improved resampling method and particle selection", but the consistency of the EKF algorithm used is poor, which makes AUV easy to diverge during autonomous navigation, and the adopted The real-time performance of the resampling algorithm is not high; the patent document with the application number "201410156978.7" discloses "a method and robot based on infinite FastSLAM algorithm and matching optimization target positioning", which combines particle filter and matching optimization positioning algorithm, but The accuracy of state estimation is not high, and problems such as divergence are prone to occur

Images