Incremental Kd-Tree laser-inertial fusion positioning method based on improvement

Abstract

The invention belongs to the technical field of unmanned aerial vehicle control, and particularly relates to an incremental Kd-Tree laser-inertial fusion positioning method based on improvement, the method provided by the invention adopts a practical filtering scheme, the operand is small, the real-time performance is high, and the requirement of a laser radar on an illumination condition is lower than that of a camera, so that the method is very practical. The noise is much lower than that of visual measurement, so that the precision of the method is higher than that of a visual algorithm; according to the method provided by the invention, two kinds of data are fused in a deep level, so that the method is not liable to fail during strenuous exercise, and a GPU is not needed during operation. The filtering algorithm adopts an iterative error state Kalman filtering algorithm, so that the precision of the system is not very low under nonlinear motion, and the map management efficiency is also improved by the improved incremental Kd-Tree algorithm in the aspect of map management. In conclusion, aiming at a special scene without a GNSS signal, the robot can efficiently position the robot and construct a surrounding environment map only by depending on the sensor of the robot.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) control technology, specifically relating to an improved incremental Kd-Tree laser-inertial fusion positioning method. Background Technology

[0002] Unmanned systems need external means, such as the Global Navigation Satellite System (GNSS), to achieve self-localization in a given area. However, GNSS is limited by signal interference, terrain, and scene conditions, and generally cannot function properly in areas with weak signals, such as under trees, overpasses, between buildings, tunnels, and underground parking lots. This prevents autonomous robots from obtaining accurate positioning in their workspace, affecting their work quality and even causing them to collide with obstacles and be damaged. In this environment, the mainstream solution for simultaneous localization and mapping (SLAM) of robots still uses single-sensor algorithms because the information obtained is relatively simple and easier to process. Currently popular single-sensor algorithms are mainly visual inertial oscillator (VIO) systems, which fuse cameras and inertial measurement units (IMUs), and radar inertial oscillator (LIO) systems, which fuse lidar and IMUs.

[0003] Currently, mainstream visual inertial algorithms mainly fall into two categories: direct-method and feature-point-based methods. Commonly used algorithms such as LSD-SLAM and DSO are primarily based on the direct method. LSD-SLAM does not rely on extracting feature points to obtain information; instead, it directly uses pixel intensity for feature matching and can run in real-time, making it applicable to dynamic environments. Although its real-time performance is excellent, it requires a large amount of data to compute in complex scenarios and is highly dependent on lighting conditions, with performance degrading in both bright light and dark environments. DSO employs an optimization method that minimizes photometric error and considers a photometric calibration model; however, this method abandons the loop closure detection part. To enhance localization functionality in DSO, all frames must be saved, whereas map reuse is relatively simple in feature-point methods. Algorithms such as ORB-SLAM are feature-based monocular SLAM algorithms that add map initialization and loop closure detection functions, significantly improving processing speed and map accuracy, and enabling more effective tracking of target objects. However, this method has a high computational load, is prone to frame loss during rotation, and has relatively sparse point cloud data. Current visual inertial odometry systems suffer from limited accuracy due to their Markovian characteristics; relying solely on the previous state makes it difficult to establish a global correlation between the current and historical states. While optimization methods can provide more accurate estimates, their computational complexity is high, and filtering-based solutions remain practical in resource-constrained scenarios. It is worth noting that due to significant visual measurement noise, even with IMU data fusion, the accuracy of visual inertial systems is still inferior to that of lidar inertial systems.

[0004] Although most current laser inertial navigation algorithms perform well in evaluations on public datasets, they still have certain limitations in practical applications. For example, in the LOAM algorithm, the IMU is only used for distortion correction and not for fusion, making it prone to failure during turbulent motion; the LeGO-LOAM algorithm's ground segmentation is prone to drift in complex terrain and is only used for initial values; the LIO-SAM algorithm is based on graph optimization, requires GPU acceleration, and accumulates significant errors over long periods without GPS; the LINS algorithm is based on the Iterative Kalman Filter (EKF) algorithm, which has low accuracy under nonlinear motion; and the DLIO algorithm, based on the direct method, experiences a sharp performance drop on low-line-count LiDARs and lacks map management.

[0005] The existing algorithms mentioned above all have obvious disadvantages. Therefore, this invention proposes an improved incremental Kd-Tree laser-inertial fusion positioning method. Summary of the Invention

[0006] The purpose of this invention is to provide an improved incremental Kd-Tree laser-inertial fusion positioning method, which uses the fusion processing and analysis of inertial measurement unit (IMU) information and lidar information to enhance the anti-interference capability of the system and achieve better capture of changes in dynamic environments for real-time positioning and decision-making.

[0007] The specific technical solution adopted by this invention is as follows:

[0008] An improved incremental Kd-Tree laser-inertial fusion positioning method includes the following steps:

[0009] Step 1: Establish the coordinate system of the laser inertial system; first, determine the world coordinate system: W system; robot body coordinate system: B system; laser radar coordinate system: L system; and IMU coordinate system: I system; then, according to the static coordinate transformation, multiply the data measured by each sensor by the corresponding static transformation matrix of the two coordinate systems to transform it into other coordinate systems for unified processing;

