Intelligent positioning system based on NDT and odometer fusion

Abstract

The invention discloses an intelligent positioning system based on NDT and odometer fusion, and relates to the technical field of intelligent vehicle positioning. The data preprocessing module is used for realizing time synchronization, coordinate transformation and filtering processing of sensor data; the NDT matching module is used for providing a local positioning result based on point cloud matching; the fusion filtering module is used for fusing NDT and odometer data by adopting a covariance-based Kalman filtering framework; and a state management module. According to the intelligent positioning system based on NDT and odometer fusion, through innovative designs such as multi-sensor space-time synchronization, dynamic anomaly detection based on mahalanobis distance, three-dimensional motion model and covariance propagation, adaptive zero offset estimation and drift correction, multi-state machine management and the like, centimeter-level positioning precision is realized; the method has the advantages of strong robustness, high adaptive ability and good real-time performance, is easy to integrate based on an ROS framework, and can meet the positioning requirements of scenes such as automatic driving.

Description

Technical Field

[0001] This invention relates to the field of intelligent vehicle positioning technology, specifically to an intelligent positioning system based on the fusion of NDT and odometer. Background Technology

[0002] In intelligent transportation systems, vehicle positioning is one of the key core technologies. Its accuracy and robustness directly affect the realization of functions such as autonomous driving. Current vehicle positioning mostly relies on single sensors, such as GNSS, IMU, LiDAR, or visual sensors, which have obvious limitations: GNSS signals are easily lost in obstructed areas such as urban canyons and tunnels, resulting in decreased positioning accuracy; although inertial navigation systems (INS) can provide continuous positioning, they have accumulated errors and drift significantly after long-term operation; although the NDT matching algorithm in LiDAR SLAM can achieve high-precision local positioning, it is prone to failure in environments with sparse features. To improve positioning performance, some systems have attempted to fuse data from multiple sensors, using extended Kalman filtering (EKF) or unscented Kalman filtering (UKF) to fuse GNSS and IMU data, but many shortcomings still exist.

[0003] Existing solutions have simple fusion strategies that do not fully consider issues such as sensor spatiotemporal alignment, dynamic adjustment of covariance, and outlier removal. They also lack adaptive capabilities, making it difficult to adjust fusion weights or switch dominant sensors according to environmental changes. Furthermore, they are highly dependent on initialization and are difficult to recover automatically after initialization, making it difficult to achieve stable and high-precision positioning in complex urban environments. Summary of the Invention

[0004] The purpose of this invention is to provide an intelligent positioning system based on the fusion of NDT and odometer, in order to solve the problems of existing intelligent positioning systems mentioned in the background art, which have simple fusion strategies, do not fully consider the problems of sensor spatiotemporal alignment, dynamic adjustment of covariance and outlier removal; lack adaptive capabilities, making it difficult to adjust the fusion weights or switch the dominant sensor according to environmental changes; and have strong initialization dependence, and are difficult to automatically recover after initialization, resulting in the difficulty of achieving stable and high-precision positioning in complex urban environments.

[0005] To achieve the above objectives, the present invention provides the following technical solution: an intelligent positioning system based on the fusion of NDT and odometer, the intelligent positioning system comprising a sensor module, a data preprocessing module, an NDT matching module, a fusion filtering module, and a state management module; Sensor module: Used to collect the required data information; Data preprocessing module: realizes time synchronization, coordinate transformation, and filtering of sensor data; NDT matching module: provides local localization results based on point cloud matching; Fusion filtering module: It adopts a covariance-based Kalman filtering framework to fuse NDT and odometer data; State Management Module: Responsible for system initialization, state switching, exception handling and recovery.

[0006] In this embodiment, the sensor module includes an IMU, a wheel speed meter (CAN bus), and a lidar (LiDAR). IMU: Provides three-axis angular velocity and linear acceleration information for high-frequency attitude prediction and motion compensation; Wheel speed meter: Acquires longitudinal speed information via the vehicle's CAN bus to assist in odometer calculation; LiDAR: Provides 3D point cloud data of the surrounding environment and is the basic input for the NDT matching algorithm.

[0007] In this embodiment, the time synchronization in the data preprocessing module is based on the ROS message_filters mechanism to perform approximate time synchronization of IMU and CAN data (tolerating ~100ms error) to ensure data timestamp consistency. Coordinate transformation: The tf2 library is used to uniformly transform the data from various sensors to the vehicle coordinate system and the global map coordinate system (map); Filtering: Low-pass filtering is applied to the raw IMU data; first-order low-pass filtering is applied to the steering wheel angle signal to eliminate high-frequency noise.

[0008] In this embodiment, the NDT matching module uses the normal distribution transformation algorithm to match the current frame point cloud with the preloaded point cloud map, outputs the 6-DOF pose of the vehicle in the current map, and provides matching quality evaluation indicators including matching score, convergence status, iteration number, and transformation probability. It supports multiple NDT implementations (PCL general version, ANH accelerated version, GPU version, OpenMP multi-threaded version) and can be flexibly selected according to computing power.

[0009] In this embodiment, the core functions of the fusion filtering module are: fusing the NDT observed pose with the predicted pose based on the motion model, performing anomaly detection on the NDT observations based on Mahalanobis distance, dynamically adjusting the fusion weights, predicting and updating the pose covariance matrix in real time, and characterizing the uncertainty of the localization results.

[0010] In this embodiment, the state management module maintains multiple system states such as UNINITIALIZED, INITIALIZED, CONVERGED, ODOM_ONLY, and RECOVERING. It automatically switches states based on sensor data quality and environmental conditions. When NDT continuous matching fails or an abnormal observation occurs, it automatically switches to pure odometer mode and attempts to recover. It also supports receiving external initial pose input via the / initialpose topic.

[0011] The system supports the following states and automatically switches between them based on sensor data quality and environmental conditions: UNINITIALIZED: Uninitialized state, awaiting initial pose provided via the / initialpose topic.

[0012] INITIALIZED: Initialized state, multi-sensor fusion positioning has begun.

[0013] CONVERGED: The positioning has converged, NDT matching is good, and the output pose has high accuracy and high reliability.

[0014] ODOM_ONLY: NDT failure state, relying solely on odometry for pose estimation to ensure basic system functions.

[0015] RECOVERING: Recovering status, attempting to reinitialize or waiting for NDT matching to recover.

[0016] State transitions are triggered by conditions such as NDT matching score, convergence state, Mahalanobis distance, and number of consecutive failures, and the current state is published externally through the / localization_state topic.

[0017] In this embodiment, a map dynamic update module is provided between the NDT matching module and the fusion filtering module. The map dynamic update module is used to detect the difference between the preloaded point cloud map and the current environment in real time, and dynamically update the map data. By receiving the point cloud matching difference information output by the NDT matching module and the pose covariance data of the fusion filtering module, it determines whether the environmental change is a permanent change (such as road construction or the addition of fixed obstacles) and performs incremental updates to the map. At the same time, the updated map is fed back to the NDT matching module to ensure the accuracy of subsequent matching. Synergistic effect: It works in conjunction with the NDT matching module to solve the problem of adapting static maps to dynamic environments, and works in conjunction with the fusion filtering module to verify the credibility of environmental changes, thereby improving the system's positioning robustness in dynamic scenarios.

[0018] In this embodiment, a positioning result verification and feedback module is set between the fusion filtering module and the state management module. This module is responsible for multi-dimensional verification of the final pose output by the fusion filtering module. It combines the motion consistency of the IMU, the reasonableness of the wheel speed meter's mileage estimation, and preset road constraints (such as lane line boundaries and road curvature) to determine whether the positioning result is reliable. If the verification finds an anomaly, it will feed back to the state management module to trigger state adjustment (such as switching from CONVERGED to RECOVERING), and at the same time output correction coefficients to the fusion filtering module to optimize the fusion weight allocation. Synergistic effect: It forms a closed-loop verification with the fusion filtering module and works in conjunction with the state management module to improve the accuracy of state switching, further enhancing the accuracy and reliability of the positioning results, which is in line with the core technical goal of the solution: "high precision and strong robustness".

