SLAM real-time positioning optimization system based on OSM class map

Abstract

The invention discloses an SLAM real-time positioning optimization system based on an OSM map. The SLAM real-time positioning optimization system comprises a priori OSM map preprocessing module, an SLAM positioning data processing module, a data alignment module and an SLAM positioning map optimization module. According to the method, the map environment information of the Open Street Map type map, the topological information and semantic information of the prior map are extracted and are aligned with the positioning information of the SLAM in real time, so that the SLAM can adjust positioning based on the OSM map information, drift is reduced, and the positioning accuracy is improved.

Description

Technical Field

[0001] The present invention relates to a technology in the field of intelligent driving, specifically a real-time positioning optimization system based on a localization and mapping technology (SLAM) based on an open source map (OpenStreet Map) type map. Background Art

[0002] When the localization and mapping (SLAM) technology works for a long time, the running results will inevitably have larger and larger cumulative errors; and in different working conditions represented by underground, field, and construction sites, and in various scenarios where there may be no GNSS technology assistance, the positioning effect of SLAM technology is less robust and the work is not stable enough. The existing road network map is the mainstream map used for vehicle driving and navigation. Road network maps such as Open Street Map (OSM) used for environmental description are not only relatively reliable in accuracy, but also rich in semantic information, which is very suitable as a benchmark for SLAM positioning optimization. Since SLAM positioning data and OSM type map data are not from the same source, SLAM positioning optimization based on OSM maps needs to solve the alignment problem of heterogeneous data, so the technology of optimizing SLAM positioning effects based on OSM maps still has a technical gap, but a SLAM positioning optimization algorithm based on such environmental description maps is the cornerstone for the future development of SLAM positioning technology. Summary of the invention

[0003] In view of the problems that the existing SLAM technology will inevitably have increasingly larger cumulative errors in the operation results when working for a long time, and the positioning effect of SLAM technology is poorly robust and unstable in different working conditions represented by underground, field and construction sites and various scenes that may not be assisted by GNSS technology, the present invention proposes a SLAM real-time positioning optimization system based on OSM-type maps, which extracts map environment information of Open Street Map-type maps, extracts topological information and semantic information of prior maps, and aligns them with the positioning information of SLAM in real time, so that SLAM can adjust positioning based on OSM map information, reduce drift and improve positioning accuracy.

[0004] The present invention is achieved through the following technical solutions:

[0005] The invention relates to a SLAM real-time positioning optimization system based on an OSM-like map, comprising: an a priori OSM map preprocessing module, a SLAM positioning data processing module, a data alignment module and a SLAM positioning graph optimization module, wherein: the a priori OSM-like map preprocessing module extracts a priori topological map with semantic nodes meaningful to driving from original map data in an offline stage; the SLAM positioning data processing module generates a SLAM topological positioning information graph with key points after sparse processing and key point extraction based on a SLAM algorithm result; the data alignment module performs alignment processing according to the a priori topological map and the SLAM topological positioning information graph, and filters out topological node pairs that meet graph optimization conditions; the graph optimization module constructs a graph optimization system based on the aligned map node data, and is used for optimizing and adjusting the original SLAM positioning data, obtaining a SLAM topological positioning information graph that reduces SLAM positioning drift, and obtaining updated SLAM positioning data after inverse processing by the SLAM positioning data processing module, thereby realizing optimized positioning.

[0006] The prior topological map G of the semantic nodes map = {V map , E map}, V map and E map They represent nodes and edges in the topological map respectively.

[0007] The SLAM topology positioning information graph G SLAM (T) = {V SLAM (T), E SLAM (T)}, V SLAM (T) and E SLAM (T) represent the nodes and edges in the SLAM topology positioning information graph at the current time T.

[0008] The original map data refers to: a priori OSM-type road network map, that is, map data in a format similar to OpenStreetMap, including: recording the latitude and longitude coordinates of a point in the map and semantic information Nodes, marked as Contains road or contour information Ways composed of Nodes, marked as And store possible related information Relations between each information.

