SLAM (Simultaneous Localization and Mapping) method and device for eliminating dynamic points in real time and electronic equipment

Abstract

The invention relates to the field of image data processing, in particular to an SLAM method and device for eliminating dynamic points in real time and electronic equipment. The method comprises the following steps: superposing laser point clouds from the beginning to a preset time period, and segmenting a plane from an initial local map point cloud to obtain an initial ground equation; performing progressive ground parameter updating on the single-frame laser point cloud at the current moment according to the initial ground parameters, and segmenting ground points and non-ground points; voting each point according to the source of the non-ground point neighbor point to judge the motion attribute of the point, and completing the distinguishing of the dynamic point and the static point; performing inter-frame matching on the ground point and the static point to obtain pose information of the current laser radar, and performing map fusion according to the pose information to obtain a three-dimensional point cloud map; according to the SLAM method and device for eliminating the dynamic points in real time and the electronic equipment provided by the invention, the pose estimation precision and the closed-loop detection success rate can be well improved, and tiny dynamic targets are not easy to omit.

Description

Technical Field

[0001] The present invention relates to the field of image data processing, and in particular to a SLAM method, a device and an electronic device for real-time elimination of dynamic points. Background Art

[0002] There are two main methods for mobile robots to locate themselves (estimate their own position and posture): one is that in an outdoor environment with good GNSS signals, the mobile robot can locate itself through the global positioning navigation data obtained by the GNSS antenna; the other is that in a GNSS-denied environment, the mobile robot obtains environmental information through its own external sensors (such as visual cameras and lidars) and uses this information to complete its own positioning and build a map of the surrounding environment using SLAM technology.

[0003] Among them, laser SLAM based on LiDAR is widely used in the construction of large and complex indoor and outdoor scene maps. However, in large and complex indoor and outdoor scenes, dynamic targets such as pedestrians, vehicles, and even leaves disturbed by wind will affect the SLAM odometer estimation, closed-loop detection, and mapping. This is because: for odometer estimation, the odometer estimation module calculates the rigid pose transformation (translation and rotation) of the mobile platform based on the correspondence between adjacent laser scanning frames (referred to as adjacent frames). For example, the time interval between adjacent frames is 0.1 seconds, and the pose transformation of the LiDAR within this 0.1 second can be calculated by the registration of the two frames of point clouds. However, dynamic targets will introduce outlier correspondences and bring erroneous information to the residual calculation process of adjacent laser observations. For closed-loop detection, the impact of dynamic targets on the closed-loop detection module is reflected in the change of the scene. Dynamic targets such as pedestrians and vehicles move at a high frequency, which will cause the current scan frame and the historical scan frame at the same position to change in a short time. This may reduce the similarity between the current scan frame and the historical scan frame, thus affecting the success rate of loop closure detection. For mapping, dynamic targets will leave strip-like traces on the generated map. These dynamic points greatly affect the aesthetics and usability of the map, and will bring ambiguity to subsequent tasks such as obstacle detection, path planning, and semantic segmentation.

[0004] Existing SLAM methods use complete maps to detect and remove dynamic objects. These methods can only improve the quality of map construction, but do not improve the accuracy of pose estimation and the success rate of closed-loop detection. Some real-time methods use target clustering or regional grids as the minimum research object, which may miss subtle dynamic targets. Therefore, existing SLAM methods have the problem of not being able to improve the accuracy of pose estimation and the success rate of closed-loop detection, and are prone to missing subtle dynamic targets. Summary of the invention

[0005] The technical problem to be solved by the present invention is to overcome the shortcomings of the above-mentioned prior art and provide a SLAM method, device and electronic device that can better improve the accuracy of posture estimation and the success rate of closed-loop detection and eliminate dynamic points in real time without missing subtle dynamic targets.

[0006] In order to solve the above technical problems, the present invention provides a SLAM method for real-time elimination of dynamic points, comprising:

[0007] Collecting laser point cloud and inertial measurement data, restoring the global coordinates of the laser point cloud, superimposing the laser point cloud from the start to a preset time period to obtain an initial local map point cloud, and segmenting the plane from the initial local map point cloud to obtain an initial ground equation;

[0008] According to the initial ground parameters, the ground parameters of the single-frame laser point cloud at the current moment are progressively updated and ground points and non-ground points are segmented to obtain a ground point set and a non-ground point set;

[0009] Compare the non-ground point set with the local map point cloud point by point, vote for each point according to the source of the non-ground point's neighboring points to determine the motion attribute of the point, and distinguish between dynamic points and static points;

[0010] The ground points and static points are matched between frames to obtain the current LiDAR posture information, and map fusion is performed based on the posture information to obtain a three-dimensional point cloud map.

[0011] As a further improvement of the present invention: the collecting of laser point cloud and inertial measurement data, restoring the global coordinates of the laser point cloud, superimposing the laser point cloud from the start to a preset time period to obtain an initial local map point cloud, and segmenting the plane from the initial local map point cloud to obtain an initial ground equation includes:

[0012] Collect laser point cloud and inertial measurement data, use SLAM algorithm to restore the global coordinates of laser point cloud, and superimpose the laser point cloud from the start to the preset time period to obtain the initial local map point cloud;

[0013] Leveling the laser point cloud based on an angle between an original measurement of an IMU accelerometer and a gravitational acceleration constant to obtain a leveled laser point cloud;

[0014] Selecting points in a certain area directly below the leveled laser point cloud as ground seed points, applying RANSAC to the ground seed points to obtain an initial ground equation of the terrain;

[0015] Filter initial ground points and non-ground points based on the distance from the point to the initial ground.

[0016] Preferably, leveling the laser point cloud based on the angle between the original measurement of the IMU accelerometer and the gravity acceleration constant comprises:

[0017] The original measurement of the IMU accelerometer at the first scanning moment is recorded as a0, the gravity acceleration constant is g, and the rotation matrix R0 is constructed using the Rodriguez rotation formula. The calculation formula of the rotation matrix R0 is:

[0018]

[0019] In the formula, I is the unit matrix, k is the unit vector representing the direction, To transform vector k into its antisymmetric matrix, γ is the relative direction of a0 and g in three-dimensional space, and ∥∥ is the L2 norm.

[0020] As a further improvement of the present invention: the step of progressively updating the ground parameters of the single-frame laser point cloud at the current moment according to the initial ground parameters and segmenting the ground points and non-ground points to obtain the ground point set and the non-ground point set includes:

[0021] According to the IMU motion integral, the relative change of the LiDAR’s current position and posture compared to the previous moment is obtained;

[0022] Inferring the relative transformation of the ground according to the relative transformation of the posture, and updating the initial ground parameters;

[0023] Filter ground points and non-ground points based on the distance from the point to the ground;

[0024] The need to reinitialize the ground equation is determined based on the number of ground points. If necessary, the ground equation is re-estimated based on the ground seed point method.

[0025] Preferably, obtaining the relative transformation of the laser radar's posture at the current moment compared to the previous moment according to the IMU motion integral includes:

[0026] Integrate the IMU measurements between two consecutive lidar scans:

[0027]

[0028] Where k is the IMU sampling time between the continuous lidar sampling time i-1 and i, k∈[i-1,i]; ω is the average angular velocity from time k to time k+1, is the original measured value k of the angular velocity at time k, is the IMU angular velocity bias at time k, is the original measured value of the angular velocity at time kk+1, is the attitude rotation matrix of the IMU from time i-1 to time k+1, is the attitude rotation matrix of IMU from time i-1 to time k+1, Δt is the sampling interval of IMU, a is the average acceleration from time k to time k+1, is the original measured value of acceleration at time k, is the IMU acceleration bias at time k, is the original measured value of acceleration at time k+1, is the displacement vector from the IMU coordinate system at time i-1 to time k+1, is the displacement vector from the IMU coordinate system at time i-1 to time k, is the velocity vector from the IMU coordinate system at time i-1 to time k, is the attitude rotation matrix transformed from the IMU coordinate system at time i-1 to the world coordinate system, g is the gravity vector constant in the world coordinate system, is the velocity vector from the IMU coordinate system at time i-1 to time k+1;

[0029] When a new laser point cloud P i When it arrives, the transformation obtained by IMU integration compared to the previous scan is:

[0030]

[0031] In the formula, is the position transformation matrix of the laser radar from time i-1 to time i, is the attitude rotation matrix from IMU to lidar, is the attitude rotation matrix of IMU from time i-1 to time i, is the attitude rotation matrix from the laser radar to the IMU, is the displacement vector of IMU from time i-1 to time i, It is the translation external parameter from IMU to LiDAR; is the attitude rotation matrix of the laser radar from time i-1 to time i, It is the external parameter transformation between LiDAR and IMU.

[0032] As a further improvement of the present invention: comparing the non-ground point set with the local map point cloud point by point, voting for each point according to the source of the non-ground point neighboring points to determine the motion attribute of the point, and completing the distinction between dynamic points and static points includes:

[0033] According to the relative transformation of the posture obtained by the IMU motion integration, the coordinates of the non-ground points in the laser point cloud of the current frame are transformed;

[0034] Construct a KD-Tree for the local map point cloud and perform k-nearest neighbor search for the non-ground points after coordinate transformation;

[0035] Voting for non-ground points based on the sources of k nearest neighbor points. If the nearest neighbor point comes from a non-ground point in the current frame, it gets one vote. If the nearest neighbor point comes from a local map point cloud, it does not get a vote.

[0036] Whether a point is a dynamic point is determined based on the voting rate of the non-ground point. Points with a voting rate greater than a threshold are considered dynamic points, and points with a voting rate less than or equal to the threshold are considered static points.

[0037] Preferably, the step of constructing a KD-Tree for the local map point cloud and performing a k-nearest neighbor search for the non-ground points after coordinate transformation includes:

[0038] The predicted point cloud in the world coordinate system Point cloud with local map Merge to form a temporary local map point cloud Temporary local map point cloud It is expressed as:

[0039]

[0040] Point In the local point cloud map Perform k nearest neighbor search to get N kNN neighbor points.

[0041] As a further improvement of the present invention: the inter-frame matching of ground points and static points to obtain the current laser radar's position information, and the map fusion according to the position information to obtain the three-dimensional point cloud map includes:

[0042] The relative transformation of the posture obtained by the IMU motion integration is used as the prediction value, the sum of the distance from the ground point to the ground equation and the distance from the static point to the nearest neighbor plane is used as the residual, and the iterative error Kalman filter is used to solve the optimal estimate of the posture change;

[0043] Restoring the laser point cloud data to a global coordinate system according to the pose information, fusing the data into a local map point cloud, and removing the earliest frame of point cloud from the local map point cloud;

[0044] The local map point clouds are fused according to the posture information to obtain a three-dimensional point cloud map.

[0045] The present invention also provides a SLAM device for real-time dynamic point elimination, which is used to complete the above-mentioned SLAM method for real-time dynamic point elimination, comprising:

[0046] An initialization unit, used to collect laser point cloud and inertial measurement data, restore the global coordinates of the laser point cloud, superimpose the laser point cloud from the start to a preset time period to obtain an initial local map point cloud, and segment the plane from the initial local map point cloud to obtain an initial ground equation;

[0047] A ground segmentation unit is used to progressively update the ground parameters of the single-frame laser point cloud at the current moment according to the initial ground parameters and segment the ground points and non-ground points to obtain a ground point set and a non-ground point set;

[0048] A dynamic point detection unit is used to compare the non-ground point set with the local map point cloud point by point, and vote for each point according to the source of the non-ground point's neighboring points to determine the motion attribute of the point, thereby distinguishing between dynamic points and static points;

[0049] The pose estimation and map fusion unit is used to perform frame-to-frame matching between ground points and static points to obtain the pose information of the current lidar, and perform map fusion based on the pose information to obtain a three-dimensional point cloud map.

[0050] The present invention also provides a SLAM electronic device for real-time dynamic point elimination, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, a SLAM method for real-time dynamic point elimination as described above is implemented.

[0051] The beneficial effects of the present invention are as follows: the SLAM method, device and electronic device for real-time elimination of dynamic points provided by the present invention can well improve the accuracy of posture estimation and the success rate of closed-loop detection, and are not easy to miss subtle dynamic targets. The method emphasizes the "real-time" of detecting dynamic points. The method detects and eliminates dynamic points through a dynamic point voting algorithm based on the consistency of the scanning point source, integrates the dynamic point detection module into the SLAM framework, and eliminates the detected dynamic points before posture estimation, which helps to eliminate outlier matching and ensure that the point cloud involved in matching does not contain interference from dynamic points, thereby improving the accuracy of posture estimation and the success rate of closed-loop detection. The global map accumulated by the static point cloud does not contain traces of dynamic targets in the strip-shaped point cloud, avoiding ambiguity in subsequent tasks. At the same time, the method detects dynamic targets in points instead of target clustering or regional grids, so the detection results will not be affected by the clustering effect or grid resolution, and still has good detection capabilities for subtle dynamic targets, and is not easy to miss subtle dynamic targets. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 It is a principle block diagram of the present invention;

[0053] Figure 2A schematic structural diagram of an embodiment of a SLAM device for removing dynamic points in real time provided by the present invention;

[0054] Figure 3 A schematic structural diagram of an embodiment of an electronic device provided by the present invention;

[0055] The names of the components corresponding to the marks in the above figures are: 1. Initialization unit; 2. Ground segmentation unit; 3. Dynamic point detection unit; 4. Posture estimation and map fusion unit; 5. Memory; 6. Processor; 7. Display. DETAILED DESCRIPTION

[0056] The present invention will be further described in detail below through specific implementation modes in conjunction with the accompanying drawings.

[0057] Wherein similar elements in different embodiments have adopted associated similar element numbers.In the following embodiments, many detailed descriptions are for making the present invention be better understood.However, those skilled in the art can recognize without difficulty that some features can be omitted in different situations, or can be replaced by other elements, materials, methods.In some cases, some operations related to the present invention are not shown or described in the specification, and this is to avoid the core part of the present invention from being overwhelmed by too much description, and for those skilled in the art, it is not necessary to describe these related operations in detail, and they can fully understand the related operations according to the description in the specification and the general technical knowledge in the art.

[0058] In addition, the features, operations or characteristics described in the specification can be combined in any appropriate manner to form various implementations. At the same time, the steps or actions in the method description can also be interchanged or adjusted in a manner that is obvious to those skilled in the art. Therefore, the various sequences in the specification and the drawings are only for the purpose of clearly describing a certain embodiment and are not meant to be a required sequence unless otherwise specified that a certain sequence must be followed.

[0059] The serial numbers assigned to the components in the present invention, such as "S1", are only used to distinguish the objects described and do not have any order or technical meaning. The "connection" and "coupling" mentioned in the present invention include direct and indirect connections (couplings) unless otherwise specified.

