Laser SLAM (Simultaneous Localization and Mapping) method and system for dynamic environment and medium

Abstract

The invention relates to a dynamic environment-oriented laser SLAM (Simultaneous Localization and Mapping) method and system and a medium. Comprising the following steps: training and deploying a target detection four-dimensional neural network; performing environment initialization and data processing, and constructing an initial dense point cloud map; forward and reverse propagation is carried out by using IMU data, and the current state of the robot is accurately estimated; performing point cloud registration to update the map; identifying and marking feature points of the dynamic target; updating the dynamic or static probability of voxels in the map according to the dynamic target detection result; and outputting the optimized map and the pose estimation of the robot. According to the method, the understanding of the dynamic environment is updated by adopting the voxel belief model, the information of the dynamic object is effectively fused into the map, meanwhile, the accurate representation of the static environment is reserved, and the accuracy and robustness of positioning, map construction and dynamic object recognition of the SLAM system in the dynamic environment are improved. And through an optimized data processing flow, efficient calculation performance and real-time performance are ensured.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving and robot positioning and navigation, and in particular to a laser SLAM method, system and medium for dynamic environments. Background Art

[0002] If a robot wants to achieve truly autonomous movement, it needs to have a certain ability to perceive and understand the environment, which is reflected in the robot's ability to independently build the surrounding environment and obtain its relative position in the built environment map. Accurate positioning requires accurate maps, and building accurate maps requires the robot's own accurate position information, so SLAM (simultaneous localization and mapping) technology, which can simultaneously locate and build maps, came into being.

[0003] SLAM methods in static environments have been well studied as a basic capability of autonomous driving systems. It should be noted that they are all based on the assumption of a static world. However, due to the complexity of the real environment, dynamic targets such as motor vehicles, non-motor vehicles and pedestrians in complex urban scenes are inevitable. At this time, the existing LiDAR SLAM methods based on the assumption of static environments will fail. In the process of constructing point cloud maps using traditional LiDAR SLAM methods, moving objects will leave motion ghosts in the point cloud maps. Therefore, it is important to discuss SLAM applications in dynamic environments.

[0004] In today's LiDAR SLAM systems, most solutions are to build systems by coupling LiDAR point cloud feature extraction and inertial odometer. The establishment of the system is based on the extraction of LiDAR point cloud features. Most of them do not consider the filtering of dynamic features in the current frame point cloud of the LiDAR. A few of them are based on the consideration of the current frame point cloud of the LiDAR and filter out dynamic objects. However, the problem is that the effect of removing dynamic objects in the laser frame is not good, which in turn affects the construction of static maps.

[0005] In view of the above situation, there is an urgent need to design a laser SLAM method and system for dynamic environments to overcome the shortcomings in current practical applications. Summary of the invention

[0006] In order to overcome the above-mentioned deficiencies of the prior art, the present disclosure provides a laser SLAM method, system and medium for dynamic environments.

[0007] According to a first aspect of the present disclosure, a laser SLAM method for a dynamic environment is provided, characterized in that it comprises the following steps:

[0008] S1. Train and deploy a 4D convolutional neural network for object detection.

[0009] S2, environment initialization and data preprocessing: collect the spatial point data obtained by the lidar scan and the motion data from the inertial measurement unit IMU, initialize the deviation of the inertial measurement unit IMU, and build the initial dense point cloud map, and preprocess the spatial point data obtained by the lidar scan;

[0010] S3, state estimation: based on the preprocessed spatial point data, the motion data collected by the IMU is used for forward and backward propagation, combined with map information for registration, and the iterative error Kalman filter is used to accurately estimate the current state of the robot, including forward propagation, backward propagation and motion compensation, residual calculation, and iterative state update process;

[0011] S4, map update: point cloud registration update based on the state estimation results;

[0012] S5, Dynamic target detection: Analyze the spatial point data and local map obtained by LiDAR scanning through a four-dimensional convolutional neural network to identify and mark dynamic targets;

