A simultaneous localization and mapping method in unstructured dynamic environments

Abstract

A simultaneous positioning and mapping method suitable for unstructured dynamic environments. First, the motion distortion of the laser radar point cloud data is removed based on the attitude angle measured by the inertial element, and then the laser point cloud data without motion distortion is fused with the attitude output by the inertial element. The laser point cloud is segmented by geometric features to obtain a more accurate cabin environment perception result; secondly, in order to obtain the real-time posture of the pusher in the high dynamic environment in the cabin, a laser odometer based on semantic feature point matching is proposed to estimate the posture of the pusher in the cabin. Based on the perception results obtained by multi-sensor fusion, the semantic information is used to extract feature points, and then the point-to-point and point-to-line matching methods are used to estimate the posture of the pusher in real time; finally, in order to reduce the memory usage of the point cloud map, a map update method in a dynamic environment based on nearest neighbor search is designed, and this method is used to reduce the spatial complexity of the algorithm.

Description

Technical Field

[0001] The present invention relates to the problem of simultaneous positioning and mapping in an unstructured dynamic environment. For a highly dynamically changing environment, the present invention proposes a laser odometer method using multi-sensor fusion and semantic feature point matching, and simultaneously uses a dynamic map update method based on nearest neighbor search to ensure simultaneous positioning and mapping in an unstructured dynamic environment. Background Art

[0002] The driverless industry has embarked on a road of rapid development. Facing driverless applications in various industries, it is indispensable to have a complete driverless system. For driverless applications in special industries, the rational use of special environmental information and the selection of appropriate software and hardware strategies can promote the implementation of driverless driving and protect the physical and mental health of workers in the harsh working environment of some special industries.

[0003] In the entire unmanned driving system, simultaneous positioning and mapping are the first and most basic steps. The most important thing is to correctly perceive the real-time status of the external environment and the vehicle itself, locate the position of the vehicle body in the environment, and establish the map information of the external environment. Only then can path planning and motion control be performed. The perception module often needs to segment and extract planes such as the ground or walls. The main segmentation methods for extracting planes include random sampling consensus algorithm (RANSAC), region growing algorithm, and linear fitting ground segmentation algorithm. The random sampling consensus algorithm can suppress the influence of point cloud noise on feature segmentation, and has high iterative calculation efficiency, but it requires manual setting of plane thresholds for pre-modeling, and has poor robustness. The region growing algorithm has high requirements for the selection of initial point clouds and is easily affected by noise points during the region growing process. The linear fitting ground segmentation algorithm uses the characteristics of the rotating laser radar to assign each point cloud to the corresponding sector for orderly processing, and then reduce the dimension of the three-dimensional data after the orderly processing. This operation can retain the information of the vertical dimension without losing the information of the horizontal dimension. Finally, each sector is fitted with a straight line and the ground points are screened.

[0004] In terms of lidar SLAM, single-line lidar SLAM mainly includes the gmapping algorithm based on particle filtering, the karto algorithm based on graph optimization, and the cartographer algorithm based on nonlinear optimization. However, since single-line lidar mainly performs two-dimensional mapping, for a three-dimensional unstructured environment, it is impossible to obtain sufficient environmental spatial change information to support simultaneous positioning and mapping. The main multi-line lidar SLAM algorithms include the LOAM algorithm (Zhang J, Singh S.LOAM: lidar odometry and mapping in real-time [C]. Robotics: Science and Systems. 2014: 33-41.) and the cartographer algorithm (Hess W, Kohler D, Rapp HH, et al. Real-time loopclosure in 2d lidar slam [C]. IEEE International Conference on Robotics and Automation. 2016: 1271-1278.), but they are all based on a structured, relatively static indoor or outdoor scene for mapping. For a highly dynamic environment, such as a warehouse where goods are constantly reduced as the operation progresses, they often cannot achieve good results. At the same time, traditional mapping is an incrementally superimposed point cloud map. The number of point clouds will increase, resulting in an increasing memory usage, and it is impossible to update the existing map.

[0005] Fan Haidong proposed an automatic coal pusher positioning system and positioning method for use in coal grids (Fan Haidong, Hu Kaibo, Wang Lingang, Li Feng, Xia Zhiling, Zhu Yuchao, Shen Binghua. An automatic coal pusher positioning system and positioning method for use in coal grids [P]. Zhejiang Province: CN111367297A, 2020-07-03.). However, it is also impossible to perform real-time updates on the existing map for dynamically changing environments. Summary of the invention