[0019] In this embodiment, the multi-sensor fusion positioning method of the intelligent positioning system includes the following steps: S1. Data synchronization: Approximate time synchronization between IMU and CAN data is achieved through ROS message_filters:Synchronizer, with a maximum interval tolerance of 0.01 seconds to ensure time consistency during joint data processing. S2. Motion Prediction: Based on a 3D motion model, the attitude is updated through quaternion integration, and the predicted position is calculated using wheel velocity data. The attitude is updated again using IMU angular velocity integration, and the wheel velocity data provides longitudinal velocity to predict the vehicle's pose at the next moment. Wherein: ① Attitude prediction: Attitude update is performed using quaternion integration.

[0020] in Represented as an angular increment vector, Angular velocity vector after IMU correction (rad / s) : Time interval (s) The attitude quaternion at the current moment. The attitude quaternion predicted for the next time step. Represented as a quaternion multiplication operator, For the angle increment Constructed quaternions.

[0021] ②Location prediction:

[0022] in, This indicates the longitudinal speed (m / s) provided by the wheel speed gauge. Velocity vector in map coordinate system : The rotation matrix corresponding to the current posture (from the vehicle coordinate system to the map coordinate system). : Current position vector , : The predicted position vector at the next moment.

[0023] S3, NDT Fusion: NDT fusion uses Mahalanobis distance to determine the reliability of observations and employs weighted fusion and spherical linear interpolation to achieve pose fusion. When the NDT output is reliable (Mahalanobis distance less than the threshold, good convergence, low score), it is weighted and fused with the predicted pose. Wherein: ① Mahalanobis distance detection: Determine whether the difference between the observed and predicted NDT values ​​is within the allowable range:

[0024] like If the value is less than the threshold, then fusion is performed; otherwise, the observation is rejected.

[0025] in, Represented as Mahalanobis distance value, : NDT observation pose vector : Observation matrix (maps the state vector to the observation space). State prediction Values ​​(including position and orientation). Predict the covariance matrix. : Matrix transpose operator; Matrix inversion operator.

[0026] ②Position fusion:

[0027] in This is represented as the merged position vector. : NDT observation location vector Predict the location vector. : Fusion ratio parameter The posture after fusion NDT observation attitude, Predicting attitude, : Spherical linear interpolation function, used for quaternion fusion.

[0028] S4. Covariance Update: Update the pose covariance matrix based on the motion model and NDT observations, and dynamically adjust the fusion weights. Wherein: ①Prediction step:

[0029] in It is represented as the prior covariance matrix at time k (prediction uncertainty). : The posterior covariance matrix at time k-1, F: The state transition Jacobian matrix (representing how the state changes over time). Process noise covariance matrix (representing the uncertainty of the prediction process).

[0030] ② Update step (when NDT is valid):

[0031]

[0032] in It is represented as the Kalman gain matrix (which determines the degree to which one trusts the prediction or the observation). Observation matrix : NDT observation noise covariance matrix (representing the uncertainty of NDT observations) Identity matrix : The posterior covariance matrix at time k (updated uncertainty).

[0033] S5. Zero Bias and Drift Correction: When the vehicle is stationary or at low speed, IMU angular velocity data is collected to estimate its zero bias. When NDT is effective, the attitude error is calculated and the zero bias is updated. Among these: ① Zero bias estimation: When the vehicle is stationary or at low speed, statistical analysis of IMU angular velocity data is performed to estimate its zero bias.

[0034] in This is represented as the estimated gyroscope zero bias vector. Number of samples : No. Each angular velocity sample value.

[0035] ② Drift correction: When NDT is active, calculate the attitude error and update the zero bias:

[0036]

[0037]

[0038] in This is expressed as the difference between the observed yaw angle and the predicted yaw angle under NDT. : Yaw angle observed by NDT Predicted yaw angle : Estimated yaw rate : Z-axis gyroscope zero bias (affects yaw angle measurement) Drift correction gain parameters ( ), Assignment operator.

[0039] Compared with the prior art, the beneficial effects of the present invention are as follows: This intelligent positioning system based on the fusion of NDT and odometry achieves centimeter-level positioning accuracy through innovative designs such as multi-sensor spatiotemporal synchronization, dynamic anomaly detection based on Mahalanobis distance, three-dimensional motion model and covariance propagation, adaptive zero-bias estimation and drift correction, and multi-state machine management. It has strong robustness, high adaptability and good real-time performance, and is easy to integrate based on the ROS framework, which can meet the positioning needs of autonomous driving and other scenarios.

[0040] 1. Further, high-precision positioning: Through multi-sensor fusion and optimized algorithm design, centimeter-level positioning accuracy can be provided in various environments.

[0041] 2. Further, strong robustness: When NDT fails, the system automatically switches to pure odometer mode to maintain basic positioning capability and ensure positioning continuity.

[0042] 3. Furthermore, it has strong adaptability: it can dynamically adjust the fusion strategy and covariance according to environmental changes, adapting to different complex scenarios.

[0043] 4. Furthermore, it has good real-time performance: all algorithms have been optimized and can run in real time, meeting the real-time requirements of scenarios such as autonomous driving; 5. Easy to integrate: Developed based on the ROS framework, it supports multiple sensors and vehicle platforms, and has strong adaptability. Attached Figure Description

[0044] Figure 1 This is a system architecture diagram of the present invention; Figure 2 This is a flowchart of the fusion filtering process of the present invention; Figure 3 This is a schematic diagram of the covariance update of the present invention. Detailed Implementation

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

[0046] This invention provides an intelligent positioning system based on NDT and odometer fusion, comprising, at its core, an intelligent positioning system including a sensor module, a data preprocessing module, an NDT matching module, a fusion filtering module, and a state management module. The sensor module is used to collect the required data information. The data preprocessing module performs time synchronization, coordinate transformation, and filtering of the sensor data. The NDT matching module provides local positioning results based on point cloud matching. The fusion filtering module uses a covariance-based Kalman filter framework to fuse NDT and odometer data. The state management module is responsible for system initialization, state switching, anomaly handling, and recovery.

[0047] Example 1: To better understand the above technical solution, the following will provide a detailed description of the technical solution in conjunction with the accompanying drawings and specific implementation methods. (Refer to...) Figure 1 As shown in the figure, this is the system architecture diagram of the present invention, which is an intelligent positioning system based on the fusion of NDT and odometer, including a sensor module, a data preprocessing module, an NDT matching module, a fusion filtering module and a status management module. Sensor module: Used to collect the required data information.

[0048] Specifically, the sensor module includes an IMU, a wheel speed sensor (CAN bus), and a lidar (LiDAR). IMU: Provides three-axis angular velocity and linear acceleration information for high-frequency attitude prediction and motion compensation; Wheel speed meter: Acquires longitudinal speed information via the vehicle's CAN bus to assist in odometer calculation; LiDAR: Provides 3D point cloud data of the surrounding environment and is the basic input for the NDT matching algorithm.

[0049] It should be noted that the sensor module is used to collect raw data on the vehicle's motion status and the surrounding environment, providing core input support for subsequent positioning calculations and ensuring the real-time, continuous, and accurate nature of data collection. The parameters and functional details of each component—IMU (Inertial Measurement Unit), wheel speedometer (CAN bus based), and LiDAR—are as follows: IMU: Employs a high-precision MEMS inertial measurement unit with a sampling frequency of no less than 100Hz. The measurement range covers angular velocity of ±2000° / s and linear acceleration of ±16g, featuring low zero-bias drift (≤0.1° / h) and low noise characteristics. It outputs raw triaxial angular velocity and linear acceleration data in real time, providing basic data for high-frequency attitude prediction to the fusion filtering module, and correcting motion distortion of the LiDAR point cloud through motion compensation algorithms to improve point cloud matching accuracy. Wheel speed sensor: Real-time acquisition of wheel speed signals via vehicle CAN bus (communication baud rate supports 500Kbps / 1Mbps), sampling period ≤10ms, speed measurement accuracy ±0.1m / s; converted into vehicle longitudinal speed information by mileage estimation algorithm, which not only assists the fusion filtering module in optimizing position prediction results, but also can be cross-validated with IMU data to eliminate abnormal speed values ​​and improve the reliability of motion model; LiDAR: Employs a 16-line or higher multi-line LiDAR with a ranging range of 0.5–150m, ranging accuracy of ±2cm, point cloud output frequency of 10Hz, horizontal field of view of 360°, and vertical field of view of -15° to +15°. By emitting laser beams to scan the surrounding environment, it generates high-density 3D point cloud data, fully characterizing environmental features such as roads and obstacles. This data serves as the core input for the NDT matching module to perform point cloud registration and output local positioning results, while also providing the original basis for the map dynamic update module to detect environmental changes.