[0060] Definitions of relevant terms involved in this invention:

[0061] (1) GNSS: Global Navigation Satellite System;

[0062] (2) SLAM: Simultaneously Localization and Mapping, also known as CMLConcurrent Mapping and Localization, concurrent mapping and positioning.

[0063] (3) Odometry: A measurement method that uses data from motion sensors to estimate the change in position over time. This method is used in many robotic systems to estimate the distance the robot has moved relative to its initial position.

[0064] (4) Loop detection: Also known as loop detection, it refers to the robot's ability to recognize that it has reached a certain scene and closed the map. In the SLAM mapping process, the visual odometry (VO) only considers key frames in adjacent time periods. The errors generated during this period will gradually accumulate and form cumulative errors. In this way, the long-term estimation results will be unreliable. Therefore, through the loop detection method, potential loops are discovered and used to correct drift errors to build a globally consistent trajectory and map.

[0065] (5) Point cloud: It is a set of point data consisting of a large number of three-dimensional coordinate points that together describe the surface of an object or scene. When one or more laser beams are irradiated onto the surface of an object, the reflected laser will carry information such as direction and distance. If the laser beam is scanned along a certain trajectory, the reflected laser point information will be recorded while scanning. Since the scanning is extremely precise, a large number of laser points can be obtained, thus forming a laser point cloud.

[0066] (6) Point cloud map construction: The laser radar scans the surrounding environment, and the SLAM algorithm estimates the current position and posture, converts the point cloud in its own coordinate system to the world coordinate system, thereby constructing a point cloud map.

[0067] (7) Point cloud segmentation: Point clouds are divided according to feature points such as space, geometry, and texture. Point clouds within the same segment have similar features. The purpose of point cloud segmentation is to divide the points into blocks so that they can be processed separately.

[0068] like Figure 1 As shown, a SLAM method for real-time elimination of dynamic points provided by the present invention comprises the following steps:

[0069] S1, respectively collecting three-dimensional laser point cloud and inertial measurement data through laser radar and IMU, restoring the global coordinates of the laser point cloud, superimposing the laser point cloud from the start to a preset time period to obtain an initial local map point cloud, and segmenting the plane from the initial local map point cloud to obtain an initial ground equation;

[0070] S101, collecting laser point cloud and inertial measurement data, using SLAM algorithm to restore the global coordinates of the laser point cloud, and superimposing the laser point cloud rows from the start to a preset time period to obtain an initial local map point cloud;

[0071] The SLAM algorithm is a SLAM algorithm based on iterative error Kalman filtering, also known as "SLAM algorithm based on FAST-LIO2" or "SLAM algorithm based on iterative Kalman filtering".

[0072] S102, leveling the laser point cloud based on the angle between the original measurement of the IMU accelerometer and the gravity acceleration constant to obtain a leveled laser point cloud;

[0073] The original measurement of the IMU accelerometer at the first scanning moment is recorded as a0, the gravity acceleration constant is g, and the rotation matrix R0 is constructed using the Rodrigues rotation formula. The rotation matrix R0 uses the IMU measurement to place the laser point cloud of any starting direction in an approximately horizontal direction. The calculation formula of the rotation matrix R0 is:

[0074]

[0075] In the formula, I is the unit matrix, k is the unit vector representing the direction, To transform vector k into its antisymmetric matrix, γ is the relative direction of a0 and g in three-dimensional space, and ∥∥ is the L2 norm.

[0076] S103, based on the laser radar scanning characteristics, selecting points in a certain area directly below the leveled laser point cloud as ground seed points, applying RANSAC to the ground seed points, and obtaining an initial ground equation of the terrain;

[0077] Points in a certain area directly below the leveled scan point cloud are selected as ground seed points. RANSAC is applied to the ground seed points to estimate the plane equation of the terrain:

[0078]

[0079] In the formula, are the initial estimates of the three components of the plane normal vector, is the initial estimated offset of the plane equation, representing the distance from the plane to the origin. x, y, z are the independent variables in the plane equation, representing the coordinates of a point in three-dimensional space.

[0080] S104. Filter initial ground points and non-ground points according to the distance from the points to the initial ground.

[0081] Use a filter with an angle threshold to determine whether points near the plane belong to the ground:

[0082]

[0083] Where, d k For point p k The distance to the rough estimate plane, p k is any laser scanning point, p0 is a virtual point directly below the laser radar; ∥∥ is the L2 norm, α Thr is a predefined angle threshold;

[0084] The calculation formula for p0 is:

[0085]

[0086] S2, progressively updating the ground parameters of the single-frame laser point cloud at the current moment according to the initial ground parameters and segmenting the ground points and non-ground points to obtain a ground point set and a non-ground point set;

[0087] S201, obtaining a relative change in the position and posture of the laser radar at the current moment compared to the previous moment according to the IMU motion integral;

[0088] The IMU measurements between two consecutive lidar scans are integrated (discrete time, first order hold) with the following iterative method:

[0089]