[0013] S6, voxel belief model update: According to the dynamic target detection results, the dynamic or static probability of the voxels in the map is updated using the Bayesian filtering algorithm to eliminate the impact of dynamic targets on positioning and mapping;

[0014] S7, output optimized map and robot pose estimation to provide accurate reference information for navigation and path planning;

[0015] Furthermore, according to the method of claim 1, the data preprocessing comprises:

[0016] The original laser radar is sampled at a frequency between 100kHz and 500kHz to obtain spatial point data, and the spatial point data obtained by scanning the laser radar is accumulated within a preset processing time;

[0017] The accumulated spatial point data set obtained by the laser radar scanning is called the current frame point cloud, and the preset processing time is recorded as t k ;

[0018] Furthermore, according to the method of claim 2, it is characterized in that step S2 and step S5 further include:

[0019] Adding a timestamp to the point cloud data of the current frame, storing the four-dimensional coordinates of each spatial point data obtained by the laser radar scan, wherein the four-dimensional coordinates include position information and time information, and organizing the four-dimensional coordinates into a four-dimensional tensor through a Minkowski engine to perform a neural network convolution operation;

[0020] Furthermore, the method according to claim 3 is characterized in that it also includes processing the timestamp:

[0021] Using the timestamp as a feature of the current frame point cloud, and normalizing the timestamp according to a minimum value and a maximum value, so that the four-dimensional convolutional neural network only focuses on the difference of the timestamps rather than the absolute value of the timestamps;

[0022] Furthermore, according to the method of claim 2, it is characterized in that the step S4 further comprises:

[0023] For the forward propagation process of the inertial measurement unit IMU data, the time interval between two IMU samplings is preset to be t i-1 ~t i Denoted as δt, take t i-1 and t i The arithmetic mean u of the IMU sampling results at the moment i , based on the state prediction value at the previous moment and the mean u within the interval δt i , get the predicted value of the state At the same time, calculate the error covariance matrix P k ;

[0024] Back propagation and motion compensation process, at t k At the time point, that is, when the accumulation time interval of the spatial point data obtained by the laser radar scanning reaches the end, the back propagation process is used to compensate for the time point at t k-1 ~t k In the time interval, due to different time t m The reference coordinate system of the space point is not unified, and the resulting distortion is obtained through back propagation. m Time relative to t k Relative position at time Through the relative pose transformation matrix, all spatial point data coordinates are unified to t k In the radar coordinate system at all times;

[0025] In the residual calculation process, the current frame point cloud that has undergone the back propagation and motion compensation process is used for residual calculation. At the same time, the measurement noise is taken into account, and each laser radar feature point is extracted from the current frame point cloud. The kd tree is used to search for the nearest point in the map, and the edge or surface composed of the nearest point is fitted and the distance is calculated, which is recorded as the residual. The point with a residual higher than the threshold is defined as an outlier or a newly observed point. If the nearest point found has been marked as dynamic, the next nearest point is searched and the residual is calculated.

[0026] Iterate the state update process, calculate the Kalman gain and optimize it to get the new state prediction value and the covariance matrix P k, and return the new state prediction value to the residual calculation process, preset the residual to 0 as the stop condition, and iterate repeatedly until the stop condition is met;

[0027] Furthermore, according to the method of claim 2, it is characterized in that the step S4 further comprises:

[0028] The map-based dynamic point cloud segmentation method uses a four-dimensional convolutional neural network to simultaneously predict the radar current frame point cloud and the local map, and obtains the prediction results of the lidar current frame point cloud and the dynamic and static prediction information of the points in the local local map respectively;

[0029] The four-dimensional convolutional neural network uses the MinkUNet network structure, which consists of an encoder and a decoder structure, and adds jump connections to fuse information of different resolutions. The last layer of the network uses a Relu activation function, and the output is a scalar. The predicted value is greater than 0, indicating that the point is predicted to be a dynamic point.

[0030] Specifically, at time t, there are N point sets P t Predict N predicted values ​​S t

