A vehicle positioning method based on multi-source data fusion

Abstract

This application discloses a vehicle positioning method based on multi-source data fusion, which relates to the field of underground mining vehicle positioning. The method comprises the following steps: collecting multi-source data; constructing a point cloud map and generating a point cloud feature map; preprocessing the real-time collected LiDAR point cloud data and extracting real-time point cloud features from the preprocessed LiDAR point cloud data using a PointNet network; establishing a posture transformation matrix T of the vehicle at the current moment based on IMU data and GNSS data, as well as the vehicle posture data at the previous moment; calculating the initial position #imgabs0# of the real-time point cloud features in the point cloud feature map using matrix T; and extracting a local point cloud feature map centered at the initial position #imgabs1#; performing feature matching on the real-time point cloud features and the local point cloud feature map to obtain matching point pairs; and obtaining the vehicle posture data of the vehicle at the current moment based on the matching point pairs and matrix T. In view of the low positioning accuracy caused by the difficulty of multi-source data fusion in underground mining environments in the prior art, this application improves positioning accuracy.

Description

Technical Field

[0001] The present application relates to the field of underground mining vehicle positioning, and in particular to a vehicle positioning method based on multi-source data fusion. Background Art

[0002] With the accelerated advancement of intelligent mining, autonomous driving technology in underground mining environments is rapidly developing, becoming a key means of improving mine production efficiency and safety. Accurate positioning is the foundational infrastructure for the safe and efficient operation of autonomous vehicles, directly impacting the reliability and stability of the entire system. While positioning technology is relatively mature in open surface environments, high-precision positioning in specialized environments like underground mining still faces numerous challenges.

[0003] On the one hand, due to the confined tunnel structures deep underground, complex metal equipment, and strong electromagnetic interference, satellite signals cannot penetrate the strata, resulting in the inability of GNSS-based positioning systems to function properly. Furthermore, the dim lighting, pervasive dust, high tunnel similarity, and dynamic environmental changes in underground mining environments severely impact LiDAR feature extraction and matching. In particular, the high similarity of tunnels makes it difficult to distinguish extracted point cloud features, leading to positioning ambiguity and reduced positioning accuracy. Furthermore, strong magnetic field interference, frequent vehicle vibration, and drastic temperature and humidity fluctuations in underground mining environments lead to rapid accumulation of errors in the IMU integration process, making it difficult for pure inertial navigation to maintain accurate positioning over long periods of time. Summary of the Invention

[0004] In response to the problem of low positioning accuracy caused by the difficulty of multi-source data fusion in underground mining environments in the existing technology, this application provides a vehicle positioning method based on multi-source data fusion, which improves the positioning accuracy in underground mining environments through time synchronization and coordinate unification.

[0005] The purpose of this application is achieved through the following technical solutions.

[0006] The present application provides a vehicle positioning method based on multi-source data fusion, including: S1, collecting multi-source data through vehicle-side sensors and roadside sensors respectively, and the multi-source data includes lidar point cloud data, image data, IMU data and GNSS data; S2, preprocessing the collected multi-source data; S3, constructing a point cloud map based on the preprocessed lidar point cloud data; S4, generating a point cloud feature map based on the point cloud map; S5, preprocessing the lidar point cloud data collected in real time, and using the PointNet network to extract the real-time point cloud features of the preprocessed lidar point cloud data; S6, establishing the vehicle's current posture transformation matrix T based on the IMU data and GNSS data, as well as the vehicle posture data at the previous moment; S7, using the posture transformation matrix T to calculate the initial position of the real-time point cloud features in the point cloud feature map ; and extract the initial position A local point cloud feature map centered on the real-time point cloud feature and the local point cloud feature map is used; S8, feature matching is performed on the real-time point cloud feature and the local point cloud feature map to obtain matching point pairs; S9, according to the matching point pairs and the posture transformation matrix T, the vehicle posture data of the vehicle at the current moment is obtained, and the vehicle posture data includes position and posture.

[0007] Among them, IMU data refers to the motion state information collected by the vehicle-mounted inertial measurement unit, mainly including three-axis acceleration and three-axis angular velocity. This application calculates the displacement and posture changes of the vehicle in a short period of time through integration to construct the prediction part of the posture transformation matrix; GNSS data refers to the position information obtained by the satellite positioning system (GPS, Beidou, GLONASS, etc.) receiver. This application uses GNSS data to provide a global position reference, construct the observation part of the posture transformation matrix, and correct the drift caused by IMU integration; the posture transformation matrix T is a 4×4 homogeneous transformation matrix that describes the rigid body transformation relationship from the vehicle coordinate system to the map coordinate system. , where R is a 3×3 rotation matrix (representing the vehicle's posture) and t is a 3×1 translation vector (representing the vehicle's position).

[0008] The local point cloud feature map is a feature representation within a specific radius R extracted from the global point cloud feature map with the predicted vehicle position as the center. In this application, S7 is extracted from the global feature map to reduce the matching search space and improve computational efficiency. In S8, it is used as a matching target to compare with the real-time point cloud features to find the corresponding relationship and establish the connection between the vehicle perception data and the pre-built map, which is the basis for feature matching and positioning. In S6, as the output of the prediction step, the IMU and GNSS data are integrated to obtain a preliminary estimate of the current posture of the vehicle; in S7, it is used to transform the real-time point cloud features from the vehicle coordinate system to the map coordinate system to determine the initial matching position; in S9, it is used as the initial value for posture optimization and is further refined by minimizing the distance between matching point pairs.

[0009] Furthermore, S2 preprocesses the collected multi-source data, including: time synchronization of the multi-source data collected by vehicle-side sensors and roadside sensors; conversion of the time-synchronized multi-source data to the same coordinate system; and filtering of the multi-source data in the same coordinate system to obtain preprocessed multi-source data. Fusion of vehicle-side and roadside sensor data, through time synchronization to resolve data asynchrony, and conversion of data from different coordinate systems to a unified coordinate system, effectively addresses the low positioning accuracy caused by the susceptibility of single sensors to interference in underground mining environments.

