Vehicle positioning method based on multi-source data fusion

Abstract

The invention discloses a vehicle positioning method based on multi-source data fusion, and relates to the field of well mining vehicle positioning, and the method comprises the steps: collecting multi-source data; constructing a point cloud map, and generating a point cloud feature map; the method comprises the following steps: preprocessing laser radar point cloud data collected in real time, and extracting real-time point cloud features of the preprocessed laser radar point cloud data by adopting a PointNet network; establishing a pose transformation matrix T of the vehicle at the current moment according to the IMU data, the GNSS data and the vehicle pose data at the previous moment; calculating an initial position # imgabs0 # of the real-time point cloud feature in the point cloud feature map by using the matrix T; a local point cloud feature map with the initial position # imgabs1 # as the center is extracted; performing feature matching on the real-time point cloud feature map and the local point cloud feature map to obtain matched point pairs; and according to the matching point pairs and the matrix T, obtaining vehicle pose data of the vehicle at the current moment. Aiming at low positioning precision caused by difficulty in multi-source data fusion in a well mining environment in the prior art, the positioning precision is improved.

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 acceleration of the process of intelligent mining, unmanned driving technology in underground mining environments is developing rapidly and becoming a key means to improve mine production efficiency and safety. Accurate positioning is the infrastructure for the safe and efficient operation of unmanned vehicles, and directly affects the reliability and stability of the entire system. In open environments on the ground, positioning technology is relatively mature, but high-precision positioning in special environments such as underground mining still faces many challenges.

[0003] On the one hand, due to the closed tunnel structure deep underground, complex metal equipment and strong electromagnetic interference, satellite signals cannot penetrate the strata, resulting in the inability of positioning systems that rely on GNSS to work properly. On the other hand, the dim light, dusty environment, high tunnel similarity and dynamic environmental changes in the underground mining environment seriously affect the feature extraction and matching effects of the lidar. In particular, the high similarity of the tunnels makes it difficult to distinguish the extracted point cloud features, causing positioning ambiguity and reduced positioning accuracy. In addition, the strong magnetic field interference, frequent vehicle vibrations and drastic changes in temperature and humidity in the underground mining environment lead to the rapid accumulation of errors in the IMU integration process, making it difficult for pure inertial navigation to maintain accurate positioning for a long time. Summary of the invention

[0004] In view of the problem of low positioning accuracy caused by the difficulty in fusing multi-source data in underground mining environments in the prior art, the present 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 according to the preprocessed lidar point cloud data; S4, generating a point cloud feature map according to 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 according to 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 by integration, and constructs 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, constructs the observation part of the posture transformation matrix, and corrects the drift caused by IMU integration; the posture transformation matrix T is a 4×4 homogeneous transformation matrix, which 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. S8 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. S6 is used as the output of the prediction step to fuse the IMU and GNSS data to obtain a preliminary estimate of the vehicle's current posture; S7 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; S9 is used as the initial value for posture optimization and is further refined by minimizing the distance between matching point pairs.

[0009] Further, S2, preprocessing the collected multi-source data, including: time synchronization of the multi-source data collected by the vehicle-side sensor and the multi-source data collected by the roadside sensor; converting the time-synchronized multi-source data to the same coordinate system; filtering the multi-source data in the same coordinate system to obtain preprocessed multi-source data. Among them, the vehicle-side and roadside sensor data are integrated, the data asynchrony problem is solved through time synchronization, and the data in different coordinate systems are converted to a unified coordinate system, which effectively solves the problem of low positioning accuracy caused by the susceptibility of a single sensor to interference in an underground mining environment.

[0010] Further, S3, constructing a point cloud map based on the preprocessed laser radar point cloud data, including: constructing an objective function based on the preprocessed laser radar 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 posture transformation relationship between adjacent frame point clouds and construct the initial point cloud map; register and fuse the preprocessed lidar point cloud data collected by the vehicle-side sensor and the preprocessed lidar point cloud data collected by the roadside sensor to obtain fused point cloud data; use the fused point cloud data to update the initial point cloud map 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; extracting features using a multi-layer convolutional neural network based on the aligned point cloud data to obtain local feature vectors; performing weighted fusion on the local feature vectors using maximum pooling and statistical feature pooling 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 system 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 the radial distance, θ and φ represent the azimuth and elevation angles; specifically, the nonlinear transformation compensates for the natural sparsity of the point cloud in the long-distance area, maintains the consistency of features through density adjustment, enhances the invariance to rotation and direction changes, and improves the stability of features under different viewing angles. Compared with the direct use of Cartesian coordinates in this application, the local structural information of the point cloud can be better maintained, which is particularly suitable for processing scenes with obvious directionality and structural characteristics such as tunnels and lanes in complex environments of underground mines.

