Laser SLAM mapping positioning method and system in dynamic environment

Abstract

The invention discloses a laser SLAM mapping positioning method and system in a dynamic environment. The method comprises the steps that point cloud information and IMU measurement data in the dynamic environment are collected; the data preprocessing module carries out pre-integration by using IMU data so as to remove point cloud distortion; the pose estimation module realizes positioning based on error Kalman filtering by using IMU measurement information and point cloud feature matching; the dynamic point filtering module stores the three-dimensional point cloud in a two-dimensional image mode, judges whether the three-dimensional point cloud is a dynamic point or not based on the change condition of the point cloud at the same pixel, and finally carries out clustering elimination to output the point cloud only containing a static scene; and the loopback detection module extracts descriptors based on the static scene point cloud, and corrects the pose deviation through ICP (Inductively Coupled Plasma) matching. According to the method, the problem that dynamic object interference occurs in the mapping positioning scene is effectively solved, the dynamic points do not interfere with positioning, the map does not contain ghosts generated by moving objects, help is provided for follow-up navigation and the like, and the robustness and accuracy of mapping positioning are improved.

Description

Technical Field

[0001] The present invention relates to the field of synchronous mapping and positioning, and in particular to a laser SLAM mapping and positioning method and system in a dynamic environment. Background Art

[0002] Synchronous mapping and positioning in dynamic scenes refers to the method of completing robust pose estimation and accurate static scene construction in scenes with interference from moving objects, such as streets with heavy traffic and factories with frequent workers. Pose estimation is performed based on sensors, and accurate pose estimation and three-dimensional scene construction are completed by processing environmental perception data.

[0003] Since the system often perceives external data through cameras or lidars, and this data passively receives all data within the surrounding environment of the sensor, pose estimation is completed based on this data through reprojection errors or feature matching methods. When interference occurs, it is easy to cause positioning failure or poor mapping effects, which in turn leads to the failure of the entire positioning and mapping system. It can be seen that filtering out dynamic point data is crucial for positioning and mapping. It is closely related to the way it processes environmental perception sensor data. The main purpose of processing environmental perception data is to detect dynamic point clouds and remove the influence of dynamic environments to achieve accurate position estimation and use precise static scene point cloud data to complete scene construction.

[0004] However, at present, most synchronous mapping and positioning solutions require the environment to be as static as possible, that is, the data does not contain interference from dynamic objects; however, this is contrary to many real environments. Scenes with frequent activities of these dynamic objects will cause deviations in the pose calculated by traditional algorithms. At the same time, ghosting caused by dynamic points during mapping will cause black spots to appear on the grid map, thereby reducing the efficiency of downstream tasks such as navigation.

[0005] At present, the solutions for dynamic point removal in dynamic scenes are mainly divided into laser, vision, and multi-sensor fusion solutions according to different sensors. The first method is to use lidar to remove dynamic points offline, such as Sungwon Hwang et al. (ERASOR: Egocentric Ratio of Pseudo Occupancy-Based DynamicObject Removal for Static 3D Point Cloud Map) After obtaining the prior map using conventional laser SLAM methods, the map is divided into grids and a descriptor expressing the point cloud is calculated. The radar data scanned in each frame is descriptor-aligned with the submap. If it is less than the threshold, it is filtered out after fitting the ground point. The second method is to remove dynamic points by visual detection. For example, in the visual SLAM visual odometer method and system based on point and line features in a dynamic environment (202410767511.X), motion estimation and dynamic area detection are performed on the reference image and the query image respectively. The dynamic area detection performs optical flow tracking on the entire image, detects the dynamic area by the direction of the optical flow field vector, and outputs the dynamic interval mask to the motion estimation module. This part extracts point and line features. Consensus points are collected for feature registration, and after dynamic features are eliminated, point-line fusion pose optimization is performed to obtain the camera pose; the third method is to fuse the camera and lidar to eliminate the influence of dynamic objects. For example, a method and system for fast point cloud elimination of dynamic obstacles based on YOLO (CN202410085741.8) mentions a method for detecting and removing dynamic points based on a neural network to directly predict the category and position of the target. This method first requires the lidar and camera to observe the calibration plate at the same time to complete the calibration, and use YOLOv5 to identify the bounding box and coordinate information of the dynamic obstacles in the RGB image, and then use the internal and external references of the camera and lidar to project the bounding box information onto the point cloud, and finally use an adaptive pass-through filtering algorithm for identification and elimination.

[0006] Mapping and positioning solutions in dynamic environments have problems such as instability, poor real-time performance, and complex solutions. Generally speaking, these are mainly caused by the following aspects: 1. Most existing solutions for detecting dynamic points use deep learning methods of cameras. This method requires a large number of real labeled data sets for training, which takes a long time and can only detect objects in existing databases. 2. Most existing methods for filtering dynamic points based on lasers are offline filtering, which adds an extra step and is time-consuming.