[0010] Step 2: Extract point cloud data; use the mean method statistical filtering to remove outliers in the point cloud, use voxel filtering to downsample the point cloud, then calculate the curvature and extract representative geometric features for inter-frame matching and map building, thereby improving the computational efficiency and robustness of the system.

[0011] Step 3: Perform motion compensation on the point cloud data after initial processing; considering the motion changes of the lidar over time, estimate and adjust the motion changes in the signal, collect all point clouds within the time period for motion compensation, and unify them to the same coordinate system;

[0012] Step 4: Fusion of LiDAR point cloud data and IMU data; After processing the LiDAR and IMU data, calculate the distance error between the current frame point cloud features and the corresponding features in the map to obtain the LiDAR residual, calculate the error between the state predicted by the IMU integral and the state estimated by the LiDAR to obtain the IMU residual, and then use the iterative error state Kalman filter algorithm to solve the optimization problem to achieve tight coupling;

[0013] Step 5: Store the point cloud map and update the point cloud data; improve the original incremental Kd-Tree algorithm to maintain the local point cloud map and improve the efficiency of adding and deleting points, and finally realize the odometry output of the laser inertial system and the construction of the point cloud map.

[0014] The technical effects achieved by this invention are as follows:

[0015] In this invention: Compared with the commonly used algorithms of visual inertial systems introduced above, the method proposed in this invention adopts a filtering scheme that is still practical, with less computation, high real-time performance, and the requirements of LiDAR for lighting conditions are lower than those of cameras, and its noise is also much lower than that of visual measurement. Therefore, the accuracy of this invention is higher than that of the aforementioned visual algorithms.

[0016] In this invention, compared to other commonly used laser inertial system algorithms, the proposed method fuses two types of data at a deep level, making it less prone to failure during turbulent motion and not requiring a GPU for computation. The filtering algorithm employs an iterative error state Kalman filter, ensuring the system's accuracy remains high even under nonlinear motion. Furthermore, the improved incremental Kd-Tree algorithm enhances map management efficiency.

[0017] In summary, this invention proposes an improved incremental Kd-Tree laser-inertial fusion positioning algorithm for special scenarios without GNSS signals, enabling robots to efficiently locate themselves and build maps of their surroundings using only their own sensors. Attached Figure Description

[0018] Figure 1 This is the present invention;

[0019] Figure 2 This is the present invention;

[0020] Figure 3 This is the present invention. Detailed Implementation

[0021] To make the objectives and advantages of this invention clearer, the invention will be specifically described below with reference to embodiments. It should be understood that the following text is merely used to describe one or more specific embodiments of the invention and does not strictly limit the scope of protection specifically claimed by the invention.

[0022] Example 1:

[0023] like Figure 1 As shown, an improved incremental Kd-Tree laser-inertial fusion positioning method is characterized by the following steps:

[0024] Step 1: Establish the coordinate system of the laser inertial system; first, determine the world coordinate system: W system; robot body coordinate system: B system; laser radar coordinate system: L system; and IMU coordinate system: I system; then, according to the static coordinate transformation, multiply the data measured by each sensor by the corresponding static transformation matrix of the two coordinate systems to transform it into other coordinate systems for unified processing;

[0025] Step 2: Extract point cloud data; use the mean method statistical filtering to remove outliers in the point cloud, use voxel filtering to downsample the point cloud, then calculate the curvature and extract representative geometric features for inter-frame matching and map building, thereby improving the computational efficiency and robustness of the system.

[0026] Step 3: Perform motion compensation on the point cloud data after initial processing; considering the motion changes of the lidar over time, estimate and adjust the motion changes in the signal, collect all point clouds within the time period for motion compensation, and unify them to the same coordinate system;

[0027] Step 4: Fusion of LiDAR point cloud data and IMU data; After processing the LiDAR and IMU data, calculate the distance error between the current frame point cloud features and the corresponding features in the map to obtain the LiDAR residual, calculate the error between the state predicted by the IMU integral and the state estimated by the LiDAR to obtain the IMU residual, and then use the iterative error state Kalman filter algorithm to solve the optimization problem to achieve tight coupling;

[0028] Step 5: Store the point cloud map and update the point cloud data; improve the original incremental Kd-Tree algorithm to maintain the local point cloud map and improve the efficiency of adding and deleting points, and finally realize the odometry output of the laser inertial system and the construction of the point cloud map.

[0029] In step 1:

[0030] The world coordinate system coincides with the lidar coordinate system at the moment of power-on. Data measured in the other three coordinate systems must eventually be converted to the world coordinate system for unified processing. The fuselage coordinate system is defined as a right-handed coordinate system with its origin located at the center of mass. The positive X-axis is in the same direction as the movement, the positive Y-axis is to the left, and the positive Z-axis is vertically upward, also satisfying the right-hand rule. The lidar coordinate system and the IMU coordinate system are also right-handed coordinate systems.