[0031] P t ={p t,1 ,p t,2 ,…,p t,N}S t ={s t,1 ,s t,2 ,…,s t,N}

[0032] in Each point is a four-dimensional tensor;

[0033] Furthermore, according to the method of claim 6, it is characterized in that the step S5 further comprises:

[0034] A voxel hash table is established and maintained for the local local map, and a belief is stored for each voxel, indicating the confidence that the voxel is occupied by a dynamic object, according to the point set P t and the predicted value S t For each point in the point set, find the voxel where it is located according to the voxel hash table, and then find the predicted value S t Compute and maintain voxel beliefs;

[0035] The prediction results of the laser radar point cloud in the current frame are integrated with the four-dimensional convolutional neural network and the dynamic and static prediction information of the points in the local map, and the recursive binary Bayesian filter is used to obtain the final prediction of the voxel points in the map.

[0036] For multiple single points p in the same voxel t,j , summarize their prediction information, and take the arithmetic mean as the voxel belief, that is, the voxel m to be updated i The voxel belief l(m i |P t ,S t )for

[0037]

[0038] Where V t,i ={j|p t,j ∈v i} is the voxel v that falls into at time t i The point set of |V t,i | is the cardinality of the set;

[0039] Apply a recursive binary Bayesian filter equation to the probability distribution of each voxel, using log-odds notation The iterative equation can be obtained:

[0040] l(m i |P 1:t ,S 1:t )=l(m i |P 1:t-1 ,S 1:t-1 )+l(m i |P t ,S t )-l(m i );

[0041] To update the belief l(m i |P 1:t ,S 1:t ), where l(m i |P 1:t-1 ,S 1:t-1 ) is the recursive term currently stored in the voxel, which summarizes the previous predictions, l(m i |P t ,S t ) is the voxel update term that integrates the prediction at the current time t, and l(m i ) is the logarithmic probability of the prior probability p0, assuming that for voxel m i There is no prior knowledge of the dynamic attribution of , so it is set to p0 = 0.5;

[0042] According to a second aspect of the present disclosure, a laser SLAM system for a dynamic environment is provided, characterized in that it includes:

[0043] Training and deployment module, environment initialization and data processing module, state estimation module, map update module, dynamic target detection module, voxel belief model update module and output module, among which,

[0044] The training and deployment module is used to train and deploy the target detection four-dimensional convolutional neural network;

[0045] The environment initialization and data processing module is used for environment initialization and data preprocessing, collects spatial point data obtained by laser radar scanning and motion data from the inertial measurement unit IMU, initializes the deviation of the inertial measurement unit IMU, and constructs an initial dense point cloud map, while preprocessing the spatial point data obtained by the laser radar scanning;

[0046] The state estimation module is used for state estimation. Based on the preprocessed spatial point data, the motion data collected by the IMU is used for forward and backward propagation, and the map information is combined for registration. The iterative error Kalman filter is used to accurately estimate the current state of the robot, including forward propagation, backward propagation and motion compensation, residual calculation, and iterative state update process;

[0047] The map update module is used to update the map and perform point cloud registration updates based on the state estimation results;

[0048] The target detection module is used for dynamic target detection, and analyzes the spatial point data and local local map obtained by laser radar scanning through a four-dimensional convolutional neural network to identify and mark dynamic targets;

[0049] The voxel belief model update module is used to update the voxel belief model. According to the dynamic target detection result, the dynamic or static probability of the voxels in the map is updated using the Bayesian filtering algorithm to eliminate the influence of the dynamic target on positioning and mapping;

[0050] The output module is used to output the optimized map and the robot's pose estimation to provide accurate reference information for navigation and path planning;

[0051] According to a third aspect of the present disclosure, there is provided a computer-readable storage medium having a computer program stored thereon, characterized in that:

[0052] When the computer program is executed by a processor, the method steps described in any one of claims 1 to 7 are implemented.

[0053] The principle of the technical solution disclosed in this disclosure is:

[0054] By coupling the lidar and inertial odometer, the preliminary map of the laser scan is registered with the robot's posture, and then a four-dimensional convolutional neural network is used to simultaneously segment and predict the dynamic objects in the lidar current frame point cloud and the locally registered local map, and to build a volume belief for the voxels in the voxel hash table. The volume belief contains the probability that each voxel in the map is a dynamic target. By using the lidar scanning information, inertial measurement unit information, and the prediction result information of the four-dimensional convolutional neural network, not only can a static map that removes dynamic objects be generated, but also a map containing the motion trajectory of dynamic objects can be generated. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] The above and other features, advantages and aspects of the embodiments of the present disclosure will become more apparent with reference to the following detailed description in conjunction with the accompanying drawings. The accompanying drawings are used to better understand the present solution and do not constitute a limitation of the present disclosure. In the accompanying drawings, the same or similar reference numerals represent the same or similar elements, among which:

[0056] Figure 1 A flow chart of a laser SLAM method for dynamic environments provided by the present disclosure;

[0057] Figure 2 This is a rendering of a laser SLAM system for dynamic environments provided by the present invention. DETAILED DESCRIPTION

[0058] In order to make the purpose, technical solution and advantages of the embodiments of the present disclosure clearer, the technical solution in the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings in the embodiments of the present disclosure. Obviously, the described embodiments are part of the embodiments of the present disclosure, not all of the embodiments. Based on the embodiments in the present disclosure, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present disclosure.

[0059] like Figure 1 As shown, in some embodiments, a laser SLAM method for a dynamic environment is provided, comprising the following steps:

[0060] Step S1, training and deploying a 4D convolutional neural network for target detection;

[0061] In some embodiments, the training process trains a 4D convolutional neural network using a supervised current frame point cloud and local map points with dynamic labels using a cross entropy loss;

[0062] Finally, save the model parameter file with the best performance on the validation set and pass the parameter file into the network model during deployment;

[0063] In some embodiments, data with a small proportion of dynamic points (the ratio between moving points and static points is less than 0:1%) is skipped during model training, thereby increasing the model convergence speed;

[0064] Use the Adam optimizer with weight decay and learning rate decay, and use cropping, rotation, flipping, and scaling for data enhancement to improve the model's generalization ability;

[0065] In some embodiments, the timestamp information in the 4D tensor is normalized in the first step of the forward propagation of the 4D convolutional neural network, and the maximum timestamp t is obtained for the tensor from the current frame point cloud or local map of the lidar. max and the minimum timestamp t min ,

[0066] If max =t min , then the timestamp information of all points is assigned to 1.0;

[0067] If max ≠t min , then the timestamps of all points are normalized, which prevents the convolutional network from overfitting due to too large timestamps;

[0068] Step S2, initializing the environment, using laser sensors and IMU inertial measurement units to collect environmental and motion data, and performing data preprocessing;

[0069] In some embodiments, the laser radar collects spatial points at a high sampling frequency, and a time interval Δt (t k-1 ~t k ), accumulate the sampling points within the time interval and record them as the point cloud of the current frame, while retaining the timestamp for each point;

[0070] In some embodiments, the motion data collected by the IMU inertial measurement unit within the time interval Δt is retained, and the multiple output times of the IMU within Δt are recorded as t i ;

[0071] Step S3, state estimation, based on the preprocessed data, uses the forward and backward propagation of the IMU inertial measurement unit data, combined with the iterative error Kalman filter, to accurately estimate the current state of the robot, including forward propagation, backward propagation and motion compensation, residual calculation, and iterative state update process;

[0072] Step S31: forward propagation using IMU inertial measurement unit data;

[0073] In some embodiments, take t i-1 and t i The IMU data measured at the time is averaged and recorded as ui , with u i As the measurement data during this period, and using t i-1 The predicted value of the state at the moment t i Status at all times Make a prediction. Specifically, it can be expressed as:

[0074]

[0075] Where δt represents t i-1 and t i the time interval between

