Unmanned vehicle mapping method and system based on improved LIO-SAM

Abstract

The invention discloses an unmanned vehicle mapping method and system based on improved LIO-SAM, and relates to the technical field of robot positioning and map construction. Point cloud data and IMU data are acquired; performing data preprocessing on the collected point cloud data to obtain preprocessed point cloud data; performing data matching on the preprocessed point cloud data to obtain matched point cloud data; pre-integration is carried out on the IMU data, a factor graph is constructed in combination with the matched point cloud data, local optimization is carried out on the factor graph based on a sliding window, and an optimized unmanned vehicle space posture is obtained; performing coordinate transformation on the matched point cloud data based on the optimized unmanned vehicle position posture to obtain point cloud data under a global coordinate system; and calculating a point cloud change error of the point cloud data under the global coordinate system based on the time sequence, filtering dynamic points to obtain global point cloud map data, and storing the global point cloud map data to obtain an unmanned vehicle mapping result, thereby realizing unmanned vehicle mapping under the condition of obstacle movement.

Description

Technical Field

[0001] The present application relates to the technical field of robot positioning and map building, and in particular to an unmanned vehicle mapping method and system based on an improved LIO-SAM. Background Art

[0002] With the development of autonomous driving technology, unmanned vehicles are placing higher demands on the accuracy and real-time performance of environmental perception and mapping. LIO-SAM (LiDAR-Inertial Odometry via Smoothing and Mapping) is a tightly coupled SLAM method that combines LiDAR and IMU information and demonstrates excellent performance in multi-sensor fusion positioning and mapping.

[0003] The existing LIO-SAM-based unmanned vehicle mapping method does not effectively identify and eliminate dynamic objects, resulting in a decrease in mapping accuracy in scenes with dynamic obstacles such as pedestrians and vehicles. Summary of the Invention

[0004] The present application aims to solve, at least to some extent, one of the technical problems in the related art. To this end, one purpose of the present application is to propose an unmanned vehicle mapping method and system based on an improved LIO-SAM, which realizes unmanned vehicle mapping in the presence of moving obstacles.

[0005] One aspect of the present application provides an unmanned vehicle mapping method based on an improved LIO-SAM, comprising:

[0006] Step S100: Acquire point cloud data and IMU data;

[0007] Step S200: performing data preprocessing on the collected point cloud data to obtain preprocessed point cloud data;

[0008] Step S300: performing data matching on the pre-processed point cloud data to obtain matched point cloud data;

[0009] Step S400: Pre-integrate the IMU data, combine it with the matched point cloud data, construct a factor graph, and locally optimize the factor graph based on a sliding window to obtain the optimized unmanned vehicle pose;

[0010] Step S500: Based on the optimized posture of the unmanned vehicle, coordinate transformation is performed on the matched point cloud data to obtain point cloud data in a global coordinate system;

[0011] Step S600: Calculate the point cloud change error of the point cloud data in the global coordinate system based on the time series, filter out dynamic points, obtain global point cloud map data, and store the global point cloud map data to obtain the unmanned vehicle mapping result;

[0012] The specific method of obtaining point cloud data and IMU data and performing time alignment thereon is as follows: obtaining IMU data through an inertial measurement unit on an unmanned vehicle, wherein the IMU data includes acceleration and angular velocity data, and simultaneously obtaining point cloud data through a lidar;

[0013] The specific method of performing data preprocessing on the collected point cloud data to obtain the preprocessed point cloud data is as follows:

[0014] First, the point cloud data is evenly downsampled using voxel grid filtering to remove noise points. Second, outliers are removed from the point cloud data based on threshold filtering of distance or intensity. Finally, the IMU data is interpolated between different timestamps using an interpolation method, and the point cloud data is time-aligned with the IMU data to obtain the pre-processed point cloud data.

[0015] The data matching includes scan-to-scan matching and scan-to-map matching. Scan-to-scan matching uses the ICP algorithm to match two consecutive frames of pre-processed point cloud data, and solves the coordinate transformation matrix between the two frames by minimizing the distance between the two frames of pre-processed point cloud data. Scan-to-map matching is used to match the current frame with the established global point cloud map data to obtain the coordinate transformation matrix of the current frame pre-processed point cloud data relative to the global point cloud map data. The optimization goal is to minimize the distance between the current frame and the global point cloud map data.

[0016] The specific method of pre-integrating the IMU data is as follows: pre-integrating the IMU data, calculating the position update, velocity update and rotation update between two timestamps, and obtaining the position, velocity and rotation matrix of the predicted point;

[0017] The specific method of constructing a factor graph and locally optimizing the factor graph based on a sliding window to obtain an optimized unmanned vehicle posture is as follows: using the unmanned vehicle posture within the sliding window as a node state, and using the error of pre-integration of IMU data and the error of matched point cloud data as edges to construct a factor graph; based on the factor graph within the sliding window, using a nonlinear least squares method to perform optimization, with the optimization goal being to minimize the error cost function of all edges of the factor graph within the sliding window;

[0018] The nonlinear least square method is used to optimize the factor graph in the sliding window. The optimization goal is to minimize the error cost function of all edges of the factor graph in the sliding window. The specific method is: define a sliding window, the current sliding window contains the most recent N w When a new data frame arrives, the old node is removed from the sliding window, including the most recent N nodes. wThe sliding window of each node contains M edges, which represent the constraints between nodes. The error cost function of the edges in the sliding window is calculated. The nonlinear least squares method is selected to minimize the error cost function as the optimization goal, and the posture of the unmanned vehicle is used as the target variable for optimization.

[0019] The specific method of minimizing the error cost function as the optimization goal and taking the posture of the unmanned vehicle as the optimization target variable is as follows: calculating the gradient and Hessian matrix of the error cost function, iteratively optimizing the posture of the unmanned vehicle based on the gradient of the error cost function and the Hessian matrix H, and obtaining the corrected posture of the unmanned vehicle;

[0020] The specific method for calculating the point cloud change error of the point cloud data in the global coordinate system based on the time series, filtering out dynamic points, and obtaining the global point cloud map data is as follows:

[0021] Step S610: Using the ICP algorithm to calculate the transformation matrix T between two adjacent frames of point cloud data in the global coordinate system b,b+1 ;