[0013] The feature vector representation with enhanced discriminability is a high-dimensional vector representation optimized by a specific multi-level feature extraction and fusion strategy, which has stronger distinguishing ability and robustness, and can more effectively identify and match different environmental scenes; including: original features: basic geometric and appearance features directly extracted from point clouds; maximum pooling features: retaining the most significant feature response in the local area, usually a 256-dimensional vector; statistical features: statistics describing the distribution of local area features, such as mean, variance, entropy, etc., usually a 512-dimensional vector. In this application, a highly discriminative scene representation is provided to make features at different locations easier to distinguish; reduce the impact of environmental changes (such as illumination and occlusion) on feature extraction, and improve matching stability.

[0014] Further, S6, based on the IMU data and GNSS data, as well as the vehicle posture data of the previous moment, establish the posture transformation matrix T of the vehicle at the current moment, including: according to the IMU data and the vehicle posture data of the previous moment, obtain the posture change and displacement change of the vehicle through piecewise integration, and the piecewise integration adjusts the integration time window according to the fluctuation amplitude of the IMU data; according to the piecewise integration results, construct the posture transformation matrix predicted by the IMU : ; ;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 to obtain: , exp represents exponential mapping; Represents 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; according to the GNSS data, the position of the vehicle 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 ; For the pose transformation matrix and the pose transformation matrix Perform weighted fusion to obtain the vehicle's current posture transformation matrix T.

[0015] On the one hand, 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. How to solve the IMU integration drift problem and cannot guarantee long-term positioning stability. On the other hand, IMU integration is a process that easily accumulates errors, and traditional fixed time window integration cannot adapt to changes in the vehicle's motion state.

[0016] This application adopts 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; represents the standard deviation of the angular velocity, represents the fluctuation amplitude of the angular velocity, and is expressed in rad / s. Preferably, the time window of the segmented integration is adjusted to meet the requirement that the window is reduced to 5ms when the dynamic coefficient D>0.7 and is expanded to 20ms when D<0.3.

[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 samplings, in seconds (s); j: The rate of change of acceleration, i.e. jerk, which indicates how fast the acceleration changes over time, in m / s³; α: The rate of change of angular velocity, i.e. 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 , , , ; : The reference value of the angular velocity fluctuation amplitude, which is the typical value in the static state obtained according to the system calibration, in 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 indicates 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, indicating the dynamic degree of the vehicle's motion state, dimensionless.

[0020] Further, 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] Further, 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. ; With the initial position As the center, set the search radius R, extract local area data from the point cloud feature map, and construct candidate matching areas; preprocess the candidate matching areas; and construct a local point cloud feature map based on the preprocessed candidate matching areas. 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 solution stops to optimize the use of computing resources.

[0022] Further, 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 according to 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 a high-density area, a medium-density area, and a low-density area according to the feature distribution density; and 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, the vehicle posture data of the vehicle at the current moment is obtained, and 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 a threshold or reaches the maximum number of iterations; 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; according to the optimal rotation matrix R and translation vector T, updating the vehicle posture data at the current moment: 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 the translation vector T and : ; ; Where diag represents the diagonal matrix constructor; to It is the weight coefficient of the six degrees of freedom corresponding to the IMU data, 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 Decompose 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 a 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 between the rotation estimated by the IMU (q_IMU) and the rotation estimated by the GNSS (q_GNSS). Ordinary linear interpolation will cause the quaternion length to be not 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 prior art, the advantages of this application are:

[0027] The tunnels of underground mines 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, so that the system can still 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 the long-distance straight tunnels of underground mines, the traditional algorithm is prone to feature matching failure due to the monotonous environment, and this method can significantly extend the effective time of pure IMU navigation.