[0050] Data preprocessing module: realizes time synchronization, coordinate transformation and filtering of sensor data.

[0051] Specifically, the time synchronization of the data preprocessing module is based on the ROS message_filters mechanism to perform approximate time synchronization of IMU and CAN data (tolerating ~100ms error) to ensure data timestamp consistency. Coordinate transformation: The tf2 library is used to uniformly transform the data from various sensors to the vehicle coordinate system and the global map coordinate system (map); Filtering: Low-pass filtering is applied to the raw IMU data; first-order low-pass filtering is applied to the steering wheel angle signal to eliminate high-frequency noise.

[0052] It should be noted that the data preprocessing module, as the core bridge between the raw sensor data and the subsequent positioning calculation module, is responsible for standardizing, denoising, and spatiotemporally aligning the multi-source heterogeneous sensor data, outputting high-quality data with unified format, consistent timing, and controllable noise, ensuring the calculation accuracy and efficiency of the NDT matching module and the fusion filtering module. The implementation details of the core functions of the data preprocessing module are as follows: Time synchronization: Based on the ApproximateTimeSynchronizer synchronization strategy in the message_filters mechanism of ROS (RobotOperatingSystem), it performs multi-topic approximate time synchronization for three types of heterogeneous data: IMU (100Hz), wheel speed meter CAN data (10Hz), and LiDAR point cloud (10Hz). The synchronization time tolerance threshold is configurable (default ≤100ms), and the time deviation of unsynchronized data is corrected through a timestamp interpolation compensation mechanism. Linear interpolation is used for high-frequency IMU data to map the IMU data at asynchronous times to the target synchronization timestamp, ensuring the continuity of attitude prediction. The lidar point cloud data is timestamped by combining the motion data of the IMU to eliminate the time distortion caused by vehicle movement during the point cloud acquisition process. Synchronized data is uniformly stamped with a system timestamp (UTC format) and written into the header field of the ROS topic to ensure that all modules perform calculations based on a unified time base.

[0053] Coordinate Transformation: Based on the ROS tf2 coordinate transformation library, a multi-sensor coordinate tree is constructed to achieve accurate transformation of data from each sensor's own coordinate system to a unified coordinate system. Prior to completing the sensor extrinsic parameter calibration: the translation matrix (x / y / z offset) and rotation matrix (roll / pitch / yaw angle) of the lidar and IMU relative to the vehicle chassis coordinate system (base_link) are obtained by hand-eye calibration method, with calibration accuracy ≤0.5cm (translation) and ≤0.1° (rotation); Real-time coordinate transformation: The raw IMU data is transformed from the IMU's own coordinate system (imu_link), the LiDAR point cloud is transformed from the radar coordinate system (lidar_link), and the wheel speed sensor data is transformed from the wheel coordinate system (wheel_link) to the vehicle center coordinate system (base_link). Then, the pose information output by the fusion filtering module is used to transform the data in base_link to the global map coordinate system (map), supporting multiple map coordinate systems such as UTM and Gaussian projection. Coordinate transformation error compensation: To address issues such as chassis deformation and loose sensor installation during vehicle operation, a real-time deviation detection mechanism is introduced. The coordinate transformation matrix is ​​corrected in reverse through NDT matching results to ensure transformation accuracy.

[0054] Filtering: A hierarchical filtering strategy is employed to reduce noise in data from different types of sensors while preserving effective signal characteristics. IMU raw data filtering: A 5th-order Butterworth low-pass filter (cutoff frequency 10Hz) is used to filter out high-frequency noise generated by vehicle vibration (such as engine vibration and road bumps causing ≥10Hz angular velocity / acceleration interference) while retaining the effective low-frequency signal of vehicle motion; after filtering, zero bias calibration is also performed, and the zero bias value is updated in real time based on the IMU data during the vehicle stationary period to eliminate system errors.

[0055] Steering wheel angle signal filtering: A first-order low-pass filter (filter coefficient 0.2) is used to smooth high-frequency vibrations of the steering wheel (such as instantaneous angle fluctuations caused by the driver making small corrections to the direction), and output a stable angle value to provide reliable steering parameters for odometer calculation.

[0056] LiDAR point cloud filtering: After coordinate transformation, voxel filtering and outlier removal are added. The voxel grid size is set to 0.1m×0.1m×0.1m to reduce the amount of point cloud data (downsampling rate of about 50%). At the same time, outlier point clouds with a distance from the mean exceeding 3 times the standard deviation are removed, reducing the computational load of NDT matching and improving matching accuracy.

[0057] NDT matching module: provides local localization results based on point cloud matching.

[0058] Specifically, the NDT matching module uses the normal distribution transformation algorithm to match the current frame point cloud with the preloaded point cloud map, outputs the 6-DOF pose of the vehicle in the current map, and provides matching quality evaluation indicators including matching score, convergence status, iteration number, and transformation probability. It supports multiple NDT implementations (PCL general version, ANH accelerated version, GPU version, OpenMP multi-threaded version) and can be flexibly selected according to computing power.

[0059] It should be noted that the NDT matching module, as the core unit of lidar positioning, is responsible for accurately registering the pre-processed real-time point cloud data with the high-precision preloaded point cloud map, outputting the local positioning result of the vehicle in the global map, and quantifying the matching quality to provide reliable observation input for the fusion filtering module. It is a key link to achieve centimeter-level positioning. The core implementation details and functions of the NDT matching module are as follows: Algorithm execution flow: The Normal Distributions Transform (NDT) algorithm is used to complete point cloud registration. The core process consists of three steps: Step 1: Map Gridding and Probability Distribution Modeling: Divide the preloaded global point cloud map into a fixed-size 3D grid (default grid size 1.0m×1.0m×0.5m, which can be adapted according to the scene). Calculate the mean and covariance matrix of the point cloud data in each grid, and construct a 3D normal distribution model to characterize the spatial features of the area. Step 2: Initial pose guidance and iterative optimization: The predicted pose output by the fusion filtering module is used as the initial registration pose (or the pose is calculated by odometry if there is no initial pose). The current frame point cloud is mapped to the map coordinate system. The likelihood function between the current frame point cloud and the normal distribution of the map grid is minimized by the Gauss-Newton iteration method. The iteration terminates when the number of iterations reaches the preset upper limit (default 50 times) or the pose update amount is less than the threshold (translation ≤ 0.01m, rotation ≤ 0.1°). Step 3: Pose Output and Quality Quantization: After iterative convergence, output the vehicle's 6-DOF pose (x / y / z translation, roll / pitch / yaw rotation) in the map, and calculate and output multi-dimensional matching quality evaluation indicators to provide a basis for subsequent fusion decisions.

[0060] Detailed Explanation of Matching Quality Assessment Indicators: The definitions, thresholds, and decision-making logic of each assessment indicator are as follows: Match score: Represents the degree of fit between the current frame point cloud and the map. The smaller the value, the higher the matching degree. The effective matching threshold is ≤100 (unit: m⁻¹). If the value exceeds the threshold, the match is considered invalid. Convergence status (has_converged): A boolean index that directly reflects whether the iterative optimization has reached the convergence condition. True indicates convergence and False indicates non-convergence. Number of iterations: The actual number of iterations for optimization. If it is much lower than the preset upper limit (e.g., ≤10 times) and the score meets the standard, it indicates that the matching process is stable. If it is close to the upper limit and still has not converged, it indicates that the environmental features are insufficient or the initial pose deviation is too large. Transformation probability: The confidence probability of the current pose transformation, ranging from 0 to 1. ≥0.8 indicates a reliable matching result, while <0.5 is marked as a low-confidence match.

[0061] Multi-version NDT algorithm adaptation and computing power optimization: Supports seamless switching between multiple NDT implementation versions, adapting to different hardware computing power scenarios. PCL General Version: Based on the native NDT implementation of the Point Cloud Library (PCL), it has strong compatibility, no hardware dependency, and is suitable for entry-level computing platforms (such as low-end industrial control computers). ANH Accelerated Version: Adaptive Neighborhood Hash (ANH) algorithm is used to optimize grid search efficiency, and the matching speed is 30% to 50% faster than the PCL version. It is suitable for medium computing power platforms. GPU version: Based on the CUDA parallel computing framework, core computing tasks such as mesh modeling and iterative optimization are offloaded to the GPU, improving matching speed by 2 to 3 times, and is suitable for high-computing-power autonomous driving domain controllers. OpenMP Multithreaded Version: By using CPU multithreading to process normal distribution calculations of different grids in parallel, it can improve matching efficiency by 40% to 60% on multi-core CPU platforms, while taking into account compatibility and computing power utilization.

