CN116026332A - Laser SLAM topological map constraint enhancement and map optimization method - Google Patents

Abstract

The invention provides a constraint enhancement and map optimization method for a laser SLAM topological map, which comprises the following steps: s1, on the basis of an existing SLAM topological map, additional pose constraint is further established among topological map nodes; the way to establish pose constraints includes two types: mode a: in a structured environment of a wall body or an upright post with a collinear relationship, pose constraint is established between related topological map nodes by utilizing the collinear characteristic of an environment contour; mode b: manually arranging markers in the environment, measuring the positions of the markers in advance, and establishing pose constraints among related topological map nodes; s2, supplementing the pose constraint newly generated in the S1 into an objective function of the existing SLAM topological map optimization, and solving the optimization problem again to obtain a mapping result. According to the method, constraints are added between sub maps established by SLAM according to the same orientation and collineation characteristics of the pillars and the walls in the environment, and the constraints are brought into the optimization problem during map optimization, so that the deformation error of SLAM map establishment is reduced.

Description

Laser SLAM topological map constraint enhancement and map optimization method

[ field of technology ]

The invention relates to the technical field of mobile robot navigation, in particular to a method for enhancing the constraint of a laser SLAM topological map and optimizing the map.

[ background Art ]

In the widely used scheme of laser synchronous positioning and mapping (i.e. SLAM, simultaneous Localization and Mapping) based on the graph optimization method, the graph optimization is needed to be performed based on the topological map so as to reduce the accumulated errors in the graph construction process. The nodes in the topological map represent sub-maps (also called local maps, which contain the laser point cloud contour information of the local environment), and the global pose of each node is the optimized state. If there is an edge between two nodes in the topological map, it is indicated that there is a constraint between the pose of the two nodes. The most basic constraint form is that when the sub-maps corresponding to the two nodes are overlapped sufficiently, the constraint of the relative pose between the two nodes is obtained through a sub-map matching algorithm.

The quality of the mapping results after the map optimization depends largely on the number of edges in the topological map and the distribution characteristics. In the standard laser SLAM method at present, although edges in a topological map can be of two types, namely a connecting edge and a loop closing edge in sequence, the two types are obtained by adopting a sub-map matching algorithm. Therefore, edges in the topological map can only be established between two nodes with sufficiently close actual positions, and pose constraints cannot be established between nodes with far distances. The topology map thus obtained is basically in a simple ring topology, as shown in fig. 1.

In many cases, the error of drawing obtained by the laser SLAM adopting the standard drawing optimization method is more obvious, and even affects the use of drawing results. The visual effect of this error is that the map has some distortion. For example, the walls and columns in a real environment are in a collinear relationship, but in the construction result, it may occur that the outlines of the walls and columns deviate significantly from a straight line, as shown in fig. 2. Part of this problem is that in performing map optimization, the pose between sub-maps is not effectively constrained without consideration of such extensive contour co-linear features in the environment, resulting in an optimization result that deviates from the true solution.

[ invention ]

The invention aims to solve the problems in the prior art, and provides a constraint enhancement and graph optimization method for a laser SLAM topological map, which can increase constraint among sub maps established by SLAM according to the same orientation and collineation characteristics of columns and walls in the environment, and bring the constraint into the optimization problem during graph optimization, so as to reduce the deformation error of the SLAM graph establishment.

In order to achieve the above purpose, the present invention provides a method for enhancing constraint and optimizing map of laser SLAM topology map, comprising the following steps:

s1, on the basis of an existing SLAM topological map, additional pose constraint is further established among topological map nodes; the method for establishing the pose constraint comprises the following two modes:

mode a: in a structured environment of a wall body or an upright post with a collinear relationship, pose constraint is established between related topological map nodes by utilizing the collinear characteristic of an environment contour;

mode b: manually arranging markers in the environment, measuring the positions of the markers in advance, and establishing pose constraints among related topological map nodes;

s2, supplementing the pose constraint newly generated in the step S1 into an objective function of the existing SLAM topological map optimization, and then solving the optimization problem again to obtain a mapping result.

Preferably, in step S1, the existing SLAM topology map is built by using a standard laser SLAM method, and the map optimization is completed.