[0009] The topological node pair (V match map (T), V match sLAM (T)) match map (T) and V matchSLAM (T) The two node sets have the same number of elements and have a one-to-one matching relationship.

[0010] The data alignment module and the SLAM positioning data processing module operate at the same frequency.

[0011] The extraction refers to: the prior OSM map processing module analyzes the latitude and longitude coordinates and semantic information of the map midpoint in the original map data and road or contour (such as walls, etc.) information According to TAGS N seg and TAGS W seg Extract vehicle-accessible roads After that, extract the As V map , retaining semantic information and building on the relationship information Relations Topological relationship between each other map , forming a topological semantic map G map = {V map , E map}.

[0012] The sparse processing means that the density of the obtained SLAM positioning trajectory is related to the frequency of the SLAM sensor. Such dense data is generally not needed during map post-processing, especially in places with little information (such as straight lines). Therefore, fewer key points will be extracted from the places with no curvature changes in the obtained SLAM positioning data to achieve the effect of sparse processing and reduce the amount of data.

[0013] The key point extraction is: based on the SLAM positioning information, a fixed time length D is selected as a sliding window, the direction of the first positioning point X0 within the D time is taken as the positive direction, the positioning points within the D time are fitted as a curve, and the extreme point of the curve is marked as a key point V SLAM (T), i.e. the inflection point, and sparsely select data according to the curvature to reduce the computational burden of subsequent optimization.

[0014] The alignment process includes: topological information alignment and semantic information alignment.

[0015] Topology information alignment means: comparing the V map and V SLAM The degree of (T) and the structure of a certain level of subtrees expanded with itself as the root node are used as the first level of alignment judgment based on the difference in degree and the similarity of the subtree structure (detected using the WL-Test method);

[0016] Semantic information alignment means: map In the case of containing semantic information, the matching V SLAM (T) Determine whether it is possible to have the semantic information, such as V with intersection semantic labels map More likely to match a V with a larger curvature SLAM (T). In addition, the alignment nodes before time T already exist, so the module at time T focuses on processing the newly added D SLAM (T), and then output it together with the previous matching point set.

[0017] The map node data refers to: topological node pairs (V match map (T), V match SLAM (T)).

[0018] The original SLAM positioning data refers to: the SLAM topology information map G constructed initially SLAM (T) = {V SLAM (T), E.S. LAM (T)}.

[0019] The optimization adjustment mentioned above refers to: match (T) = {V match (T), E match (T)}, V match (T) = V SLAM (T)∪V match map (T), and maintain V match map (T) The node attributes in the set are fixed, which is used as one of the optimization benchmarks; E match (T)=Relation(V match map (T), V match SLAM (T))∪E SLAM (T), the topological node pairs output by the data alignment module and the edges in the SLAM topological positioning information graph are used as the input of the graph optimization module again, and iterative optimization is performed until the maximum number of iterations I is reached max Or the optimization error is less than the error threshold Error th So far, the optimized SLAM topology positioning information graph is obtained.

[0020] The optimization error is: to meet E match (T) is the value of the loss function used as the optimization objective. If there is no error, it means that there is a set of solutions that can perfectly satisfy E match(T), but it is generally not possible. The optimization error is the value of the loss function at the current number of iterations.

[0021] The inverse processing refers to: restoring the optimized SLAM topology information map to the original SLAM positioning trajectory format. Technical Effects

[0022] The present invention extracts the map environment information of a priori map, that is, the Open Street Map-like map, extracts the topological information and semantic information of the priori map, and aligns it with the positioning information of SLAM in real time. The structure of the graph is used to match the two heterogeneous map data of the OSM-like road network map and the SLAM positioning trajectory, and the SLAM positioning trajectory is optimized in combination with the graph optimization algorithm. Compared with the prior art, the present invention can adjust the positioning based on the OSM map information, reduce drift, and improve the positioning accuracy. Through this optimization algorithm, the problem of SLAM positioning failure or poor robustness in some special scenarios can be well solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 It is a schematic diagram of the system of the present invention;