[0007] Although some technologies are currently aimed at mapping and positioning algorithms in dynamic environments, they all have their shortcomings. For example, using lidar to remove dynamic points offline can only remove dynamic points by recording data packets and playing them back, which doubles the time cost. For example, building a semantic model based on visual deep learning to identify dynamic objects requires a large amount of real labeled data sets to train the neural network, which makes its time cost too high and can only filter out objects identified in existing data sets. In addition, the effect will be uncontrollable as the lighting conditions change. The laser and camera fusion solution is mainly to fuse and calibrate the lidar feature points in the three-dimensional space with the two-dimensional camera feature points. Due to the fusion of data from multiple sensors, the accuracy of the data can be effectively improved, but the resulting computational cost is too high, and it is too dependent on the calibration accuracy of the internal and external parameters of the sensor. Summary of the invention

[0008] The present invention provides a laser SLAM mapping and positioning method and system in a dynamic environment, including: a data acquisition module collects point cloud information and IMU measurement data in a dynamic environment; a data preprocessing module uses IMU data for pre-integration to remove point cloud distortion; a posture estimation module uses IMU measurement information and point cloud feature matching to achieve positioning based on error Kalman filtering; a dynamic point filtering module saves a three-dimensional point cloud in the form of a two-dimensional image, and determines whether it is a dynamic point based on the change of the point cloud at the same pixel, and finally performs clustering and elimination to output a point cloud containing only static scenes; a loop detection module extracts descriptors based on the above static scene point cloud, and corrects the posture deviation through ICP matching. The method can effectively solve the scene where dynamic objects interfere in the mapping and positioning scene. This method can make the dynamic points not interfere with the positioning accuracy, and can make the established map not contain ghosts generated by moving objects, which provides help for subsequent navigation and other applications based on the map. The method provides a new idea for mapping and positioning. To achieve the above purpose, the present invention proposes a laser SLAM mapping and positioning method and system in a dynamic environment.

[0009] The present invention is achieved by at least one of the following technical solutions.

[0010] A laser SLAM mapping and positioning method in a dynamic environment comprises the following steps:

[0011] Step 1: Use the data acquisition module to collect point cloud data and inertial measurement data in a factory environment;

[0012] Step 2: downsampling the point cloud data collected in step 1, pre-integrating the inertial measurement data collected in step 1, and removing the point cloud data distortion;

[0013] Step 3: Use the pre-integral calculated in step 2 as the motion equation, perform line and surface fitting on the point cloud data in step 2, use the iterative nearest method ICP matching to obtain the observation equation, construct the error Kalman filter based on the motion equation and the observation equation, and iterate to obtain the final pose estimate;

[0014] Step 4, subscribe to the posture information obtained in step 3, and transform the output point cloud data in step 2 into a two-dimensional image through a spherical coordinate system, save N frames of two-dimensional image queues, and perform motion event detection on the N frames of two-dimensional image queues to filter dynamic point clouds, cluster dynamic points and remove them from the point cloud, so as to obtain a static scene point cloud;

[0015] Step 5: Build a local map using the static scene point cloud obtained in step 4, fit the plane, select key points to build a triangle descriptor, perform loop detection, iterate the vertex coordinates using the closest point method (ICP) to obtain the pose and correct the loop deviation.

[0016] Furthermore, in step 1, the dynamic scene is an indoor or outdoor factory environment, and the factory environment includes static scene objects and dynamic objects; static scene objects include equipment, walls, plants, and ground, and static scene objects are used to estimate posture; dynamic objects include moving objects, and moving objects are filtered out in the point cloud obtained by the lidar scan, and only static objects inherent in the scene are retained, thereby ensuring the robustness of positioning and mapping.

[0017] Furthermore, in step 1, the data acquisition module collects information through a 3D laser radar, and the collected information includes laser point cloud data and six-axis acceleration information of an IMU inertial measurement device.

[0018] Furthermore, in step 4, the formula for projecting the three-dimensional point cloud coordinates to the two-dimensional plane pixels through the spherical coordinate system transformation is:

[0019]

[0020] in θ represents the azimuth and polar angle of the point in the spherical coordinate system, d represents the depth of the point, and p x ,p y ,p z Represents the x, y, and z axis coordinates of the point cloud in the right-hand coordinate system;

[0021] Through the above formula, the calculated point cloud data is saved to the pixels of the two-dimensional plane. The pixel size is set by the horizontal and vertical resolutions in the yaml file. The pixel contains the point cloud depth range of the pixel area, the point cloud depth range of the adjacent pixels, and the position of the pixel in the two-dimensional plane.

[0022] Furthermore, in step 4, motion event detection is performed by detecting the depth change of the same pixel between the current frame image and the previous N frames saved to select the dynamic point cloud:

[0023]

[0024] Among them, d represents the depth of the current detection point, d min With d max They represent the minimum depth and maximum depth of a frame at the same pixel in a two-dimensional plane queue, d thr1 With d thr2 Represents the distance threshold. If the difference between the above two distances exceeds the threshold, the point cloud is labeled with a potential dynamic point.

