CN115031733B - An active synchronous positioning and mapping method for multi-beam bathymetry of underwater robots - Google Patents

Abstract

The invention belongs to the technical field of underwater robot positioning and navigation, and particularly relates to an underwater robot multi-beam sounding active synchronous positioning and mapping method. The method comprises an active synchronous positioning and mapping strategy of the underwater robot, a loop target point topographic information evaluation method, a utility equation for weighing exploration and loop action, and a path planning method when the loop action is executed. Compared with the traditional method for synchronously positioning and constructing the depth information of the underwater robot, the method provided by the invention has the advantages that the relationship between backtracking and exploration is balanced by actively backtracking the terrain purposefully, so that the underwater robot can obtain high-precision position information and submarine topography without depending on a mother ship more effectively.

Description

An active synchronous positioning and mapping method for multi-beam bathymetry of underwater robots

Technical Field

The invention belongs to the technical field of underwater robot positioning and navigation, and in particular relates to an underwater robot multi-beam bathymetric active synchronous positioning and mapping method.

Background Art

Limited by detection technology and cost, only 20% of the global seabed has been mapped so far. Detecting the seabed topography is helpful for people to understand the location of ocean faults and the laws of ocean currents and tides. Due to the obstruction of seawater, the topographic changes on the seabed are difficult to observe directly, so people often use multi-beam sonar for depth measurement, and underwater robots (Autonomous Underwater Vehicle, AUV) are widely used in seabed topography measurement as one of the best tools for people to explore the ocean. Because the average depth of the ocean is 3795 meters, and the range of multi-beam sonar is limited. Therefore, compared with surface vessels, underwater robots have a wider working range and can obtain seabed topography data with higher resolution. When AUV performs seabed topography measurement tasks underwater, due to its limited energy, it is necessary to use appropriate algorithms to optimize the efficiency of AUV exploring the ocean.

The patent application with the publication date of October 11, 2019, publication number CN110320520A, and invention name "A robust back-end graph optimization method for simultaneous positioning and mapping of bathymetric information". This method mainly targets the invalid data association problem that cannot be handled by traditional graph optimization methods of underwater robots, and realizes the validity judgment of data association using covariance. However, this method only improves the back-end of the underwater bathymetric simultaneous positioning and mapping algorithm, and does not involve active decision loops and exploration action selection.

The publication date is May 14, 2021, the publication number is CN112802195A, and the invention name is "A method for continuous occupation mapping of underwater robots based on sonar". This method improves the accuracy of the generated map by applying Gaussian process continuous occupancy mapping technology to underwater sonar mapping, making factors such as observation noise and outliers more robust. This method is an optimization method for mapping quality, which improves the mapping quality of underwater robots and does not involve active decision loops and exploration action selection.

Summary of the invention

The object of the present invention is to provide an underwater robot multi-beam bathymetric active synchronous positioning and mapping method.

A method for active synchronous positioning and mapping of multi-beam bathymetry of an underwater robot, comprising the following steps:

Step 1: Set the task area;

Step 2: Plan a full coverage path for the set mission area;

Step 3: Execute multi-beam bathymetric SLAM and construct the trajectory map Map _traj ;

Step 4: Calculate candidate loop targets and candidate exploration targets based on the trajectory map;

Step 4.1: Divide the trajectory map Map _traj into a set of sub-maps according to the average width w of the multi-beam survey line;

Step 4.2: Determine the candidate point search radius r by the robot's current position (x, y) and the position covariance matrix ∑:

Step 4.3: Calculate the terrain Fisher information {T _i |i＝1,2,…,Num} of all sub-maps within the search radius r

Where sub ₁ , sub ₂ ,…, sub _Num are all submaps within the search radius r, Num represents the number of submaps; _Mi , _Ni represent the size of the data matrix of submap sub _i ; h _ab represents the terrain elevation of the corresponding position (a, b); ||·|| represents the Euclidean norm;

Step 4.4: Take all sub-map center points that satisfy _Ti > T _thres as the loop target point set; T _thres is the preset terrain Fisher information threshold; according to the distance from the current robot position to the loop target, select points with the same distance in the forward direction as exploration target points, thereby generating an exploration target point set;

Step 4.5: Output the loop closure candidate point set and the exploration candidate point set;

Step 5: Calculate the benefits of reaching each candidate point according to the utility equation, select the candidate point with the best benefits, and determine whether to perform a loop task or an exploration task according to the action a ^* of the AUV driving to the candidate point with the best benefits;

Step 6: Execute the action a ^* of the AUV driving to the candidate point with the best return, and judge whether the exploration of the set task area is completed; if the exploration is completed, end; otherwise, return to step 3.

