CN109186610B - Robust BSLAM method for AUV terrain matching navigation - Google Patents

Robust BSLAM method for AUV terrain matching navigation Download PDF

Info

Publication number
CN109186610B
CN109186610B CN201811197129.0A CN201811197129A CN109186610B CN 109186610 B CN109186610 B CN 109186610B CN 201811197129 A CN201811197129 A CN 201811197129A CN 109186610 B CN109186610 B CN 109186610B
Authority
CN
China
Prior art keywords
data
closed
bslam
depth
subgraph
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811197129.0A
Other languages
Chinese (zh)
Other versions
CN109186610A (en
Inventor
李晔
徐硕
马腾
丛正
贡雨森
王汝鹏
武皓微
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201811197129.0A priority Critical patent/CN109186610B/en
Publication of CN109186610A publication Critical patent/CN109186610A/en
Application granted granted Critical
Publication of CN109186610B publication Critical patent/CN109186610B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Abstract

The invention belongs to the field of underwater submergence vehicles, and discloses a robust BSLAM method for AUV terrain matching navigation, which comprises the following steps of (1): inputting sounding and inertial navigation data; step (2): preprocessing data to obtain filtered sounding data; and (3): constructing a pose graph: calculating weak data association, constructing a new subgraph, performing closed-loop detection through the terrain status, and performing invalid closed-loop detection; and (4): optimizing a back-end graph: and calculating a consistency function by using the clustered closed-loop data, finding out the minimum value of the consistency function in all classes, then fusing the data of the key state of inertial navigation and outputting the modified BSLAM track. According to the invention, through processing and closed-loop detection of the sounding data and the inertial navigation data, global optimization of the data is realized, the real-time performance and consistency of positioning and mapping are improved, the robustness is good, and the influence of noise of the measured data on the navigation precision can be weakened.

Description

Robust BSLAM method for AUV terrain matching navigation
Technical Field
The invention belongs to the field of underwater submerging devices, and particularly relates to a robust BSLAM method for AUV terrain matching navigation.
Background
Underwater robots are mainly divided into two main categories: one is a cabled underwater robot, which is conventionally called a remote control submersible, abbreviated as ROV; the other type is a cableless underwater robot, which is commonly called as an Autonomous Underwater Vehicle (AUV) for short. The autonomous underwater robot integrates artificial intelligence and other task controllers of advanced computing technology, integrates high technologies such as a deep submergence vehicle, a sensor, an environmental effect, computer software, energy storage, conversion and propulsion, a new material and a new process, an underwater intelligent weapon and the like, is used in the fields of anti-submergence warfare, mine warfare, reconnaissance and monitoring, logistic support and the like in military, has the advantages of large range of activity, good maneuverability, safety, intellectualization and the like, and becomes an important tool for completing various underwater tasks. For example, in the civil field, the system can be used for pipeline laying, submarine investigation, data collection, drilling support, submarine construction, maintenance and repair of underwater equipment and the like; in the military field, the mine-clearing and life-saving device can be used for reconnaissance, mine laying, mine sweeping, rescue and rescue, and the like. Since the cableless underwater robot has the advantages of no limitation of a moving range by a cable, good concealment and the like, from the middle of the 60 s, the industry and the military began to have interest in the cableless underwater robot.
When the AUV moves underwater, an inertial navigation system is generally adopted, and belongs to a dead reckoning navigation mode, namely, the position of the next point is calculated from the position of a known point according to the continuously measured course angle and speed of the moving body, so that the current position of the moving body can be continuously measured. A gyroscope in the inertial navigation system is used for forming a navigation coordinate system, so that a measuring axis of the accelerometer is stabilized in the coordinate system, and a course and an attitude angle are given; the accelerometer is used for measuring the acceleration of the moving body, the speed is obtained through the first integration of the time, and the displacement can be obtained through the first integration of the speed and the time. However, after a long time operation, the errors generated will be accumulated. Currently, the BSLAM system is used to correct the inertial navigation accumulated error.
The patent with publication number CN107403163A discloses a laser SLAM closed-loop detection method based on deep learning, which converts the problem of SLAM closed-loop detection into the problem of retrieval of SALM data samples, and creatively constructs a deep Hash network to perform Hash coding on laser point cloud samples, and further performs sample similarity calculation on the basis of Hash coding, thereby realizing rapid retrieval of similar samples and SLAM closed-loop detection. However, the laser sensor cannot be used underwater, and the sounding data contains a lot of noise, which can generate an invalid closed loop, so that the method is not suitable for underwater navigation.
The patent with publication number CN7132521A discloses a method for judging the correctness of a terrain matching result in BSLAM, which inputs terrain matching data and odometer data, and realizes the judgment of the validity of terrain matching by continuously iterating and calculating a consistency function. Self-checking and multi-window methods are introduced in iteration, and the purposes of the method are respectively to avoid falling into local optimum and simultaneously ensure global consistency and local consistency. But the robust filtering method has poor reliability and poor anti-interference capability.
Disclosure of Invention
The invention aims to disclose a robust BSLAM method for AUV terrain matching navigation, which has good robustness and high error control precision.
The purpose of the invention is realized as follows:
a robust BSLAM method for AUV terrain matching navigation comprises the following steps:
step (1): inputting sounding and inertial navigation data: when the AUV sails on the seabed for a long time and the inertial navigation data drift is overlarge, initializing the BSLAM, starting the multi-beam sonar, collecting the depth measurement and inertial navigation data of the submarine topography, and continuously inputting the data into the BSLAM program;
step (2): data preprocessing: effectively eliminating field values in the depth sounding data of the multi-beam sonar by using a single ping filtering method of the multi-beam depth sounding data based on an Alpha-Shapes model to obtain filtered depth sounding data;
and (3): constructing a pose graph: estimating the terrain depth by utilizing Gaussian process regression to form weak data association; after the depth measurement and navigation data collected by the AUV exceed threshold values, storing the data in the current sub-graph and constructing a new sub-graph; after a new subgraph is constructed, performing closed-loop detection through terrain positioning, and performing invalid closed-loop detection:
step (3.1): inputting the preprocessed data: inputting inertial navigation data and filtered sounding data;
step (3.2): establishing weak data association:
regression and filtered sounding data z according to the Gaussian process+Obtaining an estimated depth z of the surroundings-Establishing a weak data association L (x)t；xt-1)：
Figure GDA0002660405230000021
In the above formula, the first and second carbon atoms are,
Figure GDA0002660405230000022
is the gaussian estimated depth of the kth sample point,
Figure GDA0002660405230000023
is the measured depth of the kth sample point, N is the number of sample points,
Figure GDA0002660405230000024
is the measured variance of the sample points,
Figure GDA0002660405230000025
is a diagonal matrix composed of measurement variances;
step (3.3): subgraph construction: storing the sounding data and the pose data in the current sub map after reaching a certain threshold value, and constructing a new sub map;
step (3.4): after the new subgraph is constructed, carrying out terrain positioning, calculating the average correction value of the new subgraph and the historical subgraph, and judging whether a closed loop is generated:
the average correction value of the new subgraph and the historical subgraph is as follows:
Figure GDA0002660405230000031
in the above formula, (y)t,k-hk(xt))2Represents a state xtCorrection values at k measurement points, N being the number of sampling points, yt,kRepresenting the measured depth, h, of the sample point kk(xt) Representing the depth of a corresponding point in the history subgraph;
if T (x)t) Less than a certain threshold value, thenIf the judgment result is that closed loops are generated, the step (3.5) is carried out, otherwise, the step (3.1) is returned;
step (3.5): and (3) dividing: in the frequency domain, checking whether the closed-loop correction distribution satisfies the Gaussian distribution, and taking N (mu, sigma)2)(μ＝±0.5,σ20.4); in a space domain, extracting all measuring points with the correction value larger than mu +/-sigma in the two matched subgraphs as a region 2, and taking the rest region as a region 1;
step (3.6): fusing topographic information: in a map with the elevation h (i, j) at the coordinate (i, j), a plurality of physical quantities are fused by utilizing a gray theory to describe the terrain information of the area 1 and the area 2, and the gray relevance degree r of the area i is calculatedi：
The optimal sequence for each region is first calculated:
x0＝[max(RT(i)),min(hT(i)),max(σT(i)),max(FT(i))](i＝1,2,...,k)；
the grey relation coefficient is:
Figure GDA0002660405230000032
in the above formula, the resolution ρ ∈ [0,1 ]]，RTIs the roughness of the terrain, HTIs the topographic entropy, σTIs the terrain standard deviation, FTIs Fisher information;
grey relevance r of area iiComprises the following steps:
Figure GDA0002660405230000033
in the above formula, w1，w2,w3And w4Is Fisher information FTThe weight of (c);
step (3.7): judging whether the closed loop is effective: identifying an invalid closed loop by using an artificial neural network and reserving the valid closed loop; inputting average correction values of the new subgraph and the historical subgraph, preprocessing information and the number of effective points of the region 1 and the region 2, if the output value is less than a threshold value of 0.1, determining that the closed loop is an effective closed loop, and entering the step (4); otherwise, the closed loop is invalid, and the step (3.1) is returned.
And (4): optimizing a back-end graph: and calculating a consistency function by using the clustered closed-loop data, finding out the minimum value of the consistency function in all classes, then fusing the data of the key state of inertial navigation and outputting the modified BSLAM track.
Step (4.1): inputting closed-loop data: initializing, namely enabling k to be 0, inputting N closed loops, initializing an OPEN table and a CLOSE table, putting all the closed loops into the OPEN table, and emptying the CLOSE table;
step (4.2): clustering and calculating a consistency function:
clustering all closed loops, and calculating consistency functions of all the classes in local optimization and global optimization:
Figure GDA0002660405230000041
in the above formula, the first and second carbon atoms are,
Figure GDA0002660405230000042
is the AUV state obtained from the inertial navigation data,
Figure GDA0002660405230000043
AUV state from BSLAM, j is the number of poses;
step (4.3): judging whether the continuity condition is met: setting a threshold value, and judging χ2If the value is smaller than the threshold value, entering a step (4.7), otherwise entering a step (4.4);
step (4.4): let k be k +1, calculate the consistency function of all classes in the OPEN table in turn
Figure GDA0002660405230000044
And minimum value
Figure GDA0002660405230000045
For class i, i ═ 1,2, N in the OPEN table, the classes are sequentially taken out of the OPEN table and calculated
Figure GDA0002660405230000046
Finding the minimum
Figure GDA0002660405230000047
And minimize the value
Figure GDA0002660405230000048
Moving the corresponding class from the OPEN table to the CLOSE table;
step (4.5): if it is not
Figure GDA0002660405230000049
Is less than
Figure GDA00026604052300000410
Entering the step (4.3), otherwise entering the step (4.6);
step (4.6): for class j, j ═ 1,2, M in the CLOSE table, the classes are sequentially taken out of the CLOSE table and calculated
Figure GDA00026604052300000411
j ═ 1,2,. said, M; if it is not
Figure GDA00026604052300000412
Minimum value of
Figure GDA00026604052300000413
Is less than
Figure GDA00026604052300000414
Then the minimum value will be
Figure GDA00026604052300000415
The corresponding class is moved from the CLOSE table to the OPEN table and step (4.6) is executed, otherwise step (4.4) is entered;
step (4.7): and fusing data of the key state of inertial navigation and outputting the modified BSLAM track.
The invention has the beneficial effects that:
according to the invention, through processing and closed-loop detection of the sounding data and the inertial navigation data, global optimization of the data is realized, global consistency is ensured, real-time performance and consistency of positioning and mapping are improved, robustness is good, and influence of noise of the measured data on navigation precision can be weakened.
Drawings
FIG. 1 is a flow chart of a robust BSLAM method for AUV terrain-matched navigation;
FIG. 2 is a pose graph construction flow diagram;
fig. 3 is a block diagram of a robust filtering flow.
Detailed Description
The invention is further described below with reference to the accompanying drawings:
as shown in fig. 1, a robust BSLAM method for AUV terrain-matched navigation includes the following steps:
step (1): inputting sounding and inertial navigation data: when the AUV sails on the seabed for a long time and the inertial navigation data drift is overlarge, initializing the BSLAM, starting the multi-beam sonar, collecting the depth measurement and inertial navigation data of the submarine topography, and continuously inputting the data into the BSLAM program;
step (2): data preprocessing: effectively eliminating field values in the depth sounding data of the multi-beam sonar by using a single ping filtering method of the multi-beam depth sounding data based on an Alpha-Shapes model to obtain filtered depth sounding data;
and (3): as shown in fig. 2, a pose graph is constructed: estimating the terrain depth by utilizing Gaussian process regression to form weak data association; after the depth measurement and navigation data collected by the AUV exceed threshold values, storing the data in the current sub-graph and constructing a new sub-graph; after a new subgraph is constructed, performing closed-loop detection through terrain positioning, and performing invalid closed-loop detection:
step (3.1): inputting the preprocessed data: inputting inertial navigation data and filtered sounding data;
step (3.2): establishing weak data association:
regression and filtered sounding data z according to the Gaussian process+Obtaining an estimated depth z of the surroundings-Establishing a weak data association L (x)t；xt-1)：
Figure GDA0002660405230000051
In the above formula, the first and second carbon atoms are,
Figure GDA0002660405230000061
is the gaussian estimated depth of the kth sample point,
Figure GDA0002660405230000062
is the measured depth of the kth sample point, N is the number of sample points,
Figure GDA0002660405230000063
is the measured variance of the sample points,
Figure GDA0002660405230000064
is a diagonal matrix composed of measurement variances;
step (3.3): subgraph construction: storing the sounding data and the pose data in the current sub map after reaching a certain threshold value, and constructing a new sub map;
step (3.4): after the new subgraph is constructed, carrying out terrain positioning, calculating the average correction value of the new subgraph and the historical subgraph, and judging whether a closed loop is generated:
the average correction value of the new subgraph and the historical subgraph is as follows:
Figure GDA0002660405230000065
in the above formula, (y)t,k-hk(xt))2Represents a state xtCorrection values at k measurement points, N being the number of sampling points, yt,kRepresenting the measured depth, h, of the sample point kk(xt) Representing the depth of a corresponding point in the history subgraph;
if T (x)t) If the value is less than a certain threshold value, judging that a closed loop is generated, and entering the step (3.5), otherwise, returning to the step (3.1);
step (3.5): and (3) dividing: in the frequency domain, checking whether the closed loop correction profile is fullGaussian distribution, taking N (mu, sigma)2)(μ＝±0.5,σ20.4); in a space domain, extracting all measuring points with the correction value larger than mu +/-sigma in the two matched subgraphs as a region 2, and taking the rest region as a region 1;
step (3.6): fusing topographic information: for a map of elevation h (i, j) at coordinates (i, j), a common quantity of topographical information is topographical roughness RTEntropy of topography HTTopographic standard deviation σTAnd Fisher information FT. However, a single quantity does not completely describe the terrain information, so all quantities are fused using grey theory to describe the terrain information for region 1 and region 2:
the optimal sequence for each region is first calculated:
x0＝[max(RT(i)),min(hT(i)),max(σT(i)),max(FT(i))](i＝1,2,...,k)；
the grey relation coefficient is:
Figure GDA0002660405230000066
in the above formula, the resolution ρ ∈ [0,1 ]]，RTIs the roughness of the terrain, HTIs the topographic entropy, σTIs the terrain standard deviation, FTIs Fisher information;
grey relevance r of area iiComprises the following steps:
Figure GDA0002660405230000071
in the above formula, w1，w2,w3And w4Is Fisher information FTThe weight of (c);
degree of gray correlation riThe larger the terrain information, the better.
Step (3.7): judging whether the closed loop is effective: identifying an invalid closed loop by using an artificial neural network and reserving the valid closed loop; inputting average correction values of the new subgraph and the historical subgraph, preprocessing information and the number of effective points of the region 1 and the region 2, if the output value is less than a threshold value of 0.1, determining that the closed loop is an effective closed loop, and entering the step (4); otherwise, the closed loop is invalid, and the step (3.1) is returned.
And (4): as in fig. 3, the back-end graph is optimized: the back-end graph optimization comprises local optimization and global optimization. And detecting that the state of the closed loop becomes a key state, distributing the navigation deviation of the key state to all states according to weak data association by local optimization, and then carrying out global optimization. And (3) calculating a global consistency function of the map through global optimization, fusing inertial navigation data if the global consistency function is smaller than a threshold value, outputting a BSLAM track, and ending the algorithm, otherwise, performing robust filtering to remove the worst closed loop until the global consistency is smaller than the threshold value.
Step (4.1): inputting closed-loop data: initializing, namely enabling k to be 0, inputting N closed loops, initializing an OPEN table and a CLOSE table, putting all the closed loops into the OPEN table, and emptying the CLOSE table;
step (4.2): clustering and calculating a consistency function:
clustering all closed loops, and calculating consistency functions of all the classes in local optimization and global optimization:
Figure GDA0002660405230000072
in the above formula, the first and second carbon atoms are,
Figure GDA0002660405230000073
is the AUV state obtained from the inertial navigation data,
Figure GDA0002660405230000074
AUV state from BSLAM, j is the number of poses;
step (4.3): judging whether the continuity condition is met: setting a threshold value, and judging χ2If the value is smaller than the threshold value, entering a step (4.7), otherwise entering a step (4.4);
step (4.4): let k be k +1, calculate the consistency function of all classes in the OPEN table in turn
Figure GDA0002660405230000081
And minimum value
Figure GDA0002660405230000082
For class i, i ═ 1,2, N in the OPEN table, the classes are sequentially taken out of the OPEN table and calculated
Figure GDA0002660405230000083
Finding the minimum
Figure GDA0002660405230000084
And minimize the value
Figure GDA0002660405230000085
Moving the corresponding class from the OPEN table to the CLOSE table;
step (4.5): if it is not
Figure GDA0002660405230000086
Is less than
Figure GDA0002660405230000087
Entering the step (4.3), otherwise entering the step (4.6);
step (4.6): for class j, j ═ 1,2, M in the CLOSE table, the classes are sequentially taken out of the CLOSE table and calculated
Figure GDA0002660405230000088
j ═ 1,2,. said, M; if it is not
Figure GDA0002660405230000089
Minimum value of
Figure GDA00026604052300000810
Is less than
Figure GDA00026604052300000811
Then the minimum value will be
Figure GDA00026604052300000812
The corresponding class is moved from the CLOSE table to the OPEN table and step (4.6) is executed, otherwise step is entered(4.4)；
Step (4.7): and fusing data of the key state of inertial navigation and outputting the modified BSLAM track.
Compared with the prior art, the method has the advantages that global optimization of data is realized through processing of sounding data and inertial navigation data and closed-loop detection, global consistency is guaranteed, real-time performance and consistency of positioning and mapping are improved, robustness is good, and influence of noise of measured data on navigation precision can be weakened.
The above description is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (2)

