CN114596360B - Double-stage active real-time positioning and mapping algorithm based on graph topology - Google Patents
Double-stage active real-time positioning and mapping algorithm based on graph topology Download PDFInfo
- Publication number
- CN114596360B CN114596360B CN202210161358.7A CN202210161358A CN114596360B CN 114596360 B CN114596360 B CN 114596360B CN 202210161358 A CN202210161358 A CN 202210161358A CN 114596360 B CN114596360 B CN 114596360B
- Authority
- CN
- China
- Prior art keywords
- path
- local
- point
- global
- graph
- 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
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 95
- 238000013507 mapping Methods 0.000 title claims abstract description 23
- 239000011159 matrix material Substances 0.000 claims description 32
- 238000010586 diagram Methods 0.000 claims description 9
- 238000004088 simulation Methods 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 7
- 238000012216 screening Methods 0.000 claims description 7
- 230000004807 localization Effects 0.000 claims description 3
- 238000000034 method Methods 0.000 description 27
- 230000008569 process Effects 0.000 description 10
- 238000004364 calculation method Methods 0.000 description 4
- 238000010276 construction Methods 0.000 description 4
- 238000013461 design Methods 0.000 description 4
- 230000004888 barrier function Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 241000764238 Isis Species 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000013138 pruning Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T19/00—Manipulating 3D models or images for computer graphics
- G06T19/003—Navigation within 3D models or images
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Software Systems (AREA)
- General Physics & Mathematics (AREA)
- Computer Graphics (AREA)
- Remote Sensing (AREA)
- Geometry (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Computer Hardware Design (AREA)
- General Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Multimedia (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a double-stage active real-time positioning and mapping algorithm based on graph topology, which is divided into global/local two stages; firstly, the algorithm obtains the current pose, the pose graph and the navigation map of the unmanned platform, and calculates the local leading edge point and the candidate loop point. If the points exist, a local exploration stage is entered, a local topological graph is constructed through a rapid random spanning tree algorithm, a Dijkstra algorithm is utilized to solve the shortest path reaching any vertex of the local topological graph, a path set is scored according to a local exploration and active loop combined objective function, an optimal path is solved, and meanwhile, a global topological graph and a global leading-edge point are updated according to the local topological graph and the local leading-edge point. Otherwise, when the local scope has no local front edge point and no loop back requirement, the algorithm enters a global exploration stage, and the unmanned platform goes to an unexplored area to conduct local exploration again according to the global topological graph until the exploration of the appointed area is completed, and returns to a starting point to complete the exploration task.
Description
Technical Field
The invention belongs to the technical field of ground unmanned platform perception and autonomous planning, and particularly relates to a double-stage active real-time positioning and mapping algorithm based on a graph topology.
Background
The technology of the active exploration (Unknown Environment Autonomous Exploration) of the unknown environment refers to that an unmanned platform searches and detects the unknown environment autonomously under the condition of lacking prior information, collects the environment information, provides favorable support for subsequent treatment, and can be widely applied to dangerous scenes such as urban explosion and danger elimination, nuclear and biochemical environment exploration, mine tunnel rescue, asteroid development and the like. The method has the advantages that the complex environment can be surveyed without depending on external positioning devices (such as Beidou positioning) or personnel manipulation, and the danger of detection tasks is greatly reduced. The instant localization and mapping (Simultaneously Localization and Mapping, SLAM) algorithm refers to determining an increment of a self-position by using an environment-aware sensor, so as to determine a position of an ontology in an environment map, and simultaneously, building an environment point cloud map according to position information and environment-aware data. The positioning accuracy and the map construction quality directly determine whether the unmanned platform can finish the exploration task and the quality of finishing the exploration task. Therefore, how to improve the accuracy of the SLAM method by the active path planning method is a focus of study by the current scholars.
The existing solutions of the unknown environment autonomous exploration algorithm for the ground unmanned platform are as follows:
scheme 1: the GBP planner algorithm is proposed in the literature (Dang T, mascarich F, khattak S, et al graph-based path planning for autonomous robotic exploration in subterranean environments [ C ]//2019IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019:3105-3112.). The algorithm mainly contributes to providing a double-stage exploration algorithm framework for the first time, utilizing two dynamically-expanded rapid random generation graphs (Rapidly Exploring Random Graph, RRG) to realize global/local exploration path planning algorithm, carrying out objective function design on exploration information and navigation cost, and simultaneously carrying out special design on an autonomous return method to realize coverage search of a large-scale multi-branch unknown tunnel environment. However, there are still problems that the calculation complexity is large and the characteristics of the SLAM algorithm are not considered.
Scheme 2: the DSVP algorithm proposed in the literature (H.Zhu, C.Cao, S.Scherer, J.Zhang, and W.Wang.DSVP: dual-Stage Viewpoint Planner for Rapid Exploration by Dynamic expansion.IEEE/RSJ intl.Conf.on Intelligent Robots and Systems (IROS). Prague, czech, sept.2021) is improved with respect to the GPB planner algorithm, and a quick-expansion random tree (Rapidly Exploring Random Tree, RRT) algorithm utilizing heuristic iterative pruning is proposed to replace the RRG algorithm in the GBP planner local exploration stage, so that the algorithm execution efficiency is improved, and meanwhile, a Gazebo-based simulation environment is developed in the literature, so that researchers can evaluate own exploration algorithm efficiency.
Scheme 3: literature (Chen Y, huang S, zhao L, et al cramer r-Rao Bounds and Optimal Design Metrics for Pose-Graph SLAM J IEEE Transactions on Robotics,2021, pp (99): 1-15.) this document proposes using fischer information matrices and minimum spanning tree numbers to measure the uncertainty of the pose map in SLAM algorithms, but does not apply this scale to active exploration.
The invention is inspired by the three schemes, experimental verification is carried out according to the self characteristics of the problem to be solved, a large-range unknown environment active positioning and image construction method based on image topology is provided, an autonomous exploration algorithm considering the SLAM algorithm precision is designed aiming at the SLAM algorithm based on the pose image, the algorithm utilizes the SLAM algorithm to provide positioning information, pose image information and unknown area information and barrier information provided by a navigation map to carry out joint modeling on the exploration problem and the active SLAM problem, and the RRT method is used for solving, so that loop constraint in the pose image is increased while safety and rapid exploration are ensured, and the SLAM algorithm precision in the exploration process is improved. In addition, the algorithm uses a global/local dual-stage algorithm architecture, so that the time complexity and the space complexity of the algorithm are reduced, and the collision-free path generation in an unknown environment is realized. Finally, the unmanned platform can track and control the path through a dynamic sliding window method and other controllers, so as to realize autonomous positioning and mapping in an unknown environment.
Disclosure of Invention
In view of the above, the invention provides a dual-stage active real-time positioning and mapping algorithm based on map topology, which can provide positioning information, pose map information and unknown area information and barrier information provided by a navigation map by utilizing a SLAM algorithm, perform joint modeling on an exploration problem and an active SLAM problem, solve the exploration problem by using an RRT method, ensure safe and rapid exploration, increase loop constraint in the pose map, and improve SLAM algorithm precision in the exploration process.
The technical scheme for realizing the invention is as follows:
a dual-stage active real-time positioning and mapping algorithm based on graph topology comprises the following steps:
step one, acquiring a real-time pose diagram and a navigation map of an unmanned platform, screening candidate loop points by using the pose diagram, and generating local leading edge points by using current position information in the pose diagram and the navigation map; if the local leading edge point or the candidate loop point exists, proving that an explorable area or an SLAM precision improving area exists in the local area, and executing the second step; otherwise, the fact that the current area has no local planning requirement is proved, the other areas need to be searched or looped back to execute the task, and the third step is executed;
step two, the unmanned platform is in a local planning range according to the self position and the navigation map
Generating a collision-free local topological graph in space by utilizing an RRT algorithm, solving the shortest path on the local topological graph by using a Dijkstra algorithm aiming at each vertex of the local topological graph on the local topological graph, and filling each path into a path set B; for each path B in path set B i Scoring is carried out by utilizing the path information gain, and the path with the highest score in the path set B is the optimal path; then, the global leading point and the global topological graph are updated by using the local leading point and the local topological graph, and if the branch Gain (B i ) If > 0, adding all sampling points of the branch into a global topological graph, and when the global leading edge point set is updated, adding +.>
Adding the last obtained global front point set, and judging each point in the point set to beWhether the grid is a passable grid and is connected with an unknown grid or not, at the same time, at least one vertex in the global topological graph can be observed, and a set of points meeting the condition is called a global front edge point set->
Step three, traversing the global front edge point set
For a certain global leading edge point f g Finding the closest point of the Euclidean distance between the closest point and the point on the global topological graph, and using Dijkstra algorithm to perform +.>
Solving the shortest path on the global topological graph, and finally obtaining a path set; the leading edge point f corresponding to the shortest path of the path concentration length g The shortest path corresponding to the target point is the optimal path; if no feasible solution exists, the global area exploration is completed, and the unmanned platform returns to the starting point through the shortest path planned by the global topological graph.
Further, a candidate set of loop points
From the local planning scope->
Pose graph G with potential loop information gain p The upper vertexes are formed; the significance is to judge->
Whether there is valuable loop back in the loop back method reduces the calculation complexity of the gain of the follow-up loop back information.
Further, the candidate loop point screening steps are as follows:
1. traversing vertex set V p If the vertex is located at all the vertices in (a)
Put in the vertex set V l p In (a) and (b);
2. v is deleted by DBSCAN (Density-Based Spatial Clustering of Applications with Noise, DBSCAN) clustering algorithm l p Obtaining vertex set by vertex with middle distance from current pose point time sequence
3. For the following
Is>
Calculate->
Wherein (1)>
Is->
Each vertex in the pose graph G p Corresponding covariance matrix Σ ++>
Is the current pose point of the unmanned platform +.>
Covariance matrix of>
If the value is greater than a certain threshold value, the candidate loop point is selected as +.>
Wherein, eigValue k The eigenvalues are taken for the matrix, k represents the kth eigenvalue,/>
Representing the dimension of the square matrix.
Further, in the second step, the path information gain includes three parts: 1. the unknown region explores the information gain; 2. active loop SLAM information gain; 3. navigation cost.
Further, the active loop SLAM information gain LoopGain refers to the degree to which the current path can reduce the uncertainty of the SLAM pose map,
wherein,,
representing the simulation execution ith planned path b i Then, the formed pose graph is subjected to logarithmic operation by taking the determinant with Dopt (x) =log det (x) as matrix, and the formula is +.>
Representing pose graph G p Is claimed in the form of a Fisher information matrix, < ">>
Representing a pose map
Is a fischer information matrix of (c).
In the second step, the selected optimal path is handed to a path tracking algorithm to solve the linear speed and the angular speed of the unmanned platform chassis, and the platform can travel along the optimal path planned at present.
In the third step, the planned path is issued to a path tracking algorithm to realize the movement of the unmanned platform.
The beneficial effects are that:
aiming at the problems of active real-time positioning and mapping of a ground unmanned platform in a large-scale unknown environment, the invention provides a double-stage active real-time positioning and mapping algorithm framework based on a graph topology, and the innovation points of the framework are mainly as follows:
the method provides a combined path planning algorithm of autonomous exploration and active loop back based on graph topology, solves the problem of error accumulation caused by long-term no loop back of the SLAM algorithm in the autonomous exploration process, and improves the positioning precision and the graph construction quality of an unmanned platform in the active graph construction process.
Secondly, the method designs a two-stage active positioning and mapping path planning framework, greatly reduces the computational complexity and the space complexity of an algorithm, can complete the autonomous exploration task of a large-scale unknown complex scene, gives consideration to SLAM precision, and can realize autonomous return voyage.
Drawings
FIG. 1 is a block diagram of a dual-stage active real-time positioning and mapping algorithm.
FIG. 2 is a schematic diagram of an active exploration process of an unknown environment, (a) a simulation environment, and (b) autonomous exploration and mapping of the simulation environment.
FIG. 3 is a graph of the square sum of the error and the change of the current position of the unmanned platform.
Fig. 4 is a comparison chart of the current position covariance matrix D optimal scale change of the unmanned platform.
Fig. 5 is a comparison diagram of a navigation map obtained by active exploration of an unmanned platform in a ground library environment, (a) a real object environment and the unmanned platform in the real object, (b) a navigation map obtained by a double-stage exploration algorithm, and (c) a navigation map obtained by a double-stage active instant positioning algorithm.
Detailed Description
The invention will now be described in detail by way of example with reference to the accompanying drawings.
The invention provides a graph topology-based double-stage active instant positioning and mapping algorithm framework, wherein the whole algorithm framework is shown in fig. 1, the algorithm effect is shown in fig. 2, fig. 2 (a) is a simulation environment and a simulation trolley, and fig. 2 (b) shows an environment model established in an autonomous exploration process of an unmanned platform and various generated topological graphs.
Step one, local/global phase decision algorithm
Firstly, a dual-stage active instant positioning and mapping algorithm gives a pose graph G of an unmanned platform through a laser inertial tight coupling SLAM algorithm (lio-sam) p (V p ，E p ) Wherein V is p Is a historical key pose point set of an unmanned platform, E p For a set of observation edges between key poses, each pose point (including the current position of the robot
) Or the observation side has its covariance matrix (uncertainty) Σ. Meanwhile, the double-stage active real-time positioning and mapping algorithm utilizes the laser radar point cloud and the current pose of the unmanned platform to establish a navigation map (a two-dimensional grid map for generating a passable path and generating local front edge points, wherein the map is used for dividing a space into grids of 0.1 x 0.1m, and grid points can be divided into three types, namely a passable grid, an obstacle grid and an unexplored grid). Then, based on the information given in the previous step, the double-stage active real-time positioning and mapping algorithm performs screening of candidate loop points and generation of local leading edge points, if the local leading edge points or the candidate loop points exist, the existence of an explorable region or a region capable of improving SLAM precision in the local region is proved, and the second step is executed; otherwise, the current area is proved to have no local planning requirement, and the search or loop-back task is required to be executed to other areas, so that the third step is executed.
(1) Candidate loop point screening
Candidate loop point set
From the local planning scope->
(/>
Defined as->
For the center, a square with a length-width of 15m x 15 m) a pose graph G with a potential loop information gain p The upper vertices. The significance is to judge->
Whether there is valuable loop back in the loop back method reduces the calculation complexity of the gain of the follow-up loop back information.
The candidate loop point screening steps are as follows:
1. traversing all vertices in the vertex set Vp if the vertex is located
Put in the vertex set V l p In (a) and (b);
2. v is deleted by DBSCAN (Density-Based Spatial Clustering ofApplications-with Noise) clustering algorithm l p Obtaining vertex set by vertex with middle distance from current pose point time sequence
3. For the following
Is>
Calculate->
Wherein (1)>
Is->
Each vertex in the pose graph G p Corresponding covariance matrix Σ ++>
Is the current pose point of the unmanned platform +.>
Covariance matrix of>
If the value is greater than a certain threshold value, the candidate loop point is selected as +.>
Wherein eigValuek is a eigenvalue of the matrix, k represents the kth eigenvalue, ++>
Representing the dimension of the square matrix.
The significance is as follows:
1. meeting local region constraints;
2. a small amount of loop information gain exists at a point which is closer to the current pose point time sequence, but the exploration efficiency is greatly reduced, so that the loop information gain is eliminated;
3. based on the SLAM algorithm of the pose graph, each vertex on the pose graph is subjected to Gaussian distribution v-N (mu, sigma), wherein mu is the pose of the vertex, sigma is the covariance matrix of the vertex, and the larger the covariance matrix is, the larger the error value is. The dopt converts the vertex covariance matrix to be judged into a scalar quantity, judges the scalar quantity with the covariance matrix dopt of the current pose, and can be listed as candidate loop points by proving that the positioning accuracy of the vertex covariance matrix can be improved or the drawing error can be reduced through loop constraint if the difference is large.
(2) Local leading edge point generation
Local leading edge point set
Refers to the local planning scope +.>
Known interface of inner and unexplored gridsGrid point, which is used for judging +.>
Whether there are unexplored areas within range that are reachable by the unmanned platform.
Step two, local active real-time positioning and map-building path planning algorithm
If the step two is entered, the unmanned platform should execute a local active real-time positioning and map-building path planning algorithm, and the stage algorithm aims at active unknown environment exploration or active loop-back based on the pose map. As shown in the flow chart of fig. 1, the algorithm flow is divided into five parts, namely, establishing local RRT, searching the shortest paths for all vertexes by using Dijkstra algorithm, evaluating path information gain, selecting optimal paths and global leading edge points, and updating global topological graph. The unmanned platform is in a local planning range according to the self position and the navigation map
And generating a collision-free local topological graph in space by utilizing an RRT algorithm, solving the shortest path on the local topological graph by using a Djjkstra algorithm aiming at each vertex of the local topological graph on the local topological graph, and filling each path into a path set B. For each path B in path set B i And scoring by using the path information gain, wherein the path with the highest score in the path set B is the optimal path. Then, the global leading point and the global topological graph are updated by using the local leading point and the local topological graph, and if the branch Gain (B i ) If > 0, adding all sampling points of the branch into a global topological graph, and when the global leading edge point set is updated, adding +.>
Adding the last obtained global front point set, judging each point in the point set, and judging whether the grid of the point set is a passable grid and is connected with an unknown grid, wherein the point set can be observed by at least one vertex in the global topological graph, and the set of points meeting the conditions is called global front point set>
And finally, delivering the selected optimal path to a path tracking algorithm to solve the linear speed and the angular speed of the unmanned platform chassis, wherein the platform can travel along the optimal path planned at present. The innovation of the invention is mainly embodied in the proposal of the path information gain.
The path information gain mainly includes three parts: 1. the unknown region explores the information gain; 2. active loop SLAM information gain; 3. navigation cost.
Wherein Gain (b) i ) Planning path b for ith i Represented information gain, lambda 1 ...λ 4 LoopGain (b) as a weight coefficient i ) B is i The represented loop information gain; explorescence gain (b) i ) B is i The represented unknown region explores information gain;
which is the reciprocal of the navigation cost.
(1) Unknown region exploration information gain
The unknown region exploration information gain needs to satisfy the formula as follows
Wherein b i The i-th planned path is a set of a plurality of sampling points on the local topological graph.
Is b i The j-th sampling point on the upper. VoxelGain represents the number of all unknown grids that can be seen in the field of view of the calculated sampling point. The implementation in the grid map may be performed using the RayTrace algorithm in computer graphics.
(2) Active loop SLAM information gain
The active loop SLAM information gain LoopGain refers to the degree that the uncertainty of the SLAM pose graph can be reduced by the current path, and aims to improve the SLAM positioning precision and the mapping precision through the loop constraint function of the SLAM graph optimization algorithm.
Slam measurement model definition:
the unmanned platform state space is
Is composed of 3-dimensional translation vector and rotation lie algebra. The state vector to be estimated is +.>
Wherein (1)>
The measurement equation h between two nodes i, j can be expressed as:
wherein,,
and->
In order to observe the noise it is possible,
2. loop information gain definition:
wherein,,
representing the simulation execution ith planned path b i And then forming a pose chart. Dopt (= log det (=) is a matrix determinant performing a logarithmic operation.
If b i Sampling point on
And any key point n on the graph r Is smaller than a certain threshold tau loop Then consider that the pose graph may generate loop, build edge +.>
E loop I.e. the set of all the above-mentioned edges.
Wherein L is a pose graph G p Is used to determine the matrix of the laplace matrix,
is the pose graph G p Translation component->
Is a weighted Laplace matrix of +.>
Is the pose graph G p Rotational component->
Is a weighted laplacian matrix of (c).
Similarly, the number of the devices to be used in the system,
representing the pose map->
The calculation process is the same as the above process.
(3) Navigation cost
DTW means a dynamic time warping algorithm that calculates the similarity between the currently selected path and the last selected path, and also reflects whether the search direction is consistent. dist is the euclidean length of the current path. Therefore, the exploration direction is consistent with the exploration direction of the current unmanned platform, and the path length is shortest, so that the navigation cost is lowest, and the exploration benefit is highest.
Step three, global active instant positioning and mapping algorithm
When in local planning scope
There is no local leading edge point +.>
Or candidate loop point->
And when the algorithm is switched to the third step. The goal of the global planning algorithm is to guide the unmannedThe platform reaches unexplored areas in the global scope to be explored again.
Traversing global leading edge point set
For a certain global leading edge point f g Finding the closest point of the Euclidean distance between the closest point and the point on the global topological graph, and using Dijkstra algorithm to perform +.>
Solving the shortest path on the global topological graph, and finally obtaining a path set. The leading edge point f corresponding to the shortest path of the path concentration length g The shortest path corresponding to the target point is the optimal path. If there is no feasible solution (+)>
Or->
And (3) the method can not be reached), the global area exploration is completed, and the unmanned platform returns to the starting point through the shortest path planned by the global topological graph. And similarly to the second step, the planned path is issued to a path tracking algorithm to realize the movement of the unmanned platform.
In the algorithm operation of the invention, in the algorithm starting stage, as the local (square area centered by the robot) reachable unknown area exists, the algorithm continuously executes the second step until no explorable area exists in the current local range, the algorithm shifts to the third step, and the rest reachable unknown areas in the current navigation map are explored, namely the second step is executed again.
Finally, the algorithm of the present invention is compared with the two-stage search algorithm in document 2, and the performance improvement is shown in fig. 3, 4 and 5. Specifically, in fig. 3, the horizontal axis represents algorithm execution time, the vertical axis represents the square sum of positioning errors, the dashed line (- -) represents a dual-stage exploration algorithm, and the solid line (-) represents the method provided by the invention, so that the method can greatly improve the positioning accuracy of the SLAM algorithm in the exploration process; drawing of the figure4, the horizontal axis represents time, and the vertical axis represents
Namely, uncertainty of the pose graph is obtained, a dashed line (- - -) represents a double-stage exploration algorithm, a solid line (-) represents the method provided by the invention, and the uncertainty of the pose graph in the exploration process can be reduced by the method; the effectiveness of the algorithm is proved by a physical experiment (an unmanned platform and a physical environment are shown as 5 (a)) in fig. 5, a large difference between the point cloud image established by the double-stage exploration algorithm in fig. 5 (b) and the grid map can be seen, and the point cloud image established by the method provided by the invention in fig. 5 (c) is not different from the grid map, so that the image establishing precision of the algorithm is improved.
In summary, the above embodiments are only preferred embodiments of the present invention, and are not intended to limit the scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (3)
1. The double-stage active real-time positioning and mapping algorithm based on the graph topology is characterized by comprising the following steps of:
step one, acquiring a real-time pose diagram and a navigation map of an unmanned platform, screening candidate loop points by using the pose diagram, and generating local leading edge points by using current position information in the pose diagram and the navigation map; if the local leading edge point or the candidate loop point exists, proving that an explorable area or an SLAM precision improving area exists in the local area, and executing the second step; otherwise, the fact that the current area has no local planning requirement is proved, the other areas need to be searched or looped back to execute the task, and the third step is executed;
step two, the unmanned platform is in a local planning range according to the self position and the navigation map
Generating a collision-free local topological graph in space by utilizing RRT algorithm, and solving a problem by using Dijkstra algorithm on the local topological graph for each vertex of the local topologyFilling each path into a path set B by the shortest path on the partial topology map; for each path B in path set B i Scoring is carried out by utilizing the path information gain, and the path with the highest score in the path set B is the optimal path; then, the global leading point and the global topological graph are updated by using the local leading point and the local topological graph, and if the branch Gain (B i )>0, adding all sampling points of the branch into a global topological graph, and when the global leading edge point set is updated, adding the local leading edge point set>
Adding the last obtained global front point set, judging each point in the point set, and judging whether the grid of the point set is a passable grid and is connected with an unknown grid, wherein the point set can be observed by at least one vertex in the global topological graph, and the set of points meeting the conditions is called global front point set>
Gain(b i ) Planning path b for ith i The represented information gain;
step three, traversing the global front edge point set
For a certain global leading edge point f g Finding the closest point of the Euclidean distance between the closest point and the point on the global topological graph, and using Dijkstra algorithm to perform +.>
Solving the shortest path on the global topological graph, and finally obtaining a path set; the leading edge point f corresponding to the shortest path of the path concentration length g The shortest path corresponding to the target point is the optimal path; if no feasible solution exists, the global area exploration is completed, and the unmanned platform returns to the starting point through the shortest path planned by the global topological graph;
candidate loop point set
From the local planning scope->
Pose graph G with potential loop information gain p The upper vertexes are formed;
the candidate loop point screening steps are as follows:
(1) Traversing vertex set V p If the vertex is located at all the vertices in (a)
Put in the vertex set V l p In (a) and (b);
(2) V is deleted by DBSCAN clustering algorithm l p Obtaining vertex set by vertex with middle distance from current pose point time sequence
(3) For the following
Is>
Calculate->
Wherein (1)>
Is->
Each vertex in the pose graph G p Corresponding covariance matrix Σ, ++>
Is the current pose point of the unmanned platform +.>
Is used for the co-variance matrix of (a),
if the value is greater than a certain threshold value, the candidate loop point is selected as +.>
Wherein, eigValue k Taking eigenvalues from the matrix, wherein k represents the kth eigenvalue, and l represents the dimension of the square matrix;
in the second step, the path information gain includes three parts: (1) unknown region exploration information gain; (2) active loop SLAM information gain; (3) navigation cost;
the active loop SLAM information gain LoopGain refers to the degree to which the current path can reduce SLAM pose map uncertainty,
wherein,,
representing the simulation execution ith planned path b i Then, the formed pose graph is subjected to logarithmic operation by taking Dopt (x) = () as determinant of matrix, and the matrix is +.>
Representing pose graph G p Is claimed in the form of a Fisher information matrix, < ">>
Representing the pose map->
Is a fischer information matrix of (c).
2. The dual-stage active real-time positioning and mapping algorithm based on graph topology as claimed in claim 1, wherein in the second step, the selected optimal path is given to a path tracking algorithm to solve the linear speed and angular speed of the unmanned platform chassis, and the platform can travel along the optimal path currently planned.
3. The dual-stage active real-time localization and mapping algorithm based on graph topology as claimed in claim 1, wherein in step three, the planned path is issued to the path tracking algorithm to realize the motion of the unmanned platform.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210161358.7A CN114596360B (en) | 2022-02-22 | 2022-02-22 | Double-stage active real-time positioning and mapping algorithm based on graph topology |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210161358.7A CN114596360B (en) | 2022-02-22 | 2022-02-22 | Double-stage active real-time positioning and mapping algorithm based on graph topology |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114596360A CN114596360A (en) | 2022-06-07 |
| CN114596360B true CN114596360B (en) | 2023-06-27 |
Family
ID=81804656
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210161358.7A Active CN114596360B (en) | 2022-02-22 | 2022-02-22 | Double-stage active real-time positioning and mapping algorithm based on graph topology |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114596360B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114998540A (en) * | 2022-06-12 | 2022-09-02 | 哈尔滨工程大学 | Smart city sensor detection active synchronous positioning and mapping method |
| CN115617034B (en) * | 2022-09-01 | 2024-06-14 | 清华大学 | Multi-agent environment exploration method and device, electronic equipment and storage medium |
| CN116182840B (en) * | 2023-04-28 | 2023-07-25 | 科大讯飞股份有限公司 | Map construction method, device, equipment and storage medium |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110703747B (en) * | 2019-10-09 | 2021-08-03 | 武汉大学 | Robot autonomous exploration method based on simplified generalized Voronoi diagram |
| CN111427047B (en) * | 2020-03-30 | 2023-05-05 | 哈尔滨工程大学 | SLAM method for autonomous mobile robot in large scene |
| CN111583136B (en) * | 2020-04-25 | 2023-05-23 | 华南理工大学 | Method for simultaneously positioning and mapping autonomous mobile platform in rescue scene |
| CN111638526B (en) * | 2020-05-20 | 2022-08-26 | 电子科技大学 | Method for robot to automatically build graph in strange environment |
-
2022
- 2022-02-22 CN CN202210161358.7A patent/CN114596360B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114596360A (en) | 2022-06-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114596360B (en) | Double-stage active real-time positioning and mapping algorithm based on graph topology | |
| Zhang et al. | 2D Lidar‐Based SLAM and Path Planning for Indoor Rescue Using Mobile Robots | |
| Lacaze et al. | Path planning for autonomous vehicles driving over rough terrain | |
| Ström et al. | Predictive exploration considering previously mapped environments | |
| Jian et al. | Putn: A plane-fitting based uneven terrain navigation framework | |
| Schmid et al. | A unified approach for autonomous volumetric exploration of large scale environments under severe odometry drift | |
| Zhang et al. | Fast active aerial exploration for traversable path finding of ground robots in unknown environments | |
| CN111487960A (en) | Mobile robot path planning method based on positioning capability estimation | |
| CN114296474A (en) | Unmanned aerial vehicle path planning method and system based on path time cost | |
| Balan et al. | Optimal trajectory planning for multiple waypoint path planning using tabu search | |
| CN112857370A (en) | Robot map-free navigation method based on time sequence information modeling | |
| Zhang et al. | Dual-Layer path planning with pose SLAM for autonomous exploration in GPS-denied environments | |
| Zheng et al. | A hierarchical approach for mobile robot exploration in pedestrian crowd | |
| Wang et al. | Virtual maps for autonomous exploration with pose SLAM | |
| Soni et al. | Multi-robot unknown area exploration using frontier trees | |
| Collins et al. | Efficient planning for high-speed mav flight in unknown environments using online sparse topological graphs | |
| CN111912411B (en) | Robot navigation positioning method, system and storage medium | |
| Huang et al. | A real-time fast incremental SLAM method for indoor navigation | |
| Zhang et al. | 2D map building and path planning based on LiDAR | |
| Luo et al. | Planning optimal trajectory for histogram-enabled mapping and navigation by an efficient PSO algorithm | |
| Murtra et al. | Efficient active global localization for mobile robots operating in large and cooperative environments | |
| Zhang et al. | An adaptive artificial potential function approach for geometric sensing | |
| Lu et al. | A randomized hybrid system approach to coordinated robotic sensor planning | |
| Domınguez et al. | Internal simulation for autonomous robot exploration of lava tubes | |
| Ribeiro et al. | Cooperative 3D Exploration and Mapping using Distributed Multi-Robot Teams |
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 |