Multi-source adaptive fusion positioning method, system and medium

Abstract

The invention provides a multi-source adaptive fusion positioning method and system and a medium, and belongs to the technical field of automatic driving and navigation positioning, and the method comprises the steps: receiving original data synchronously collected by each positioning module, extracting an input feature vector, and carrying out the standardization; inputting the standardized input feature vector into a pre-trained K-nearest neighbor model, and calculating to obtain an initial confidence coefficient corresponding to each positioning module; jointly inputting the initial confidence coefficient and the input feature vector into a back propagation neural network model to obtain a corrected sensor confidence coefficient vector, and generating a positioning mode recommendation classification; according to the positioning mode recommendation classification, adaptively selecting a working mode of the positioning system; based on an extended Kalman filter algorithm, the sensor confidence vector is used to perform state update, and parameters for vehicle navigation control are output. According to the method, GNSS false fixation is effectively detected, multi-source data are fused, the confidence coefficient is dynamically adjusted, and the positioning reliability is improved.

Description

Technical Field

[0001] This application belongs to the field of autonomous driving and navigation positioning technology, specifically relating to a multi-source adaptive fusion positioning method, system and medium. Background Technology

[0002] Global Navigation Satellite Systems (GNSS) are a key technology for achieving global positioning of autonomous vehicles. However, in complex environments such as industrial parks with many factories, GNSS signals are easily blocked and reflected, causing multipath effects and resulting in a false fixed-position phenomenon in the receiver. That is, the solution status shows a fixed solution, but the actual positioning error is far beyond the acceptable range. This hidden anomaly seriously threatens the operational safety and positioning reliability of autonomous vehicles.

[0003] To address this problem, existing technologies have proposed several solutions, but all have significant limitations. First, the loosely coupled GNSS / IMU navigation method uses inertial navigation to perform calculations during short-term GNSS signal interruptions. However, when GNSS experiences a false fixation rather than complete loss of lock, the IMU cannot identify and correct this error, instead accumulating errors due to the fusion of erroneous position information. Second, LiDAR or visual SLAM methods do not rely on GNSS, but in environments with many dynamic obstacles and similar scenes, their positioning results are prone to abrupt changes, exhibiting poor adaptability. Third, methods based on fixed threshold single anomaly detection detect anomalies by judging whether the barrier signal quality index (such as signal-to-noise ratio) exceeds the threshold. This method has poor adaptability, with high false alarm and false negative rates in complex and changing park environments.

[0004] Therefore, there is an urgent need for a systematic solution that can identify GNSS false fixes in real time and accurately in order to achieve smooth switching and continuous positioning. Summary of the Invention

[0005] In a first aspect, embodiments of this application provide a multi-source adaptive fusion positioning method applied to an autonomous vehicle, wherein the vehicle's controller is configured to perform the following operations: S1. Receive raw data synchronously collected by the vehicle-mounted GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; S2. Process the raw data, extract preset features including GNSS carrier phase residual, IMU acceleration variance, LiDAR point cloud matching residual, and UWB ranging residual, construct an input feature vector of preset dimensions and standardize it; S3. Input the standardized input feature vector into the pre-trained K-nearest neighbor model to calculate the initial confidence level corresponding to the GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; S4. Input the initial confidence score and the input feature vector together into the pre-trained backpropagation neural network model to obtain the sensor confidence score vector after nonlinear correction, and generate the positioning pattern recommendation classification; S5. Based on the positioning mode, the system recommends classification and adaptively selects the working mode of the positioning system. Based on the extended Kalman filter algorithm, the system dynamically constructs the observation noise covariance matrix using the sensor confidence vector and introduces the Huber function to robustly weight the observation residuals to perform state updates. Finally, the system outputs the positioning, speed, and heading status for vehicle navigation control.

[0006] Furthermore, the specific steps of step S2 are as follows: S21. Extract the standard deviation of carrier phase residuals from the data collected by the received vehicle-mounted GNSS receiver. Pseudorange multipath error standard deviation L1 signal-to-noise ratio mean L1 signal-to-noise ratio standard deviation Ambiguity ratio Fuzzy fixed success rate Position accuracy factor ; S22. Extract the acceleration variance from the data collected by the received IMU inertial measurement unit. angular velocity variance Attitude drift rate ; S23. Extract the mean value of ICP matching residuals from the received LiDAR data. Valid matching point ratio Point cloud density ; S24. Extract the standard deviation of the ranging residual from the data collected by the received UWB positioning module. Mean signal strength ; S25. The extracted data is used as the original features and processed using the Z-score normalization method:

[0007] in, Original features σ is the mean of each original feature on the historical dataset, and σ is the standard deviation of each original feature on the historical dataset.

[0008] Furthermore, the specific steps of step S3 are as follows: S31. Match the standardized input feature vector with a pre-established historical sample library containing historical data of typical park environment scenarios, and calculate the relationship between the current feature vector x and each historical sample in the historical sample library. European distance :

[0009] Where D is the dimension of the feature vector; S32. Calculate each historical sample using inverse distance weighting. weight :

[0010] Where ε is the correction coefficient; S33. Select the k nearest neighbors by Euclidean distance to form a neighbor set. ; S34. Based on Neighbor Set and neighbors Chinese historical samples weight The initial confidence levels of four sensors—GNSS receiver, IMU inertial measurement unit, LiDAR, and UWB positioning module—are calculated in parallel under the current environment to form an initial confidence vector. ; The initial confidence level for any sensor S is calculated using the following formula:

[0011] In the formula, ∈{0,1} are the pre-labeled labels for sensor S in the historical sample database. 0 indicates that sensor S is reliable under the historical sample conditions, and 1 indicates that sensor S is unreliable or abnormal under the historical sample conditions. S represents GNSS receiver, IMU inertial measurement unit, LiDAR lidar, and UWB positioning module, respectively. S35. Output the initial confidence vector .

[0012] Furthermore, the specific steps of step S4 are as follows: S41. Initial confidence vector The input feature vector is concatenated with the standardized input feature vector to generate a comprehensive input vector; S42. Input the combined input vector into the pre-trained backpropagation neural network model, and output the following two vectors: Output the sensor confidence vector after nonlinear correction :

[0013] Among them, each value ∈(0,1) represents the probability that the corresponding sensor S will have an observation anomaly or be unavailable; Output localization pattern recommendation probability vector ; in, The following four predefined positioning modes are used to represent the probability of the system making a recommendation: It is a tightly coupled GNSS / IMU mode, serving as the dominant mode when GNSS signals are reliable; It is a GNSS / IMU / LiDAR fusion mode, serving as an auxiliary enhancement mode when GNSS reliability declines; It is an IMU / LiDAR / UWB alternative mode, serving as a substitute for GNSS when GNSS is unreliable; It is a LiDAR / IMU combined mode, serving as an alternative mode to the no-GNSS mode when GNSS is unreliable.

[0014] Furthermore, the specific steps of step S42 are as follows: S421. Constructing a backpropagation neural network model: The input layer is configured with a number of nodes corresponding to the dimension of the comprehensive input vector. Construct an intermediate layer containing at least two fully connected hidden layers. Each intermediate layer uses the ReLU activation function for non-linear transformation, and the number of neurons in each fully connected hidden layer decreases from front to back, forming a feature compression structure. The output layer is configured with a dual-branch parallel output structure. The first output branch is configured with neurons corresponding to the number of sensors and uses the Sigmoid activation function to output the sensor confidence vector. The second output branch is configured with neurons corresponding to the number of localization modes and uses the Softmax activation function to output the localization mode recommendation probability vector. S422. Training the backpropagation neural network: Supervised training was performed using historical datasets covering typical scenarios in the park. A multi-task learning framework is adopted, and a composite loss function is defined as the total loss function: The binary cross-entropy loss function is applied to the first output branch to calculate the prediction error of the sensor anomaly probability; The classification cross-entropy loss function is applied to the second output branch to calculate the prediction error of the localization pattern classification; The total loss function is a weighted combination of the binary cross-entropy loss function and the branching function is a classification cross-entropy loss function; During network training, Dropout regularization and L2 weight regularization are applied to the hidden layers, the network parameters are optimized using the adaptive moment estimation algorithm, and an early stopping strategy is adopted to monitor the validation set loss. Training is terminated when the validation set loss no longer decreases within a preset number of rounds. S423. Input the combined input vector into the trained backpropagation neural network model; The sensor confidence vector is obtained by parsing from the first output branch, where each confidence value represents the probability of the corresponding sensor exhibiting an observation anomaly; The location mode recommendation probability vector is obtained from the second output branch, and the mode with the highest probability is selected as the recommended location mode. The sensor confidence vector and the positioning mode recommendation result are output together.

[0015] Furthermore, the specific steps of step S5 are as follows: S51. Based on the probability vector recommended by the positioning mode, select the positioning mode with the highest probability as the current working mode of the vehicle positioning system, so as to determine the sensor that adopts the observation data in the filtering. S52. Within the framework of extended Kalman filtering, using data acquired by the IMU (Inertial Measurement Unit), perform the state prediction step: State prediction:

[0016] Covariance prediction:

[0017] Where F is the state transition matrix based on the vehicle kinematics model; The control input, which serves as the motion increment, is obtained by integrating the data acquired by the IMU (Inertial Measurement Unit). is the process noise covariance matrix, used to characterize the uncertainty of the motion model; P is the error covariance matrix of the state estimation. The system state vector includes the vehicle's position, velocity, and heading angle in a two-dimensional plane; subscripts... This represents the estimation of the state vector at time k based on information from time k-1 and earlier. This represents the optimal state estimation result at time k-1; S53. Using the corrected sensor confidence vector Dynamically construct an adaptive observation noise covariance matrix for the currently activated sensor. To automatically reduce the weight of sensors with low confidence; S54. When observation data from GNSS receivers, LiDAR laser radars, or UWB positioning modules arrive, combine it with the adaptive observation covariance matrix. It performs observation updates that incorporate the Huber robust factor to suppress the impact of outlier observations and outputs the final vehicle state estimate.

[0018] Furthermore, in step S53, an adaptive observation noise covariance matrix is ​​dynamically constructed for the currently activated sensor. The specific steps are as follows: Based on the selected positioning mode, select a subset A of the four sensors that are activated; Construct an adaptive scaling vector s corresponding to the dimension of the sensor subset A; Wherein, for each sensor S in the sensor subset A, the corresponding scaling factor is: For sensors that are not activated, they do not participate in the adaptive observation noise covariance matrix. The structure; This represents the confidence vector of sensor S; Adaptive observation noise covariance matrix The calculation method is as follows:

[0019] in, This represents the Hadamard product, which is the product of elements. This is the preset benchmark observation noise covariance matrix corresponding to the sensor subset A.

[0020] Furthermore, the specific steps for performing the observation update that introduces the Huber robust factor in step S54 are as follows: S541. Based on current observation data With state prediction value Calculate the following residual vector As a priori residual:

[0021] in, The observation matrix; S542. For the residual vector Each component Calculate the corresponding Huber weight factor : ; in, For component thresholds, , For online estimation of the residual standard deviation; S543. Construct a diagonal weight matrix based on all calculated Huber weight factors. : ; S544. Using the diagonal weight matrix With adaptive observation covariance matrix Calculate the robust final observation covariance matrix. ; S545. Using a robust covariance matrix Calculate Kalman gain And through Kalman gain Update system state vector With covariance matrix This outputs the updated positioning, speed, and heading status.

[0022] Secondly, embodiments of this application also provide a multi-source adaptive fusion positioning system for use in autonomous vehicles, comprising: The data acquisition module is used to receive raw data synchronously acquired by the vehicle-mounted GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; The feature extraction and processing module is used to process the raw data, extract preset features including GNSS carrier phase residual, IMU acceleration variance, LiDAR point cloud matching residual, and UWB ranging residual, construct an input feature vector of preset dimensions and standardize it; The initial confidence calculation module is used to input the standardized input feature vector into the pre-trained K-nearest neighbor model to calculate the initial confidence corresponding to the GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; The confidence correction and decision module is used to input the initial confidence and the input feature vector into the pre-trained backpropagation neural network model to obtain the sensor confidence vector after nonlinear correction, and generate a positioning pattern recommendation classification. The adaptive fusion positioning module is used to recommend classifications based on positioning modes and adaptively select the operating mode of the positioning system. Based on the extended Kalman filter algorithm, it uses sensor confidence vectors to dynamically construct the observation noise covariance matrix and introduces the Huber function to robustly weight the observation residuals to perform state updates. Finally, it outputs the positioning, speed and heading status for vehicle navigation control.

[0023] Thirdly, embodiments of this application also provide a storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the multi-source adaptive fusion localization method as described in the first aspect.

[0024] As can be seen from the above technical solutions, this application has the following advantages: The multi-source adaptive fusion positioning method, system, and medium provided in this application effectively solve the false fixation problem caused by interference with GNSS signals in complex environments through multi-sensor data fusion, dynamic confidence adjustment, and robust weighting, thereby improving the positioning accuracy and reliability of unmanned vehicles. Through multi-sensor data fusion, dynamic confidence adjustment, and robust weighting, adaptive switching of positioning modes is achieved, enhancing the adaptability to complex environments and improving operational safety and navigation performance. Attached Figure Description

[0025] To more clearly illustrate the technical solution of this application, the accompanying drawings used in the description will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0026] Figure 1 This is a flowchart illustrating the multi-source adaptive fusion localization method of the present invention.

[0027] Figure 2 This is a schematic diagram of the multi-source adaptive fusion positioning system of the present invention. Detailed Implementation

[0028] The various embodiments of this disclosure will be described more fully in the detailed steps of the multi-source adaptive fusion localization method described below. This disclosure may have various embodiments, and adjustments and changes may be made therein. However, it should be understood that there is no intention to limit the various embodiments of this disclosure to the specific embodiments disclosed herein, but rather this disclosure should be understood to cover all adjustments, equivalents, and / or alternatives falling within the spirit and scope of the various embodiments of this disclosure.

[0029] This embodiment provides a multi-source adaptive fusion positioning method that effectively detects GNSS false fixation, fuses multi-source data, dynamically adjusts confidence levels, and improves positioning reliability.

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

[0031] Please see Figure 1 The diagram shows a flowchart of a multi-source adaptive fusion localization method in a specific embodiment. The method includes the following steps: S1. Receive raw data synchronously collected by the vehicle-mounted GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; It should be noted that receiving raw data synchronously collected by multiple sensors provides input information for processing; the fusion of multi-source data can compensate for the shortcomings of a single sensor and enhance the adaptability of the positioning system in complex environments. S2. Process the raw data, extract preset features including GNSS carrier phase residual, IMU acceleration variance, LiDAR point cloud matching residual, and UWB ranging residual, construct an input feature vector of preset dimensions and standardize it; It should be noted that by extracting preset features and performing standardization, the quality and consistency of the input data are ensured; the standardized feature vectors can better reflect the state of each sensor, providing a data foundation for subsequent confidence calculation and positioning mode recommendation. S3. Input the standardized input feature vector into the pre-trained K-nearest neighbor model to calculate the initial confidence level corresponding to the GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; It should be noted that by calculating the initial confidence level using a pre-trained K-nearest neighbor model, the reliability of each sensor in the current environment can be quickly assessed; this fully utilizes the experience gained from historical data, improving the adaptability of the positioning system to new environments and the accuracy of the initial confidence level. S4. Input the initial confidence score and the input feature vector together into the pre-trained backpropagation neural network model to obtain the sensor confidence score vector after nonlinear correction, and generate the positioning pattern recommendation classification; It should be noted that by using a backpropagation neural network to perform nonlinear correction on the initial confidence level and generate a positioning mode recommendation classification, the sensor confidence level can be further optimized. At the same time, it provides data support for the selection of positioning modes, enhancing the adaptability and decision accuracy of the positioning system in complex environments. S5. Based on the positioning mode, recommend classification and adaptively select the working mode of the positioning system; based on the extended Kalman filter algorithm, use the sensor confidence vector to dynamically construct the observation noise covariance matrix, and introduce the Huber function to robustly weight the observation residuals to perform state updates, and finally output the positioning, speed and heading status for vehicle navigation control; It should be noted that, based on the extended Kalman filter algorithm, the observation noise covariance matrix is ​​dynamically constructed by combining sensor confidence, and the Huber function is introduced for robust weighting, which effectively suppresses the influence of abnormal observations, improves the accuracy and reliability of the positioning results, and enhances the adaptability to complex environments.