[0025] Furthermore, it is determined whether the dynamic point of the current frame image is close to the inherent static object in the aforementioned list of ten frames of images. If it is determined to be yes, the label of the potential dynamic point of the current pixel block is cancelled.

[0026] Furthermore, in step 5, the plane fitting is calculated on the static scene point cloud data output in step 4, and the point cloud output in step 4 needs to be accumulated. When the number of point clouds accumulates to the set value, a key frame will be formed. Voxels are divided in the form of a grid on this key frame, and plane fitting and growth are performed in units of voxels. Key points are extracted from the key frame. The main information of the descriptor constructed using the key points includes vertex coordinates, side lengths calculated from vertex data, and triangle plane normal vectors.

[0027] Furthermore, if a certain voxel can form a plane, the plane is recorded. After all voxels are constructed, a plane is randomly grown from a certain voxel. That is, if adjacent voxels have the same plane, they are merged into the same plane. The judgment is as follows:

[0028]

[0029] in represents the plane normal vector, θ is the cosine angle between the two plane normal vectors, θ thr is the cosine angle determination threshold of the plane normal vector; repeat the process until all adjacent voxels are detected by growth; after all voxels are expanded, the plane boundary is generated, and the total voxels become boundary voxels. After obtaining the boundary voxels, the point cloud in the plane is divided into grids according to the resolution, and the maximum distance of the point cloud in each grid projected onto the plane is calculated. If the distance is the largest among the adjacent grids, the point is set as a key point. Each key point contains its position information and the normal information of the plane extracted from it.

[0030] A system for implementing the laser SLAM mapping and positioning method in a dynamic environment includes:

[0031] The data acquisition module collects 3D lidar information and inertial measurement information through sensors mounted on the mobile robot and publishes it on the ROS framework;

[0032] The data preprocessing module is used to receive information from the data acquisition module, and to establish the coordinate space transformation equation through forward prediction through pre-integration of inertial measurement data to remove the point cloud distortion caused by the movement of the mobile robot through back propagation;

[0033] The pose estimation module estimates the pose through the point cloud released by the data preprocessing module and the inertial measurement data of the data acquisition module through the error Kalman filtering method, and outputs the odometer;

[0034] Dynamic point filtering module, subscribes to point cloud data and pose odometer information, detects dynamic point clouds, and completes clustering and mapping;

[0035] The loop detection module extracts descriptors from the static scene point cloud after the dynamic points are filtered out by the dynamic point filtering module, and queries the loop through the odometer loop, and corrects the posture deviation through the descriptor iterative nearest method ICP matching.

[0036] A computer device of the present invention comprises: a memory and a processor and a computer program stored in the memory. When the computer program is executed on the processor, the laser SLAM mapping and positioning method in a dynamic environment is implemented.

[0037] Compared with the prior art, the present invention has the following beneficial effects:

[0038] The method for mapping and positioning in dynamic scenes proposed in this invention removes dynamic point clouds from the overall point cloud through motion point cloud detection and density clustering-based methods, and introduces a loop detection algorithm based on triangle descriptors, which significantly improves the robustness of positioning in dynamic environments and reduces the ghosting phenomenon in the mapping process, thereby improving the accuracy and reliability of mapping and positioning. In addition, this method also has the ability of real-time detection, meeting the requirements of online mapping and positioning.

[0039] The present invention provides an entire module for mapping, positioning and loop closure in dynamic scenes, which is divided into: a data acquisition module: using a 3D laser radar to collect point cloud data and inertial measurement data in a factory environment, a point cloud data preprocessing module: performing voxel downsampling and distortion removal on the collected point cloud data, a posture estimation module: using the above point cloud and IMU data, through ICP matching and IMU pre-integration and other methods combined with error Kalman filtering to obtain the posture, a dynamic point filtering module: the posture obtained from the above module is detected by motion time detection, and the dynamic points are clustered and filtered out, and a loop detection module: the static scene point cloud obtained from the above module calculates a triangle descriptor, and calculates iterative nearest method ICP matching to correct the posture deviation.

[0040] The present invention can effectively filter out dynamic objects in the scene, and does not require data set training and complex sensor calibration processes, which can effectively save time. At the same time, when facing complex dynamic scenes, due to the existence of the loop detection module, the static scene point cloud extraction descriptor output by the dynamic point filtering module can significantly eliminate the cumulative error of the posture. After experimental testing, the entire algorithm can run stably and in real time. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly demonstrate the technical solution of the present invention, further description will be given below with reference to the accompanying drawings.

[0042] Figure 1 It is a module diagram of an embodiment of the present invention;

[0043] Figure 2 Detailed step flow chart of an embodiment of the present invention;

[0044] Figure 3 This is a rendering of the original laser radar mapping of an embodiment of the present invention;

[0045] Figure 4 This is a flow chart of a dynamic point filtering module according to an embodiment of the present invention;

