A laser SLAM topology map constraint enhancement and graph optimization method

Abstract

The present invention proposes a laser SLAM topology map constraint enhancement and graph optimization method, comprising the following steps: S1. Based on the existing SLAM topology map, additional pose constraints are further established between topology map nodes. There are two ways to establish pose constraints: Method a: In a structured environment with collinear walls or columns, pose constraints are established between related topology map nodes by utilizing the collinearity of the environment's contours; Method b: Markers are manually placed in the environment, and their positions are measured in advance to establish pose constraints between related topology map nodes. S2. The pose constraints generated in S1 are added to the objective function of the existing SLAM topology map optimization, and the optimization problem is re-solved to obtain the mapping result. This method adds constraints between submaps created by SLAM based on the same orientation and collinearity of columns and walls in the environment, and incorporates the constraints into the optimization problem during graph optimization, thereby reducing SLAM mapping deformation errors.

Description

Technical field

[0001] The present invention relates to the technical field of mobile robot navigation, in particular to a laser SLAM topology map constraint enhancement and graph optimization method. [Background Technology]

[0002] In the currently widely used laser simultaneous positioning and mapping (SLAM) scheme based on graph optimization methods, it is necessary to perform graph optimization based on the topological map to reduce the accumulated errors in the mapping process. The nodes in the topological map represent submaps (also called local maps, which contain 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 means that there is a constraint between the poses of the two nodes. The most basic form of constraint is that when the submaps corresponding to the two nodes have sufficient overlap, the constraint on the relative pose between the two nodes is obtained through the submap matching algorithm.

[0003] The quality of the map construction result after graph optimization depends largely on the number and distribution characteristics of the edges in the topological map. In the current standard laser SLAM method, although the edges in the topological map can be divided into two types: sequential connection edges and loop closure edges, both are obtained using the sub-map matching algorithm. Therefore, the edges in the topological map can only be established between two nodes whose actual positions are close enough, and it is impossible to establish posture constraints between nodes that are far away. The topological map obtained in this way basically presents a simple ring topology, such as Figure 1 shown.

[0004] In many cases, the error in mapping obtained by laser SLAM using standard map optimization methods will be more obvious, and even affect the usability of the mapping results. The intuitive effect of this error is that the map has a certain degree of distortion. For example, in the real environment, the walls and pillars are collinear, but in the mapping results, the outlines of the walls and pillars may deviate significantly from a straight line, such as Figure 2 Part of the reason for this problem is that when performing graph optimization, the large-scale collinearity of the environment is not taken into account, and the poses between submaps lack effective constraints, causing the optimization results to deviate from the true solution. [Summary of the invention]

[0005] The purpose of the present invention is to solve the problems in the prior art and propose a laser SLAM topological map constraint enhancement and graph optimization method. According to the same orientation and collinearity characteristics of pillars and walls in the environment, constraints can be added between sub-maps established by SLAM, and constraints can be incorporated into the optimization problem during graph optimization to reduce SLAM mapping deformation errors.

[0006] To achieve the above objectives, the present invention proposes a laser SLAM topology map constraint enhancement and graph optimization method, comprising the following steps:

[0007] S1. Based on the existing SLAM topology map, additional pose constraints are further established between the topology map nodes. The pose constraints can be established in the following two ways:

[0008] Method a: In a structured environment with collinear walls or columns, the collinearity of the environment contours is used to establish pose constraints between related topological map nodes.

[0009] Method b: Manually place markers in the environment, measure their positions in advance, and establish pose constraints between related topological map nodes;

[0010] S2. Add the newly generated pose constraints in step S1 to the objective function of the existing SLAM topology map optimization, and then re-solve the optimization problem to obtain the mapping result.

[0011] Preferably, in step S1, the existing SLAM topology map is a topology map established by using a standard laser SLAM method and having completed graph optimization.

[0012] Preferably, method a specifically comprises the following steps:

[0013] a1. Use an algorithm to automatically identify or manually operate the method to determine the sub-map of the wall or column outline with a collinear relationship in the existing SLAM topology map;

[0014] a2. Based on the collinear relationship of the wall or column outlines in the sub-map, establish pose constraints between the relevant topological map nodes.

[0015] Preferably, in step a1, the specific method for determining the sub-maps of the wall or column outlines having a collinear relationship is:

[0016] a11. Automatic recognition using an algorithm: In each submap, a line feature extraction algorithm is used to identify straight line contours and rectangular column contours. For straight line contours, each segment of the straight line contour uses the line orientation as its description parameter. For column contours, if multiple column contours can be extracted, the straight lines fitted from the collinear edges of the column contours are used as description parameters. If straight line contours are identified in both submaps and the difference in their corresponding line orientations in the global coordinate system is less than a pre-specified threshold, the same straight line orientation is used as a pose constraint between the two submaps. If straight lines fitting column contours are identified in both submaps and the angular deviation of the straight lines in the global coordinate system is less than a pre-specified threshold, the collinearity of the lines is used as a pose constraint between the two submaps.

[0017] a12. Manual operation method: First, display the straight line contour features and column contour features in each sub-map automatically identified by the algorithm on the graphical interface. Then, manually mark these features with the same orientation or collinearity through mouse operation to establish posture constraints between the sub-maps.

[0018] Preferably, method b specifically includes the following steps:

[0019] b11. Arrange a certain number of reflective modules in the environment so that the reflective modules can be detected within the range of multiple sub-maps in the sub-map established by SLAM;

[0020] b12. Use a total station to measure the relative position of the reflective modules;

[0021] b13. Use the relative position information of the reflective modules provided by the total station to calculate the pose constraints between the SLAM submaps.

[0022] Preferably, method b specifically includes the following steps:

[0023] b21. Lay a QR code on the ground in the environment and use a measuring device to measure the relative position of the QR code;

[0024] b22. When performing laser SLAM, the laser radar is connected to the QR code detection camera. When the radar is measured in the area near the QR code, the relative position of the laser radar with respect to the QR code is determined;

[0025] b23. Using the known QR code coordinates, the relative poses between the laser point cloud submaps are converted and the pose constraints between the SLAM submaps are formed.

[0026] The beneficial effects of the present invention: Compared with current standard laser SLAM solutions, the present invention can add a large number of constraints to the graph-optimized topological map in structured environments with collinear walls or columns. These constraints are significantly different from conventional constraints in two ways: 1. Conventional constraints can only be established between nodes whose submaps are close enough and have a certain degree of overlap; 2. By leveraging the collinearity of contours, constraints can be established between nodes that are farther apart, as long as their submaps contain collinear walls or columns.

[0027] On the other hand, the present invention can not only increase the number of graph optimization constraints, but also significantly improve the structure of the topological map, which can improve the conventional ring topology to a mesh topology, such as Figure 3 As shown, the present invention is conducive to the graph optimization algorithm to achieve better results and reduce the distortion of the mapping results.

[0028] The features and advantages of the present invention will be described in detail through embodiments with reference to the accompanying drawings.

Brief Description of the Drawings

[0029] Figure 1 It is a topological map obtained using the standard laser SLAM method;

[0030] Figure 2 It is a topological map obtained by laser SLAM using the standard graph optimization method;

[0031] Figure 3 The invention discloses a topological map obtained by adopting a laser SLAM topological map constraint enhancement and graph optimization method. [Specific implementation method]

[0032] Example 1

[0033] This embodiment is applicable to indoor environments, such as factories and warehouses, where collinear walls or columns are common. When using SLAM in such environments, the collinearity of the environment's contours can be exploited to add constraints between submaps and improve mapping results. Specifically, the following steps are included:

[0034] S1. On the basis of the existing SLAM topological map, additional pose constraints are further established between the topological map nodes; wherein the existing SLAM topological map is established using a standard laser SLAM method and a topological map optimized by graph optimization. In a structured environment with collinear walls or columns, pose constraints are established between the relevant topological map nodes by utilizing the collinear characteristics of the environmental contours;

[0035] a1. Use an algorithm to automatically identify or manually operate the method to determine the sub-map of the wall or column outline with a collinear relationship in the existing SLAM topology map;

[0036] The specific method for determining the submap of the wall or column outline with a collinear relationship is as follows:

[0037] a11. Automatic recognition using an algorithm: In each submap, a line feature extraction algorithm (such as the iterative endpoint fitting method (IEPF)) is used to identify straight line contours and rectangular column contours. For straight line contours, each segment of the straight line contour uses the line orientation as its description parameter. For column contours, if multiple column contours can be extracted, the straight lines fitted from the collinear edges of the column contours are used as description parameters. If straight line contours are identified in both submaps and the difference in their corresponding line orientations in the global coordinate system is less than a pre-specified threshold (such as 5 degrees), the same straight line orientation is used as a pose constraint between the two submaps. If straight lines fitting column contours are identified in both submaps and the angular deviation of the straight lines in the global coordinate system is less than a pre-specified threshold (such as 5 degrees), the collinearity of the straight lines is used as a pose constraint between the two submaps.

[0038] a12. Manual operation method: First, display the straight line contour features and column contour features in each sub-map automatically identified by the algorithm on the graphical interface. Then, manually mark these features with the same orientation or collinearity through mouse operation to establish posture constraints between the sub-maps.

[0039] a2. Based on the collinear relationship of the wall or column outlines in the sub-map, establish pose constraints between the relevant topological map nodes; thereby adding a large number of edges between nodes that are far apart in the topological map.

[0040] S2. Add the newly generated pose constraints in step S1 to the objective function of the existing SLAM topology map optimization, and then re-solve the optimization problem to obtain the mapping result.

[0041] To establish additional pose constraints between submaps, you can also artificially add markers to the environment and use other sensors to measure the positions of the markers in advance. For details, see the following Examples 2 and 3.

[0042] Example 2

[0043] A laser SLAM topology map constraint enhancement and graph optimization method of the present invention comprises the following steps:

[0044] S1. Establishing pose constraints based on an existing SLAM topology map; wherein the existing SLAM topology map is established using a standard laser SLAM method and a graph-optimized topology map, manually placing markers in the environment, and measuring the positions of the markers in advance, and establishing pose constraints between related topology map nodes; specifically comprising the following steps:

[0045] b11. Arrange a certain number of reflective modules (such as reflective plates, reflective columns, etc.) in the environment so that the reflective modules can be detected within the range of multiple sub-maps in the sub-map established by SLAM;

[0046] b12. Use a total station to measure the relative position of the reflective modules;

[0047] b13. Use the relative position information of the reflective modules provided by the total station to calculate the pose constraints between the SLAM submaps.

[0048] S2. Add the newly generated pose constraints in step S1 to the objective function of the existing SLAM topology map optimization, and then re-solve the optimization problem to obtain the mapping result.

[0049] Example 3

[0050] A laser SLAM topology map constraint enhancement and graph optimization method of the present invention comprises the following steps:

[0051] S1. Establishing pose constraints based on an existing SLAM topology map; wherein the existing SLAM topology map is established using a standard laser SLAM method and a graph-optimized topology map, manually placing markers in the environment, and measuring the positions of the markers in advance, and establishing pose constraints between related topology map nodes; specifically comprising the following steps:

[0052] b21. Lay a QR code on the ground in the environment and use a measuring device to measure the relative position of the QR code;

[0053] b22. When performing laser SLAM, the laser radar is connected to the QR code detection camera. When the radar is measured in the area near the QR code, the relative position of the laser radar with respect to the QR code is determined;

[0054] b23. Using the known QR code coordinates, the relative poses between the laser point cloud submaps are converted and the pose constraints between the SLAM submaps are formed.

[0055] S2. Add the newly generated pose constraints in step S1 to the objective function of the existing SLAM topology map optimization, and then re-solve the optimization problem to obtain the mapping result.

[0056] The above embodiments are intended to illustrate the present invention, not to limit the present invention. Any solution that is a simple transformation of the present invention falls within the protection scope of the present invention.

Claims

1. A laser SLAM topology map constraint enhancement and graph optimization method, characterized by: The following steps are involved: S1. Based on the existing SLAM topology map, additional pose constraints are further established between the topology map nodes. The pose constraints can be established in the following two ways: Method a: In a structured environment with collinear walls or columns, the collinearity of the environment contours is used to establish pose constraints between related topological map nodes. Method b: Manually place markers in the environment, measure their positions in advance, and establish pose constraints between related topological map nodes; S2. Add the newly generated pose constraints in step S1 to the objective function of the existing SLAM topology map optimization, and then re-solve the optimization problem to obtain the mapping result.

2. A laser SLAM topology map constraint enhancement and graph optimization method as claimed in claim 1, characterized in that: In step S1, the existing SLAM topology map is a topology map established by using a standard laser SLAM method and completing graph optimization.

3. A laser SLAM topology map constraint enhancement and graph optimization method as described in claim 1 or 2, characterized in that: Method a specifically includes the following steps: a1. Use an algorithm to automatically identify or manually operate the method to determine the sub-map of the wall or column outline with a collinear relationship in the existing SLAM topology map; a2. Based on the collinear relationship of the wall or column outlines in the sub-map, establish pose constraints between the relevant topological map nodes.

4. A laser SLAM topology map constraint enhancement and graph optimization method as described in claim 3, characterized in that: In step a1, the specific method for determining the sub-maps of the wall or column outlines having a collinear relationship is: a11. Automatic recognition using an algorithm: In each submap, a line feature extraction algorithm is used to identify straight line contours and rectangular column contours. For straight line contours, each segment of the straight line contour uses the line orientation as its description parameter. For column contours, if multiple column contours can be extracted, the straight lines fitted from the collinear edges of the column contours are used as description parameters. If straight line contours are identified in both submaps and the difference in their corresponding line orientations in the global coordinate system is less than a pre-specified threshold, the same straight line orientation is used as a pose constraint between the two submaps. If straight lines fitting column contours are identified in both submaps and the angular deviation of the straight lines in the global coordinate system is less than a pre-specified threshold, the collinearity of the lines is used as a pose constraint between the two submaps. a12. Manual operation method: First, display the straight line contour features and column contour features in each sub-map automatically identified by the algorithm on the graphical interface. Then, manually mark these features with the same orientation or collinearity through mouse operation to establish posture constraints between the sub-maps.

5. A laser SLAM topology map constraint enhancement and graph optimization method as described in claim 1 or 2, characterized in that: Method b specifically includes the following steps: b11. Arrange a certain number of reflective modules in the environment so that the reflective modules can be detected within the range of multiple sub-maps in the sub-map established by SLAM; b12. Use a total station to measure the relative position of the reflective modules; b13. Use the relative position information of the reflective modules provided by the total station to calculate the pose constraints between the SLAM submaps.

6. A laser SLAM topology map constraint enhancement and graph optimization method as described in claim 1 or 2, characterized in that: Method b specifically includes the following steps: b21. Lay a QR code on the ground in the environment and use a measuring device to measure the relative position of the QR code; b22. When performing laser SLAM, the laser radar is connected to the QR code detection camera. When the radar is measured in the area near the QR code, the relative position of the laser radar with respect to the QR code is determined; b23. Using the known QR code coordinates, the relative poses between the laser point cloud submaps are converted and the pose constraints between the SLAM submaps are formed.

Images