A laser slam method and system that combines ground constraints with dynamic point cloud removal

Abstract

The application discloses a laser method and system combining ground constraint and dynamic point cloud removal, comprising the following steps: segmenting point cloud data into a plurality of local areas, distinguishing ground areas and non-ground areas according to the covariance matrix C of the point cloud data of the local areas and geometric centers, determining and removing the local areas corresponding to dynamic non-ground areas according to the eigenvalues of each frame of the points in the non-ground areas in the space-time neighborhood λ、 The global nonlinear factor graph optimization model is solved based on the pose residual error calculated from the point cloud data of the removed dynamic non-ground areas, the displacement residual error calculated from the point cloud data, the ground constraint residual error calculated from the point cloud data of the ground areas, and the loop closure residual error calculated from the data between the current position and historical frames. The application effectively eliminates dynamic environment interference, suppresses the Z-axis elevation cumulative error under long-distance operation by increasing ground constraint, and improves the mapping and positioning precision.

Description

Technical Field

[0001] This invention relates to the field of mobile robot localization and mapping, and more specifically, to a laser-based method combining ground constraints and dynamic point cloud removal. Methods and systems. Background Technology

[0002] With the rapid development of autonomous driving and mobile robot technologies, Simultaneous Localization and Mapping (SLAM) has become a core technology for achieving autonomous navigation. Among them, 3D laser SLAM has become the mainstream solution in complex outdoor environments due to its robustness to changes in lighting and its ability to acquire high-precision 3D environmental information. However, existing laser SLAM systems face two major challenges in practical applications.

[0003] First, traditional laser SLAM generally relies on the assumption of a strongly static environment. However, real-world urban roads or campus environments are filled with dynamic objects such as vehicles and pedestrians. Dynamic features can easily disrupt the geometric consistency of point cloud registration, leading to drift in front-end pose estimation and leaving "ghosting" traces in the global map. Existing dynamic removal methods are either computationally too time-consuming to meet real-time requirements (e.g., batch processing methods) or prone to misclassification.

[0004] Secondly, in scenarios with degraded geometric features, such as long-distance driving or narrow tunnels, lidar struggles to extract sufficient constraints, easily leading to severe cumulative errors and divergence on the Z-axis elevation, commonly known as "heading up" or "sinking down". Summary of the Invention

[0005] This invention provides a laser SLAM method, system, and device that combines ground constraints with dynamic point cloud removal, aiming to solve the shortcomings of existing technologies that are prone to feature mismatch in dynamic scenes and Z-axis drift and divergence during long-distance operation.

[0006] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows: A laser SLAM method combining ground constraints and dynamic point cloud removal includes segmenting the point cloud, projecting the point cloud data acquired by the mobile device into planar data, and dividing the planar data into several local regions with concentric fan-ring structures. Separate the ground point cloud and determine the covariance matrix C and geometric center of the point cloud data in the local area using principal component analysis. The unit eigenvector corresponding to the smallest eigenvalue in the covariance matrix C is defined as the normal vector of the point cloud region. n m,k Based on normal vector n m,k Corresponding calculated included angle and geometric center The calculated sensor heights together distinguish between ground and non-ground areas; Remove dynamic point clouds and obtain spatiotemporal neighborhood point sets for points within non-ground regions. These spatiotemporal neighborhood point sets are then determined using principal component analysis to identify eigenvalues ​​λ representing the diffusion degree of point cloud data across different frames within the same region, and the normal vector n for each frame. Based on these eigenvalues... λ、 The normal vector n calculates the dynamic score of the point's movement over several consecutive frames, and dynamic points are determined and eliminated based on the dynamic score; For localization and mapping, a global nonlinear factor graph optimization model is constructed for continuous localization and mapping, and the solution is obtained based on point cloud data after removing dynamic non-ground regions.

[0007] Further, the normal vector is calculated. n m,k Corresponding angle, geometric center The formula corresponding to the sensor height is: , , in, e z =[0,0,1] T This is the vertical axis vector of the world coordinate system. H sensor The installation height of the lidar relative to the ground. θ th Indicates the threshold of the normal vector tilt angle. h th This indicates the height threshold of the lidar.

[0008] Furthermore, the separation of the ground point cloud includes: dividing the locally high-consistent areas into ground regions based on the consistency of their sensor heights in adjacent local areas.