[0022] Step S620: For each point in the point cloud in the global coordinate system, calculate the point pair correspondence error in adjacent frames, and perform time series comparison on the point cloud data of the global coordinate system of consecutive Z frames to obtain the point cloud change error;

[0023] Step S630: Set the point cloud change threshold. If the point cloud change error of a point in the point cloud in the global coordinate system in consecutive Z frames is greater than or equal to the point cloud change threshold, the point is considered to be a dynamic point. Otherwise, it is a static point. Use the static points to build a map and obtain the global point cloud map data after filtering out the dynamic point cloud.

[0024] The point cloud data in the global coordinate system of two adjacent frames are calculated using the ICP algorithm to calculate the transformation matrix between adjacent frames. Specifically, assuming that the point cloud data in the global coordinate system of the bth and b+1th adjacent frames are P b and P b+1 , calculate the transformation matrix between two adjacent frames through the ICP algorithm to obtain the point cloud data P in the global coordinate system of the b+1th frame b+1 Relative to the point cloud data P in the global coordinate system of frame b b movement.

[0025] One aspect of the present application provides an unmanned vehicle mapping system based on an improved LIO-SAM, comprising:

[0026] Data acquisition module, used to obtain point cloud data and IMU data;

[0027] A data preprocessing module is used to perform data preprocessing on the collected point cloud data to obtain preprocessed point cloud data;

[0028] The point cloud matching module is used to perform data matching on the pre-processed point cloud data to obtain matched point cloud data;

[0029] The local optimization module is used to pre-integrate the IMU data, combine it with the matched point cloud data, construct a factor graph, and perform local optimization on the factor graph based on a sliding window to obtain the optimized unmanned vehicle posture;

[0030] The global point cloud transformation module is used to perform coordinate transformation on the matched point cloud data based on the optimized unmanned vehicle posture to obtain point cloud data in the global coordinate system;

[0031] The dynamic point filtering and mapping module is used to calculate the point cloud change error of the point cloud data in the global coordinate system based on the time series, filter out dynamic points, obtain the global point cloud map data, and store the global point cloud map data to obtain the unmanned vehicle mapping result.

[0032] The unmanned vehicle mapping method and system based on the improved LIO-SAM proposed in this application has the following advantages over existing technologies:

[0033] This application introduces a local graph optimization method based on sliding windows. Compared with the global graph optimization in the original LIO-SAM algorithm, local graph optimization only optimizes the nodes and constraints within the current sliding window, which greatly reduces the scale of the optimization problem and reduces the computational complexity.

[0034] This application achieves real-time pose estimation and map update by continuously sliding the sliding window forward, meeting the needs of real-time mapping of unmanned vehicles. It innovatively limits the graph optimization problem to a local range, significantly improving computing efficiency while ensuring mapping accuracy.

[0035] This application proposes a dynamic point cloud filtering method based on time series. By analyzing the changes between consecutive multi-frame point cloud data, dynamic obstacles can be effectively identified and filtered out, solving the problem of mapping in dynamic environments.

[0036] The dynamic point cloud filtering method proposed in this application utilizes time series information. Compared to traditional single-frame point cloud processing methods, it can more accurately distinguish between dynamic and static objects, improving the robustness of mapping. By filtering out the influence of dynamic obstacles, a global point cloud map containing only static environmental information is obtained, improving the map's accuracy and reliability.

[0037] This application integrates LiDAR point cloud data and IMU pre-integration into the same optimization problem, achieving deep fusion of sensor data. IMU pre-integration provides high-frequency pose prediction, which makes up for the relatively low frequency of LiDAR data and improves the real-time performance of the system. LiDAR point cloud data provides high-precision environmental geometry information. Through matching and optimization, accurate pose estimation and environmental maps are obtained. The innovative combination of IMU pre-integration and point cloud data matching gives full play to the complementary advantages of the two sensors and realizes robust and high-precision mapping.

[0038] This application effectively addresses the complex environmental challenges faced by unmanned vehicles through sliding window local graph optimization and dynamic point cloud filtering. Local graph optimization ensures the real-time performance and computational efficiency of the system, enabling the method to operate stably under the condition of limited computing resources of unmanned vehicles. Dynamic point cloud filtering improves the robustness of mapping, enabling unmanned vehicles to obtain reliable map information in dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A flowchart of a method for unmanned vehicle mapping based on improved LIO-SAM provided in this application;

[0040] Figure 2 Flowchart of the optimization algorithm provided for this application;

[0041] Figure 3 The algorithm framework diagram of LIO-SAM provided in this application;

[0042] Figure 4 Flowchart of the dynamic point cloud filtering algorithm based on time series provided in this application;

[0043] Figure 5 The improved LIO-SAM algorithm framework diagram provided by this application;

[0044] Figure 6 Maps created to improve the previous LIO-SAM algorithm;

[0045] Figure 7 Map created for the improved LIO-SAM algorithm;

[0046] Figure 8 This is the interface diagram of Rviz software;

[0047] Figure 9 This is the Gazebo software interface diagram;

[0048] Figure 10 This is the rendering of the warehouse-type unmanned vehicle with a four-wheel differential steering structure approved by this application in the Gazebo software;

[0049] Figure 11 The rendering of the 3D model of the simulation environment provided in this application in Gazebo software;

[0050] Figure 12 An operating interface for creating a global map by jointly running Gazebo and Rviz software. DETAILED DESCRIPTION

[0051] To better understand the present application, various aspects of the present application will be described in more detail with reference to the accompanying drawings. It should be understood that these detailed descriptions are merely descriptions of exemplary embodiments of the present application and are not intended to limit the scope of the present application in any way. Throughout the specification, the same reference numerals refer to the same elements. The expression "and / or" includes any and all combinations of one or more of the associated listed items.

[0052] In the accompanying drawings, the size, dimensions, and shapes of the elements have been slightly adjusted for ease of illustration. The accompanying drawings are for illustration only and are not drawn strictly to scale. As used in this application, the terms "substantially," "approximately," and similar terms are used to indicate approximations, not degrees, and are intended to illustrate inherent deviations in measurements or calculations that would be recognized by a person of ordinary skill in the art. In addition, in this application, the order in which the steps are described does not necessarily represent the order in which these steps would occur in actual operation, unless otherwise specified or inferred from the context.