[0010] Furthermore, S3 constructs a point cloud map based on the pre-processed LiDAR point cloud data, including: constructing an objective function based on the pre-processed LiDAR point cloud data collected by the vehicle-side sensor , ,in, is a point in the target point cloud, Is the source point cloud The corresponding nearest point, N is the number of point pairs, R is the rotation matrix, and t is the translation vector; Represents the Euclidean distance; by minimizing the objective function , obtain the pose transformation relationship between adjacent frame point clouds and construct the initial point cloud map; the pre-processed lidar point cloud data collected by the vehicle-side sensor and the pre-processed lidar point cloud data collected by the roadside sensor are aligning and fused to obtain fused point cloud data; using the fused point cloud data, the initial point cloud map is updated through the extended Kalman filter algorithm to obtain the final point cloud map.

[0011] Furthermore, S4 generates a point cloud feature map based on the point cloud map, including: performing a nonlinear coordinate system transformation on the point cloud map to obtain aligned point cloud data; using a multi-layer convolutional neural network to extract features based on the aligned point cloud data to obtain local feature vectors; using maximum pooling and statistical feature pooling to perform weighted fusion on the local feature vectors to obtain feature vectors with enhanced discriminability; and generating a point cloud feature map through feature mapping based on the feature vectors with enhanced discriminability.

[0012] Among them, nonlinear coordinate transformation refers to the mathematical operation of converting point cloud data from the original Cartesian coordinate system (x, y, z) to another nonlinear representation. The mapping relationship of this transformation is not a simple linear function. Preferably, in this application, the rectangular coordinates (x, y, z) are converted to spherical coordinates. , where r represents radial distance, and θ and φ represent azimuth and elevation angles. Specifically, the nonlinear transformation compensates for the natural sparsity of the point cloud at long distances, maintaining feature consistency through density adjustment. This also enhances invariance to rotation and orientation changes, improving feature stability across different viewpoints. Compared to directly using Cartesian coordinates, this method better preserves the local structural information of the point cloud and is particularly suitable for processing scenes with distinct directional and structural features, such as tunnels and roadways in complex underground mining environments.

[0013] Enhanced discriminative feature vector representations are high-dimensional vector representations optimized through specific multi-level feature extraction and fusion strategies. These representations possess greater discriminative power and robustness, enabling more effective identification and matching of diverse environmental scenes. These representations include: Raw features: fundamental geometric and appearance features extracted directly from point clouds; Max-pooling features: retaining the most significant feature responses within a local region, typically a 256-dimensional vector; and Statistical features: statistics describing the distribution of features within a local region, such as mean, variance, and entropy, typically a 512-dimensional vector. This application provides a highly discriminative scene representation, making it easier to distinguish features at different locations. This reduces the impact of environmental changes (such as illumination and occlusion) on feature extraction, improving matching stability.

[0014] Further, S6, based on the IMU data and GNSS data, as well as the vehicle posture data of the previous moment, establishes the posture transformation matrix T of the vehicle at the current moment, including: obtaining the posture change and displacement change of the vehicle through piecewise integration based on the IMU data and the vehicle posture data of the previous moment, and adjusting the integration time window according to the fluctuation amplitude of the IMU data by piecewise integration; constructing the posture transformation matrix predicted by the IMU based on the piecewise integration results : ; ;in, Represents the vehicle posture transformation matrix at the previous moment; Represents the basic pose transformation matrix; Represents the rotation increment matrix at the current moment, through the IMU angular velocity In the adaptive time window The inner integral is calculated as: , exp represents exponential mapping; Indicates the displacement increment vector at the current moment, through the IMU acceleration The quadratic integral calculation after gravity compensation and coordinate system conversion yields: ,in is the velocity at the previous moment, g is the gravitational acceleration vector; Represents a time-based drift compensation model, which is used to correct the cumulative error generated during the IMU integration process; based on the GNSS data, the vehicle's position in the global coordinate system is obtained, and the position transformation matrix of the GNSS observation is constructed by converting the position into the coordinate system of the point cloud map. ; Pose transformation matrix and the pose transformation matrix Perform weighted fusion to obtain the vehicle's current posture transformation matrix T.

[0015] First, underground mine tunnels are generally highly similar, and the features extracted by traditional PointNet may not be able to effectively distinguish these similar environments, leading to positioning ambiguity. Addressing IMU integration drift also makes it impossible to ensure long-term positioning stability. Furthermore, IMU integration is prone to error accumulation, and traditional fixed-time window integration cannot adapt to changes in the vehicle's motion state.

[0016] This application uses piecewise integration. Specifically, the fluctuation amplitude is evaluated first to calculate the angular velocity fluctuation amplitude: ;in, : The three-axis angular velocity components measured by the IMU at the current moment, in rad / s; : The average value of the three-axis angular velocity within the sliding window (usually 0.5 seconds), in rad / s; The standard deviation of the angular velocity is expressed in rad / s, which indicates the amplitude of the angular velocity fluctuation. Preferably, the time window of the segmented integration is adjusted to meet the following conditions: when the dynamic coefficient D>0.7, the window is reduced to 5ms, and when D<0.3, the window is expanded to 20ms.

[0017] Calculate the acceleration fluctuation amplitude: ,in, : The three-axis acceleration components measured by the IMU at the current moment, in m / s²; : The average value of the three-axis acceleration within the sliding window (usually 0.5 seconds), in m / s²; : Standard deviation of acceleration, indicating the amplitude of acceleration fluctuation, in m / s².

[0018] Calculate the rate of change of acceleration and angular velocity (jerk and angular acceleration): ; . : The time interval for calculating the rate of change, usually the time difference between two IMU samples, in seconds (s); j: The rate of change of acceleration, also known as jerk, which indicates how fast the acceleration changes over time, in m / s³; α: The rate of change of angular velocity, also known as angular acceleration, which indicates how fast the angular velocity changes over time, in rad / s².

