Positioning method and system based on loosely coupled multi-source fusion and continuous-time state estimation

Abstract

This application relates to the field of autonomous driving positioning technology, specifically proposing a positioning method and system based on loosely coupled multi-source fusion and continuous-time state estimation. The method includes: stitching and filtering multiple LiDAR point clouds; employing adaptive voxel sparsification of the point cloud and estimating the point cloud normal vector; using pose increments provided by gyroscopes and wheel speedometers as priors, performing point cloud matching with normal vector constraints, and combining covariance recovery to achieve alignment with historical reference point cloud frames to obtain discrete pose estimation; constructing a dynamic local point cloud map centered on the vehicle body; using the discrete pose as B-spline control points to perform continuous-time modeling of the vehicle's trajectory, solving for curve parameters to calculate the pose at any given time, and outputting continuous, smooth, and robust positioning results. This method balances the reliability of multi-source fusion with continuous-time accuracy, and is suitable for real-time positioning of autonomous driving in complex environments.

Description

Technical Field

[0001] This application relates to the field of autonomous driving positioning technology, and specifically proposes a positioning method and system based on loosely coupled multi-source fusion and continuous-time state estimation. Background Technology

[0002] In fields such as autonomous driving and mobile robotics, high-precision and high-reliability real-time positioning is the foundation for achieving autonomous navigation and decision-making control. Currently, mainstream positioning solutions typically rely on the fusion of multiple sensors, such as lidar, inertial measurement units (IMUs), and wheel speedometers, to estimate the pose of the vehicle.

[0003] Multi-sensor fusion localization (LIO) methods generally fall into two categories: tightly coupled and loosely coupled. Existing tightly coupled methods typically suffer from the problem of considering as many sensor data factors as possible during data fusion, leading to an overall inaccuracy of the calculated equilibrium solution potentially affected by the quality of a single data source. Other issues include ineffective removal of inertial navigation bias, improper calibration of radar and inertial navigation extrinsic parameters, and experimentally untuned state transition matrix parameters in filters, all of which impact state estimation accuracy. Furthermore, the failure of laser point cloud matching due to severe vehicle movement or road bumps can also cause pose estimation errors, affecting subsequent pose estimations. This is because a momentary pose error can lead to ghosting or misalignment of the dynamic local point cloud map, which in turn causes laser matching errors. In contrast, loosely coupled LIO methods primarily process point cloud data from a single sensor, typically lidar, while other sensors, such as inertial navigation, only provide initial priors for laser matching and are not considered during the optimization process. Loosely coupled frameworks facilitate modular component design, with low interdependence between modules. If a single module malfunctions, it can be replaced without affecting the functionality of other modules. Existing loosely coupled methods, such as LOAM using gyroscopes as priors, suffer from insufficient accuracy in actual positioning. To address this issue, this application investigates a novel framework for loosely coupled multi-source fusion LIO laser inertial navigation odometry.

[0004] Furthermore, existing positioning methods typically generate a series of discrete pose estimates at specific time intervals when outputting the final result. In challenging scenarios involving high-speed vehicle movement, sharp turns, or sparse or repetitive geometric features such as corridors or tunnels, these discrete estimates are prone to large cumulative errors, and may even lead to positioning and tracking failures. This results in the calculated trajectory being not only discontinuous in time but also exhibiting abrupt and uneven spatial transitions, failing to accurately and completely reflect the vehicle's true continuous motion, thus leading to poor overall reliability and continuity of the positioning results.

[0005] In view of this, this application proposes a localization method and system based on loosely coupled multi-source fusion and continuous-time state estimation. Summary of the Invention

[0006] To achieve the above objectives, this application provides a localization method and system based on loosely coupled multi-source fusion and continuous-time state estimation, the specific technical solution of which is as follows: Localization methods based on loosely coupled multi-source fusion and continuous-time state estimation include: The point cloud data acquired by the vehicle body's multiple lidar sensors are stitched together and filtered. Adaptive voxel filtering is used to sparsify the point cloud, and the normal vector of the sparsified point cloud is calculated. Using the pose increments of the vehicle body gyroscope and wheel speedometer as prior data, point cloud matching is performed based on the point cloud normal vector. The covariance matrix is ​​calculated through the normal vector to achieve the matching between the current frame point cloud and the historical reference point cloud frame, and to calculate the discrete pose estimation results of the vehicle body. A dynamic local point cloud map centered on the vehicle body is constructed, and the confidence of point cloud matching is calculated. When the confidence is higher than a preset threshold and the relative displacement of the vehicle body exceeds the threshold, the current frame point cloud is updated to the dynamic local point cloud map. When the confidence is lower than the preset threshold, the map update is paused and relocalization based on the dynamic local point cloud map is triggered, and the current pose of the vehicle body is corrected in the local point cloud map. The discrete pose localization results of the vehicle body are used as control points for the B-spline curve. The B-spline curve is used to model the motion trajectory of the vehicle body in continuous time. By solving the curve parameters, the pose of the vehicle body at any time is calculated, and continuous and smooth localization results are output.

[0007] Preferably, the raw point cloud data from different lidar sensors in the vehicle body are processed in a unified manner. The point cloud data collected by multiple lidars installed at different locations on the vehicle body are transformed into a unified vehicle coordinate system and stitched together using their respective extrinsic parameter calibration matrices. By setting the range of the vehicle body surround box, point cloud data falling within the range of the vehicle body's own structure is removed, filtering out point cloud data around the vehicle body that interferes with radar. An adaptive voxel filtering strategy is used to sparsify the point cloud. The adaptive voxel filtering strategy dynamically adjusts the voxel resolution according to the ratio of the number of original point clouds in the current frame to the number of point clouds processed in the previous frame, so as to ensure that a fixed number of point clouds can be used for data processing after the original point cloud is sparsified.

[0008] Preferably, after the point cloud sparsification process is completed, the normal vector of each retained point is calculated, and the normal vector is used for subsequent geometric feature description. The calculation of the normal vector includes: for each point after sparsification, constructing a covariance matrix through the set of points in its neighborhood, and obtaining the normal vector through eigenvalue decomposition; The normal vector and point cloud coordinates of each point are stored for fast recovery of the covariance matrix based on the normal vector.