[0046] Figure 5 This is a schematic diagram of the effect of creating a grid map before and after removing dynamic points in an embodiment of the present invention. DETAILED DESCRIPTION

[0047] The present invention will be described in detail and completely below in conjunction with the drawings in the embodiments of the present invention. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0048] like Figure 1 As shown, in a laser SLAM mapping and positioning system in a dynamic environment of this embodiment, the hardware system of the data acquisition module includes a mobile robot platform, a sensor module and a host computer module, wherein the mobile robot platform adopts a four-wheel differential design, and its overall architecture can be specifically divided into four parts: a power module, a sensor, a power system, and a control platform. The FreeRTOS operating system is adopted to realize the mutual cooperation of various modules; the sensors are mainly 3D laser radars and inertial measurement units; the host computer module realizes distributed message sending and receiving through the ROS framework.

[0049] The specific process of mapping and positioning of the present invention is as follows Figure 1 As shown, the data acquisition module: on the mobile robot, MID-360 provides point cloud information and inertial measurement data;

[0050] Data preprocessing module: The data preprocessing module performs preprocessing operations on the point cloud information and inertial data information collected by the data acquisition module, including point cloud voxelization downsampling, invalid point removal, point cloud distortion removal, etc.

[0051] The motion estimation module performs pose estimation based on the processed point cloud data and inertial measurement data, which includes two parts: lidar odometer and IMU pre-integration, and the process of performing error Kalman filtering to calculate pose.

[0052] Dynamic point filtering module: The pose obtained by the above module is used to detect dynamic points through motion time detection, and the dynamic points are clustered and filtered out.

[0053] Loop detection module: The static scene point cloud computing triangle descriptor obtained by the above module is used to calculate ICP-like matching to correct the posture deviation.

[0054] A laser SLAM mapping and positioning method in a dynamic environment implemented in this embodiment includes the following steps:

[0055] Step 1. Use the data acquisition module to collect point cloud data and inertial measurement data in a factory environment. The factory environment is a dynamic environment, which includes stationary objects such as equipment, walls, plants, and the ground for estimating posture, and also includes dynamic objects such as workers, vehicles, mobile robots and other constantly moving objects, which can be filtered out by the described method, retaining only the static objects inherent in the scene, thereby ensuring the robustness of positioning and mapping.

[0056] Select a suitable factory environment, use a four-wheel differential car to collect data through the data acquisition module, and subscribe to 3D lidar point cloud data and IMU data.

[0057] Step 2, downsample the point cloud data collected in step 1, integrate the six-axis acceleration information between the two radar frames collected in step 1, that is, pre-integrate, estimate the posture change, and remove the distortion caused by the point cloud due to the movement of the mobile robot itself through the principle that the point cloud posture remains unchanged in the global coordinate system; specifically, divide the point cloud into different blocks according to the timestamp of the IMU. In this paper, the IMU is 100Hz, the laser radar frequency is 10Hz, and each frame of the laser radar has 20,000 points, which is equivalent to 10 frames of IMU in two adjacent radar frames. Then the block in each two frames of IMU is 2000 points, and the point cloud in each block is unified to the initial time of the IMU frame. Since the posture at each IMU timestamp can be obtained in the pre-integration process, the relative posture from the IMU frame timestamp to the radar frame can be reversely deduced, and then the distortion is obtained through equal transformation to remove the distortion of the point cloud in each module.

[0058] Step 3: Use the pre-integration obtained in step 2 to perform line and surface fitting with the point cloud data in step 2, use the iterative closest point (ICP) method to obtain the pose transformation, and construct an error Kalman filter based on the pose transformation and pre-integration to iteratively obtain the final pose estimate.

[0059] By fitting the point cloud into a straight line or plane and performing inter-frame registration, the pose increment between two radar frames is calculated as the motion equation of the error Kalman filter. At the same time, according to the pre-integration calculated in step 2, the observation equation of the error Kalman filter is obtained, and the pose is iteratively calculated. In this way, the pose is made more accurate. The specific steps are as follows:

[0060] Step 3.1. Query the point cloud on the map through ikdtree, select the five nearest points, fit a line or plane, calculate the distance from the point to the line or plane, and iterate using the Gauss-Newton method to obtain the pose increment.

[0061] Step 3.2: Use the IMU pre-integration and the posture increment obtained in step 3.1 as the observation equation, obtain the error increment through iteration, and obtain a more accurate posture after a certain number of iterations.

[0062] like Figure 2 The IMU pre-integration module shown in the figure reads the acceleration, angular velocity and other data of the inertial measurement unit and integrates them based on the previous frame pose. The specific estimates are as follows:

[0063] Inertial Measurement Unit IMU model:

[0064]

[0065] ω m =a+b ω +n ω

[0066] Among them, a, ω, b, and n represent the acceleration measurement value, angular velocity measurement value, zero bias, and noise respectively. m ,ω m Represents the true value of acceleration and angular velocity.