[0076] in According to the IMU data u i and The obtained derivative of is obtained by discretization. The “+” indicates the state The components in accumulating integral values ​​are not equivalent to direct addition. For rotation matrices, the addition of Lie group SO(3) and corresponding Lie algebra is used. For other input values, it is equivalent to vector addition.

[0077] Since the sampling time of the IMU measurement unit is not aligned with the radar sampling time, that is, the last IMU sampling time within Δt is not equal to t k , the final state of the forward propagation within Δt is recorded as

[0078] At the same time, calculate the error covariance P k , similarly P k The propagation of i The process continues until the end time t of a new frame of point cloud is reached. k ;

[0079] Step S32: Back propagation and motion compensation step;

[0080] In some embodiments, the radar spatial points accumulated in step S2 are at different times t m The sampled position is unknown at the time of sampling, and t is obtained through back propagation. m The position at the moment;

[0081] In some embodiments, the state information obtained by the forward propagation is back-propagated;

[0082] In some embodiments, it is assumed that within Δt, all radar sampling times are recorded as t m , through the status Back propagation is performed to obtain t m Status of the moment

[0083] By Status The pose matrix in t m The radar space point coordinates at time t are transformed and unified to time t k In the coordinate system of , motion distortion is eliminated and the dedistorted point cloud coordinates are obtained;

[0084] In some embodiments, Indicates that IMU is at t m Time relative to t k Relative position at the moment;

[0085] The specific process of eliminating motion distortion:

[0086]

[0087] in, represents the coordinates of the spatial point at time tm in the radar coordinate system at time tm, that is, the coordinates when the distortion is not eliminated. I T L Represents the calibration external parameters of the radar and IMU measurement units, Indicates t m The space point at the moment is t k The coordinates in the radar coordinate system at the time, that is, the coordinates obtained after dedistortion;

[0088] Step S33, residual calculation;

[0089] Use the kd tree to find the 5 points closest to the current feature point and fit the plane:

[0090]

[0091] D1x i +D2y i +D3z i =-1

[0092]

[0093] AX=b

[0094] Solve and calculate the distance from the point to the plane through QR decomposition, recorded as residual z j , points with residuals higher than the threshold (0.5m) are defined as outliers or newly observed points;

[0095] In some instances, for edge feature fitting, a calculation similar to principal component analysis can be performed to find the main direction of the five nearest points (ie, the direction of maximum variance), thereby finding the target straight line for fitting and calculating the residual.

[0096] Step S34, iterative status update;

[0097] In some embodiments, the residual minimization is taken as the optimization goal, and the error iterative Kalman filter is used. Each time the iteration is performed, the predicted value of the state will change, and then the error will also change. Therefore, the iterative process will simultaneously update the covariance matrix of the state prediction value and the error, and return the result to step S33 for repeated iterations until the difference between two iterations is less than a given threshold.

[0098] Step S4, map update;

[0099] In some embodiments, the update of the local map requires calculating the distance between the center of the radar and each surface of the local map. If the distance is less than a threshold, the radar is close to the boundary and needs to be moved to update the local map;

[0100] Get the rotation matrix and translation vector in the solved state, then transform the point in the radar coordinate system to the global coordinate system, and finally add it to the global map;

[0101] Step S5, dynamic target detection;

[0102] The model inputs the radar current frame point cloud and local map, predicts each point through the 4D neural network, and finally obtains a logits output, which represents the dynamic and static information of the point;

[0103] In some embodiments, the predicted value can be converted into a dynamic or static label. Since the last layer of the network adopts the Relu activation function, the point with a predicted value greater than 0 is predicted as a dynamic point, and vice versa.

[0104] Step S6, probability volume belief model update;

[0105] In some embodiments, points in the local map with predicted values ​​greater than 0 and all points in the point cloud of the current radar current frame are retained and integrated, and their predicted values ​​are integrated;

[0106] In some embodiments, the volume belief model is updated using the set of points to be updated and their predicted values. The voxels corresponding to all points are found through the hash table, which are called the voxels to be updated. For all voxels to be updated, the arithmetic mean of the predicted values ​​of all the points to be updated is calculated and regarded as the current belief of the voxel to be updated, that is, l(m i |P 1:t ,S 1:t ), followed by an iterative Bayesian filter to update the voxel belief;