[0009] Preferably, the angular velocity measurement value and encoder pulse count provided by the vehicle body gyroscope and wheel speed meter are obtained, and the orientation angular displacement increment and linear displacement increment between adjacent time moments are calculated as the initial pose estimation for point cloud matching; Based on the point cloud normal vectors calculated in the preprocessing stage, the generalized iterative nearest point algorithm is used for point cloud matching. The optimal rotation and translation parameters are solved by minimizing the average geometric distance error between matching pairs in the point cloud data association. The objective function of point cloud matching 0 is weighted by the covariance matrix, and the matching weights in different directions are adaptively adjusted according to the local geometric characteristics.

[0010] Preferably, the covariance matrix is ​​quickly recovered using the calculated normal vector, which is constructed using regularization parameters, the distance from the point to the local fitting plane, and the unit normal vector of the point. An iterative optimization strategy is adopted, which searches for nearest neighbor pairs through a spatial index structure and uses the recovered covariance matrix to construct a weighted least squares problem to solve for the pose increment; When establishing point correspondence, a dual constraint mechanism of distance threshold and normal vector angle threshold is used to remove outliers. At the same time, a hierarchical matching strategy is adopted, which first performs matching at low resolution and then gradually increases the resolution for matching.

[0011] Preferably, an incremental update strategy centered on the vehicle body is used to construct a dynamic local point cloud map. The dynamic local point cloud map always maintains a fixed-size spatial range defined by a cube bounding box and is continuously updated as the vehicle body moves. When the vehicle body moves beyond a preset threshold, the map center is translated. The data structure of the dynamic local point cloud map uses an octree to organize the point cloud data. The confidence score for point cloud matching is calculated. The confidence score is based on statistical information of the average nearest neighbor distance and the number of interior points in the point cloud matching process, which characterizes the matching accuracy and matching rate.

[0012] Preferably, the map update strategy is adaptively adjusted based on the calculated confidence score. When the confidence score is higher than a preset threshold and the relative displacement of the vehicle exceeds the threshold, the current frame point cloud is inserted into the dynamic local point cloud map after coordinate transformation. When the confidence level is lower than the preset threshold, map updates are paused and a two-stage relocalization process based on laser template matching is triggered, including: coarse localization to find candidate frame positions by extracting and matching template matching descriptors of global position recognition, taking the pose corresponding to the candidate frame position as the candidate pose, and using the candidate pose to perform precise matching with the local map and selecting the pose with the highest matching score as the relocalization result. After successful relocalization, the vehicle trajectory is corrected. A multi-threaded parallel processing architecture is adopted to ensure the real-time update of the map, and the point cloud corresponding to dynamic objects is identified and removed by analyzing the consistency of the point cloud over time.

[0013] Preferably, a uniform B-spline curve is used to continuously parameterize the motion trajectory of the vehicle body, and the discrete pose estimation results are used as control points to construct a cubic B-spline curve; When performing continuous-time representation, position and attitude components are processed simultaneously. The attitude components are represented by quaternions and spline interpolation is performed in the quaternion space, and normalization is performed after each evaluation. The time parameterization of control points adopts the cumulative chord length parameterization method, which assigns parameter values ​​based on the spatial distance between adjacent control points.

[0014] Preferably, a least squares-based curve fitting optimization is introduced to construct an optimization objective function to minimize the observation error and the second derivative smoothing term of the curve. The weights of the observation error term and the smoothing term are dynamically adjusted according to the confidence level of the pose estimation. The optimal B-spline curve parameters are solved by an iterative optimization algorithm. A sliding window strategy is adopted to maintain the B-spline curves for online updates; based on the parameter-identified B-spline curves, a recursive derivation method is used to calculate the corresponding pose of the vehicle body according to the timestamp; Velocity and acceleration information are obtained by differentiating the B-spline curve, and a trajectory consistency verification mechanism is introduced to identify and correct trajectory segments that violate preset maximum velocity or maximum angular velocity thresholds.

[0015] A positioning system based on loosely coupled multi-source fusion and continuous-time state estimation is used to implement the positioning method based on loosely coupled multi-source fusion and continuous-time state estimation, including: a point cloud data preprocessing module, a multi-source fusion pose calculation module, a dynamic map optimization and management module, and a continuous-time trajectory modeling module. The point cloud data preprocessing module is used to stitch and filter the point cloud data acquired by the vehicle's multi-LiDAR, use adaptive voxel filtering to sparsify the point cloud, and calculate the normal vector of the sparsified point cloud. The multi-source fusion pose calculation module uses the pose increments of the vehicle body gyroscope and wheel speed meter as prior data, performs point cloud matching based on point cloud normal vectors, calculates the covariance matrix through the normal vectors, realizes the matching between the current frame point cloud and the historical reference point cloud frame, and calculates the discrete pose estimation results of the vehicle body. The dynamic map optimization management module constructs a dynamic local point cloud map centered on the vehicle body and calculates the confidence level of point cloud matching. When the confidence level is higher than a preset threshold and the relative displacement of the vehicle body exceeds the threshold, the current frame point cloud is updated to the dynamic local point cloud map. When the confidence level is lower than the preset threshold, the map update is paused and the vehicle body repositioning is triggered, and the current pose of the vehicle body is corrected. The continuous-time trajectory modeling module uses the discrete pose positioning results of the vehicle body as control points of the B-spline curve, and uses the B-spline curve to perform continuous-time modeling of the vehicle body's motion trajectory. By solving the curve parameters, it calculates the vehicle body pose at any time and outputs continuous and smooth positioning results.

[0016] The beneficial effects of this application are as follows: This application significantly reduces point cloud redundancy and noise and improves data uniformity and computational efficiency by using multi-radar point cloud stitching and adaptive voxel filtering; the normal vector provides a more efficient computational source, and the covariance matrix can be directly calculated through the normal vector for point cloud registration, which is more efficient than calculating the covariance matrix through the point cloud, and still has good observability and robustness in large plane, sparse texture and other scenarios.

[0017] This application utilizes the pose increments of the gyroscope and wheel speedometer as priors to reduce the search space and accelerate convergence. Matching based on normal and covariance recovery can distinguish uncertainties in all directions, focus on constraining normal deviation, and reduce the influence of noise and outliers, thereby obtaining more accurate and stable discrete pose estimation.

[0018] This application constructs a dynamic local map centered on the vehicle body, ensuring that registration is performed within the latest and most relevant subset of the environment, reducing drift and computational load; confidence determination adaptively controls map updates and relocalization triggers, automatically correcting the current pose when degradation or occlusion occurs, significantly improving the system's robustness and continuity.