Furthermore, the step 5 is specifically as follows:

According to the utility equation, the benefit ηI of reaching each candidate point is calculated, and the candidate point with the best corresponding benefit is selected. The process is expressed as:

Among them, a ^* represents the action of the AUV driving to the corresponding candidate point when the benefit is optimal; η represents the balance factor; _Va is the map volume affected by action a, which can be calculated by the ray casting method; since each revisit action has a corresponding exploration action, _Vexplore is the map volume affected by the exploration action; I is the mutual information when executing action a, u represents the set of historical control vectors, z represents the set of all historical observations, m(a) represents the map information detected by executing action a, and is represented in the form of an occupied grid map, so m represents the map information of one of the grids; H _α＝1 [P(m|x,u,z)] represents the Shannon entropy of grid m; H _α(a) [P(m|x,u,z)] represents the Rayleigh entropy of grid m, and the calculation formula is as follows,

In order to avoid the algorithm only considering the grid map occupancy information, the robot state information will be integrated into the calculation of α(a), which is calculated as follows:

Among them, σ represents the position uncertainty of the robot after executing action a,

Among them, ∑ _target represents the position covariance of the robot when it reaches the target point obtained by prediction; ∑ _now represents the position covariance of the robot at the current moment; _Ti represents the terrain Fisher information of the target submap sub _i ; T _max represents the maximum terrain Fisher information within the search range.

The beneficial effects of the present invention are:

Compared with the traditional underwater robot bathymetric information simultaneous positioning and mapping method, the present invention balances the relationship between backtracking and exploration by actively and purposefully backtracking the terrain, thereby enabling the underwater robot to more effectively obtain high-precision position information and seabed topographic maps without relying on a mother ship.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is an overall flow chart of the present invention.

FIG. 2 is a flow chart of calculating candidate loop points and exploration points in the present invention.

DETAILED DESCRIPTION

The present invention is further described below in conjunction with the accompanying drawings.

The present invention relates to an active synchronous positioning and mapping method for an underwater robot. The method actively selects a loop action or an exploration action, so that the exploration efficiency of an AUV is improved while taking into account the accuracy of the constructed map.

A method for active synchronous positioning and mapping of multi-beam bathymetry of an underwater robot, characterized in that it comprises the following steps:

Step 1: Set the task area;

Step 2: Plan a full coverage path for the set mission area;

Step 3: Execute multi-beam bathymetric SLAM and construct a trajectory map Map _traj ; multi-beam bathymetric SLAM is to obtain underwater terrain through multi-beam bathymetric sonar.

Step 4: Calculate candidate loop targets and candidate exploration targets based on the trajectory map;

Step 4.1: Divide the trajectory map Map _traj into a set of sub-maps according to the average width w of the multi-beam survey line;

Step 4.2: Determine the candidate point search radius r by the robot's current position (x, y) and the position covariance matrix ∑:

Step 4.3: Calculate the terrain Fisher information {T _i |i＝1,2,…,Num} of all sub-maps within the search radius r

Where sub ₁ , sub ₂ ,…, sub _Num are all submaps within the search radius r, Num represents the number of submaps; _Mi , _Ni represent the size of the data matrix of submap sub _i ; h _ab represents the terrain elevation of the corresponding position (a, b); ||·|| represents the Euclidean norm;

Step 4.4: Take all sub-map center points that satisfy _Ti > T _thres as the loop target point set; T _thres is the preset terrain Fisher information threshold; according to the distance from the current robot position to the loop target, select points with the same distance in the forward direction as exploration target points, thereby generating an exploration target point set;

Step 4.5: Output the loop candidate point set and the exploration candidate point set;

Step 5: Calculate the benefits of reaching each candidate point according to the utility equation, select the candidate point with the best benefits, and determine whether to perform a loop task or an exploration task according to the action a ^* of the AUV driving to the candidate point with the best benefits;

According to the utility equation, the benefit ηI of reaching each candidate point is calculated, and the candidate point with the best corresponding benefit is selected. The process is expressed as:

Among them, a ^* represents the action of the AUV driving to the corresponding candidate point when the benefit is optimal; η represents the balance factor; _Va is the map volume affected by action a, which can be calculated by the ray casting method; since each revisit action has a corresponding exploration action, _Vexplore is the map volume affected by the exploration action; I is the mutual information when executing action a, u represents the set of historical control vectors, z represents the set of all historical observations, m(a) represents the map information detected by executing action a, and is represented in the form of an occupied grid map, so m represents the map information of one of the grids; H _α＝1 [P(m|x,u,z)] represents the Shannon entropy of grid m; H _α(a) [P(m|x,u,z)] represents the Rayleigh entropy of grid m, and the calculation formula is as follows,