[0053] It should also be understood that expressions such as "including", "comprising", "having", "containing" and / or "comprising" are open rather than closed expressions in this specification, which indicate the presence of the stated features, elements and / or components, but do not exclude the presence of one or more other features, elements, components and / or combinations thereof. In addition, when expressions such as "at least one of..." appear after a list of listed features, they modify the entire list of features rather than just the individual elements in the list. In addition, when describing embodiments of the present application, "may" is used to mean "one or more embodiments of the present application". And, the term "exemplary" is intended to refer to an example or illustration.

[0054] Unless otherwise specified, all words used in this application (including engineering terms and scientific and technological terms) have the same meaning as commonly understood by those skilled in the art to which this application belongs. It should also be understood that, unless otherwise specified in this application, words defined in commonly used dictionaries should be interpreted as having the same meaning as they have in the context of the relevant technology, and should not be interpreted in an idealized or overly formal sense.

[0055] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in this application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0056] Example 1

[0057] like Figure 1 As shown in FIG, a method for mapping an unmanned vehicle based on an improved LIO-SAM is provided in this application, comprising:

[0058] Step S100: Acquire point cloud data and IMU data;

[0059] The specific method of obtaining point cloud data and IMU data and performing time alignment thereon is as follows: obtaining IMU data through an inertial measurement unit on an unmanned vehicle, wherein the IMU data includes acceleration and angular velocity data, and simultaneously obtaining point cloud data through a lidar;

[0060] Step S200: performing data preprocessing on the collected point cloud data to obtain preprocessed point cloud data;

[0061] The specific method of performing data preprocessing on the collected point cloud data to obtain the preprocessed point cloud data is as follows:

[0062] First, the point cloud data is evenly downsampled using voxel grid filtering to remove noise points. Second, outliers are removed from the point cloud data based on threshold filtering of distance or intensity. Finally, the IMU data is interpolated between different timestamps using an interpolation method, and the point cloud data is time-aligned with the IMU data to obtain the pre-processed point cloud data.

[0063] The calculation formula for downsampling point cloud data using voxel grid filtering is: Among them, p filtered represents the point cloud data after filtering out noise points, N represents the number of points in the voxel, and Pi is the coordinate of the i-th point in the point cloud data;

[0064] Step S300: performing data matching on the pre-processed point cloud data to obtain matched point cloud data;

[0065] The data matching includes scan-to-scan matching and scan-to-map matching. Scan-to-scan matching uses the ICP algorithm to match two consecutive frames of pre-processed point cloud data, and solves the coordinate transformation matrix between the two frames by minimizing the distance between the two frames of pre-processed point cloud data. Scan-to-map matching is used to match the current frame with the established global point cloud map data to obtain the coordinate transformation matrix of the current frame pre-processed point cloud data relative to the global point cloud map data. The optimization goal is to minimize the distance between the current frame and the global point cloud map data.

[0066] The distance between the two frames of pre-processed point cloud data is minimized as follows: Among them, R represents the rotation matrix to be optimized, t represents the translation vector to be optimized, and p k Represents the point in the point cloud data after preprocessing of the current k-th frame, || || 2 represents the distance, T is the coordinate transformation matrix between the two frames, Represents the coordinate transformation matrix T that can minimize the distance between two frames of pre-processed point cloud data;

[0067] The coordinate transformation matrix between the two frames is expressed as T=[R|t]; the coordinate transformation matrix between the two frames is solved by minimizing the distance between the pre-processed point cloud data of the two frames;

[0068] The minimization of the distance between the current frame and the global point cloud map data is expressed as: Among them, p m is the point in the global point cloud map data corresponding to the pre-processed point cloud data of the current frame, T' represents the coordinate transformation matrix of the pre-processed point cloud data of the current frame relative to the global point cloud map data, R' represents the rotation matrix to be optimized of the coordinate transformation matrix T', and t' represents the translation vector to be optimized of the coordinate transformation matrix T'. represents the coordinate transformation matrix T' that can minimize the distance between the current frame and the global point cloud map data;

[0069] The coordinate transformation matrix of the current frame pre-processed point cloud data relative to the global point cloud map data is expressed as T'=[R'|t'];

[0070] The optimization goal of scan-to-scan matching is to minimize the distance between the pre-processed point cloud data of two frames and solve the coordinate transformation matrix between the two frames. The optimization goal of scan-to-map matching is to minimize the distance between the current frame and the global point cloud map data and solve the coordinate transformation matrix of the pre-processed point cloud data of the current frame relative to the global point cloud map data.

[0071] Step S400: Pre-integrate the IMU data, combine it with the matched point cloud data, construct a factor graph, and locally optimize the factor graph based on a sliding window to obtain the optimized unmanned vehicle pose;

[0072] The specific method of pre-integrating the IMU data is as follows: pre-integrating the IMU data, calculating the position update, velocity update and rotation update between two timestamps, and obtaining the position, velocity and rotation matrix of the predicted point;

[0073] The calculation formula for the position of the predicted point is: Among them, p k is the point p in the point cloud data after preprocessing based on the k-1th frame k-1 The predicted position of the kth frame point, Δt is the time interval between two frames, v k-1 is the velocity of the point in the point cloud data after preprocessing of the k-1th frame, and a is the acceleration in the IMU data;

[0074] The calculation formula of the predicted point velocity is: k =v k-1 +Δt×a, where v k is the velocity of the point in the point cloud data after preprocessing of the kth frame;

[0075] The calculation formula of the rotation matrix of the predicted point is: R k =R k-1 ×exp(ω×Δt), where ω is the angular velocity in the IMU data, R k 、R k-1 are the rotation matrices of the points in the pre-processed point cloud data of the kth frame and the k-1th frame respectively;

[0076] The specific method of constructing a factor graph and locally optimizing the factor graph based on a sliding window to obtain an optimized unmanned vehicle posture is as follows: using the unmanned vehicle posture within the sliding window as a node state, and using the error of pre-integration of IMU data and the error of matched point cloud data as edges to construct a factor graph; based on the factor graph within the sliding window, using a nonlinear least squares method to perform optimization, with the optimization goal being to minimize the error cost function of all edges of the factor graph within the sliding window;