1. A robust BSLAM method for AUV terrain matching navigation is characterized in that: comprises the following steps:
step (1): inputting sounding and inertial navigation data: when the AUV sails on the seabed for a long time and the inertial navigation data drift is overlarge, initializing the BSLAM, starting the multi-beam sonar, collecting the depth measurement and inertial navigation data of the submarine topography, and continuously inputting the data into the BSLAM program;
step (2): data preprocessing: effectively eliminating field values in the depth sounding data of the multi-beam sonar by using a single ping filtering method of the multi-beam depth sounding data based on an Alpha-Shapes model to obtain filtered depth sounding data;
and (3): constructing a pose graph: estimating the terrain depth by utilizing Gaussian process regression to form weak data association; after the depth measurement and navigation data collected by the AUV exceed threshold values, storing the data in the current sub-graph and constructing a new sub-graph; after a new subgraph is constructed, performing closed-loop detection through topographic positioning, and then performing invalid closed-loop detection;
and (4): optimizing a back-end graph: calculating a consistency function by using the clustered closed-loop data, finding out the minimum value of the consistency function in all classes, then fusing the data of the key state of inertial navigation and outputting a modified BSLAM track;
the step (3) is specifically as follows:
step (3.1): inputting the preprocessed data: inputting inertial navigation data and filtered sounding data;
step (3.2): establishing weak data association:
regression and filtered sounding data z according to the Gaussian process+Obtaining an estimated depth z of the surroundings-Establishing a weak data association L (x)t；xt-1)：
Figure FDA0002660405220000011
In the above formula, the first and second carbon atoms are,
Figure FDA0002660405220000012
is the gaussian estimated depth of the kth sample point,
Figure FDA0002660405220000013
is the measured depth of the kth sample point, N is the number of sample points,
Figure FDA0002660405220000014
is the measured variance of the sample points,
Figure FDA0002660405220000015
is a diagonal matrix composed of measurement variances;
step (3.3): subgraph construction: storing the sounding data and the pose data in the current sub map after reaching a certain threshold value, and constructing a new sub map;
step (3.4): after the new subgraph is constructed, carrying out terrain positioning, calculating the average correction value of the new subgraph and the historical subgraph, and judging whether a closed loop is generated:
the average correction value of the new subgraph and the historical subgraph is as follows:
Figure FDA0002660405220000021
in the above formula, (y)t,k-hk(xt))2Represents a state xtCorrection values at k measurement points, N being the number of sampling points, yt,kRepresenting the measured depth, h, of the sample point kk(xt) Representing the depth of a corresponding point in the history subgraph;
if T (x)t) If the value is less than a certain threshold value, judging that a closed loop is generated, and entering the step (3.5), otherwise, returning to the step (3.1);
step (3.5): and (3) dividing: in the frequency domain, checking whether the closed-loop correction distribution satisfies the Gaussian distribution, and taking N (mu, sigma)2)(μ＝±0.5,σ20.4); in a space domain, extracting all measuring points with the correction value larger than mu +/-sigma in the two matched subgraphs as a region 2, and taking the rest region as a region 1;
step (3.6): fusing topographic information: in a map with the elevation h (i, j) at the coordinate (i, j), a plurality of physical quantities are fused by utilizing a gray theory to describe the terrain information of the area 1 and the area 2, and the gray relevance degree r of the area i is calculatedi：
The optimal sequence for each region is first calculated:
x0＝[max(RT(i)),min(hT(i)),max(σT(i)),max(FT(i))](i＝1,2,...,k)；
the grey relation coefficient is:
Figure FDA0002660405220000022
in the above formula, the resolution ρ ∈ [0,1 ]]，RTIs the roughness of the terrain, HTIs the topographic entropy, σTIs the terrain standard deviation, FTIs Fisher information;
grey relevance r of area iiComprises the following steps:
Figure FDA0002660405220000023
in the above formula, w1，w2,w3And w4Is Fisher information FTThe weight of (c);
step (3.7): judging whether the closed loop is effective: identifying an invalid closed loop by using an artificial neural network and reserving the valid closed loop; inputting average correction values of the new subgraph and the historical subgraph, preprocessing information and the number of effective points of the region 1 and the region 2, if the output value is less than a threshold value of 0.1, determining that the closed loop is an effective closed loop, and entering the step (4); otherwise, the closed loop is invalid, and the step (3.1) is returned.
2. The robust BSLAM method for AUV terrain-matched navigation according to claim 1, wherein: the step (4) is specifically as follows:
step (4.1): inputting closed-loop data: initializing, namely enabling k to be 0, inputting N closed loops, initializing an OPEN table and a CLOSE table, putting all the closed loops into the OPEN table, and emptying the CLOSE table;
step (4.2): clustering and calculating a consistency function:
clustering all closed loops, and calculating consistency functions of all the classes in local optimization and global optimization:
Figure FDA0002660405220000031
in the above formula, the first and second carbon atoms are,
Figure FDA0002660405220000032
is the AUV state obtained from the inertial navigation data,
Figure FDA0002660405220000033
AUV state from BSLAM, j is the number of poses;
step (4.3): judging whether the continuity condition is met: setting a threshold value, and judging χ2If the value is smaller than the threshold value, entering a step (4.7), otherwise entering a step (4.4);
step (4.4): let k be k +1, calculate the consistency of all classes in the OPEN table in turnFunction(s)
Figure FDA0002660405220000034
And minimum value
Figure FDA0002660405220000035
For class i, i ═ 1,2, N in the OPEN table, the classes are sequentially taken out of the OPEN table and calculated
Figure FDA0002660405220000036
Finding the minimum
Figure FDA0002660405220000037
And minimize the value
Figure FDA0002660405220000038
Moving the corresponding class from the OPEN table to the CLOSE table;
step (4.5): if it is not
Figure FDA0002660405220000039
Is less than
Figure FDA00026604052200000310
Entering the step (4.3), otherwise entering the step (4.6);
step (4.6): for class j, j ═ 1,2, M in the CLOSE table, the classes are sequentially taken out of the CLOSE table and calculated
Figure FDA00026604052200000311
Figure FDA00026604052200000312
If it is not
Figure FDA00026604052200000313
Minimum value of
Figure FDA00026604052200000314
Is less than
Figure FDA00026604052200000315
Then the minimum value will be
Figure FDA00026604052200000316
The corresponding class is moved from the CLOSE table to the OPEN table and step (4.6) is executed, otherwise step (4.4) is entered;
step (4.7): and fusing data of the key state of inertial navigation and outputting the modified BSLAM track.
CN201811197129.0A 2018-10-15 2018-10-15 Robust BSLAM method for AUV terrain matching navigation Active CN109186610B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811197129.0A CN109186610B (en) 2018-10-15 2018-10-15 Robust BSLAM method for AUV terrain matching navigation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811197129.0A CN109186610B (en) 2018-10-15 2018-10-15 Robust BSLAM method for AUV terrain matching navigation