[0028] In the mine working environment, due to roof reflection, signal shielding at the corners of the tunnel, and electromagnetic interference from metal equipment, the sensor data quality is unstable and the error distribution is uneven. 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 average to maintain position accuracy. This strategy can dynamically adjust the fusion weights according to the reliability of different sensors in different environments, so that the system can still maintain stable positioning when the signal is interfered. The advantage of this fusion method is that it can intelligently identify and reduce the impact of abnormal data, especially in the complex tunnel structure of mines, the reliability of different sensors will change significantly with environmental changes. The traditional fixed weight fusion method cannot adapt to this change, and 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 the mine working environment, the vibration caused by the frequent start and stop of the mine car, sharp turns, and uneven road surface causes the IMU data noise characteristics to change dynamically. This application analyzes the IMU data change rate and dynamically adjusts the size of the integration time window. In high-dynamic scenes, the window is reduced to improve accuracy, and in low-dynamic scenes, the window is expanded to reduce the impact of noise. This adaptive time window mechanism solves the limitations of the traditional fixed window integration method. In high-dynamic scenes such as mine cars driving at high speed or making sharp turns, a small time window can accurately capture motion details; in low-dynamic scenes such as vehicles driving 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 obtain the best IMU data quality under various 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 by the accompanying drawings. These embodiments are not restrictive, and in these embodiments, the same number represents the same structure, 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] As shown in 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, wherein the multi-source data includes lidar point cloud data, IMU data and GNSS data; preprocessing the collected multi-source data; constructing a point cloud map according to the preprocessed lidar point cloud data; generating a point cloud feature map according to 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 according to 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 ; performing feature matching on the real-time point cloud features and the local point cloud feature map to obtain matching point pairs; 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.

[0036] Specifically, in this embodiment, S1, a vehicle-side sensor is configured, and a 360° rotating 16-line or 32-line laser radar is installed on the top of the vehicle. The sampling frequency is set to 10Hz, the measurement range is 0.5-100 meters, and the angular resolution is 0.1° to 0.4°, which is used to obtain three-dimensional point cloud data of the surrounding environment. Four high dynamic range (HDR) industrial cameras are installed on the front, rear and both sides of the vehicle, with a resolution of 1920×1080 pixels and a frame rate of 30fps. They are equipped with an automatic exposure adjustment function to adapt to the weak light environment of mines and mines, and are used to collect image data of the surrounding environment. A six-axis IMU sensor is installed at the center of gravity of the vehicle, with a sampling frequency of 200Hz, an angular velocity measurement range of ±450° / s, and an acceleration measurement range of ±16g, which is used to obtain the vehicle's posture and motion state data. Although the GNSS signal is weak in the mine environment, a GNSS receiver is still installed in some shallow layers or near the wellhead area, supporting Beidou / GPS / GLONASS multi-mode reception, which is used to provide an absolute position reference in the signal available area.

[0037] Equipped with roadside sensors, 8-line or 16-line laser radars are fixedly installed at key locations in underground mine tunnels (such as intersections and corners), with a sampling frequency of 5Hz and a coverage radius of 30 meters, to provide a static reference point cloud of the environment. An explosion-proof industrial camera is installed on the top of the tunnel, using a fisheye lens to achieve a 180° field of view, a resolution of 1280×720 pixels, and a frame rate of 15fps to monitor dynamic environmental changes in the tunnel. A UWB (ultra-wideband) base station is installed every 50-100 meters in the tunnel to provide relative positioning information as an auxiliary positioning reference in an environment without GNSS.

[0038] Collect data, LiDAR point cloud data: record three-dimensional point cloud including distance, reflection intensity and timestamp, with data volume of about 300K-600K points per second. Image data: collect RGB or grayscale images, and record internal and external parameters and timestamp information of the camera. IMU data: record three-axis angular velocity, three-axis acceleration and timestamp information. GNSS data: record geographic coordinates and accuracy assessment values ​​in the signal available area. 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, preprocessing the collected multi-source data, including: time synchronization of the multi-source data collected by the vehicle-side sensors and the multi-source data collected by the roadside sensors; converting the time-synchronized multi-source data to the same coordinate system; establishing a vehicle coordinate system with the center of the rear axle of the vehicle as the origin (the x-axis points to the front of the vehicle, the y-axis points to the left, and the z-axis points upward). Filtering is performed on the multi-source data in the same coordinate system to obtain preprocessed multi-source data. A radius filter is applied to the point cloud data to remove outliers, the search radius is set to 0.5 meters, and the threshold of the number of neighboring points is set to 10; a denoising method based on wavelet transform is used for the IMU data, and the db4 wavelet is selected with a decomposition level of 3.