[0024] Figure 2 It is a schematic diagram of the effect of the embodiment;

[0025] In the figure: (a) is the visualization of the prior topological map; (b) is the visualization of the SLAM trajectory map, where the yellow bold nodes are key nodes; (c) is the visualization result after alignment. DETAILED DESCRIPTION

[0026] like Figure 1 As shown, this embodiment relates to a SLAM real-time positioning optimization system based on OSM-type maps, including: a priori OSM map preprocessing module, a SLAM positioning data processing module, a data alignment module and a SLAM positioning map optimization module.

[0027] The input of the prior OSM map preprocessing module is the original map data D map The output is a priori topological map G with semantic nodes that are meaningful for driving. map = {V map , E map}, where V map and E map Represents nodes and edges in the topological map respectively. Figure 1The data format is usually XML, which supports multiple data processing methods. The main contents of the original data include three types of map information: Nodes, Ways and Relations. Nodes are the most basic data information, which records the latitude and longitude coordinates and semantic information of a point in the map, marked as Ways contains information about roads or contours (such as walls) composed of Nodes, marked as Relations stores the possible relationships between various information. The prior OSM map processing module will mainly analyze and Category information, according to TAGS N seg and TAGS W seg Extract vehicle-accessible roads Then the road Extracted as V map , retaining semantic information and building on Relations The topological relationship between them is map , forming a topological semantic map G map = {V map , E map}.

[0028] The SLAM positioning data processing module runs synchronously with the SLAM positioning algorithm in real time, and the input of this module is the result of the SLAM algorithm. SLAM (T), the output is the SLAM topology positioning information graph G after processing the SLAM positioning data at the current time T (T∈{0, 1, 2......}) SLAM (T) = {V SLAM (T), E SLAM (T)}, where V SLAM (T) and E SLAM (T) represent the nodes and edges in the SLAM topology positioning information graph at the current time T. SLAM (T) performs sparse processing and key point extraction, and takes the key points as V SLAM (T) Construct a topological map. One of the implementation logics of key point extraction is to select a fixed time length D as a sliding window based on SLAM positioning information, take the direction of the first positioning point X0 within D time as the positive direction, fit the positioning points within D time as a curve, and mark the extreme point of the curve as the key point V SLAM (T), which is the inflection point in the true sense, and sparsely selects data according to the curvature size to reduce the computational burden of later optimization. SLAM(T) Semantic descriptors are extracted as global alignment operators, such as intersections, buildings, etc.

[0029] The input of the data alignment module is the output of the first two modules {G map , G SLAM (T)}, this module will align the input data and filter out the topological node pairs that meet the graph optimization conditions (V match map (T), V match SLAM (T)), V match map (T) and V match SLAM (T) The number of elements in the two node sets is the same and they have a one-to-one matching relationship. The implementation logic of alignment mainly includes two parts: topological information and semantic information: topological information alignment is to compare the V map and V SLAM The degree of (T) and the structure of a certain level of subtrees expanded with itself as the root node are used as the first level of alignment judgment based on the difference in degree and the similarity of the subtree structure (detected using the WL-Test method); semantic information alignment is based on G map In the case of containing semantic information, the matching V SLAM (T) Determine whether it is possible to have the semantic information, such as V with intersection semantic labels map More likely to match a V with a larger curvature SLAM (T). In addition, the alignment nodes before time T already exist, so the module at time T focuses on processing the newly added D SLAM (T), and then combined with the previous matching point set and output. This module is a real-time module and runs at the same frequency as the SLAM positioning data processing module.