[0009] Furthermore, the function for calculating the dynamic score is: , in, , These are the weighting coefficients. and Let these represent the maximum and minimum eigenvalues ​​corresponding to the i-th point, respectively. Let represent the normal vector of the i-th point in the current frame. Let represent the normal vector corresponding to the minimum eigenvalue of the i-th point.

[0010] Furthermore, the localization and mapping includes: a global nonlinear factor graph optimization model is jointly solved based on point cloud data acquired by lidar, IMU data acquired by inertial sensors, and timekeeping data acquired by wheel counters to achieve localization and mapping.

[0011] Furthermore, the constraint factor graph of the global nonlinear factor graph optimization model includes pose residuals calculated based on point cloud data excluding dynamic non-ground regions, displacement residuals calculated based on programmable data, ground constraint residuals calculated based on point cloud data of ground regions, and loop closure residuals calculated based on data between the current frame and historical frames at the current position.

[0012] Furthermore, the function of the ground constraint residual is: , in, Represents the global normal vector of a local area of ​​the ground region. The parameters Roll and Pitch represent the roll and pitch angles of the mobile device's estimated attitude in the Euler angles. This represents the estimated Z-axis elevation of the mobile device in the world coordinate system. This represents the theoretical height of the sensor calculated using local ground plane equations and ground area point cloud data.

[0013] Furthermore, the displacement residual is obtained by: acquiring the cumulative pulse count of the encoders of the left and right wheels of the mobile device, calculating the linear displacement increment and heading angle increment of the mobile device body in a very short time, using the average heading angle of the previous moment and the current moment as the median heading angle, and projecting the cumulative relative motion into a high-frequency odometer pose transformation in the global coordinate system, which is then introduced into the constraint factor diagram as the displacement residual.

[0014] A laser SLAM system combining ground constraints and dynamic point cloud removal includes a data preprocessing module for acquiring multi-source sensor data and using IMU data to perform distortion removal on point cloud data. The segmentation and dynamic removal module is used to segment point cloud data and remove point cloud data corresponding to dynamic objects; The solver module is used to construct a global nonlinear factor graph optimization model and a constraint factor graph, and to perform joint optimization to solve for pose and build the graph.

[0015] A terminal device includes a processor, a memory, and a computer program stored in the memory and executable on the processor.

[0016] The beneficial effects of this invention are: 1. Enhancing robustness in dynamic environments and map purity: This invention proposes a dynamic point cloud removal algorithm based on spatiotemporal normal vector consistency. By analyzing the surface thickness and deflection of features in the spatiotemporal dimension, it accurately identifies and removes moving interference such as pedestrians and vehicles in real time, avoiding feature mismatch from the source and successfully constructing a high-precision static skeleton map without artifacts.

[0017] 2. Effectively suppresses Z-axis elevation drift (divergence): This invention fully explores and utilizes physical ground geometry information, creatively introducing a strong ground constraint factor in the backend factor graph. This factor applies a virtual "gravity alignment" force to the system, keeping the robot trajectory tightly locked on the ground. In long-distance testing, the Z-axis error was reduced to 0.264m, solving the persistent problems of traditional SLAM robots easily "lifting" or "sinking".

[0018] 3. High-precision and stable positioning in degraded scenarios: By introducing a pre-integration factor for the wheeled odometry optimized by median integration, the system can still maintain reliable relative displacement constraints in environments with severe rotation or long straight corridors where laser features are missing. In tests on public datasets and real campus scenarios, the positioning accuracy (APE) of this invention is up to 71.9% higher than mainstream algorithms (such as FAST-LIO2 and LeGO-LOAM). Attached Figure Description

[0019] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings: Figure 1 This is a system algorithm framework structure diagram according to an embodiment of the present invention; Figure 2 This is a schematic diagram of ground segmentation space division combined with the concentric zone model (CZM) in an embodiment of the present invention; Figure 3 This is a schematic diagram illustrating the comparison of the spatiotemporal neighborhood distribution of static and dynamic objects in an embodiment of the present invention. Figure 4 This is a schematic diagram of the back-end factor graph structure of multi-source constraint coupling in an embodiment of the present invention; Figure 5 This is a geometric constraint diagram of the strong ground constraint factor in this embodiment of the invention. Figure 6 This is the ground segmentation effect for areas where the bottom surface was not identified; Figure 7 This is the ground segmentation effect after identifying the bottom area using the algorithm of this invention; Figure 8 It is a static skeleton map before dynamic objects are removed; Figure 9 It is a static skeleton map after removing dynamic objects using the algorithm of this invention. Detailed Implementation