[0090] Where k is the IMU sampling time between the continuous lidar sampling time i-1 and i, k∈[i-1,i]; ω is the average angular velocity from time k to time k+1, is the original measured value of the angular velocity at time k, is the IMU angular velocity bias at time k, is the original measured value of the angular velocity at time k+1, is the attitude rotation matrix of the IMU from time i-1 to time k+1, is the attitude rotation matrix of IMU from time i-1 to time k+1, Δt is the sampling interval of IMU, a is the average acceleration from time k to time k+1, is the original measured value of acceleration at time k, is the IMU acceleration bias at time k, is the original measured value of acceleration at time k+1, is the displacement vector from the IMU coordinate system at time i-1 to time k+1, is the displacement vector from the IMU coordinate system at time i-1 to time k, is the velocity vector from the IMU coordinate system at time i-1 to time k, is the attitude rotation matrix transformed from the IMU coordinate system at time i-1 to the world coordinate system, g is the gravity vector constant in the world coordinate system, is the velocity vector from the IMU coordinate system at time i-1 to time k+1;

[0091] IMU gyro bias b ω and the accelerometer bias b a is modeled as a random walk whose derivative is Gaussian white noise.

[0092] When a new laser point cloud P i When it arrives, the transformation obtained by IMU integration compared to the previous scan is:

[0093]

[0094] In the formula, is the position transformation matrix of the laser radar from time i-1 to time i, is the attitude rotation matrix from IMU to lidar, is the attitude rotation matrix of IMU from time i-1 to time i, is the attitude rotation matrix from the laser radar to the IMU, is the displacement vector of IMU from time i-1 to time i, It is the translation external parameter from IMU to LiDAR; is the attitude rotation matrix of the laser radar from time i-1 to time i, It is the external parameter transformation between LiDAR and IMU;

[0095] External parameter conversion between LiDAR and IMU It is expressed as:

[0096]

[0097] In the formula, is the rotation matrix from the IMU coordinate system to the lidar coordinate system, is the translation vector from the IMU coordinate system to the lidar coordinate system;

[0098] S202, calculating the relative transformation of the ground according to the relative transformation of the posture, and updating the initial ground parameters;

[0099] The terrain plane at the previous moment f i-1 Used to obtain a current terrain plane f i The initial guess Current terrain plane f i The initial guess It is expressed as:

[0100]

[0101] In the formula, is the position transformation matrix of the laser radar from time i-1 to time i;

[0102] use Get the guessed ground equation:

[0103]

[0104] In the formula, are the predicted values ​​of the three components of the plane normal vector, x, y, z are the independent variables in the plane equation, representing the coordinates of a point in three-dimensional space, D i is the predicted offset of the plane equation;

[0105] S203, screening ground points and non-ground points according to the distance from the point to the ground;

[0106] Use the filter with angle threshold to filter the ground points again to get the ground point set of the i-th frame and non-ground point sets

[0107] S204, judging whether it is necessary to reinitialize the ground equation according to the number of ground points, and if so, re-estimating the ground equation based on the ground seed point method; if not, not reinitializing the ground equation.

[0108] The ground points are subjected to PCA to obtain the terrain plane equation that is progressively updated:

[0109] A i x+B i y+C i z+D i =0,

[0110] In the formula, A i , B i , C i are the updated values ​​of the three components of the plane normal vector, D i is the updated offset of the plane equation, which represents the distance from the plane to the origin. x, y, z are the independent variables in the plane equation, representing the coordinates of a point in three-dimensional space.

[0111] The ground point is obtained through PCA to obtain the terrain plane equation that is updated gradually, that is, the ground vector corresponding to the i-th frame

[0112] When the proportion of the number of extracted ground points in a single-frame point cloud is less than the threshold, the ground segmentation is reinitialized, and subsequent progressive updates will be performed on the basis of the reinitialization.

[0113] S3, comparing the non-ground point set with the local map point cloud point by point, voting for each point according to the source of the non-ground point's neighboring points to determine the motion attribute of the point, and completing the distinction between dynamic points and static points;

[0114] S301, performing coordinate transformation on non-ground points in the laser point cloud of the current frame according to the relative transformation of the posture obtained by the IMU motion integration;

[0115] First, the non-ground points Transform to obtain the predicted point cloud in the world coordinate system

[0116]

[0117] In the formula, is the non-ground point in the world coordinate system after transformation, and They are homogeneous transformation matrices The rotational and translational part, p nt is any non-ground point;

[0118] Homogeneous transformation matrix The calculation formula is:

[0119]

[0120] In the formula, the homogeneous transformation matrix represents the predicted pose in the world coordinate system at the i-th moment, is the optimal estimate of the pose at the i-1th moment, Integration from the IMU.

[0121] S302, constructing a KD-Tree for the local map point cloud, and performing a k-nearest neighbor search for the non-ground points after coordinate transformation;

[0122] Prediction point cloud in world coordinate system Point cloud with local map Merge to form a temporary local map point cloud Temporary local map point cloud It is expressed as:

[0123]