[0040] like Figure 3 As shown, S3, constructing a point cloud map based on the preprocessed laser radar point cloud data, including: first, selecting two adjacent frames of point cloud data (frame i and frame i+1) from the preprocessed vehicle-side laser radar 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 , construct a point pair set; construct 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 m.

[0041] Solving pose transformation: Using the pose change provided by the IMU data as an initial estimate and ; The maximum number of iterations is set to 30; The correspondence between the closest points is recalculated in each iteration; The optimal R and t of the current iteration are solved using singular value decomposition (SVD); Stop when the change in posture is less than the preset threshold (translation threshold 0.001 meters, rotation threshold 0.01 radians) 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 point pairs with similar geometric features.

[0042] Point cloud registration and fusion converts the vehicle-side LiDAR point cloud data into the world coordinate system. For the static point cloud data collected by the roadside sensor, it is converted to the same world coordinate system through a pre-calibrated external parameter matrix. The weighted average method is used to fuse the point cloud information of the overlapping area: the space is divided into voxels of 0.05 meters in size. For multiple points in the same voxel, the weighted average position is calculated. The weight is based on the measurement uncertainty of the point. For the overlapping area of ​​the 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 to achieve multi-resolution processing: high-resolution voxels (0.05 meters) are used in close-range areas (within 10 meters), and lower-resolution voxels (0.2 meters) are used in 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 ; Normalize the converted spherical coordinates and normalize the distance value r to the range of [0, 1]. In order to compensate for the sparse point cloud caused by the increase in distance, an adaptive density adjustment factor is introduced: short-distance area (0-10 meters): density adjustment factor α=1.0; medium-distance area (10-30 meters): density adjustment factor α=1.5; long-distance area (>30 meters): density adjustment factor α=2.0; Implement local coordinate system alignment: standardize the direction of the point cloud with 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 to extract features and obtain local feature vectors; Using maximum pooling and statistical feature pooling, the local feature vectors are weighted fused 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, and minimum of the local feature vector to generate a 512-dimensional statistical feature vector; build 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 statistical feature weight; for feature rich areas, increase the maximum feature weight; 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, preprocess the laser radar point cloud data collected in real time, and use the PointNet network to extract the real-time point cloud features of the preprocessed laser radar point cloud data; wherein the preprocessing includes motion compensation, point cloud downsampling and filtering. The PointNet network uses a 3-layer MLP (64, 128, 1024). Preferably, the laser radar point cloud is screened for reflection intensity, and the features of points with reflection intensity ≥ Ic are retained, where Ic = 200 × exp (-0.05d), d is the detection distance (meters), to improve the robustness of the features in dusty environments.

[0048] S6, based on the IMU data and GNSS data, as well as the vehicle posture data at the previous moment, establish the vehicle posture transformation matrix T at the current moment, including: first determine the adaptive integration time window, calculate the standard deviation based on the fluctuation amplitude of the IMU data (angular velocity) and (acceleration); set the base window size 10ms. Window adjustment rules: High dynamic state ( or ): The window shrinks 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), it is mandatory to set , to prevent speed drift.

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

[0054] Construct a drift compensation model, initialize the drift coefficient matrix K as 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 the error in each degree of freedom ; Update the drift coefficient for each degree of freedom: .

[0055] Construct scene feature vector: including features such as average curvature, surface normal vector distribution, channel width, etc.; 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 GNSS observations, check GNSS signal quality indicators (such as the number of satellites ≥ 6, HDOP ≤ 2.0), and exclude abnormal jump points (points where the position change exceeds the expected maximum speed). Convert GNSS raw data (WGS84 longitude and latitude) to UTM coordinates, apply pre-calibrated conversion parameters, and convert UTM coordinates to the point cloud map coordinate system. The conversion formula is: .

[0057] The position part directly uses the converted GNSS coordinates, and the orientation is determined by: mainly relying on the heading angle formed by continuous GNSS positions; using a sliding window (including 3-5 historical points) to calculate the average heading; applying a low-pass filter to smooth the heading changes. Constructing 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: Calculated based on GNSS positioning accuracy index (DOP): ; Heading uncertainty is inversely proportional to speed: ; Calculate the weight of each degree of freedom: .