[0019] Calculate the dynamic coefficient, , where w is the weight coefficient of each component, usually , , , ; : Reference value of angular velocity fluctuation amplitude, typical value in static state obtained according to system calibration, unit is rad / s; : The reference value of the acceleration fluctuation amplitude, the typical value in the static state obtained according to the system calibration, the unit is m / s²; : Reference value of jerk, indicating the typical jerk value under normal driving conditions, in m / s³; : The reference value of angular acceleration, which represents the typical angular acceleration value under normal driving conditions, in rad / s²; : The weight coefficient of each component in the calculation of the dynamic coefficient, dimensionless; D: Dynamic coefficient, which represents the dynamic degree of the vehicle's motion state, dimensionless.

[0020] Furthermore, the time-based drift compensation model for: ; Where I is the unit matrix, K is the drift coefficient matrix, t is the cumulative integration time, and K is obtained by dynamic learning of the posture differences of similar scenes in the historical trajectory.

[0021] Furthermore, S7 uses the pose transformation matrix T to calculate the initial position of the real-time point cloud feature in the point cloud feature map. ; and extract the initial position The local point cloud feature map centered on the vehicle includes: using the pose transformation matrix T to transform the real-time point cloud features from the vehicle coordinate system to the point cloud feature map coordinate system, and obtaining the initial position of the real-time point cloud features in the point cloud feature map. ; Initial position As the center, set the search radius R, extract the local area data from the point cloud feature map, and construct the candidate matching area; pre-process the candidate matching area; and construct the local point cloud feature map based on the pre-processed candidate matching area. The search radius R is set as the center to extract local area data to avoid global search; the pose is solved iteratively and the convergence condition is set. When the pose change is less than the threshold or the maximum number of iterations is reached, the algorithm stops and optimizes the use of computing resources.

[0022] Furthermore, S8 performs feature matching on the real-time point cloud features and the local point cloud feature map to obtain matching point pairs, including: constructing a high-dimensional feature space based on the real-time point cloud features and the local point cloud feature map; calculating the feature distribution density of the local point cloud feature map, and dividing the high-dimensional feature space into high-density areas, medium-density areas, and low-density areas according to the feature distribution density; setting the maximum leaf node capacity of the high-density area, the medium-density area, and the low-density area respectively. 、 and ;in, ; Based on the maximum leaf node capacity 、 and , construct a KD tree; for each real-time point cloud feature, obtain candidate matching point pairs through nearest neighbor search in the KD tree; perform geometric consistency check on the candidate matching point pairs to obtain the final matching point pairs.

[0023] Further, S9, according to the matching point pairs and the posture transformation matrix T, obtains the vehicle posture data of the vehicle at the current moment, wherein the vehicle posture data includes position and posture, including: constructing a posture solution objective function according to the matching point pairs; using the posture transformation matrix T as the initial value, iteratively solving the posture solution objective function to obtain the optimal rotation matrix R and translation vector T, and stopping the iteration when the posture change is less than the threshold or the maximum number of iterations is reached; using the Gauss-Newton method to calculate the covariance matrix of the optimal rotation matrix R and translation vector T, and obtaining the uncertainty estimate of the optimal rotation matrix R and translation vector T as the input of the posture uncertainty estimation in step S6 at the next moment; updating the vehicle posture data at the current moment according to the optimal rotation matrix R and translation vector T: converting the optimal rotation matrix R into Euler angle representation , where α represents the yaw angle, β represents the pitch angle, and γ represents the roll angle; the translation vector T is aligned with the point cloud feature map in step S4 to obtain the absolute position coordinates of the vehicle; a time series smoothing algorithm based on Kalman filtering is used to smooth the updated vehicle posture data at the current moment to obtain the smoothed vehicle posture data at the current moment as the vehicle posture data of the vehicle at the current moment.

[0024] Furthermore, the pose transformation matrix and the pose transformation matrix Perform weighted fusion to obtain the vehicle's current posture transformation matrix T, including: constructing an adaptive weight matrix based on the uncertainty estimation of the optimal rotation matrix R and translation vector T and : ; ; Where diag represents the diagonal matrix constructor; to It is the weight coefficient of the IMU data corresponding to the six degrees of freedom, which is inversely proportional to the IMU measurement noise and integration time; to is the weight coefficient of the six degrees of freedom corresponding to the GNSS data, which is proportional to the GNSS positioning accuracy; and satisfies (i=1, 2, 3, 4, 5, 6); based on the adaptive weight matrix and , the pose transformation matrix and the pose transformation matrix Decomposed into rotating parts 、 and pan part 、 ; For the rotating part, and Convert to quaternion representation and , using spherical linear interpolation SLERP for fusion: ; Among them, slerp is the spherical linear interpolation function; w is the fusion weight parameter, which is and The weight coefficient related to the rotation is calculated as follows: ; For the translation part, weighted average is used for fusion: ;in, is a 3×3 diagonal matrix with diagonal elements The weight coefficient corresponding to the translation ; I is the 3×3 unit matrix; the fused quaternion Convert back to rotation matrix , and the fused translation vector Combine to get the final pose transformation matrix T: .

[0025] Among them, SLERP (Spherical Linear interpolation) is a nonlinear interpolation algorithm performed on a four-dimensional unit hypersphere. In this application, SLERP implements the optimal weighted combination of the IMU-estimated rotation (q_IMU) and the GNSS-estimated rotation (q_GNSS). Ordinary linear interpolation will cause the quaternion length to be non-1, while SLERP can strictly keep the interpolation result as a unit quaternion, ensuring continuous and smooth attitude changes in vehicle navigation and preventing mutations or jitters caused by improper interpolation.

[0026] Compared with the existing technology, the advantages of this application are:

[0027] Underground mine tunnels are generally highly similar, and the features extracted by traditional PointNet may not be able to effectively distinguish these similar environments, resulting in positioning ambiguity. Due to the strong magnetic field interference, frequent vehicle vibrations, and drastic changes in temperature and humidity in the underground mining environment, the IMU integral drift problem is caused. The time-based drift compensation model of this application introduces the drift coefficient matrix K, establishes a drift compensation model related to the integration time t, and dynamically learns the K value through the posture differences of similar scenes in the historical trajectory, thereby realizing real-time compensation of IMU integral drift. This compensation mechanism fundamentally solves the error accumulation problem in the traditional IMU integration method, enabling the system to rely on the compensated IMU data to maintain reliable positioning for a certain period of time when the point cloud feature matching fails. Especially in long-distance straight tunnels in underground mines, traditional algorithms are prone to feature matching failure due to the monotonous environment, while this method can significantly extend the effective time of pure IMU navigation.

[0028] In underground working environments, sensor data quality is unstable and error distribution is uneven due to roof reflections, signal obstruction at tunnel corners, and electromagnetic interference from metal equipment. This application constructs an adaptive weight matrix based on uncertainty estimation. The rotation part uses spherical linear interpolation (SLERP) to maintain rotation continuity, and the translation part uses weighted averaging to maintain position accuracy. This strategy can dynamically adjust the fusion weights based on the reliability of different sensors in different environments, so that the system can maintain stable positioning when the signal is interfered with. The advantage of this fusion method is that it can intelligently identify and reduce the impact of abnormal data, especially in the complex tunnel structures of underground mines, where the reliability of different sensors will vary significantly with environmental changes. Traditional fixed-weight fusion methods cannot adapt to such changes. This method achieves complementary advantages of sensors by evaluating the uncertainty of sensor data in real time, greatly improving the environmental adaptability of the system.

[0029] In mine working environments, the noise characteristics of IMU data dynamically change due to vibrations caused by frequent starts and stops, sharp turns, and uneven road surfaces in mine cars. This application analyzes the rate of change of IMU data and dynamically adjusts the size of the integration time window, reducing the window size to improve accuracy in high-dynamic scenarios and expanding the window size to reduce the impact of noise in low-dynamic scenarios. This adaptive time window mechanism overcomes the limitations of traditional fixed-window integration methods. In high-dynamic scenarios, such as mine cars traveling at high speeds or making sharp turns, a small time window can accurately capture motion details; in low-dynamic scenarios, such as vehicles traveling slowly or stationary, a large time window can effectively smooth noise and reduce the impact of random errors. This dynamic balance enables the system to achieve optimal IMU data quality in a variety of complex working conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] The present application will be further described in the form of exemplary embodiments, which will be described in detail with reference to the accompanying drawings. These embodiments are not limiting, and in these embodiments, the same numbers represent the same structures, wherein:

[0031] Figure 1 is an exemplary flow chart of a vehicle positioning method based on multi-source data fusion according to some embodiments of the present application;

[0032] Figure 2 is an exemplary flow chart of another vehicle positioning method based on multi-source data fusion according to some embodiments of the present application;

[0033] Figure 3 This is an exemplary flowchart of map updating according to some embodiments of the present application. DETAILED DESCRIPTION

[0034] The method and system provided in the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0035] Figure 1 and Figure 2 As shown, a vehicle positioning method based on multi-source data fusion includes: collecting multi-source data through vehicle-side sensors and roadside sensors respectively, the multi-source data including lidar point cloud data, IMU data and GNSS data; preprocessing the collected multi-source data; constructing a point cloud map based on the preprocessed lidar point cloud data; generating a point cloud feature map based on the point cloud map; preprocessing the lidar point cloud data collected in real time, and using the PointNet network to extract the real-time point cloud features of the preprocessed lidar point cloud data; establishing the vehicle's current posture transformation matrix T based on the IMU data and GNSS data, as well as the vehicle posture data at the previous moment; and using the posture transformation matrix T to calculate the initial position of the real-time point cloud features in the point cloud feature map. ; and extract the initial position A local point cloud feature map centered on the real-time point cloud feature and the local point cloud feature map is matched to obtain matching point pairs; according to the matching point pairs and the posture transformation matrix T, the vehicle posture data of the vehicle at the current moment is obtained, wherein the vehicle posture data includes position and posture.

[0036] Specifically, in this embodiment, S1 configures vehicle-side sensors. A 360° rotating 16- or 32-line LiDAR is installed on the vehicle's roof. The sampling frequency is set to 10 Hz, with a measurement range of 0.5-100 meters and an angular resolution of 0.1° to 0.4°. This sensor is used to acquire 3D point cloud data of the surrounding environment. Four high-dynamic range (HDR) industrial cameras are installed on the front, rear, and sides of the vehicle. These cameras have a resolution of 1920×1080 pixels and a frame rate of 30 fps, equipped with automatic exposure adjustment to accommodate the low-light conditions found in underground mining operations. They are used to capture image data of the surrounding environment. A six-axis IMU sensor is installed at the vehicle's center of gravity. The sensor has a sampling frequency of 200 Hz, an angular velocity measurement range of ±450° / s, and an acceleration measurement range of ±16 g. This sensor is used to acquire vehicle posture and motion data. Although GNSS signals are weak in underground mining environments, GNSS receivers supporting BeiDou, GPS, and GLONASS multi-mode reception are installed in some shallow areas or near the wellhead to provide an absolute position reference where signals are available.

[0037] Roadside sensors are deployed, and 8- or 16-line LiDAR sensors are fixedly installed at key locations in underground mine tunnels (such as intersections and corners). They have a sampling frequency of 5 Hz and a coverage radius of 30 meters, providing a static reference point cloud of the environment. Explosion-proof industrial cameras are installed at the top of the tunnels, using a fisheye lens with a 180° field of view, a resolution of 1280 × 720 pixels, and a frame rate of 15 fps. These cameras monitor dynamic environmental changes within the tunnels. UWB (ultra-wideband) base stations are installed every 50–100 meters in the tunnels to provide relative positioning information, serving as a supplementary positioning reference in GNSS-deprived environments.

