CN112241002B - Novel robust closed-loop detection method based on Karto SLAM - Google Patents
Novel robust closed-loop detection method based on Karto SLAM Download PDFInfo
- Publication number
- CN112241002B CN112241002B CN202011081092.2A CN202011081092A CN112241002B CN 112241002 B CN112241002 B CN 112241002B CN 202011081092 A CN202011081092 A CN 202011081092A CN 112241002 B CN112241002 B CN 112241002B
- Authority
- CN
- China
- Prior art keywords
- loop detection
- closed
- slam
- data
- karto
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Theoretical Computer Science (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Evolutionary Computation (AREA)
- Evolutionary Biology (AREA)
- General Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Bioinformatics & Computational Biology (AREA)
- Manufacturing & Machinery (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a novel robust closed-loop detection method based on Karto SLAM, which comprises the steps of firstly designing a rapid window filtering algorithm to filter laser data, then optimizing a closed-loop detection part by using a delay decision strategy, wherein the optimized closed-loop detection method is different from the original closed-loop detection method of the algorithm in that the strategy does not consider that a robot comes to the original place after the same environmental information is observed at a point a and a point b, and triggers the closed-loop detection after the same environmental information is observed at a point c and a point d. The accuracy of closed loop detection of the system is improved to a certain extent, the precision of related work such as follow-up path planning, navigation and the like of the robot is further improved, and the problems of positioning and drawing of the robot in special scenes (such as a roadway and a corridor) are solved, so that the practicability and robustness of the algorithm are improved to a certain extent.
Description
Technical Field
The invention belongs to the technical field of robot positioning and navigation, and particularly relates to a novel robust closed-loop detection method based on Karto SLAM.
Background
Synchronized positioning and composition (SLAM) is described as a robot updating its own position and map at a moment in time at an unknown location in an unknown environment through its own sensors. In recent years, SLAM-related intelligent robot related industries are well-behaved, and laser SLAM key technologies are also widely applied to intelligent robot products, such as sweeping robots, unmanned planes, auto-driven automobiles and the like. The SLAM algorithm based on the graph optimization method in the laser SLAM is different from the SLAM algorithm based on filtering in that the pose of the robot at different moments is used as a vertex according to all historical observation data, constraints generated by the observation of the robot at different positions are used as edges or constraints between graph optimization vertices, the SLAM algorithm based on the graph optimization method can well complete positioning and graph building tasks under the condition of a large environmental map, and the SLAM algorithm based on the graph optimization method has the advantages of high precision and high speed.
The SLAM closed loop detection process based on the graph optimization method mainly comprises three detection methods of frame-frame, frame-graph and graph-graph. The frame-graph is a mainstream closed-loop detection method at present because the calculated amount is less than that of other two methods and the detection is more accurate. Loop detection, closed loop detection, refers to the ability of a robot to identify a place that has been in the process of mapping. With the lapse of time, the walking path of the robot is prolonged, the accumulated error is increased, and the optimization of the pose of the robot and the reduction of the accumulated error by using closed-loop detection become important. The correct closed-loop detection can correct the map in time, the robot can be helped to rapidly and accurately carry out subsequent path planning or navigation and other works, and the wrong closed-loop detection can cause the composition to be disordered and distorted, and the positioning fails.
Under normal conditions, the existing Karto SLAM algorithm can well meet the requirements of actual mapping and positioning, but under some special application scenes, such as roadway and corridor application scenes with similar environmental characteristics, the algorithm also has the condition that closed-loop detection is easy to generate errors, and laser data returned by robot laser data acquisition equipment easily has jitter and noise due to uneven road surface and instable equipment, so in order to improve the precision of closed-loop detection, the invention provides a novel robust closed-loop detection method based on Karto SLAM.
Disclosure of Invention
Technical problem to be solved
In order to solve the problem that the conventional Karto SLAM algorithm is easy to generate wrong closed-loop detection under the condition that ground unstable equipment shakes and under the application scene with similar environmental characteristics (such as a roadway and a corridor), the invention provides a novel robust closed-loop detection method based on the Karto SLAM.
Technical scheme
A novel robust closed-loop detection method based on Karto SLAM is characterized by comprising the following steps:
step 1: receiving environmental information scanned by a laser radar scanner;
step 2: preprocessing input laser radar jitter information by using a fast window filtering algorithm:
step 2.1: setting the size of a window, and determining the number of laser radar data points in the window;
step 2.2: calculate the mean of all points within the window
Step 2.3: setting a parameter delta, and comparing the magnitudes of p (x) and delta & p' (x);
step 2.4: if p (x) is larger than delta.p ' (x), replacing p (x) with delta.p ' (x), otherwise, if p (x) is smaller than delta.p ' (x), considering that the data do not fluctuate, and keeping p (x) unchanged;
and step 3: predicting the initial position through the pose data and the milemeter data at the last moment;
and 4, step 4: if the current frame is not the initial pose, judging whether the current frame is a key frame, and if the current frame is the key frame, performing the next scanning matching work;
and 5: projecting data information obtained by the laser radar according to the set angular resolution and angular translation value to obtain a lookup table;
step 6: scanning and matching in a frame-image mode on the lookup table, traversing the search area and then obtaining the position with the highest response value to obtain the angle of the robot;
and 7: projecting the lookup table onto a sub-map, wherein the position with the highest response value is the position of the robot;
and 8: the scanning matching search process adopts coarse matching and fine matching to improve the search efficiency;
and step 9: scanning and matching to obtain a node position mean value and covariance, and adding edges and vertexes;
step 10: and optimizing and identifying the closed loop detection process by adopting a delay decision strategy:
step 10.1: calculating the matched response and covariance of the points a and b;
step 10.2: when the responses of the two points a and b reach a threshold value, calculating the covariance of the matched responses of the two points c and d;
step 10.3: if the c point and the d point also meet the condition, calculating the product T of the relative positions and postures of the four position points 1 ·T 2 ·T 3 ·T 4 Judgment of T 1 ·T 2 ·T 3 ·T 4 Whether the current value is smaller than a set threshold value or not, if the current value meets the condition, carrying out closed-loop detection, and if the current value does not meet the condition, returning to find a new closed-loop point;
step 11: closed loop detection is carried out when the closed loop detection condition is met;
step 12: adjusting edges, performing global closed loop, and performing rear-end optimization by adopting a sparse pose adjustment mode;
step 13: and outputting the pose and the map subjected to global optimization.
The technical scheme of the invention is further that: judging whether the current frame in the step 4 is a key frame or not if the current frame is not the initial pose, wherein the node is established according to the following three conditions:
(1) Setting a minimum time period 3600s for establishing a node, and if the minimum time period is exceeded and the odometer is smaller than the minimum moving distance, establishing a node;
(2) Setting a minimum distance of 0.2m, and if the moving length of the odometer exceeds the minimum distance, establishing a node;
(3) A minimum deflection angle of 10 is set, and if the odometer is turned beyond the minimum deflection angle, a node is established.
The technical scheme of the invention is further that: in the step 5, the data information obtained by the laser radar is projected according to the set angular resolution and the angular translation value to obtain the lookup tables of n angles, the current estimated attitude angle can be obtained through the prediction of the odometer, and the real attitude angle is certainly close to the estimated attitude angle.
The technical scheme of the invention is further described as follows: the step 6 of performing frame-graph scanning matching specifically includes: matching a current frame with running-scans, wherein the running-scans refers to a local laser data chain which meets a certain data scale and is maintained in real time with the distance between the first frame and the last frame within a certain distance range; projecting the lookup table onto a sub-map, wherein the sub-map is generated by a local laser data link; maintaining the current local data chain and utilizing Gaussian to carry out fuzzy operation, supposing that the lookup table hits n points, accumulating the scores of the hit points and dividing the scores by the highest score which can be reached to obtain a response value under the condition that each point hit under the action of Gaussian fuzzy operation has different scores.
The technical scheme of the invention is further described as follows: the sub-map in step 7 refers to a rectangular region of interest, which may also be understood as a reference model, the search region is a rectangular region centered on the position estimated by the odometer, the search region represents a possible range of the final position of the robot, and when scanning is matched, the position with the highest response value is obtained by traversing the search region.
The technical scheme of the invention is further that: the step 7 is specifically as follows:
step 7.1: constructing two likelihood fields of coarse resolution and fine resolution;
and 7.2: firstly, searching on a coarse resolution likelihood field to obtain an optimal pose;
step 7.3: carrying out fine resolution division on the grids corresponding to the coarse resolution optimal pose, and then carrying out fine resolution search to obtain the optimal pose again;
step 7.4: the likelihood field value of the grid of the coarse resolution map is the maximum value of all grids corresponding to the corresponding fine resolution map.
The technical scheme of the invention is further that: step 8 is carried out in a mode of traversing, optimizing and scanning and matching the real-time correlation of the three-dimensional window; two times of search of coarse resolution and fine resolution of double resolutions are adopted; the method comprises the following four steps:
step 8.1: constructing two likelihood fields of coarse resolution and fine resolution;
step 8.2: firstly, searching on a coarse resolution likelihood field to obtain an optimal pose;
step 8.3: carrying out fine resolution division on the grids corresponding to the coarse resolution optimal pose;
step 8.4: and then, carrying out fine resolution search to obtain the optimal pose again, wherein the likelihood field value of the grid of the coarse resolution map is the maximum value of all grids of the space corresponding to the corresponding fine resolution map.
The technical scheme of the invention is further described as follows: the step 9 is specifically as follows:
step 9.1: connecting the current frame with the previous frame node and adding edges;
step 9.2: the current frame is connected with a data chain within a certain distance from the current frame and is added with edges;
step 9.3: and if the response value reaches a certain threshold value, generating an edge of which one end is the current node and the other end is the node with the centroid closest to the centroid of the current node in the data chain.
Advantageous effects
The invention provides a novel robust closed-loop detection method based on Karto SLAM, which comprises the steps of firstly designing a rapid window filtering algorithm to filter laser data, then optimizing a closed-loop detection part by using a delay decision strategy, wherein the optimized closed-loop detection method is different from the original closed-loop detection method of the algorithm in that the strategy does not consider that a robot comes to the original place after the same environmental information is observed at a point a and a point b, and triggers the closed-loop detection after the same environmental information is observed at a point c and a point d. The accuracy of system closed loop detection is improved to a certain extent, the precision of relevant work such as follow-up path planning and navigation of the robot is further improved, and the problems of positioning and mapping of the robot in special scenes (such as a roadway and a corridor) are solved, so that the practicability and robustness of the algorithm are improved to a certain extent.
Drawings
FIG. 1 is a schematic view of a scan matching search area;
FIG. 2 is a schematic diagram of coarse matching and fine matching;
FIG. 3 is a schematic diagram of a delayed decision strategy;
FIG. 4 is a schematic diagram of the scan matching principle;
FIG. 5 is a schematic diagram of the closed loop detection principle;
FIG. 6 is a graph of experimental results before and after fast window filtering;
FIG. 7 is a graph illustrating the effect of the Karto SLAM and Lazy Karto algorithms;
fig. 8 is a plot of relative error contrast for both algorithms.
Detailed Description
The invention will now be further described with reference to the following examples and drawings:
in the existing laser SLAM algorithm, a covariance matrix and state quantity stored by the laser SLAM based on a filtering algorithm are in a square growth relation along with the expansion of an environment map, a Karto SLAM is based on a map optimization method, the algorithm adopts a correlation acceleration method and a correlation matching method, the problems of large calculation pressure and map building collapse of the SLAM based on a filtering mode in a large map range environment are solved, and meanwhile, the addition of a loop detection link makes the laser SLAM possible to correct the map in time and construct a large-scale high-precision map. However, karto SLAM has a poor mapping effect when the device is jittered or in an environment with similar characteristics, and even the closed-loop detection fails and the mapping collapses.
The embodiment of the invention provides a method for designing a rapid window filtering algorithm aiming at equipment jitter and adopting a delay decision strategy based on Karto SLAM, and solves the technical problems that laser data is burred due to the conditions of bumpy road surface and equipment jitter of a robot and loop detection misjudgment is easy to occur in a characteristic similar environment of the robot.
Step 1: receiving environmental information scanned by a laser radar scanner;
characteristic object distance information in the environment is read from the data set. In synchronous positioning and mapping, map representation methods are roughly classified into four types according to different sensors and algorithms: grid maps, feature point maps, direct representation methods, topological maps. Karto SLAM uses a map representation of occupying a grid map, where each pixel in the grid map represents the probability of the distribution of an obstacle. In the occupancy grid map, an unoccupied point is represented by p (s = 1), the probability that the point is occupied is represented by p (s = 0), and the state of a point is represented by the ratio of:
after the measurement is obtained, the state at this point is updated as:
indicating the state of s under the measured value. In an occupancy grid map, a darker color of a point represents a greater likelihood that the point is free, and a lighter color of a point represents a greater likelihood of being occupied.
And 2, step: preprocessing input laser radar data information by using a rapid window filtering algorithm;
the median filtering can overcome fluctuation interference under the accidental condition, the median filtering is not applicable when the parameter changes rapidly, and the arithmetic mean filtering algorithm is not applicable under the condition that the calculation speed is required to be rapid, so that the rapid window filtering algorithm related by the invention reduces the calculation amount while smoothing data. The algorithm firstly converts input laser radar information, specifically comprises the steps of firstly converting distance and angle information under a polar coordinate into data under a rectangular coordinate system, then selecting a window and setting the size of the window, calculating the number of laser information points contained in the window and recording the number of the laser information points as K, and then calculating the average value of all the points in the window:
the magnitude of p (x) is compared to δ · p' (x) as follows:
where the parameter δ is set to 0.7 in the present invention, for all data points in the window, if p (x) is greater than δ · p '(x), it is assumed that the data fluctuation here exceeds the set range, where p (x) is replaced by p' (x), and data smoothing is performed, otherwise, it is assumed that the device is not jittered. And finally outputting all processed laser data for subsequent scanning matching and loop detection.
In order to clearly show the capability of processing jitter data by the rapid window filtering algorithm, the filtering effect simulation is carried out on the laser radar data set in the Matlab simulation environment, and the method specifically comprises the following steps:
the bag file is converted into a mat data set form available for Matlab.
And starting a fast window filtering algorithm to filter the converted data set.
Fig. 6 (1) and (2) are a result graph of the transformed data set and a result graph of the data set after processing the dithered data using a fast window filtering algorithm, respectively.
And 3, step 3: predicting the initial position of the robot through the pose data and the milemeter data at the last moment:
p(x 0:t ,m|z 1:t ,u 1:t ) (5)
and 4, step 4: judging whether the current frame is a key frame or not if the current frame is not the initial pose, if so, establishing a node, and performing the next scanning matching work;
the key frame is judged to be established according to the following main criteria:
(1) Setting the minimum time period of the established node to be 3600s, and if the minimum time period is exceeded and the milemeter is smaller than the minimum moving distance, establishing a node;
(2) Setting a minimum distance of 0.2m, and if the moving length of the odometer exceeds the minimum distance, establishing a node;
(3) A minimum deflection angle of 10 is set, and if the odometer is turned beyond the minimum deflection angle, a node is established.
And 5: projecting the laser data information with a certain angular resolution and an angular translation value to obtain a lookup table; the data information comprises position information and angle information of obstacles around the data information;
fig. 1 shows the search intention, the laser data information is projected with a certain angular resolution and angular translation value to obtain a lookup table of n angles, the current estimated attitude angle can be obtained by odometer prediction, the real attitude angle is necessarily nearby, and the angle calculation method of the lookup table is as follows:
as in fig. 1, the white grid represents the gaussian blur effect, the black position points represent the actual predicted positions of the robot, and the gray points represent the laser data points in the grid map.
Step 6: scanning and matching are carried out on the lookup table through a correlation scanning and matching method, and the position with the highest response value is obtained through traversing the search area, so that the angle information of the robot is obtained;
suppose the robot is from position x 0 And x 1 Sense environmental information, robot at x 0 And x 1 Where one can see some part of the environment, there is a rigid body transformation T to align z 0 And z 1 Scan matching can be interpreted as a process of solving two frames of radar data or a rotation matrix (displacement and rotation) between the radar data and the map;
firstly, a scan matching is modeled by means of maximum likelihood estimation:
wherein x t To be solved for the system, x t-1 Is the pose z of the robot at the previous moment t Set of all data points of the laser radar of the current frame, m t-1 For existing map information, u t-1 The observed quantity (such as IMU data, odometry data, etc.) related to the movement from the previous moment to this moment.
Obviously, the lidar observation data contains mutually independent point data, the model can be simplified, and equation (7) is converted into:
prior to calculating equation (8), the map needs to be rasterized, which splits the probability of an entire frame of observation data into the probability of each point of this frame of data.
And 7: projecting the lookup table onto a sub-map, wherein the position with the highest response value is the final position of the robot;
step 7.1: constructing two likelihood fields of coarse resolution and fine resolution;
and 7.2: firstly, searching on a coarse resolution likelihood field to obtain an optimal pose;
step 7.3: carrying out fine resolution division on the grids corresponding to the coarse resolution optimal pose, and then carrying out fine resolution search to obtain the optimal pose again;
step 7.4: the likelihood field value of the grid of the coarse resolution map is the maximum value of all grids corresponding to the corresponding fine resolution map.
And 8: the search process adopts coarse matching and fine matching to improve the search efficiency;
in the Karto SLAM algorithm, the default search angle range is 20 ° in the rough matching, and the default search angle range is 0.2 ° in the fine matching.
And step 9: scanning and matching to obtain a node position mean value and covariance, and adding edges and vertexes;
in the course of coarse matching and fine matching, setting the tolerance of the optimal response value to 10 -6 If the response value of a certain pose state and the optimal response value are within the tolerance in the matching process, the response value and the optimal response value can be regarded as equal, and the node value is the average value of the pose state response value and the optimal response value.
Adding the vertex in step 9 is equivalent to adding the pose of the keyframe in the graph.
The step 9 of adding edges is mainly divided into the following steps:
step 9.1: connecting the current laser data frame with the previous laser data frame and adding edges;
step 9.2: the current laser data frame is connected with a data chain which is at a certain distance and quantity from the current laser data frame;
step 9.3: searching in a certain range by using a breadth-first searching method, starting traversing all adjacent associated nodes in the certain range by taking a current node as a searching center to generate a data chain meeting a certain quantity, matching the current node with a laser data chain, and generating an edge if response and covariance reach a certain threshold, wherein one end point of the edge is the current node, and the other end point is a node of the laser data chain, the centroid of which is closest to the centroid of the current node.
The schematic diagram of closed loop detection in step 10 is shown in fig. 5, and includes the following steps:
step 10.1: according to the current node, all adjacent nodes are found from the graph optimized in a certain range;
step 10.2: adding adjacent nodes and connected nodes into adjacent associated frames in a BFS (Breadth First Search) Breadth-First Search mode;
step 10.3: selecting candidate nodes which meet a certain number from front to back and are not in the adjacent associated frames within a certain distance range from the current according to the id serial numbers from the laser data manager;
step 10.4: scanning and matching in a frame-graph mode, and when the matched mean value and covariance meet certain requirements, considering that a closed loop is detected, and obtaining an adjusted node;
step 10.5: carrying out global closed loop detection;
step 10.6: and adjusting the sparse pose.
The delayed decision strategy in the step 10 refers to that in general, when the same environmental information is observed at the position a and the position b, the algorithm determines that the robot returns to a place that the robot has come before, and closed-loop detection is performed to optimize the pose. After the delay decision strategy is added, the algorithm does not perform closed-loop detection immediately, and the closed-loop detection is performed only when the same environmental information is observed at the c position point and the d position point.
Step 11: and (3) satisfying the delay decision closed-loop detection condition, and performing closed-loop detection, as shown in fig. 5, wherein the closed-loop detection comprises the following processes:
step 11.1: according to the current node, firstly finding all nodes adjacent to the current node within a certain range from the graph;
step 11.2: adding adjacent nodes and connected nodes into adjacent scanning frames in a breadth-first searching mode;
breadth First Search (also known as Breadth First Search), which is a Search from an initial node V 0 Initially, all nodes V adjacent to the initial node are accessed 1 ,V 2 ,…V n Then sequentially access and V 1 ,V 2 ,…V n And traversing all the non-visited vertexes adjacent to the vertex in the same way until all the nodes are visited, and finding out the nodes meeting the requirements.
Step 11.3: removing nodes adjacent to the current node and connected nodes from the laser radar data, and sequentially selecting candidate frames within a certain range according to the id value until the number requirement is met;
step 11.4: scanning and matching in a frame-graph mode, detecting a closed loop according to the delay decision strategy in the step 9, and obtaining an adjusted and optimized pose;
step 12: adjusting edges, performing global closed loop, and performing rear-end optimization by adopting a sparse pose adjustment mode;
the graph optimization problem is actually a least squares problem in nature, that is, solving the best pose such that the error square function is minimized:
wherein x is i 、x j For vertices in the optimization of the graph, i.e. robot pose, z ij For radar measurements, Ω ij For the information matrix, i.e. the error weight matrix, x can be expressed to some extent i 、x j And the measured value z ij Degree of anastomosis.
For ease of presentation, we shorthand the error function as:
e(x i ,x j ,z ij )＝e ij (x i ,x j )＝e ij (x) (11)
due to the influence of white gaussian noise, the multivariate measurement values of the sensor can be regarded as multivariate gaussian distribution centered on the true values:
in a multivariate gaussian distribution, the higher the gaussian curve is, the more definite it proves to be in this direction, so for the maximum likelihood probability we should make it occur where the probability is the greatest, taking the logarithmic transformation of the measured squared error function as follows:
obviously, the graph optimization problem is essentially solving a least square problem, however, in an actual situation, a variation function between positions and postures of the robot is nonlinear, the nonlinear optimization problem is solved by adopting a method of iterative solution of numerical values, specifically, an initial value is given, variables to be optimized are continuously iteratively updated from the initial value so as to minimize an error of a target function, a convergence condition is met, iteration is stopped, and an optimal solution is obtained. The nonlinear least square problem is solved by using an iterative method, which is mainly divided into Gauss-Newton and Levenberg-Marquardt methods in general, and the linear optimization problem is solved by using an LM method in a Karto SLAM algorithm.
Assume an initial value of
The error function is first order Taylor expanded around the initial value to obtain the optimal solution:
wherein, J ij As a function of error e ij (x) In that
Nearby jacobian matrix, e ij Is composed of
In the form of abbreviations.
Substituting equation (14) into the sum of squared errors:
wherein the content of the first and second substances,
substituting equation (19) into the sum of squared errors function:
in the above formula, c = ∑ c ij ，b＝∑b ij ，H＝∑H ij To minimize the sum of squared errors, equation (20) is derived and made equal to 0:
HΔx * ＝-b (21)
in the above formula, H is the information matrix of the system, and the solution of the system in euclidean space is represented as:
in fact, in this system, which is a non-euclidean space, the solution for the system is expressed as:
the Levenberg-Marquardt method introduces an iteration factor λ to control the iteration speed to reach convergence:
(H+λI)Δx * ＝-b (24)
and continuously iterating until convergence.
Step 13: outputting the pose and the map subjected to global optimization;
the specific embodiment is as follows:
step 1: the experiment adopts a data set to test the algorithm, and a computer used in the experiment is configured as follows: the system is Ubuntu 16.04, the CPU is i5 9400A, the basic frequency is 2666MHz, and the memory is 8GB.
And 2, step: firstly, converting an input laser data set format into a usable Matlab data file format by using a program.
And step 3: and operating a fast window filtering algorithm program, and performing data preprocessing work on the converted data set.
And 4, step 4: fig. 6 is a comparison graph of laser data before and after optimization of the laser data before and after the rapid window filtering algorithm filtering under the condition of data jitter, and a test result shows that: the fast window filtering algorithm can well smooth map burrs of the laser device caused by jitter, and filtered data are used for subsequent loop detection work of the algorithm, so that the accuracy of loop detection and the robustness of the algorithm are further improved.
And 5: the Karto SLAM algorithm after the data processed by the rapid window filtering and the delay decision strategy is called as Lazy Karto, relative position and attitude errors (RPE) are adopted as evaluation indexes in the experiment to evaluate the positioning accuracy of the algorithm, and the positioning accuracy of the Karto SLAM algorithm and the positioning accuracy of the Lazy Karto algorithm are compared.
FIG. 7 shows the prior Karto SLAM algorithm and Lazy Karto mapping effect, eight feature points are selected, a relative error comparison graph is drawn as shown in FIG. 8, the optimized Lazy Karto algorithm has the maximum relative error of 28.99% and the minimum relative error of 4.312% compared with the prior Karto SLAM algorithm, the problems of distortion, ghosting and the like of mapping are caused in the prior Karto SLAM algorithm at the feature points 6, 7 and 8 where closed loops easily occur, the optimized Lazy Karto algorithm can well detect the closed loop points under the condition of equipment jitter of a bumpy road section, and the accuracy and robustness of closed loop detection are improved to a certain extent.
Claims (8)
1. A novel robust closed-loop detection method based on Karto SLAM is characterized by comprising the following steps:
step 1: receiving environmental information scanned by a laser radar scanner;
step 2: preprocessing input laser radar jitter information by using a fast window filtering algorithm:
step 2.1: setting the size of a window, and determining the number of laser radar data points in the window;
step 2.2: calculate the mean of all points within the window
Step 2.3: setting a parameter delta, and comparing the magnitudes of p (x) and delta & p' (x);
step 2.4: if p (x) is larger than delta.p ' (x), replacing p (x) with delta.p ' (x), and if p (x) is smaller than delta.p ' (x), considering that the data is not fluctuated and keeping p (x) unchanged;
and step 3: predicting the initial position through the pose data and the odometer data at the last moment;
and 4, step 4: if the current frame is not the initial pose, judging whether the current frame is a key frame, and if the current frame is the key frame, performing the next scanning matching work;
and 5: projecting data information obtained by a laser radar by using a set angular resolution and an angular translation value to obtain a lookup table;
and 6: scanning and matching in a frame-image mode on the lookup table, traversing the search area and then obtaining the position with the highest response value to obtain the angle of the robot;
and 7: projecting the lookup table onto a sub-map, wherein the position with the highest response value is the position of the robot;
and 8: the scanning matching search process adopts coarse matching and fine matching to improve the search efficiency;
and step 9: scanning and matching to obtain a node position mean value and covariance, and adding edges and vertexes;
step 10: and optimizing and identifying the closed loop detection process by adopting a delay decision strategy:
step 10.1: calculating the matched response and covariance of the points a and b;
step 10.2: when the responses of the points a and b reach a threshold value, calculating the matched responses and covariance of the points c and d;
step 10.3: if the c point and the d point also meet the condition, calculating the product T of the relative poses of the four position points 1 ·T 2 ·T 3 ·T 4 Judgment of T 1 ·T 2 ·T 3 ·T 4 Whether the current value is smaller than a set threshold value or not, if the current value meets the condition, closed-loop detection is carried out, and if the current value does not meet the condition, a new closed-loop point is searched for;
step 11: closed loop detection is carried out when the closed loop detection condition is met;
step 12: adjusting edges, performing global closed loop, and performing rear-end optimization by adopting a sparse pose adjustment mode;
step 13: and outputting the pose and the map subjected to global optimization.
2. The novel robust closed-loop detection method based on Karto SLAM as claimed in claim 1, wherein the current frame in step 4 is not an initial pose, and whether the current frame is a key frame is determined, wherein the node is established according to the following three conditions:
(1) Setting a minimum time period 3600s for establishing a node, and if the minimum time period is exceeded and the odometer is smaller than the minimum moving distance, establishing a node;
(2) Setting a minimum distance of 0.2m, and if the moving length of the odometer exceeds the minimum distance, establishing a node;
(3) A minimum deflection angle of 10 is set, and if the odometer is turned beyond the minimum deflection angle, a node is established.
3. The new Karto SLAM-based robust closed-loop detection method as claimed in claim 1, wherein the step 5 projects the data information obtained from the lidar with a set angular resolution and angular translation value to obtain a lookup table of n angles, the current estimated attitude angle can be obtained by odometer prediction, and the true attitude angle is necessarily near the estimated attitude angle.
4. The new robust closed-loop detection method based on Karto SLAM as claimed in claim 1, wherein the step 6 of performing frame-graph mode scan matching specifically comprises: matching a current frame with running-scans, wherein the running-scans refers to a local laser data chain which meets a certain data scale and is maintained in real time with the distance between the first frame and the last frame within a certain distance range; projecting the lookup table onto a sub-map, wherein the sub-map is generated by a local laser data link; maintaining the current local data chain and carrying out fuzzy by using Gaussian, assuming that the lookup table hits n points, accumulating the scores of the hit points and dividing by the highest score which can be reached to obtain a response value under the condition that each point hit under the action of the Gaussian fuzzy is different in score.
5. The new Karto SLAM-based robust closed-loop detection method as claimed in claim 1, wherein the sub-map in step 7 refers to a rectangular region of interest, which can also be understood as a reference model, the search region is a rectangular region centered on the odometer-estimated position, the search region represents a possible range of the final position of the robot, and the position with the highest response value is obtained by traversing the search region when scanning matches.
6. The novel robust closed-loop detection method based on Karto SLAM as claimed in claim 1, wherein the step 7 is as follows:
step 7.1: constructing two likelihood fields of coarse resolution and fine resolution;
and 7.2: firstly, searching on a coarse resolution likelihood field to obtain an optimal pose;
step 7.3: carrying out fine resolution division on the grids corresponding to the coarse resolution optimal pose, and then carrying out fine resolution search to obtain the optimal pose again;
step 7.4: the likelihood field value of the grid of the coarse resolution map is the maximum value of all grids corresponding to the corresponding fine resolution map.
7. The novel Karto SLAM-based robust closed-loop detection method as claimed in claim 1, wherein step 8 is performed by three-dimensional window traversal optimization real-time correlation scan matching; two times of search of coarse resolution and fine resolution of double resolutions are adopted; the method comprises the following four steps:
step 8.1: constructing two likelihood fields of coarse resolution and fine resolution;
step 8.2: firstly, searching on a coarse resolution likelihood field to obtain an optimal pose;
step 8.3: carrying out fine resolution division on the grids corresponding to the coarse resolution optimal pose;
step 8.4: and then, fine resolution searching is carried out, the optimal pose is obtained again, and the likelihood field value of the grid of the coarse resolution map is the maximum value of all grids of the corresponding space of the corresponding fine resolution map.
8. The novel robust closed-loop detection method based on Karto SLAM as claimed in claim 1, wherein the step 9 is as follows:
step 9.1: connecting the current frame with the previous frame node and adding edges;
step 9.2: the current frame is connected with a data chain within a certain distance from the current frame and is added with edges;
step 9.3: and if the response value reaches a certain threshold value, generating an edge of which one end is the current node and the other end is the node with the centroid closest to the centroid of the current node in the data chain.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011081092.2A CN112241002B (en) | 2020-10-11 | 2020-10-11 | Novel robust closed-loop detection method based on Karto SLAM |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011081092.2A CN112241002B (en) | 2020-10-11 | 2020-10-11 | Novel robust closed-loop detection method based on Karto SLAM |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112241002A CN112241002A (en) | 2021-01-19 |
| CN112241002B true CN112241002B (en) | 2022-10-18 |
Family
ID=74168724
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011081092.2A Active CN112241002B (en) | 2020-10-11 | 2020-10-11 | Novel robust closed-loop detection method based on Karto SLAM |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112241002B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117291973B (en) * | 2023-11-24 | 2024-02-13 | 城市之光(深圳)无人驾驶有限公司 | Quick robust initial positioning method based on laser point cloud |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107741743A (en) * | 2017-11-06 | 2018-02-27 | 深圳精智机器有限公司 | Improved figure optimizes SLAM methods |
| CN109682373A (en) * | 2018-12-28 | 2019-04-26 | 中国兵器工业计算机应用技术研究所 | A kind of sensory perceptual system of unmanned platform |
| US10318659B1 (en) * | 2015-12-17 | 2019-06-11 | Google Llc | Systems and methods of detecting loop closure in simultaneous localization and mapping (SLAM) applications |
| WO2019122939A1 (en) * | 2017-12-21 | 2019-06-27 | University of Zagreb, Faculty of Electrical Engineering and Computing | Interactive computer-implemented method, graphical user interface and computer program product for building a high-accuracy environment map |
| CN110907947A (en) * | 2019-12-04 | 2020-03-24 | 同济人工智能研究院(苏州)有限公司 | Real-time loop detection method in SLAM problem of mobile robot |
| CN111079826A (en) * | 2019-12-13 | 2020-04-28 | 武汉科技大学 | SLAM and image processing fused construction progress real-time identification method |
| CN111258313A (en) * | 2020-01-20 | 2020-06-09 | 深圳市普渡科技有限公司 | Multi-sensor fusion SLAM system and robot |
| CN111427370A (en) * | 2020-06-09 | 2020-07-17 | 北京建筑大学 | Sparse pose adjustment-based Gmapping mapping method for mobile robot |
| CN111551953A (en) * | 2020-05-06 | 2020-08-18 | 天津博诺智创机器人技术有限公司 | Indoor map construction optimization method based on SLAM algorithm |
| CN111551184A (en) * | 2020-03-27 | 2020-08-18 | 上海大学 | Map optimization method and system for SLAM of mobile robot |
| CN111583369A (en) * | 2020-04-21 | 2020-08-25 | 天津大学 | Laser SLAM method based on facial line angular point feature extraction |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140031980A1 (en) * | 2011-11-11 | 2014-01-30 | Jens-Steffen Gutmann | Systems and methods for extending slam to multiple regions |
| CN107239076B (en) * | 2017-06-28 | 2020-06-23 | 仲训昱 | AGV laser SLAM method based on virtual scanning and distance measurement matching |
-
2020
- 2020-10-11 CN CN202011081092.2A patent/CN112241002B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10318659B1 (en) * | 2015-12-17 | 2019-06-11 | Google Llc | Systems and methods of detecting loop closure in simultaneous localization and mapping (SLAM) applications |
| CN107741743A (en) * | 2017-11-06 | 2018-02-27 | 深圳精智机器有限公司 | Improved figure optimizes SLAM methods |
| WO2019122939A1 (en) * | 2017-12-21 | 2019-06-27 | University of Zagreb, Faculty of Electrical Engineering and Computing | Interactive computer-implemented method, graphical user interface and computer program product for building a high-accuracy environment map |
| CN109682373A (en) * | 2018-12-28 | 2019-04-26 | 中国兵器工业计算机应用技术研究所 | A kind of sensory perceptual system of unmanned platform |
| CN110907947A (en) * | 2019-12-04 | 2020-03-24 | 同济人工智能研究院(苏州)有限公司 | Real-time loop detection method in SLAM problem of mobile robot |
| CN111079826A (en) * | 2019-12-13 | 2020-04-28 | 武汉科技大学 | SLAM and image processing fused construction progress real-time identification method |
| CN111258313A (en) * | 2020-01-20 | 2020-06-09 | 深圳市普渡科技有限公司 | Multi-sensor fusion SLAM system and robot |
| CN111551184A (en) * | 2020-03-27 | 2020-08-18 | 上海大学 | Map optimization method and system for SLAM of mobile robot |
| CN111583369A (en) * | 2020-04-21 | 2020-08-25 | 天津大学 | Laser SLAM method based on facial line angular point feature extraction |
| CN111551953A (en) * | 2020-05-06 | 2020-08-18 | 天津博诺智创机器人技术有限公司 | Indoor map construction optimization method based on SLAM algorithm |
| CN111427370A (en) * | 2020-06-09 | 2020-07-17 | 北京建筑大学 | Sparse pose adjustment-based Gmapping mapping method for mobile robot |
Non-Patent Citations (14)
| Title |
|---|
| Cartographer 2D SLAM算法室内建图分析;崔志等;《电子世界》;20200615(第11期);全文 * |
| Efficient Constellation-Based Map-Merging for Semantic SLAM;Kristoffer M. Frey;《2019 International Conference on Robotics and Automation (ICRA)》;20190812;全文 * |
| Efficient Sparse Pose Adjustment for 2D Mapping;Kurt Konolige;《The 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems》;20101203;全文 * |
| Real-Time Correlative Scan Matching;Edwin B. Olson;《2009 IEEE International Conference on Robotics and Automation》;20090706;全文 * |
| Real-Time Loop Closure in 2D LIDAR SLAM;Wolfgang Hess;《2016 IEEE International Conference on Robotics and Automation (ICRA)》;20160609;全文 * |
| SLAM室内三维重建技术综述;危双丰等;《测绘科学》;20180720(第07期);全文 * |
| Towards lazy data association in SLAM;Dirk Hahnel;《11th International Symposium on Robotics Research》;20031022;全文 * |
| 一种基于多传感融合的室内建图和定位算法;纪嘉文等;《成都信息工程大学学报》;20180815(第04期);全文 * |
| 基于ORB关键帧匹配算法的机器人SLAM实现;艾青林等;《机电工程》;20160520(第05期);全文 * |
| 基于SLAM的机器人的导航算法设计;王闯等;《智能机器人》;20191215(第06期);全文 * |
| 基于复合点云帧改进的激光SLAM闭环检测算法;黄永新;《自动化与仪表》;20200625(第06期);全文 * |
| 基于激光雷达的同时定位与地图构建方法综述;危双丰等;《计算机应用研究》;20200228(第02期);全文 * |
| 基于闭环检测的三维点云地图创建;付光耀等;《传感器与微系统》;20200826(第09期);全文 * |
| 融合视觉信息的激光定位与建图;骆燕燕等;《工业控制计算机》;20171225(第12期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112241002A (en) | 2021-01-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Konolige et al. | FrameSLAM: From bundle adjustment to real-time visual mapping | |
| CN111076733B (en) | Robot indoor map building method and system based on vision and laser slam | |
| Lenac et al. | Fast planar surface 3D SLAM using LIDAR | |
| Nieto et al. | Recursive scan-matching SLAM | |
| Minguez et al. | Metric-based scan matching algorithms for mobile robot displacement estimation | |
| CN105760811B (en) | Global map closed loop matching process and device | |
| CN109597864B (en) | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering | |
| Mu et al. | Research on SLAM algorithm of mobile robot based on the fusion of 2D LiDAR and depth camera | |
| WO2012176249A1 (en) | Self-position estimation device, self-position estimation method, self-position estimation program, and mobile object | |
| CN114565616B (en) | Unstructured road state parameter estimation method and system | |
| CN114998276B (en) | Robot dynamic obstacle real-time detection method based on three-dimensional point cloud | |
| CN110763239A (en) | Filtering combined laser SLAM mapping method and device | |
| CN111578959A (en) | Unknown environment autonomous positioning method based on improved Hector SLAM algorithm | |
| Demim et al. | Simultaneous localisation and mapping for autonomous underwater vehicle using a combined smooth variable structure filter and extended kalman filter | |
| CN116758153A (en) | Multi-factor graph-based back-end optimization method for accurate pose acquisition of robot | |
| CN112241002B (en) | Novel robust closed-loop detection method based on Karto SLAM | |
| Donoso-Aguirre et al. | Mobile robot localization using the Hausdorff distance | |
| CN113160280A (en) | Dynamic multi-target tracking method based on laser radar | |
| Reina et al. | A two-stage mobile robot localization method by overlapping segment-based maps | |
| Deng et al. | Improved closed-loop detection and Octomap algorithm based on RGB-D SLAM | |
| Leung et al. | Evaluating set measurement likelihoods in random-finite-set slam | |
| CN115857495A (en) | Vehicle motion state estimation method based on three-dimensional point cloud under curved road environment | |
| CN115950414A (en) | Adaptive multi-fusion SLAM method for different sensor data | |
| CN115512054A (en) | Method for constructing three-dimensional space-time continuous point cloud map | |
| CN111239761B (en) | Method for indoor real-time establishment of two-dimensional map |
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 |