[0062] Robustness Enhancement Mechanisms: To cope with complex environments (such as tunnels, open roads, and other scenarios with sparse features), the module adds two key mechanisms: Dynamic mesh adjustment: When the matching score exceeds the threshold for 3 consecutive frames, the mesh size is automatically reduced to 0.5m×0.5m×0.25m to improve the feature modeling accuracy in sparse point cloud scenarios; Historical pose constraints: If the matching of the current frame fails, the pose sequence of the last 5 valid matches is called, and a temporary observation pose is generated through linear prediction to ensure the continuity of positioning until the matching returns to normal. Point cloud validity screening: Only valid point clouds within 0.5 to 100m of the vehicle after preprocessing are retained for matching, and interference point clouds that are too close (such as points occluded by the vehicle body) or too far away (such as invalid background points) are removed to reduce the amount of computation and improve the matching accuracy.

[0063] The map dynamic update module is used to detect the differences between the preloaded point cloud map and the current environment in real time, and dynamically update the map data. By receiving the point cloud matching difference information output by the NDT matching module and the pose covariance data from the fusion filtering module, it determines whether the environmental change is permanent (such as road construction or the addition of fixed obstacles) and performs incremental updates to the map. At the same time, the updated map is fed back to the NDT matching module to ensure the accuracy of subsequent matching. Synergistic effect: It works with the NDT matching module to solve the problem of adapting static maps to dynamic environments, and works with the fusion filtering module to verify the credibility of environmental changes, thereby improving the system's positioning robustness in dynamic scenarios.

[0064] It should be noted that the map dynamic update module, as the core extension unit for the system to adapt to dynamic environments, is responsible for perceiving the differences between the preloaded point cloud map and the current actual environment in real time. Through multi-module data linkage, it realizes incremental and high-precision map updates, solves the adaptability problem of static maps in scenarios such as road construction and the addition of fixed facilities, and continuously provides high-quality map data that fits the real-time environment for the NDT matching module, ensuring the stability of positioning accuracy from the source. The core implementation details, functional logic, and collaboration mechanism of the map dynamic update module are as follows: Data Input and Preprocessing: Multi-source Data Acquisition: Real-time reception of three types of core data—the current frame point cloud, map matching residual (distance deviation between point cloud and map grid), and matching quality indicators (score, probability) output by the NDT matching module; the vehicle's 6-DOF pose and corresponding covariance matrix (characterizing pose reliability) output by the fusion filtering module; and the preprocessed raw point cloud of the LiDAR from the sensor module (preserving complete environmental details).

[0065] Data validity screening: Based on the pose covariance matrix of the fusion filtering module, a covariance threshold is set (e.g., translation covariance ≤ 0.01m², rotation covariance ≤ 0.001rad²). Difference detection is only enabled for the current frame data when the pose confidence meets the standard. At the same time, low-quality matching data with NDT matching score > 100 or probability < 0.6 are filtered to avoid erroneous updates caused by invalid data.

[0066] Environmental difference detection and change type judgment: Difference area extraction: The point cloud difference algorithm is used to compare the effective point cloud of the current frame with the point cloud of the corresponding area in the preloaded map (based on the vehicle pose to lock the map range) point by point, and calculate the point cloud distance deviation (Δd). When Δd > 0.3m (configurable) and the number of difference points accounts for more than 15% of the total number of points in the area, it is marked as a potential difference area.

[0067] Permanent change determination: Accurate determination is achieved through multi-frame data accumulation verification and feature analysis. Time dimension verification: Continuously monitor the potential difference area for 5 to 10 frames (about 1 to 2 seconds). If the spatial distribution and number of difference points remain stable (fluctuation ≤ 5%), and there is no "following" change with vehicle movement (excluding dynamic obstacles such as pedestrians and vehicles), it is preliminarily determined to be a permanent change. Feature-assisted verification: Combined with the NDT matching residual distribution, if the matching residual in the difference region is consistently higher than that in other regions (≥2 times the average residual), and the pose of the fusion filtering module does not show abnormal drift (no sudden change in covariance), the authenticity of the change is further confirmed. Change type classification: The change type is distinguished by point cloud clustering algorithm. The addition of fixed obstacles (such as guardrails and street lights) is represented as "new point cloud cluster blocks", and road construction (such as road excavation and laying of fences) is represented as "existing map point cloud missing + new construction area point cloud". The boundary coordinates and feature parameters of the changed area are recorded.

[0068] Incremental map update mechanism: Update range locking: Based on the vehicle's pose and the spatial coordinates of the different areas, only the map within a 5m radius of the changed area is locally updated, avoiding the computational cost and time delay caused by global map reconstruction.

[0069] Update execution logic: New content integration: For newly added fixed obstacles, the point cloud cluster blocks are calculated to have their precise coordinates in the global map coordinate system, added to the corresponding grid on the map, and the normal distribution model (mean, covariance matrix) of the grid is updated. Invalid content removal: For original map features that have disappeared in scenarios such as road construction (such as original road surface and old facilities), after verifying through multi-frame point cloud that there is no corresponding real-time point cloud feedback, the invalid point cloud data of that area in the map is deleted, and the normal distribution of the surrounding grid is remodeled. Update log recording: Automatically records the timestamp of each update, coordinates of the changed area, change type, and data frame information used. Supports map rollback function (when an update error is detected later, it can be restored to the most recent effective map version).

[0070] Updated map feedback and verification: Real-time feedback to the NDT matching module: The incrementally updated local map data is pushed to the NDT matching module in real time, replacing the old data of the corresponding area in the original map, ensuring that subsequent NDT matching is based on the latest environmental features, improving matching accuracy and convergence speed; at the same time, the boundary information of the area is changed synchronously, so that the NDT module automatically adjusts the grid size (such as shrinking it to 0.5m×0.5m×0.25m) when matching in that area, optimizing the matching effect.

[0071] Update effect verification: Closed-loop verification is used to ensure the reliability of the update. The matching score and convergence status of the NDT matching module are compared after the update. If there is no abnormal fluctuation in the pose covariance of the fusion filtering module, the update is deemed valid. If the matching quality deteriorates or the positioning drifts, map rollback is automatically triggered and the difference detection process is restarted.

[0072] Synergistic Effects and System Value: Synergistic with the NDT matching module: It resolves the compatibility contradiction between static maps and dynamic environments. By updating map data in real time, it avoids matching failures and positioning deviations caused by environmental changes in the NDT module. Especially in scenarios such as road construction and urban infrastructure upgrades, it can maintain positioning accuracy at the centimeter level. At the same time, the matching residual data of the NDT module provides direct feature basis for difference detection, improving the accuracy of change judgment.

[0073] Linked with the fusion filtering module: Based on the pose covariance of the fusion filtering module, point cloud data corresponding to low-confidence poses are filtered to avoid map updates due to positioning drift. Conversely, the updated high-precision map can optimize the NDT matching results, providing more reliable observation input for the fusion filtering module, forming a positive closed loop of "confidence in pose → map update → accurate matching → more stable pose".

[0074] Additional system value: It eliminates the need to rely on external map update services, enabling autonomous and real-time map iteration and reducing the system's timeliness requirements for pre-loaded maps; at the same time, the local incremental update mode significantly reduces computing power consumption, ensuring that the system's real-time performance is not affected, and further improving the robustness of the intelligent positioning system in complex and dynamic urban environments.

[0075] Fusion filtering module: It adopts a covariance-based Kalman filter framework to fuse NDT and odometer data.

[0076] refer to Figure 2 As shown in the figure, this is a flowchart of the fusion filtering process of the present invention. Specifically, the core functions of the fusion filtering module are: fusing the NDT observed pose with the predicted pose based on the motion model, performing anomaly detection on the NDT observed values ​​based on Mahalanobis distance, dynamically adjusting the fusion weights, predicting and updating the pose covariance matrix in real time, and characterizing the uncertainty of the localization result.