[0006] The present invention aims to overcome the shortcomings of the prior art and provides a simultaneous positioning and mapping method suitable for unstructured dynamic environments.

[0007] The problems solved by the present invention are: (1) Due to the unstructured dynamic environment, such as the goods in the warehouse will continue to decrease with the operation, the traditional SLAM algorithm will extract the dynamically changing goods pile as feature points, and finally the positioning will fail because the positioning cannot be corrected. The present invention uses a linear fitting plane algorithm to extract the wall, separate the dynamic object from the static wall, and then use the semantic feature points extracted from the static wall for subsequent frame matching; (2) The traditional map is an incrementally superimposed point cloud map. The point cloud will become more and more, resulting in an increasing memory usage. At the same time, the point cloud that has been reached will not be updated, which is inconsistent with the actual situation that the goods are decreasing with the operation. The present invention uses a map update method in a dynamic environment based on the nearest neighbor search to optimize the map update strategy. Therefore, the present invention proposes a laser odometer method using multi-sensor fusion and semantic feature point matching, and uses a dynamic map update method based on the nearest neighbor search to ensure simultaneous positioning and mapping in an unstructured dynamic environment.

[0008] The method for simultaneous positioning and mapping in an unstructured dynamic environment provided by the present invention comprises the following steps:

[0009] Step 1: Acquire and preprocess the LiDAR and inertial navigation unit data, including:

[0010] (11) Use laser radar to obtain environmental point cloud data, and use inertial sensors to obtain vehicle acceleration and angular velocity data;

[0011] (12) Eliminate invalid values ​​generated during the measurement process and invalid points of the vehicle's own contour; (13) Perform spatial calibration between multiple sensors and use the calibration results to correct the laser radar point cloud distortion caused by vehicle movement.

[0012] Step 2: Use the information fused from the LiDAR and inertial navigation sensors to perform semantic segmentation of point clouds in unstructured and highly dynamic environments. Because objects in unstructured environments will constantly change as the work progresses, and indoor walls are stationary relative to dynamically changing objects, it is necessary to separate dynamic objects from static walls, including:

[0013] (21) Design a linear fitting plane algorithm suitable for environmental features to extract walls. Calculate the angle between the connecting line of adjacent point clouds in the same sector and the horizontal plane, and set a threshold. If the point cloud exceeds the threshold and reaches a certain number of consecutive points, it is classified as a wall point cloud.

[0014] (22) Then point cloud clustering is performed. After DBSCAN clustering, the point cloud in each clustering result is analyzed, and the points that are higher than the vertical part of the wall point cloud are retained as point cloud clusters for extracting feature points, and then input into the front-end odometer module.

[0015] Step 3: Based on the previous perception results with semantic information, the point cloud data is downsampled and semantic feature points are extracted, including:

[0016] (31) Select static points and extract feature points by calculating the curvature of the points within the local small plane;

[0017] (32) Calculating the plane smoothness according to the curvature of the point as an indicator for extracting the feature information of the current frame, and dividing the feature points into edge points with high curvature and plane points with low curvature;

[0018] (33) Remove the occlusion points and parallel points with greater influence.

[0019] Step 4: Use the feature points after segmenting the dynamic objects to perform point cloud registration, including:

[0020] (41) Different inter-frame matching strategies are adopted based on different feature points. The inter-frame matching criteria are based on the registration of the new scanned feature points with the features of the previous moment in the cache. An optimization problem with Euclidean distance as the objective function and transformation matrix as the optimization variable is established to solve the transformation matrix, thus obtaining the local pose transformation estimate of the mobile robot.

[0021] (42) aligning the feature points of the current scan with the features obtained by principal component analysis in the environment at a relatively low frequency of 1 Hz to obtain an optimized transformation matrix;

[0022] (43) The optimized transformation matrix converts the map into the world coordinate system to form a map, and corrects the real-time pose estimation obtained by inter-frame registration.

[0023] Step 5: Map update method in dynamic environment using nearest neighbor search, including:

[0024] (51) Perform binary tree segmentation on the three-dimensional X, Y, and Z directions of the point cloud data in turn to construct a KD tree, and perform nearest neighbor search based on the KD tree;