[0059] Weight standardization: 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 Blending Weights ; Apply SLERP algorithm: ; where θ is and The angle between Calculate; back to the rotation matrix: .

[0062] Translational fusion: Constructing the translational weight matrix: ; Weighted average calculation: ; For components with too low weights (such as or ), completely adopting 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, usually set to 0.2 to 0.5. This application can make full use of the high temporal resolution of IMU and the absolute positioning capability of GNSS, overcome the defects of a single sensor, and achieve reliable pose estimation in underground mining environments. Especially when the GNSS signal is unstable or missing, high positioning accuracy can still be maintained through adaptive drift compensation and dynamic weight fusion.

[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] 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 the candidate matching area; build a local point cloud feature map based on the preprocessed candidate matching area. Preprocessing includes removing invalid feature points, timeliness check, etc.

[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 according to 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 bandwidth 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] Constructing a mixed capacity KD tree: first determine the density area where the node is located; allocate the corresponding maximum leaf node capacity according to the density area; node splitting condition: split when the number of midpoints in the node exceeds the maximum capacity of the corresponding area. High-density area leaf nodes: store the 5 most representative feature points; medium-density area leaf nodes: store the 15 most representative feature points; low-density area leaf nodes: store the 30 most representative feature points.

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

[0071] 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, wherein the vehicle posture data includes the position and posture, including: the error minimization objective function based on the 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 pair weight 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] Taking the pose transformation matrix T as the initial value, the Levenberg-Marquardt (LM) method is used to solve the objective function and 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 differentiation 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 areas, uncertainty decreases (usually rice, In areas where features are sparse, uncertainty increases (usually , Spend).

[0077] According to the optimal rotation matrix R and translation vector T, update the vehicle posture 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; ; ; . 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 attitude ; Measurement vector: ; Position and attitude 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; , the 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 respectively, wherein the multi-source data includes lidar point cloud 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 according to the point cloud map; S5, preprocessing the laser radar point cloud data collected in real time, and using the PointNet network to extract the real-time point cloud features of the preprocessed laser radar point cloud data; S6, establishing a posture transformation matrix T of the vehicle at the current moment according to the IMU data and the GNSS data, as well as the vehicle posture data at the previous moment; 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, preprocessing 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 to the same coordinate system; The multi-source data in the same coordinate system are filtered to obtain pre-processed multi-source data.

3. The vehicle positioning method based on multi-source data fusion according to claim 1, characterized in that: S3, constructs a point cloud map based on the preprocessed lidar point cloud data, including: Construct the objective function based on the preprocessed 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 registered and 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, generating a point cloud feature map based on the point cloud map, including: Perform nonlinear coordinate transformation on the point cloud map to obtain aligned point cloud data; According to 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 weighted and fused to obtain feature vectors with enhanced discriminability; Based on the feature vectors that enhance 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 is characterized in that: S6, establish the vehicle's current posture transformation matrix T, including: According to the IMU data and the vehicle posture data at the previous moment, the posture change and displacement change of the vehicle are obtained through piecewise integration, and the piecewise integration adjusts the integration time window according to the fluctuation amplitude of the IMU data; According to the segmented 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; According to the GNSS data, the position of the vehicle 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.

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

7. The vehicle positioning method based on multi-source data fusion according to claim 6 is characterized in that: S7, take the initial position The local point cloud feature map centered on is composed of: 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. ; 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; Preprocessing the candidate matching area; Based on the preprocessed candidate matching area, a local point cloud feature map is constructed.

8. 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 areas, medium-density areas, 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.

9. The vehicle positioning method based on multi-source data fusion according to claim 5, characterized in that: S9, obtaining the vehicle posture data of the vehicle at the current moment according to the matching point pair 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 the translation vector T, and the uncertainty estimate of the optimal rotation matrix R and the translation vector T is obtained as the input of the pose uncertainty estimate in the next step S6; According to the optimal rotation matrix R and translation vector T, update the vehicle posture data at the current moment: Convert the optimal rotation matrix R to 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, so as to obtain the smoothed vehicle posture data at the current moment as the vehicle posture data of the vehicle at the current moment.

10. The vehicle positioning method based on multi-source data fusion according to claim 9, 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 the translation vector T, an adaptive weight matrix is ​​constructed. and : ; ; Among them, diag represents the diagonal matrix constructor; to It 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 Decompose 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 average 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