[0031] Assume the robot's pose in the world coordinate system at time k-1 is By fusing information collected from lidar and IMU, the robot's pose transformation from time k-1 to time k can be estimated. The pose in the world coordinate system at time k is then easily calculated as follows, and the pose at each time step forms an odometry.

[0032]

[0033] All sensors are fixed to the robot, so the coordinate system transformations between the IMU, LiDAR, and robot body are all static coordinate transformations. The data measured by each sensor can be multiplied by these static transformation matrices to transform to other coordinate systems for unified processing.

[0034] In step 2:

[0035] Step 2: Use statistical filtering to remove outliers from the point cloud, and use voxel filtering to downsample the point cloud to reduce its density. This invention uses the mean method to perform voxel filtering on the point cloud, calculating the average coordinates of all points within each voxel to represent all points within the entire voxel, as shown in the following formula:

[0036]

[0037] In the formula, X is the average coordinate of all laser points within the voxel; n is the number of laser points within the voxel; X i Let i be the coordinates of the laser point i within the voxel;

[0038] Then curvature calculation is performed for each point p in the point cloud. i Select p i The local neighborhood; for example, the 10 nearest points, calculate the covariance matrix of the points in the neighborhood.

[0039]

[0040] Where N is the number of points in the neighborhood. The covariance matrix C is the mean of the points in the neighborhood; eigenvalue decomposition is performed on the obtained covariance matrix C to obtain eigenvalues ​​λ1, λ2, λ3; assuming λ1 < λ2 < λ3; calculate the value of point p. i curvature

[0041]

[0042] Then based on curvature c i Points are classified into edge points and planar points: Edge points: curvature c i Greater than a certain threshold; such as c i >0.1; Plane point: curvature c i Less than a certain threshold; such as c i <0.05;

[0043] Finally, edge points and planar points are filtered separately. In each scan line, the edge points with the largest curvature, such as the first 5 points, are retained; the edge points with the smallest curvature, such as the last 5 points, are retained.

[0044] In step 3:

[0045] Based on the timestamp of each laser point, the system is positioned to the adjacent inertial sampling period, based on the assumption that the carrier maintains a uniform speed motion within the adjacent sampling interval Δt of the inertial measurement unit; where Δt = t k+1 -t k The laser pulse emission time t is calculated using a linear interpolation method. nowThe pose state is determined by calculating the relative position ratio of the laser point timestamp within the inertial sampling interval. Combined with the adjacent pose transformations obtained through pre-integration, a pose interpolation model for the corresponding time moment is constructed. Finally, the original point cloud data is uniformly compensated to the reference time coordinate system, thereby eliminating the point cloud deformation error caused by the carrier motion during the scanning process. This method effectively solves the problem of laser radar point cloud distortion in complex scenes and provides geometrically consistent input data for subsequent high-precision matching algorithms. The interpolation ratio coefficient in the interpolation calculation is calculated using the following formula.

[0046] b front =(t now -t k ) / Δt(5)

[0047] b back =(t k+1 -t now ) / Δt(6)

[0048] In the formula b front b is the interpolation scaling factor; back This is the post-interpolation scaling factor;

[0049] At the current point cloud emission time, the pose transformation of the lidar relative to the frame start time is calculated using linear interpolation, as shown in the following formula:

[0050]

[0051] In the formula For t k The pose change of the lidar relative to the start of the current frame; For t k+1 The pose change of the lidar relative to the start of the current frame;

[0052] In cases of intense motion, the following method can be used to fully utilize the velocity and acceleration measurements taken by the IMU for estimation. As shown in the following formula;

[0053]

[0054] To address the time synchronization correction issue of lidar point cloud data, point cloud coordinate normalization is achieved by establishing spatial transformation relationships between coordinate systems at multiple time points. Specifically, the pre-calibration pose of the lidar at its initial time relative to the global coordinate system is first obtained. Simultaneously calculate the relative pose transformation of the laser pulse emission time relative to the initial time. Based on the timestamp information of the point cloud acquisition time, the original measurement point P now Through two consecutive spatial coordinate transformations, the coordinates are successively mapped to P in the radar's initial time coordinate system.start and P in the global world coordinate system W Finally, standardized point cloud coordinates that eliminate time asynchrony errors are obtained. This method effectively solves the spatiotemporal synchronization problem during the scanning process of the moving vehicle by combining timestamp alignment and motion compensation. The coordinates of point P transmitted after the lidar scans a frame of data are the lidar coordinates L at the time of emission. now The three-dimensional coordinates in the image need to be transformed to the lidar coordinate system at the start of the frame, which yields the following result.

[0055]

[0056] Multiply both sides by the left side The true coordinates P of laser point P in the world coordinate system after motion compensation are obtained. W The calculation formula is shown below:

[0057]

[0058] In step 4, the data from the lidar and IMU are processed jointly in a common filter. The lidar scanning data and the IMU measurement data are input together into the filter for complementary correction. The IMU provides high-frequency, short-term, accurate motion estimation, while the lidar provides low-frequency but more accurate position information and environmental features. The data from both complement and correct each other, thereby improving the overall system's accuracy and robustness. This fusion directly uses the point cloud residual equation as the observation equation and writes it into the observation model, essentially embedding an ICP or NDT within the filter or optimization algorithm. The specific implementation steps are as follows:

