An active synchronous positioning and mapping method for multi-beam bathymetry of underwater robots
Through the active synchronous positioning and mapping method of multi-beam depth sounding, candidate loops and exploration target points are calculated, and loops and exploration actions are balanced, which solves the problem of inefficiency of underwater robots in submarine terrain measurement, and achieves the acquisition of high-precision terrain maps and improve task efficiency.
Patent Information
- Application Number
- CN202210488829.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-05-06
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2042-05-06
AI Technical Summary
The existing methods of underwater robots' synchronous positioning and mapping of depth measurements cannot effectively handle active decision looping and exploration action selection, resulting in inefficient efficiency and insufficient data accuracy in submarine terrain measurement.
Using the multi-beam depth sounding active synchronous positioning and mapping method, by setting the task area, fully covering path planning, performing multi-beam depth sounding SLAM, calculating candidate loops and exploring target points, using utility equations to select the action with the best returns, and balance The relationship between loopback and exploration is optimized to optimize the task execution of underwater robots.
It improves the ability of underwater robots to obtain high-precision position information and submarine topographic maps without relying on the mother ship, and improves exploration efficiency and resolution of topographic measurement data.
Smart Images

Figure CN115031733B_ABST
Abstract
Description
Technical Field
[0001] 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
[0002] 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.
[0003] 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.
[0004] 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
[0005] The object of the present invention is to provide an underwater robot multi-beam bathymetric active synchronous positioning and mapping method.
[0006] A method for active synchronous positioning and mapping of multi-beam bathymetry of an underwater robot, comprising the following steps:
[0007] Step 1: Set the task area;
[0008] Step 2: Plan a full coverage path for the set mission area;
[0009] Step 3: Perform multi-beam bathymetric SLAM and build a trajectory map traj ;
[0010] Step 4: Calculate candidate loop targets and candidate exploration targets based on the trajectory map;
[0011] Step 4.1: Map the trajectory according to the average width w of the multi-beam survey line traj Divide into a collection of sub-maps;
[0012] Step 4.2: Determine the candidate point search radius r by the robot's current position (x, y) and the position covariance matrix ∑:
[0013]
[0014] Step 4.3: Calculate the terrain Fisher information {T i |i=1,2,…,Num}
[0015]
[0016] Among them, sub1, sub2, …, sub Num To search all submaps within the radius r, Num represents the number of submaps; M i 、N i Indicates submap sub i The size of the data matrix; h ab represents the terrain elevation at the corresponding position (a, b); ||·|| represents the Euclidean norm;
[0017] Step 4.4: Take all the i >T thres The center point of the sub-map is taken as the loop target point set; thres is the preset terrain Fisher information threshold; according to the distance from the current robot position to the loop target, points with the same distance in the forward direction are selected as exploration target points, thereby generating an exploration target point set;
[0018] Step 4.5: Output the loop candidate point set and the exploration candidate point set;
[0019] Step 5: Calculate the benefits of reaching each candidate point according to the utility equation, select the candidate point with the best benefits, and follow the action a of the AUV driving to the candidate point with the best benefits. * , determine whether to perform a loop task or an exploration task;
[0020] Step 6: Execute action a of the AUV driving to the candidate point with the best profit * , determine whether the set task area has been explored; if the exploration is completed, end; otherwise, return to step 3.
[0021] Furthermore, the step 5 is specifically as follows:
[0022] 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:
[0023]
[0024]
[0025]
[0026] Among them, a * represents the action of the AUV driving to the corresponding candidate point when the benefit is optimal; η represents the balance factor; V a is the map volume affected by action a, which can be calculated by ray casting. Since each revisit action has a corresponding exploration action, V explore 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, and m(a) represents the map information detected by executing action a, which is represented in the form of an occupied grid map, so m represents the map information of a grid; 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:
[0027]
[0028]
[0029] 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:
[0030]
[0031] Among them, σ represents the uncertainty of the robot's posture after executing action a,
[0032]
[0033] Among them, ∑ target Represents the position covariance of the robot when it reaches the target point obtained by prediction; ∑ nowrepresents the position covariance of the robot at the current moment; T i Indicates the target submap sub i The topographic Fisher information of T max Indicates the maximum amount of terrain Fisher information within the search range.
[0034] The beneficial effects of the present invention are:
[0035] 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
[0036] Figure 1 It is the overall flow chart of the present invention.
[0037] Figure 2 Flowchart for calculating candidate loop points and exploration points in the present invention. DETAILED DESCRIPTION
[0038] The present invention is further described below in conjunction with the accompanying drawings.
[0039] 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.
[0040] A method for active synchronous positioning and mapping of multi-beam bathymetry of an underwater robot, characterized in that it comprises the following steps:
[0041] Step 1: Set the task area;
[0042] Step 2: Plan a full coverage path for the set mission area;
[0043] Step 3: Perform multi-beam bathymetric SLAM and build a trajectory map traj ; Multi-beam bathymetric SLAM is to obtain underwater terrain through multi-beam bathymetric sonar.
[0044] Step 4: Calculate candidate loop targets and candidate exploration targets based on the trajectory map;
[0045] Step 4.1: Map the trajectory according to the average width w of the multi-beam survey line traj Divide into a collection of sub-maps;
[0046] Step 4.2: Determine the candidate point search radius r by the robot's current position (x, y) and the position covariance matrix ∑:
[0047]
[0048] Step 4.3: Calculate the terrain Fisher information {T i |i=1,2,…,Num}
[0049]
[0050] Among them, sub1, sub2, …, sub Num To search all submaps within the radius r, Num represents the number of submaps; M i 、N i Indicates submap sub i The size of the data matrix; h ab represents the terrain elevation at the corresponding position (a, b); ||·|| represents the Euclidean norm;
[0051] Step 4.4: Take all the i >T thres The center point of the sub-map is taken as the loop target point set; thres is the preset terrain Fisher information threshold; according to the distance from the current robot position to the loop target, points with the same distance in the forward direction are selected as exploration target points, thereby generating an exploration target point set;
[0052] Step 4.5: Output the loop candidate point set and the exploration candidate point set;
[0053] Step 5: Calculate the benefits of reaching each candidate point according to the utility equation, select the candidate point with the best benefits, and follow the action a of the AUV driving to the candidate point with the best benefits. * , determine whether to perform a loop task or an exploration task;
[0054] 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:
[0055]
[0056]
[0057]
[0058] Among them, a * represents the action of the AUV driving to the corresponding candidate point when the benefit is optimal; η represents the balance factor; V a is the map volume affected by action a, which can be calculated by ray casting. Since each revisit action has a corresponding exploration action, V exploreis 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, and m(a) represents the map information detected by executing action a, which is represented in the form of an occupied grid map, so m represents the map information of a grid; 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:
[0059]
[0060]
[0061] 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:
[0062]
[0063] Among them, σ represents the uncertainty of the robot's posture after executing action a,
[0064]
[0065] 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; T i Indicates the target submap sub i The topographic Fisher information of T max Indicates the maximum amount of terrain Fisher information within the search range;
[0066] Step 6: Execute action a of the AUV driving to the candidate point with the best profit * , determine whether the set task area has been explored; if the exploration is completed, end; otherwise, return to step 3.
[0067] 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.
[0068] 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
1. A method for active synchronous positioning and mapping of multi-beam bathymetry of an underwater robot, characterized in that: The following steps are involved: Step 1: Set the task area; Step 2: Plan a full coverage path for the set mission area; Step 3: Perform multi-beam bathymetric SLAM and build a trajectory map traj ; Step 4: Calculate candidate loop targets and candidate exploration targets based on the trajectory map; Step 4.1: Map the trajectory according to the average width w of the multi-beam survey line traj Divide into a collection of sub-maps; 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} Among them, sub1, sub2, …, sub Num To search all submaps within the radius r, Num represents the number of submaps; M i 、N i Indicates submap sub i The size of the data matrix; h ab represents the terrain elevation at the corresponding position (a, b); ||·|| represents the Euclidean norm; Step 4.4: Take all the i >T thres The center point of the sub-map is taken as the loop target point set; thres is the preset terrain Fisher information threshold; according to the distance from the current robot position to the loop target, points with the same distance in the forward direction are selected 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 follow the action a of the AUV driving to the candidate point with the best benefits. * , determine whether to perform a loop task or an exploration task; Step 6: Execute action a of the AUV driving to the candidate point with the best profit * , determine whether the set task area has been explored; 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 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; V a is the map volume affected by action a, which can be calculated by ray casting. Since each revisit action has a corresponding exploration action, V explore 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, and m(a) represents the map information detected by executing action a, which is represented in the form of an occupied grid map, so m represents the map information of a grid; 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, 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; T i Indicates the target submap sub i The topographic Fisher information of T max Indicates the maximum amount of terrain Fisher information within the search range.