[0077] The error of the IMU data pre-integration refers to the IMU pre-integration error between the position, velocity and rotation matrix of the predicted point obtained by the IMU data pre-integration and the position, velocity and rotation matrix of the actual point;

[0078] The error of the matched point cloud data refers to the matching error between the matched point cloud data and the real point cloud data;

[0079] The specific method of using nonlinear least squares method to optimize the factor graph in the sliding window and minimizing the error cost function of all edges of the factor graph in the sliding window is as follows:

[0080] Define a sliding window. The current sliding window contains the most recent N w When a new data frame arrives, the old node is removed from the sliding window, including the most recent N nodes. w The sliding window of each node contains M edges, which represent the constraints between nodes. The error cost function of the edges in the sliding window is calculated. The nonlinear least squares method is selected to minimize the error cost function as the optimization goal, and the posture of the unmanned vehicle is used as the target variable for optimization.

[0081] The optimization process only considers nodes and edges within the sliding window;

[0082] The calculation formula of the error cost function is: Where i' represents the number of edges in the sliding window, M is the total number of edges in the sliding window, and e i' (x) is the error of the i'th edge, ρ(.) is the robust loss function used to reduce the impact of outliers on the optimization results, and x is the node state;

[0083] The error of the i'th edge usually represents the difference between the actual measurement value and the predicted point position, velocity and rotation matrix and the matched point cloud data. For different edges, e i' The specific form of (x) is different. For example, the error of the constraint of the unmanned vehicle posture is e i' (x) = h(x i' ,x j )-z i' , where h(x i' ,x j ) represents the node state x i' and node status x j The predicted value obtained by the relative motion between the predicted points is the position, velocity and rotation matrix of the predicted point or the matched point cloud data, z i' is the true value of the measurement, which is the position, velocity and rotation matrix of the real point or the real point cloud data;

[0084] The optimization goal is:

[0085] Furthermore, the specific method of minimizing the error cost function as the optimization goal and taking the posture of the unmanned vehicle as the optimization target variable is as follows: calculating the gradient and Hessian matrix of the error cost function, iteratively optimizing the posture of the unmanned vehicle based on the gradient of the error cost function and the Hessian matrix H, and obtaining the corrected posture of the unmanned vehicle;

[0086] The calculation formula of the gradient of the error cost function is: in, is the transpose of the Jacobian matrix of the i'th edge, representing the error e i' (x) partial derivative with respect to the node state x;

[0087] The calculation formula of the Hessian matrix is: Among them, W i' represents the weight matrix of the i'th edge, where the weight matrix is ​​the inverse matrix of the covariance matrix of the error;

[0088] like Figure 2 As shown in the figure, the optimization algorithm flow chart provided by this application starts from the previous timestamp, takes the node state as the initialization, calculates the error of each edge for the node state of the current timestamp, and calculates the Jacobian matrix J of its error to the node state for each edge. i' ; Based on the Jacobian matrix, construct the Hessian matrix H and the gradient of the error cost function, solve the linear system, and update the node state of the previous timestamp;

[0089] The calculation formula of the linear system is: Here, Δx is the updated value of the node state; the linear system describes how to minimize the error cost function by adjusting the updated node state. Solving for the updated node state value is to find the optimal node state update direction in the current iterative optimization, that is, the corrected self-driving vehicle posture. It is worth noting that because matrices in graph optimization are often sparse, sparse solver methods can be used when solving linear systems to improve computational efficiency.

[0090] The calculation formula for updating the node status of the previous timestamp is: new =x old +Δx; where x old is the node status at the previous timestamp, x new The node status at the current timestamp;

[0091] The core idea of ​​sliding window-based local graph optimization is to limit the scope of optimization through the sliding window, optimizing only the currently relevant parts. Graph optimization is performed using nonlinear least squares methods, and sparse matrix solvers are used to improve computational efficiency. By comparing the total error function of global graph optimization with the window error function under the sliding window strategy, it can be found that considering only the nodes and edges within the window can significantly reduce the complexity of graph optimization calculations, thereby improving real-time performance and effectively accelerating the graph optimization process. While meeting accuracy requirements, it also effectively corrects errors accumulated over long periods of time by the IMU.

[0092] Step S500: Based on the optimized posture of the unmanned vehicle, coordinate transformation is performed on the matched point cloud data to obtain point cloud data in a global coordinate system;

[0093] Step S600: Calculate the point cloud change error of the point cloud data in the global coordinate system based on the time series, filter out dynamic points, obtain global point cloud map data, and store the global point cloud map data to obtain the unmanned vehicle mapping result;

[0094] The specific method for calculating the point cloud change error of the point cloud data in the global coordinate system based on the time series, filtering out dynamic points, and obtaining the global point cloud map data is as follows:

[0095] Step S610: Using the ICP algorithm to calculate the transformation matrix T between two adjacent frames of point cloud data in the global coordinate system b,b+1 ;

[0096] The point cloud data in the global coordinate system of two adjacent frames are calculated using the ICP algorithm to calculate the transformation matrix between adjacent frames. Specifically, assuming that the point cloud data in the global coordinate system of the bth and b+1th adjacent frames are P b and P b+1 , calculate the transformation matrix between two adjacent frames through the ICP algorithm to obtain the point cloud data P in the global coordinate system of the b+1th frame b+1 Relative to the point cloud data P in the global coordinate system of frame b b movement;

[0097] Step S620: For each point in the point cloud in the global coordinate system, calculate the point pair correspondence error in adjacent frames, and perform time series comparison on the point cloud data of the global coordinate system of consecutive Z frames to obtain the point cloud change error;

[0098] The calculation formula of the point pair correspondence error is: in, Represents the point cloud data P in the global coordinate system b The cth point in Δd b,b+1 represents the point pair correspondence error between the midpoints of the point cloud data in the global coordinate system of adjacent frames, |P b| represents the number of points in the point cloud data in the global coordinate system, ||·|| is the Euclidean distance;