[0038] Data collection: LiDAR point cloud data: records three-dimensional point clouds containing distance, reflection intensity, and timestamps, with a data volume of approximately 300K-600K points per second. Image data: collects RGB or grayscale images and records internal and external camera parameters and timestamp information. IMU data: records three-axis angular velocity, three-axis acceleration, and timestamp information. GNSS data: records geographic coordinates and accuracy assessment values in signal-available areas. Data storage and transmission: Vehicle-side data is processed and stored in real time by the on-board industrial computer, and roadside data is transmitted to the control center via industrial Ethernet or Wi-Fi, with a data transmission rate of no less than 100Mbps.

[0039] S2 preprocesses the collected multi-source data, including time synchronization of the multi-source data collected by vehicle-side sensors and roadside sensors; converting the time-synchronized multi-source data to a common coordinate system; and establishing a vehicle coordinate system with the center of the vehicle's rear axle as the origin (with the x-axis pointing toward the front of the vehicle, the y-axis pointing to the left, and the z-axis pointing upward). The multi-source data in the common coordinate system is filtered to obtain preprocessed multi-source data. A radius filter is applied to the point cloud data to remove outliers, with a search radius of 0.5 meters and a neighbor count threshold of 10. A wavelet-based denoising method is used for the IMU data, using the db4 wavelet with a decomposition level of 3.

[0040] like Figure 3 As shown, S3, based on the pre-processed lidar point cloud data, constructs a point cloud map, including: first, selecting two adjacent frames of point cloud data (frame i and frame i+1) from the pre-processed vehicle-side lidar point cloud data; for each point in frame i+1 , use the KD tree search algorithm to find the nearest corresponding point in frame i , build a point pair set; build the objective function , where N is the number of valid point pairs; preferably, in order to reduce the influence of outliers, the Huber robust kernel function is introduced and the modified objective function is ,in, is the Huber function, and the threshold δ is set to 0.15 meters.

[0041] Solve pose transformation: Use the pose change provided by IMU data as the initial estimate and The maximum number of iterations is set to 30. The closest point correspondences are recalculated at each iteration. Singular Value Decomposition (SVD) is used to find the optimal R and t for the current iteration. The algorithm stops when the pose change is less than a preset threshold (0.001 meters for translation and 0.01 radians for rotation) or when the maximum number of iterations is reached. For scenes with large changes or poor lighting conditions, a point pair screening process based on normal vectors and curvature is added to retain only points with similar geometric features.

[0042] Point cloud registration and fusion converts vehicle-side LiDAR point cloud data into a world coordinate system. Static point cloud data collected by roadside sensors is converted to the same world coordinate system using a pre-calibrated extrinsic parameter matrix. A weighted average method is used to fuse point cloud information in overlapping areas: the space is divided into voxels of 0.05 meters. For multiple points within the same voxel, a weighted average position is calculated, with the weights based on the measurement uncertainty of the points. For overlapping areas of vehicle-side and roadside point clouds, the weight of the roadside static point cloud is set to 0.7, and the weight of the vehicle-side point cloud is set to 0.3. This enables multi-resolution processing: high-resolution voxels (0.05 meters) are used for close-range areas (within 10 meters), and lower-resolution voxels (0.2 meters) are used for long-range areas.

[0043] By using the fused point cloud data, the initial point cloud map is updated through the extended Kalman filter algorithm to obtain the final point cloud map.

[0044] S4, based on the point cloud map, generates a point cloud feature map, including: transforming the point cloud nonlinear coordinates, using the spherical coordinate transformation method to convert the rectangular coordinates (x, y, z) into spherical coordinates The converted spherical coordinates are normalized, and the distance value r is normalized to the range [0, 1]. To compensate for the point cloud sparsity caused by increasing distance, an adaptive density adjustment factor is introduced: close-range area (0-10 meters): density adjustment factor α = 1.0; medium-range area (10-30 meters): density adjustment factor α = 1.5; long-range area (>30 meters): density adjustment factor α = 2.0. Local coordinate system alignment is implemented: the point cloud is directional-normalized using the main direction of the local area as a reference. Based on the aligned point cloud data, a multi-layer convolutional neural network is used for feature extraction to obtain local feature vectors. Maximum pooling and statistical feature pooling are used to perform weighted fusion on the local feature vectors to obtain feature vectors with enhanced discriminability. Based on the feature vectors with enhanced discriminability, a point cloud feature map is generated through feature mapping.

[0045] A multi-layer convolutional neural network is used for feature extraction. First, the network architecture is designed. The first layer: the input dimensions are point cloud coordinates (x, y, z) and reflection intensity, and the output is 64-dimensional features; the second layer: 64-dimensional input, 128-dimensional output, and the convolution kernel size is 1×1; the third layer: 128-dimensional input, 256-dimensional output, and the convolution kernel size is 1×1; each layer uses batch normalization and ReLU activation function to extract features and obtain local feature vectors.

[0046] Extract the maximum value of each dimension of the extracted local feature vector to generate a 256-dimensional global feature vector; calculate the statistical features such as the mean, variance, maximum value, and minimum value of the local feature vector to generate a 512-dimensional statistical feature vector; construct an attention mechanism to dynamically calculate the weight coefficients of maximum pooling and statistical pooling; fusion formula: F_fusion=α*F_maximum+(1-α)*F_statistics, where α is adaptively calculated based on data characteristics; for feature-sparse areas, increase the weight of statistical features; for feature-rich areas, increase the weight of maximum features; obtain a feature vector with enhanced discriminability, and organize the feature map based on the feature vector with enhanced discriminability by using the octree data structure.

[0047] S5 preprocesses the real-time LiDAR point cloud data and uses the PointNet network to extract real-time point cloud features from the preprocessed LiDAR point cloud data. Preprocessing includes motion compensation, point cloud downsampling, and filtering. The PointNet network uses a three-layer MLP (64, 128, 1024) layer. Preferably, the LiDAR point cloud is filtered for reflection intensity, retaining features with reflection intensity ≥ Ic, where Ic = 200 × exp(-0.05d), where d is the detection distance (meters). This improves feature robustness in dusty environments.