In order to avoid the algorithm only considering the grid map occupancy information, the robot state information will be integrated into the calculation of α(a), which is calculated as follows:

Among them, σ represents the uncertainty of the robot's posture after executing action a,

Wherein, ∑ _target represents the position covariance of the robot when it reaches the target point obtained by prediction; ∑ _now represents the position covariance of the robot at the current moment; _Ti represents the terrain Fisher information of the target submap sub _i ; T _max represents the maximum terrain Fisher information within the search range;

Step 6: Execute the action a ^* of the AUV driving to the candidate point with the best return, and judge whether the exploration of the set task area is completed; if the exploration is completed, end; otherwise, return to step 3.

The present invention is an underwater robot multi-beam bathymetric active synchronous positioning and mapping method. It includes an underwater robot active synchronous positioning and mapping strategy, a loop target point terrain information evaluation method, a utility equation for weighing exploration and loop action, and a path planning method when executing loop action. Compared with the traditional underwater robot bathymetric information synchronous positioning and mapping method, the present invention balances the relationship between backtracking and exploration by actively and purposefully backtracking the terrain, so that the underwater robot can more effectively obtain high-precision position information and seabed terrain maps without relying on the mother ship.

The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims (2)

1. A method for active synchronous positioning and mapping of multi-beam bathymetry of an underwater robot, characterized in that it comprises the following steps: Step 1: Set the task area; Step 2: Plan a full coverage path for the set mission area; Step 3: Execute multi-beam bathymetric SLAM and construct the trajectory map Map _traj ; Step 4: Calculate candidate loop targets and candidate exploration targets based on the trajectory map; Step 4.1: Divide the trajectory map Map _traj into a set of sub-maps according to the average width w of the multi-beam survey line; Step 4.2: Determine the candidate point search radius r by the robot's current position (x, y) and the position covariance matrix ∑: Step 4.3: Calculate the terrain Fisher information {T _i |i＝1,2,…,Num} of all sub-maps within the search radius r Where sub ₁ , sub ₂ ,…, sub _Num are all submaps within the search radius r, Num represents the number of submaps; _Mi , _Ni represent the size of the data matrix of submap sub _i ; h _ab represents the terrain elevation of the corresponding position (a, b); ||·|| represents the Euclidean norm; Step 4.4: Take all sub-map center points that satisfy _Ti > T _thres as the loop target point set; T _thres is the preset terrain Fisher information threshold; according to the distance from the current robot position to the loop target, select points with the same distance in the forward direction as exploration target points, thereby generating an exploration target point set; Step 4.5: Output the loop candidate point set and the exploration candidate point set; Step 5: Calculate the benefits of reaching each candidate point according to the utility equation, select the candidate point with the best benefits, and determine whether to perform a loop task or an exploration task according to the action a ^* of the AUV driving to the candidate point with the best benefits; Step 6: Execute the action a ^* of the AUV driving to the candidate point with the best return, and judge whether the exploration of the set task area is completed; if the exploration is completed, end; otherwise, return to step 3. 2. The method for active synchronous positioning and mapping of multi-beam bathymetry of an underwater robot according to claim 1, characterized in that: the step 5 is specifically: According to the utility equation, the benefit ηI of reaching each candidate point is calculated, and the candidate point with the best corresponding benefit is selected. The process is expressed as: Among them, a ^* represents the action of the AUV driving to the corresponding candidate point when the benefit is optimal; η represents the balance factor; _Va is the map volume affected by action a, which can be calculated by the ray casting method; since each revisit action has a corresponding exploration action, _Vexplore is the map volume affected by the exploration action; I is the mutual information when executing action a, u represents the set of historical control vectors, z represents the set of all historical observations, m(a) represents the map information detected by executing action a, and is represented in the form of an occupied grid map, so m represents the map information of one of the grids; H _α＝1 [P(m|x,u,z)] represents the Shannon entropy of grid m; H _α(a) [P(m|x,u,z)] represents the Rayleigh entropy of grid m, and the calculation formula is as follows, In order to avoid the algorithm only considering the grid map occupancy information, the robot state information will be integrated into the calculation of α(a), which is calculated as follows: Among them, σ represents the position uncertainty of the robot after executing action a, Among them, ∑ _target represents the position covariance of the robot when it reaches the target point obtained by prediction; ∑ _now represents the position covariance of the robot at the current moment; _Ti represents the terrain Fisher information of the target submap sub _i ; T _max represents the maximum terrain Fisher information within the search range.