Publications (2)

Publication Number Publication Date
CN109186610A CN109186610A (en) 2019-01-11
CN109186610B true CN109186610B (en) 2020-12-08

Family

ID=64944585

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811197129.0A Active CN109186610B (en) 2018-10-15 2018-10-15 Robust BSLAM method for AUV terrain matching navigation

Country Status (1)

Country Link
CN (1) CN109186610B (en)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110006428A (en) * 2019-01-21 2019-07-12 武汉大学 A kind of overlay path method and device for planning based on unmanned plane energy
CN110260859B (en) * 2019-06-06 2022-12-13 哈尔滨工程大学 Pose graph correction method considering coupling relation between data associations
CN110320520B (en) * 2019-06-26 2022-08-02 哈尔滨工程大学 Robust back-end graph optimization method for depth measurement information synchronous positioning and graph building
CN110675450B (en) * 2019-09-06 2020-09-29 武汉九州位讯科技有限公司 Method and system for generating orthoimage in real time based on SLAM technology
CN110763234B (en) * 2019-10-15 2022-10-28 哈尔滨工程大学 Submarine topography matching navigation path planning method for underwater robot
CN111220146B (en) * 2019-12-10 2022-10-14 哈尔滨工程大学 Underwater terrain matching and positioning method based on Gaussian process regression learning
CN113008223B (en) * 2021-03-03 2022-10-28 哈尔滨工程大学 Seabed terrain synchronous positioning and mapping method based on particle filtering and mapping optimization
CN113256697B (en) * 2021-04-27 2023-07-18 武汉理工大学 Three-dimensional reconstruction method, system, device and storage medium for underwater scene
CN114216465B (en) * 2021-12-08 2022-11-01 哈尔滨工程大学 Submarine topography navigation parallel matching method

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104075715A (en) * 2014-07-07 2014-10-01 东南大学 Underwater navigation and positioning method capable of combining terrain and environment characteristics
CN107132521A (en) * 2017-05-16 2017-09-05 哈尔滨工程大学 A kind of BSLAM mesorelief matching result correctness method of discrimination

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104075715A (en) * 2014-07-07 2014-10-01 东南大学 Underwater navigation and positioning method capable of combining terrain and environment characteristics
CN107132521A (en) * 2017-05-16 2017-09-05 哈尔滨工程大学 A kind of BSLAM mesorelief matching result correctness method of discrimination

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
An Efficient Approach to Bathymetric SLAM;Stephen Barkby et al.;《2009 IEEE/RSJ International Conference on Intelligent Robots and Systems》;20091215;第219-224页 *
Evaluation of spatial resolution and estimation error of seafloor displacement observation from vessel-based bathymetric survey by use of AUV-based bathymetric data;Fujiwara et al.;《MARINE GEOPHYSICAL RESEARCH》;20150331;第36卷;第45-60页 *
Survey on advances on terrain based navigation for autonomous underwater vehicles;José Melo et al.;《Ocean Engineering》;20170512;第139卷;第250-264页 *