[0067] Integrate all IMU data between the current two radar frames to obtain the relative increments of displacement, velocity, and rotation:

[0068]

[0069] in They represent the displacement, velocity, and rotation increments from time k to k+1, respectively. represents the rotation from time t to time k, a t , bat 、n a ,ω t , b ωt 、n w Represents acceleration, accelerometer bias, accelerometer noise, angular velocity, gyroscope bias, and gyroscope noise at time t. Using IMU pre-integration and three-dimensional space coordinate transformation, the pose change between two radar frames is obtained through pre-integration, and the increment is added to the pose of the previous frame. The distortion of the point cloud caused by motion is removed through reverse calculation. This step is to ensure the reliability of the calculation results when performing motion estimation.

[0070] The covariance of the inertial measurement unit (IMU) data is recursively calculated. Since the IMU has a high frequency and high computational complexity, the relative pose transformation mentioned above is expanded with respect to the zero bias in a first-order Taylor manner to prevent the need to recalculate the pose after it is updated, i.e., the first-order approximation is used to quickly update the pose.

[0071] like Figure 2 The point cloud matching of the pose estimation module shown in the figure performs feature matching of the point cloud received in the current frame with the point cloud of the previous frame, takes the distance between the two points as the target residual, and connects the two points by the iterative closest point (ICP) method. In this way, an equation associated with the pose is constructed, which is continuously iterated by the Gauss-Newton method and other methods, and will eventually converge to a target value. The target value of this pose can be used as the relative odometer of the lidar.

[0072] Through the above IMU pre-integration and laser odometer, two equations can be obtained respectively. Here, the error Kalman filter is introduced. Kalman filter is an algorithm that uses the linear system state equation to optimally estimate the system state through the system input and output observation data, and introduces covariance to characterize the noise and other interferences suffered by the system, which is equivalent to filtering the final posture result to increase the reliability of motion estimation. Here, IMU pre-integration is used as the motion equation, feature matching is used as the observation equation, and the posture increment is iteratively calculated. After convergence, the final odometer is output, such as Figure 2 As shown in the figure, the odometer will be output to the loop detection module and the dynamic point filtering module. The positioning and mapping effect in the factory environment is as follows Figure 3 shown.

[0073] Step 4: Subscribe to the posture information obtained in step 3, transform the point cloud data output in step 2 into a two-dimensional image through the spherical coordinate system, save a 10-frame two-dimensional image queue, perform motion event detection on the two-dimensional image queue to filter dynamic point clouds, cluster dynamic points and remove them from the point cloud; by comparing the point cloud information at the same position of the current frame and the list frame, determine the point cloud attributes, and assign potential dynamic point labels if they are moving points. At the same time, in order to eliminate misjudgments caused by changes in the perspective of the mobile robot and relative motion, compare the point with the static object in the image list. If the distance is close, it is considered a misjudgment. This step is the key to detecting dynamic objects. The specific steps are as follows:

[0074] Dynamic point removal module: The above data acquisition module and pose estimation module will obtain point cloud and odometer data respectively. On this basis, the dynamic point detection module subscribes to these two topics respectively, creates a list to store point cloud messages of the past N frames, and projects these three-dimensional point clouds to a two-dimensional plane through spherical coordinate transformation, which is more convenient for storage and comparison. Each pixel saves the depth range of each point projected into the pixel range, the depth range of adjacent pixels, the position of the "pixel" in the image, etc. Since the motion is detected by point-by-point detection, it is necessary to register the point cloud to a two-dimensional plane and use spherical coordinate transformation:

[0075]

[0076] In the formula, p x ,p y ,p z Represents the coordinates of a point on the point cloud; θ and d represent the azimuth, polar angle, and depth of a point. By dividing the azimuth and polar angle by the horizontal and vertical resolutions, the coordinates i and j of each pixel block on the image can be obtained.

[0077]

[0078] where r h 、r v is the horizontal and vertical resolution.

[0079] Through the above formula, the point cloud data is saved in pixels of the two-dimensional plane. The pixel size is set by the horizontal and vertical resolutions in the yaml file. The pixel contains the point cloud depth range in the pixel area, the point cloud depth range of the adjacent pixels, the position of the pixel in the two-dimensional plane (indicated by the index in the horizontal and vertical domains of the image), etc. Save ten frames of point cloud data as an image list according to the above method, which is used for comparison and judgment with the latest frame of point cloud data.

[0080] At the same time, save the depth range of each pixel block and the pixel blocks within a certain range around it. Let point P be the point where event detection is required. According to the formula, it is projected into the depth image and its position information, depth range, etc. are obtained. If the depth of the point is greater than the maximum depth saved by the previous pixel and the adjacent pixels, the point is considered to be blocked by the point in the depth image. If the depth of the point is less than the minimum depth saved by the current pixel and the adjacent pixels, the point is considered to be blocked in the depth image:

[0081]