[0025] (52) After the point cloud information is saved as a KD tree structure, a KNN search is performed on the keyframe and the historical keyframe while the keyframe is generated. If a keyframe with a posture close to the keyframe is found, the current keyframe point cloud is used to replace the historical keyframe point cloud, thereby updating the map and reducing memory usage.

[0026] The advantages and positive effects of the present invention are:

[0027] 1. The present invention uses a linear fitting plane algorithm to extract walls, separates dynamic objects from static walls, and then uses the semantic feature points extracted from the static walls for subsequent frame matching, thereby solving the positioning and mapping problems in unstructured dynamic environments. For example, the goods in the warehouse will continue to decrease as the operation proceeds. The warehouse as a dynamic object is extracted as a feature point, which will cause positioning failure because the positioning cannot be corrected.

[0028] 2. The present invention performs spatial calibration between multiple sensors, uses the calibration results to correct the motion distortion of the lidar point cloud, and uses the perception algorithm of multi-sensor fusion to enable the vehicle to have better terrain adaptability and perception accuracy.

[0029] 3. The present invention optimizes the map update strategy by using the nearest neighbor search in a dynamic environment map update method. This solves the problem that the traditional incremental overlay point cloud map will have more and more point clouds, resulting in more and more memory usage, and the point clouds that have been reached will not be updated, which is more in line with the environmental characteristics of a highly dynamic changing environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 It is the flow chart of the perception module of the present invention.

[0031] Figure 2 It is a flow chart of the laser odometer module of the present invention.

[0032] Figure 3 It is a schematic diagram of KNN search of the present invention.

[0033] Figure 4 This is a point cloud segmentation map of the method of the present invention in the warehouse.

[0034] Figure 5 This is the positioning and mapping of the method of the present invention in the warehouse.

[0035] Figure 6 This is the positioning and mapping of the method of the present invention in outdoor scenes. DETAILED DESCRIPTION

[0036] The simultaneous positioning and mapping method applicable to unstructured dynamic environments of the present invention is further described in detail below with reference to the accompanying drawings by using simple examples.

[0037] The method for simultaneous positioning and mapping in an unstructured dynamic environment provided by the present invention comprises the following steps:

[0038] Step 1: The perception module flow chart is as follows Figure 1As shown in the figure, the laser radar and inertial navigation element data are acquired and preprocessed; (1) the environmental point cloud data is acquired by the laser radar, and the vehicle acceleration and angular velocity data are acquired by the inertial sensor; (2) the invalid values ​​generated during the measurement process and the invalid points of the vehicle's own contour are removed; (3) spatial calibration is performed between multiple sensors, and the laser radar point cloud distortion caused by vehicle movement is corrected using the calibration results.

[0039] Step 2: Use the information fused from the LiDAR and inertial navigation sensors to perform semantic segmentation of point clouds in unstructured and highly dynamic environments. Because objects in unstructured environments will constantly change as the work progresses, and the indoor walls are stationary relative to dynamically changing objects, it is necessary to separate dynamic objects from static walls. (1) Design a linear fitting plane algorithm suitable for environmental features to extract walls. First, divide the point cloud in the world coordinate system into 1800 sectors with the radar as the origin and parallel to the horizontal plane according to an angular resolution of 1 / 1800°. Then divide the point cloud into 90 sectors with the radar as the origin and perpendicular to the horizontal plane. Through these two steps of division, the dimensionality of the three-dimensional point cloud information is reduced, and most of the point clouds can be labeled, which can be used to store and access them in an orderly manner. By calculating the angle between the connecting line of adjacent point clouds in the same sector and the horizontal plane, a threshold of 60 degrees is set. If the point cloud exceeds the threshold, it is classified as a static wall point cloud. (2) Then point cloud clustering is performed. After DBSCAN clustering, the point cloud in each clustering result is analyzed, and the points higher than the vertical part of the wall point cloud are retained as the point cloud cluster for extracting feature points. Then, it is input to the front-end odometer module. The minimum enclosing rectangle can be calculated through the projection of the vertical wall for warehouse corner point calibration. The warehouse point cloud segmentation result is shown in the figure below. Figure 4 As shown, the red part of the point cloud is the ground and cargo, the plum-red part of the point cloud is the vertical wall, and the green part is the ceiling.