[0019] This application uses discrete poses as B-spline control points for continuous-time modeling, allowing smooth poses to be queried at any time; the trajectory possesses high-order continuity such as position and velocity, improving the coupling effect between control and perception, and outputting high-precision, low-jitter positioning results. Attached Figure Description

[0020] Figure 1 The flowchart of the localization method based on loosely coupled multi-source fusion and continuous-time state estimation provided in this application is shown below. Figure 2A logic diagram of the localization method based on loosely coupled multi-source fusion and continuous-time state estimation provided in this application; Figure 3 The diagram shows the structure of the positioning system based on loosely coupled multi-source fusion and continuous-time state estimation provided in this application. Detailed Implementation

[0021] To make the above-mentioned objectives, features and advantages of this application more readily understood, the specific embodiments of this application will be described in detail below with reference to the accompanying drawings.

[0022] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of this application. Therefore, this application is not limited to the specific embodiments disclosed below.

[0023] Secondly, the term "an embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of this application. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single embodiment or an embodiment selectively excluded from other embodiments.

[0024] Example 1 Reference Figure 1 and Figure 2 This is the first embodiment of the present application, such as Figure 1 As shown, a localization method based on loosely coupled multi-source fusion and continuous-time state estimation is provided; Figure 2 The logic diagram of the localization method based on loosely coupled multi-source fusion and continuous-time state estimation provided in this application.

[0025] Step 1: The point cloud data acquired by the vehicle's multi-LiDAR is stitched and filtered. Adaptive voxel filtering is used to sparsify the point cloud, and the normal vector of the sparsified point cloud is calculated.

[0026] The raw point cloud data from different lidar sensors in the vehicle body are processed in a unified manner. The point cloud data collected by multiple lidar sensors installed at different locations on the vehicle body are transformed into a unified vehicle coordinate system through their respective extrinsic parameter calibration matrices and then stitched together to form a complete environmental perception point cloud.

[0027] During the point cloud data stitching process, for each LiDAR point cloud data... The rotation matrix obtained through pre-calibration Translation vector Transform it to the vehicle coordinate system: ,in This represents the point cloud in the vehicle coordinate system after transformation. For the first The rotation matrix of the lidar relative to the vehicle coordinate system. This is the corresponding translation vector.

[0028] After point cloud stitching is completed, point cloud data that may interfere with radar near the vehicle body needs to be filtered out. By setting the bounding box range of the vehicle body, point clouds falling within the vehicle's own structural area are removed to avoid interference from the vehicle's own reflections on subsequent matching. Filtering interfering point clouds is achieved by determining whether the point cloud coordinates fall within the preset vehicle geometric boundaries, which effectively improves the quality of the point cloud data.

[0029] To balance computational efficiency and point cloud density, an adaptive voxel filtering strategy is employed to sparsify the point cloud. This strategy dynamically adjusts the voxel resolution based on the ratio of the number of points in the current frame to the number of points in the previous frame. Let the original number of points in the current frame be... The target point cloud number is Then the voxel resolution is adjusted adaptively. It can be represented as: ,in Using the base voxel resolution, this adaptive adjustment mechanism ensures that a relatively stable number of point clouds can be maintained in different scenarios, thereby ensuring that the computational complexity of the algorithm is controllable.

[0030] For example, when the reference voxel resolution Set to 0.2 meters, target point cloud number The number of points is set to 10,000, while the original point cloud count in the current frame is... When the number of points is 80,000, the adaptive voxel resolution calculated according to the above formula is... The distance is approximately 0.4 meters, which allows the number of point clouds to be controlled within a reasonable range.

[0031] After sparsification, the normal vector of each retained point needs to be calculated for subsequent geometric feature description. For each point after sparsification... The covariance matrix is ​​constructed using the set of points in its neighborhood, and the normal vector is obtained through eigenvalue decomposition.

[0032] Specifically, firstly in the radius Search points within range nearest neighbor set Calculate the centroid of these points. Then construct the covariance matrix. ,in The number of nearest neighbors. For the first The coordinates of the nearest neighbor points This represents the transpose of a matrix. It is achieved by considering the covariance matrix... Eigenvalue decomposition is performed, and the eigenvector corresponding to the smallest eigenvalue is the normal vector of that point. .

[0033] Normal vectors not only provide local geometric feature information of point clouds, but can also be used for the rapid recovery of the covariance matrix based on normal vectors. Storing the normal vector of each point along with its point cloud coordinates forms an enhanced point cloud data structure, where each point contains location information. and normal vector This preprocessing method significantly improves the computational efficiency of subsequent point cloud matching because the covariance matrix can be directly recovered from the calculated normal vectors during the matching process, without having to repeat neighborhood search and matrix operations.

[0034] This step, through the preprocessing steps of multi-LiDAR point cloud stitching, adaptive voxel filtering, and normal vector calculation, effectively integrates and extracts features from the original point cloud data. While ensuring appropriate point cloud density, it provides rich geometric feature information for subsequent point cloud matching, effectively improving the computational efficiency and robustness of the entire localization method.

[0035] Step 2: Using the pose increments from the vehicle body gyroscope and wheel speedometer as prior data, point cloud matching is performed based on the point cloud normal vector. The covariance matrix is ​​calculated through the normal vector to achieve matching between the current frame point cloud and the historical reference point cloud frame, and the discrete pose estimation result of the vehicle body is calculated.

[0036] A loosely coupled multi-source fusion framework is constructed to acquire prior information about vehicle motion from vehicle sensors, including gyroscopes and wheel speed meters. The angular velocity measurements provided by the gyroscopes are integrated to obtain the angular displacement increment between adjacent time points, while the wheel speed meters are converted by encoder pulse counting to obtain the linear displacement increment corresponding to wheel rotation.

[0037] Assume the gyroscope operates at a time interval. The internally measured angular velocity is Then the directional angle increment Wheel speed gauges are based on wheel radius. and encoder pulse count Calculated linear displacement increment ,in This represents the total number of pulses per encoder revolution. This incremental information constitutes the initial pose estimate for point cloud matching, providing a good initial value for subsequent accurate matching.