[0082] Among them, d represents the depth of the current detection point, d min With d max They represent the minimum depth and maximum depth of a frame at the same pixel in the 2D image queue, respectively. thr1 With d thr2 Indicates the distance threshold.

[0083] Motion event detection: It is mainly divided into two forms: 1. Vertical motion event detection 2. Parallel motion event detection; event detection makes two independent judgments on each current point based on two occlusion principles. If any one of them meets the requirements, the point label is set as a potential moving point.

[0084] In an embodiment of the present invention, Figure 4 As shown in , the above motion point detection method is divided into two event detection examples. The first is a vertical motion detection example, which detects objects moving perpendicular to the laser beam. This type of motion is reflected in the following: a pixel block somewhere in the current frame represents a dynamic object, while it is a background object in the image list. Therefore, the depth change of the point cloud at the same pixel can be used to determine whether a dynamic object appears.

[0085] Vertical motion event detection: The first test detects event points of objects whose motion direction is perpendicular to the LiDAR laser beam. In this case, the point cloud must cover the pixel at the same position in the image list, that is, determine the degree of change between the depth value of the point and the depth value of the same position in the image list.

[0086] Event detection with parallel motion: The second test detects object points that are parallel to the sensor laser ray and are moving away from the LiDAR. In this case, the point cloud must be repeatedly occluded by itself in the image list, i.e. the pixel in the image list will be enlarged or reduced, with a synchronous change in the point cloud depth.

[0087] It should be noted that the change in perspective and relative motion caused by the movement of the mobile robot itself will also make the motion event detection positive. Figure 4As shown in the flow chart, consider a false detection event detection module, which determines whether the point is close to an inherent static object in the aforementioned ten-frame image list. If so, the label of the dynamic point is cancelled.

[0088] In the above event detection examples, sometimes misjudgments may occur due to changes in the perspective of the car's movement. In order to eliminate this error, the following correction examples are needed to reduce the false detection rate: It is easy to imagine that when the car moves, the edge pixel point cloud of fixed background objects such as walls will appear positive in the above detection examples due to relative motion. Based on this relationship, the correction example of the false detection event detection module in the dynamic point detection module detects whether the point cloud judged as positive is close to the background object in the image list. If the distance is within the threshold, it is considered to be an erroneous judgment caused by the change in perspective caused by relative motion, and it is reassigned to negative.

[0089] In the above detection and correction examples, more reliable judgments will be drawn, but it cannot be ruled out that isolated point clouds will be misdetected, especially ground points. Due to the small angle, the ground points change greatly when the car moves. Therefore, the ground points can be segmented to eliminate the misdetected points on the ground. After the ground points are segmented, dynamic point cloud clustering is performed to separate the dynamic points from the point cloud data and filter out the dynamic points.

[0090] In another embodiment, point cloud clustering adopts a density-based clustering method, with the center of the point cloud block as the midpoint, and gradually expands outward to collect point cloud blocks with a certain density of dynamic points. When there is no dynamic point cloud within the expansion range, the expansion is stopped. The point cloud collected in the process is the overall point cloud of the dynamic object. This part of the point cloud is removed from the registered point cloud, and the dynamic points are filtered out, and the static scene point cloud is output. The specific effect can be seen Figure 5 ,After being converted to a raster map, the black shadow caused by the ,dynamic point cloud is significantly reduced.

[0091] After the above-mentioned pose estimation module and dynamic point filtering module, the odometer and static scene point cloud will be obtained. The point cloud will be registered to the map through ikdtree to complete the mapping.

[0092] Through the above embodiments, point cloud information of a static scene without interference from dynamic points is obtained. Descriptors can be extracted from the point cloud at the key frame to describe the position information. When the mobile robot returns to the vicinity of the original position, that is, a loop is formed, the same descriptor will be obtained to correct the posture deviation. According to this principle, the cumulative error can be reduced.

[0093] Step 5: Accumulate the static scene point cloud obtained in step 4 into key frames and divide it into voxels according to a grid of a certain resolution. After fitting the plane in the voxels, use the stable characteristics of the triangle to extract the triangle descriptor composed of the key points in the static scene. When a loop is detected, use the matching relationship between vertices and the iterative nearest neighbor method (ICP) to obtain the pose, correct the loop deviation, and obtain the corrected pose. The specific steps are as follows:

[0094] Step 5.1: From the key frame constructed by the above method, grid division is performed according to certain voxels, and the covariance matrix is ​​decomposed to determine whether the points inside each voxel form a plane. If a plane is formed, the plane is expanded by searching for neighboring voxels until all added neighboring voxels are expanded, which are called boundary voxels.

[0095] In a specific embodiment of the present invention, by receiving the static scene point cloud output by the dynamic point filtering module and registering it as a local map, when the number of subframes accumulates to a certain number, a key frame will be formed, and plane fitting will be performed on this key frame to complete the plane growth voxel by voxel.