[0032] This embodiment improves positioning accuracy and system robustness through multi-sensor data fusion and dynamic confidence adjustment, effectively copes with GNSS signal interference in complex environments, and ensures the safe operation of unmanned vehicles.

[0033] Furthermore, as a refinement and extension of the specific implementation of the above embodiments, in order to fully illustrate the specific implementation process in this embodiment, taking an unmanned logistics vehicle in an industrial park as an application scenario, the vehicle is equipped with a GNSS receiver (e.g., Trimble R10), an IMU inertial measurement unit (e.g., BMI088), a LiDAR laser radar (e.g., Velodyne VLP-16), and a UWB positioning module (e.g., Decawave DW1000). The system sampling frequency is set to 10Hz, and the historical sample library contains 100,000 labeled data points from eight typical scenarios within the park, including densely populated factory areas, open roads, and warehouse passages. This provides another multi-source adaptive fusion positioning method, which includes the following steps: S1. Receive raw data synchronously collected by the vehicle-mounted GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; S2. Process the raw data, extract preset features including GNSS carrier phase residual, IMU acceleration variance, LiDAR point cloud matching residual, and UWB ranging residual, construct an input feature vector of preset dimensions and standardize it; For example, the data acquisition synchronously receives raw data from four types of sensors, and the sampled data at a certain moment is as follows: GNSS receiver: Carrier phase observation 3245678.9012 cycles, pseudorange observation 25678901.345 m, L1 band signal-to-noise ratio 42 dB; IMU (Inertial Measurement Unit): x-axis acceleration 2.3 m / s² 2 y-axis angular velocity 0.05 rad / s, z-axis attitude angle 35.2°; LiDAR: A single frame of point cloud data contains 38,400 points, with a horizontal scanning angle of 360° and a vertical scanning angle of -15° to 15°; UWB positioning module: distance measurements to the three anchor points are 8.3m, 12.1m, and 15.7m, respectively, with a signal strength of -65dBm; The specific steps of step S2 are as follows: S21. Extract the standard deviation of carrier phase residuals from the data collected by the received vehicle-mounted GNSS receiver. Pseudorange multipath error standard deviation L1 signal-to-noise ratio mean L1 signal-to-noise ratio standard deviation Ambiguity ratio Fuzzy fixed success rate Position accuracy factor ; S22. Extract the acceleration variance from the data collected by the received IMU inertial measurement unit. angular velocity variance Attitude drift rate ; S23. Extract the mean value of ICP matching residuals from the received LiDAR data. Valid matching point ratio Point cloud density ; S24. Extract the standard deviation of the ranging residual from the data collected by the received UWB positioning module. Mean signal strength ; S25. The extracted data is used as the original features and processed using the Z-score normalization method:

[0034] in, Original features σ is the mean of each original feature on the historical dataset, and σ is the standard deviation of each original feature on the historical dataset; For example, following steps S21-S24, the features of each sensor are extracted, and the feature extraction results at a certain moment are as follows: GNSS characteristics: carrier phase residual standard deviation 0.02 cycles, pseudorange multipath error standard deviation 0.3 m, mean L1 signal-to-noise ratio 38 dB, standard deviation L1 signal-to-noise ratio 3 dB, ambiguity ratio 1.8, ambiguity fixation success rate 92%, position accuracy factor (PDOP) 1.5; IMU characteristics: Acceleration variance 0.015m 2 / s 4 Angular velocity variance 0.002 rad 2 / s 4 Attitude drift rate: 0.03° / h; LiDAR characteristics: mean ICP matching residual 0.08m, effective matching point ratio 85%, point cloud density 1200 points / m 2 ; UWB characteristics: ranging residual standard deviation 0.25m, mean signal strength -70dBm; The Z-score standardization method is used to process the above original features. Assuming the mean μ = 0.02 and standard deviation σ = 0.01 for a certain feature in the historical dataset, if the original value of this feature x = 0.02, then the standardized value is: ; Similarly, after standardizing all 15-dimensional features, the input feature vector is [0,0.5,1.2,0.8,0.3,0.6,0.2,0.4,0.1,0.5,0.7,0.3,0.9,0.4,0.6]. S3. Input the standardized input feature vector into the pre-trained K-nearest neighbor model to calculate the initial confidence level corresponding to the GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; The specific steps of step S3 are as follows: S31. Match the standardized input feature vector with a pre-established historical sample library containing historical data of typical park environment scenarios, and calculate the relationship between the current feature vector x and each historical sample in the historical sample library. European distance :

[0035] Where D is the dimension of the feature vector; S32. Calculate each historical sample using inverse distance weighting. weight :

[0036] Where ε is a correction coefficient, for example... To prevent division by zero errors; S33. Select the k nearest neighbors by Euclidean distance (e.g., 5) to form a neighbor set. ; S34. Based on Neighbor Set and neighbors Chinese historical samples weight The initial confidence levels of four sensors—GNSS receiver, IMU inertial measurement unit, LiDAR, and UWB positioning module—are calculated in parallel under the current environment to form an initial confidence vector. ; The initial confidence level for any sensor S is calculated using the following formula:

[0037] In the formula, ∈{0,1} are the pre-labeled labels for sensor S in the historical sample database. 0 indicates that sensor S is reliable under the historical sample conditions, and 1 indicates that sensor S is unreliable or abnormal under the historical sample conditions. S represents GNSS receiver, IMU inertial measurement unit, LiDAR lidar, and UWB positioning module, respectively. S35. Output the initial confidence vector ; For example, first, calculate the Euclidean distance: Let the standardized input feature vector x be 15-dimensional, and a historical sample y in the historical sample database be [0.1, 0.4, 1.1, 0.9, 0.2, 0.7, 0.3, 0.3, 0.2, 0.4, 0.6, 0.4, 0.8, 0.3, 0.5]. Then the Euclidean distance between the two is:

[0038] Secondly, the historical sample weights are adjusted using corrective coefficients. ε=0.001, the weight of this historical sample is:

[0039] Then, neighbor set selection is performed: The k=5 nearest neighbors by Euclidean distance have Euclidean distances of 0.387, 0.421, 0.453, 0.489, and 0.512, respectively, and corresponding weights of 2.577, 2.375, 2.208, 2.045, and 1.953. Finally, the initial confidence level is calculated: Assuming the five neighbors have pre-labeled the GNSS sensor as [0,0,1,0,1] (0 representing reliable, 1 representing anomalous), then the initial confidence level of the GNSS is:

[0040] Similarly, the initial confidence levels were calculated to be 0.12 for IMU, 0.08 for LiDAR, and 0.21 for UWB, with the initial confidence vector being [0.43, 0.12, 0.08, 0.21]. S4. Input the initial confidence score and the input feature vector together into the pre-trained backpropagation neural network model to obtain the sensor confidence score vector after nonlinear correction, and generate the positioning pattern recommendation classification; The specific steps of step S4 are as follows: S41. Initial confidence vector The input feature vector is concatenated with the standardized input feature vector to generate a comprehensive input vector; S42. Input the combined input vector into the pre-trained backpropagation neural network model, and output the following two vectors: Output the sensor confidence vector after nonlinear correction :

[0041] Among them, each value ∈(0,1) represents the probability that the corresponding sensor S will have an observation anomaly or be unavailable; Output localization pattern recommendation probability vector ; in, The following four predefined positioning modes are used to represent the probability of the system making a recommendation: It is a tightly coupled GNSS / IMU mode, serving as the dominant mode when GNSS signals are reliable; It is a GNSS / IMU / LiDAR fusion mode, serving as an auxiliary enhancement mode when GNSS reliability declines; It is an IMU / LiDAR / UWB alternative mode, serving as a substitute for GNSS when GNSS is unreliable; It is a LiDAR / IMU combined mode, serving as an alternative mode to the no-GNSS mode when GNSS is unreliable; The specific steps of step S42 are as follows: S421. Constructing a backpropagation neural network model: The input layer is configured with a number of nodes corresponding to the dimension of the comprehensive input vector. Construct an intermediate layer containing at least two fully connected hidden layers. Each intermediate layer uses the ReLU activation function for non-linear transformation, and the number of neurons in each fully connected hidden layer decreases from front to back, forming a feature compression structure. The output layer is configured with a dual-branch parallel output structure. The first output branch is configured with neurons corresponding to the number of sensors and uses the Sigmoid activation function to output the sensor confidence vector. The second output branch is configured with neurons corresponding to the number of localization modes and uses the Softmax activation function to output the localization mode recommendation probability vector. S422. Training the backpropagation neural network: Supervised training was performed using historical datasets covering typical scenarios in the park. A multi-task learning framework is adopted, and a composite loss function is defined as the total loss function: The binary cross-entropy loss function is applied to the first output branch to calculate the prediction error of the sensor anomaly probability; The classification cross-entropy loss function is applied to the second output branch to calculate the prediction error of the localization pattern classification; The total loss function is a weighted combination of the binary cross-entropy loss function and the branching function is a classification cross-entropy loss function; During network training, Dropout regularization and L2 weight regularization are applied to the hidden layers, the network parameters are optimized using the adaptive moment estimation algorithm, and an early stopping strategy is adopted to monitor the validation set loss. Training is terminated when the validation set loss no longer decreases within a preset number of rounds. S423. Input the combined input vector into the trained backpropagation neural network model; The sensor confidence vector is obtained by parsing from the first output branch, where each confidence value represents the probability of the corresponding sensor exhibiting an observation anomaly; The location mode recommendation probability vector is obtained from the second output branch, and the mode with the highest probability is selected as the recommended location mode. The sensor confidence vector and the positioning mode recommendation result are output together; For example, the first step is to construct the composite input vector: The initial confidence vector [0.43, 0.12, 0.08, 0.21] is concatenated with the 15-dimensional standardized input feature vector to obtain a 19-dimensional comprehensive input vector; Secondly, there is the backpropagation neural network model computation: Model structure: The input layer has 19 nodes, the hidden layer has 2 layers (the first layer has 64 neurons and the second layer has 32 neurons), and the output layer has two branches (4 neurons for sensor confidence output and 4 neurons for localization pattern recommendation output).

[0042] Model output: The trained model outputs a sensor confidence vector of [0.38, 0.10, 0.07, 0.19], representing anomalies of 38% for GNSS, 10% for IMU, 7% for LiDAR, and 19% for UWB; the recommended positioning mode probability vector is [0.25, 0.62, 0.08, 0.05], with the GNSS / IMU / LiDAR fusion mode having the highest probability (62%), which is determined as the recommended positioning mode; S5. Based on the positioning mode, recommend classification and adaptively select the working mode of the positioning system; based on the extended Kalman filter algorithm, use the sensor confidence vector to dynamically construct the observation noise covariance matrix, and introduce the Huber function to robustly weight the observation residuals to perform state updates, and finally output the positioning, speed and heading status for vehicle navigation control; The specific steps of step S5 are as follows: S51. Based on the probability vector recommended by the positioning mode, select the positioning mode with the highest probability as the current working mode of the vehicle positioning system, so as to determine the sensor that adopts the observation data in the filtering. S52. Within the framework of extended Kalman filtering, using data acquired by the IMU (Inertial Measurement Unit), perform the state prediction step: State prediction:

[0043] Covariance prediction:

[0044] Where F is the state transition matrix based on the vehicle kinematics model; The control input, which serves as the motion increment, is obtained by integrating the data acquired by the IMU (Inertial Measurement Unit). is the process noise covariance matrix, used to characterize the uncertainty of the motion model; P is the error covariance matrix of the state estimation. The system state vector includes the vehicle's position, velocity, and heading angle in a two-dimensional plane; subscripts... This represents the estimation of the state vector at time k based on information from time k-1 and earlier. This represents the optimal state estimation result at time k-1; S53. Using the corrected sensor confidence vector Dynamically construct an adaptive observation noise covariance matrix for the currently activated sensor. To automatically reduce the weight of sensors with low confidence; It should be noted that this step results in a low confidence level (i.e., Sensor S, which is closer to 1), is given greater observation noise in the filter, thus automatically reducing its weight; S54. When observation data from GNSS receivers, LiDAR laser radars, or UWB positioning modules arrive, combine it with the adaptive observation covariance matrix. It performs observation updates that incorporate the Huber robust factor to suppress the impact of outlier observations and outputs the final vehicle state estimate. For example, the first step is to select the location mode: Based on the recommendation results, the system adopts the GNSS / IMU / LiDAR fusion mode, and the activated sensor subset A consists of GNSS, IMU, and LiDAR. Secondly, state prediction is based on the vehicle kinematics model, and the state transition matrix F is set as follows:

[0045] Where T = 0.1s (sampling period), the control input matrix The control input obtained by IMU integration Let the process noise covariance matrix Q be diag([0.01,0.001,0.0001]), and the optimal state estimate at time k-1 be... Error covariance matrix Then: State prediction:

[0046] Covariance prediction:

[0047] Then comes the construction of the adaptive observation noise covariance matrix: Calculate the scaling vector s: GNSS scaling factor for the active sensor. IMU scaling factor LiDAR scaling factor The scaling vector is s = [1.38, 1.10, 1.07]. Calculate the baseline observation noise covariance matrix Then the adaptive observation noise covariance matrix is:

[0048] Finally, robust observation updates: Residual calculation: The observation matrix H is selected as the identity matrix according to the fusion mode, and the current observation data... residual ; Huber weight calculation: Online estimation of residual standard deviation threshold Calculate the weights for each component of the residual: Weight ; Weight ; Weight

[0049] weight matrix Robust observation covariance matrix ; Kalman gain calculation: ; Status Update: Output the vehicle's location, speed, and heading status at that moment.

[0050] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0051] like Figure 2As shown, the following are embodiments of the multi-source adaptive fusion positioning system provided in this disclosure. This system and the multi-source adaptive fusion positioning methods in the above embodiments belong to the same inventive concept. For details not described in detail in the embodiments of the multi-source adaptive fusion positioning system, please refer to the embodiments of the above multi-source adaptive fusion positioning methods.

[0052] The system includes: The data acquisition module is used to receive raw data synchronously acquired by the vehicle-mounted GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; The feature extraction and processing module is used to process the raw data, extract preset features including GNSS carrier phase residual, IMU acceleration variance, LiDAR point cloud matching residual, and UWB ranging residual, construct an input feature vector of preset dimensions and standardize it; The initial confidence calculation module is used to input the standardized input feature vector into the pre-trained K-nearest neighbor model to calculate the initial confidence corresponding to the GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; The confidence correction and decision module is used to input the initial confidence and the input feature vector into the pre-trained backpropagation neural network model to obtain the sensor confidence vector after nonlinear correction, and generate a positioning pattern recommendation classification. The adaptive fusion positioning module is used to recommend classifications based on positioning modes and adaptively select the operating mode of the positioning system. Based on the extended Kalman filter algorithm, it uses sensor confidence vectors to dynamically construct the observation noise covariance matrix and introduces the Huber function to robustly weight the observation residuals to perform state updates. Finally, it outputs the positioning, speed and heading status for vehicle navigation control.

[0053] This embodiment solves the false fixation problem caused by GNSS signal interference in complex environments by the interactive collaboration of the data acquisition module, feature extraction and processing module, initial confidence calculation module, confidence correction and decision module, and adaptive fusion positioning module. This improves the positioning accuracy and reliability of unmanned vehicles, enhances the adaptability of the positioning system to complex environments, and improves operational safety.

[0054] The storage medium provided in this application stores a program product capable of implementing a multi-source adaptive fusion positioning method.

[0055] The multi-source adaptive fusion positioning method includes: applied to an autonomous vehicle, wherein the vehicle controller is configured to perform the following operations: receiving raw data synchronously acquired by an onboard GNSS receiver, an IMU inertial measurement unit, a LiDAR laser radar, and a UWB positioning module; processing the raw data to extract preset features including GNSS carrier phase residuals, IMU acceleration variance, LiDAR point cloud matching residuals, and UWB ranging residuals, constructing an input feature vector of a preset dimension and standardizing it; and inputting the standardized input feature vector into a pre-trained K-nearest neighbor model to calculate the corresponding GNSS... The initial confidence levels of the receiver, IMU (Inertial Measurement Unit), LiDAR (Light Detection and Ranging) and UWB (Ultra-Wideband) positioning module are obtained. These initial confidence levels, along with the input feature vector, are fed into a pre-trained backpropagation neural network model to obtain a nonlinearly corrected sensor confidence vector, which is then used to generate a positioning mode recommendation classification. Based on this classification, the operating mode of the positioning system is adaptively selected. Using the extended Kalman filter algorithm, the observation noise covariance matrix is ​​dynamically constructed using the sensor confidence vector, and the Huber function is introduced to robustly weight the observation residuals for state updates. The final output is the positioning, speed, and heading status used for vehicle navigation control.

