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 PDF

Info

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
Application number
CN202210161358.7A
Other languages
Chinese (zh)
Other versions
CN114596360A (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202210161358.7A priority Critical patent/CN114596360B/en
Publication of CN114596360A publication Critical patent/CN114596360A/en
Application granted granted Critical
Publication of CN114596360B publication Critical patent/CN114596360B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

Double-stage active real-time positioning and mapping algorithm based on graph topology
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
Figure BDA0003514848900000031
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 +.>
Figure BDA0003514848900000041
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->
Figure BDA0003514848900000042
Step three, traversing the global front edge point set
Figure BDA0003514848900000043
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 +.>
Figure BDA0003514848900000044
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
Figure BDA0003514848900000045
From the local planning scope->
Figure BDA0003514848900000046
Pose graph G with potential loop information gain p The upper vertexes are formed; the significance is to judge->
Figure BDA0003514848900000047
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)
Figure BDA0003514848900000048
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
Figure BDA0003514848900000049
3. For the following
Figure BDA00035148489000000410
Is>
Figure BDA00035148489000000411
Calculate->
Figure BDA00035148489000000412
Wherein (1)>
Figure BDA00035148489000000413
Is->
Figure BDA00035148489000000414
Each vertex in the pose graph G p Corresponding covariance matrix Σ ++>
Figure BDA00035148489000000415
Is the current pose point of the unmanned platform +.>
Figure BDA00035148489000000416
Covariance matrix of>
Figure BDA00035148489000000417
If the value is greater than a certain threshold value, the candidate loop point is selected as +.>
Figure BDA0003514848900000051
Wherein, eigValue k The eigenvalues are taken for the matrix, k represents the kth eigenvalue,/>
Figure BDA0003514848900000052
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,
Figure BDA0003514848900000053
wherein,,
Figure BDA0003514848900000054
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 +.>
Figure BDA0003514848900000055
Representing pose graph G p Is claimed in the form of a Fisher information matrix, < ">>
Figure BDA0003514848900000056
Representing a pose map
Figure BDA0003514848900000057
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
Figure BDA0003514848900000061
) 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
Figure BDA00035148489000000717
From the local planning scope->
Figure BDA0003514848900000071
(/>
Figure BDA0003514848900000072
Defined as->
Figure BDA0003514848900000073
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->
Figure BDA0003514848900000074
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
Figure BDA0003514848900000075
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
Figure BDA0003514848900000076
3. For the following
Figure BDA0003514848900000077
Is>
Figure BDA0003514848900000078
Calculate->
Figure BDA0003514848900000079
Wherein (1)>
Figure BDA00035148489000000710
Is->
Figure BDA00035148489000000711
Each vertex in the pose graph G p Corresponding covariance matrix Σ ++>
Figure BDA00035148489000000712
Is the current pose point of the unmanned platform +.>
Figure BDA00035148489000000713
Covariance matrix of>
Figure BDA00035148489000000714
If the value is greater than a certain threshold value, the candidate loop point is selected as +.>
Figure BDA00035148489000000715
Wherein eigValuek is a eigenvalue of the matrix, k represents the kth eigenvalue, ++>
Figure BDA00035148489000000716
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
Figure BDA0003514848900000081
Refers to the local planning scope +.>
Figure BDA0003514848900000082
Known interface of inner and unexplored gridsGrid point, which is used for judging +.>
Figure BDA0003514848900000083
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
Figure BDA0003514848900000084
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 +.>
Figure BDA0003514848900000085
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>
Figure BDA0003514848900000091
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.
Figure BDA0003514848900000092
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;
Figure BDA0003514848900000093
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
Figure BDA0003514848900000094
Wherein b i The i-th planned path is a set of a plurality of sampling points on the local topological graph.
Figure BDA0003514848900000095
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
Figure BDA0003514848900000101
Is composed of 3-dimensional translation vector and rotation lie algebra. The state vector to be estimated is +.>
Figure BDA0003514848900000102
Wherein (1)>
Figure BDA0003514848900000103
Figure BDA0003514848900000104
The measurement equation h between two nodes i, j can be expressed as:
Figure BDA0003514848900000105
wherein,,
Figure BDA0003514848900000106
and->
Figure BDA0003514848900000107
In order to observe the noise it is possible,
Figure BDA0003514848900000108
Figure BDA0003514848900000109
2. loop information gain definition:
Figure BDA00035148489000001010
wherein,,
Figure BDA00035148489000001011
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.
Figure BDA00035148489000001012
If b i Sampling point on
Figure BDA00035148489000001013
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 +.>
Figure BDA00035148489000001014
E loop I.e. the set of all the above-mentioned edges.
Figure BDA00035148489000001023
Representing a pose graph G p The fischer information matrix of (2) can be given by document 3.
Figure BDA00035148489000001015
Figure BDA00035148489000001016
Wherein L is a pose graph G p Is used to determine the matrix of the laplace matrix,
Figure BDA00035148489000001017
is the pose graph G p Translation component->
Figure BDA00035148489000001018
Is a weighted Laplace matrix of +.>
Figure BDA00035148489000001019
Is the pose graph G p Rotational component->
Figure BDA00035148489000001020
Is a weighted laplacian matrix of (c).
Similarly, the number of the devices to be used in the system,
Figure BDA00035148489000001021
representing the pose map->
Figure BDA00035148489000001022
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
Figure BDA0003514848900000111
There is no local leading edge point +.>
Figure BDA0003514848900000112
Or candidate loop point->
Figure BDA0003514848900000113
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
Figure BDA0003514848900000114
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 +.>
Figure BDA0003514848900000115
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 (+)>
Figure BDA0003514848900000116
Or->
Figure BDA0003514848900000117
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
Figure BDA0003514848900000121
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
Figure FDA0004146259510000014
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>
Figure FDA0004146259510000015
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>
Figure FDA0004146259510000011
Gain(b i ) Planning path b for ith i The represented information gain;
step three, traversing the global front edge point set
Figure FDA0004146259510000012
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 +.>
Figure FDA0004146259510000013
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
Figure FDA0004146259510000021
From the local planning scope->
Figure FDA0004146259510000022
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)
Figure FDA0004146259510000023
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
Figure FDA0004146259510000024
(3) For the following
Figure FDA0004146259510000025
Is>
Figure FDA0004146259510000026
Calculate->
Figure FDA0004146259510000027
Wherein (1)>
Figure FDA0004146259510000028
Is->
Figure FDA0004146259510000029
Each vertex in the pose graph G p Corresponding covariance matrix Σ, ++>
Figure FDA00041462595100000210
Is the current pose point of the unmanned platform +.>
Figure FDA00041462595100000211
Is used for the co-variance matrix of (a),
Figure FDA00041462595100000212
if the value is greater than a certain threshold value, the candidate loop point is selected as +.>
Figure FDA00041462595100000213
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,
Figure FDA00041462595100000214
wherein,,
Figure FDA00041462595100000215
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 +.>
Figure FDA00041462595100000216
Representing pose graph G p Is claimed in the form of a Fisher information matrix, < ">>
Figure FDA00041462595100000217
Representing the pose map->
Figure FDA00041462595100000218
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.
CN202210161358.7A 2022-02-22 2022-02-22 Double-stage active real-time positioning and mapping algorithm based on graph topology Active CN114596360B (en)

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 (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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)