[0077] It should be noted that the fusion filtering module, as the core fusion hub of the system's positioning data, integrates the high-precision observation pose of the NDT matching module and the continuous motion estimation data of the odometry based on the covariance Kalman filtering framework. Through dynamic anomaly detection, adaptive weight allocation and fine management of covariance, it outputs a final positioning result with both high accuracy and high stability. It is the core unit to ensure the continuity and reliability of positioning in complex environments. The core implementation details, algorithm logic, and functions of the fusion filtering module are as follows: The core workflow of the Kalman filter framework: It employs the Extended Kalman Filter (EKF) to adapt to nonlinear motion models. The complete workflow is divided into two main stages: prediction and update, achieving a closed-loop operation of "prediction-observation-fusion-correction". Prediction phase (based on motion model): using the fused pose from the previous time step ( , Based on this, and combined with the three-axis angular velocity, linear acceleration, and longitudinal velocity data from the IMU and wheel speedometer, the predicted pose at the current moment is calculated using a three-dimensional motion model. ) and the predicted covariance matrix ( In this process, attitude prediction uses quaternion integration to eliminate singularities, while position prediction incorporates the transformation between the vehicle coordinate system and the map coordinate system to ensure that the prediction results closely match the actual driving conditions. Simultaneously, a process noise covariance matrix is ​​introduced. ), quantifying the uncertainty of the motion model itself (such as speed error caused by road bumps). The matrix can be dynamically adjusted according to the vehicle's speed (at high speeds). The value increases at low speeds. (Value decreases).

