Tight coupling LIO system based on equivariant filter and Surfel map

Abstract

The invention discloses a tight coupling LIO system based on an equivariant filter and a Surfel map, the whole system is composed of two modules: an equivariant filter (EqF) state estimator and a Surfel map, and the input data is a three-dimensional laser radar point and an IMU measurement value; motion compensation is carried out on the distorted point clouds through IMU pre-integration, and the point clouds between the two radar frames are compensated to the same moment; an equivariant filtering (EqF) state estimator is applied to LIO, IMU measurement values and original LiDAR scanning are directly fused, and a global Sufer Mapping map is closely integrated into an EqF-based LIO system. According to the method, the laser radar and the IMU are tightly coupled, the LIO system is constructed based on the equivariant filter and the Sufel map, the state of an observer is placed on a symmetric group, the global error dynamic state derived from the system equivariance is linearized, the precision and efficiency of the Surfel map are higher than those of a traditional point cloud map in the laser radar surveying and mapping process, and the method is suitable for the surveying and mapping of the laser radar. LiDAR points and map updating can be quickly and effectively fused.

Description

Technical Field

[0001] The present invention belongs to the laser odometry system in the SLAM algorithm, and specifically relates to a tightly coupled LIO system based on an equivariant filter and a Surfel map. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) technology is the core foundation for mobile robots to perform tasks such as exploration, reconnaissance, and navigation in unknown environments. By collecting observation data of the surrounding environment through the equipped sensors, the robot can construct detailed map information. In this process, the robot not only generates a map of the environment but also uses the existing map parts to accurately estimate its own position and pose in space. This ability is crucial for the robot to achieve autonomous navigation in complex and dynamically changing environments.

[0003] In recent years, Light Detection and Ranging (LiDAR) sensors have been widely used in Real-Time Synchronized Localization and Mapping (SLAM). This is because 3D LiDAR sensors can provide direct, active, and accurate depth measurements and show significant robustness in some harsh environments (such as dusty, foggy, or low-light conditions). However, LiDAR-based SLAM systems rely heavily on point cloud registration, which is easily affected by fast motion distortion and may degrade in some challenging situations. Inertial Measurement Unit (IMU) sensors can directly provide high-sampling-rate motion measurements without being affected by external environmental conditions. Therefore, integrating IMU data into an independent LiDAR SLAM can effectively improve the accuracy and robustness of the system.

[0004] The present invention proposes a fast, accurate, and reliable tightly coupled LIO framework with an equivariant filtering (Eqf) state estimator and Surfel Mapping. Surfel mapping has advantages in processing point cloud data and providing an accurate environmental representation, while EqF excels in leveraging system symmetries and improving filter performance. Both provide effective solutions for specific application scenarios and demonstrate significant performance improvements in their respective fields. Summary of the Invention

[0005] To address the above problems, the present invention proposes a tightly coupled LIO system based on an equivariant filter and a Surfel map, which iteratively updates the pose through an equivariant filter (Eqf) and converts the point cloud into a Surfel to improve the efficiency of localization and mapping.

[0006] The technical solution adopted by the present invention is as follows: A tightly coupled LIO system based on an equivariant filter and a Surfel map includes the following steps

[0007] Step (1): The 3D point cloud data of the surrounding environment and the motion state information of the device are collected by a lidar and an IMU respectively. The original point cloud data obtained by the lidar is downsampled using a voxel grid downsampling method.

[0008] Step (2): Based on the kinematic model of the IMU and its discretized representation, the collected IMU data is pre-integrated to calculate the incremental state quantity within the time interval, and the state estimate value X at time t is obtained t , including the position, velocity, attitude, and bias parameters of the IMU sensor;

[0009] Step (3): Using the state pre-estimate value X at time t obtained by IMU pre-integration t , backpropagation is performed again to perform motion compensation on the original point cloud data collected by the lidar. All points from time t - 1 to time t are compensated to time t. The compensated point cloud data is denoted as P t ;

[0010] Step (4): The equivariant filter algorithm is used to optimize and update the initial state estimate value obtained in Step (2) . Meanwhile, the Kalman gain and covariance are continuously adjusted and updated by calculating the magnitude of the residual until convergence, so as to obtain a more accurate and robust state estimate value

[0011] Step (5): Based on the updated state estimate value The motion-compensated point cloud data P t is transformed into the global coordinate system. The hash function index algorithm with locality priority is introduced to index the point cloud data, and it is efficiently merged into the global Surfel map through the indexing mechanism and the hash function. At the same time, the surface features in the Surfel map are dynamically updated according to the newly added point cloud data to maintain the real-time and accuracy of the map.

[0012] On the basis of the above solution, preferably, in Step (1), the collected lidar point cloud is downsampled using a voxel grid.