[0020] The technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0021] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the specification of this invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed terms.

[0022] This invention provides a laser SLAM method that combines ground constraints with dynamic point cloud removal, such as... Figure 1 As shown, this includes acquiring multi-source sensor data preprocessing and point cloud distortion correction: acquiring data from a 3D lidar (10Hz), IMU (100Hz), and wheeled odometer (50Hz). Assuming the IMU maintains uniform acceleration and uniform angular velocity motion within small time intervals, discrete-time kinematics derivation is performed using the IMU's high-frequency measurement data. This is because the sampling times of each point within a lidar scan frame... t i Located between two adjacent IMU measurement times, linear interpolation is used to obtain... t i The precise motion state at any given moment is obtained, and the relative rotation and translation with respect to the start of the frame are calculated. Based on this, the collected laser points are transformed from the "radar frame" to the "IMU frame," and after motion compensation, they are returned to the lidar coordinate system, effectively eliminating point cloud distortion caused by vehicle motion.

[0023] Point cloud segmentation: Projecting point cloud data acquired by mobile devices into planar data, and dividing the planar data into several local regions with concentric fan-ring structures.

[0024] like Figure 2 As shown, the distortion-free 3D point cloud data is projected onto a horizontal plane and then meshed using a polar coordinate system, resulting in the following divisions: M Each region is divided into several concentric ring regions, and each region is further divided into multiple sectors along the azimuth angle to form several local regions with concentric sector ring structures.

[0025] Separate the ground point cloud and determine the covariance matrix C and geometric center of the point cloud data in the local area using principal component analysis. The unit eigenvector corresponding to the smallest eigenvalue in the covariance matrix C is defined as the normal vector of the point cloud region. n m,k Based on normal vectorn m,k Corresponding calculated included angle and geometric center The calculated sensor heights together distinguish between ground and non-ground areas.

[0026] Assuming local region S m,k The point set of the point cloud data within is In this local area S m,k Principal component analysis (PCA) is applied to calculate the geometric centers of the point sets. With covariance matrix C : , Where N represents the number of point cloud data within the point set.

[0027] For covariance matrix C Perform eigenvalue decomposition and extract the unit eigenvector corresponding to the smallest eigenvalue as the normal vector of the local plane. n m,k .

[0028] To verify the effectiveness of the plane fitting results and filter out vertical structures such as curbs and flower beds, a threshold for the normal vector tilt angle was set. θ th and sensor height threshold h th Perform geometric constraint verification. If the sector meets the following conditions, it is marked as a candidate ground region: , , In the formula, e z =[0,0,1] T This is the vertical axis vector of the world coordinate system. H sensor This refers to the installation height of the lidar relative to the ground.

[0029] Furthermore, by combining the height consistency check of adjacent local regions, i.e. comparing the planar parameters of adjacent local regions, if there is a step change in height, the points of the lower plane are retained according to the vehicle passability principle, and finally the original point cloud is collected. P raw Strictly separate traffic into ground-based point clusters G k Non-ground point clusters N k It meets the following conditions: , .

[0030] Remove dynamic point clouds and obtain spatiotemporal neighborhood point sets for points within non-ground regions. These spatiotemporal neighborhood point sets are then determined using principal component analysis to identify eigenvalues ​​λ representing the diffusion degree of point cloud data across different frames within the same region, and the normal vector n for each frame. Based on these eigenvalues... λ、 The normal vector n calculates the dynamic score of the point's movement over several consecutive frames, and dynamic points are determined and eliminated based on the dynamic score.

[0031] Mobile devices continuously collect point cloud data during continuous movement to obtain non-ground point sets. N k Its W A local sliding window map of frame history point clouds. (Targeting...) N k any point in p i The KD-Tree is used to search for the spatiotemporal neighborhood point set in the window map (a point has several point clouds in its adjacent space, and the point cloud data in several consecutive frames are used to form the spatiotemporal neighborhood point set).