[0056] In some possible implementations, the multi-source adaptive fusion positioning method of this disclosure can be implemented as a program product including program code that, when the program product is run on a terminal device, causes the terminal device to perform the steps described in the "Exemplary Methods" section of this specification according to various exemplary embodiments of this disclosure.

[0057] The storage medium disclosed herein may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples (a non-exhaustive list) of readable storage media include: an electrical connection having one or more wires, a portable disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof.

[0058] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A multi-source adaptive fusion localization method, applied to autonomous vehicles, characterized in that, The vehicle's overall controller is configured to perform the following operations: S1. Receive raw data synchronously collected by the vehicle-mounted GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; S2. Process the raw data, extract preset features including GNSS carrier phase residual, IMU acceleration variance, LiDAR point cloud matching residual, and UWB ranging residual, construct an input feature vector of preset dimensions and standardize it; S3. Input the standardized input feature vector into the pre-trained K-nearest neighbor model to calculate the initial confidence level corresponding to the GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; S4. Input the initial confidence score and the input feature vector together into the pre-trained backpropagation neural network model to obtain the sensor confidence score vector after nonlinear correction, and generate the positioning pattern recommendation classification; S5. Based on the positioning mode, the system recommends classification and adaptively selects the working mode of the positioning system. Based on the extended Kalman filter algorithm, the system dynamically constructs the observation noise covariance matrix using the sensor confidence vector and introduces the Huber function to robustly weight the observation residuals to perform state updates. Finally, the system outputs the positioning, speed, and heading status for vehicle navigation control.

2. The multi-source adaptive fusion localization method according to claim 1, characterized in that, The specific steps of step S2 are as follows: S21. Extract the standard deviation of carrier phase residuals from the data collected by the received vehicle-mounted GNSS receiver. Pseudorange multipath error standard deviation L1 signal-to-noise ratio mean L1 signal-to-noise ratio standard deviation Ambiguity ratio Fuzzy fixed success rate Position accuracy factor ; S22. Extract the acceleration variance from the data collected by the received IMU inertial measurement unit. angular velocity variance Attitude drift rate ; S23. Extract the mean value of ICP matching residuals from the received LiDAR data. Valid matching point ratio Point cloud density ; S24. Extract the standard deviation of the ranging residual from the data collected by the received UWB positioning module. Mean signal strength ; S25. The extracted data is used as the original features and processed using the Z-score normalization method: in, Original features σ is the mean of each original feature on the historical dataset, and σ is the standard deviation of each original feature on the historical dataset.

3. The multi-source adaptive fusion localization method according to claim 2, characterized in that, The specific steps of step S3 are as follows: S31. Match the standardized input feature vector with a pre-established historical sample library containing historical data of typical park environment scenarios, and calculate the relationship between the current feature vector x and each historical sample in the historical sample library. European distance : Where D is the dimension of the feature vector; S32. Calculate each historical sample using inverse distance weighting. weight : Where ε is the correction coefficient; S33. Select the k nearest neighbors by Euclidean distance to form a neighbor set. ; S34. Based on Neighbor Set and neighbors Chinese historical samples weight The initial confidence levels of four sensors—GNSS receiver, IMU inertial measurement unit, LiDAR, and UWB positioning module—are calculated in parallel under the current environment to form an initial confidence vector. ; The initial confidence level for any sensor S is calculated using the following formula: In the formula, ∈{0,1} are the pre-labeled labels for sensor S in the historical sample database. 0 indicates that sensor S is reliable under the historical sample conditions, and 1 indicates that sensor S is unreliable or abnormal under the historical sample conditions. S represents GNSS receiver, IMU inertial measurement unit, LiDAR lidar, and UWB positioning module, respectively. S35. Output the initial confidence vector .

4. The multi-source adaptive fusion localization method according to claim 3, characterized in that, The specific steps of step S4 are as follows: S41. Initial confidence vector The input feature vector is concatenated with the standardized input feature vector to generate a comprehensive input vector; S42. Input the combined input vector into the pre-trained backpropagation neural network model, and output the following two vectors: Output the sensor confidence vector after nonlinear correction : Among them, each value ∈(0,1) represents the probability that the corresponding sensor S will have an observation anomaly or be unavailable; Output localization pattern recommendation probability vector ; in, The following four predefined positioning modes are used to represent the probability of the system making a recommendation: It is a tightly coupled GNSS / IMU mode, serving as the dominant mode when GNSS signals are reliable; It is a GNSS / IMU / LiDAR fusion mode, serving as an auxiliary enhancement mode when GNSS reliability declines; It is an IMU / LiDAR / UWB alternative mode, serving as a substitute for GNSS when GNSS is unreliable; It is a LiDAR / IMU combined mode, serving as an alternative mode to the no-GNSS mode when GNSS is unreliable.