[0096] Prioritize, if a certain voxel can form a plane, then record the plane. After all voxels are constructed, randomly start growing a plane from a certain voxel. That is, if adjacent voxels have the same plane (normal vectors are the same and the tangent distance is less than the threshold), the judgment is as follows:

[0097]

[0098] in represents the plane normal vector, θ thr is the angle determination threshold.

[0099] Then they are grouped into the same plane, and the cycle repeats until all adjacent voxels are detected by growth; after all voxels are expanded, the plane boundary is generated, and the total voxels become boundary voxels. After obtaining the boundary voxels, the point cloud in the plane is divided into grids according to the resolution, and the maximum distance of the point cloud projected onto the plane in each grid is calculated. If the distance is the largest among the adjacent grids within a certain range, the point is set as a key point. Each of the above key points contains its position information and the normal information of the plane extracted from it.

[0100] Step 5.2, after obtaining the boundary voxels, calculate the distance of each point projected to the plane in the grid, save the maximum distance, and when the distance is the largest among the values ​​saved in the grid within the adjacent range, set the point as a key point, and build a KD tree for these points for easy search. As an embodiment, search for 15 adjacent points for each point to form a triangle descriptor. Each triangle descriptor contains three vertex information and the normal vectors of the plane from which the three vertices are extracted. The specific descriptor information is as follows:

[0101]

[0102] Step 5.3: Since the number of descriptors in each key frame may be as high as hundreds, the fast query feature of the hash table is used to achieve fast matching;

[0103] Often, dozens or even hundreds of descriptors can be obtained through a key frame, so all descriptors can be stored in a hash table. Based on the rotation and translation invariance of the descriptors, the hash keys of descriptors with the same six-dimensional attributes are saved in the same container through the three-dimensional side length and the three-dimensional vector product.

[0104] Step 5.4: When a candidate loop keyframe is selected, since the triangle descriptors are matched in pairs, the matching relationship between the triangle vertices is used to construct an iterative closest point formula, and the pose error is obtained by solving its SVD decomposition. The specific method is as follows:

[0105] When querying a keyframe, all descriptors are extracted and their hash keys are calculated. The corresponding container is located in the hash table. The keyframe containing the descriptor in this container has its vote count increased by one. When all descriptors in the keyframe are processed, the matching process ends and the keyframe with more than 10 votes is set as a candidate with a matching descriptor.

[0106] In the thread of the closed loop query, when the above candidate items meet the requirements, it enters the pose calculation thread. Since the triangle is uniquely determined after the side length is determined, once two descriptors match, the vertices between them can also form a match. Through this matching relationship, combined with the pose (R, T), the following formula can be obtained:

[0107]

[0108] SVD(H)=[U|Σ|V]

[0109] R=VU T ,t=-R·q a +q b

[0110] where p ai , p bi , is the coordinates of the vertices of the matching triangle, q a ,q b To match the coordinates of the triangle centroid, H is the covariance matrix, which is obtained by SVD decomposition of H. U and V are orthogonal matrices, and Σ is the singular value matrix.

[0111] In order to prevent false matches and increase the reliability of the results, the random sampling consensus algorithm RANSAC is used to find the transformation that maximizes the number of correctly matched descriptors.

[0112] Step 5.5: After obtaining the pose between the loop frames, use the properties of the plane from which the triangle descriptor is extracted to verify the result, that is, the plane from which the triangle vertices of each matching pair are extracted should be the same plane.

[0113] In order to ensure the correctness of the above matching, the extracted planes need to be verified: the plane group of the current frame and the candidate frame is determined by the angle between the normal vectors to determine whether the two planes overlap. This verifies whether the planes overlap and improves robustness.

[0114] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, a person skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A laser SLAM mapping and positioning method in a dynamic environment, characterized in that: The steps include: Step 1: Use the data acquisition module to collect point cloud data and inertial measurement data in a factory environment; Step 2: downsampling the point cloud data collected in step 1, pre-integrating the inertial measurement data collected in step 1, and removing the point cloud data distortion; Step 3: Use the pre-integral calculated in step 2 as the motion equation, perform line and surface fitting on the point cloud data in step 2, use the iterative nearest method ICP matching to obtain the observation equation, construct the error Kalman filter based on the motion equation and the observation equation, and iterate to obtain the final pose estimate; Step 4, subscribe to the posture information obtained in step 3, and transform the output point cloud data in step 2 into a two-dimensional image through a spherical coordinate system, save N frames of two-dimensional image queues, and perform motion event detection on the N frames of two-dimensional image queues to filter dynamic point clouds, cluster dynamic points and remove them from the point cloud, so as to obtain a static scene point cloud; Step 5: Build a local map using the static scene point cloud obtained in step 4, fit the plane, select key points to build a triangle descriptor, perform loop detection, iterate the vertex coordinates using the closest point method (ICP) to obtain the pose and correct the loop deviation.