[0013] (a) First, calculate the bounding box, calculate the minimum three-dimensional cuboid bounding box of the point cloud data set. Assume the point cloud data set is P = {p i ∣i = 1, 2,..., N}, each point pi has coordinates (xi, yi, zi), and the boundaries of the bounding box can be calculated by the following formula:

[0014] x min = min(x i ), x max= max(x i )

[0015] y min = min(y i ), y max = max(y i )

[0016] z min = min(z i ), z max = max(z i )

[0017] (b) Divide the cube

[0018] Divide the bounding box into small cubes along the x, y, and z directions. Each small cube serves as a voxel, and the side length of each voxel is l. The size of the voxel grid can be calculated using the following formula:

[0019]

[0020] (c) Calculate the centroid or center

[0021] In each non-empty voxel, calculate the centroid of all points within the voxel or use the voxel center to approximate all points. The formula for calculating the centroid is as follows:

[0022]

[0023]

[0024] Step (2) Package the point cloud after downsampling each frame with several frames of IMU (until the next radar frame) and transmit it to the LIO system.

[0025] On the basis of the above scheme, preferably, in step (2), the collected IMU data is pre-integrated. The entire implementation process is as follows:

[0026] (a) First, define the state quantity X t , where R t is the rotation matrix at time t, v t is the velocity at time t, p t is the pose at time t,

[0027]

[0028] (b) From the IMU measurement model: Measurement value = True value + Noise + Bias, the following formula can be deduced:

[0029]

[0030] where both the noise and the bias conform to the Gaussian distribution,

[0031] The discrete model of the IMU is as follows:

[0032] R(t + Δt) = R(t)Exp(Ω)

[0033]

[0034]

[0035] According to the IMU measurement model and the continuous-discrete model, the state quantity X is obtained by integrating each frame of IMU data t , and the velocity and displacement are calculated.

[0036] On the basis of the above scheme, preferably, in step (3), the point cloud is motion-compensated according to the change of the state quantity, and the whole motion compensation process is as follows:

[0037] Step (a) IMU data pre-integration: Using the angular velocity and acceleration data provided by the IMU, the motion state of the LiDAR sensor at each point cloud data acquisition moment is estimated by numerical integration method to obtain the pose change of the sensor;

[0038] Step (b) Timestamp processing of point cloud data: Each LiDAR point cloud data has a timestamp associated with it, indicating when the point was acquired, and the timestamp is used to determine the estimated pose of the sensor at the time of acquisition of each point.

[0039] Step (c) Backpropagation: Using the attitude information obtained by IMU integration, backpropagation is performed on each LiDAR point cloud data to compensate for the distortion caused by sensor motion. For each point cloud point, a transformation relative to a certain reference moment is calculated;

[0040] Step (d) Point cloud transformation compensation: Each point cloud point is transformed according to the IMU attitude corresponding to its acquisition moment to simulate the position if the point was acquired in a stationary state, so that all point cloud data are regarded as being acquired at the same moment, thereby enabling more accurate subsequent processing.

[0041] On the basis of the above scheme, preferably, the equivariant filtering algorithm is used to update the state quantity X t The whole process is as follows:

[0042] Step (a) Match the local point cloud information scanned at time t with the full map point cloud information to calculate the residual;

[0043] Step (b) Use the system input and measurement value to calculate the origin velocity, and use the origin velocity to obtain the state matrix;

[0044] Step (c) calculates the standard output matrix or the equivariant output matrix;

[0045] Step (d) selects the state and output gain matrices, and approximately solves the equation of EqF numerically to update the observer state and the Riccati state;

[0046] The specific state update equation is as follows:

[0047] Observer dynamics:

[0048]

[0049] where, is the observer state, Λ is the equivariant lift, φ is the state symmetry, u is the input, Δ is the correction term,

[0050] Correction term calculation:

[0051]

[0052] where, ∑ is the Riccati term, C t is the output matrix, N t is the output gain matrix,

[0053] Riccati equation:

[0054]

[0055] where, is the state matrix, M t is the state gain matrix,

[0056] Step (e) introduces an adaptive weight adjustment strategy to improve the robustness and accuracy of the system in dealing with complex environments. For each observation value y i , the weight w can be dynamically adjusted according to its corresponding measurement noise i . Set the weight according to the noise level:

[0057]

[0058] When calculating the gain, use the adjusted measurement noise covariance matrix R, where R is a diagonal matrix whose diagonal elements are the inverse variances of the respective measurements:

[0059] R = diag(1 / w1, 1 / w2,..., 1 / w n )

[0060] Use the adjusted R in the dynamic equation of the state error to calculate the residual Δ and update the state error e

[0061]

[0062] Among them, h(x i ) and respectively represent the i-th actual measurement value and the estimated value. The adjusted residual Δ and the state error e are used to update the state estimate and its covariance matrix P.

[0063] On the basis of the above scheme, preferably, in step (5), the current robot pose is calculated, and according to the robot pose, the scanned point cloud data is added to the global map.