[0059] Step 401: Acquire LiDAR and IMU data;

[0060] Step 402: Data preprocessing;

[0061] Step 403: State prediction, using IMU data to calculate the robot's motion through integration;

[0062] Step 404: Data association. The LiDAR uses ICP or NDT to find the corresponding relationship, and the IMU associates the predicted state with the LiDAR matching result to ensure that the two are consistent.

[0063] Step 405: Joint optimization, calculate the lidar residual and IMU residual and use the iterative error state Kalman filter algorithm to solve the optimization problem; the lidar residual is obtained by calculating the distance error between the current frame point cloud features and the corresponding features in the map; the distance error includes the distance from the point to the edge and the distance from the point to the plane; the IMU residual is obtained by calculating the error between the state predicted by the IMU integral and the state estimated by the lidar;

[0064] Let the nominal state be x, the error state be δx, and the true state be...

[0065]

[0066] in This represents state combination operations, such as addition or Lie group operations.

[0067] The nominal state is predicted using the system model f:

[0068] x k =f(x) k-1 ,u k (12)

[0069] Where x k It is the nominal state at the current moment, u k f is the control input; such as the acceleration and angular velocity of an IMU; f is the system model; such as the IMU integral model.

[0070] The dynamic model of the error state can be represented as follows:

[0071] δx k =F k δx k-1 +w k (13)

[0072] Where F k It is the error state transition matrix, representing how the error propagates over time, w k It is process noise, assumed to be zero-mean Gaussian noise, with a covariance of Q. k ;

[0073] The covariance matrix of the error state is predicted using formula (14):

[0074]

[0075] Where P k It is the error state covariance at the current moment, Q k It is the covariance matrix of the process noise;

[0076] The observation model describes how to generate observations from states:

[0077] z k =h(x true,k )+v k (15)

[0078] Where z k h represents the observed value; such as a lidar point cloud or visual features. v represents the observation model; such as a point cloud matching or reprojection error model. kIt is observation noise, assumed to be zero-mean Gaussian noise, with a covariance of R. k ;

[0079] Observation residuals are the differences between actual and predicted observations.