[0038] Based on the point cloud normal vectors calculated in the preprocessing stage, the Generalized Iterative Closest Point (GICP) algorithm is used for point cloud matching. The GICP algorithm no longer distinguishes between line and surface features, but uniformly uses the local geometric properties of the point cloud for matching, which can improve the algorithm's adaptability in different scenarios. The core of point cloud matching lies in constructing and optimizing the objective function, solving for the optimal rotation and translation parameters by minimizing the average geometric distance error between matching pairs in the point cloud data association.

[0039] The objective function expression for point cloud matching is: ; in, This represents the rotation matrix of the retrieved frame point cloud relative to the reference frame point cloud. This represents the corresponding translation vector. For the first frame in the retrieval frame One point, For subscript parameters, For reference frame and The corresponding nearest neighbor, To the total number of matching point pairs, For the first The covariance matrix of each matching point pair. This objective function, through covariance matrix weighting, can adaptively adjust the matching weights in different directions according to local geometric characteristics, thus providing a more stringent constraint on the matching error in the normal vector direction.

[0040] Fast reconstruction of the covariance matrix is ​​a key technique for improving computational efficiency. Using pre-computed normal vectors, the covariance matrix can be directly constructed without repeatedly performing neighborhood searches. For a point in the reference frame, the expression for reconstructing its covariance matrix using normal vectors is: ;in This is a regularization parameter used to ensure the positive definiteness of the matrix. It is the identity matrix. This represents the distance from a point to its local fitted plane. Let be the unit normal vector at that point. yes transpose, , , These represent the components of the normal vector along the three coordinate axes. The covariance matrix recovery method based on the normal vector avoids the process of calculating the covariance of the neighborhood point set for each point, which is required in traditional methods, and can significantly reduce computational complexity.

[0041] For example, when the regularization parameter Set to 0.01, the distance from the point to the local plane. It is 0.05 meters, and the normal vector is... for When the covariance matrix at a given point is obtained, it can be quickly recovered using the formula above, and used for subsequent weighted least squares optimization.

[0042] An iterative optimization strategy is employed during point cloud matching, progressively improving pose estimation through multiple iterations. In each iteration, the pose estimation is first determined based on the current pose estimate. Establish the correspondence between point clouds, quickly search for nearest neighbor pairs using spatial indexing structures such as KD trees or octrees, then construct a weighted least squares problem using the recovered covariance matrix, and solve for the pose increment using singular value decomposition (SVD) or the Gauss-Newton method. Update pose estimation: , ;in, Indicates the first The rotation matrix updated in the next iteration; Indicates the first The estimated value of the rotation matrix at the next iteration; Indicates the first The correction amount for the rotation estimate in the next iteration; Indicates the first The translation vector updated in the next iteration; Indicates the first The estimated translation vector value at the next iteration; Indicates the first The correction amount of the translation vector during each iteration; the iteration process continues until the pose increment is less than a preset threshold or the maximum number of iterations is reached.

[0043] To improve the robustness of matching, outlier removal is necessary when establishing point correspondences. This can be achieved by setting a distance threshold. Eliminate matching point pairs that are too far apart; at the same time, use the angle threshold of the normal vectors. Filter out point pairs whose normal vector directions differ too much. The valid matching point pairs that meet the conditions are: ; in and These are the normal vectors of the two matching points. This indicates the preset maximum distance threshold. The AND operator represents a logical AND, specifically indicating that both conditions on its left and right sides must be satisfied simultaneously. Normal vector and The angle between them; This represents the preset maximum normal vector angle threshold; the dual constraint mechanism can effectively reduce the impact of erroneous point cloud matching on the optimization results.

[0044] Point cloud matching includes inter-frame matching and point cloud-to-map matching. Inter-frame matching utilizes point cloud data from adjacent time points; due to the short time interval and high point cloud overlap, it can quickly obtain preliminary pose estimation. Building upon inter-frame matching, further matching is performed between the current frame's point cloud and historical accumulated information. Historical reference point cloud frames can be collections of point clouds from previous frames or pre-constructed local map fragments. Through this multi-layered matching strategy, temporal and spatial information can be fully utilized to obtain more accurate and stable pose estimation.

[0045] In the optimization process, a hierarchical matching strategy is adopted to accelerate convergence and improve numerical stability. First, coarse matching is performed at a lower resolution to obtain a rough pose estimate; then, fine matching is performed by gradually increasing the resolution. Each resolution level uses a different voxel size to downsample the point cloud, optimizing layer by layer from coarse to fine. This hierarchical matching strategy not only speeds up convergence but also effectively avoids getting trapped in local optima.

[0046] The advantage of the loosely coupled framework lies in the independence of data from each sensor. Even with certain deviations in the gyroscope or wheel speedometer, accurate vehicle pose estimation can still be obtained through the point cloud matching process. Although the bias error of the gyroscope and the scaling factor error of the wheel speedometer affect the accuracy of the prior estimation, the point cloud matching process can correct these errors to a certain extent. The point cloud matching uses an enhanced matching function based on the calculation of the covariance matrix using normal vectors, which reduces the dependence on the initial prior pose and improves the applicability of this method.

[0047] This step utilizes the aforementioned efficient point cloud matching method based on normal vectors, combined with motion priors provided by gyroscopes and wheel speedometers, to achieve accurate and reliable vehicle pose estimation. This step fully leverages the geometric features of the point cloud, significantly improving computational efficiency through a fast covariance matrix recovery technique. Simultaneously, the loosely coupled framework avoids the impact of single sensor failure on robustness, providing high-quality discrete pose estimation results for subsequent map construction and continuous-time state estimation.

[0048] Step 3: Construct a dynamic local point cloud map centered on the vehicle body and calculate the confidence level of point cloud matching. When the confidence level is higher than a preset threshold and the relative displacement of the vehicle body exceeds the threshold, update the current frame point cloud to the dynamic local point cloud map. When the confidence level is lower than the preset threshold, pause map updates and trigger relocalization based on the dynamic local point cloud map, and correct the current pose of the vehicle body in the local point cloud map.

[0049] A vehicle-centric incremental update strategy is employed to construct a dynamic local point cloud map. This dynamic local point cloud map maintains a fixed spatial extent at all times, continuously updating its content as the vehicle moves. This approach ensures real-time map performance while controlling memory consumption. The spatial extent of the dynamic local point cloud map is defined using a cube bounding box with a set side length of [value missing]. The current position of the vehicle body is The spatial range covered by the map is: ; When the vehicle moves beyond the preset update distance threshold At that time, the map center is shifted, and old point cloud data that exceeds the new boundary is removed to keep the map centered on the vehicle.