[0064] First, a surfel is defined, which is usually composed of a 3D center point (G p ), a surface normal vector (n), and a radius (r), jointly describing a local surface patch in the environment.

[0065] Then, the space is divided into voxels of a fixed size, each voxel corresponding to a small cube in the space. The preprocessed point cloud data is traversed, and each point is assigned to its corresponding voxel. For each voxel, if it contains enough points, it is considered that the points in this voxel can represent a surfel.

[0066] (a) Calculate the normal: In the discrete point cloud data, the normal is usually estimated by the points in the local neighborhood. To estimate the normal of a certain point, the points in the local neighborhood around this point need to be considered. The local neighborhood can be defined by the search radius or the number of nearest neighbors. The search radius defines the maximum distance of the points in the neighborhood, while the number of nearest neighbors defines the number of points in the neighborhood. The search radius of the local neighborhood is defined as 0.1, and the normal of each point in this radius is calculated.

[0067] (b) Point cloud clustering: Cluster the point cloud based on the Euclidean distance. The Euclidean distance is the most intuitive distance metric between two points, defined as the straight-line distance between two points. For two points P1(x1, y1, z1) and P2(x2, y2, z2) in three-dimensional space, their Euclidean distance d can be expressed as:

[0068]

[0069] If the Euclidean distance between two points is less than the distance d, then these two points are considered to belong to the same cluster. If the Euclidean distance between two points is greater than the distance d, then these two points are considered not to belong to the same cluster. By traversing all points and connecting the points with similar distances, multiple independent clusters are finally formed.

[0070] (c) Create a surfel model: Calculate the center point and average normal for each cluster, and create a surfel model. By looping through each cluster, calculate the center point and average normal of each cluster. The center point is the average of the coordinates of all points in the cluster. For a cluster containing n points, assuming the coordinates of these points are p1, p2, …, pn, where each point pi = (xi, yi, zi), the center point c can be expressed as:

[0071]

[0072] The average normal is the average of the normal vectors of all points in the cluster. For a cluster containing n points, assuming the normal vectors of these points are n1, n2, …, nn, where each normal vector n i = (n ix , n iy , n iz ), the average normal A can be expressed as:

[0073]

[0074] The normal represents the direction of the surface. Calculating the average normal provides an overall estimate of the surface direction of the cluster. Based on the average normal and center point of each cluster, create a surfel and add it to the global surfel map.

[0075] In this way, the LiDAR point cloud can be converted into a surfel representation. At the same time, a priority-based indexing algorithm is proposed based on the hash index to ensure the efficiency and real-time performance of this process. This surfel-based map representation method can not only provide a more accurate environmental description but also support efficient data association and state estimation. The specific hash index algorithm is introduced as follows:

[0076] Step (a) For each transformed point P = {p1, p2,..., p n}), where each point p i is a three-dimensional vector p i = (x i , y i , z i [[ID=D38]]). Using the method of random projection, for each dimension, generate a random hyperplane. The hyperplane can be defined by a weight vector w = (w x , w y , w z ) and a bias b. The weight vector w can be regarded as the normal vector of the hyperplane, and the bias b determines the position of the hyperplane in space.

[0077] Step (b) proposes a novel hash index mechanism based on local priority. The voxel index i is mapped to a hash table using a hash function to quickly search for and update information in the voxel. Assuming the hash function is h(i), the hash value h w (p) is:

[0078]

[0079] where w·p represents the dot product of point p and weight vector w, and r is the bucket width, which controls the granularity of the hash value.

[0080] Multiple hash tables are created, and each table uses a different combination of hash functions. For example, if k independent hash functions are used, each point will be mapped to k different hash values and thus assigned to k different buckets. For the query point q, the hash values of q in all hash tables are calculated using the same family of hash functions. For the j-th hash function hj, calculate hj(q).

[0081] Step (c) uses the hash value h w (p) to search for the corresponding point cloud in the hash table. For the query point q, the hash values of q in all hash tables are calculated using the same family of hash functions. For the j-th hash function hj, calculate hj(q). Check whether there is a surfel in the point cloud and its surrounding voxels. If there is, calculate the residual from the point to the surfel and determine whether to add the point to the surfel according to the criteria. If there is no surfel, create a new surfel in the voxel.

[0082] Step (d) If the point is successfully associated with a certain surfel, update the attributes of the surfel, such as the center position, normal direction, and radius. To add the new point to the map, the point-to-surfel association operation also needs to be performed. This process involves calculating the distance from the point to the nearest surfel and determining whether to add the point to the surfel according to certain criteria (such as the flatness threshold and association score).

[0083] Compared with the prior art, the advantages of the present invention are:

[0084] The present invention proposes a tightly coupled LIO system based on an equivariant filter and a Surfel map. Compared with the current LIO system, the present invention can perform iterative updates more efficiently, improving the overall robustness and stability of the system;