[0099] The point pair correspondence error refers to the average distance difference between points in the point cloud;

[0100] For dynamic objects, the position of each point in different time frames varies greatly, so Δd b,b+1 The points of static objects change little between multiple frames, Δd b,b+1 Will be smaller.

[0101] Optionally, the point cloud data of multiple frames in the global coordinate system are compared in time series, and the changes in the point cloud data of each frame in the global coordinate system are analyzed. For each frame of point cloud data in the global coordinate system, the overlapping part with the point cloud data of the previous nt frames is calculated, that is, the point cloud change error in the time series.

[0102] Step S630: Setting a point cloud change threshold. If the point cloud change error of a point in the global coordinate system in consecutive Z frames is greater than or equal to the point cloud change threshold, the point is considered a dynamic point. Otherwise, it is a static point. The static points are used to build a map to obtain the global point cloud map data after filtering out the dynamic point cloud.

[0103] Optionally, for a dynamic point, the point is replaced with a predicted value from the point cloud in the global coordinate system. The predicted value refers to the position where the point should appear in the static environment based on the static points around the dynamic point and prior knowledge, and the original dynamic point is replaced with the position. The replacement with the predicted value can adopt methods such as time series interpolation, spatial domain difference, and surface fitting. The time series interpolation estimates the static position of the point in the current frame by interpolation based on the position of the dynamic point in the previous and next frames. The spatial domain difference uses the static point information around the dynamic point to estimate the static position of the point by spatial interpolation. The surface fitting performs surface fitting on the local area where the dynamic point is located, and replaces the dynamic point with the corresponding position on the fitted surface.

[0104] Example 2

[0105] LIO-SAM is a tightly coupled lidar inertial odometry framework based on factor graph optimization. The LIO-SAM algorithm provides more stable and accurate positioning and map construction by jointly optimizing lidar point cloud data and IMU data, especially for dynamic environments and complex scenes. The core idea of ​​the LIO-SAM algorithm is to use nonlinear optimization to minimize the residual between lidar point cloud data and IMU data, thereby estimating the robot's posture. The LIO-SAM workflow can be divided into four key steps: IMU pre-integration, lidar point cloud data processing and matching, factor graph generation and optimization, and loop detection. Figure 3The figure shows the algorithm framework diagram of LIO-SAM.

[0106] The core task of the IMU pre-integration module is to provide short-term motion estimates by integrating the IMU's acceleration and angular velocity information. This information can then be optimized together with the lidar data to ensure the accuracy and robustness of the overall system. The IMU itself provides high-frequency motion data, such as acceleration and angular velocity, but due to its drift over time, it cannot provide long-term precise positioning alone. The main function of pre-integration is to correct and integrate this data using mathematical models and optimize it together with the lidar data to improve system accuracy.

[0107] LiDAR generates point cloud data by scanning space using multiple laser beams. In the LIO-SAM algorithm, data preprocessing is a key step in improving subsequent algorithm performance. This includes denoising and filtering, outlier removal, and time synchronization. LIO-SAM typically uses voxel grid filtering to downsample point clouds to reduce noise.

[0108] LIO-SAM's lidar point cloud data matching mainly includes two steps: scan-to-scan matching and scan-to-map matching. In LIO-SAM, scan-to-scan matching is used to estimate the relative motion between the current frame and the previous frame. Scan-to-map matching is used to align the current frame with the point cloud in the global or local map. The key to this process is to provide more accurate pose estimation through the global map. By continuously processing and optimizing the lidar data, LIO-SAM constructs a map M that contains the processed point cloud data. In each frame, the current frame point cloud will be compared with the point cloud M in the map. Using 3DICP or other point feature matching methods, the current point cloud is matched with the point cloud in the map to determine the relative position of the current frame.

[0109] In the LIO-SAM algorithm, lidar and IMU fusion is achieved through a factor graph. In LIO-SAM, constructing a factor graph is a key optimization step. It is used to represent the constraints between lidar and IMU data and to solve for the optimal state estimate using graph optimization techniques. A factor graph is a graphical model in which nodes represent system states, such as the robot's position and pose, and edges represent constraints between these states, such as lidar matching error and IMU pre-integration error. In LIO-SAM, factors represent constraints between different time steps or between different data sources, such as an IMU and lidar. The main factors in LIO-SAM are IMU pre-integration factors and lidar matching factors. Because IMU data in the IMU pre-integration factors typically contains continuous acceleration and angular velocity information, LIO-SAM uses IMU pre-integration techniques to convert this data into constraints and add them as factors to the factor graph. The lidar matching factors contain lidar data providing relative pose constraints between the current and previous frames. Through scan-to-scan matching or scan-to-map matching, LIO-SAM can calculate the relative displacement between the current frame and the previous frame.

[0110] LIO-SAM uses graph optimization technology to solve the factor graph. The goal of graph optimization is to minimize the errors introduced by all factors, thereby obtaining the optimal state estimate. LIO-SAM typically uses nonlinear least squares to optimize the factor graph. It first constructs the error terms, calculating one for each factor. Then, all the error terms are added together, and LIO-SAM optimizes the factor graph by minimizing the total error function. LIO-SAM uses the Gauss-Newton iteration method to optimize the residual equations to solve the keyframe pose. Through the optimization process, LIO-SAM adjusts the value of each state node, ultimately obtaining the optimal trajectory and IMU bias estimate. After optimization, the robot's pose and IMU parameters at each moment are accurately estimated.

[0111] Finally, loop detection in the LIO-SAM algorithm involves detecting similarities between the robot's current position and a location in its historical trajectory, triggering a loop correction process. When the system detects a loop—that is, when the current LiDAR data is similar to LiDAR data from a certain location in the past—loop optimization is triggered. At this point, LIO-SAM performs a global optimization, adjusting the entire path and map to correct errors and improve pose accuracy.

[0112] Although LIO-SAM demonstrates excellent performance in combining lidar and inertial measurement units for simultaneous localization and mapping, the algorithm still has some limitations, including:

[0113] First, the algorithm's graph optimization process places enormous demands on computing resources. This is especially true when building large maps or running them for extended periods, as the computational overhead and memory consumption of graph optimization can increase significantly. These factors can prevent LIO-SAM from meeting real-time requirements on resource-constrained devices, such as embedded and real-time systems, leading to performance degradation or even inability to run in real time.