[0050] The map's data structure uses an octree to organize the point cloud data, enabling efficient spatial indexing and dynamic updates. Each node of the octree corresponds to a spatial cube region, and multi-resolution spatial representation is achieved through recursive subdivision. Let the maximum depth of the octree be... The minimum voxel size is The spatial size corresponding to the root node is When inserting point clouds, the octree is recursively traversed based on their spatial coordinates until a leaf node or maximum depth is reached. The hierarchical data structure of the octree not only supports fast nearest neighbor search but also facilitates batch update and deletion operations for local regions.

[0051] To ensure map quality, the confidence level of the point cloud matching results needs to be evaluated. The confidence level reflects the reliability of the current pose estimation and directly determines whether to add the current frame's point cloud to the map. The confidence level is calculated based on statistical information from the point cloud matching process, and the expression for confidence level calculation is: ; in The confidence score ranges from [value range missing]. , Represents an exponential function. This represents the average nearest neighbor distance for all matching pairs of interior points. The standard deviation parameter of the distance. To satisfy the distance threshold for the number of interior points, The total number of point clouds involved in the matching is represented. The confidence score calculation expression takes into account both matching accuracy and matching rate. When the matching distance is small and the proportion of interior points is high, the confidence score is close to 1, indicating that the pose estimation is reliable.

[0052] For example, when the average nearest neighbor distance The standard deviation parameter is 0.05 meters. Set to 0.1 meters, number of interior points There are 800 points in total. When the number is 1000, the confidence score calculated according to the above formula is approximately 0.72, indicating that the current pose estimation has high reliability.

[0053] The map update strategy is adaptively adjusted based on the confidence score, and a confidence threshold is set. and the vehicle body relative displacement threshold ,when as well as If the current pose estimation is considered reliable, the point cloud of the current frame is transformed and inserted into the dynamic local map. This indicates the current displacement of the vehicle. This indicates the vehicle displacement at the previous time step. This represents the relative displacement of the vehicle body. The insertion process uses incremental updates, first transforming the point cloud to the world coordinate system: ,in The point cloud in the vehicle coordinate system. and These are the currently estimated rotation matrix and translation vector, respectively. The transformed point cloud is placed in the world coordinate system. The transformed point cloud is then inserted into an octree structure, and the occupancy probability of the corresponding nodes is updated.

[0054] When confidence level When the current pose estimation is unreliable, map updates are paused and a relocalization process is triggered. Relocalization employs a two-stage strategy based on laser template matching. The first stage performs coarse localization by matching the global descriptor of the current frame's point cloud with the descriptors of historical reference point cloud frames to quickly determine the approximate location. The global descriptor of the point cloud uses statistical features based on the point cloud distribution, including information such as the point cloud centroid, principal orientation, and scatter matrix. Let the descriptor of the current frame's point cloud be... , history The frame descriptor is By calculating the similarity between descriptors Select the historical frames with the highest similarity as candidate positions.

[0055] The second stage involves fine-tuning the localization process. Using the candidate poses obtained from coarse localization as initial values, the current frame's point cloud is precisely matched with the dynamic local map. To improve the success rate and accuracy of relocalization, a multi-hypothesis verification strategy is employed; for each candidate pose... ,in, Indicates the first Rotation matrix for each candidate pose; Indicates the first Translation vectors of candidate poses The index represents the candidate pose; point cloud-to-map matching optimization is performed to obtain the refined pose. and corresponding matching score , Indicates the first Matching scores after refining each candidate pose. Select the highest score that exceeds the relocalization threshold. The pose is used as the relocalization result. Indicates the first The rotation matrix after refining each candidate pose; Indicates the first The translation vectors refined from candidate poses.

[0056] After successful repositioning, the vehicle trajectory is corrected; let the pose before repositioning be... The repositioned pose is Calculate the pose correction amount: , This correction is used to update subsequent pose estimates, ensuring the continuity of the trajectory. This represents the rotation matrix before repositioning. This indicates the translation amount before repositioning; This represents the rotation matrix after repositioning. This represents the translation vector after repositioning; Indicates the rotation correction amount. This indicates the amount of translation correction.

[0057] To ensure real-time map updates, a multi-threaded parallel processing architecture is adopted. The main thread is responsible for point cloud matching and pose estimation, the map update thread is responsible for point cloud insertion and octree maintenance, and the relocation thread is started when needed for position recovery. Data is passed between threads through a message queue to avoid direct data contention. The map update thread maintains a point cloud buffer queue, and when it receives high-confidence point cloud data, it performs an asynchronous insertion operation. This design ensures the real-time performance of the system, and even when map updates are time-consuming, it will not affect the pose estimation of the main thread.

[0058] The maintenance of dynamic maps also includes regular map optimization and compression. As point clouds accumulate, redundant point clouds may exist in some areas. Voxel grid filtering is used for downsampling to maintain map sparsity. Simultaneously, areas that have not been updated for a long time are reduced in weight or selectively deleted to ensure the map reflects the latest environmental conditions. This dynamic maintenance mechanism allows the map to adapt to environmental changes, such as the appearance and disappearance of moving objects.

[0059] The map construction process also needs to consider the impact of dynamic objects. By analyzing the consistency of point clouds over time, point clouds corresponding to dynamic objects can be identified and removed. For static environmental point clouds, their spatial position should remain stable across multiple frames of observation. An occupancy probability threshold should be set. Only when the occupancy probability of a voxel exceeds the threshold is it considered to correspond to a static environment and participate in subsequent matching and localization.

[0060] This step, through the aforementioned mechanism for constructing and maintaining dynamic local point cloud maps, combined with a confidence-based adaptive update strategy and a robust relocalization method, achieves stable localization in various scenarios. During dynamic map updates, location confidence is used as the evaluation criterion to avoid inserting misaligned point clouds into the dynamic map when localization fails, thus ensuring the clarity and accuracy of the local dynamic map and improving the accuracy of localization during point cloud map matching. This step not only continuously accumulates environmental information to improve localization accuracy under normal conditions but also enables rapid recovery when localization fails, ensuring the continuity and reliability of the entire localization process. The incremental update of the dynamic map and the fixed memory consumption design ensure stable operation over long periods. Step 4: Use the discrete pose estimation results of the vehicle body as the key control points of the B-spline curve (B-Spline). Use the B-spline curve to model the motion trajectory of the vehicle body in continuous time. By solving the curve parameters, calculate the vehicle body pose at any time and output continuous and smooth positioning results.