5. The multi-source adaptive fusion localization method according to claim 4, characterized in that, The specific steps of step S42 are as follows: S421. Constructing a backpropagation neural network model: The input layer is configured with a number of nodes corresponding to the dimension of the comprehensive input vector. Construct an intermediate layer containing at least two fully connected hidden layers. Each intermediate layer uses the ReLU activation function for non-linear transformation, and the number of neurons in each fully connected hidden layer decreases from front to back, forming a feature compression structure. The output layer is configured with a dual-branch parallel output structure. The first output branch is configured with neurons corresponding to the number of sensors and uses the Sigmoid activation function to output the sensor confidence vector. The second output branch is configured with neurons corresponding to the number of localization modes and uses the Softmax activation function to output the localization mode recommendation probability vector. S422. Training the backpropagation neural network: Supervised training was performed using historical datasets covering typical scenarios in the park. A multi-task learning framework is adopted, and a composite loss function is defined as the total loss function: The binary cross-entropy loss function is applied to the first output branch to calculate the prediction error of the sensor anomaly probability; The classification cross-entropy loss function is applied to the second output branch to calculate the prediction error of the localization pattern classification; The total loss function is a weighted combination of the binary cross-entropy loss function and the branching function is a classification cross-entropy loss function; During network training, Dropout regularization and L2 weight regularization are applied to the hidden layers, the network parameters are optimized using the adaptive moment estimation algorithm, and an early stopping strategy is adopted to monitor the validation set loss. Training is terminated when the validation set loss no longer decreases within a preset number of rounds. S423. Input the combined input vector into the trained backpropagation neural network model; The sensor confidence vector is obtained by parsing from the first output branch, where each confidence value represents the probability of the corresponding sensor exhibiting an observation anomaly; The location mode recommendation probability vector is obtained from the second output branch, and the mode with the highest probability is selected as the recommended location mode. The sensor confidence vector and the positioning mode recommendation result are output together.

6. The multi-source adaptive fusion localization method according to claim 4, characterized in that, The specific steps of step S5 are as follows: S51. Based on the probability vector recommended by the positioning mode, select the positioning mode with the highest probability as the current working mode of the vehicle positioning system, so as to determine the sensor that adopts the observation data in the filtering. S52. Within the framework of extended Kalman filtering, using data acquired by the IMU (Inertial Measurement Unit), perform the state prediction step: State prediction: Covariance prediction: Where F is the state transition matrix based on the vehicle kinematics model; The control input, which serves as the motion increment, is obtained by integrating the data acquired by the IMU (Inertial Measurement Unit). is the process noise covariance matrix, used to characterize the uncertainty of the motion model; P is the error covariance matrix of the state estimation. The system state vector includes the vehicle's position, velocity, and heading angle in a two-dimensional plane; subscripts... This represents the estimation of the state vector at time k based on information from time k-1 and earlier. This represents the optimal state estimation result at time k-1; S53. Using the corrected sensor confidence vector Dynamically construct an adaptive observation noise covariance matrix for the currently activated sensor. To automatically reduce the weight of sensors with low confidence; S54. When observation data from GNSS receivers, LiDAR laser radars, or UWB positioning modules arrive, combine it with the adaptive observation covariance matrix. It performs observation updates that incorporate the Huber robust factor to suppress the impact of outlier observations and outputs the final vehicle state estimate.

7. In the multi-source adaptive fusion localization method according to claim 6, step S53 involves dynamically constructing an adaptive observation noise covariance matrix for the currently activated sensor. The specific steps are as follows: Based on the selected positioning mode, select a subset A of the four sensors that are activated; Construct an adaptive scaling vector s that corresponds to the dimension of the sensor subset A; in, For each sensor S in the sensor subset A, the corresponding scaling factor is For sensors that are not activated, they do not participate in the adaptive observation noise covariance matrix. The structure; This represents the confidence vector of sensor S; Adaptive observation noise covariance matrix The calculation method is as follows: in, This represents the Hadamard product, which is the product of elements. This is the preset benchmark observation noise covariance matrix corresponding to the sensor subset A.

8. The multi-source adaptive fusion localization method according to claim 6, characterized in that, The specific steps for implementing the observation update that introduces the Huber robust factor in step S54 are as follows: S541. Based on current observation data With state prediction value Calculate the following residual vector As a priori residual: in, The observation matrix; S542. For the residual vector Each component Calculate the corresponding Huber weight factor : ； in, For component thresholds, , For online estimation of the residual standard deviation; S543. Construct a diagonal weight matrix based on all calculated Huber weight factors. : ； S544. Using the diagonal weight matrix With adaptive observation covariance matrix Calculate the robust final observation covariance matrix. ; S545. Using a robust covariance matrix Calculate Kalman gain And through Kalman gain Update system state vector With covariance matrix This outputs the updated positioning, speed, and heading status.

9. A multi-source adaptive fusion positioning system for use in autonomous vehicles, characterized in that, include: The data acquisition module is used to receive raw data synchronously acquired by the vehicle-mounted GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; The feature extraction and processing module is used to process the raw data, extract preset features including GNSS carrier phase residual, IMU acceleration variance, LiDAR point cloud matching residual, and UWB ranging residual, construct an input feature vector of preset dimensions and standardize it; The initial confidence calculation module is used to input the standardized input feature vector into the pre-trained K-nearest neighbor model to calculate the initial confidence corresponding to the GNSS receiver, IMU inertial measurement unit, LiDAR lidar and UWB positioning module; The confidence correction and decision module is used to input the initial confidence and the input feature vector into the pre-trained backpropagation neural network model to obtain the sensor confidence vector after nonlinear correction, and generate a positioning pattern recommendation classification. The adaptive fusion positioning module is used to recommend classifications based on positioning modes and adaptively select the operating mode of the positioning system. Based on the extended Kalman filter algorithm, it uses sensor confidence vectors to dynamically construct the observation noise covariance matrix and introduces the Huber function to robustly weight the observation residuals to perform state updates. Finally, it outputs the positioning, speed and heading status for vehicle navigation control.

10. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the multi-source adaptive fusion localization method as described in any one of claims 1 to 8.

Images