[0078] Update phase (fusion of NDT observations): Receives the observation pose output by the NDT matching module ( ) and the observation noise covariance matrix ( First, the reliability of the observations is tested using Mahalanobis distance; if the observations are valid, the Kalman gain is calculated. ), weighing the reliability of the predicted pose (by The reliability of the observed pose (characterization) and the reliability of the observed pose (by...) The predicted pose is corrected by characterization, and the fused optimal pose is output. ) and the updated posterior covariance matrix ( If the observation is invalid, the predicted pose is output directly to ensure the continuity of positioning.

[0079] Dynamic anomaly detection mechanism based on Mahalanobis distance: Core principle: Calculate the NDT observation pose ( ) and predicted pose ( Mahalanobis distance between ) The formula for determining whether an observed value deviates from a reasonable range is: ,in The observation matrix (maps the state vector to the observation space).

[0080] Threshold setting and decision logic: A preset Mahalanobis distance threshold (default 3.0, based on the χ² distribution confidence interval) is used. When 3.0 < 3.0, the NDT observation is deemed valid, and a fusion update is performed; when 3.0 ≤ When the value is <5.0, it is marked as a low-confidence observation, and its fusion weight is reduced; when When the value is ≥5.0, it is judged as an abnormal observation (such as NDT matching getting stuck in a local optimum or sudden change in environmental characteristics), and the observation is directly rejected to avoid erroneous data from contaminating the fusion results.

[0081] Anomaly accumulation handling: If an NDT observation anomaly is detected for 3 consecutive frames, a trigger signal is sent to the state management module to push the system to switch from the "CONVERGED" state to the "ODOM_ONLY" state. At the same time, the anomaly start time and environmental characteristics are recorded to provide a reference for subsequent recovery.

[0082] Dynamic fusion weight adjustment strategy: Weight calculation logic: Fusion weight ( (Value range 0-1) Dynamic correlation Mahalanobis distance ( ), NDT matching quality indicators (score, probability) and prediction covariance matrix ( The core formula is:

[0083] in To predict the trace of the covariance matrix, we characterize the prediction uncertainty.

[0084] Weight adaptation scenarios: When NDT matching quality is excellent (score≤50, probability≥0.9) and When <1.0, A value of 0.7 to 0.9 is used to prioritize trusting the NDT-observed pose and improve positioning accuracy. When the NDT matching quality is average (50 < score ≤ 100, 0.7 ≤ probability < 0.9) or When the value is between 1.0 and 3.0, The value is set between 0.4 and 0.6 to balance the weights of prediction and observation, taking into account both accuracy and stability. When NDT observations have low confidence ( When the value is between 3.0 and 5.0, The value ranges from 0.1 to 0.3, mainly relying on odometry to predict pose, with only fine adjustments made using observed values; When the system is in the "RECOVERING" state, The initial value is 0.2, which is gradually increased as the NDT matching quality improves to avoid positioning fluctuations caused by rapid switching.

[0085] refer to Figure 3 As shown, this figure is a schematic diagram of the covariance update of the present invention. Real-time prediction and update of the pose covariance matrix: prediction step covariance calculation: based on the state transition Jacobian matrix ( ) and process noise covariance matrix ( The formula is: .in The matrix is ​​dynamically adjusted based on the vehicle's motion state; for example, when the vehicle is driving on a slope, optimization is performed. Pitch parameters in the matrix improve the accuracy of covariance prediction; The matrix adopts an adaptive adjustment strategy, which updates in real time based on the noise statistics of IMU data to ensure that the process noise modeling is consistent with reality.

[0086] Update step covariance calculation: When NDT observations are valid, combine with Kalman gain ( ) and observation matrix ( The formula is:

[0087] in It is the identity matrix; when the observation is invalid,

[0088] ( This is a pre-defined uncertainty increment, representing the accumulation of positioning error when there are no effective observations.

[0089] The application value of covariance: posterior covariance matrix ( traces Directly characterizing the uncertainty of the final localization result, when When the preset threshold (default 0.05m²) is reached, a "low location reliability" signal is sent to the status management module, and a data validity judgment basis is provided to the map dynamic update module (only when...). (The data from this frame will only be used for map updates when the area is ≤0.05m²).

[0090] Adaptive zero-bias estimation and drift correction linkage: Zero-bias estimation trigger condition: When the vehicle is stationary (wheel speedometer speed ≤ 0.1m / s) or traveling at low speed (0.1m / s < speed ≤ 1m / s) and 5 consecutive frames of NDT observations are valid, IMU gyroscope zero-bias estimation is started.

[0091] Zero bias calculation and update: The zero bias estimate is obtained by statistically averaging the raw IMU angular velocity data. It compensates the original IMU data in real time to eliminate attitude drift caused by system bias; at the same time, it writes the bias value into the state vector of the fusion filtering module to participate in the covariance propagation calculation.

[0092] Drift correction logic: When NDT observations are valid, calculate the difference in yaw angle between the observed attitude and the predicted attitude. Calculate the yaw drift rate The z-axis gyroscope zero bias is updated using a proportional-integral (PI) control algorithm. ( To correct the gain, the cumulative drift of the IMU is continuously suppressed to ensure the stability of long-term positioning.

[0093] Synergistic effect and system value: Synergizes with NDT matching module: Through Mahalanobis distance detection and weight adjustment, it filters effective NDT observation data to avoid low-quality matching results from affecting positioning accuracy; at the same time, it feeds back the fused pose to the NDT module as the initial matching pose to improve the convergence speed and success rate of NDT matching.

[0094] Collaboration with odometer (sensor module): Deeply mine motion data from IMU and wheel speedometer to build a high-precision three-dimensional motion model, providing continuous positioning support when NDT fails, ensuring the basic functions of the system; through zero bias estimation and drift correction, optimize sensor data quality, forming a positive cycle of "data optimization - accurate model - reliable fusion".

[0095] In conjunction with the state management module: it outputs the location reliability (based on the trace of the covariance matrix) and NDT observation anomaly status in real time, providing the core decision basis for system state switching; it receives state instructions (such as "ODOM_ONLY" and "RECOVERING") from the state management module, and adaptively adjusts the fusion strategy to improve the system's adaptability under different states.

[0096] Core value: Breaking through the limitations of single-sensor positioning, through the design of sophisticated fusion algorithms, it achieves the complementary advantages of "high-precision observation + continuous motion inference". Even in extreme scenarios such as lack of GNSS and sparse environmental features, it can still output centimeter-level positioning accuracy and highly robust positioning results, meeting the needs of core scenarios such as autonomous driving.

[0097] The positioning result verification and feedback module is responsible for multi-dimensional verification of the final pose output by the fusion filtering module. It combines the motion consistency of the IMU, the rationality of the wheel speedometer's mileage estimation, and preset road constraints (such as lane line boundaries and road curvature) to determine whether the positioning result is reliable. If the verification finds an anomaly, it will feed back to the state management module to trigger state adjustment (such as switching from CONVERGED to RECOVERING), and at the same time output correction coefficients to the fusion filtering module to optimize the fusion weight allocation. Synergistic effect: It forms a closed-loop verification with the fusion filtering module, and works in conjunction with the state management module to improve the accuracy of state switching, further enhancing the accuracy and reliability of the positioning result, which is in line with the core technical goal of "high precision and strong robustness" of the solution.

[0098] It should be noted that the positioning result verification and feedback module, as the "last line of defense" for the system's positioning accuracy, uses a multi-dimensional, multi-data source cross-verification mechanism to comprehensively verify the final pose output by the fusion filtering module. At the same time, it constructs a closed-loop feedback link to dynamically optimize the fusion strategy and system state, ensuring that the positioning results meet both accuracy requirements and scenario adaptability. It is a key supporting unit for strengthening the system's core objective of "high precision and strong robustness". The core implementation details, verification logic, and collaboration mechanism of the location result verification and feedback module are as follows: Multi-dimensional verification system design: A layered verification model is constructed around three core dimensions: "motion consistency, mileage rationality, and road constraint adaptation." The verification logic and judgment criteria for each dimension are as follows: (1) IMU motion consistency verification Core principle: Based on the three-axis angular velocity and linear acceleration output by the IMU, the "IMU-estimated pose" is calculated by numerical integration. The pose is then compared with the final pose of the fusion filtering module to verify the continuity of the motion state.

[0099] Verification metrics and thresholds: Inter-frame pose change difference ≤0.5° / frame, inter-frame position change difference ≤0.3m / frame; if the threshold is exceeded for 3 consecutive frames, it is judged as "inconsistent motion", indicating that there may be a jump in the fused pose.

[0100] Special scenario adaptation: When the vehicle accelerates, brakes or turns suddenly, the threshold is automatically relaxed by 20% (e.g., Δyaw≤0.6° / frame) to avoid misjudgment caused by normal violent movement.

[0101] (2) Verification of the rationality of wheel speed meter mileage estimation Core logic: Based on the longitudinal speed and steering wheel angle data from the wheel speed meter, the theoretical driving distance and heading change of the vehicle are calculated through the odometer model, and then quantitatively compared with the actual driving trajectory of the fused pose.

[0102] Verification indicators and thresholds: Cumulative mileage deviation rate (|estimated mileage - fused trajectory mileage| / fused trajectory mileage) ≤ 5% (within 100m driving distance), heading change deviation ≤ 1° / 100m; if the deviation rate continuously exceeds 5% and continues for 20m driving distance, it is judged as "unreasonable mileage".

[0103] Auxiliary verification: Combine the vehicle's CAN bus shift signals (such as high speed and low speed) to dynamically adjust the mileage deviation threshold (tighten the high speed threshold to 3% and loosen the low speed threshold to 8%) to adapt to different driving conditions.

[0104] (3) Preset road constraint verification The core basis is to load road constraint information (including lane boundary coordinates, road curvature range, slope restrictions, prohibited areas, etc.) from high-precision electronic maps and verify whether the fused pose conforms to the actual physical rules of the road.

[0105] Verification content and judgment criteria: Lane boundary constraints: The x / y coordinates of the fused pose must fall within the current lane boundary (left and right deviation ≤ 0.5m); if they exceed the boundary, they will be marked as "lane deviation anomaly". Road curvature constraint: The curvature of the driving trajectory calculated by fusion pose must deviate from the preset road curvature on the map by ≤0.01rad / m. If the deviation is too large, a "trajectory curvature abnormality" message will be displayed (such as a sharp curve trajectory on a straight road). Slope and area constraints: The road slope corresponding to the z-coordinate of the fused pose must deviate from the slope information on the map by ≤3°; at the same time, the pose must not fall in the prohibited areas marked on the map (such as green belts and sidewalks), otherwise it will be directly judged as a serious anomaly.

[0106] Verification result grading and decision-making logic: A "three-level verification" mechanism is adopted, which outputs corresponding decisions based on the severity of the anomaly, avoiding misjudgments from a single dimension that could affect system stability. Level 1 verification (normal): All three dimensions of verification pass, or only a single dimension shows a slight deviation (and does not exceed 80% of the upper limit of the threshold). The positioning result is determined to be reliable, no feedback adjustment is triggered, and only the verification data is recorded for subsequent statistical analysis.

[0107] Level 2 verification (minor anomaly): If a single dimension exceeds the threshold but does not meet the "continuous anomaly" condition (e.g., a single mileage deviation rate of 6%), or if two dimensions show slight deviations, it is judged as a minor anomaly. At this time, a "weak correction coefficient" is output to the fusion filtering module (e.g., the NDT fusion weight is reduced by 0.1 to 0.2), while continuously monitoring the subsequent 3 frames of data. If the anomaly is alleviated, it returns to normal; if it worsens, it is upgraded to Level 3 anomaly.

[0108] Level 3 Verification (Severe Anomaly): A severe anomaly is defined as the occurrence of consecutive anomalies in any dimension (e.g., inconsistent IMU motion for 3 consecutive frames), two or more dimensions exceeding the threshold simultaneously, or triggering a "serious violation" in road constraints (e.g., entering a prohibited area). A trigger signal is immediately sent to the state management module, pushing the system to switch from the current state (e.g., CONVERGED) to RECOVERING. Simultaneously, a "strong correction coefficient" is output to the fusion filtering module, forcibly reducing the NDT fusion weight to below 0.2, prioritizing stable data from the odometer and IMU to prevent the anomaly from escalating.

[0109] Closed-loop feedback mechanism and execution details: (1) Feedback optimization to the fusion filtering module Correction coefficient generation: Based on the three-level verification results, dynamically generate the fusion weight adjustment coefficient and covariance correction parameters. In case of minor anomalies, output the "weight fine-tuning coefficient", such as reducing the NDT fusion weight. =0.1~0.2, while increasing the process noise Q value of the fused covariance matrix by 20%, thereby improving the system's fault tolerance to anomalies; In case of severe anomalies, output the "weighting emphasis coefficient". The value is forcibly set to 0.05–0.2, and the Kalman gain in the covariance update formula is adjusted accordingly. (Multiplied by an attenuation factor of 0.8) to reduce the impact of observational data on the fusion results; After the anomaly is recovered, the "weight recovery coefficient" is output, and the NDT fusion weight is gradually restored in an increment of 0.05 to 0.1 per frame to avoid positioning fluctuations caused by sudden weight changes.

[0110] (2) Feedback linkage to the status management module State transition triggering: Clearly define the mapping relationship between abnormal conditions and state transitions to ensure accurate state adjustments. Switching from CONVERGED to RECOVERING: The trigger condition is a Level 3 severe anomaly, or a Level 2 minor anomaly that persists for 5 frames without being alleviated; Switching from RECOVERING to ODOM_ONLY: The trigger condition is that in RECOVERING state, a level 3 serious anomaly lasts for 20m of driving distance, or the road constraint check shows "serious violation" continuously; Switching back from RECOVERING to CONVERGED: The trigger condition is that all 8 consecutive frames of verification are at level 1 and the pose covariance matrix trace tr of the fusion filtering module is normal. ≤0.03m² (high positioning reliability).

[0111] Status feedback supplement: Synchronize the specific dimensions, deviation values ​​and duration of the anomaly to the status management module to provide a reference for the status recovery strategy (e.g., for anomalies caused by unreasonable mileage, priority should be given to strengthening the odometer data verification during status recovery).

[0112] Synergistic effect and system value: Synergize with the fusion filtering module: Construct a closed-loop link of "fusion output → multi-dimensional verification → feedback correction → optimized fusion" to solve the "local optimum" problem that may exist in the fusion filtering module (such as pose deviation caused by NDT abnormal observations not being completely filtered). Continuously optimize the fusion weight and covariance management through dynamic correction coefficients to ensure that the fusion result is both accurate and conforms to the rules of the actual scenario.

[0113] In conjunction with the status management module: it provides a more comprehensive basis for status judgment than data from a single sensor, avoiding the status management module from erroneously switching status due to anomalies in a single data source (such as NDT single-match failure), thus improving the accuracy and stability of status switching; at the same time, during the status recovery phase, it guides the recovery strategy through verification results, shortening the recovery time.

[0114] Linked with the map dynamic update module: The "difference between map and actual environment" data (such as lane line boundary deviation exceeding 0.5m) in the road constraint verification is fed back to the map dynamic update module to help it determine whether there are changes in road facilities (such as lane line replanning), providing additional feature basis for incremental map updates and further enhancing the system's adaptability to dynamic environments.

[0115] Core system value: Through a full-process mechanism of "pre-verification, in-process feedback, and post-optimization", the abnormality rate of positioning results is reduced by more than 40%. Without affecting the real-time performance of the system (verification time ≤ 5ms / frame), the stability of positioning accuracy is further improved (centimeter-level accuracy retention rate is increased from 92% to 97%), ensuring that the system continuously outputs reliable positioning results in complex urban environments.

[0116] State Management Module: Responsible for system initialization, state switching, exception handling and recovery.

[0117] Specifically, the state management module maintains multiple system states, including UNINITIALIZED, INITIALIZED, CONVERGED, ODOM_ONLY, and RECOVERING. It automatically switches states based on sensor data quality and environmental conditions. When NDT continuous matching fails or an abnormal observation occurs, it automatically switches to pure odometer mode and attempts to recover. It also supports receiving external initial pose input via the / initialpose topic.

[0118] The system supports the following states and automatically switches between them based on sensor data quality and environmental conditions: UNINITIALIZED: Uninitialized state, awaiting initial pose provided via the / initialpose topic.

[0119] INITIALIZED: Initialized state, multi-sensor fusion positioning has begun.

[0120] CONVERGED: The positioning has converged, NDT matching is good, and the output pose has high accuracy and high reliability.

[0121] ODOM_ONLY: NDT failure state, relying solely on odometry for pose estimation to ensure basic system functions.

[0122] RECOVERING: Recovering status, attempting to reinitialize or waiting for NDT matching to recover.

[0123] State transitions are triggered by conditions such as NDT matching score, convergence state, Mahalanobis distance, and number of consecutive failures, and the current state is published externally through the / localization_state topic.

[0124] It should be noted that the state management module, as the "central scheduling unit" of the system operation, is responsible for the state control of the entire system lifecycle. Its core maintenance includes five states: UNINITIALIZED, INITIALIZED, CONVERGED, ODOM_ONLY, and RECOVERING. It clearly defines the triggering conditions, maintenance logic, and switching rules for each state. The UNINITIALIZED state requires receiving an external initial pose (pose error ≤ 0.5m, attitude error ≤ 1°) via the / initialpose topic before switching to the INITIALIZED state to initiate multi-sensor fusion positioning. In the INITIALIZED state, if the NDT matching score is ≤ 80 for 5 consecutive frames, the convergence state is True, the Mahalanobis distance is < 2.0, and the positioning result verification and feedback module shows no abnormalities, then it switches to the CONVERGED state and outputs a high-precision pose. When the number of consecutive NDT matching failures is ≥ 3 frames (or the matching score is > 120, the convergence state is Fals), the state remains unchanged. If the Mahalanobis distance is ≥5.0, the system automatically switches to the ODOM_ONLY state, relying solely on the IMU and wheel velocity sensor to maintain basic positioning while simultaneously initiating environmental feature monitoring. If a serious anomaly occurs in the positioning result verification under the ODOM_ONLY state, or if NDT rematching fails after ≥10 frames, the system switches to the RECOVERING state. It attempts to recover by loading surrounding map fragments and optimizing the initial matching pose. When three consecutive NDT matches meet the standard and verification is error-free, the system can switch back to the CONVERGED state. All state transitions are linked to sensor data quality, the covariance data of the fusion filtering module, and the verification results of the positioning result verification and feedback module, ensuring precise and controllable switching. The current state and the reason for the state transition (e.g., "NDT matching failed continuously, switching from CONVERGED to ODOM_ONLY") are published in real-time via the / localization_state topic, providing visual support for system integration and debugging.

[0125] Example 2: This invention also discloses a multi-sensor fusion positioning method for an intelligent positioning system, comprising the following steps: S1 data will be synchronized: Approximate time synchronization between IMU and CAN data is achieved through ROS message_filters:Synchronizer, with a maximum interval tolerance of 0.01 seconds to ensure time consistency during joint data processing.

[0126] It should be noted that data time synchronization is achieved by using the ROS message_filters:Synchronizer component (using the ApproximateTime strategy) to achieve approximate time synchronization between IMU high-frequency data (100Hz) and CAN bus wheel speed / turn angle data (10Hz). The maximum time interval tolerance is precisely set to 0.01 seconds. At the same time, linear interpolation compensation is performed on the IMU data at asynchronous times to ensure strict alignment of timestamps when multiple data sources are processed together.

[0127] After synchronization is complete, all data are uniformly stamped with a high-precision system UTC timestamp and written into the ROS message header field. This not only ensures the timing consistency of the calculation by the fusion filtering module, but also provides a traceable time reference for subsequent location result verification and anomaly tracing.

[0128] S2 motion prediction: Based on a three-dimensional motion model, the attitude is updated through quaternion integration, the position prediction value is calculated by combining wheel speed sensor data, the attitude is updated using IMU angular velocity integration, and the wheel speed sensor provides longitudinal velocity to predict the vehicle's position and attitude at the next moment.

[0129] Specifically: ① Attitude prediction: Attitude update is performed using quaternion integration.

[0130] in Represented as an angular increment vector, Angular velocity vector after IMU correction (rad / s) : Time interval (s) The attitude quaternion at the current moment. The attitude quaternion predicted for the next time step. Represented as a quaternion multiplication operator, For the angle increment Constructed quaternions.

[0131] ②Location prediction:

[0132] in, This indicates the longitudinal speed (m / s) provided by the wheel speed gauge. Velocity vector in map coordinate system : The rotation matrix corresponding to the current posture (from the vehicle coordinate system to the map coordinate system). : Current position vector , : The predicted position vector at the next moment.

[0133] It should be noted that the motion prediction is based on a 3D motion model that incorporates slope adaptation. First, the angular velocity integral is corrected by an IMU (to remove zero bias and high-frequency noise), then combined with the quaternion integral formula. Update the attitude, and then use the longitudinal velocity provided by the wheel speedometer to... , Calculate the predicted position value to accurately predict the vehicle's position and orientation at the next moment.

[0134] Wherein, rotation matrix From the current attitude quaternion The transformation ensures accurate mapping of the velocity vectors between the vehicle coordinate system and the map coordinate system, and By using the actual time interval after data synchronization (adaptively matched with the sensor sampling frequency), the accuracy of motion prediction is further improved.

[0135] NDT fusion: NDT fusion judges the credibility of the observation by Mahalanobis distance, and uses weighted fusion and spherical linear interpolation to achieve pose fusion. When the NDT output is credible (Mahalanobis distance is less than the threshold, good convergence, low score), it is fused with the predicted pose by weighted fusion.

[0136] Specifically, ① Mahalanobis distance detection: determining whether the difference between the observed and predicted NDT values ​​is within the allowable range:

[0137] like If the value is less than the threshold, then fusion is performed; otherwise, the observation is rejected.

[0138] in, Represented as Mahalanobis distance value, : NDT observation pose vector : Observation matrix (maps the state vector to the observation space). State prediction Values ​​(including position and orientation). Predict the covariance matrix. : Matrix transpose operator; Matrix inversion operator.

[0139] ②Position fusion:

[0140] in This is represented as the merged position vector. : NDT observation location vector Predict the location vector. : Fusion ratio parameter The posture after fusion NDT observation attitude, Predicting attitude, : Spherical linear interpolation function, used for quaternion fusion.

[0141] It should be noted that NDT fusion: firstly, it uses the Mahalanobis distance formula. The deviation between the observed pose and the predicted pose in the NDT is calculated, with a preset Mahalanobis distance threshold of 3.0 (based on the 99.7% confidence interval of the χ² distribution). The reliability of the observed values ​​is determined by a dual condition: an NDT matching score ≤ 100 and a convergence state of True. When the value is deemed reliable, a weighted fusion formula is used to complete the position fusion, employing a spherical linear interpolation function. Achieving pose fusion, where the fusion ratio is... Dynamically adjust based on NDT matching probability (0≤ ≤1, the higher the probability The closer to 1), the better; otherwise, the NDT observation is rejected outright.

[0142] S4. Covariance Update: Update the pose covariance matrix based on the motion model and NDT observations, and dynamically adjust the fusion weights.

[0143] Specifically, ① Prediction step:

[0144] in It is represented as the prior covariance matrix at time k (prediction uncertainty). The posterior covariance matrix at time k-1. : The state transition Jacobian matrix (representing how the state changes over time). Process noise covariance matrix (representing the uncertainty of the prediction process).

[0145] ② Update step (when NDT is valid):

[0146]

[0147] in It is represented as the Kalman gain matrix (which determines the degree to which one trusts the prediction or the observation). Observation matrix : NDT observation noise covariance matrix (representing the uncertainty of NDT observations) Identity matrix : The posterior covariance matrix at time k (updated uncertainty).

[0148] It should be noted that covariance update: first, through the prediction step formula...

[0149] (in Dynamically optimize based on vehicle gradient and driving status. Calculate the prior covariance matrix at time k based on the IMU noise statistical characteristics (adjusted in real time); when the NDT observation is valid, use the Kalman gain formula. ( (Negatively correlated with NDT matching scores, with lower scores resulting in smaller R-values) and the posterior covariance formula. After completing the update, the fusion weights are dynamically adjusted based on the trace of the posterior covariance matrix. This enables the coordinated optimization of uncertainty and integration strategies.

[0150] S5. Zero bias and drift correction: When the vehicle is stationary or at low speed, the IMU angular velocity data is collected to estimate its zero bias. When NDT is effective, the attitude error is calculated and the zero bias is updated.

[0151] Specifically, ① Zero bias estimation: When the vehicle is stationary or at low speed, the IMU angular velocity data is collected and its zero bias is estimated.

[0152] in This is represented as the estimated gyroscope zero bias vector. Number of samples : No. Each angular velocity sample value.

[0153] ② Drift correction: When NDT is active, calculate the attitude error and update the zero bias:

[0154] in This is expressed as the difference between the observed yaw angle and the predicted yaw angle under NDT. : Yaw angle observed by NDT Predicted yaw angle : Estimated yaw rate : Z-axis gyroscope zero bias (affects yaw angle measurement) Drift correction gain parameters ( ), Assignment operator.

[0155] It should be noted that zero bias and drift correction: when the vehicle is stationary (wheel speedometer speed ≤ 0.1 m / s) or traveling at low speed (0.1 m / s < speed ≤ 1 m / s) and the data is stable for 5 consecutive frames, the zero bias and drift correction are achieved using the formula... (N is set to 500 sets of sampled data, sampling period 0.01s) Estimate the gyroscope zero bias vector by statistically analyzing IMU angular velocity data; when NDT observations are valid, first... ,

[0156] Calculate the yaw angle difference and drift rate, then... Update the z-axis gyroscope zero bias ( The value is set to 0.05 to 0.1 and is dynamically adjusted according to the NDT matching probability to continuously suppress IMU cumulative drift.

[0157] The contents not described in detail in this specification are existing technologies known to those skilled in the art.

[0158] Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. An intelligent positioning system based on the fusion of NDT and odometer, characterized in that: The intelligent positioning system includes a sensor module, a data preprocessing module, an NDT matching module, a fusion filtering module, and a status management module. Sensor module: Used to collect the required data information; Data preprocessing module: realizes time synchronization, coordinate transformation, and filtering of sensor data; NDT matching module: provides local localization results based on point cloud matching; Fusion filtering module: It adopts a covariance-based Kalman filtering framework to fuse NDT and odometer data; State Management Module: Responsible for system initialization, state switching, exception handling and recovery.

2. The intelligent positioning system based on the fusion of NDT and odometer as described in claim 1, characterized in that: The sensor module includes an IMU, a wheel speedometer, and a lidar; IMU: Provides three-axis angular velocity and linear acceleration information for high-frequency attitude prediction and motion compensation; Wheel speed meter: Acquires longitudinal speed information via the vehicle's CAN bus to assist in odometer calculation; LiDAR: Provides 3D point cloud data of the surrounding environment and is the basic input for the NDT matching algorithm.

3. The intelligent positioning system based on the fusion of NDT and odometer according to claim 1, characterized in that: Time synchronization in the data preprocessing module: Based on the ROS message_filters mechanism, approximate time synchronization is performed on IMU and CAN data to ensure data timestamp consistency; Coordinate transformation: The tf2 library is used to uniformly transform the data from various sensors to the vehicle coordinate system and the global map coordinate system; Filtering: Low-pass filtering is applied to the raw IMU data; first-order low-pass filtering is applied to the steering wheel angle signal to eliminate high-frequency noise.

4. The intelligent positioning system based on the fusion of NDT and odometer according to claim 1, characterized in that: The NDT matching module uses a normal distribution transformation algorithm to match the current frame point cloud with the preloaded point cloud map, outputs the 6-DOF pose of the vehicle in the current map, and provides matching quality evaluation indicators including matching score, convergence state, number of iterations and transformation probability. It supports multiple NDT implementation versions and can be flexibly selected according to computing power.

5. The intelligent positioning system based on the fusion of NDT and odometer according to claim 1, characterized in that: The core functions of the fusion filtering module are: fusing the NDT observed pose with the predicted pose based on the motion model, performing anomaly detection on the NDT observations based on Mahalanobis distance, dynamically adjusting the fusion weights, predicting and updating the pose covariance matrix in real time, and characterizing the uncertainty of the localization results.

6. The intelligent positioning system based on the fusion of NDT and odometer according to claim 1, characterized in that: The state management module maintains multiple system states, including UNINITIALIZED, INITIALIZED, CONVERGED, ODOM_ONLY, and RECOVERING. It automatically switches states based on sensor data quality and environmental conditions. When NDT matching fails continuously or an abnormal observation occurs, it automatically switches to pure odometer mode and attempts to recover. It also supports receiving external initial pose input via the / initialpose topic.

7. The intelligent positioning system based on the fusion of NDT and odometer according to claim 1, characterized in that: A map dynamic update module is provided between the NDT matching module and the fusion filtering module. The map dynamic update module is used to detect the difference between the preloaded point cloud map and the current environment in real time, and dynamically update the map data. By receiving the point cloud matching difference information output by the NDT matching module and the pose covariance data of the fusion filtering module, it determines whether the environmental change is permanent and performs incremental updates to the map. At the same time, the updated map is fed back to the NDT matching module to ensure the accuracy of subsequent matching.

8. The intelligent positioning system based on the fusion of NDT and odometer according to claim 1, characterized in that: A positioning result verification and feedback module is set between the fusion filtering module and the state management module. It is responsible for multi-dimensional verification of the final pose output by the fusion filtering module. It combines the motion consistency of the IMU, the rationality of the mileage estimation of the wheel speed meter, and the preset road constraints to determine whether the positioning result is reliable. If an anomaly is detected during the verification, feedback will be sent to the status management module to trigger status adjustment, and at the same time, a correction coefficient will be output to the fusion filtering module to optimize the fusion weight allocation.

9. The intelligent positioning system based on the fusion of NDT and odometer according to claim 1, characterized in that: The multi-sensor fusion positioning method of the intelligent positioning system includes the following steps: S1. Data synchronization: Approximate time synchronization between IMU and CAN data is achieved through ROS message_filters:Synchronizer, with a maximum interval tolerance of 0.01 seconds to ensure time consistency during joint data processing. S2. Motion Prediction: Based on the three-dimensional motion model, the attitude is updated by quaternion integration, the position prediction value is calculated by combining wheel speed sensor data, the attitude is updated by IMU angular velocity integration, the wheel speed sensor provides longitudinal velocity, and the vehicle's position and attitude are predicted at the next moment. S3, NDT Fusion: NDT fusion uses Mahalanobis distance to determine the reliability of the observations and employs weighted fusion and spherical linear interpolation to achieve pose fusion. When the NDT output is reliable, it is weighted and fused with the predicted pose. S4, Covariance Update: Update the pose covariance matrix based on the motion model and NDT observations, and dynamically adjust the fusion weights; S5. Zero bias and drift correction: When the vehicle is stationary or at low speed, the IMU angular velocity data is collected to estimate its zero bias. When NDT is effective, the attitude error is calculated and the zero bias is updated.

Images