[0124] Predicted point cloud in world coordinate system and temporary local map point cloud Perform voxel downsampling to make The density is 2 times of the downsampled point Point cloud on local map Perform k nearest neighbor search in;

[0125] S303, voting for non-ground points according to the sources of k nearest neighbor points, if the nearest neighbor point comes from the non-ground point of the current frame, it obtains one vote, if the nearest neighbor point comes from the local map point cloud, it does not obtain a vote;

[0126] Get N by k nearest neighbor search kNN neighbor points, if p kNN A nearest neighbor point of comes from the current non-ground point set. kNN You will get one ballot, otherwise you will not get any ballot.

[0127] S304. Determine whether the point is a dynamic point according to the voting rate of the non-ground point, and identify the point whose voting rate is greater than the threshold as a dynamic point, and the point whose voting rate is less than or equal to the threshold as a static point.

[0128] The voting rate of point p is defined as the set of its neighboring points The ratio of the total number of votes to the number of points searched by kNN is expressed as:

[0129]

[0130] In the formula, vr() is the voting rate, and vote() is p kN The votes of the point, N kNN is the number of neighboring points;

[0131] Points with voting rates greater than the threshold are considered dynamic points, and vice versa, static points. The threshold is 0.2. The threshold is determined by: and local map point cloud The k-nearest neighbor search process is modeled, and the calculation ideally comes from The proportion of neighboring points seIf (N kNN ) with N kNN Changing relationships:

[0132]

[0133] When N kN ∈(0,124], r self (N kN ) is:

[0134]

[0135] Considering 1) N kN There is no need to make it too large; 2) In actual situations, the points scanned by the laser do not cover the entire three-dimensional space, so a more relaxed threshold of 0.2 is set.

[0136] S4, performing frame-to-frame matching between ground points and static points to obtain the current laser radar's position information, and performing map fusion according to the position information to obtain a three-dimensional point cloud map;

[0137] S401, using the relative transformation of the posture obtained by the IMU motion integration as the prediction value, using the sum of the distance from the ground point to the ground equation and the distance from the static point to the nearest neighbor plane as the residual, and using iterative error Kalman filtering to solve the optimal estimate of the posture change;

[0138] Record any point in the current scan point cloud:

[0139]

[0140] In the formula, is the homogeneous coordinate of p, p t is the transpose of the three-dimensional coordinate vector of point p;

[0141] When p is a ground point, the residual is calculated using the ground equation The calculation formula is:

[0142]

[0143] In the formula, e terr () is the residual model of the ground point, is the ground equation at time i; is the homogeneous transformation matrix, which represents the predicted pose in the world coordinate system at the i-th moment; is the homogeneous coordinate of p, ∈ is a predefined threshold used to remove outliers, set to an empirical value of 0.2, is the set of ground points at time i;

[0144] When p is a static point, the KD-Tree of the local map point cloud is used to search for neighboring points to form a plane. This is used to calculate the residual

[0145]

[0146] In the formula, e sta () is the residual model of the static point, are the homogeneous coordinates of p, For plane The transpose of the equation, is the homogeneous transformation matrix, which represents the predicted pose in the world coordinate system at the i-th moment. is the static point set at time i;

[0147] Using IMU integration results and residuals to solve the optimal pose estimation in the iterative error Kalman filter framework

[0148]

[0149] In the formula, is the set of ground points at time i, is the static point set at time i;

[0150] S402, restoring the laser point cloud data to a global coordinate system according to the position information, fusing the data into a local map point cloud, and removing the earliest frame of point cloud from the local map point cloud;

[0151] Laser point cloud of the current frame pass Convert to the world coordinate system to get

[0152] S403: Fusing the local map point clouds according to the posture information to obtain a three-dimensional point cloud map.

[0153] Point cloud with local map Merge to form the local map point cloud at the i-th moment With global map Merge to form the complete map at time i

[0154] like Figure 2 As shown, a SLAM device for real-time dynamic point elimination provided by the present invention is used to implement a SLAM method for real-time dynamic point elimination, comprising:

[0155] Initialization unit 1, used for collecting laser point cloud and inertial measurement data, restoring the global coordinates of the laser point cloud, superimposing the laser point cloud from the start to a preset time period to obtain an initial local map point cloud, and segmenting the plane from the initial local map point cloud to obtain an initial ground equation;

[0156] A ground segmentation unit 2 is used to progressively update the ground parameters of the single-frame laser point cloud at the current moment according to the initial ground parameters and segment the ground points and non-ground points to obtain a ground point set and a non-ground point set;

[0157] The dynamic point detection unit 3 is used to compare the non-ground point set with the local map point cloud point by point, and vote for each point according to the source of the non-ground point's neighboring points to determine the motion attribute of the point, thereby distinguishing between dynamic points and static points;

[0158] The pose estimation and map fusion unit 4 is used to perform frame-to-frame matching between ground points and static points to obtain the pose information of the current laser radar, and perform map fusion based on the pose information to obtain a three-dimensional point cloud map.