[0107] In some embodiments, the voxel belief is obtained by the following update formula

[0108] l(m i |P1:t ,S 1:t )=l(m i |P 1:t-1 ,S 1:t-1 )+l(m i |P t ,S t )-l(m i )

[0109] The belief is expressed in logarithmic probability. l(m i |P 1:t ,S 1:t ) represents the voxel unit belief obtained by integrating current and historical information, l(m i |P 1:t-1 ,S 1:t-1 ) represents the historical prediction information stored in the voxel, l(m i ) represents the prior assumption of the confidence of the point. Since there is no prior information, it is set to p(x) = 0.5, that is, l(m i )=0;

[0110] Step S7, outputting the map after marking or removing the dynamic targets and the estimated pose of the robot, providing a map with a better understanding of the dynamic environment;

[0111] In some embodiments, for all points P in the map, n}, respectively search for the voxel m in the voxel hash table i , the belief value l(m i ) and convert it into a label value label i , and assign it to point p i . The way to convert to a label is

[0112]

[0113] At this point, all points in the map have marking information. The map without dynamic points is returned to the residual calculation process and is continued to be used for residual calculation to obtain a more accurate state estimation result. You can choose to hide dynamic points or display the movement trajectory of dynamic points in the map as needed, as shown in Figure 2 As shown in the figure, the white part is the static environment structure, and the red part is the position of the dynamic object at different times, that is, the motion trajectory of the dynamic object in the environment;

[0114] In some embodiments, the model requires initialization.

[0115] In some embodiments, a laser SLAM system for a dynamic environment is provided, including:

[0116] Training and deployment module, environment initialization and data processing module, state estimation module, map update module, dynamic target detection module, voxel belief model update module and output module, among which,

[0117] The training and deployment module is used to train and deploy the target detection four-dimensional convolutional neural network;

[0118] The environment initialization and data processing module is used for environment initialization and data preprocessing, collects spatial point data obtained by laser radar scanning and motion data from the inertial measurement unit IMU, initializes the deviation of the inertial measurement unit IMU, and constructs an initial dense point cloud map, while preprocessing the spatial point data obtained by the laser radar scanning;

[0119] The state estimation module is used for state estimation. Based on the preprocessed spatial point data, the motion data collected by the IMU is used for forward and backward propagation, and the map information is combined for registration. The iterative error Kalman filter is used to accurately estimate the current state of the robot, including forward propagation, backward propagation and motion compensation, residual calculation, and iterative state update process;

[0120] The map update module is used to update the map and perform point cloud registration updates based on the state estimation results;

[0121] The target detection module is used for dynamic target detection, and analyzes the spatial point data and local local map obtained by laser radar scanning through a four-dimensional convolutional neural network to identify and mark dynamic targets;

[0122] The voxel belief model update module is used to update the voxel belief model. According to the dynamic target detection result, the dynamic or static probability of the voxels in the map is updated using the Bayesian filtering algorithm to eliminate the influence of the dynamic target on positioning and mapping;

[0123] The output module is used to output the optimized map and the robot's pose estimation to provide accurate reference information for navigation and path planning.

[0124] An embodiment of the present disclosure further provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the method steps described above are implemented.

[0125] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided by the present disclosure can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0126] The technical features of the above embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above embodiments are not described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification. The above embodiments only express several implementation methods of the present disclosure, and the description is relatively specific and detailed, but it cannot be understood as a limitation on the scope of the invention patent. It should be pointed out that for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present disclosure, which all belong to the protection scope of this application. Therefore, the protection scope of the present disclosure shall be based on the attached claims.

[0127] The embodiments described above are only descriptions of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, ordinary technicians in this field may make various modifications and improvements to the technical solutions of the present invention, which should all fall within the scope of protection determined by the claims of the present invention.

Claims