Preferably, the mode a specifically includes the following steps:

a1. adopting an algorithm automatic identification or manual operation method to determine a sub map of the wall body or upright column outline with the collinear relation existing in the existing SLAM topological map;

a2. and establishing pose constraint between the nodes of the related topological map according to the collinear relation of the outlines of the walls or the upright posts in the sub map.

Preferably, in the step a1, the specific method for determining the sub map of the wall or column outline with the collinear relationship is as follows:

a11. the method for automatically identifying by adopting the algorithm comprises the following steps: in each sub map, a straight line contour and a column contour with a rectangular section are identified by adopting a straight line feature extraction algorithm; for the straight line profile, each section of straight line profile adopts straight line orientation as a description parameter; for the column profiles, if the profiles of a plurality of columns can be extracted, straight lines fitted by collinear edges in the column profiles are used as description parameters; if the straight line outline is identified in both sub-maps and the difference of the corresponding straight line orientations in the global coordinate system is smaller than a pre-specified threshold value, the straight line orientations are the same as a pose constraint between the two sub-maps; if the straight line fitted by the column outline is identified in the two sub-maps and the angle deviation of the straight line in the global coordinate system is smaller than a pre-specified threshold value, taking the straight line colinear as a pose constraint between the two sub-maps;

a12. the method adopts manual operation: firstly, linear contour features and upright post contour features in all sub-maps which are automatically identified by an algorithm are displayed on a graphical interface, then, the characteristics which are identical in orientation or linear collineation are manually marked through mouse operation, and pose constraint among the sub-maps is established.

Preferably, the mode b specifically includes the following steps:

b11. arranging a certain number of light reflecting modules in the environment, so that the light reflecting modules can be detected in the range of a plurality of sub-maps in the sub-map established by SLAM;

b12. measuring the relative position of the reflecting module by adopting a total station;

b13. and calculating the pose constraint relation between SLAM sub-maps by utilizing the relative position information of the reflection modules provided by the total station.

Preferably, the mode b specifically includes the following steps:

b21. paving a two-dimensional code on the ground in the environment, and measuring the relative pose of the two-dimensional code by adopting a measuring device;

b22. when the laser SLAM is carried out, the laser radar is fixedly connected with the two-dimensional code detection camera, radar detection can be carried out in the area near the two-dimensional code, and the relative pose of the laser radar relative to the two-dimensional code can be determined;

b23. and converting the known two-dimensional code coordinates to obtain the relative pose between the laser point cloud sub-maps to form a pose constraint relation between SLAM sub-maps.

The invention has the beneficial effects that: compared with the current standard laser SLAM scheme, the method can add a large amount of constraints to the map optimization topological map in the structured environment of the wall body or the upright post with the collinear relation. And these constraints are significantly different from conventional ones in that they are two points: 1. conventional constraints can only be established between nodes which need to be close enough in the sub-map range and have a certain coincidence; 2. by utilizing the contour collineation characteristic, constraints can be established between nodes farther apart, as long as the sub-maps of the nodes contain collinear relationship walls or columns.

On the other hand, the invention not only can increase the number of optimization constraints of the graph, but also can obviously improve the structure of the topological map, and can improve the conventional ring topological structure into a net topological structure, as shown in fig. 3, the invention is beneficial to the graph optimization algorithm to exert better effect and reduce the distortion of the graph construction result.

The features and advantages of the present invention will be described in detail by way of example with reference to the accompanying drawings.

[ description of the drawings ]

FIG. 1 is a topological map obtained using a standard laser SLAM method;

FIG. 2 is a topological map obtained by laser SLAM using a standard map optimization method;

FIG. 3 is a topological map obtained by using a laser SLAM topological map constraint enhancement and map optimization method of the invention.

[ detailed description ] of the invention

Example 1

The embodiment can be suitable for indoor environments such as factory buildings, warehouses and the like, and indoor environments with walls or upright posts widely in collinear relation. When SLAM is used in the environment, the collineation characteristic of the environment outline can be utilized to increase the constraint between sub-maps, and the mapping effect is improved. The method specifically comprises the following steps:

s1, on the basis of an existing SLAM topological map, additional pose constraint is further established among topological map nodes; the existing SLAM topological map is established by adopting a standard laser SLAM method, and the map optimization topological map is completed, and pose constraint is established among relevant topological map nodes by utilizing the collinear characteristic of an environment contour in a structured environment of a wall body or an upright post with a collinear relation;

a1. adopting an algorithm automatic identification or manual operation method to determine a sub map of the wall body or upright column outline with the collinear relation existing in the existing SLAM topological map;

the specific method for determining the sub map with the contour of the wall body or the upright post with the collinear relation comprises the following steps:

a11. the method for automatically identifying by adopting the algorithm comprises the following steps: in each sub map, a straight line feature extraction algorithm (such as an iterative endpoint fitting method IEPF) is adopted to identify straight line contours and upright post contours with rectangular cross sections; for the straight line profile, each section of straight line profile adopts straight line orientation as a description parameter; for the column profiles, if the profiles of a plurality of columns can be extracted, straight lines fitted by collinear edges in the column profiles are used as description parameters; if the straight line outline is identified in both sub-maps and the corresponding straight line orientation is less than a pre-specified threshold (e.g., 5 degrees) in the global coordinate system, the straight line orientation is the same as a pose constraint between the two sub-maps; if the straight line fitted by the column outline is identified in the two sub-maps and the angle deviation of the straight line in the global coordinate system is smaller than a pre-specified threshold (such as 5 degrees), taking the straight line colinear as a pose constraint between the two sub-maps;

a12. the method adopts manual operation: firstly, linear contour features and upright post contour features in all sub-maps which are automatically identified by an algorithm are displayed on a graphical interface, then, the characteristics which are identical in orientation or linear collineation are manually marked through mouse operation, and pose constraint among the sub-maps is established.

a2. Establishing pose constraint between related topological map nodes according to collinear relations of wall body or column outlines in the sub map; thus adding a large number of edges between nodes farther apart in the topological map.

S2, supplementing the pose constraint newly generated in the step S1 into an objective function of the existing SLAM topological map optimization, and then solving the optimization problem again to obtain a mapping result.

In order to establish additional pose constraints between sub-maps, markers may also be artificially added to the environment and their positions measured in advance using some other sensor. See in particular example 2, example 3 below.

Example 2

The invention discloses a constraint enhancement and map optimization method for a laser SLAM topological map, which comprises the following steps of:

s1, establishing pose constraint on the basis of an existing SLAM topological map; the existing SLAM topological map is established by adopting a standard laser SLAM method, and is subjected to map optimization, markers are manually arranged in the environment, the positions of the markers are measured in advance, and pose constraints are established among related topological map nodes; the method specifically comprises the following steps:

b11. arranging a certain number of light reflecting modules (such as light reflecting plates, light reflecting columns and the like) in the environment, so that the light reflecting modules can be detected in the range of a plurality of sub-maps in the sub-map established by the SLAM;

b12. measuring the relative position of the reflecting module by adopting a total station;

b13. and calculating the pose constraint relation between SLAM sub-maps by utilizing the relative position information of the reflection modules provided by the total station.

S2, supplementing the pose constraint newly generated in the step S1 into an objective function of the existing SLAM topological map optimization, and then solving the optimization problem again to obtain a mapping result.

Example 3

The invention discloses a constraint enhancement and map optimization method for a laser SLAM topological map, which comprises the following steps of:

s1, establishing pose constraint on the basis of an existing SLAM topological map; the existing SLAM topological map is established by adopting a standard laser SLAM method, and is subjected to map optimization, markers are manually arranged in the environment, the positions of the markers are measured in advance, and pose constraints are established among related topological map nodes; the method specifically comprises the following steps:

b21. paving a two-dimensional code on the ground in the environment, and measuring the relative pose of the two-dimensional code by adopting a measuring device;

b22. when the laser SLAM is carried out, the laser radar is fixedly connected with the two-dimensional code detection camera, radar detection can be carried out in the area near the two-dimensional code, and the relative pose of the laser radar relative to the two-dimensional code can be determined;

b23. and converting the known two-dimensional code coordinates to obtain the relative pose between the laser point cloud sub-maps to form a pose constraint relation between SLAM sub-maps.