[0159] like Figure 3 As shown, a SLAM electronic device for real-time dynamic point elimination provided by the present invention includes a memory 5 for storing programs, a processor 6 for executing the program after receiving an execution instruction, a computer program stored in the memory 5 and executable on the processor 6, and a display 7 for displaying the process of the processor 6 executing a SLAM method for real-time dynamic point elimination and various information of the electronic device. When the processor 6 executes the program, the SLAM method for real-time dynamic point elimination is implemented.

[0160] The memory 5 is a random access memory (RAM), a read-only memory (ROM), a secure digital (SD card), a flash card, etc. The processor 6 is an integrated circuit chip with signal processing capabilities. The processor 6 is a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC) or any conventional processor. General-purpose processors include a central processing unit (CPU), a network processor (NP), a microprocessor, etc. The methods, steps and logic block diagrams disclosed in the embodiments of the present invention can be implemented or executed. The process definition method disclosed in any of the aforementioned embodiments of the present invention can be applied to the processor 6, or implemented by the processor 6. The display 7 is an LED display, a liquid crystal display or a touch-screen display, etc. Figure 3 The structure shown is only a schematic diagram of the structure of the electronic device. The electronic device may also include Figure 3 More or fewer components as shown and Figure 3 Each component shown in the figure can be implemented by hardware, software or a combination thereof.

[0161] The above specific examples are used to illustrate the present invention, which is only used to help understand the present invention and is not intended to limit the present invention. For those skilled in the art, according to the idea of ​​the present invention, several simple deductions, modifications or substitutions can be made.

Claims

1. A SLAM method for eliminating dynamic points in real time, characterized in that: include: Collecting laser point cloud and inertial measurement data, restoring the global coordinates of the laser point cloud, superimposing the laser point cloud from the start to a preset time period to obtain an initial local map point cloud, and segmenting the plane from the initial local map point cloud to obtain an initial ground equation; According to the initial ground parameters, the ground parameters of the single-frame laser point cloud at the current moment are progressively updated and ground points and non-ground points are segmented to obtain a ground point set and a non-ground point set; Compare the non-ground point set with the local map point cloud point by point, vote for each point according to the source of the non-ground point's neighboring points to determine the motion attribute of the point, and distinguish between dynamic points and static points; The ground points and static points are matched between frames to obtain the current LiDAR posture information, and map fusion is performed based on the posture information to obtain a three-dimensional point cloud map.

2. A SLAM method for real-time elimination of dynamic points according to claim 1, characterized in that: The collecting of laser point cloud and inertial measurement data, restoring the global coordinates of the laser point cloud, superimposing the laser point cloud from the start to a preset time period to obtain an initial local map point cloud, and segmenting the plane from the initial local map point cloud to obtain an initial ground equation includes: Collect laser point cloud and inertial measurement data, use SLAM algorithm to restore the global coordinates of laser point cloud, and superimpose the laser point cloud from the start to the preset time period to obtain the initial local map point cloud; Leveling the laser point cloud based on an angle between an original measurement of an IMU accelerometer and a gravitational acceleration constant to obtain a leveled laser point cloud; Selecting points in a certain area directly below the leveled laser point cloud as ground seed points, applying RANSAC to the ground seed points to obtain an initial ground equation of the terrain; Filter initial ground points and non-ground points based on the distance from the point to the initial ground.

3. A SLAM method for real-time elimination of dynamic points according to claim 2, characterized in that: The leveling of the laser point cloud based on the angle between the original measurement of the IMU accelerometer and the gravity acceleration constant includes: The original measurement of the IMU accelerometer at the first scanning moment is recorded as a0, the gravity acceleration constant is g, and the rotation matrix R0 is constructed using the Rodriguez rotation formula. The calculation formula of the rotation matrix R0 is: In the formula, I is the unit matrix, k is the unit vector representing the direction, To transform vector k into its antisymmetric matrix, γ is the relative direction of a0 and g in three-dimensional space, and ∥∥ is the L2 norm.

4. A SLAM method for real-time elimination of dynamic points according to claim 1, characterized in that: The step of progressively updating the ground parameters of the single-frame laser point cloud at the current moment according to the initial ground parameters and segmenting the ground points and non-ground points to obtain the ground point set and the non-ground point set comprises: According to the IMU motion integral, the relative change of the LiDAR’s current position and posture compared to the previous moment is obtained; Inferring the relative transformation of the ground according to the relative transformation of the posture, and updating the initial ground parameters; Filter ground points and non-ground points based on the distance from the point to the ground; The need to reinitialize the ground equation is determined based on the number of ground points. If necessary, the ground equation is re-estimated based on the ground seed point method.