1. A laser SLAM method for dynamic environments, characterized in that: The following steps are involved: S1. Train and deploy a 4D convolutional neural network for object detection. S2, environment initialization and data preprocessing: collect the spatial point data obtained by the lidar scan and the motion data from the inertial measurement unit IMU, initialize the deviation of the inertial measurement unit IMU, and build the initial dense point cloud map, and preprocess the spatial point data obtained by the lidar scan; S3, state estimation: based on the preprocessed spatial point data, the motion data collected by the IMU is used for forward and backward propagation, combined with map information for registration, and the iterative error Kalman filter is used to accurately estimate the current state of the robot, including forward propagation, backward propagation and motion compensation, residual calculation, and iterative state update process; S4, map update: point cloud registration update based on the state estimation results; S5, Dynamic target detection: Analyze the spatial point data and local map obtained by LiDAR scanning through a four-dimensional convolutional neural network to identify and mark dynamic targets; S6, voxel belief model update: According to the dynamic target detection results, the dynamic or static probability of the voxels in the map is updated using the Bayesian filtering algorithm to eliminate the impact of dynamic targets on positioning and mapping; S7: Output the optimized map and the robot’s pose estimation to provide accurate reference information for navigation and path planning.

2. The method according to claim 1, characterized in that The data preprocessing includes: The original laser radar is sampled at a frequency between 100kHz and 500kHz to obtain spatial point data, and the spatial point data obtained by the laser radar scanning is accumulated within a preset processing time; The accumulated spatial point data set obtained by the laser radar scanning is called the current frame point cloud, and the preset processing time is recorded as t k .

3. The method according to claim 2, characterized in that Step S2 and step S5 also include: A timestamp is added to the point cloud data of the current frame, and the four-dimensional coordinates of each spatial point data obtained by the laser radar scan are stored, wherein the four-dimensional coordinates include position information and time information, and the four-dimensional coordinates are organized into a four-dimensional tensor through the Minkowski engine to facilitate neural network convolution operations.

4. The method according to claim 3, characterized in that It also includes the processing of timestamps: The timestamp is used as a feature of the point cloud of the current frame, and the timestamp is normalized according to the minimum value and the maximum value, so that the four-dimensional convolutional neural network only focuses on the difference of the timestamp rather than the absolute value of the timestamp.

5. The method according to claim 2, characterized in that: The step S4 further comprises: For the forward propagation process of the inertial measurement unit IMU data, the time interval between two IMU samplings is preset to be t i-1 ~t i Denoted as δt, take t i-1 and t i The arithmetic mean u of the IMU sampling results at the moment i , based on the state prediction value at the previous moment and the mean u within the interval δt i , get the predicted value of the state At the same time, calculate the error covariance matrix P k ; Back propagation and motion compensation process, at t k At the time point, that is, when the accumulation time interval of the spatial point data obtained by the laser radar scanning reaches the end, the back propagation process is used to compensate for the time point at t k-1 ~t k In the time interval, due to different time t m The reference coordinate system of the space point is not unified, and the resulting distortion is obtained through back propagation. m Time relative to t k Relative position at time Through the relative pose transformation matrix, all spatial point data coordinates are unified to t k In the radar coordinate system at all times; In the residual calculation process, the current frame point cloud that has undergone the back propagation and motion compensation process is used for residual calculation. At the same time, the measurement noise is taken into account, and each laser radar feature point is extracted from the current frame point cloud. The kd tree is used to search for the nearest point in the map, and the edge or surface composed of the nearest point is fitted and the distance is calculated, which is recorded as the residual. The point with a residual higher than the threshold is defined as an outlier or a newly observed point. If the nearest point found has been marked as dynamic, the next nearest point is searched and the residual is calculated. Iterate the state update process, calculate the Kalman gain and optimize it to get the new state prediction value and the covariance matrix P k , and returns the new state prediction value to the residual calculation process, presets the residual to 0 as the stop condition, and iterates repeatedly until the stop condition is met.