[0032] Principal component analysis was performed on the spatiotemporal neighborhood to extract eigenvalues. λ and the normal vector n. Where, λ This reflects the degree of dispersion of the point set in the normal direction (i.e., as shown in the image). Figure 3 (Surface thickness shown). At any point, its adjacent space contains static objects. As the moving device gradually approaches the static objects, the dispersion increases uniformly over time (e.g., ...). Figure 3 (Left side); If a dynamic object exists in the adjacent space of any point, and the dispersion degree changes abruptly after the dynamic object leaves the adjacent space (e.g., ... Figure 3 (The green on the right).

[0033] Therefore, the normal vector corresponding to the smallest eigenvalue is denoted as the spatiotemporal normal vector of that point. n i st Based on this, a dynamic confidence scoring function is designed: , in, , These are the weighting coefficients. and Let these represent the maximum and minimum eigenvalues ​​corresponding to the i-th point, respectively. Let represent the normal vector of the i-th point in the current frame. Let represent the normal vector corresponding to the minimum eigenvalue of the i-th point.

[0034] If a static object has its historical observation points with overlapping spatial heights ( λ 3The score tends to 0 and the normal vector remains unchanged; however, for dynamic targets (such as vehicles and pedestrians), trailing diffusion and normal vector deflection occur, leading to an increase in the score. Points with scores exceeding the threshold are discarded, and only pure static skeleton points are output.

[0035] For localization and mapping, a global nonlinear factor graph optimization model is constructed for continuous localization and mapping, and the solution is obtained based on point cloud data after removing dynamic non-ground regions.

[0036] Localization and mapping involve jointly solving a global nonlinear factor graph optimization model based on point cloud data acquired by lidar, IMU data acquired by inertial sensors, and speed measurement data acquired by wheel counters to achieve localization and mapping. For example... Figure 4 As shown, the constraint factor graph of the global nonlinear factor graph optimization model includes pose residuals calculated based on point cloud data after removing dynamic non-ground regions, displacement residuals calculated based on programmable data, ground constraint residuals calculated based on point cloud data of ground regions, and loop closure residuals calculated based on data between the current frame and historical frames at the current position.

[0037] The local ground global normal vector extracted from the front-end segmentation As the absolute attitude constraint input, the pose quaternion of the node state is converted into Euler angles, and the function of the ground constraint residual is: , Represents the global normal vector of a local area of ​​the ground region. The parameters Roll and Pitch represent the roll and pitch angles of the mobile device's estimated attitude in the Euler angles. This represents the estimated Z-axis elevation of the mobile device in the world coordinate system. This represents the theoretical sensor height calculated using local ground plane equations and ground area point cloud data. The ground constraint residuals provide a virtual "gravity alignment" force in the factor map, enabling the algorithm to completely eliminate drift or depression on the Z-axis over long distances or in corridors with uniform geometric features.

[0038] The displacement residual is obtained by acquiring the cumulative pulse count of the encoders of the left and right wheels of the mobile device, calculating the linear displacement increment and heading angle increment of the mobile device body in a very short time, using the average heading angle of the previous moment and the current moment as the median heading angle, and projecting the cumulative relative motion into a high-frequency odometer pose transformation in the global coordinate system, which is then introduced into the constraint factor diagram as the displacement residual.

[0039] One embodiment of this application is as follows: A second-order Runge-Kutta method (median integration strategy) is employed, utilizing the average heading angle between the previous and current time points. θ midThe method calculates the projection of the current position. It effectively reduces the truncation error introduced by the traditional first-order Euler integral and outputs a high-frequency relative pose transformation, which is used as a pre-integration factor for back-end optimization.

[0040] Construct a global nonlinear factor graph optimization model. State nodes include the robot's pose, velocity, and IMU bias. Add the following observation residuals to the factor graph: (1) Lidar-IMU residual: relative pose constraints calculated based on static skeleton points and ground points after removing dynamic objects; (2) Wheel speed odometer residual: displacement constraint derived from median integral; (3) Loopback residual: triggered by global loop closure detection.

[0041] (4) Strong ground constraint residuals.

[0042] All residuals are solved globally by minimizing the Mahalanobis distance, ultimately outputting a high-precision optimized trajectory and a clean global point cloud map.

[0043] A laser SLAM system combining ground constraints and dynamic point cloud removal includes: a data preprocessing module for acquiring multi-source sensor data and using IMU data to perform distortion correction on point cloud data; a segmentation and dynamic removal module for segmenting point cloud data and removing point cloud data corresponding to dynamic objects; and a solution module for constructing a global nonlinear factor graph optimization model and a constraint factor graph, and performing joint optimization to solve for pose and map generation.

[0044] A terminal device includes a processor, a memory, and a computer program stored in the memory and executable on the processor.

[0045] The specific implementation process and beneficial effects of this application plan are as follows: Use such as Figure 5 The mobile device shown is used to verify the effectiveness of the robust laser SLAM method (DWG-LIO) combining strong ground constraints and dynamic point cloud removal described in this invention, and comparative experiments are conducted using the internationally publicly available KITTI dataset.

[0046] Figure 6 and Figure 7 The images show the segmentation results before and after the improvement. Figure 6 As can be seen from the area marked by the red box, the traditional method before the improvement exhibits significant under-segmentation when segmenting undulating or complex terrain. Some non-ground points (such as curbs and low obstacles) are mixed with ground points, containing a lot of noise; while from... Figure 7As can be seen from the area marked by the red box, after adopting the improved ground segmentation strategy of this invention, outliers in the red box area are effectively removed, and the ground point cloud is accurately and cleanly separated, verifying the robustness of the segmentation algorithm of this invention, thus providing a reliable data foundation for the backend to construct strong ground constraint factors.

[0047] Figure 8 and Figure 9 The images show a comparison of the static skeleton maps constructed before and after dynamic point cloud culling. Figure 8 Before removing the dynamic point cloud, the map's outline edges exhibit obvious "trailing" and "ghosting" phenomena (appearing as divergent redundant point clouds) due to the movement interference of dynamic objects such as pedestrians and vehicles in the environment, severely damaging the map's geometric structure; while Figure 9 After removing dynamic point clouds, these dynamic interference features were successfully filtered out, and the constructed static skeleton map retained a clear and sharp static skeleton structure, significantly improving the consistency and mapping accuracy of the global map in complex dynamic environments.

[0048] The experiment selected four typical sequences (00, 05, 07, 10) from the KITTI dataset, covering long-distance urban roads, closed-loop routes in residential areas, high-speed dynamic scenarios, and complex road conditions. The comparison algorithms selected were the currently mainstream laser SLAM algorithms LIO-SAM (the KITTI00 sequence data was abnormal and not included in the table), LeGO-LOAM, and FAST-LIO2. The evaluation metrics used were absolute trajectory error (APE, used to evaluate global consistency) and relative trajectory error (RPE, used to evaluate local drift rate).

[0049] 1. Verification of global positioning accuracy Table 1 shows a comparison of the overall localization errors of each algorithm on the KITTI dataset: Experimental data analysis shows that in the KITTI 07 sequence, which contains many high-speed moving vehicles, the system effectively filters out erroneous matches caused by dynamic objects, resulting in an APE of only 0.526m, which is more than 50% higher than the positioning accuracy of FAST-LIO2 (1.157m). In the KITTI 00 sequence, which has the longest path and is prone to divergence, the present invention controls the APE to 3.527m through multi-source fusion, which is about 71.9% higher than the accuracy of FAST-LIO2 (12.571m).

[0050] 2. Z-axis (elevation) anti-drift verification: To verify the effectiveness of the "strong ground constraint factor" in suppressing high-frequency cumulative errors in this invention, error data along the Z-axis (vertical direction) was extracted separately for comparison, as shown in Table 2: Experimental data analysis shows that in the long-range closed-loop KITTI00 and 05 sequences, LeGO-LOAM, while locally smooth, suffers from severe global cumulative drift (Z-axis APE as high as 4.209m). This invention reduces the Z-axis APE of KITTI 00 to 0.493m. In the KITTI10 sequence with its dramatic terrain undulations, by introducing ground constraints to correct the absolute attitude, the Z-axis APE of this invention is only 1.049m, effectively avoiding the "sinking" or divergence phenomena that occur in other algorithms under complex slopes. This fully demonstrates that this invention achieves robust vertical positioning across all scenarios while ensuring local accuracy.

[0051] The specific embodiments of the present invention have been described in detail above with reference to the examples. However, the present invention is not limited to the above embodiments. Within the scope of knowledge possessed by those skilled in the art, various changes can be made without departing from the spirit of the present invention.

Claims

1. A laser SLAM method combining ground constraints and dynamic point cloud removal, characterized in that, include: Point cloud segmentation involves projecting point cloud data acquired by mobile devices into planar data and dividing the planar data into several local regions with concentric fan-ring structures. Separate the ground point cloud and determine the covariance matrix C and geometric center of the point cloud data in the local area based on principal component analysis. The unit eigenvector corresponding to the smallest eigenvalue in the covariance matrix C is defined as the normal vector of the point cloud region. n m,k Based on normal vector n m,k Corresponding calculated included angle and geometric center The calculated sensor heights together distinguish between ground and non-ground areas; Remove dynamic point clouds and obtain spatiotemporal neighborhood point sets for points within non-ground regions. These spatiotemporal neighborhood point sets are then determined using principal component analysis to identify eigenvalues ​​λ representing the diffusion degree of point cloud data across different frames within the same region, and the normal vector n for each frame. Based on these eigenvalues... λ、 The normal vector n calculates the dynamic score of the point's movement over several consecutive frames, and dynamic points are determined and eliminated based on the dynamic score; For localization and mapping, a global nonlinear factor graph optimization model is constructed for continuous localization and mapping, and the solution is obtained based on point cloud data after removing dynamic non-ground regions.

2. The laser SLAM method combining ground constraints and dynamic point cloud removal according to claim 1, characterized in that, Calculate the normal vector n m,k Corresponding angle, geometric center The formula corresponding to the sensor height is: ， ， in, e z =[0,0,1] T This is the vertical axis vector of the world coordinate system. H sensor The installation height of the lidar relative to the ground. θ th Indicates the threshold of the normal vector tilt angle. h th This indicates the height threshold of the lidar.

3. The laser SLAM method combining ground constraints and dynamic point cloud removal according to claim 1, characterized in that, The separation of ground point clouds includes: dividing lower local areas with consistent sensor heights into ground regions based on the consistency of sensor heights in adjacent local areas.

4. The laser SLAM method combining ground constraints and dynamic point cloud removal according to claim 1, characterized in that, The function for calculating the dynamic score is: ， in, , These are the weighting coefficients. and Let these represent the maximum and minimum eigenvalues ​​corresponding to the i-th point, respectively. Let represent the normal vector of the i-th point in the current frame. Let represent the normal vector corresponding to the minimum eigenvalue of the i-th point.

5. The laser SLAM method combining ground constraints and dynamic point cloud removal according to claim 1, characterized in that, The localization and mapping process includes: a global nonlinear factor graph optimization model that is jointly solved based on point cloud data acquired by lidar, IMU data acquired by inertial sensors, and speed measurement data acquired by wheel counters to achieve localization and mapping.

6. The laser SLAM method combining ground constraints and dynamic point cloud removal according to claim 5, characterized in that, The constraint factor graph of the global nonlinear factor graph optimization model includes pose residuals calculated based on point cloud data excluding dynamic non-ground regions, displacement residuals calculated based on programmable data, ground constraint residuals calculated based on point cloud data of ground regions, and loop closure residuals calculated based on data between the current frame and historical frames at the current position.

7. The laser SLAM method combining ground constraints and dynamic point cloud removal according to claim 6, characterized in that, The function of the ground constraint residual is: ， in, Represents the global normal vector of a local area of ​​the ground region. The parameters Roll and Pitch represent the roll and pitch angles of the mobile device's estimated attitude in the Euler angles. This represents the estimated Z-axis elevation of the mobile device in the world coordinate system. This represents the theoretical height of the sensor calculated using local ground plane equations and ground area point cloud data.

8. The laser SLAM method combining ground constraints and dynamic point cloud removal according to claim 6, characterized in that, The displacement residual is obtained by: acquiring the cumulative pulse count of the encoders of the left and right wheels of the mobile device, calculating the linear displacement increment and heading angle increment of the mobile device body in a very short time, using the average heading angle of the previous moment and the current moment as the median heading angle, and projecting the cumulative relative motion into a high-frequency odometer pose transformation in the global coordinate system, which is then introduced into the constraint factor diagram as the displacement residual.

9. A laser SLAM system combining ground constraints and dynamic point cloud removal, and a laser SLAM method combining ground constraints and dynamic point cloud removal according to any one of claims 1-8, characterized in that, include: The data preprocessing module is used to collect multi-source sensor data and use IMU data to perform distortion correction on point cloud data; The segmentation and dynamic removal module is used to segment point cloud data and remove point cloud data corresponding to dynamic objects; The solver module is used to construct a global nonlinear factor graph optimization model and a constraint factor graph, and to perform joint optimization to solve for pose and build the graph.

10. A terminal device, comprising a laser SLAM method combining ground constraints and dynamic point cloud removal according to any one of claims 1-8, characterized in that, It includes a processor, a memory, and a computer program stored in the memory and capable of running on the processor.

Images