[0048] S6, based on the IMU data and GNSS data, as well as the vehicle posture data of the previous moment, establish the vehicle posture transformation matrix T at the current moment, including: first determining the adaptive integration time window, calculating the standard deviation based on the fluctuation amplitude of the IMU data (angular velocity) and (acceleration); set the base window size 10ms. Window adjustment rule: High dynamic state ( or ): The window is reduced to , i.e. 5ms; medium dynamic state ( or ): The window remains , i.e. 10ms; low dynamic state ( and ): The window expands to , i.e. 20ms; Smooth transition: Exponential moving average is used when the window size changes to avoid sudden changes.

[0049] Calculate attitude integral, angular velocity preprocessing: Apply temperature compensation: ,in is the temperature coefficient matrix, T is the current temperature, is the reference temperature; zero bias compensation: ,in Estimated by averaging at rest.

[0050] Rotation increment calculation: Convert angular velocity from sensor coordinate system to vehicle coordinate system: ; Use the midpoint method for integration: ; Construct an antisymmetric matrix ; Use the Rodriguez formula to calculate the rotation increment: .

[0051] Displacement integral calculation: acceleration preprocessing, temperature compensation: ; Gravity compensation: ,in, Is the gravity vector in the vehicle coordinate system. Speed and displacement update: The velocity increment is calculated using the trapezoidal integration method: ; Update current speed: ; Calculate the displacement increment: , Indicates the gravity compensation at the current moment, represents the displacement increment; Indicates the speed at the previous moment.

[0052] Speed Constraint: When the vehicle is detected to be stationary ( and Lasts for 0.5 seconds), forced setting , to prevent speed drift.

[0053] Construct the incremental transformation matrix: ; Calculate the basic pose transformation: .

[0054] Build a drift compensation model, initialize the drift coefficient matrix K to a 6×6 zero matrix, and set different learning rates for the rotation and translation components: , Collect historical pose correction data: record in GNSS available areas or when known feature points are observed and , calculate the pose error: ; Extract the deviation value of error in each degree of freedom ; Update the drift coefficient for each degree of freedom: .

[0055] Construct a scene feature vector: including features such as average curvature, surface normal distribution, and channel width; calculate the similarity score S∈[0, 1] between the current scene and the historical scene; adjust the drift coefficient based on the similarity: . Construct the drift matrix :Separate K into the rotating part and pan part ;calculate , where t is the cumulative time since the last pose reset; calculate the final IMU predicted pose: .

[0056] Construct the pose transformation matrix of the GNSS observation, check the GNSS signal quality indicators (such as the number of satellites ≥ 6, HDOP ≤ 2.0), and eliminate abnormal jump points (points where the position change exceeds the expected maximum speed). Convert the GNSS raw data (WGS84 longitude and latitude) to UTM coordinates, apply the pre-calibrated conversion parameters, and convert the UTM coordinates to the point cloud map coordinate system. The conversion formula is: .

[0057] The position part directly uses the converted GNSS coordinates. The orientation is determined by: mainly relying on the heading angle formed by continuous GNSS positions; using a sliding window (containing 3-5 historical points) to calculate the average heading; applying a low-pass filter to smooth the heading changes. Construct the pose matrix: ,in is the rotation matrix constructed based on the heading angle, is the GNSS position.

[0058] Pose transformation matrix fusion, IMU uncertainty calculation: rotation uncertainty is proportional to the integration time: ; The translation uncertainty is proportional to the square of the integration time: ; Calculate the weight of each degree of freedom: GNSS uncertainty calculation: based on GNSS positioning accuracy index (DOP) calculation: ; Heading uncertainty is inversely proportional to speed: ; Calculate the weight of each degree of freedom: .

[0059] Weight normalization: Ensure that the sum of the weights on each degree of freedom is 1: , ; Construct the diagonal weight matrix: , .

[0060] Rotation Fusion: Quaternion Conversion: Convert to quaternion ;Will Convert to quaternion ; Ensure that the rotation path represented by the two quaternions is the shortest (if , then Negative).

[0061] Spherical linear interpolation: Calculating fusion weights ; Apply the SLERP algorithm: ; where θ is and The angle between Calculate; return the rotation matrix: .

[0062] Translation fusion: Construct translation weight matrix: ; Weighted average calculation: ; For components with too low weight (such as or ), which completely adopts high-weight data sources.

[0063] Final pose construction: Combine the fused rotation matrix and translation vector: Apply smooth transition: In the case of sudden changes in sensor reliability (such as sudden recovery of GNSS signal), use exponential smooth transition to avoid posture jumps; smoothing formula: , where γ is a smoothing coefficient, typically set to 0.2 to 0.5. This application fully leverages the high temporal resolution of the IMU and the absolute positioning capability of the GNSS, overcoming the limitations of a single sensor and enabling reliable pose estimation in underground mining environments. In particular, high positioning accuracy can be maintained through adaptive drift compensation and dynamic weight fusion when the GNSS signal is unstable or missing.

[0064] S7, using the pose transformation matrix T, calculates the initial position of the real-time point cloud feature in the point cloud feature map ; and extract the initial position The local point cloud feature map centered on , including: feature vectors in the vehicle coordinate system and its corresponding three-dimensional position , , for each feature point position, perform homogeneous coordinate transformation and calculate the geometric center of the transformed feature set as the initial position, , i ranges from 1 to n.

[0065] At the initial position As the center, set the search radius R, extract local area data from the point cloud feature map, and construct the candidate matching area; basic search radius Set to 30 meters. Preprocess candidate matching areas; construct a local point cloud feature map based on the preprocessed candidate matching areas. Preprocessing includes removing invalid feature points and checking timeliness.

[0066] S8, performing feature matching on the real-time point cloud features and the local point cloud feature map to obtain matching point pairs, including: constructing a high-dimensional feature space based on the real-time point cloud features and the local point cloud feature map; wherein, a 128-dimensional Euclidean space is used to represent the features: ; Real-time feature set: ,in is a single feature vector; a map feature set: ,in is the feature vector in the map. Preferably, point pairs are selected based on normal vector consistency (angle < 10°) and curvature similarity (difference < 15%).