[0080] δz k =z k -h(x k (16)

[0081] Where δz k It is the observation residual, h(x) k These are predicted observations based on nominal states;

[0082] Observation matrix H k It is the Jacobian matrix of the observation model with respect to the error state:

[0083]

[0084] Formula (17) shows how the observed values ​​change with the error state;

[0085] Kalman gain K k Used to weigh predicted and observed values:

[0086]

[0087] Where R k It is the covariance matrix of the observation noise, and the Kalman gain determines the impact of the observation residual on the error state update;

[0088] Update the error state using Kalman gain:

[0089]

[0090] in This is the updated error state, δz k -H k δx k It is the corrected observation residual;

[0091] Update the covariance matrix of the error state:

[0092]

[0093] in It is the updated covariance matrix, and I is the identity matrix;

[0094] The core improvement of IESKF lies in the introduction of iterative optimization. With each observation update, IESKF iteratively updates the error state and covariance matrix multiple times. In each iteration, the observation residuals and Kalman gain are recalculated using the current error state. This process is repeated until the error state converges, i.e., the change is less than a certain threshold. The formula for iterative optimization is as follows:

[0095] For each iteration i, calculate the observation error:

[0096]

[0097] Update error status:

[0098]

[0099] Update the covariance matrix:

[0100]

[0101] Repeat the above steps until convergence, then correct the updated error state into the nominal state:

[0102]

[0103] in, It is the corrected nominal state;

[0104] Step 406: Status Update;

[0105] Step 407: Map Update;

[0106] Step 408: Output odometer information.

[0107] In step 5, the point cloud map is managed based on the improved incremental Kd-Tree; the map is initialized as a cube region with a fixed side length centered on the starting position of the carrier; the newly scanned point cloud data is fused in real time, and the map range is dynamically expanded; when the carrier moves and its perceptual domain touches the map boundary, the map is expanded by a fixed distance along the direction of movement, and the point cloud of the overlapping part of the new and old regions is deleted using the cube space clipping algorithm.

[0108] This incremental Kd-Tree allows all nodes to store point cloud data. Its construction process uses recursive spatial partitioning, selecting the dimension with the largest data distribution variance for partitioning each time, and using the average value of all points in that dimension as the partitioning threshold, until each leaf node contains only one point. The system supports point-by-point insertion and deletion operations. Deletion operations are delayed by marking nodes as pending deletion. Region-level deletion is determined by spatial bounding boxes. If the node's bounding box is completely within the region to be deleted, the entire subtree is marked. If there is partial overlap, the points within the node that belong to that region are deleted first, and then the child nodes are recursively processed.

[0109] The system periodically checks the balance of subtrees, making a judgment based on two conditions: the number of nodes and the proportion of invalid nodes. If a subtree is unbalanced, all its valid nodes are extracted into a point set and a balanced Kd-Tree is reconstructed. This reconstruction process is executed by an independent thread, while the main thread continues to process incremental operations and record logs. After the new tree is built, the operation logs are replayed and the atomic tree structure is replaced.

[0110] For K-nearest neighbor search, the algorithm starts from the root node, calculates the distance between the current node and the query point, and determines whether to recursively search the left and right subtrees based on the dynamically maintained spatial bounding box metadata, avoiding invalid traversals through spatial pruning.

[0111] In this embodiment, step 5 specifically includes:

[0112] This invention achieves intelligent maintenance of point cloud maps based on an improved incremental Kd-Tree, controlling map size through a dynamic update mechanism driven by spatial constraints and perception. The map is initialized as a cubic region with a side length of L, centered on the carrier's initial position, and dynamically expanded by real-time fusion of newly scanned point clouds. A spherical sensing domain is defined, centered on the carrier's current position, with a radius r = γR (R is the sensor's line-of-sight, γ > 1 is the expansion coefficient). When the carrier moves, causing the sensing domain to touch the map boundary, the map area is expanded by a fixed distance d = (γ-1)R along the direction of movement. A cubic spatial pruning algorithm is used to remove redundant point clouds in the overlapping parts of the old and new regions, ensuring map coverage of the effective detection range of the LiDAR while maintaining a stable and controllable storage size.

[0113] The improved incremental Kd-Tree is still a binary search tree. Compared to the traditional Kd-Tree design that only stores data points in leaf nodes, this structure allows all nodes (including internal nodes) to carry point cloud information, achieving efficient dynamic balancing through an incremental update mechanism. The construction process inherits the spatial recursive partitioning principle of the Kd-Tree, employing a maximum variance dimension partitioning strategy for recursive space division—that is, each time a hyperplane is cut along the longest distribution dimension of the current space until the single-point storage condition of the leaf node is met.

[0114] (1) Input: Point cloud data X = {x1, ..., x2} i}, where x i ∈R 3 .

[0115] (2) Subset X n Insert node n.

[0116] (3) If X n If empty, then the process ends.

[0117] (4) If X nIf there is only one node, mark it as a leaf node and then end.

[0118] (5) Calculate X n Among the variances of each axis, select the axis with the largest variance, denoted as j; and take its average as the segmentation threshold m. j =X n [j].

[0119] (6) Traverse x∈X n If x[j] < 0, insert the left node; otherwise, insert the right node.

[0120] (7) Repeat the above steps until all points are inserted into the tree.

[0121] During the construction process, variables such as the size and range of the tree will be initialized.

[0122] This tree supports dynamic incremental updates through point-by-point operations and region-level bounding box operations. Point-by-point operations handle the addition, deletion, and modification of individual points, while region-level operations process points within an axis-aligned cuboid in batches. After state estimation and downsampling of point p, the algorithm divides its space into a cubic grid with side length L, locates the cube C containing p, and executes a nearest neighbor retention strategy: it quickly retrieves all points within C using a kd-tree, caches them together with the new point p, selects the point closest to the cube's center, clears the original data within C, and recursively inserts the preferred point into the tree structure. The recursive process starts from the root node, searches layer by layer along the spatial partitioning rules until an empty node is located and insertion is completed, thus achieving dynamic optimization and efficient management of point cloud data. At each non-empty node, the new point is compared with the points stored on the node, and the recursion continues along the partitioning axis. Deletion is not a direct removal of the tree node, but rather a marking of the node as pending deletion. When all nodes at a certain subtree level are marked as pending deletion, its parent node identifies the deletion status. These two hierarchical status markings constitute a delayed processing label system. The marked nodes will be physically removed during subsequent tree structure reconstruction. This hierarchical marking mechanism effectively balances dynamic update efficiency and data structure stability. Its region deletion mechanism is achieved through the collaboration of spatial bounding boxes and delayed marking. The core process is as follows: Given an axis-aligned bounding box O to be deleted, the algorithm recursively traverses the tree structure starting from the root node, dynamically comparing the spatial relationship between the bounding boxes C and O of each node: if they have no intersection, the current branch processing is terminated; if C is completely covered by O, then the node and its subtree are marked as both to be deleted and to be deleted; when C and O partially overlap, the system prioritizes deleting the data points belonging to O within the current node, and then recursively checks the child nodes. After the operation is completed, the node spatial attributes are updated synchronously, and a balance maintenance mechanism is triggered. Through this strategy of hierarchical spatial judgment and delayed marking linkage, efficient region data cleaning is achieved while ensuring structural stability.

[0123] Kd-Tree ensures data structure efficiency through a dynamic balancing mechanism, whose balance is determined by two conditions. A subtree is balanced if and only if the following condition is met:

[0124] |S(T.left)-S(T.right)|<α bal (25)

[0125] Where 0 < α bal <S(T), where S(T) is the number of nodes under node T. The deletion criterion for the subtree rooted at T is:

[0126] I(T)<α bel (28)

[0127] Where 0 < α bel <S(T),I(T) represents the number of invalid nodes in the subtree rooted at T.

[0128] The former constrains subtree height to maintain optimal query efficiency, while the latter controls storage size through an invalid node ratio threshold. When a subtree simultaneously violates either criterion, a reconstruction mechanism is triggered. First, valid nodes are flattened into a point set, and a new balanced kd-tree is constructed. To address potential performance delays caused by large-scale reconstruction, the system employs a dual-thread collaborative strategy: the main thread handles small-scale reconstructions and incremental operation records, while an independent thread synchronizes incremental update requests through an operation logger when performing large-scale subtree reconstruction. This maintains atomic tree query functionality while achieving lock-free parallel processing. During reconstruction, the atomic tree only locks incremental updates to maintain query availability. After the new tree is built, the incremental operation log is replayed to the new structure, achieving seamless data structure switching through an atomic replacement mechanism. This hierarchical reconstruction strategy significantly reduces interference with real-time graph building and localization processes while ensuring K-nearest neighbor search accuracy. Finally, the algorithm replaces the old node T with a new node T' and releases the atomic tree's memory. Thus, during reconstruction, the main thread's mapping operations can continue uninterrupted at a mileage-based rate.

[0129] The K-nearest neighbor and range nearest neighbor search algorithms for the improved incremental Kd-Tree inherit the core mechanism of the traditional k-d tree and significantly improve the query efficiency through a dynamic space pruning strategy. The system uses node-level spatial bounding box metadata - this bounding box dynamically maintains the minimum three-dimensional envelope space of valid nodes (excluding marked deleted points) in the subtree - for rapid screening of the search space. During traversal, after calculating the distance between the current node and the query point, the algorithm synchronously checks the geometric relationship between the left and right subtree bounding boxes and the target search area: if the subtree bounding box has no intersection with the query space, the path pruning mechanism is triggered to terminate the depth traversal of this branch; only the subtrees with spatial overlap are recursively searched. This hierarchical screening mechanism based on real-time spatial bounding boxes effectively avoids redundant calculations in invalid areas and achieves exponential optimization of search performance while ensuring the integrity of query results. The search steps are as follows:

[0130] (1) Input: K-d tree T, query point x, and the number of nearest neighbors k.

[0131] (2) Output: Set N of K nearest neighbors.

[0132] (3) Initialize the current node n c as the root node. Let the function S(n c ) perform K-nearest neighbor search under the current node. If the current node is a leaf, calculate whether the distance between n c and x is less than the maximum distance in N; if so, add this node to N. Otherwise, delete the point with the farthest distance in N. Then determine on which side of the current node x is located. Finally, determine whether the other side of the current node needs to be expanded. The condition for expansion is judged as follows: if r|N| < k, it must be expanded; if |N| = k and the distance between x and the splitting plane of the current node is less than the distance of the maximum matching point in N, it also needs to be expanded. If the other side of the current node does not need to be expanded, the function returns; otherwise, continue to perform nearest neighbor search on the other side.

[0133] The above is the basic process of the present invention. Based on the improved incremental Kd-Tree algorithm, the present invention proposes a laser-inertial fusion algorithm that enables a robot to construct the surrounding environment and locate its own position relying only on its own sensors in an unfamiliar environment.

[0134] Embodiment 2:

[0135] This embodiment is a test description of the laser-inertial fusion positioning algorithm based on the improved incremental Kd-Tree proposed by the present invention. In order to verify that this algorithm can still accurately position itself and construct high-quality maps by itself in scenarios where GNSS signals are unavailable, the experimental location for this time was selected on the fourth floor of a teaching building, a fully enclosed space with many feature points in the surrounding environment, covering an area of 8m × 50m. The real-scene pictures are as Figure 1 shown.

[0136] The drone departs from the test starting point, flies around the site to construct a point cloud map, and the resulting map, after fusing information from both LiDAR and IMU data, is as follows: Figure 2 As shown. Figure 2 This is the mapping result of the fusion of lidar and IMU data, which is the starting point of this invention.

[0137] The drone departs from the test endpoint, flies around the site to build a point cloud map, and the resulting map, after fusing LiDAR and IMU data, is as follows: Figure 3 As shown. Figure 3 The mapping results of the fusion of lidar and IMU data starting from the endpoint of this invention can be seen from the two point cloud maps above. It can be seen that the local map constructed by this invention largely restores the real scene.

[0138] The above description is merely a preferred embodiment of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention. Structures, devices, and operating methods not specifically described or explained in this invention are implemented according to conventional methods in the art unless otherwise specified or limited.

Claims

1. A laser-inertial fusion positioning method based on an improved incremental Kd-Tree system, characterized in that: Includes the following steps: Step 1: Establish the coordinate system of the laser inertial system; first, determine the world coordinate system: W-frame; robot body coordinate system: B-frame; LiDAR coordinate system: L-frame; And IMU coordinate system: I-frame; Then, based on the static coordinate transformation, the data measured by each sensor is multiplied by the corresponding static transformation matrix of the two coordinate systems to transform it into other coordinate systems for unified processing; Step 2: Extract point cloud data; Outliers in the point cloud are removed by mean-based statistical filtering, and voxel filtering is used to downsample the point cloud. Then, curvature is calculated and representative geometric features are extracted for inter-frame matching and map building. Step 3: Perform motion compensation on the point cloud data after initial processing; by estimating and adjusting the motion changes in the signal, perform motion compensation on all point clouds acquired within the time period and unify them to the same coordinate system; Step 4: Fusion of LiDAR point cloud data and IMU data; After processing the LiDAR and IMU data, calculate the distance error between the current frame point cloud features and the corresponding features in the map to obtain the LiDAR residual, calculate the error between the state predicted by the IMU integral and the state estimated by the LiDAR to obtain the IMU residual, and then use the iterative error state Kalman filter algorithm to solve the optimization problem to achieve tight coupling; Step 5: Store the point cloud map and update the point cloud data; improve the original incremental Kd-Tree algorithm to maintain the local point cloud map and improve the efficiency of adding and deleting points, and finally realize the odometry output of the laser inertial system and the construction of the point cloud map.

2. The improved incremental Kd-Tree laser-inertial fusion positioning method according to claim 1, characterized in that: In step 1: The world coordinate system coincides with the lidar coordinate system at the moment of power-on. Data measured in the other three coordinate systems must eventually be converted to the world coordinate system for unified processing. The fuselage coordinate system is defined as a right-handed coordinate system with its origin located at the center of mass. The positive X-axis is in the same direction as the movement, the positive Y-axis is to the left, and the positive Z-axis is vertically upward, also satisfying the right-hand rule. The lidar coordinate system and the IMU coordinate system are also right-handed coordinate systems. Assume the robot's pose in the world coordinate system at time k-1 is By fusing information collected from lidar and IMU, the robot's pose transformation from time k-1 to time k can be estimated. The pose in the world coordinate system at time k is then easily calculated as follows, and the pose at each time step forms an odometry.

3. The improved incremental Kd-Tree laser-inertial fusion positioning method according to claim 1, characterized in that: In step 2: Step 2: Remove outliers from the point cloud using statistical filtering, downsample the point cloud using voxel filtering, and perform voxel filtering using the mean method. The average coordinates of all points within each voxel are used to represent the coordinates of all points within the entire voxel, as shown in the following formula: In the formula, X is the average coordinate of all laser points within the voxel; n is the number of laser points within the voxel; X i Let i be the coordinates of the laser point i within the voxel; Then curvature calculation is performed for each point p in the point cloud. i Select p i The local neighborhood; Calculate the covariance matrix of points in the neighborhood. Where N is the number of points in the neighborhood, and p is the mean of the points in the neighborhood; the covariance matrix C is decomposed into eigenvalues ​​to obtain eigenvalues ​​λ1, λ2, and λ3; assuming λ1 < λ2 < λ3; the point p is calculated. i curvature Then based on curvature c i Points are classified into edge points and planar points: Edge points: curvature c i Greater than a certain threshold; Planar point: curvature c i If the value is less than a certain threshold, then edge points and planar points are filtered separately, and the edge points with the largest curvature are retained in each scan line.

4. The improved incremental Kd-Tree laser-inertial fusion positioning method according to claim 1, characterized in that: In step 3: Based on the timestamp of each laser point, the system is positioned to the adjacent inertial sampling period, based on the assumption that the carrier maintains a uniform speed motion within the adjacent sampling interval Δt of the inertial measurement unit; where Δt = t k+1 -t k The laser pulse emission time t is calculated using a linear interpolation method. now The pose state; by calculating the relative position ratio of the laser point timestamp within the inertial sampling interval, and combining the adjacent pose transformations obtained by pre-integration, a pose interpolation model for the corresponding time moment is constructed, and finally the original point cloud data is uniformly compensated to the reference time coordinate system, thereby eliminating the point cloud deformation error caused by the carrier motion during the scanning process; the interpolation ratio coefficient in the interpolation calculation is calculated as shown in the following formula; b front =(t now -t k ) / Δt(5) b back =(t k+1 -t now ) / Δt (6) In the formula b front b is the interpolation scaling factor; back This is the post-interpolation scaling factor; At the current point cloud emission time, the pose transformation of the lidar relative to the frame start time is calculated using linear interpolation, as shown in the following formula: In the formula For t k The pose change of the lidar relative to the start of the current frame; For t k+1 The pose change of the lidar relative to the start of the current frame; In cases of intense motion, the following method can be used to fully utilize the velocity and acceleration measurements taken by the IMU for estimation. As shown in the following formula; To address the time synchronization correction issue of lidar point cloud data, point cloud coordinate normalization is achieved by establishing spatial transformation relationships between coordinate systems at multiple time points. Specifically, the pre-calibration pose of the lidar at its initial time relative to the global coordinate system is first obtained. Simultaneously calculate the relative pose transformation of the laser pulse emission time relative to the initial time. Based on the timestamp information of the point cloud acquisition time, the original measurement point P now Through two consecutive spatial coordinate transformations, the coordinates are successively mapped to P in the radar's initial time coordinate system. start and P in the global world coordinate system W Finally, standardized point cloud coordinates that eliminate time asynchrony errors are obtained; the coordinates of point P transmitted after the lidar scans a frame of data are the lidar coordinates L at the time of emission. now The three-dimensional coordinates in the image need to be transformed to the lidar coordinate system at the start of the frame, which yields the following result. Multiply both sides by the left side The true coordinates P of laser point P in the world coordinate system after motion compensation are obtained. W The calculation formula is shown below:

5. The improved incremental Kd-Tree laser-inertial fusion positioning method according to claim 1, characterized in that: In step 4, the specific implementation steps are as follows: Step 401: Acquire LiDAR and IMU data; Step 402: Data preprocessing; Step 403: State prediction, using IMU data to calculate the robot's motion through integration; Step 404: Data association. The LiDAR uses ICP or NDT to find the corresponding relationship, and the IMU associates the predicted state with the LiDAR matching result to ensure that the two are consistent. Step 405: Joint optimization, calculate the lidar residual and IMU residual and use the iterative error state Kalman filter algorithm to solve the optimization problem; the lidar residual is obtained by calculating the distance error between the current frame point cloud features and the corresponding features in the map; the IMU residual is obtained by calculating the error between the state predicted by the IMU integral and the state estimated by the lidar. Let the nominal state be x, the error state be δx, and the true state be... in This represents state combination operations, such as addition or Lie group operations. The nominal state is predicted using the system model f: x k =f(x k-1 ,u k ) (12) Where x k It is the nominal state at the current moment, u k It is the control input; f is the system model; such as the IMU integral model; The dynamic model of the error state can be represented as follows: δx k =F k δx k-1 +w k (13) Where F k It is the error state transition matrix, representing how the error propagates over time, w k It is process noise, assumed to be zero-mean Gaussian noise, with a covariance of Q. k ; The covariance matrix of the error state is predicted using formula (14): Where P k It is the error state covariance at the current moment, Q k It is the covariance matrix of the process noise; The observation model describes how to generate observations from states: z k =h(x true,k )+v k (15) Where z k v is the observed value, h is the observation model; k It is observation noise, assumed to be zero-mean Gaussian noise, with a covariance of R. k ; Observation residuals are the differences between actual and predicted observations. δz k =z k -h(x k ) (16) Where δz k It is the observation residual, h(x) k These are predicted observations based on nominal states; Observation matrix H k It is the Jacobian matrix of the observation model with respect to the error state: Formula (17) shows how the observed values ​​change with the error state; Kalman gain K k Used to weigh predicted and observed values: Where R k It is the covariance matrix of the observation noise, and the Kalman gain determines the impact of the observation residual on the error state update; Update the error state using Kalman gain: in This is the updated error state, δz k -H k δx k It is the corrected observation residual; Update the covariance matrix of the error state: in It is the updated covariance matrix, and I is the identity matrix; During each observation update, IESKF iteratively updates the error state and covariance matrix multiple times. In each iteration, the observation residuals and Kalman gain are recalculated using the current error state. This process is repeated until the error state converges. The formula for iterative optimization is as follows: For each iteration i, calculate the observation error: Update error status: Update the covariance matrix: Repeat the above steps until convergence, then correct the updated error state into the nominal state: in, It is the corrected nominal state; Step 406: Status Update; Step 407: Map Update; Step 408: Output odometer information.

6. The improved incremental Kd-Tree laser-inertial fusion positioning method according to claim 1, characterized in that: In step 5, the point cloud map is managed based on the improved incremental Kd-Tree; the map is initialized as a cube region with a fixed side length centered on the starting position of the carrier; the newly scanned point cloud data is fused in real time, and the map range is dynamically expanded; when the carrier moves and its perceptual domain touches the map boundary, the map is expanded by a fixed distance along the direction of movement, and the point cloud of the overlapping part of the new and old regions is deleted using the cube space clipping algorithm. This incremental Kd-Tree allows all nodes to store point cloud data; Its construction process adopts recursive spatial partitioning. Each time, the dimension with the largest data distribution variance is selected for partitioning, and the average value of all points in that dimension is used as the partitioning threshold until each leaf node contains only one point. The system supports point-by-point insertion and deletion operations. Deletion operations are delayed by marking nodes as pending deletion. Region-level deletion is determined by spatial bounding boxes. If the bounding box of a node is completely within the region to be deleted, the entire subtree is marked. If there is partial overlap, the points within the node that belong to the region are deleted first, and then the child nodes are recursively processed. The system periodically checks the balance of subtrees, making a judgment based on two conditions: the number of nodes and the proportion of invalid nodes. If a subtree is unbalanced, all its valid nodes are extracted into a point set and a balanced Kd-Tree is reconstructed. This reconstruction process is executed by an independent thread, while the main thread continues to process incremental operations and record logs. After the new tree is built, the operation logs are replayed and the atomic tree structure is replaced. For K-nearest neighbor search, the algorithm starts from the root node, calculates the distance between the current node and the query point, and determines whether to recursively search the left and right subtrees based on the dynamically maintained spatial bounding box metadata, avoiding invalid traversals through spatial pruning.

Images