[0085] The present invention uses surfels to construct a global map, greatly improving the efficiency and accuracy of SLAM;

[0086] At the same time, the hash algorithm is used to accelerate indexing, improving the speed of local map matching search. Brief Description of the Drawings

[0087] Figure 1 This is a comparison chart of the algorithm of the present invention and the A-LOAM algorithm;

[0088] Figure 2 This is the comparison of the y-z axis trajectories;

[0089] Figure 3 This is the comparison of the x-y axis trajectories;

[0090] Figure 4 This is a comparison chart of the algorithm of the present invention and the LIO-SAM algorithm;

[0091] Figure 5 This is a comparison chart of the algorithm of the present invention and the LIO-SAM algorithm;

[0092] Figure 6 This is the flow chart of the present invention. Detailed Embodiment

[0093] For a better understanding of the present invention, the present invention will be further described below with reference to the accompanying drawings in the embodiments of the present invention, but it is not a limitation of the present invention. Without departing from the design concept of the present invention, various variations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope of the present invention.

[0094] A tightly coupled LIO system based on an equivariant filter and a Surfel map includes a remote control vehicle, a PC, an IMU, and a lidar. The lidar and the IMU (Inertial Measurement Unit) are fixed on the vehicle, and the PC is used to control the movement of the vehicle. The time stamps of the lidar and the IMU are synchronized. The IMU collects the pose transformation of the vehicle body, and the lidar collects the surrounding environment data to sense the external environment. A corresponding radar driver package is installed on the PC to visualize the mapping results of the lidar.

[0095] The specific solutions are as follows:

[0096] I. Installation of experimental tools. By installing a lidar and an IMU on the remote control vehicle, the environment and the pose transformation of the vehicle body are recorded to obtain the three-dimensional point cloud of the environment and the state transformation amount.

[0097] (1) Install the lidar and the IMU on the vehicle. The lidar needs to be installed on the roof to avoid being blocked by the vehicle body during scanning, and the IMU needs to be installed in a pose that is always relatively fixed with the lidar.

[0098] (2) Install the lidar driver to obtain an ordered laser point cloud data frame from the driver.

[0099] (3) Judge the laser point cloud information to ensure the orderliness and integrity of the data;

[0100] (4) Perform necessary calibration work on the IMU and synchronize the timestamps of the IMU and lidar.

[0101] (5) Run this system to start collecting data.

[0102] Collect the surrounding environment and the motion state of the device through the lidar and IMU. Since the scanning frequency of the lidar is much lower than the working frequency of the IMU, there must be several IMU frames between two lidar frames. This system plans to package and store one frame of lidar point cloud and multiple frames of IMU, that is, package the two lidar frames and several IMU frames from time t - 1 to time t into a unified data frame for subsequent processing. At the same time, perform voxel downsampling on the point cloud obtained by the lidar. The purpose of voxel downsampling is to reduce the data volume by reducing the number of points in the point cloud while trying to maintain the shape characteristics of the point cloud.

[0103] Voxel grid downsampling:

[0104] (a) First, calculate the bounding box, which is the minimum three-dimensional cuboid bounding box of the point cloud data set. Assume the point cloud data set is P = {pi | i = 1, 2,..., N}, and each point pi has coordinates (xi, yi, zi). The boundaries of the bounding box can be calculated by the following formulas:

[0105] x min = min(x i ), x max = max(x i )

[0106] y min = min(y i ), y max = max(y i )

[0107] z min = min(z i ), z max = max(z i )

[0108] (b) Divide the cube, divide the bounding box into small cubes (voxels) along the x, y, and z directions, and the side length of each voxel is l. The size of the voxel grid can be calculated by the following formula:

[0109]

[0110] (c) Calculate the centroid or center. In each non-empty voxel, calculate the centroid of all points in the voxel or use the voxel center to approximate all points. The calculation formula for the centroid is as follows:

[0111]

[0112] 2. Parse the packaged data frame. According to the IMU motion model and discrete model, pre-integrate all IMUs between two radar frames starting from time t-1, and calculate a state quantity X at time t. t .

[0113] First, define the state quantity X t , where R t is the rotation matrix at time t, v t is the velocity at time t, p t is the position at time t,

[0114]

[0115] From the IMU measurement model: measurement value = true value + noise + zero bias, the following formula can be derived:

[0116]

[0117] The noise and zero bias both conform to Gaussian distribution.

[0118] The discrete model of IMU is as follows:

[0119] R(t+Δt)=R(t)Exp(Ω)

[0120]

[0121] According to the IMU measurement model and the continuous discrete model, each frame of IMU data is integrated to obtain the state quantity X t , calculate the velocity and displacement.

[0122] 3. Compensate for the distorted point cloud. Since the laser radar scan takes a certain amount of time to complete, and the remote control car may move or rotate during this period, the collected point cloud data will be distorted. This application will calculate the state quantity X at time t. t Used for back propagation, all the point cloud data scanned from time t-1 to time t are compensated to time t. The whole process involves the following steps:

[0123] Step (a) IMU data pre-integration: Using the angular velocity and acceleration data provided by the IMU, the motion state of the LiDAR sensor at each point cloud data acquisition moment is estimated through a numerical integration method to obtain the sensor's pose change;

[0124] Step (b) Timestamp processing of point cloud data: Each LiDAR point cloud data has a timestamp associated with it, indicating when the point was collected. The timestamp is used to determine the estimated pose of the sensor for each point at the time of collection.

[0125] Step (c) Backpropagation: Using the attitude information obtained by IMU integration, perform backpropagation on each LiDAR point cloud data to compensate for the distortion caused by sensor movement. For each point cloud point, a transformation relative to a certain reference time will be calculated;

[0126] Step (d) Point cloud transformation compensation: Transform each point cloud point according to the IMU attitude corresponding to its acquisition time to simulate the position if the point was acquired in a stationary state, so that all point cloud data are regarded as being acquired at the same time, enabling more accurate subsequent processing.

[0127] During the backpropagation process, the discrete model of the IMU is used to gradually update the state of the remote control vehicle. The specific formula is as follows:

[0128]

[0129] After the above steps, all point cloud data are projected to the end time t of the scan, eliminating the scan distortion caused by the movement of the unmanned aerial vehicle. The compensated point cloud data can be directly used for subsequent map construction and state estimation.

[0130] IV. Calculate the residual, input the equivariant filter (EqF) and iterate until convergence. Match the corrected local point cloud map obtained by backpropagation with the global map to calculate the residual. At the same time, input the rough state quantity X t obtained by IMU pre-integration and the calculated residual into the equivariant filter (EqF), and update the gain of the equivariant filter according to the convergence of the residual. The specific steps are as follows:

[0131] Step (a) Match the local point cloud information scanned at time t with the global point cloud information to calculate the residual. The residual r can be calculated by the following formula:

[0132]

[0133] where h(ξ) is the actual measurement value. is the value predicted based on the current state estimate ξ.

[0134] Step (b) Adjust the iteration strategy of the equivariant filter (EqF) according to the magnitude of the residual r. If the value of the current residual r is greater than the set threshold, continue to iterate the covariance and gain function of the equivariant filter (EqF); if the value of the current residual r is less than the set threshold, it means that the iteration update is complete. The iteration update process is as follows:

[0135] Step (1) Use system inputs (e.g., IMU data) and measurements to calculate the origin velocity, and use the origin velocity to obtain the state matrix.

[0136] Step (2) Calculate the standard output matrix or the equivariant output matrix.

[0137] Step (3) Select the state and output gain matrices, and approximate the equation of EqF numerically to update the observer state and the Riccati state.

[0138] The specific state update equation is as follows:

[0139]

[0140] where, the observer state, Λ is the equivariant lift, φ is the state symmetry, u is the input, and Δ is the correction term.

[0141] Correction term calculation:

[0142]

[0143] where, ∑ is the Riccati term, C t is the output matrix, N t is the output gain matrix.

[0144] Riccati equation:

[0145]

[0146] where, is the state matrix, M t is the state gain matrix.

[0147] Step (4) Introduce an adaptive weight adjustment strategy to improve the robustness and accuracy of the system in dealing with complex environments. For each observation value y i , the weight w can be dynamically adjusted according to its corresponding measurement noise i . Set the weight according to the noise level:

[0148]

[0149] When calculating the gain, use the adjusted measurement noise covariance matrix R, where R is a diagonal matrix whose diagonal elements are the inverse variances of the respective measurements:

[0150] R = diag(1 / w1, 1 / w2,..., 1 / w n )

[0151] Use the adjusted R in the dynamic equation of the state error to calculate the residual Δ and update the state error e

[0152]

[0153] where h(x i ) and represent the i-th actual measurement value and estimated value respectively. Use the adjusted residual Δ and state error e to update the state estimate and its covariance matrix P.

[0154] V. Construct a surfel and add the local surfel map to the global surfel. Further, according to the state quantity updated by the above iteration Take

[0155] Furthermore, step (5) uses the state quantity updated in the previous step to calculate the current robot pose. According to the robot pose, add the scanned point cloud data to the global map. The overall implementation steps are as follows:

[0156] First, define a surfel, which usually consists of a 3D center point (G p ), a surface normal vector (n), and a radius (r). These parameters jointly describe a local surface patch in the environment.

[0157] Then, divide the space into voxels of a fixed size. Each voxel corresponds to a small cube in the space. Traverse the preprocessed point cloud data and assign each point to its corresponding voxel. For each voxel, if it contains enough points (for example, more than a certain threshold), then the points within this voxel are considered to be able to represent a surfel.

