The invention belongs to the field of an
underwater vehicle and discloses a robust BSLAM method for AUV
terrain matching navigation. The method comprises the steps of (1), inputting
depth sounding andinertia navigation data; (2), carrying out data preprocessing to obtain filtered
depth sounding data; (3), establishing an attitude graph, specifically, computing weak
data association, establishinga new subgraph, carrying out
closed loop detection through
terrain positioning, and then carrying out invalid
closed loop detection; and (4), optimizing a rear end graph, specifically, computing consistency functions through utilization of clustered
closed loop data, finding out the minimum values of the consistency functions in all classes, integrating data of key statuses of
inertia navigation,and outputting a modified BSLAM track. According to the method, through
processing and closed loop detection of the
depth sounding and
inertia navigation data,
global optimization of the data is realized, timeliness and consistency of positioning and graph establishment are improved, robustness is high, and influence of noises of measurement data on navigation precision can be reduced.