[0114] Second, the LIO-SAM algorithm is less robust in dynamic environments, especially in the presence of dynamic obstacles, which can severely impact system performance. LIO-SAM relies on joint optimization of the LiDAR and IMU to estimate pose, but dynamic obstacles can interfere with LiDAR data, leading to point cloud matching errors or unstable pose estimation.

[0115] Third, the LIO-SAM algorithm has limitations in handling long-term IMU errors. Although LIO-SAM uses a tight coupling between the IMU and the lidar to mitigate the impact of IMU errors on positioning, long-term IMU drift, such as bias and gyroscope and accelerometer noise, still accumulates over time. This accumulated error can lead to positioning deviations or reduced map accuracy over long periods of operation, especially when lidar data is insufficient for effective correction.

[0116] Based on the limitations of the above LIO-SAM algorithm, this application proposes an improved LIO-SAM algorithm, which includes a local graph optimization method based on a sliding window and a dynamic point cloud filtering method based on a time series. Figure 5 , which is a framework diagram of the improved LIO-SAM algorithm provided by this application.

[0117] Compared to sliding window-based local graph optimization methods, graph optimization in LIO-SAM typically involves reoptimizing the entire map based on global optimization. However, global optimization is computationally intensive. To improve the algorithm's computational efficiency and real-time performance to meet the computing power requirements of embedded devices, local graph optimization can be used instead of global graph optimization strategies. The key idea of ​​local graph optimization is to optimize only the local graph relevant to the current state, rather than reoptimizing the entire map. This significantly accelerates the graph optimization process while maintaining high positioning accuracy.

[0118] This application implements a local graph optimization strategy based on a sliding window strategy. This strategy maintains a sliding window that contains several recent robot state nodes, typically positions and postures, as well as the constraints between these nodes, typically provided by LiDAR and IMU sensor data. As new data arrives, the sliding window slides forward, removing old nodes and adding new ones.

[0119] On the other hand, IMU drift error stems from the accumulation of IMU errors, typically manifesting as long-term pose drift or trajectory offset. Without external constraints, IMU data cannot provide absolute position, resulting in increasing errors over time. Sliding window optimization is an effective strategy to address this issue. This method only considers the IMU and lidar data within the current time window and performs local optimization on this data. As new data is added, the window slides continuously, discarding old data, thereby establishing local constraints within each time window. These local constraints help optimize the pose and map within the current window, thereby correcting IMU drift errors. By introducing a sliding window-based local graph optimization method, not only can the limitations of the LIO-SAM algorithm in graph optimization performance be effectively overcome, but the long-term IMU error accumulation problem can also be effectively addressed.

[0120] The goal of local graph optimization is to optimize the robot's state by minimizing the errors between nodes and edges in the graph. During the optimization process, only the nodes and constraints within the sliding window are optimized, thus reducing computational complexity.

[0121] LIO-SAM is less robust in dynamic environments because the presence of dynamic objects interferes with the LiDAR point cloud data, affecting trajectory estimation and map construction. In dynamic environments, moving objects can cause the LiDAR to detect false obstacles or unstable features, which in turn affects the positioning and mapping performance of both the front-end and back-end. To address this issue, this paper proposes a time-series-based dynamic point cloud filtering method for LiDAR point cloud data. The basic idea is that by comparing the point cloud data between consecutive frames, dynamic objects will change position at multiple time points, while static objects remain stable. Based on this, dynamic and static objects can be distinguished based on the change information between the point clouds.

[0122] For the dynamic point cloud filtering method based on time series, the key is to analyze multiple consecutive lidar frames, calculate the changes between point clouds, and then identify dynamic objects. The position changes of dynamic objects at different time points are usually large, while the position changes of static objects are small or almost unchanged. In order to distinguish these dynamic objects from static objects, this paper adopts a feature change analysis strategy, that is, comparing the changes of feature points or key points between multiple frames. Dynamic objects usually have large changes between different frames. Figure 4 As shown, this is a flow chart of the dynamic point cloud filtering algorithm based on time series provided by this application. The point cloud data of a continuous time series is input through a laser radar, and the point clouds between adjacent frames are matched and motion estimated. The change errors of the point clouds of consecutive frames are compared, and points with point cloud change errors greater than a threshold are identified as dynamic points. Dynamic point clouds are filtered out, and the remaining static points are used to generate a static environment for mapping. The process of filtering dynamic objects is usually a recursive process. As more frame data is added, the recognition of dynamic objects will become more accurate. In practical applications, the detection of dynamic objects can be further optimized by optimizing local graphs, and historical information can be used to determine which objects should be filtered out. This can improve the real-time performance of the algorithm while ensuring accuracy.

[0123] Example 3

[0124] This application replaces the original global graph optimization strategy with a local graph optimization method based on a sliding window. It only optimizes the state within a limited time window, rather than globally optimizing all historical data, thereby achieving a balance between computational efficiency and accuracy, and helping to reduce the long-term error caused by IMU drift. In terms of front-end input data processing, a dynamic point cloud filtering method based on time series is used to further process the point cloud data of the lidar. By analyzing the change error between multiple frames of point clouds, dynamic objects can be effectively distinguished from static objects. Dynamic objects are eliminated during the mapping process, further improving the robustness and accuracy of the method in mapping and positioning in complex environments.

[0125] like Figure 6 and Figure 7The following diagrams show the maps created by the LIO-SAM algorithm before and after the improvements. The point cloud maps generated by the original LIO-SAM algorithm suffer from two issues: First, due to the low computational efficiency and large optimization range of global map optimization, point cloud updates are slow and experience lag, resulting in a significant amount of redundancy in the generated point cloud map. This redundancy has little impact on building the global map and increases the memory space occupied by the global map, which is not user-friendly for embedded devices. Second, due to the original LIO-SAM algorithm's poor ability to handle dynamic obstacles, drift points caused by pedestrians appear in the point cloud map. This affects the construction of the global map, which is the foundation for unmanned vehicle navigation and obstacle avoidance. Therefore, drift points directly affect the navigation and obstacle avoidance capabilities of the unmanned vehicle.