5. A SLAM method for real-time elimination of dynamic points according to claim 4, characterized in that: The relative transformation of the laser radar's posture at the current moment compared to the previous moment obtained according to the IMU motion integral includes: Integrate the IMU measurements between two consecutive lidar scans: Where k is the IMU sampling time between the continuous lidar sampling time i-1 and i, k∈[i-1,i]; ω is the average angular velocity from time k to time k+1, is the original measured value k of the angular velocity at time k, is the IMU angular velocity bias at time k, is the original measured value of the angular velocity at time kk+1, is the attitude rotation matrix of the IMU from time i-1 to time k+1, is the attitude rotation matrix of IMU from time i-1 to time k+1, Δt is the sampling interval of IMU, a is the average acceleration from time k to time k+1, is the original measured value of acceleration at time k, is the IMU acceleration bias at time k, is the original measured value of acceleration at time k+1, is the displacement vector from the IMU coordinate system at time i-1 to time k+1, is the displacement vector from the IMU coordinate system at time i-1 to time k, is the velocity vector from the IMU coordinate system at time i-1 to time k, is the attitude rotation matrix transformed from the IMU coordinate system at time i-1 to the world coordinate system, g is the gravity vector constant in the world coordinate system, is the velocity vector from the IMU coordinate system at time i-1 to time k+1; When a new laser point cloud P i When it arrives, the transformation obtained by IMU integration compared to the previous scan is: In the formula, is the position transformation matrix of the laser radar from time i-1 to time i, is the attitude rotation matrix from IMU to lidar, is the attitude rotation matrix of IMU from time i-1 to time i, is the attitude rotation matrix from the laser radar to the IMU, is the displacement vector of IMU from time i-1 to time i, It is the translation external parameter from IMU to LiDAR; is the attitude rotation matrix of the laser radar from time i-1 to time i, It is the external parameter transformation between LiDAR and IMU.

6. A SLAM method for real-time elimination of dynamic points according to claim 1, characterized in that: The non-ground point set is compared point by point with the local map point cloud, and each point is voted according to the source of the non-ground point's neighboring points to determine the motion attribute of the point, so as to distinguish between dynamic points and static points. The method comprises: According to the relative transformation of the posture obtained by the IMU motion integration, the coordinates of the non-ground points in the laser point cloud of the current frame are transformed; Construct a KD-Tree for the local map point cloud and perform k-nearest neighbor search for the non-ground points after coordinate transformation; Voting for non-ground points based on the sources of k nearest neighbor points. If the nearest neighbor point comes from a non-ground point in the current frame, it gets one vote. If the nearest neighbor point comes from a local map point cloud, it does not get a vote. Whether a point is a dynamic point is determined based on the voting rate of the non-ground point. Points with a voting rate greater than a threshold are considered dynamic points, and points with a voting rate less than or equal to the threshold are considered static points.

7. A SLAM method for real-time elimination of dynamic points according to claim 6, characterized in that: The construction of KD-Tree for the local map point cloud and the k-nearest neighbor search for the non-ground points after coordinate transformation include: The predicted point cloud in the world coordinate system Point cloud with local map Merge to form a temporary local map point cloud Temporary local map point cloud It is expressed as: Point In the local point cloud map Perform k nearest neighbor search to get N kNN neighbor points.

8. A SLAM method for real-time elimination of dynamic points according to claim 1, characterized in that: The step of performing frame matching between ground points and static points to obtain the position and posture information of the current laser radar, and performing map fusion according to the position and posture information to obtain a three-dimensional point cloud map includes: The relative transformation of the posture obtained by the IMU motion integration is used as the prediction value, the sum of the distance from the ground point to the ground equation and the distance from the static point to the nearest neighbor plane is used as the residual, and the iterative error Kalman filter is used to solve the optimal estimate of the posture change; Restoring the laser point cloud data to a global coordinate system according to the pose information, fusing the data into a local map point cloud, and removing the earliest frame of point cloud from the local map point cloud; The local map point clouds are fused according to the posture information to obtain a three-dimensional point cloud map.

9. A SLAM device for real-time dynamic point removal, characterized in that: A SLAM method for eliminating dynamic points in real time according to any one of claims 1 to 8, comprising: An initialization unit (1) is used to collect laser point clouds and inertial measurement data, restore the global coordinates of the laser point clouds, superimpose the laser point clouds from the start to a preset time period to obtain an initial local map point cloud, and segment a plane from the initial local map point cloud to obtain an initial ground equation; A ground segmentation unit (2) is used to progressively update the ground parameters of a single frame of laser point cloud at the current moment according to the initial ground parameters and segment the ground points and non-ground points to obtain a ground point set and a non-ground point set; A dynamic point detection unit (3) is used to compare the non-ground point set with the local map point cloud point by point, and to vote for each point according to the source of the non-ground point's neighboring points to determine the motion attribute of the point, thereby distinguishing between dynamic points and static points; The pose estimation and map fusion unit (4) is used to perform frame-to-frame matching between ground points and static points to obtain the pose information of the current laser radar, and to perform map fusion based on the pose information to obtain a three-dimensional point cloud map.

10. A SLAM electronic device for real-time dynamic point removal, comprising a memory (5), a processor (6), and a computer program stored in the memory (5) and executable on the processor (6), characterized in that: When the processor (6) executes the program, a SLAM method for real-time elimination of dynamic points as claimed in any one of claims 1 to 8 is implemented.

Images