[0067] Kernel density estimation (KDE) is used to calculate the feature space density: ,in It is a Gaussian kernel function with a bandwidth of h, which divides the feature space based on the density threshold: high-density area: ,generally Set to 0.7; medium density area: ,generally Set to 0.3; low density area: .

[0068] Set the maximum leaf node capacity for different density areas: High density area: , ensure fine division and improve differentiation; medium-density area: , balance query efficiency and differentiation ability; low-density area: , reduce the depth of the tree and improve query efficiency.

[0069] To construct a mixed-capacity KD tree, we first determine the density region in which the node resides. We then assign the corresponding maximum leaf node capacity based on the density region. Node splitting occurs when the number of midpoints in a node exceeds the maximum capacity of the corresponding region. Leaf nodes in high-density regions store the five most representative feature points; leaf nodes in medium-density regions store the 15 most representative feature points; and leaf nodes in low-density regions store the 30 most representative feature points.

[0070] For each real-time point cloud feature, a nearest neighbor search is performed in the KD tree to obtain candidate matching point pairs; a geometric consistency check is performed on the candidate matching point pairs to obtain the final matching point pairs; the geometric consistency check can use the RANSAC algorithm.

[0071] S9, based on the matching point pairs and the posture transformation matrix T, obtain the vehicle posture data of the vehicle at the current moment, wherein the vehicle posture data includes position and posture, including: an error minimization objective function based on point pair matching: The Huber loss function is introduced to reduce the influence of outliers: ,in is the Huber function: , Set to 0.1 meters.

[0072] Point-to-point weighting strategy: assign weights based on matching confidence: , where d is the characteristic distance, is the scaling parameter; weights are assigned based on spatial distribution balance: , is the number of points in the local neighborhood; weights are assigned based on feature discriminability: , that is, the modulus of the feature gradient; comprehensive weight: . Therefore, the objective function is: .

[0073] The pose transformation matrix T is used as the initial value, and the Levenberg-Marquardt (LM) method is used to solve the objective function to obtain the optimal rotation matrix R and translation vector T. The iteration is stopped when the pose change is less than the threshold or the maximum number of iterations is reached;

[0074] Using the Gauss-Newton method, for each point pair, the derivative of the residual with respect to the pose parameters is calculated. ; For the rotational component, numerical differencing is used: , .

[0075] Information matrix calculation: , where W is the weight diagonal matrix and the covariance matrix is estimated: ,in is the residual variance estimate: ; N is the number of point pairs, and 6 is the degree of freedom of posture.