[0126] In contrast, the improved LIO-SAM algorithm creates a significantly smaller number of map point clouds, but it is still sufficient to construct a complete global map from these point clouds. Fewer point cloud data also reduces the memory footprint of the map, making it suitable for embedded devices. Furthermore, because the improved LIO-SAM algorithm incorporates a dynamic LiDAR object filtering strategy, the created point cloud map removes drift points introduced by pedestrians, effectively resolving a problem with the original LIO-SAM algorithm. This demonstrates that the improved LIO-SAM algorithm significantly improves mapping performance compared to the original algorithm.

[0127] In the experiments and result analysis of the embodiments of this application, the improved LIO-SAM was directly used as the mapping algorithm. The algorithm was written in the simulation system to build a global map and conduct experiments on path planning and obstacle avoidance. The control information sent to the chassis of the unmanned vehicle in the simulation system is exactly the same as the control information received by the real unmanned vehicle. However, the motion trajectory of the unmanned vehicle can be identified in the simulation system, which makes the experimental results more intuitive. Moreover, by accurately modeling factors such as collision effects, friction coefficients, and environment in the simulation system, the driving effect of a real unmanned vehicle can be fully simulated. This application uses the open source framework currently widely used in the field of robotics, the ROS (Robot Operating System) system as the robot operating system, and the version selected is ROS1 Melodic version. The system is modified to support the Linux distribution Ubuntu18.04 LTS, which is also consistent with the face recognition module experiment mentioned above, providing convenience for the subsequent overall system implementation.

[0128] For this experiment, the unmanned vehicle and environment modeling and simulation were performed using the Gazebo robotics simulation tool, which is compatible with the ROS system. It creates a virtual 3D world and provides a physics engine, sensor models, and environmental interaction capabilities, allowing developers to simulate and test robotic systems. Gazebo provides high-precision physics simulation, enabling developers to implement real-world kinematic and dynamic behaviors within the simulation environment. For example, the robot can navigate diverse terrains and handle physical phenomena such as collisions and slippage.

[0129] For the detection and display of the status, sensor data, global map and other information of the unmanned vehicle, this article uses the RViz (Robot Visualization) visualization tool that matches the ROS system. By connecting to the ROS node, RViz can display various sensor information, such as lidar, depth camera, RGB-D camera, IMU, and robot status information, providing an intuitive debugging and analysis platform. Figure 8 and Figure 9 Shown are the Rviz software interface diagram and the Gazebo software interface diagram.

[0130] In the ROS system, you can create a model for an unmanned vehicle by writing a URDF (Unified Robot Description Format) file. A URDF file is a description file in XML (eXtensible Markup Language) format that provides a description of the robot's structure, joints, sensors, materials, and other related information. When writing a URDF file, focus on the unmanned vehicle's main structural design, wheel dynamics description, sensor placement, and physical properties to ensure consistency with the simulation.

[0131] The design of an autonomous vehicle's frame and chassis can be described using various shapes, such as a cuboid or cylinder. For each wheel, its dimensions, mass, and moment of inertia must be defined. "Joints" are used to describe the connection between each component. The rotation of the wheels and the connection to the vehicle body must be precisely described, which impacts robot control and dynamic simulation. Sensor placement and orientation must be precisely calibrated to ensure accurate performance of the perception system in simulation or actual operation. Finally, the inertia, mass, and shape descriptions in the URDF file must be consistent with those of the actual vehicle. Figure 10 This is a rendering of the approved four-wheel differential steering unmanned vehicle in the Gazebo software. The red portion of the unmanned vehicle model represents the lidar sensor, the yellow portion represents the camera, and the black portion on top of the storage compartment represents the IMU inertial navigation module.

[0132] Modeling the autonomous vehicle's driving environment can be achieved by creating a World file. World files are XML-based files, typically with a .world extension. They describe and define the physical world of the environment, including terrain, obstacles, lighting, materials, sensors, and more. Within the file, you can specify the 3D shape, size, position, and material of objects, and also support the addition of dynamic elements such as moving people or objects. Figure 11 This is a rendering of the 3D model of the simulated environment provided in this application, rendered in Gazebo software. A movable person is added as a dynamic obstacle to support the navigation and obstacle avoidance experiment. By implementing the improved LIO-SAM algorithm and the unmanned vehicle chassis control algorithm implemented previously in the experimental project, the unmanned vehicle is controlled to create a global map within the hospital environment model, providing global map support for the unmanned vehicle's navigation and obstacle avoidance experiment. Figure 12 An operating interface for creating a global map by jointly running Gazebo and Rviz software.

[0133] Example 4

[0134] This application provides an unmanned vehicle mapping system based on an improved LIO-SAM, including:

[0135] Data acquisition module, used to obtain point cloud data and IMU data;

[0136] A data preprocessing module is used to perform data preprocessing on the collected point cloud data to obtain preprocessed point cloud data;

[0137] The point cloud matching module is used to perform data matching on the pre-processed point cloud data to obtain matched point cloud data;

[0138] The local optimization module is used to pre-integrate the IMU data, combine it with the matched point cloud data, construct a factor graph, and perform local optimization on the factor graph based on a sliding window to obtain the optimized unmanned vehicle posture;

[0139] The global point cloud transformation module is used to perform coordinate transformation on the matched point cloud data based on the optimized unmanned vehicle posture to obtain point cloud data in the global coordinate system;

[0140] The dynamic point filtering and mapping module is used to calculate the point cloud change error of the point cloud data in the global coordinate system based on the time series, filter out dynamic points, obtain the global point cloud map data, and store the global point cloud map data to obtain the unmanned vehicle mapping result.

[0141] In addition, the parts of the above technical solutions provided in the embodiments of the present application that are consistent with the implementation principles of the corresponding technical solutions in the prior art are not described in detail to avoid excessive redundancy.