[0030] The SLAM positioning graph optimization module constructs a graph optimization system G based on the graph node data to be aligned. match (T) = {V match (T), E match (T)} optimizes and adjusts the original SLAM positioning data. match (T) = V SLAM (T)∪V match map (T) and maintain The node attributes in the set are fixed, which is used as one of the optimization benchmarks; match (T)=Relation(V match map (T), V match SLAM (T))∪E SLAM(T), the output of the previous alignment module and the edges of the SLAM topology map are used as edges of the graph optimization to participate in the optimization process. The optimization process will continue to iterate until the maximum number of iterations I is reached max The optimization error is less than the error threshold Errorth. Finally, a new set of SLAM topology positioning information graphs G′ is output. SLAM (T) = {V′ SLAM (T), E′ SLAM (T)}, and then through the inverse process of the logic in the SLAM positioning data processing module, the topological positioning information map is converted into new SLAM positioning data and added to the subsequent SLAM positioning algorithm.

[0031] The SLAM positioning data processing module, data alignment module, and SLAM positioning map optimization module are all real-time operation modules that together with the SLAM positioning algorithm constitute the real-time technical part of the present invention, and the operating frequency of the real-time part is consistent or in multiple relationship.

[0032] This embodiment relates to a global map alignment method for SLAM technology for the above system, including:

[0033] Step 1: Process the prior OSM map and SLAM real-time positioning data to obtain the corresponding topological map;

[0034] Step 2: Perform node matching on the topology graph obtained in step 1;

[0035] Step 3: During the real-time operation of the SLAM algorithm, the node information in the SLAM topology map is optimized synchronously through graph optimization, the SLAM positioning information is updated and then transmitted back to the SLAM algorithm for the next step of real-time positioning.

[0036] Compared with the existing technology, this method can utilize the prior information of OSM-type road network maps to assist the SLAM positioning algorithm in real time and stabilize the positioning under various working conditions.

[0037] The above-mentioned specific implementation can be partially adjusted in different ways by those skilled in the art without departing from the principle and purpose of the present invention. The protection scope of the present invention shall be based on the claims and shall not be limited by the above-mentioned specific implementation. Each implementation scheme within its scope shall be subject to the constraints of the present invention.

Claims

1. A SLAM real-time positioning optimization system based on OSM-like maps, characterized in that: It includes: a priori OSM map preprocessing module, a SLAM positioning data processing module, a data alignment module and a SLAM positioning graph optimization module, wherein: the priori OSM map preprocessing module extracts a priori topological map with semantic nodes meaningful to driving from the original map data in the offline stage; the SLAM positioning data processing module generates a SLAM topological positioning information graph with key points after sparse processing and key point extraction based on the SLAM algorithm results; the data alignment module performs alignment processing based on the priori topological map and the SLAM topological positioning information graph, and filters out topological node pairs that meet the graph optimization conditions; the graph optimization module constructs a graph optimization system based on the aligned map node data, which is used to optimize and adjust the original SLAM positioning data, obtain a SLAM topological positioning information graph that reduces SLAM positioning drift, and obtain updated SLAM positioning data after inverse processing by the SLAM positioning data processing module to achieve optimized positioning; The prior topological map G of the semantic nodes map = {V map , E map }, V map and E map Represent the nodes and edges in the topological map respectively; The SLAM topology positioning information graph G SLAM (T) = {V SLAM (T), E SLAM (T)}, V SLAM (r) and E SLAM (T) represents the nodes and edges in the SLAM topology positioning information graph at the current time T; The original map data refers to: a priori OSM-type road network map, that is, map data in a format similar to OpenStreetMap, including: recording the latitude and longitude coordinates of a point in the map and semantic information Nodes, marked as Contains road or contour information Ways composed of Nodes, marked as And store possible related information Relations between each information; The topological node pair (V match map (T), V match SLAM (T)) match map (T) and V match SLAM (T) The number of elements in the two node sets is the same and they have a one-to-one matching relationship; The map node data refers to: topological node pairs (V match map (T), V match SLAM (T)); The original SLAM positioning data refers to: the SLAM topology information map G constructed initially SLAM (T) = {V SLAM (T), E SLAM (T)}.

2. The SLAM real-time positioning optimization system based on OSM-like maps according to claim 1 is characterized in that: The data alignment module and the SLAM positioning data processing module operate at the same frequency.