[0040] Step 3: The laser odometer module flow chart is as follows Figure 2 As shown in the figure, based on the perception results with semantic information, the point cloud data is downsampled and semantic feature points are extracted. (1) Select static points and extract feature points by calculating the curvature of the points in the local small plane; (2) Calculate the plane smoothness based on the curvature of the points as an indicator for extracting the feature information of the current frame, and divide the feature points into edge points with high curvature and plane points with low curvature; (3) Remove the occluded points and parallel points with greater influence.

[0041] Step 4: Use the feature points after segmenting the dynamic objects to perform point cloud registration. (1) Different inter-frame matching strategies are adopted based on different feature points. The inter-frame matching criteria are based on the new scanned feature points and the features of the previous moment in the cache. An optimization problem with Euclidean distance as the objective function and transformation matrix as the optimization variable is established to solve the transformation matrix, that is, to obtain the local pose transformation estimate of the mobile robot; (2) At the same time, the feature points of the current scan are registered with the features obtained by principal component analysis in the environment at a relatively low frequency of 1 Hz to obtain the optimized transformation matrix; (3) The optimized transformation matrix converts the map to the world coordinate system to form a map, and corrects the real-time pose estimate obtained by inter-frame registration.

[0042] Step 5: Map update method in dynamic environment using nearest neighbor search. Binary tree segmentation is performed on the three-dimensional X, Y, and Z directions of the point cloud data in turn to construct a KD tree, and the nearest neighbor search is performed based on the KD tree. The specific steps are: (1) Starting from the root node, use the binary tree search method to compare the values ​​of the query node and the split node in this dimension. If the former is less than or equal to the latter, enter the left subtree branch, if it is greater, enter the right subtree branch; (2) Search downward according to the logic of step 1 until the nearest approximate point, that is, the leaf node in the same space as the query point; (3) Backtrack the search path to determine whether the leaf nodes of other paths are closer than the current approximate point. If so, update the approximate point. If not, continue the backtracking search; (4) Use the distance between the query point and the approximate point as the radius and the query point as the center to create a candidate space. If the candidate space intersects with the split axis of the backtracking point, add the nodes on the other side of the axis to the search path; (5) Repeat steps 2, 3, and 4 until the search path is empty. At this time, the saved approximate point is regarded as the nearest neighbor of the query point. Figure 3 As shown, the vertical distance between the query point and the segmentation axis and the query radius can be compared to determine whether the candidate space intersects with the segmentation axis. i represents the radius of the candidate space, (x i ,y i ) represents the coordinates of the query point. Assuming that the split point is perpendicular to the axis, the expression of the split axis is y = y0. iIf -y0 < r1, it means that the candidate space intersects with the splitting axis, and the nodes on the other side of the splitting axis need to be added to the search path. Otherwise, it means they do not intersect. After saving the point cloud information in the structure of the KD tree, while generating key frames, perform KNN search between the key frames and the historical key frames. If a key frame with a neighboring pose is searched, use the current key frame point cloud to replace the historical key frame point cloud. Thus, the map is updated, and at the same time, the memory occupancy is reduced. This method has good positioning and mapping effects in unstructured dynamic indoor environments and also has good effects in outdoor environments. The positioning and mapping results in the warehouse are as Figure 5 shown, and the positioning and mapping outdoors are as Figure 6 shown. The white points are the vehicle's historical path points.

[0043] It should be emphasized that the embodiments described in the present invention are illustrative rather than restrictive. Therefore, the present invention includes but is not limited to the embodiments described in the specific implementation schemes. Any other similar implementation manners obtained by those skilled in the art according to the technical solutions of the present invention also fall within the protection scope of the present invention.

Claims