[0158] (a) Calculate the normal: In discrete point cloud data, the normal is usually estimated by the points within a local neighborhood. To estimate the normal of a certain point, it is necessary to consider the points within the local neighborhood around this point. The local neighborhood can be defined by a search radius or the number of nearest neighbors. The search radius defines the maximum distance of the points within the neighborhood, while the number of nearest neighbors defines the number of points within the neighborhood. In this patent, the search radius of the local neighborhood is defined as 0.1. Calculate the normal of each point within this radius.

[0159] (b) Point cloud clustering: Perform clustering on the point cloud based on the Euclidean distance. The Euclidean distance is the most intuitive distance metric between two points, defined as the straight-line distance between two points. For two points P1(x1, y1, z1) and P2(x2, y2, z2) in three-dimensional space, their Euclidean distance d can be expressed as:

[0160]

[0161] If the Euclidean distance between two points is less than distance d, then these two points are considered to belong to the same cluster. If the Euclidean distance between two points is greater than distance d, then these two points are considered not to belong to the same cluster. By traversing all points and connecting points with close distances, multiple independent clusters are finally formed.

[0162] (c) Create a surfel model: Calculate the center point and average normal for each cluster to create a surfel model. By looping through each cluster, calculate the center point and average normal of each cluster. The center point is the average of the coordinates of all points in the cluster. For a cluster containing n points, assuming the coordinates of these points are p1, p2, …, pn, where each point pi = (xi, yi, zi), then the center point c can be expressed as:

[0163]

[0164] The average normal is the average of the normal vectors of all points in the cluster. For a cluster containing n points, assuming the normal vectors of these points are n1, n2, …, nn, where each normal vector n i =(n ix , n iy , n iz ), then the average normal A can be expressed as:

[0165]

[0166] The normal represents the direction of the surface and is very important for applications such as 3D reconstruction and surface rendering. Calculating the average normal can provide an overall estimate of the surface direction of the cluster. Next, create a surfel based on the average normal and center point of each cluster and add it to the global surfel map.

[0167] In this way, the LiDAR point cloud can be converted into a surfel representation. At the same time, a priority-based indexing algorithm is proposed based on the hash index to ensure the efficiency and real-time performance of this process. This surfel-based map representation method can not only provide a more accurate environmental description but also support efficient data association and state estimation. The specific hash index algorithm is introduced as follows:

[0168] Step (a) For each transformed point P = {p1, p2,..., p n}), where each point p i is a three-dimensional vector p i =(x i , y i , zi ). Using the method of random projection, for each dimension, a random hyperplane is generated. The hyperplane can be defined by a weight vector w = (w x , w y , w z ) and a bias b. The weight vector w can be regarded as the normal vector of the hyperplane, and the bias b determines the position of the hyperplane in space.

[0169] Step (b) proposes a new hash index mechanism based on local priority. Using a hash function, the voxel index i is mapped into the hash table to quickly search and update the information in the voxel. Assuming the hash function is h(i), then the hash value h w (p) is:

[0170]

[0171] where w·p represents the dot product of the point p and the weight vector w, and r is the bucket width, which controls the granularity of the hash value.

[0172] Multiple hash tables are created, and each table uses a different combination of hash functions. For example, using k independent hash functions, each point will be mapped to k different hash values and thus assigned to k different buckets. For the query point q, use the same family of hash functions to calculate its hash values in all hash tables. For the j-th hash function hj, calculate hj(q).

[0173] Step (c) uses the hash value h w (p) to search for the corresponding point cloud in the hash table. For the query point q, use the same family of hash functions to calculate its hash values in all hash tables. For the j-th hash function hj, calculate hj(q). Check whether there is a surfel in this point cloud and the surrounding voxels. If so, calculate the residual from the point to the surfel and determine whether to add the point to this surfel according to the criteria. If there is no surfel, create a new surfel in this voxel.

[0174] Step (d) If the point is successfully associated with a certain surfel, update the attributes of this surfel, such as the center position, normal direction, and radius, etc. To add a new point to the map, the association operation of the point to the surfel also needs to be performed. This process involves calculating the distance from the point to the nearest surfel and determining whether to add the point to this surfel according to certain criteria (such as the flatness threshold and association score).

[0175] Now, this method is experimentally compared with several current classic SLAM algorithms to verify the superiority of this algorithm. The specific algorithms for comparison are A-LOAM, LIO-SAM, and FAST-LIO.

[0176] As Figure 1 shown, it can be seen from the trajectory comparison diagram that the A-LOAM algorithm can maintain a high accuracy in the initial stage of the running result, but there is a large deviation in the middle and late stages, and even fails to complete the mapping. Compared with this algorithm, a complete trajectory can be generated.