[0076] Through the trace of the covariance matrix Measure the overall pose uncertainty; extract the square root of the diagonal elements of the covariance matrix, the uncertainty in each direction, specifically, the translation uncertainty: ; Rotation uncertainty: In feature-rich regions, uncertainty decreases (usually rice, In regions with few features, uncertainty increases (usually , Spend).

[0077] According to the optimal rotation matrix R and translation vector T, update the current vehicle posture data: convert the optimal rotation matrix R into Euler angle representation , where α represents the yaw angle, β represents the pitch angle and γ represents the roll angle; ; ; Yaw angle α: [-π, π]; pitch angle β: [-π / 2, π / 2]; roll angle γ: [-π, π].

[0078] Coordinate system reference point for loading point cloud feature map and its direction ; Convert the local coordinate t to the global coordinate: , get the absolute position coordinates of the vehicle; use the time series smoothing algorithm based on Kalman filter to smooth the updated vehicle posture data at the current moment, and get the smoothed vehicle posture data at the current moment as the vehicle posture data of the vehicle at the current moment. The specific configuration of Kalman filter, state vector: ;Location ;speed ; Quaternion representation of posture ;Measurement vector: ; Position and pose from matching solution.

[0079] System dynamics model: Position update: ; Speed update: ; Posture update: ;in and From IMU measurements, Represents quaternion multiplication.

[0080] Process noise covariance matrix Q: Location noise: , increasing the impact of vehicle speed on position uncertainty; speed noise: , increasing the impact of acceleration on velocity uncertainty; attitude noise: , increasing the impact of angular velocity on attitude uncertainty.

[0081] Measurement noise covariance matrix R: Dynamic adjustment of uncertainty estimation based on pose matching; , position uncertainty from S3; , converted from the Euler angle uncertainty.

[0082] Final pose output: Position: ; Euler angle: converted from quaternion ; Uncertainty estimation: From the state covariance matrix extract.

Claims

1. A vehicle positioning method based on multi-source data fusion, characterized in that: include: S1, collecting multi-source data through vehicle-side sensors and roadside sensors, wherein the multi-source data includes lidar point cloud data, IMU data and GNSS data; S2, preprocessing the collected multi-source data; S3, constructs a point cloud map based on the preprocessed lidar point cloud data; S4, generating a point cloud feature map based on the point cloud map; S5, preprocessing the real-time collected LiDAR point cloud data, and using the PointNet network to extract the real-time point cloud features of the preprocessed LiDAR point cloud data; S6, based on the IMU data and GNSS data, as well as the vehicle posture data of the previous moment, establish the vehicle's current posture transformation matrix T, including: based on the IMU data and the vehicle posture data of the previous moment, obtain the vehicle's posture change and displacement change through piecewise integration, and adjust the integration time window according to the fluctuation amplitude of the IMU data; based on the piecewise integration results, construct the IMU predicted posture transformation matrix : ； ； in, Represents the vehicle posture transformation matrix at the previous moment; Represents the basic pose transformation matrix; Represents the rotation increment matrix at the current moment; Represents the displacement increment vector at the current moment; Represents a time-based drift compensation model; based on GNSS data, the vehicle's position in the global coordinate system is obtained, and the GNSS observation pose transformation matrix is constructed by converting the position into the coordinate system of the point cloud map ; Pose transformation matrix and the pose transformation matrix Perform weighted fusion to obtain the vehicle's current posture transformation matrix T; S7, using the pose transformation matrix T, calculates the initial position of the real-time point cloud feature in the point cloud feature map ; and extract the initial position The local point cloud feature map centered on ; S8, performing feature matching on the real-time point cloud features and the local point cloud feature map to obtain matching point pairs; S9, obtaining vehicle posture data of the vehicle at the current moment according to the matching point pairs and the posture transformation matrix T, wherein the vehicle posture data includes position and posture.

2. The vehicle positioning method based on multi-source data fusion according to claim 1, characterized in that: S2, pre-processing the collected multi-source data, including: Time synchronization of multi-source data collected by vehicle-side sensors and multi-source data collected by roadside sensors; Convert the time-synchronized multi-source data into the same coordinate system; The multi-source data in the same coordinate system are filtered to obtain preprocessed multi-source data.

3. The vehicle positioning method based on multi-source data fusion according to claim 1, characterized in that: S3 builds a point cloud map based on the preprocessed lidar point cloud data, including: Construct the objective function based on the pre-processed lidar point cloud data collected by the vehicle-side sensor , ,in, is a point in the target point cloud, Is the source point cloud The corresponding nearest point, N is the number of point pairs, R is the rotation matrix, and t is the translation vector; represents the Euclidean distance; By minimizing the objective function , obtain the pose transformation relationship between adjacent frame point clouds and construct the initial point cloud map; The pre-processed LiDAR point cloud data collected by the vehicle-side sensor and the pre-processed LiDAR point cloud data collected by the roadside sensor are fused to obtain fused point cloud data. By using the fused point cloud data, the initial point cloud map is updated through the extended Kalman filter algorithm to obtain the final point cloud map.

4. The vehicle positioning method based on multi-source data fusion according to claim 1, characterized in that: S4, based on the point cloud map, generates a point cloud feature map, including: Perform nonlinear coordinate transformation on the point cloud map to obtain aligned point cloud data; Based on the aligned point cloud data, a multi-layer convolutional neural network is used to extract features and obtain local feature vectors; Using maximum pooling and statistical feature pooling, local feature vectors are weightedly fused to obtain feature vectors with enhanced discriminability; According to the feature vectors that enhance the discriminability, a point cloud feature map is generated through feature mapping.

5. The vehicle positioning method based on multi-source data fusion according to claim 1, characterized in that: Time-based drift compensation model for: Where I is the identity matrix, K is the drift coefficient matrix, and t is the cumulative integration time.

6. The vehicle positioning method based on multi-source data fusion according to claim 5, characterized in that: S7, take the initial position The local point cloud feature map centered on , including: Using the pose transformation matrix T, the real-time point cloud features are transformed from the vehicle coordinate system to the point cloud feature map coordinate system to obtain the initial position of the real-time point cloud features in the point cloud feature map. ; At the initial position As the center, set the search radius R, extract the local area data from the point cloud feature map, and construct the candidate matching area; Preprocess the candidate matching areas; Based on the preprocessed candidate matching areas, a local point cloud feature map is constructed.

7. The vehicle positioning method based on multi-source data fusion according to claim 1, characterized in that: S8, performing feature matching on the real-time point cloud features and the local point cloud feature map to obtain matching point pairs, including: Construct a high-dimensional feature space based on real-time point cloud features and local point cloud feature maps; Calculate the feature distribution density of the local point cloud feature map, and divide the high-dimensional feature space into high-density area, medium-density area and low-density area according to the feature distribution density; Set the maximum leaf node capacity for high-density, medium-density, and low-density areas respectively 、 and ;in, ; Based on the maximum leaf node capacity 、 and , construct a KD tree; For each real-time point cloud feature, obtain candidate matching point pairs through nearest neighbor search in the KD tree; Perform geometric consistency check on the candidate matching point pairs to obtain the final matching point pairs.

8. The vehicle positioning method based on multi-source data fusion according to claim 1, characterized in that: S9, obtaining the vehicle posture data of the current moment according to the matching point pairs and the posture transformation matrix T, including: According to the matching point pairs, the pose solving objective function is constructed; Using the pose transformation matrix T as the initial value, iteratively solve the pose solution objective function to obtain the optimal rotation matrix R and translation vector T. The iteration is stopped when the pose change is less than the threshold or the maximum number of iterations is reached; The Gauss-Newton method is used to calculate the covariance matrix of the optimal rotation matrix R and translation vector T, and the uncertainty estimate of the optimal rotation matrix R and translation vector T is obtained as the input of the pose uncertainty estimation in step S6 at the next moment; According to the optimal rotation matrix R and translation vector T, update the vehicle pose data at the current moment: Convert the optimal rotation matrix R into Euler angle representation , where α represents the yaw angle, β represents the pitch angle and γ represents the roll angle; Align the translation vector T with the point cloud feature map in step S4 to obtain the absolute position coordinates of the vehicle; A time series smoothing algorithm based on Kalman filtering is used to smooth the updated vehicle posture data at the current moment, and the smoothed vehicle posture data at the current moment is obtained as the vehicle posture data of the vehicle at the current moment.

9. The vehicle positioning method based on multi-source data fusion according to claim 8, characterized in that: Get the vehicle's current posture transformation matrix T, including: According to the uncertainty estimation of the optimal rotation matrix R and translation vector T, an adaptive weight matrix is constructed. and : Among them, diag represents the diagonal matrix constructor; to is the weight coefficient of the six degrees of freedom corresponding to the IMU data; to is the weight coefficient of the six degrees of freedom corresponding to the GNSS data; Based on adaptive weight matrix and , the pose transformation matrix and the pose transformation matrix Decomposed into rotating parts 、 and pan part 、 ; For the rotating part, and Convert to quaternion representation and , using spherical linear interpolation SLERP for fusion: Among them, slerp is the spherical linear interpolation function; w is the fusion weight parameter; For the translation part, weighted averaging is used for fusion: in, is a 3×3 diagonal matrix; I is a 3×3 identity matrix; The fused quaternion Convert back to rotation matrix , and the fused translation vector Combine to get the final pose transformation matrix T: 。

Images