Also Published As

Publication number Publication date
CN109186610A (en) 2019-01-11

Similar Documents

Publication Publication Date Title
CN109186610B (en) Robust BSLAM method for AUV terrain matching navigation
CN110703762B (en) Hybrid path planning method for unmanned surface vehicle in complex environment
EP3737912B1 (en) Determining the location of a mobile device
CN112639502A (en) Robot pose estimation
Laftchiev et al. Vehicle localization using in-vehicle pitch data and dynamical models
Jacobson et al. Semi-supervised slam: Leveraging low-cost sensors on underground autonomous vehicles for position tracking
CN111002346B (en) Robot trapped detection method and robot
CN111707272A (en) Underground garage automatic driving laser positioning system
JP2016110576A (en) Self position estimation device and mobile body with self position estimation device
JP7047576B2 (en) Cartography device
Huang et al. An autonomous navigation algorithm for underwater vehicles based on inertial measurement units and sonar
Tang et al. OdoNet: Untethered speed aiding for vehicle navigation without hardware wheeled odometer
Zhang et al. An integrated navigation method for small-sized AUV in shallow-sea applications
Garcia et al. Portable multi-hypothesis Monte Carlo localization for mobile robots
Cheng et al. Graph-based proprioceptive localization using a discrete heading-length feature sequence matching approach
CN116385493A (en) Multi-moving-object detection and track prediction method in field environment
CN115373383A (en) Autonomous obstacle avoidance method and device for garbage recovery unmanned boat and related equipment
CN116563341A (en) Visual positioning and mapping method for processing dynamic object in complex environment
Sim et al. Scalable real-time vision-based SLAM for planetary rovers
Hu et al. A modified particle filter for simultaneous robot localization and landmark tracking in an indoor environment
CN113917930A (en) Unmanned ship navigation state control method based on sensing data
CN113554705A (en) Robust positioning method for laser radar in changing scene
Badalkhani et al. Effects of Moving Landmark's Speed on Multi-Robot Simultaneous Localization and Mapping in Dynamic Environments.
Ma et al. A contour-based path planning method for terrain-aided navigation systems with a single beam echo sounder
Huang et al. Seafloor obstacle detection by sidescan sonar scanlines for submarine cable construction

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant