CN114596360A - Double-stage active instant positioning and graph building algorithm based on graph topology - Google Patents

Double-stage active instant positioning and graph building algorithm based on graph topology Download PDF

Info

Publication number
CN114596360A
CN114596360A CN202210161358.7A CN202210161358A CN114596360A CN 114596360 A CN114596360 A CN 114596360A CN 202210161358 A CN202210161358 A CN 202210161358A CN 114596360 A CN114596360 A CN 114596360A
Authority
CN
China
Prior art keywords
graph
path
local
point
global
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.)
Granted
Application number
CN202210161358.7A
Other languages
Chinese (zh)
Other versions
CN114596360B (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 instant positioning and graph building algorithm based on graph topology, which is divided into a global stage and a local stage; firstly, an algorithm acquires the current pose, a pose graph and a navigation map of the unmanned platform, and calculates local leading edge points and candidate looping points. And if the point exists, entering a local exploration stage, constructing a local topological graph through a fast random spanning tree algorithm, solving the shortest path reaching any vertex of the local topological graph by utilizing a Dijkstra algorithm, scoring the path set according to a local exploration and active loop combined objective function, solving an optimal path, and updating a global topological graph and a global leading point according to the local topological graph and the local leading point. Otherwise, when the local range has no local front point and no loop requirement, the algorithm enters a global exploration stage, the unmanned platform goes to an unexplored area to perform local exploration again according to the global topological graph until the exploration of the designated area is completed, the unmanned platform returns to the starting point, and the exploration task is completed.

Description

Double-stage active instant positioning and graph building 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 two-stage active instant positioning and map building algorithm based on a map topology.
Background
The Unknown Environment active Exploration (Unknown Environment automation Exploration) technology refers to that an unmanned platform autonomously searches and detects an Unknown Environment under the condition of lacking prior information, collects environmental information, provides favorable support for subsequent disposal, and can be widely applied to dangerous scenes such as urban explosion elimination and risk elimination, nuclear biochemical Environment survey, mine cavity rescue, asteroid development and the like. The method has the significance that the survey of the complex environment can be realized without depending on an external positioning device (such as Beidou positioning) or personnel operation, and the danger of a detection task is greatly reduced. An instant positioning and Mapping (SLAM) algorithm is to determine the increment of the position of the self by using an environment perception sensor so as to determine the position of an ontology in an environment map, and meanwhile, an environment point cloud map is established according to position information and environment perception data. The positioning accuracy and the mapping quality directly determine whether the unmanned platform can complete the exploration task and the quality of the exploration task. Therefore, how to improve the precision of the SLAM method by the active path planning method is a hot point of research of the present researchers.
For an unknown environment autonomous exploration algorithm of a ground unmanned platform, the existing solutions are as follows:
scheme 1: the GBP planer algorithm is proposed in the literature (Dang T, MasCarich F, Khattak S, et al, graph-based path planning for autonomus roboficial amplification in sub-tertiary environments [ C ]//2019IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE,2019: 3105-. The algorithm mainly contributes to providing a dual-stage exploration algorithm framework for the first time, performing global/local exploration path planning algorithm implementation by using two dynamically-expanded rapid Random generation graphs (RRG), performing target function design on exploration information and navigation cost, and performing special design on an autonomous return method to realize coverage search of a large-range multi-branch unknown tunnel environment. But still has the problems of large computational complexity, no consideration of SLAM algorithm characteristics and the like.
Scheme 2: the DSVP algorithm proposed in the document (H.Zhu, C.Cao, S.Scherer, J.Zhang, and W.Wang.DSVP: Dual-Stage Viewpoint platform for Rapid expansion by Dynamic expansion delivery. IEEE/RSJ Intl.Conf.on Intelligent Robots and Systems (IROS). Prag, Czech, Sept.2021) is improved for GPB plane algorithm, and a heuristic iterative fast expanding Random Tree (RRG) algorithm is proposed to replace the RRG algorithm in GBP plane local Exploration phase, thereby improving algorithm execution efficiency.
Scheme 3: the document (Chen Y, Huang S, ZHao L, et al. Cram er-Rao Bounds and optical Design Metrics for position-Graph SLAM [ J ]. IEEE Transactions on Robotics,2021, PP (99):1-15.) proposes to use a Fisher information matrix and a minimum spanning tree number to measure the uncertainty of the Pose Graph in the SLAM algorithm, but does not apply the scale to active exploration.
The invention is inspired by the three schemes, and experimental verification is carried out according to the characteristics of the problems to be solved, so that a large-range unknown environment active positioning and mapping method based on map topology is provided, an autonomous exploration algorithm considering SLAM algorithm precision is designed for a SLAM algorithm based on a pose map, the algorithm provides positioning information and pose map information and unknown region information and obstacle information provided by a navigation map by utilizing the SLAM algorithm, joint modeling is carried out on the exploration problem and the active SLAM problem, the RRT method is used for solving, the safety and the fast exploration are ensured, the loop constraint in the pose map is increased, and the SLAM algorithm precision in the exploration process is improved. In addition, the algorithm uses a global/local dual-stage algorithm framework, so that the time complexity and the space complexity of the algorithm are reduced, and the collision-free path generation under an unknown environment is realized. Finally, the unmanned platform can perform tracking control on the path through controllers such as a dynamic sliding window method and the like, and autonomous positioning and image building under an unknown environment are achieved.
Disclosure of Invention
In view of the above, the invention provides a two-stage active instant positioning and mapping algorithm based on graph topology, which can provide positioning information and pose graph information and unknown region information and obstacle information provided by a navigation map by using an SLAM algorithm, perform joint modeling on an exploration problem and an active SLAM problem, solve the problem by using an RRT method, increase loop constraints in the pose graph while ensuring safe and rapid exploration, and improve the precision of the SLAM algorithm in an exploration process.
The technical scheme for realizing the invention is as follows:
a two-stage active instant positioning and mapping algorithm based on graph topology comprises the following steps:
the method comprises the steps of firstly, obtaining a real-time pose graph and a navigation map of the unmanned platform, screening candidate looping points by using the pose graph, and generating local front edge points by using current position information and the navigation map in the pose graph; if the local front edge point or the candidate loopback point exists, proving that an explorable area or an area capable of improving the SLAM precision exists in the local area, and executing a second step; otherwise, the current area is proved to have no local planning requirement, and an exploration task or a loop task needs to be executed in another area to execute the third step;
step two, the unmanned platform plans the scope in local according to self position and navigation map
Figure BDA0003514848900000031
Using RRT algorithm to generate a collision-free local topological graph in space, using Dijkstra algorithm to solve the shortest path on the local topological graph aiming at each vertex of the local topology on the local topological graph, and filling each path into a path set B; for each path B in path set BiScoring 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 local front edge point and the local topological graph are used for updating the global front edge point and the global topological graph, if the branch Gain (B) exists in the path set Bi) If more than 0, adding all sampling points of the branch into the global topological graph, and when the global leading edge point set is updated, adding all sampling points of the branch into the global topological graph
Figure BDA0003514848900000041
Adding the global leading edge point set obtained last time, then judging each point in the point set, judging whether the grid where the point set is located is a passable grid and is connected with an unknown grid, and simultaneously observing by at least one vertex in the global topological graph, wherein a set formed by the points meeting the conditions is called the global leading edge point set
Figure BDA0003514848900000042
Step three, traversing the global leading edge point set
Figure BDA0003514848900000043
For a certain global leading edge point fgFinding out the Euclidean distance closest point on the global topological graph and utilizing Dijkstra algorithm to carry out Euclidean distance measurement on the closest point and the current position of the robot
Figure BDA0003514848900000044
Solving the shortest path on the global topological graph to finally obtain a path set; the front edge point f corresponding to the shortest path of the path set lengthgThe 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 plans the shortest path through the global topological graphThe path returns to the starting point.
Further, the candidate loopback point set
Figure BDA0003514848900000045
By planning the range locally
Figure BDA0003514848900000046
Pose graph G with potential loop information gain in memorypThe upper vertex; the significance of which lies in judging
Figure BDA0003514848900000047
Whether valuable loop back behaviors exist in the loop back device or not is judged, and the calculation complexity of subsequent loop back information gain is reduced.
Further, the candidate looppoint screening steps are as follows:
1. traversal vertex set VpIf the vertex is located at all of the vertices in (1)
Figure BDA0003514848900000048
Put into the vertex set Vl pThe preparation method comprises the following steps of (1) performing;
2. deleting V by using DBSCAN (sensitivity-Based Spatial Clustering of Applications with Noise, DBSCAN) Clustering algorithml pThe vertex which is closer to the time sequence of the current pose point is obtained to obtain a vertex set
Figure BDA0003514848900000049
3. For the
Figure BDA00035148489000000410
Each vertex in
Figure BDA00035148489000000411
Computing
Figure BDA00035148489000000412
Wherein the content of the first and second substances,
Figure BDA00035148489000000413
is that
Figure BDA00035148489000000414
In the pose graph G of each vertexpThe corresponding covariance matrix sigma,
Figure BDA00035148489000000415
is the current pose point of the unmanned platform
Figure BDA00035148489000000416
The covariance matrix of (a) is determined,
Figure BDA00035148489000000417
if the value is greater than a certain threshold, it can be selected as a candidate loopback point
Figure BDA0003514848900000051
Among them, eigValuek(x) is the eigenvalue of the matrix, k represents the k-th eigenvalue,
Figure BDA0003514848900000052
representing the dimensions of the square matrix.
Further, in step two, the path information gain includes three parts: 1. searching information gain in an unknown area; 2. active loop SLAM information gain; 3. and (4) navigation cost.
Further, the active loop back SLAM information gain LoopGain (x) refers to the degree to which the uncertainty of the SLAM pose graph can be reduced by the current path,
Figure BDA0003514848900000053
wherein the content of the first and second substances,
Figure BDA0003514848900000054
represents the ith planning path b of simulation executioniThen, the formed position diagram takes the logarithm operation of the determinant with matrix Dopt (log det),
Figure BDA0003514848900000055
diagram G for showing posepThe matrix of the fisher information of (a),
Figure BDA0003514848900000056
diagram for showing pose
Figure BDA0003514848900000057
The fisher information matrix of (a).
Further, in the second step, the selected optimal path is given to a path tracking algorithm to solve the linear velocity and the angular velocity of the chassis of the unmanned platform, and the platform can travel along the optimal path planned at the current position.
Further, in the third step, the planned path is issued to a path tracking algorithm to realize the motion of the unmanned platform.
Has the advantages that:
the invention provides a two-stage active instant positioning and mapping algorithm framework based on graph topology, aiming at the problem of active instant positioning and mapping of a ground unmanned platform in a large-scale unknown environment, and the innovation points of the framework are mainly embodied in the following aspects:
the method provides a combined path planning algorithm of autonomous exploration and active loopback based on graph topology, solves the problem of error accumulation caused by no loopback of the SLAM algorithm for a long time in the autonomous exploration process, and improves the positioning accuracy and the graph building quality of an unmanned platform in the active graph building 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 the algorithm, can complete the autonomous exploration task of a large-scale unknown complex scene, considers SLAM precision, and can realize autonomous return voyage.
Drawings
FIG. 1 is a framework diagram of a two-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) an autonomous exploration and mapping of the simulation environment.
FIG. 3 is a comparison graph of the square sum of the current position error of the unmanned platform.
FIG. 4 is a comparison graph of the optimal scale change of the covariance matrix D of the current position of the unmanned platform.
FIG. 5 is a map comparing navigation maps obtained by active exploration of an unmanned platform in a ground-based environment, (a) a real environment and a real unmanned platform, (b) a navigation map obtained by a two-stage exploration algorithm, and (c) a navigation map obtained by a two-stage active real-time positioning algorithm.
Detailed Description
The invention is described in detail below by way of example with reference to the accompanying drawings.
The invention provides a two-stage active instant positioning and graph building algorithm framework based on graph topology, the overall algorithm framework is shown in figure 1, the algorithm effect is shown in figure 2, wherein figure 2(a) is a simulation environment and a simulation trolley, and figure 2(b) shows an environment model established and various generated topological graphs of an unmanned platform in an autonomous exploration process.
Step one, local/global stage decision algorithm
Firstly, a pose graph G of the unmanned platform is given by a double-stage active instant positioning and mapping algorithm through a laser inertia tight coupling SLAM algorithm (lio-sam)p(Vp，Ep) Wherein V ispSet of historical key pose points for unmanned platform, EpFor the set of observed edges between key poses, each pose point (including the current position of the robot)
Figure BDA0003514848900000061
) Or the observed edges all with their covariance matrix (uncertainty) Σ. Meanwhile, a navigation map (a two-dimensional grid map used for generating a passable path and a local front point and used for dividing the space into grids of 0.1m by 0.1m, wherein grid points can be divided into three types, namely a passable grid, an obstacle grid and an unexplored grid) is established by using the laser radar point cloud and the current pose of the unmanned platform through a two-stage active instant positioning and mapping algorithm. Then, based on the information given in the previous step, a double-stage active instant positioning and mapping algorithm is used for screening candidate loop points and generating local leading edge points, and if the local leading edge points or the candidate loop points existPoint, proving that a searchable area or an area capable of improving SLAM precision exists in the local area, and executing a step two; otherwise, it is proved that the current area has no local planning requirement, and an exploration or loop task needs to be executed in another area, and the third step is executed.
(1) Candidate looppoint screening
Set of candidate loopback points
Figure BDA00035148489000000717
By planning the range locally
Figure BDA0003514848900000071
(
Figure BDA0003514848900000072
Is defined as
Figure BDA0003514848900000073
A square with a length of 15m x 15m as a center) has potential loop information gainpThe upper vertex. The significance of which lies in judging
Figure BDA0003514848900000074
Whether valuable loop back behaviors exist in the loop back information reduces the calculation complexity of subsequent loop back information gain.
The candidate looppoint screening steps are as follows:
1. traversing all the vertexes in the vertex set Vp if the vertex is positioned
Figure BDA0003514848900000075
In, the vertex set V is setl pThe preparation method comprises the following steps of (1) performing;
2. deleting V by using DBSCAN (Density-Based Spatial Clustering of applications-with Noise, DBSCAN) Clustering algorithml pThe vertex which is closer to the time sequence of the current pose point is obtained to obtain a vertex set
Figure BDA0003514848900000076
3. For the
Figure BDA0003514848900000077
Each vertex in
Figure BDA0003514848900000078
Computing
Figure BDA0003514848900000079
Wherein the content of the first and second substances,
Figure BDA00035148489000000710
is that
Figure BDA00035148489000000711
In the pose graph G of each vertexpThe corresponding covariance matrix sigma,
Figure BDA00035148489000000712
is the current pose point of the unmanned platform
Figure BDA00035148489000000713
The covariance matrix of (a) is determined,
Figure BDA00035148489000000714
if the value is greater than a certain threshold, it can be selected as a candidate loopback point
Figure BDA00035148489000000715
Wherein eigValuek () is a feature value for the matrix, k represents the kth feature value,
Figure BDA00035148489000000716
representing the dimensions of the square matrix.
The significance lies in that:
1. local area constraints are satisfied;
2. a small amount of loop information gain exists at a point which is close to the time sequence of the current pose point, but the exploration efficiency is greatly reduced, so that the loop information gain is eliminated;
3. according to the SLAM algorithm based on the pose graph, each vertex on the pose graph obeys Gaussian distribution v-N (mu, sigma), wherein mu is the pose of the vertex, sigma is a covariance matrix of the vertex, and the larger the covariance matrix is, the larger the error value is. And (4) converting the covariance matrix of the vertex to be judged into a scalar, judging the scalar and the covariance matrix dopt () of the current pose, and if the difference is larger, proving that the self positioning precision can be improved or the mapping error can be reduced through loop constraint, and listing the mapping error as a candidate loop point.
(2) Local leading edge point generation
Local front edge point set
Figure BDA0003514848900000081
Refers to the local planning range
Figure BDA0003514848900000082
Known grid points of the inter-and unexplored grid boundary are 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 mapping path planning algorithm
If the unmanned platform enters the step two, the unmanned platform should execute a local active real-time positioning and mapping path planning algorithm, and the algorithm at the stage aims at active unknown environment exploration or active loop based on a pose graph. As shown in the process block diagram of fig. 1, the algorithm flow includes five parts, namely, establishing local RRT, finding the shortest path for all the vertices by using Dijkstra algorithm, evaluating path information gain, selecting the optimal path and the global leading point, and updating the global topological graph. The unmanned platform plans the range in the local area according to the self position and the navigation map
Figure BDA0003514848900000084
The method comprises the steps of generating a collision-free local topological graph in space by utilizing an RRT algorithm, solving the shortest path on the local topological graph by utilizing a Djkstra algorithm aiming at each vertex of the local topology on the local topological graph, and filling each path into a path set B. For each path B in path set BiUsing path information to increaseAnd scoring is carried out, and the path with the highest score in the path set B is the optimal path. Then, the local front edge point and the local topological graph are used for updating the global front edge point and the global topological graph, if the branch Gain (B) exists in the path set Bi) If more than 0, adding all sampling points of the branch into the global topological graph, and when the global leading edge point set is updated, adding all sampling points of the branch into the global topological graph
Figure BDA0003514848900000085
Adding the global leading edge point set obtained last time, then judging each point in the point set, judging whether the grid where the point set is located is a passable grid and is connected with an unknown grid, and simultaneously observing by at least one vertex in the global topological graph, wherein a set formed by the points meeting the conditions is called the global leading edge point set
Figure BDA0003514848900000091
And finally, the selected optimal path is given to a path tracking algorithm to solve the linear velocity and the angular velocity of the chassis of the unmanned platform, and the platform can travel along the optimal path planned at the current position. The innovation of the invention is mainly embodied in the proposal of path information gain.
The path information gain mainly comprises three parts: 1. searching information gain in an unknown area; 2. active loop SLAM information gain; 3. and (4) navigation cost.
Figure BDA0003514848900000092
Wherein, Gain (b)i) For the ith path biGain of information represented, λ1...λ4As a weight coefficient, LoopGain (b)i) Is b isiThe represented loop information gain; exploratingain (b)i) Is b isiThe represented unknown region exploration information gain;
Figure BDA0003514848900000093
the inverse of the navigation cost.
(1) Unknown region search information gain
The unknown region exploration information gain needs to satisfy the formula as follows
Figure BDA0003514848900000094
Wherein, biThe ith planned path is a set of a plurality of sampling points on the local topological graph.
Figure BDA0003514848900000095
Is biThe jth sample point above. VoxelGain (×) represents the number of all unknown grids visible within the field of view of the sample point. The method can be implemented in a grid map by using a RayTrace algorithm in computer graphics.
(2) Active loop SLAM information gain
The active loop SLAM information gain LoopGain (x) refers to the degree that the uncertainty of an SLAM pose graph can be reduced by a current path, and the purpose is to improve the positioning accuracy and the mapping accuracy of the SLAM through the loop constraint function of an SLAM graph optimization algorithm.
SLAM measurement model definition:
the unmanned platform state space is
Figure BDA0003514848900000101
Consisting of a 3-dimensional translation vector and a rotated lie algebra. The state vector to be estimated is
Figure BDA0003514848900000102
Wherein the content of the first and second substances,
Figure BDA0003514848900000103
Figure BDA0003514848900000104
the measurement equation h between two nodes i, j can be expressed as:
Figure BDA0003514848900000105
wherein the content of the first and second substances,
Figure BDA0003514848900000106
and
Figure BDA0003514848900000107
in order to observe the noise, it is,
Figure BDA0003514848900000108
Figure BDA0003514848900000109
2. loop back information gain definition:
Figure BDA00035148489000001010
wherein the content of the first and second substances,
Figure BDA00035148489000001011
represents the ith planning path b of simulation executioniAnd forming a pose graph. Dopt (x) ═ log det (x) is a determinant of the matrix and a logarithmic operation is performed.
Figure BDA00035148489000001012
If b isiSampling point of
Figure BDA00035148489000001013
And any key point n on the graphrHas a Euclidean distance smaller than a certain threshold value tauloopThen the pose graph is considered to be possible to generate a loop and construct an edge
Figure BDA00035148489000001014
EloopI.e. the set of all edges mentioned above.
Figure BDA00035148489000001023
Shows a position diagram GpThe fisher information matrix of (1), which can be given by document 3.
Figure BDA00035148489000001015
Figure BDA00035148489000001016
Wherein L is a pose graph GpThe matrix of the laplacian of (c),
Figure BDA00035148489000001017
is a pose graph GpComponent of translation
Figure BDA00035148489000001018
The weighted laplacian matrix of (a) is,
Figure BDA00035148489000001019
is a pose graph GpComponent of rotation
Figure BDA00035148489000001020
The weighted laplacian matrix of (a).
In a similar manner to that described above,
Figure BDA00035148489000001021
diagram for showing pose
Figure BDA00035148489000001022
The calculation process of the fisher information matrix is the same as that described above.
(3) Navigation cost
DTW (x) means a dynamic time warping algorithm, which calculates the similarity between the current selection path and the last selection path and reflects whether the exploration 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 profit is highest.
Step three, global active instant positioning and mapping algorithm
When in the local planning range
Figure BDA0003514848900000111
Absence of local front edge points
Figure BDA0003514848900000112
Or candidate loopback point
Figure BDA0003514848900000113
Then, the algorithm proceeds to step three. The purpose of the global planning algorithm is to guide the unmanned platform to an unexplored area within the global scope for re-exploration.
Traversing a global set of leading edges
Figure BDA0003514848900000114
For a certain global leading edge fgFinding out the Euclidean distance closest point on the global topological graph and utilizing Dijkstra algorithm to carry out Euclidean distance measurement on the closest point and the current position of the robot
Figure BDA0003514848900000115
And solving the shortest path on the global topological graph to finally obtain a path set. The front edge point f corresponding to the shortest path of the path set lengthgNamely the target point, the shortest path corresponding to the point is the optimal path. If no feasible solution exists (
Figure BDA0003514848900000116
Or
Figure BDA0003514848900000117
None of the nodes can reach), the global area exploration is completed, and the unmanned platform plans the shortest path through the global topological graph and returns to the starting point. And similarly to the second step, the planned path is issued to a path tracking algorithm to realize the motion of the unmanned platform.
In the operation of the algorithm, in the initial stage of the algorithm, as local (a square area taking a robot as a center) reachable unknown areas exist, the algorithm continuously executes the step two until no searchable area exists in the current local range, the algorithm is transferred to the step three, the user goes to the rest reachable unknown areas in the current navigation map for searching, and then the step two is executed again.
Finally, the performance of the algorithm of the present invention is improved as shown in fig. 3, 4, and 5, compared with the two-stage search algorithm in document 2. Specifically, in fig. 3, the horizontal axis is algorithm execution time, the vertical axis is sum of squares of positioning errors, the dotted line (- - -) represents a two-stage exploration algorithm, and the solid line (-) represents the method provided by the present invention, so that the method greatly improves the positioning accuracy of the SLAM algorithm in the exploration process; in FIG. 4, the horizontal axis represents time and the vertical axis represents time
Figure BDA0003514848900000121
Namely the uncertainty of the pose graph, the dotted line (- - -) represents a two-stage exploration algorithm, and the solid line (-) represents the method provided by the invention, so that the uncertainty of the pose graph in the exploration process can be reduced by the method; fig. 5 proves the effectiveness of the algorithm through a physical experiment (shown in fig. 5(a) for an unmanned platform and a physical environment), and it can be seen that a point cloud graph established by the two-stage exploration algorithm in fig. 5(b) has a larger difference from a grid map, while a point cloud graph established by the method provided by the invention in fig. 5(c) has no difference from the grid map, so that the mapping accuracy of the algorithm is improved.
In summary, the above description is only a preferred embodiment of the present invention, and is not intended to limit the scope of the present invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (7)

1. A two-stage active instant positioning and mapping algorithm based on graph topology is characterized by comprising the following steps:
the method comprises the steps of firstly, obtaining a real-time pose graph and a navigation map of the unmanned platform, screening candidate looping points by using the pose graph, and generating local front edge points by using current position information and the navigation map in the pose graph; if the local front edge point or the candidate loopback point exists, proving that an explorable area or an area capable of improving the SLAM precision exists in the local area, and executing a second step; otherwise, the current area is proved to have no local planning requirement, and an exploration task or a loop task needs to be executed in another area to execute the third step;
step two, the unmanned platform plans the scope in local according to self position and navigation map
Figure FDA0003514848890000015
Using RRT algorithm to generate a collision-free local topological graph in space, using Dijkstra algorithm to solve the shortest path on the local topological graph aiming at each vertex of the local topology on the local topological graph, and filling each path into a path set B; for each path B in path set BiScoring 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 local front edge point and the local topological graph are used for updating the global front edge point and the global topological graph, if the branch Gain (B) exists in the path set Bi)>0, adding all sampling points of the branch into the global topological graph, and when the global leading edge point set is updated, adding all sampling points of the branch into the global topological graph
Figure FDA0003514848890000011
Adding the global leading edge point set obtained last time, then judging each point in the point set, judging whether the grid where the point set is located is a passable grid and is connected with an unknown grid, and simultaneously observing by at least one vertex in the global topological graph, wherein a set formed by the points meeting the conditions is called the global leading edge point set
Figure FDA0003514848890000012
Step three, traversing the global leading edge point set
Figure FDA0003514848890000013
For a certain global leading edge point fgFinding out the Euclidean distance from the point on the global topological graphNear point, utilizing Dijkstra algorithm to make said near point and robot current position
Figure FDA0003514848890000014
Solving the shortest path on the global topological graph to finally obtain a path set; the front edge point f corresponding to the shortest path of the path set lengthgThe 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 plans the shortest path through the global topological graph and returns to the starting point.
2. The dual-stage active instant positioning and mapping algorithm based on graph topology as claimed in claim 1, wherein the candidate loopback point set
Figure FDA0003514848890000021
By planning the range locally
Figure FDA0003514848890000022
Pose graph G with potential loop information gain in memorypThe upper vertex; the significance of which lies in judging
Figure FDA0003514848890000023
Whether valuable loop back behaviors exist in the loop back information reduces the calculation complexity of subsequent loop back information gain.
3. The dual-stage active instant positioning and mapping algorithm based on graph topology according to claim 2, wherein the candidate loopback point screening step is as follows:
1. traversal vertex set VpIf the vertex is located at all of the vertices in (1)
Figure FDA0003514848890000024
Put into the vertex set Vl pPerforming the following steps;
2. deleting V by using DBSCAN clustering algorithml pMiddle distance current positionThe vertex with the closer time sequence of the gesture points is obtained to obtain a vertex set
Figure FDA0003514848890000025
3. For the
Figure FDA0003514848890000026
Each vertex in
Figure FDA0003514848890000027
Calculating out
Figure FDA0003514848890000028
Wherein the content of the first and second substances,
Figure FDA0003514848890000029
is that
Figure FDA00035148488900000210
In the pose graph G of each vertexpThe corresponding covariance matrix sigma is then calculated,
Figure FDA00035148488900000211
is the current pose point of the unmanned platform
Figure FDA00035148488900000212
The covariance matrix of (a) is determined,
Figure FDA00035148488900000213
if the value is greater than a certain threshold, it can be selected as a candidate loopback point
Figure FDA00035148488900000214
Among them, eigValuekAnd (, k) represents the k-th eigenvalue, and l represents the dimension of the square matrix.
4. The dual-stage active real-time location and mapping algorithm based on graph topology as claimed in claim 1, wherein in step two, the path information gain comprises three parts: 1. searching information gain in an unknown area; 2. active loop SLAM information gain; 3. and (4) navigation cost.
5. The dual-stage active real-time localization and mapping algorithm based on graph topology as claimed in claim 4, wherein the active loop SLAM information gain LoopGain (x) refers to the degree that the uncertainty of the SLAM pose graph can be reduced by the current path,
Figure FDA00035148488900000215
wherein the content of the first and second substances,
Figure FDA00035148488900000216
represents the ith planning path b of simulation executioniThen, the formed position diagram takes Dopt (one) ═ logdet (one) — as the determinant of the matrix to carry out the logarithm operation,
Figure FDA00035148488900000217
show the position and orientation diagram GpThe matrix of the fisher information of (a),
Figure FDA0003514848890000031
diagram for showing pose
Figure FDA0003514848890000032
The fisher information matrix of (a).
6. The dual-stage active real-time positioning and mapping algorithm based on graph topology as claimed in claim 1, wherein in step two, the selected optimal path is given to a path tracking algorithm to solve the linear velocity and angular velocity of the unmanned platform chassis, and the platform can travel along the optimal path currently planned.
7. The dual-stage active real-time location 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 true CN114596360A (en) 2022-06-07
CN114596360B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115617034A (en) * 2022-09-01 2023-01-17 清华大学 Multi-agent environment exploration method and device, electronic equipment and storage medium
CN116182840A (en) * 2023-04-28 2023-05-30 科大讯飞股份有限公司 Map construction method, device, equipment and storage medium

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110703747A (en) * 2019-10-09 2020-01-17 武汉大学 Robot autonomous exploration method based on simplified generalized Voronoi diagram
CN111427047A (en) * 2020-03-30 2020-07-17 哈尔滨工程大学 Autonomous mobile robot S L AM method in large scene
CN111583136A (en) * 2020-04-25 2020-08-25 华南理工大学 Method for simultaneously positioning and establishing image of autonomous mobile platform in rescue scene
CN111638526A (en) * 2020-05-20 2020-09-08 电子科技大学 Method for robot to automatically build graph in strange environment

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110703747A (en) * 2019-10-09 2020-01-17 武汉大学 Robot autonomous exploration method based on simplified generalized Voronoi diagram
CN111427047A (en) * 2020-03-30 2020-07-17 哈尔滨工程大学 Autonomous mobile robot S L AM method in large scene
CN111583136A (en) * 2020-04-25 2020-08-25 华南理工大学 Method for simultaneously positioning and establishing image of autonomous mobile platform in rescue scene
CN111638526A (en) * 2020-05-20 2020-09-08 电子科技大学 Method for robot to automatically build graph in strange environment

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
胡丹丹等: "面向室内退化环境的多传感器建图方法" *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115617034A (en) * 2022-09-01 2023-01-17 清华大学 Multi-agent environment exploration method and device, electronic equipment and storage medium
CN116182840A (en) * 2023-04-28 2023-05-30 科大讯飞股份有限公司 Map construction method, device, equipment and storage medium
CN116182840B (en) * 2023-04-28 2023-07-25 科大讯飞股份有限公司 Map construction method, device, equipment and storage medium

Also Published As

Publication number Publication date
CN114596360B (en) 2023-06-27

Similar Documents

Publication Publication Date Title
CN111780777B (en) Unmanned vehicle route planning method based on improved A-star algorithm and deep reinforcement learning
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
Ström et al. Predictive exploration considering previously mapped environments
Jian et al. Putn: A plane-fitting based uneven terrain navigation framework
CN113359718A (en) Method and equipment for fusing global path planning and local path planning of mobile robot
Zhang et al. Fast active aerial exploration for traversable path finding of ground robots in unknown environments
CN113110455A (en) Multi-robot collaborative exploration method, device and system for unknown initial state
CN114296474A (en) Unmanned aerial vehicle path planning method and system based on path time cost
CN112612290A (en) Underwater vehicle three-dimensional multi-task path planning method considering ocean currents
Ryu et al. Local map-based exploration using a breadth-first search algorithm for mobile robots
Garcia et al. Portable multi-hypothesis Monte Carlo localization for mobile robots
Wang et al. Virtual maps for autonomous exploration with pose SLAM
Si et al. TOM-Odometry: A generalized localization framework based on topological map and odometry
Zhang et al. Exploration with global consistency using real-time re-integration and active loop closure
Soni et al. Multi-robot unknown area exploration using frontier trees
Zhang et al. Dual-Layer path planning with pose SLAM for autonomous exploration in GPS-denied environments
CN111912411B (en) Robot navigation positioning method, system and storage medium
Murtra et al. Action evaluation for mobile robot global localization in cooperative environments
Kwon et al. A new feature commonly observed from air and ground for outdoor localization with elevation map built by aerial mapping system
Murtra et al. Efficient active global localization for mobile robots operating in large and cooperative environments
CN114815899A (en) Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor
Yang et al. SLAM self-cruise vehicle based on ROS platform
Zhang et al. An adaptive artificial potential function approach for geometric sensing
Wang et al. Research on SLAM road sign observation based on particle filter

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