[0061] When performing continuous-time state estimation, uniform B-spline curves are used to parameterize the vehicle's motion trajectory. B-spline curves have excellent properties such as local support, controllable continuity, and computational stability, making them particularly suitable for describing the smooth motion trajectory of the vehicle.

[0062] Using the discrete pose estimation results of the vehicle body obtained in the previous steps as control points, a cubic B-spline curve is constructed to realize the transformation from discrete to continuous. The cubic B-spline ensures the second-order continuity of the trajectory, that is, the continuous change of position, velocity and acceleration, which conforms to the physical constraints of vehicle motion.

[0063] The mathematical expression for a B-spline curve is: ; in Indicates parameters The corresponding points on the curve For the first One control point, Subtract 1 from the total number of control points. for B-order spline basis functions, Let be the order of the spline. For a cubic B-spline, The basis functions are calculated recursively, which guarantees the local properties of the curve, meaning that each control point only affects its neighboring curve segments.

[0064] When constructing a continuous-time representation of pose, both position and orientation components need to be processed simultaneously. For the position component, B-spline curves in three-dimensional Euclidean space are directly used for modeling. Let the vehicle body at time... The position is , Indicates the vehicle body is in The coordinate along the X-axis at any given time. Indicates the vehicle body is in The coordinate along the Y-axis at any given time; Indicates the vehicle body is in The coordinates along the Z-axis at any given time; these discrete positions are used as control points to construct a position spline curve. For attitude components, due to the non-Euclidean nature of the rotation space, quaternions are used for representation, and spline interpolation is performed in the quaternion space. The rotation matrix is ​​then converted to a unit quaternion. , The real part (scalar part) of a quaternion, and the rotation angle. Directly related, usually ; The imaginary part (vector part) of a quaternion represents the rotation axis vector in three-dimensional space. Multiply Results; Construction of attitude spline curves Normalization is performed after each evaluation to maintain the unit quaternion constraint.

[0065] Temporal parameterization of control points is a crucial step in constructing B-spline curves. The cumulative chord length parameterization method is employed, assigning parameter values ​​based on the spatial distance between adjacent control points. Let the first... The parameter value corresponding to each control point is ,but: ; in , , It represents the Euclidean norm; this parameterization method makes the rate of change of the curve's parameters proportional to the spatial distance, avoiding over-smoothing in regions of drastic motion.

[0066] To improve the accuracy and smoothness of the trajectory, a least-squares-based curve fitting optimization is introduced. The optimization objective function is constructed as follows: ; in, This represents the overall objective function for curve fitting optimization. The number of observation points. For the first One observation location, For the corresponding weighting coefficients, This indicates that the value calculated based on the current B-spline curve parameters is... The theoretical position of the vehicle body at any given time; The second derivative of the curve. Weights for the smoothing term; weight coefficients The confidence level of the pose estimation is dynamically adjusted, with higher confidence observations given greater weight.

[0067] For example, when a trajectory contains 10 control points, the smoothing term weights... Set to 0.01, the weight of high-confidence observations. When the weight is set to 1.0 and the weight of low-confidence observations is set to 0.3, the optimal B-spline curve parameters are obtained by solving the above optimization problem, thus achieving a balance between data fitting and curve smoothing.

[0068] The curve parameters are solved using an iterative optimization algorithm. First, the initial control point positions are obtained using the least squares method, and then the Levenberg-Marquardt algorithm is used for iterative optimization. In each iteration, the gradient of the objective function with respect to the control points and the Hessian matrix are calculated, and the control point positions are updated. ,in For step size parameters, This indicates the updated control point position obtained after the current round of optimization iterations. This indicates the position of the old (previous) control point before the start of the current optimization iteration. At the start of the first iteration, These are obtained through an initialization step, such as by directly using discrete pose observations and performing a preliminary fitting using the least squares method to obtain initial control points. In subsequent iterations, the control points calculated in the previous round are... It will be automatically assigned to the next round. This process is repeated iteratively until the position of the control point no longer changes significantly (i.e., the objective function converges). For Hessian matrix, This represents the gradient vector. The iterative process continues until the objective function converges or the maximum number of iterations is reached.

[0069] In continuous-time state estimation, real-time performance is a critical requirement; to support online updates, a sliding window strategy is employed to maintain the B-spline curve. A fixed-size time window is set. Only control points within the most recent time frame are retained. When a new pose estimate arrives, it is added to the control point set, while older control points that have exceeded the time window are removed. This incremental update method avoids rebuilding the entire curve each time, significantly reducing computational overhead.