2. According to the method for laser SLAM mapping and positioning in a dynamic environment as described in claim 1, it is characterized by: In step 1, the dynamic scene is an indoor or outdoor factory environment, which includes static scene objects and dynamic objects; static scene objects include equipment, walls, plants, and the ground, and static scene objects are used to estimate posture; dynamic objects include moving objects, and moving objects are filtered out in the point cloud obtained by the lidar scan, and only the static objects inherent in the scene are retained, thereby ensuring the robustness of positioning and mapping.

3. According to the method for laser SLAM mapping and positioning in a dynamic environment as described in claim 1, it is characterized by: In step 1, the data acquisition module collects information through 3D laser radar, and the collected information includes laser point cloud data and six-axis acceleration information of IMU inertial measurement device.

4. According to the method of laser SLAM mapping and positioning in a dynamic environment as described in claim 3, it is characterized by: In step 4, the formula for projecting the three-dimensional point cloud coordinates to the two-dimensional plane pixels through the spherical coordinate system transformation is: in θ represents the azimuth and polar angle of the point in the spherical coordinate system, d represents the depth of the point, and p x ,p y ,p z Represents the x, y, and z axis coordinates of the point cloud in the right-hand coordinate system; Through the above formula, the calculated point cloud data is saved to the pixels of the two-dimensional plane. The pixel size is set by the horizontal and vertical resolutions in the yaml file. The pixel contains the point cloud depth range of the pixel area, the point cloud depth range of the adjacent pixels, and the position of the pixel in the two-dimensional plane.

5. According to the method for laser SLAM mapping and positioning in a dynamic environment as described in claim 1, it is characterized by: In step 4, motion event detection is to filter out dynamic point clouds by detecting the depth change of the same pixel between the current frame image and the previous N frames saved: Among them, d represents the depth of the current detection point, d mi With d max They represent the minimum depth and maximum depth of a frame at the same pixel in a two-dimensional plane queue, d thr1 With d thr2 Represents the distance threshold. If the difference between the above two distances exceeds the threshold, the point cloud is labeled with a potential dynamic point.

6. The laser SLAM mapping and positioning method in a dynamic environment according to claim 5, characterized in that: It is determined whether the dynamic point of the current frame image is close to the inherent static object in the aforementioned list of ten frames of images. If it is determined to be yes, the label of the potential dynamic point of the current pixel block is cancelled.

7. The laser SLAM mapping and positioning method in a dynamic environment according to claim 1, characterized in that: Step 5: The plane fitting is calculated on the static scene point cloud data output in step 4, and the point cloud output in step 4 needs to be accumulated. When the number of point clouds accumulates to the set value, a key frame will be formed. Voxels are divided in the form of a grid on this key frame, and plane fitting and growth are performed in units of voxels. Key points are extracted on the key frame. The main information of the descriptor composed of key points includes vertex coordinates, side lengths calculated from vertex data, and triangle plane normal vectors.

8. The laser SLAM mapping and positioning method in a dynamic environment according to claim 8, characterized in that: If a certain voxel can form a plane, then record the plane. After all voxels are constructed, randomly start growing a plane from a certain voxel. That is, if adjacent voxels have the same plane, they are merged into the same plane. The judgment is as follows: in represents the plane normal vector, θ is the cosine angle between the two plane normal vectors, θ thr is the cosine angle determination threshold of the plane normal vector; repeat the process until all adjacent voxels are detected by growth; after all voxels are expanded, the plane boundary is generated, and the total voxels become boundary voxels. After obtaining the boundary voxels, the point cloud in the plane is divided into grids according to the resolution, and the maximum distance of the point cloud in each grid projected onto the plane is calculated. If the distance is the largest among the adjacent grids, the point is set as a key point. Each key point contains its position information and the normal information of the plane extracted from it.

9. A system for implementing the laser SLAM mapping and positioning method in a dynamic environment as described in claim 1, characterized in that: include: The data acquisition module collects 3D lidar information and inertial measurement information through sensors mounted on the mobile robot and publishes it on the ROS framework; The data preprocessing module is used to receive information from the data acquisition module, and to establish the coordinate space transformation equation through forward prediction through pre-integration of inertial measurement data to remove the point cloud distortion caused by the movement of the mobile robot through back propagation; The pose estimation module estimates the pose through the point cloud released by the data preprocessing module and the inertial measurement data of the data acquisition module through the error Kalman filtering method, and outputs the odometer; Dynamic point filtering module, subscribes to point cloud data and pose odometer information, detects dynamic point clouds, and completes clustering and mapping; The loop detection module extracts descriptors from the static scene point cloud after the dynamic points are filtered out by the dynamic point filtering module, and queries the loop through the odometer loop, and corrects the posture deviation through the descriptor iterative nearest method ICP matching.

10. A computer device, characterized in that: The invention comprises: a memory and a processor and a computer program stored in the memory. When the computer program is executed on the processor, a laser SLAM mapping and positioning method in a dynamic environment as claimed in any one of claims 1 to 8 is implemented.

Images