S2, supplementing the pose constraint newly generated in the step S1 into an objective function of the existing SLAM topological map optimization, and then solving the optimization problem again to obtain a mapping result.

The above embodiments are illustrative of the present invention, and not limiting, and any simple modifications of the present invention fall within the scope of the present invention.

Claims (6)

1. A constraint enhancement and graph optimization method for a laser SLAM topological map is characterized by comprising the following steps of: the method comprises the following steps:

s1, on the basis of an existing SLAM topological map, additional pose constraint is further established among topological map nodes; the method for establishing the pose constraint comprises the following two modes:

mode a: in a structured environment of a wall body or an upright post with a collinear relationship, pose constraint is established between related topological map nodes by utilizing the collinear characteristic of an environment contour;

mode b: manually arranging markers in the environment, measuring the positions of the markers in advance, and establishing pose constraints among related topological map nodes;

s2, supplementing the pose constraint newly generated in the step S1 into an objective function of the existing SLAM topological map optimization, and then solving the optimization problem again to obtain a mapping result.

2. The laser SLAM topology map constraint enhancement and map optimization method of claim 1, wherein: in step S1, the existing SLAM topology map is built by using a standard laser SLAM method, and the map optimization is completed.

3. The laser SLAM topology map constraint enhancement and map optimization method of claim 1 or 2, wherein: the mode a specifically comprises the following steps:

a1. adopting an algorithm automatic identification or manual operation method to determine a sub map of the wall body or upright column outline with the collinear relation existing in the existing SLAM topological map;

a2. and establishing pose constraint between the nodes of the related topological map according to the collinear relation of the outlines of the walls or the upright posts in the sub map.

4. The laser SLAM topology map constraint enhancement and map optimization method of claim 3, wherein: in the step a1, the specific method for determining the sub map of the wall body or the column outline with the collinear relation comprises the following steps:

a11. the method for automatically identifying by adopting the algorithm comprises the following steps: in each sub map, a straight line contour and a column contour with a rectangular section are identified by adopting a straight line feature extraction algorithm; for the straight line profile, each section of straight line profile adopts straight line orientation as a description parameter; for the column profiles, if the profiles of a plurality of columns can be extracted, straight lines fitted by collinear edges in the column profiles are used as description parameters; if the straight line outline is identified in both sub-maps and the difference of the corresponding straight line orientations in the global coordinate system is smaller than a pre-specified threshold value, the straight line orientations are the same as a pose constraint between the two sub-maps; if the straight line fitted by the column outline is identified in the two sub-maps and the angle deviation of the straight line in the global coordinate system is smaller than a pre-specified threshold value, taking the straight line colinear as a pose constraint between the two sub-maps;

a12. the method adopts manual operation: firstly, linear contour features and upright post contour features in all sub-maps which are automatically identified by an algorithm are displayed on a graphical interface, then, the characteristics which are identical in orientation or linear collineation are manually marked through mouse operation, and pose constraint among the sub-maps is established.

5. The laser SLAM topology map constraint enhancement and map optimization method of claim 1 or 2, wherein: the mode b specifically comprises the following steps:

b11. arranging a certain number of light reflecting modules in the environment, so that the light reflecting modules can be detected in the range of a plurality of sub-maps in the sub-map established by SLAM;

b12. measuring the relative position of the reflecting module by adopting a total station;

b13. and calculating the pose constraint relation between SLAM sub-maps by utilizing the relative position information of the reflection modules provided by the total station.

6. The laser SLAM topology map constraint enhancement and map optimization method of claim 1 or 2, wherein: the mode b specifically comprises the following steps:

b21. paving a two-dimensional code on the ground in the environment, and measuring the relative pose of the two-dimensional code by adopting a measuring device;

b22. when the laser SLAM is carried out, the laser radar is fixedly connected with the two-dimensional code detection camera, radar detection can be carried out in the area near the two-dimensional code, and the relative pose of the laser radar relative to the two-dimensional code can be determined;

b23. and converting the known two-dimensional code coordinates to obtain the relative pose between the laser point cloud sub-maps to form a pose constraint relation between SLAM sub-maps.

Images