[0177] From the comparison Figure 2 it can be seen that the LIO-SAM algorithm and this algorithm have different degrees of drift on the z-axis at the horizontal scale, but the loop detection mechanism of this algorithm effectively reduces the drift, demonstrating more stable robustness. From the comparison Figure 3 it can be seen that they show similar effects at the horizontal scale. Generally speaking, this algorithm is superior to the LIO-SAM algorithm.

[0178] From Figure 4 it can be observed that the A-LOAM algorithm fails to complete the mapping, showing performance degradation; while the FAST-LIO algorithm has unacceptable drift. In contrast, this algorithm has excellent mapping effects in complex scenarios, with strong robustness and stability. This result shows the importance and effectiveness of the multi-sensor tight coupling strategy in maintaining trajectory accuracy and stability.

[0179] The above embodiments are only for illustrating the technical concept and features of the present invention, and the purpose is to enable those who are familiar with this technology to understand the content of the present invention and implement it accordingly, and it cannot be used to limit the protection scope of the present invention. Any equivalent transformation or modification made according to the spirit and essence of the present invention should be covered by the present invention.

Claims

1. A tightly-coupled LIO system based on equivariant filters and Surfel maps, characterized in that, It includes the following steps: Step (1): Collect the three-dimensional point cloud data of the surrounding environment and the motion state information of the device through a lidar and an IMU respectively, and downsample the original point cloud data obtained by the lidar using a voxel grid downsampling method. Step (2) performs pre-integration operations on the collected IMU data based on the kinematic model of the IMU and its discretized representation, calculates the incremental state quantities within the time interval, and obtains the state estimate value X at time t t , including position, velocity, attitude, and the bias parameters of the IMU sensor; Step (3) uses the state prediction value X at time t obtained by IMU pre-integration t , and performs backpropagation again to perform motion compensation on the original point cloud data collected by the lidar. All points from time t-1 to time t are compensated to time t. The compensated point cloud data is denoted as P t ; Step (4) uses an equivariant filter algorithm to optimize and update the initial state estimate obtained in step (2). Meanwhile, the Kalman gain and covariance are continuously adjusted and updated by calculating the magnitude of the residual until convergence, so as to obtain a more accurate and robust state estimate. Step (5) is based on the updated state estimate value Convert the motion-compensated point cloud data P t to the global coordinate system, introduce a locality-preferred hash function indexing algorithm to index the point cloud data, and efficiently merge it into the global Surfel map through the indexing mechanism and the hash function. At the same time, dynamically update the surface features in the Surfel map according to the newly added point cloud data to maintain the real-time performance and accuracy of the map.

2. The tightly coupled LIO system based on an equivariant filter and a Surfel map according to claim 1, characterized in that In step (1.1), the collected lidar point cloud is downsampled using a voxel grid. (a) First, calculate the bounding box, calculate the minimum three-dimensional cuboid bounding box of the point cloud data set. Assume the point cloud data set is P = {pi | i = 1, 2,..., N}, and each point pi has coordinates (xi, yi, zi). The boundaries of the bounding box can be calculated by the following formula: x min = min(x i ), x max = max(x i ) y min = min(y i ), y max = max(y i ) z min = min(z i ), z max = max(z i ) (b) Divide the cube Divide the bounding box into small cubes equally along the x, y, and z directions. Each small cube is used as a voxel, and the side length of each voxel is l. The size of the voxel grid can be calculated by the following formula: (c) Calculate the centroid or center In each non-empty voxel, calculate the centroid of all points within the voxel or use the voxel center to approximate all points. The formula for calculating the centroid is as follows: Step (1.2): Package each frame of downsampled point cloud with several frames of IMU (until the next lidar frame) and transmit it to the LIO system.

3. The tightly coupled LIO system based on equivariant filters and Surfel maps according to claim 1, characterized in that: Step (2): Pre-integrate the collected IMU data. The entire implementation process is as follows: (a) First, define the state quantity X t , where R t is the rotation matrix at time t, v t is the velocity at time t, p t is the pose at time t, (b) From the IMU measurement model: measurement value = true value + noise + bias, the following formula can be deduced: where both the noise and the bias conform to a Gaussian distribution. The discrete model of the IMU is as follows: R(t + Δt) = R(t)Exp(Ω) Integrate each frame of IMU data according to the IMU measurement model and the continuous-discrete model to obtain the state quantity X t , and calculate the velocity and displacement.

4. The tightly coupled LIO system based on the equivariant filter and the Surfel map according to claim 1, characterized in that: In step (3), perform motion compensation on the point cloud according to the change of the state quantity. The entire motion compensation process is as follows: Step (a): IMU data pre-integration: Use the angular velocity and acceleration data provided by the IMU to estimate the motion state of the LiDAR sensor at each point cloud data acquisition moment through numerical integration to obtain the pose change of the sensor. Step (b): Timestamp processing of point cloud data: Each LiDAR point cloud data has a timestamp associated with it, indicating when the point was acquired. The timestamp is used to determine the estimated pose of the sensor at the time of acquisition of each point. Step (c): Backpropagation: Use the attitude information obtained by IMU integration to perform backpropagation on each LiDAR point cloud data to compensate for the distortion caused by sensor motion. For each point cloud point, a transformation relative to a certain reference moment will be calculated. Step (d): Point cloud transformation compensation: Transform each point cloud point according to the IMU attitude corresponding to its acquisition moment to simulate the position if the point was acquired in a stationary state, so that all point cloud data are regarded as being acquired at the same moment, and subsequent processing can be performed more accurately.