1. A real-time positioning and mapping method in an unstructured dynamic environment, comprising the following steps: Step 1: Acquire and preprocess the LiDAR and inertial navigation unit data, including: (11) Use LiDAR to obtain environmental point cloud data, and use inertial sensors to obtain vehicle acceleration and angular velocity data; (12) Remove invalid values ​​generated during the measurement process and invalid points of the vehicle's own contour; (13) Perform spatial calibration between multiple sensors, and use the calibration results to correct the LiDAR point cloud distortion caused by vehicle movement; Step 2: Use the information fused from the LiDAR and inertial navigation sensors to perform semantic segmentation of point clouds in unstructured and highly dynamic environments. Because objects in unstructured environments will constantly change as the work progresses, and indoor walls are stationary relative to dynamically changing objects, dynamic objects and static walls need to be segmented, including: (21) Design a linear fitting plane algorithm suitable for environmental features to extract walls. Calculate the angle between the line connecting adjacent point clouds in the same sector and the horizontal plane, and set a threshold. If the angle exceeds the threshold and reaches a certain number continuously, it is classified as a wall point cloud. Specifically, the point cloud in the world coordinate system is first divided into 1800 sectors with the radar as the origin and parallel to the horizontal plane according to an angular resolution of 1 / 1800°, and then the point cloud is divided into 90 sectors with the radar as the origin and perpendicular to the horizontal plane. Through these two steps, the three-dimensional point cloud information is reduced in dimension, and the point cloud is labeled. These labels can be used to store and access in an orderly manner. Calculate the angle between the line connecting adjacent point clouds in the same sector and the horizontal plane, and set a threshold of 60 degrees. If the angle exceeds the threshold, it is classified as a static wall point cloud. (22) Then perform point cloud clustering. After DBSCAN clustering, analyze the point clouds in each clustering result, retain the points that are higher than the vertical wall point cloud, use them as point cloud clusters for extracting feature points, and then input them into the front-end odometer module; Step 3: Based on the previous perception results with semantic information, the point cloud data is downsampled and semantic feature points are extracted, including: (31) selecting static points and extracting feature points by calculating the curvature of the points in the local small plane; (32) Calculate the plane smoothness according to the curvature of the point as an indicator for extracting the feature information of the current frame, and divide the feature points into edge points with high curvature and plane points with low curvature; (33) Remove occluded points and parallel points; Step 4: Use the feature points after segmenting the dynamic object to perform point cloud registration, including: (41) adopting different inter-frame matching strategies based on different feature points. The inter-frame matching criteria are based on the new scanned feature points and the features of the previous moment in the cache. An optimization problem with Euclidean distance as the objective function and transformation matrix as the optimization variable is established to solve the transformation matrix, thus obtaining the local pose transformation estimate of the mobile robot; (42) The feature points of the current scan are aligned with the features obtained by principal component analysis in the environment at a relatively low frequency of 1 Hz to obtain an optimized transformation matrix; (43) The optimized transformation matrix transforms the map into the world coordinate system to form a map, and corrects the real-time pose estimation obtained by inter-frame registration; Step 5: Map update method in dynamic environment using nearest neighbor search, including: (51) Perform binary tree segmentation on the three-dimensional X, Y, and Z directions of the point cloud data in turn to construct a KD tree, and perform nearest neighbor search based on the KD tree; (52) After the point cloud information is saved as a KD tree structure, a KNN search is performed on the keyframe and the historical keyframe while the keyframe is generated. If a keyframe with a posture close to the keyframe is found, the current keyframe point cloud is used to replace the historical keyframe point cloud, thereby updating the map and reducing memory usage.

2. The real-time positioning and mapping method in an unstructured dynamic environment as claimed in claim 1, characterized in that: Step (51) specifically includes: (511) Starting from the root node, using the binary tree search method, compare the values of the query node and the split node in this dimension. If the former is less than or equal to the latter, enter the left subtree branch; if it is greater, enter the right subtree branch. (512) Conduct a downward search according to the logic of step 511 until the nearest approximate point, that is, the leaf node in the same space as the query point. (513) Backtrack the search path and determine whether the leaf nodes of other paths are points closer to the query point than the current approximate point. If so, update the approximate point; if not, continue backtracking the search. (514) Create a candidate space with the distance between the query point and the approximate point as the radius and the query point as the center. If the candidate space intersects the split axis of the backtracking point, add the nodes on the other side of the axis to the search path. (515) Repeat steps 512, 513, and 514 until the search path is empty. At this time, the saved approximate point is regarded as the nearest neighbor point of the query point; determine whether the candidate space intersects the split axis by comparing the perpendicular distance between the query point and the split axis with the query radius, where r i represents the radius of the candidate space, (x i , y i ) represents the coordinates of the query point. Assuming the split point is perpendicular to the axis, the expression of the split axis is y = y0; if |y i - y0| < r1, it means that the candidate space intersects the split axis and the nodes on the other side of the split axis need to be added to the search path; otherwise, it means they do not intersect.

Images