[0142] The above-described specific embodiments further illustrate the objectives, technical solutions, and beneficial effects of the present invention. It should be understood that the above description is merely a specific embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A mapping method for unmanned vehicles based on improved LIO-SAM, characterized by: include: Obtain point cloud data and IMU data; Performing data preprocessing on the collected point cloud data to obtain preprocessed point cloud data; Perform data matching on the pre-processed point cloud data to obtain matched point cloud data; Pre-integrate the IMU data, combine it with the matched point cloud data, construct a factor graph, and perform local optimization on the factor graph based on the sliding window to obtain the optimized unmanned vehicle posture; Based on the optimized unmanned vehicle posture, coordinate transformation is performed on the matched point cloud data to obtain point cloud data in the global coordinate system; Based on the time series, the point cloud change error of the point cloud data in the global coordinate system is calculated, the dynamic points are filtered out, and the global point cloud map data is obtained. The global point cloud map data is stored to obtain the unmanned vehicle mapping result.

2. The unmanned vehicle mapping method based on improved LIO-SAM according to claim 1, characterized in that: The specific method for obtaining point cloud data and IMU data and time-aligning them is: obtaining IMU data through the inertial measurement unit on the unmanned vehicle, wherein the IMU data includes acceleration and angular velocity data, and obtaining point cloud data through the lidar.

3. The unmanned vehicle mapping method based on improved LIO-SAM according to claim 2, characterized in that: The specific method for performing data preprocessing on the collected point cloud data to obtain preprocessed point cloud data is as follows: first, using voxel grid filtering to uniformly downsample the point cloud data to filter out noise points; second, removing outliers in the point cloud data, and the removal of outliers is based on threshold filtering of point distance or intensity; finally, interpolating IMU data between different time stamps through an interpolation method, and time-aligning the point cloud data with the IMU data to obtain preprocessed point cloud data.

4. The unmanned vehicle mapping method based on improved LIO-SAM according to claim 3, characterized in that: The data matching includes scan-to-scan matching and scan-to-map matching. Scan-to-scan matching uses the ICP algorithm to match two consecutive frames of pre-processed point cloud data, and solves the coordinate transformation matrix between the two frames by minimizing the distance between the two frames of pre-processed point cloud data; scan-to-map matching is used to match the current frame with the established global point cloud map data to obtain the coordinate transformation matrix of the current frame pre-processed point cloud data relative to the global point cloud map data. The optimization goal is to minimize the distance between the current frame and the global point cloud map data.

5. The unmanned vehicle mapping method based on improved LIO-SAM according to claim 4, characterized in that: The specific method for pre-integrating the IMU data is: pre-integrating the IMU data, and the IMU data pre-integration calculates the position update, velocity update and rotation update between two timestamps to obtain the position, velocity and rotation matrix of the predicted point.

6. The unmanned vehicle mapping method based on improved LIO-SAM according to claim 5, characterized in that: The specific method for constructing a factor graph and locally optimizing the factor graph based on a sliding window to obtain the optimized unmanned vehicle posture is as follows: using the unmanned vehicle posture within the sliding window as the node state, and using the error of the pre-integration of IMU data and the error of the matched point cloud data as the edges to construct a factor graph; based on the factor graph within the sliding window, using nonlinear least squares optimization, the optimization goal is to minimize the error cost function of all edges of the factor graph within the sliding window.

7. The unmanned vehicle mapping method based on improved LIO-SAM according to claim 6, characterized in that: The nonlinear least square method is used to optimize the factor graph in the sliding window. The optimization goal is to minimize the error cost function of all edges of the factor graph in the sliding window. The specific method is: define a sliding window, the current sliding window contains the most recent N w When a new data frame arrives, the old node is removed from the sliding window, including the most recent N nodes. w The sliding window of each node contains M edges, which represent the constraints between nodes. The error cost function of the edges in the sliding window is calculated. The nonlinear least squares method is selected to minimize the error cost function as the optimization goal, and the posture of the unmanned vehicle is used as the target variable for optimization.

8. The unmanned vehicle mapping method based on improved LIO-SAM according to claim 7, characterized in that: The specific method of minimizing the error cost function as the optimization goal and taking the unmanned vehicle posture as the optimization target variable is as follows: calculating the gradient and Hessian matrix of the error cost function, iteratively optimizing the unmanned vehicle posture based on the gradient of the error cost function and the Hessian matrix H, and obtaining the corrected unmanned vehicle posture.

9. The unmanned vehicle mapping method based on improved LIO-SAM according to claim 8, characterized in that: The specific method for calculating the point cloud change error of the point cloud data in the global coordinate system based on the time series, filtering out dynamic points, and obtaining the global point cloud map data is as follows: For the point cloud data in the global coordinate system of two adjacent frames, the ICP algorithm is used to calculate the transformation matrix T between adjacent frames. b,b+1 ; For each point in the point cloud in the global coordinate system, calculate the point pair correspondence error in adjacent frames, and compare the point cloud data of the global coordinate system of consecutive Z frames in time series to obtain the point cloud change error; Set the point cloud change threshold. If the point cloud change error of a point in the point cloud in the global coordinate system in consecutive Z frames is greater than or equal to the point cloud change threshold, the point is considered to be a dynamic point. Otherwise, it is a static point. Use the static points to build a map and obtain the global point cloud map data after filtering out the dynamic point cloud.

10. An unmanned vehicle mapping system based on improved LIO-SAM, which is implemented based on an unmanned vehicle mapping method based on improved LIO-SAM according to any one of claims 1 to 9, characterized in that: include: Data acquisition module, used to obtain point cloud data and IMU data; A data preprocessing module is used to perform data preprocessing on the collected point cloud data to obtain preprocessed point cloud data; The point cloud matching module is used to perform data matching on the pre-processed point cloud data to obtain matched point cloud data; The local optimization module is used to pre-integrate the IMU data, combine it with the matched point cloud data, construct a factor graph, and perform local optimization on the factor graph based on a sliding window to obtain the optimized unmanned vehicle posture; The global point cloud transformation module is used to perform coordinate transformation on the matched point cloud data based on the optimized unmanned vehicle posture to obtain point cloud data in the global coordinate system; The dynamic point filtering and mapping module is used to calculate the point cloud change error of the point cloud data in the global coordinate system based on the time series, filter out dynamic points, obtain the global point cloud map data, and store the global point cloud map data to obtain the unmanned vehicle mapping result.

Images