6. The method according to claim 2, characterized in that The step S4 further comprises: The map-based dynamic point cloud segmentation method uses a four-dimensional convolutional neural network to simultaneously predict the radar current frame point cloud and the local map, and obtains the prediction results of the lidar current frame point cloud and the dynamic and static prediction information of the points in the local local map respectively; The four-dimensional convolutional neural network uses the MinkUNet network structure, which consists of an encoder and a decoder structure, and adds jump connections to fuse information of different resolutions. The last layer of the network uses a Relu activation function, and the output is a scalar. The predicted value is greater than 0, indicating that the point is predicted to be a dynamic point. Specifically, at time t, there are N point sets P t Predict N predicted values ​​S t Pt ＝ {p t,1 ,p t,2 ,…,p t,N } S t ={s t，1 ,s t，2 ,…,s t,N } in Each point is a four-dimensional tensor.

7. The method according to claim 6, characterized in that The step S5 further comprises: A voxel hash table is established and maintained for the local local map, and a belief is stored for each voxel, indicating the confidence that the voxel is occupied by a dynamic object, according to the point set P t and the predicted value S t For each point in the point set, find the voxel where it is located according to the voxel hash table, and then find the predicted value S t Compute and maintain voxel beliefs; The prediction results of the laser radar point cloud in the current frame are integrated with the four-dimensional convolutional neural network and the dynamic and static prediction information of the points in the local map, and the recursive binary Bayesian filter is used to obtain the final prediction of the voxel points in the map. For multiple single points pt,j in the same voxel, their prediction information is summarized and the arithmetic mean is taken as the voxel belief, that is, the voxel m to be updated i The voxel belief l(m i |P t ,S t )for Where V t，i ={j|p t,j ∈v i } is the voxel v that falls into at time t i The point set of |V t，i | is the cardinality of the set; Apply a recursive binary Bayesian filter equation to the probability distribution of each voxel, using log-odds notation The iterative equation can be obtained: l(m i |P 1：t ,S 1：t )=l(m i |P 1:t-1 S 1:t-1 )+l(m i |P t ,S t )-l(m i ); To update the belief l(m i |P 1:t , S 1:t ), where l(m i |P 1：t-1 , S 1:t-1 ) is the recursive term currently stored in the voxel, which summarizes the previous predictions, l(m i |P t ,S t ) is the voxel update term that integrates the prediction at the current time t, and l(m i ) is the logarithmic probability of the prior probability p0, assuming that for voxel m i There is no prior knowledge of the dynamic attribution of , and it is set to p0 = 0.

5.

8. A laser SLAM system for dynamic environments, characterized in that: include: Training and deployment module, environment initialization and data processing module, state estimation module, map update module, dynamic target detection module, voxel belief model update module and output module, among which, The training and deployment module is used to train and deploy the target detection four-dimensional convolutional neural network; The environment initialization and data processing module is used for environment initialization and data preprocessing, collects spatial point data obtained by laser radar scanning and motion data from the inertial measurement unit IMU, initializes the deviation of the inertial measurement unit IMU, and constructs an initial dense point cloud map, while preprocessing the spatial point data obtained by the laser radar scanning; The state estimation module is used for state estimation. Based on the preprocessed spatial point data, the motion data collected by the IMU is used for forward and backward propagation, and the map information is combined for registration. The iterative error Kalman filter is used to accurately estimate the current state of the robot, including forward propagation, backward propagation and motion compensation, residual calculation, and iterative state update process; The map update module is used to update the map and perform point cloud registration updates based on the state estimation results; The target detection module is used for dynamic target detection, and analyzes the spatial point data and local local map obtained by laser radar scanning through a four-dimensional convolutional neural network to identify and mark dynamic targets; The voxel belief model update module is used to update the voxel belief model. According to the dynamic target detection result, the dynamic or static probability of the voxels in the map is updated using the Bayesian filtering algorithm to eliminate the influence of the dynamic target on positioning and mapping; The output module is used to output the optimized map and the robot's pose estimation to provide accurate reference information for navigation and path planning.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method steps described in any one of claims 1 to 7 are implemented.

Images