5. The tightly coupled LIO system based on the equivariant filter and the Surfel map according to claim 4, wherein: Update the state quantity X using the equivariant filtering algorithm t The entire process is as follows: Step (a): Match the local point cloud information scanned at time t with the full map point cloud information to calculate the residual. Step (b): Calculate the origin velocity using the system input and the measurement value, and use the origin velocity to obtain the state matrix. Step (c) calculates the standard output matrix or the equivariant output matrix; Step (d) selects the state and output gain matrices, numerically approximates the equations of EqF, and updates the observer state and the Riccati state; The specific state update equations are as follows: Observer dynamics: where, is the observer state, Λ is the equivariant lift, φ is the state symmetry, u is the input, and Δ is the correction term, Correction term calculation: where, ∑ is the Riccati term, C t is the output matrix, N t is the output gain matrix, Riccati equation: Among them, is the state matrix, M t is the state gain matrix, Step (e) introduces an adaptive weight adjustment strategy to improve the robustness and accuracy of the system in dealing with complex environments. For each observation y i , the weight w can be dynamically adjusted according to its corresponding measurement noise . Set the weight according to the noise level: i ​ When calculating the gain, use the adjusted measurement noise covariance matrix R, where R is a diagonal matrix whose diagonal elements are the inverse variances of the respective measurements: R = diag(1 / w1, 1 / w2,..., 1 / w n ) Use the adjusted R in the dynamic equation of the state error to calculate the residual Δ and update the state error e where h(x i ) and represent the i-th actual measurement value and the estimated value, respectively. The adjusted residual Δ and the state error e are used to update the state estimate and its covariance matrix P.

6. The tight-coupled LIO system based on equivariant filters and Surfel maps according to claim 1, characterized in that Step (5) calculates the current robot pose, and according to the robot pose, adds the scanned point cloud data to the global map. First, define a surfel, which is typically composed of a 3D center point (G p ), a surface normal vector (n), and a radius (r), together describing a local surface patch in the environment. Then, the space is divided into voxels of a fixed size, each voxel corresponding to a small cube in space. Traverse the preprocessed point cloud data and assign each point to its corresponding voxel. For each voxel, if it contains enough points, it is considered that the points within this voxel can represent a surfel. (a) Calculate the normal: In discrete point cloud data, the normal is usually estimated by the points within the local neighborhood. To estimate the normal of a certain point, the points within the local neighborhood around this point need to be considered. The local neighborhood can be defined by the search radius or the number of nearest neighbors. The search radius defines the maximum distance of the points within the neighborhood, while the number of nearest neighbors defines the number of points within the neighborhood. Define the search radius of the local neighborhood as 0.1 and calculate the normal of each point within this radius; (b) Point cloud clustering: Perform clustering on the point cloud based on the Euclidean distance. The Euclidean distance is the most intuitive distance metric between two points, defined as the straight-line distance between two points. For two points P1(x1, y1, z1) and P2(x2, y2, z2) in three-dimensional space, their Euclidean distance d can be expressed as: If the Euclidean distance between two points is less than the distance d, then these two points are considered to belong to the same cluster. If the Euclidean distance between two points is greater than the distance d, then these two points are considered not to belong to the same cluster. By traversing all points and connecting the points with close distances, multiple independent clusters are finally formed; (c) Create a surfel model: Calculate the center point and the average normal for each cluster to create a surfel model. By looping through each cluster, calculate the center point and the average normal of each cluster. The center point is the average of the coordinates of all points in the cluster. For a cluster containing n points, assuming the coordinates of these points are p1, p2, …, pn, where each point pi = (xi, yi, zi), then the center point c can be expressed as: The average normal is the average of the normal vectors of all points in the cluster. For a cluster containing n points, assuming the normal vectors of these points are n1, n2, …, nn respectively, where each normal vector n i =(n ix , n iy , n iz ), the average normal A can be expressed as: The normal represents the direction of the surface. Calculating the average normal provides an overall estimate of the direction of the clustered surface. Based on the average normal and the center point of each cluster, a surfel is created and added to the global surfel map. In this way, the LiDAR point cloud can be converted into a surfel representation. At the same time, a priority-based indexing algorithm is proposed based on the hash index to ensure the efficiency and real-time performance of this process. This surfel-based map representation method can not only provide a more accurate environmental description but also support efficient data association and state estimation.

Images