[0070] For any query time To calculate the pose, first determine the parameter values ​​corresponding to that moment. Through a time-to-parameter mapping function Perform the conversion. Indicates from physical time Internal parameters of B-spline curve The mapping function, constructed based on the timestamps of control points through linear or spline interpolation, is used in continuous-time trajectory modeling to determine the intrinsic parameters of the B-spline curve. It is usually normalized to Between, and actual physical time It is the most commonly used and most efficient mapping function with a specific timestamp. It is a linear mapping (linear interpolation). For example, suppose a sliding window strategy is used, and the starting timestamp of the time window corresponding to the currently maintained B-spline curve is... The end timestamp is So, time to parameters The linear mapping function can be expressed as: This paper describes an efficient method for calculating B-spline curves with varying parameters using a recursive algorithm (DeBoor's algorithm). The value at that point is obtained at that moment. Location and posture The DeBoor algorithm avoids explicit evaluation of basis functions through recursive computation, and has the advantages of good numerical stability and high computational efficiency.

[0071] Velocity and acceleration information are obtained by differentiating the B-spline curve; the first derivative corresponds to velocity. : ; The second derivative corresponds to acceleration. : ; in, For velocity vectors, In terms of physical time Let be the trajectory position function of the independent variable. The mathematical function expression of B-spline curves , The acceleration vector is used; the kinematic information of velocity and acceleration is of great significance for vehicle control and trajectory prediction.

[0072] To handle abnormal situations and improve robustness, a trajectory consistency verification mechanism is introduced; by monitoring the pose change rate between adjacent control points, possible abnormal jumps are identified.

[0073] When an anomaly is detected, a local reconstruction strategy is employed, refitting the curve segment using nearby reliable control points. A maximum velocity threshold is set. and maximum angular velocity threshold Trajectory segments that violate physical constraints are corrected or eliminated.

[0074] Time synchronization of multiple sensors within a continuous time frame is achieved, where different sensors have different sampling frequencies and delays. B-spline curves can be used to evaluate the vehicle's state at any given moment, achieving true time alignment. When it is necessary to synchronize a specific sensor at a given time... When the observation is correlated with the vehicle's status, a direct query can be performed. The corresponding pose can be obtained without complex interpolation or extrapolation.

[0075] Boundary conditions are used to constrain the endpoints of the curve. At the starting point of the trajectory, boundary derivative constraints are set using initial velocity and acceleration information; at the ending point of the trajectory, reasonable extrapolation is performed based on the motion trend. These boundary conditions are implemented by adding additional constraint terms during the optimization process to ensure the natural extension of the trajectory at the endpoints.

[0076] This step utilizes a continuous-time state estimation method based on B-splines to transform discrete pose observations into continuous motion trajectories. This method not only provides accurate pose lookup capabilities at any given time but also outputs a smooth and continuous motion trajectory, effectively suppressing noise and jumps in discrete estimation. The introduction of a continuous-time frame makes multi-sensor fusion more flexible and efficient, providing high-quality positioning information for autonomous driving and meeting the stringent requirements of path planning and motion control for trajectory continuity and smoothness.

[0077] Example 2 Reference Figure 3 This is the second embodiment of the present application, which provides a positioning system based on loosely coupled multi-source fusion and continuous-time state estimation.

[0078] The system includes: The module includes a point cloud data preprocessing module, a multi-source fusion pose calculation module, a dynamic map optimization and management module, and a continuous time trajectory modeling module.

[0079] The point cloud data preprocessing module is used to stitch and filter the point cloud data acquired by the vehicle's multi-LiDAR, use adaptive voxel filtering to sparsify the point cloud, and calculate the normal vector of the sparsified point cloud.

[0080] The multi-source fusion pose estimation module uses the pose increments of the vehicle body gyroscope and wheel speedometer as prior data, performs point cloud matching based on point cloud normal vectors, calculates the covariance matrix through the normal vectors, realizes the matching between the current frame point cloud and the historical reference point cloud frame, and calculates the discrete pose estimation results of the vehicle body.

[0081] The dynamic map optimization management module constructs a dynamic local point cloud map centered on the vehicle body and calculates the confidence level of point cloud matching. When the confidence level is higher than a preset threshold and the relative displacement of the vehicle body exceeds the threshold, the current frame point cloud is updated to the dynamic local point cloud map. When the confidence level is lower than the preset threshold, the map update is paused and the vehicle body repositioning is triggered, and the current pose of the vehicle body is corrected.

[0082] The continuous-time trajectory modeling module uses the discrete pose positioning results of the vehicle body as control points of the B-spline curve, and uses the B-spline curve to perform continuous-time modeling of the vehicle body's motion trajectory. By solving the curve parameters, it calculates the vehicle body pose at any time and outputs continuous and smooth positioning results.

[0083] In the embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. Furthermore, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Additionally, the displayed or discussed mutual couplings or direct couplings or communication connections may be through some communication interfaces; indirect couplings or communication connections between devices or units may be electrical, mechanical, or other forms.

[0084] The embodiments of this application have been described above with reference to the accompanying drawings. However, this application is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments under the guidance of this application without departing from the spirit and scope of protection of the claims. All of these variations are within the protection scope of this application.

Claims

1. A localization method based on loosely coupled multi-source fusion and continuous-time state estimation, characterized in that, include: The point cloud data acquired by the vehicle body's multiple lidar sensors are stitched together and filtered. Adaptive voxel filtering is used to sparsify the point cloud, and the normal vector of the sparsified point cloud is calculated. Using the pose increments of the vehicle body gyroscope and wheel speedometer as prior data, point cloud matching is performed based on the point cloud normal vector. The covariance matrix is ​​calculated through the normal vector to achieve the matching between the current frame point cloud and the historical reference point cloud frame, and to calculate the discrete pose estimation results of the vehicle body. A dynamic local point cloud map centered on the vehicle body is constructed, and the confidence level of point cloud matching is calculated. The confidence level is based on the statistical information of the average nearest neighbor distance and the number of interior points in the point cloud matching process to characterize the matching accuracy and matching rate. When the confidence level is higher than a preset threshold and the relative displacement of the vehicle body exceeds the threshold, the current frame point cloud is updated to the dynamic local point cloud map. When the confidence level is lower than the preset threshold, the map update is paused and relocalization based on the dynamic local point cloud map is triggered, and the current pose of the vehicle body is corrected in the local point cloud map. The discrete pose localization results of the vehicle body are used as control points for the B-spline curve. The B-spline curve is used to model the motion trajectory of the vehicle body in continuous time. The continuous time modeling introduces curve fitting optimization based on least squares to construct an optimization objective function to minimize the observation error and the smoothing term of the second derivative of the curve. The weights of the observation error term and the smoothing term are dynamically adjusted according to the confidence level. The optimal B-spline curve parameters are solved through an iterative optimization algorithm. Based on the solved B-spline curve parameters, the vehicle pose at any time is calculated, and the continuous and smooth localization results are output.

2. The localization method based on loosely coupled multi-source fusion and continuous-time state estimation according to claim 1, characterized in that, The raw point cloud data from different lidar sensors in the vehicle body are processed in a unified manner. The point cloud data collected by multiple lidar sensors installed at different locations on the vehicle body are transformed into a unified vehicle coordinate system and stitched together using their respective extrinsic parameter calibration matrices. By setting the range of the vehicle body surround box, point cloud data falling within the range of the vehicle body's own structure is removed, filtering out point cloud data around the vehicle body that interferes with radar. An adaptive voxel filtering strategy is used to sparsify the point cloud. The adaptive voxel filtering strategy dynamically adjusts the voxel resolution based on the ratio of the number of original point clouds in the current frame to the number of point clouds processed in the previous frame.

3. The localization method based on loosely coupled multi-source fusion and continuous-time state estimation according to claim 2, characterized in that, After the point cloud sparsification process is completed, the normal vector of each retained point is calculated, and the normal vector is used for subsequent geometric feature description. The calculation of the normal vector includes: for each point after sparsification, constructing the covariance matrix through the set of points in its neighborhood, and obtaining the normal vector through eigenvalue decomposition; The normal vector and point cloud coordinates of each point are stored for fast recovery of the covariance matrix based on the normal vector.

4. The localization method based on loosely coupled multi-source fusion and continuous-time state estimation according to claim 3, characterized in that, The system acquires angular velocity measurements and encoder pulse counts from the vehicle's gyroscope and wheel speedometer, and calculates the angular displacement increment and linear displacement increment between adjacent time points as the initial pose estimate for point cloud matching. Based on the point cloud normal vectors calculated in the preprocessing stage, the generalized iterative nearest point algorithm is used for point cloud matching. The optimal rotation and translation parameters are solved by minimizing the average geometric distance error between matching pairs in the point cloud data association. The objective function of point cloud matching is weighted by the covariance matrix, and the matching weights in different directions are adaptively adjusted according to the local geometric characteristics.

5. The localization method based on loosely coupled multi-source fusion and continuous-time state estimation according to claim 4, characterized in that, The covariance matrix is ​​quickly recovered using the calculated normal vector, which is constructed using regularization parameters, the distance from the point to the local fitting plane, and the unit normal vector of that point. An iterative optimization strategy is adopted, which searches for nearest neighbor pairs through a spatial index structure and uses the recovered covariance matrix to construct a weighted least squares problem to solve for the pose increment; When establishing point correspondence, a dual constraint mechanism of distance threshold and normal vector angle threshold is used to remove outliers. At the same time, a hierarchical matching strategy is adopted, which first performs matching at low resolution and then gradually increases the resolution for matching.

6. The localization method based on loosely coupled multi-source fusion and continuous-time state estimation according to claim 5, characterized in that, An incremental update strategy centered on the vehicle body is adopted to construct a dynamic local point cloud map. The dynamic local point cloud map always maintains a fixed-size spatial range defined by a cube bounding box and is continuously updated as the vehicle body moves. When the vehicle body moves beyond a preset threshold, the map center is triggered to shift. The data structure of the dynamic local point cloud map uses an octree to organize the point cloud data.

7. The localization method based on loosely coupled multi-source fusion and continuous-time state estimation according to claim 6, characterized in that, The map update strategy is adaptively adjusted based on the calculated confidence score. When the confidence score is higher than a preset threshold and the relative displacement of the vehicle exceeds the threshold, the current frame point cloud is inserted into the dynamic local point cloud map after coordinate transformation. When the confidence level is lower than the preset threshold, map updates are paused and a two-stage relocalization process based on laser template matching is triggered, including: coarse localization to find candidate frame positions by extracting and matching template matching descriptors of global position recognition, taking the pose corresponding to the candidate frame position as the candidate pose, and using the candidate pose to perform precise matching with the local map and selecting the pose with the highest matching score as the relocalization result. After successful relocalization, the vehicle trajectory is corrected. A multi-threaded parallel processing architecture is adopted to ensure the real-time update of the map, and the point cloud corresponding to dynamic objects is identified and removed by analyzing the consistency of the point cloud over time.

8. The localization method based on loosely coupled multi-source fusion and continuous-time state estimation according to claim 7, characterized in that, A uniform B-spline curve is used to continuously parameterize the motion trajectory of the vehicle body, and the discrete pose estimation results are used as control points to construct a cubic B-spline curve. When performing continuous-time representation, position and attitude components are processed simultaneously. The attitude components are represented by quaternions and spline interpolation is performed in the quaternion space, and normalization is performed after each evaluation. The time parameterization of control points adopts the cumulative chord length parameterization method, which assigns parameter values ​​based on the spatial distance between adjacent control points.

9. The localization method based on loosely coupled multi-source fusion and continuous-time state estimation according to claim 8, characterized in that, A sliding window strategy is adopted to maintain the B-spline curves for online updates. Based on the parameter-identified B-spline curves, a recursive derivation method is used to calculate the corresponding pose of the vehicle body according to the timestamp. Velocity and acceleration information are obtained by differentiating the B-spline curve, and a trajectory consistency verification mechanism is introduced to identify and correct trajectory segments that violate preset maximum velocity or maximum angular velocity thresholds.

10. A positioning system based on loosely coupled multi-source fusion and continuous-time state estimation, used to implement the positioning method based on loosely coupled multi-source fusion and continuous-time state estimation as described in any one of claims 1-9, characterized in that, include: Point cloud data preprocessing module, multi-source fusion pose calculation module, dynamic map optimization and management module, and continuous time trajectory modeling module; The point cloud data preprocessing module stitches and filters the point cloud data acquired by the vehicle's multiple lidar sensors, uses adaptive voxel filtering to sparsify the point cloud, and calculates the normal vector of the sparsified point cloud. The multi-source fusion pose calculation module uses the pose increments of the vehicle body gyroscope and wheel speed meter as prior data, performs point cloud matching based on point cloud normal vectors, calculates the covariance matrix through the normal vectors, realizes the matching between the current frame point cloud and the historical reference point cloud frame, and calculates the discrete pose estimation results of the vehicle body. The dynamic map optimization management module constructs a dynamic local point cloud map centered on the vehicle body and calculates the confidence level of point cloud matching. The confidence level is based on the statistical information of the average nearest neighbor distance and the number of interior points in the point cloud matching process to represent the matching accuracy and matching rate. When the confidence level is higher than a preset threshold and the relative displacement of the vehicle body exceeds the threshold, the current frame point cloud is updated to the dynamic local point cloud map. When the confidence level is lower than the preset threshold, the map update is paused and relocalization based on the dynamic local point cloud map is triggered, and the current pose of the vehicle body is corrected in the local point cloud map. The continuous-time trajectory modeling module uses the discrete pose localization results of the vehicle body as control points for the B-spline curve, and uses the B-spline curve to perform continuous-time modeling of the vehicle body's motion trajectory. The continuous-time modeling introduces curve fitting optimization based on least squares, constructs an optimization objective function to minimize the observation error and the smoothing term of the second derivative of the curve, wherein the weights of the observation error term and the smoothing term are dynamically adjusted according to the confidence level, and the optimal B-spline curve parameters are solved through an iterative optimization algorithm. Based on the solved B-spline curve parameters, the vehicle body pose at any time is calculated, and continuous and smooth localization results are output.

Images