* Cited by examiner, † Cited by third party
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

Also Published As

Publication number Publication date
CN114596360A (en) 2022-06-07

Similar Documents

Publication Publication Date Title
Zhang et al. 2D Lidar‐Based SLAM and Path Planning for Indoor Rescue Using Mobile Robots
CN114596360B (en) Double-stage active real-time positioning and mapping algorithm based on graph topology
Lacaze et al. Path planning for autonomous vehicles driving over rough terrain
Ström et al. Predictive exploration considering previously mapped environments
Schmid et al. A unified approach for autonomous volumetric exploration of large scale environments under severe odometry drift
Jian et al. Putn: A plane-fitting based uneven terrain navigation framework
Ab Wahab et al. Path planning for mobile robot navigation in unknown indoor environments using hybrid PSOFS algorithm
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
Balan et al. Optimal trajectory planning for multiple waypoint path planning using tabu search
CN114296474A (en) Unmanned aerial vehicle path planning method and system based on path time cost
CN112857370A (en) Robot map-free navigation method based on time sequence information modeling
Zheng et al. A hierarchical approach for mobile robot exploration in pedestrian crowd
Zhang et al. Dual-Layer path planning with pose SLAM for autonomous exploration in GPS-denied environments
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
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
Zhang et al. 2D map building and path planning based on LiDAR
Lu et al. A randomized hybrid system approach to coordinated robotic sensor planning
Luo et al. Planning optimal trajectory for histogram-enabled mapping and navigation by an efficient PSO algorithm
Domınguez et al. Internal simulation for autonomous robot exploration of lava tubes
Zheng et al. A method for UAV tracking target in obstacle environment

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