3. The SLAM real-time positioning optimization system based on OSM-like maps according to claim 1 is characterized in that: The extraction refers to: the prior OSM map processing module analyzes the latitude and longitude coordinates and semantic information of the map midpoint in the original map data and road or contour (such as walls, etc.) information According to TAGS N seg and TAGS W seg Extract vehicle-accessible roads After that, extract the As V map , retaining semantic information and building on the relationship information Relations Topological relationship between each other map , forming a topological semantic map G map = {V map , E map }.

4. The SLAM real-time positioning optimization system based on OSM-like maps according to claim 1 is characterized in that: The sparse processing means that the density of the obtained SLAM positioning trajectory is related to the frequency of the SLAM sensor. Such dense data is generally not needed during map post-processing, especially in places where there is little information (such as straight lines). Therefore, fewer key points will be extracted from places where there is no curvature change in the obtained SLAM positioning data to achieve the effect of sparse processing and reduce the amount of data.

5. The SLAM real-time positioning optimization system based on OSM-like maps according to claim 1 is characterized in that: The key point extraction is: based on the SLAM positioning information, a fixed time length D is selected as a sliding window, the direction of the first positioning point X0 within the D time is taken as the positive direction, the positioning points within the D time are fitted as a curve, and the extreme points of the curve are marked as key points V SLAM (T), i.e. the inflection point, and sparsely select data according to the curvature to reduce the computational burden of subsequent optimization.

6. The SLAM real-time positioning optimization system based on OSM-like maps according to claim 1 is characterized in that: The alignment process includes: topological information alignment and semantic information alignment; Topology information alignment means: comparing the V map and V SLAM The degree of (T) and the structure of a certain level of subtrees expanded with itself as the root node are used as the first level of alignment judgment based on the difference in degree and the similarity of the subtree structure (detected using the WL-Test method); Semantic information alignment means: map In the case of containing semantic information, the matching V SLAM (T) Determine whether it is possible to have the semantic information, such as V with intersection semantic labels map More likely to match a V with a larger curvature SLAM (T), in addition, the alignment nodes before time T already exist, so the module at time T focuses on processing the newly added D SLAM (T), and then output it together with the previous matching point set.

7. The SLAM real-time positioning optimization system based on OSM-like maps according to claim 1 is characterized in that: The optimization adjustment mentioned above refers to: match (T) = {V match (T), E match (T)}, V match (T) = V SLAM (T)∪V match map (T), and maintain V match map (T) The node attributes in the set are fixed, which is used as one of the optimization benchmarks; E match (T)=Relation(V match map (T), V match SLAM (T))∪E SLAM (T), the topological node pairs output by the data alignment module and the edges in the SLAM topological positioning information graph are used as the input of the graph optimization module again, and iterative optimization is performed until the maximum number of iterations I is reached max Or the optimization error is less than the error threshold Error th So far, the optimized SLAM topology positioning information graph is obtained.

8. The SLAM real-time positioning optimization system based on OSM-like maps according to claim 1 is characterized in that: The optimization error is: to meet E match (T) is the value of the loss function as the optimization objective. If there is no error, it means that there is a set of solutions that can perfectly satisfy E. match (T), but it is generally not possible to achieve this. The optimization error is the value of the loss function at the current number of iterations.

9. The SLAM real-time positioning optimization system based on OSM-like maps according to claim 1 is characterized in that: The inverse processing refers to: restoring the optimized SLAM topology information map to the original SLAM positioning trajectory format.

10. A global map alignment method for SLAM technology for the system according to any one of claims 1 to 9, characterized in that: include: Step 1: Process the prior OSM map and SLAM real-time positioning data to obtain the corresponding topological map; Step 2: Perform node matching on the topology graph obtained in step 1; Step 3: During the real-time operation of the SLAM algorithm, the node information in the SLAM topology map is optimized synchronously through graph optimization, the SLAM positioning information is updated and then transmitted back to the SLAM algorithm for the next step of real-time positioning.

Images