Multi-source data fusion method based on AGV vehicle cooperative positioning

Abstract

The present application relates to the field of data processing, especially to a multi-source data fusion method based on AGV trolley cooperative positioning, which comprises: AGV trolley positioning original data acquisition and preprocessing through multi-source sensor fusion to obtain standardized characteristic value and prediction parameter for cooperative updating; noise factor optimization adjustment through fusion of physical propagation characteristics and ranging fluctuation information to obtain preliminary dynamic correction UWB measurement noise variance; deviation risk identification through risk factor adjustment based on innovation consistency test to obtain final dynamic correction UWB measurement noise variance; variance matrix is constructed through dynamic variance and filtering update is executed to obtain corrected AGV state estimation and variance; recursive input prediction process through optimized state estimation result to realize dynamic update of multi-AGV cooperative positioning, thereby enhancing the stability and overall performance of AGV positioning in dynamic environment.

Description

Technical Field

[0001] This invention relates to the field of data processing technology, and in particular to a multi-source data fusion method based on AGV (Automated Guided Vehicle) collaborative positioning. Background Technology

[0002] Automated Guided Vehicles (AGVs) play a central role in modern automation systems, and their operational efficiency and safety heavily rely on real-time, accurate self-positioning. To achieve this, AGVs typically integrate multiple sensors, such as wheeled odometers, inertial measurement units (IMUs), and lidar (LiDAR). However, single sensors have inherent limitations, such as accumulated errors in odometers, IMU drift, and performance degradation of LiDAR in specific environments. Therefore, employing multi-source data fusion technology has become a key means to improve AGV positioning accuracy. The Extended Kalman Filter (EKF) and its variants are currently the mainstream fusion framework widely used for state estimation of AGV nonlinear systems.

[0003] To further improve the overall performance of AGV clusters in complex environments (such as those with obstructions or dynamic changes), Cooperative Localization (CL) technology has been introduced. This technology achieves information sharing and mutual constraints through communication and relative measurement between AGVs. Ultra-wideband (UWB) technology, due to its excellent ranging accuracy and penetration, is often used as a key sensor for obtaining the relative distance between AGVs. In a typical EKF-based cooperative localization scheme, when UWB ranging data from other AGVs is received, this data is integrated into the EKF update step. Existing methods typically assign a fixed measurement noise variance to the UWB measurement in the EKF, calibrated under ideal conditions, or make limited adjustments based only on simple indicators such as Received Signal Strength Indication (RSSI), and use this variance to construct the measurement noise variance matrix.

[0004] However, in complex and dynamic indoor environments such as warehousing and manufacturing, the UWB signal propagation path between AGVs is easily obstructed by shelves (especially metal shelves), mobile devices, personnel, or other AGVs themselves, causing the signal to undergo non-line-of-sight (NLOS) propagation and strong multipath effects. In this situation, the actual measurement performance of UWB will deteriorate significantly, specifically manifested in the measured distance value... This often involves a significant positive bias, meaning the measured distance is much larger than the actual distance. Simultaneously, the random noise characteristics of the measurement also change drastically, with its variance typically increasing significantly and exhibiting time-varying characteristics. Existing techniques, which use fixed or simple metrics like RSSI to adjust the measurement noise variance, cannot accurately and in real-time capture and reflect the complex, dynamic, and often significantly positively biased statistical characteristics of UWB measurement errors caused by NLOS and strong multipath effects. The correlation between simple metrics like RSSI and NLOS states is not strong enough or timely enough in all scenarios, leading to a significant discrepancy between the model and the actual error situation. Therefore, when EKF fuses this NLOS with unknown bias and inaccurate noise models for UWB measurements, the filter may over-rely on or inappropriately process this bad data, severely contaminating the AGV's state estimation. This results in jumps in positioning results, a significant decrease in accuracy, and even filter divergence, ultimately severely damaging the accuracy and reliability of the entire AGV cooperative positioning system. How to enable EKF to intelligently and adaptively assess and process dynamically changing noise and potential biases in UWB cooperative ranging data under complex environments is a technical problem that urgently needs to be solved by existing technologies. Summary of the Invention

[0005] In view of this, the present invention aims to propose a multi-source data fusion method based on AGV cooperative positioning to solve the problem that when the extended Kalman filter fuses NLOS with unknown bias and inaccurate noise models for UWB measurement, the filter will over-rely on or inappropriately process these noisy data, resulting in serious contamination of AGV state estimation, causing abrupt changes in positioning results, significant decrease in accuracy, or even filter divergence.

[0006] To achieve the above objectives, the technical solution of the present invention is implemented as follows: A multi-source data fusion method based on AGV collaborative localization, the method comprising the following steps: Step S1: Collect and preprocess the raw data of AGV positioning through multi-source sensor fusion to obtain standardized feature values ​​and prediction parameters for collaborative updates; Step S2: By fusing physical propagation characteristics and ranging fluctuation information, the noise factor is optimized and adjusted to obtain the preliminary dynamically corrected UWB measurement noise variance; Step S3: Identify bias risks by adjusting risk factors based on the consistency test of information, and obtain the final dynamically corrected UWB measurement noise variance; Step S4: Construct the variance matrix through dynamic variance and perform filtering update to obtain the corrected AGV state estimate and variance; Step S5: By recursively inputting the optimized state estimation results into the prediction process, the dynamic update of the AGV collaborative positioning system is achieved.

[0007] Furthermore, based on the aforementioned acquisition and preprocessing of raw AGV vehicle positioning data through multi-source sensor fusion, standardized feature values ​​and prediction parameters for collaborative updates are obtained, specifically including: The AGV collects encoder readings from the left and right wheels using a wheel-type odometer; it also collects angular velocity and linear acceleration using an inertial measurement unit (IMU); when the AGV is within the communication range of other AGVs, it receives UWB signals from other AGVs via its UWB module and decodes them to obtain ranging data. Simultaneously, the UWB receiving module acquires received signal strength indication, channel impulse response data, the predicted state vector of the other AGV, and the predicted state variance matrix associated with the other AGV. The ratio of first-path power to total received power and the RMS spread delay are extracted from the channel impulse response data and normalized together with the received signal strength indication data to obtain the first first-path power to total received power ratio feature, the first RMS spread delay feature, and the first received signal strength indication feature. A time window is set, and the sample standard deviation of the ranging data is calculated based on the UWB ranging data sequence received within the most recent time window.

[0008] Furthermore, based on the aforementioned optimization and adjustment of the noise factor by fusing physical propagation characteristics and ranging fluctuation information, a preliminary dynamically corrected UWB measurement noise variance is obtained, specifically including: By comprehensively analyzing the degree to which the signal deviates from the ideal line-of-sight propagation, a first adjustment term is obtained to characterize the physical propagation quality of the signal; By evaluating the stability of the ranging data fluctuations within a short time window, a second adjustment term is obtained to reflect the trend of measurement stability changes. By integrating and evaluating the first and second adjustment terms, the first optimization factor for UWB measurement is obtained. The basic measurement noise variance is dynamically adjusted by using the first optimization factor of UWB measurement to obtain the preliminary dynamically corrected UWB measurement noise variance.

[0009] Furthermore, based on the aforementioned comprehensive analysis of the degree to which the signal deviates from the ideal line-of-sight propagation, a first adjustment term is obtained to characterize the physical propagation quality of the signal; and by evaluating the stability of the fluctuations in the ranging data within a short time window, a second adjustment term is obtained to reflect the trend of measurement stability changes, specifically including: The first characteristic of the ratio of the first path power to the total received power, the first characteristic of the RMS spread delay, and the first characteristic of the received signal strength indication are obtained. The first characteristic of the received signal strength indication is subtracted from a set characteristic threshold and scaled by a normalization factor. The maximum value after comparison with zero is selected as its deviation degree. The first evaluation factor is calculated by subtracting the ratio characteristic from 1. This first evaluation factor is added to the RMS spread delay characteristic and the deviation degree as the second evaluation factor. The second evaluation factor is multiplied by the positive adjustment coefficient and input into the exponential function mapping to obtain the first adjustment term. Obtain the set baseline distance measurement standard deviation and the distance measurement sample standard deviation within the current time window, calculate the ratio between the two and subtract 1, and take the maximum value between the ratio and zero as the deviation factor; multiply the deviation factor by the second positive adjustment coefficient and add 1 to obtain the second adjustment term.

[0010] Furthermore, based on the aforementioned method of fusing and evaluating the first and second adjustment terms, a first optimization factor for UWB measurement is obtained; the basic measurement noise variance is dynamically adjusted using the first optimization factor for UWB measurement to obtain a preliminary dynamically corrected UWB measurement noise variance, specifically including: Obtain a first adjustment term to characterize the physical propagation quality of the signal; obtain a second adjustment term to reflect the trend of measurement stability changes; use the calculation result of multiplying the first and second adjustment terms as the first optimization factor for UWB measurement; obtain the basic UWB measurement noise variance value preset by the system or obtained through offline calibration as the first basic ranging variance feature; use the calculation result of multiplying the first optimization factor for UWB measurement and the first basic ranging variance feature as the preliminary dynamically corrected UWB measurement noise variance.

[0011] Furthermore, based on the aforementioned risk factor adjustment using the information consistency test, the final dynamically corrected UWB measurement noise variance is obtained, specifically including: By analyzing the difference between the current UWB measurement and the predicted measurement in real time, new information data reflecting the degree of inconsistency between measurement and prediction can be obtained; By normalizing and comparing the deviation of the new information data with its expected statistical uncertainty, a normalized deviation index is obtained to identify potential positive deviation risks. By threshold determination and exponential adjustment of the normalized deviation index, the second optimization factor for UWB measurement is obtained; The UWB measurement noise variance, which was initially dynamically corrected, was optimized and adjusted using a second optimization factor to obtain the final dynamically corrected UWB measurement noise variance.

[0012] Furthermore, based on the aforementioned method of real-time analysis of the difference between current UWB measurements and predicted measurements, new information data reflecting the degree of inconsistency between measurements and predictions is obtained, specifically including: The real-time UWB ranging data of the AGV vehicle and the measurement prediction value calculated based on the EKF prediction state are obtained. The result of subtracting the real-time UWB ranging data of the AGV vehicle from the measurement prediction value calculated based on the EKF prediction state is used as information data reflecting the degree of inconsistency between measurement and prediction.

[0013] Furthermore, based on the above, by normalizing and comparing the deviation of the new information data with its expected statistical uncertainty, a normalized deviation index is obtained for identifying potential positive deviation risks, specifically including: Obtain the original variance of the innovation data, and use the result of adding the original variance of the innovation data to the variance of the UWB measurement noise with the initial dynamically corrected variance as the optimized variance of the innovation data. Obtain the standard deviation of the innovation data based on the optimized variance of the innovation data. The result of dividing the new information data by the standard deviation of the new information data is compared with the constant 0, and the larger value is selected as the normalized deviation index for identifying potential positive deviation risk.

[0014] Furthermore, based on the threshold determination and exponential adjustment of the normalized deviation index, a second optimization factor for UWB measurement is obtained; the UWB measurement noise variance after preliminary dynamic correction is optimized and adjusted using the second optimization factor to obtain the final dynamically corrected UWB measurement noise variance, specifically including: The process involves: obtaining a set positive normalized innovation threshold and a set third positive adjustment coefficient; obtaining a normalized deviation index for identifying potential positive bias risks; multiplying the third positive adjustment coefficient by the normalized deviation index and performing an exponential function mapping with the natural constant as the base, then subtracting the result from the constant 1 to obtain a first optimization evaluation factor; subtracting the normalized deviation index from the positive normalized innovation threshold and mapping the result through a unit step function, then using the mapping result as a second optimization evaluation factor; multiplying the first optimization evaluation factor by the second optimization evaluation factor and adding the result to the constant 1 to obtain a second optimization factor for UWB measurement; and multiplying the second optimization factor for UWB measurement by the initially dynamically corrected UWB measurement noise variance to obtain the final dynamically corrected UWB measurement noise variance.

[0015] Furthermore, based on the aforementioned method of constructing a variance matrix through dynamic variance and performing filtering updates, the corrected AGV state estimate and variance are obtained, specifically including: In the calculation of extended Kalman filtering, the predicted state variance is combined with the final dynamically corrected UWB measurement noise variance to recalculate the final innovation variance, and the Kalman gain is calculated based on the final innovation variance. The predicted state and predicted variance are updated by using the calculated Kalman gain and innovation data to obtain the optimal state estimate and updated state variance at the current time.

[0016] Compared with the prior art, the present invention has the following advantages: The multi-source data fusion method based on AGV cooperative positioning described in this invention overcomes the limitations of using a fixed measurement noise model in traditional extended Kalman filters. It intelligently evaluates the reliability of each measurement data point based on the propagation characteristics and measurement stability of the UWB ranging signal in the actual environment. By introducing two optimization factors—one focusing on the physical propagation path of the signal and the other on the fluctuation of the ranging time series—it dynamically constructs the measurement noise variance, achieving accurate modeling of measurement uncertainty. This allows the system to maintain high positioning robustness and estimation accuracy even in complex scenarios such as non-line-of-sight propagation, multipath interference, and environmental changes.

[0017] Furthermore, this invention introduces a strategy for identifying and suppressing potential ranging bias risks by integrating a prediction consistency verification mechanism within the fusion filter. For systematic positive biases that may occur under non-line-of-sight propagation, a risk avoidance mechanism with adjustable capabilities is constructed, effectively reducing the interference of abnormal measurements on state estimation. This mechanism not only enhances the intelligence of single measurement processing but also lays the foundation for reliable sharing of collaborative positioning information in multi-AGV systems, ultimately significantly improving the stability and overall performance of the group positioning system in dynamic environments. Attached Figure Description

[0018] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings: Figure 1 This is a flowchart of the multi-source data fusion method based on AGV vehicle collaborative positioning as described in an embodiment of the present invention. Detailed Implementation

[0019] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0020] In the description of this invention, it should be noted that the terms "upper," "lower," "inner," and "back," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing this invention and for simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention. Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0021] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0022] See Figure 1 This is a flowchart of a multi-source data fusion method based on AGV collaborative positioning provided in Embodiment 1 of the present invention, as follows: Figure 1 As shown, a multi-source data fusion method based on AGV collaborative localization can include: S1 uses multi-source sensor fusion to collect and preprocess the raw data for AGV positioning, obtaining standardized feature values ​​and prediction parameters for collaborative updates.

[0023] The AGV collects encoder readings from the left and right wheels via a wheel-type odometer; it also collects angular velocity and linear acceleration via an inertial measurement unit; and when the AGV is within the communication range of other AGVs, it receives ultra-wideband (UWB) signals from other AGVs via its UWB module and decodes them to obtain ranging data. Simultaneously, the UWB receiving module acquires Received Signal Strength Indication (RSSI), Channel Impulse Response (CIR) data, the predicted state vector of another AGV, and the predicted state variance matrix associated with the other AGV. From the CIR data, the ratio of first-path power to total received power and the RMS spread delay are calculated and extracted, and these are normalized together with the RSI data to obtain the first first-path power to total received power ratio feature, the first RMS spread delay feature, and the first RSI feature. A time window is set, and the sample standard deviation of the ranging data is calculated based on the UWB ranging data sequence received within the most recent time window.

[0024] S2, by integrating physical propagation characteristics and ranging fluctuation information to optimize and adjust the noise factor, obtains the preliminary dynamically corrected UWB measurement noise variance.

[0025] Existing technologies for processing ultra-wideband (UWB) cooperative ranging data in extended Kalman filters (EKFs) typically employ a fixed measurement noise variance calibrated under ideal line-of-sight (LOS) conditions, or rely solely on simple adjustments to the Received Signal Strength Indication (RSSI). However, in the complex and dynamic environments where AGVs actually operate (such as warehouses and factories), UWB signals are highly susceptible to obstruction, reflection, and diffraction from obstacles, experiencing non-line-of-sight (NLOS) propagation and multipath effects. These physical phenomena directly lead to drastic and dynamic changes in the actual quality of the UWB signal. Specifically, signal energy may diffuse over time (multipath effect), the energy of the first-path may be significantly reduced (NLOS), the signal waveform may be distorted, and the short-term fluctuations of the measured values ​​(i.e., noise levels) will also increase.

[0026] Using a fixed measurement noise variance implies that EKF incorrectly assumes the reliability of UWB measurements remains constant. When signal quality actually deteriorates (e.g., in NLOS or strong multipath environments), the true measurement uncertainty is far greater than the fixed measurement noise variance. If EKF continues to use this small value, it will over-rely on the poor-quality measurement data, causing it to have an excessive and inappropriate impact on state estimation, thus contaminating the filtering results. Conversely, if a large measurement noise variance is uniformly set for conservatism, the value of UWB measurements will be underestimated when signal quality is good (LOS), reducing the utilization of effective information. Relying solely on RSSI for adjustment also has limitations, because RSSI sometimes does not fully and accurately reflect the severity of NLOS or multipath; for example, multipath superposition can sometimes even lead to an increase in RSSI, but at the expense of decreasing ranging accuracy.

[0027] Therefore, to more accurately characterize the current actual uncertainty level of UWB measurements, their noise variance should be dynamically and adaptively adjusted based on richer information before being incorporated into EKF updates. This invention proposes that features that more directly reflect the physical condition of the signal propagation channel, along with the statistical properties of the measurement data itself, should be used to jointly assess the current signal quality and adjust the underlying measurement noise variance accordingly. Specifically, channel impulse response (CIR) information typically provided by modern UWB chips can be used to extract physical layer features such as the ratio of first-path power to total power (reflecting whether signal energy is concentrated) and RMS delay spread (reflecting the degree of multipath temporal dispersion). Simultaneously, RSSI is used as auxiliary information, and the statistical standard deviation of the UWB distance measurement value within the most recent short time window (directly reflecting current measurement volatility) is calculated. By fusing these multi-dimensional features that can reflect signal transmission quality and stability in real time, a comprehensive evaluation index can be constructed. When these metrics indicate a decline in signal quality (e.g., low first-path power share, large RMS delay spread, low RSSI, and large short-term fluctuations), the baseline measurement noise variance should be increased accordingly; conversely, when signal quality is good, the variance remains close to the baseline value. This method yields a preliminarily adjusted noise variance that better reflects the inherent random error level under current signal propagation conditions, providing a more realistic basis for measuring measurement uncertainty in subsequent EKF updates.

[0028] In summary, this invention obtains a first adjustment term characterizing the physical propagation quality of the signal by comprehensively analyzing the degree of deviation of the signal from the ideal line-of-sight propagation; obtains a second adjustment term reflecting the trend of measurement stability by evaluating the stability of the ranging data within a short time window; obtains a first optimization factor for UWB measurement by fusing and evaluating the first and second adjustment terms; and obtains a preliminary dynamically corrected UWB measurement noise variance by dynamically adjusting the basic measurement noise variance using the first optimization factor for UWB measurement.

[0029] Specifically, by comprehensively analyzing the degree to which the signal deviates from the ideal line-of-sight propagation, a first adjustment term is obtained to characterize the physical propagation quality of the signal. Furthermore, by evaluating the stability of the ranging data within a short time window, a second adjustment term is obtained to reflect the trend of measurement stability changes. This process specifically includes: The algorithm acquires the first first-path power to total received power ratio characteristic, the first RMS spread delay characteristic, the first received signal strength indication characteristic, a set first positive adjustment coefficient, a set threshold for the first received signal strength indication characteristic, and a normalization factor for the first received signal strength indication characteristic. It then divides the result of subtracting the threshold from the first received signal strength indication characteristic by the normalization factor and compares it with a constant 0, selecting the maximum value as the deviation of the first received signal strength indication characteristic. Finally, it subtracts a constant 1 from the first first-path power to total received power ratio characteristic as the first evaluation factor for the first adjustment term. The algorithm adds the first evaluation factor, the first RMS spread delay characteristic, and the deviation of the first received signal strength indication characteristic as the second evaluation factor for the first adjustment term. Finally, it maps the result of multiplying the positive adjustment coefficient by the second evaluation factor using an exponential function with the natural constant e as the base, as the first adjustment term used to characterize the physical propagation quality of the signal.

[0030] In one embodiment, it is assumed that the ratio of the first-path power to the total received power is characterized as follows: The first RMS extended delay characteristic is The first received signal strength indication feature is: The first positive adjustment coefficient is Then, the calculation expression for the first adjustment term, used to characterize the physical propagation quality of the signal, is: in, This represents the first adjustment term used to characterize the physical propagation quality of the signal; This represents the first positive adjustment coefficient, which is set in the implementation of this invention. It can be adjusted according to the actual scenario; no specific requirements are imposed. This indicates the characteristic of the ratio of the first-path power to the total received power; This indicates the first RMS extended delay characteristic; , This indicates the first received signal strength indicator threshold. denoted by , which represents the normalization factor for the first received signal strength indication characteristic; e represents the natural constant e.

[0031] It should be noted that in this formula, and It directly quantifies the degree to which the signal deviates from the ideal state. The function is used to quantify the degree to which the RSSI deviates from its normal range (when... (Its value increases when it falls below a threshold). These indicators of quality degradation are accumulated and then adjusted by a positive control coefficient. Controlling the overall sensitivity and substituting it into an exponential function has the effect of addressing adverse physical propagation paths of the signal ( Lower higher When the value is low, the value of the exponent term increases sharply, leading to A value significantly greater than 1 significantly increases the noise variance estimate, effectively reducing the EKF's confidence in the low-quality signal. This directly compensates for the deficiency in existing technologies that neglect signal physical layer information, leading to variance model mismatch. (Coefficients) It can be calibrated through system testing to adapt to specific hardware and environmental characteristics.

[0032] After obtaining the first adjustment term used to characterize the physical propagation quality of the signal, the system continues to obtain the standard deviation of the baseline UWB measurement noise, either preset by the system or obtained through offline calibration, as the first baseline ranging standard deviation feature; the sample standard deviation of the ranging data calculated based on the UWB ranging data sequence received within the most recent time window is obtained as the first ranging standard deviation feature; the set second positive adjustment coefficient is obtained; the first ranging standard deviation feature is divided by the first baseline ranging standard deviation feature to obtain the relative multiple between the current ranging fluctuation level and the baseline fluctuation level, as the fluctuation amplification factor index; the calculation result of subtracting a constant 1 from the fluctuation amplification factor index is compared with a constant 0, and the maximum value of the two is taken as the deviation factor of the fluctuation exceeding the baseline level; the deviation factor of the fluctuation exceeding the baseline level is multiplied by the positive adjustment coefficient to obtain the weight term of the ranging stability deviation degree; the calculation result of adding the constant 1 and the weight term of the ranging stability deviation degree is used as the second adjustment term used to characterize the trend of ranging stability change.

[0033] In one implementation, it is assumed that the first basic ranging standard deviation characteristic is: The first characteristic of the standard deviation of the ranging measurement is: The second positive adjustment coefficient is Then, the calculation expression for the second adjustment term, used to characterize the trend of change in ranging stability, is: in, This represents the second adjustment term used to characterize the trend of change in ranging stability. This represents the second positive adjustment coefficient, which is set in this embodiment of the invention. It can be adjusted according to the actual scenario; no specific requirements are imposed. Indicates the characteristic of the first distance measurement standard deviation; This represents the characteristic of the first basic distance measurement standard deviation.

[0034] It should be noted that in this formula, This indicates the ratio of the current volatility to the benchmark volatility. Only if this ratio is greater than [a certain value] (i.e., when the current fluctuation is abnormally large) If the term is positive, it is adjusted by a positive adjustment coefficient. Control its contribution to the adjustment items. The form ensures that the item is at least It will not reduce variance without reason. It provides direct statistical evidence to support the adjustment of noise variance, and can respond to measurement instabilities caused by factors that are not fully captured by physical layer features (such as transient interference), thus improving the comprehensiveness of variance estimation.

[0035] After obtaining the first adjustment term for characterizing the physical propagation quality of the signal and the second adjustment term for characterizing the trend of ranging stability changes, the first and second adjustment terms can be fused and evaluated to obtain the first optimization factor for UWB measurement. This first optimization factor is then used to dynamically adjust the basic measurement noise variance, resulting in a preliminary dynamically corrected UWB measurement noise variance. This process specifically includes: Obtain a first adjustment term to characterize the physical propagation quality of the signal; obtain a second adjustment term to reflect the trend of measurement stability changes; use the calculation result of multiplying the first and second adjustment terms as the first optimization factor for UWB measurement; obtain the basic UWB measurement noise variance value preset by the system or obtained through offline calibration as the first basic ranging variance feature; use the calculation result of multiplying the first optimization factor for UWB measurement and the first basic ranging variance feature as the preliminary dynamically corrected UWB measurement noise variance.

[0036] Thus, by integrating physical propagation characteristics and ranging fluctuation information to optimize and adjust the noise factor, a preliminary dynamically corrected UWB measurement noise variance is obtained.

[0037] S3 identifies deviation risks by adjusting risk factors based on the consistency test of information, and obtains the final dynamically corrected UWB measurement noise variance.

[0038] In step S2, the UWB measurement noise variance is initially adjusted using the first optimization factor, resulting in a preliminary dynamically corrected UWB measurement noise variance. This adjustment is primarily based on the physical characteristics and short-term statistical stability of the signal, aiming to make the variance more accurately reflect the inherent random noise level under the current signal propagation conditions. However, simply adjusting the magnitude of the random noise variance does not completely solve the significant positive measurement bias problem that often accompanies UWB under NLOS conditions. The preliminary dynamically corrected UWB measurement noise variance calculated in step S2 mainly reflects the random fluctuation amplitude of the UWB measurement value itself, i.e., the noise level. While it addresses the issue of magnitude, it doesn't directly solve another problem under NLOS conditions: the systematic positive measurement bias. The bias is primarily positive because UWB ranging is based on signal transmission time. In NLOS, signals cannot propagate in a straight line; they must travel longer paths, such as through reflection and diffraction, to reach the receiver. Longer propagation paths inevitably lead to longer transmission times, resulting in calculated distances always exceeding the actual straight-line distance. Even if this positive bias caused by the path length is significant, the random jitter of the measurement signal itself (i.e., the portion quantified by the initial dynamically corrected UWB measurement noise variance) is not severe. In this situation, the value of the initial dynamically corrected UWB measurement noise variance alone cannot fully reflect the overall severity of the deviation from the true distance, and the hidden systematic positive bias is also not effectively measured.

[0039] The Extended Kalman Filter (EKF) framework itself compares actual measurements. And prediction based on the current state Derived measurement predictions (Right now The update is performed using ) and the difference between the two is the new information. This is crucial for EKF to determine whether measurements and predictions are consistent. In UWB cooperative localization scenarios, a significantly positive news item... (Right now Much larger This is a significant indication of positive bias in NLOS. If only the preliminary dynamically corrected UWB measurement noise variance obtained in step S2 is used as the measurement uncertainty, the Kalman gain calculated by EKF will still be large when the preliminary dynamically corrected UWB measurement noise variance fails to fully reflect the risk of this potential bias. This results in a measurement with significant positive bias. State estimation This creates excessive stretching, causing it to deviate from its true state.

[0040] Therefore, a mechanism based on EKF internal consistency testing is needed to identify and suppress the destructive effect of positive bias caused by NLOS on the filtering results.

[0041] After obtaining the initial dynamically corrected UWB measurement noise variance, this invention further evaluates the second optimization factor based on the specific value of the current innovation and the EKF's own estimate of the uncertainty of the innovation, thereby further optimizing the initial dynamically corrected UWB measurement noise variance.

[0042] In summary, this invention obtains information data reflecting the degree of inconsistency between measurement and prediction by real-time analysis of the difference between current and predicted UWB measurements; it obtains a normalized deviation index for identifying potential positive bias risks by normalizing and comparing the deviation of the information data with its expected statistical uncertainty; it obtains a second optimization factor for UWB measurement by threshold determination and exponential adjustment of the normalized deviation index; and it optimizes and adjusts the initially dynamically corrected UWB measurement noise variance using the second optimization factor to obtain the final dynamically corrected UWB measurement noise variance.

[0043] Specifically, by performing real-time analysis of the differences between current and predicted UWB measurements, new information reflecting the degree of inconsistency between measurements and predictions is obtained, including: The real-time UWB ranging data of the AGV vehicle and the measurement prediction value calculated based on the EKF prediction state are obtained. The result of subtracting the real-time UWB ranging data of the AGV vehicle from the measurement prediction value calculated based on the EKF prediction state is used as information data reflecting the degree of inconsistency between measurement and prediction.

[0044] Subsequently, by normalizing and comparing the deviation of the new information data with its expected statistical uncertainty, a normalized deviation index is obtained to identify potential positive deviation risks, specifically including: Obtain the original variance of the innovation data, and use the result of adding the original variance of the innovation data to the variance of the UWB measurement noise with the initial dynamically corrected variance as the optimized variance of the innovation data. Obtain the standard deviation of the innovation data based on the optimized variance of the innovation data. The result of dividing the new information data by the standard deviation of the new information data is compared with the constant 0, and the larger value is selected as the normalized deviation index for identifying potential positive deviation risk.

[0045] In one implementation, it is assumed that the standard deviation of the new information data is The new information is The formula for calculating the normalized deviation index used to identify potential positive deviation risk is: in, This represents the normalized deviation index used to identify potential positive deviation risks; Indicates new information data; This represents the standard deviation of the new information data.

[0046] It should be noted that, The value represents the actual measured value. Compared to the predicted value How many standard deviations larger (considering only positive deviations). When When the value is significantly greater than a certain threshold, it indicates that the current measurement is very likely to contain an unacknowledged value. Significant positive bias fully explained.

[0047] After obtaining the normalized deviation index used to identify potential positive bias risks, a second optimization factor for UWB measurement can be obtained by threshold determination and exponential adjustment of the normalized deviation index. This second optimization factor is then used to optimize and adjust the initially dynamically corrected UWB measurement noise variance, resulting in the final dynamically corrected UWB measurement noise variance. This process specifically includes: The process involves: obtaining a set positive normalized innovation threshold and a set third positive adjustment coefficient; obtaining a normalized deviation index for identifying potential positive bias risks; multiplying the third positive adjustment coefficient by the normalized deviation index and performing an exponential function mapping with the natural constant as the base, then subtracting the result from the constant 1 to obtain a first optimization evaluation factor; subtracting the normalized deviation index from the positive normalized innovation threshold and mapping the result through a unit step function, then using the mapping result as a second optimization evaluation factor; multiplying the first optimization evaluation factor by the second optimization evaluation factor and adding the result to the constant 1 to obtain a second optimization factor for UWB measurement; and multiplying the second optimization factor for UWB measurement by the initially dynamically corrected UWB measurement noise variance to obtain the final dynamically corrected UWB measurement noise variance.

[0048] In one embodiment, it is assumed that the third positive adjustment coefficient is The normalized innovation threshold is The expression for calculating the second optimization factor of UWB measurement is: in, This represents the second optimization factor for UWB measurements; This represents the third positive adjustment coefficient, used to control the intensity of risk aversion, with an initial value of [value missing]. This value makes when When the value is 3 (exceeding the threshold by 1 standard deviation), With a factor of approximately 20, it effectively suppresses suspicious data. This represents the normalized deviation index used to identify potential positive deviation risks; This represents the positively normalized information threshold. This represents the unit step function.

[0049] It should be noted that, It is a positive adjustment coefficient, controlling when After exceeding the threshold, Factor The rate of growth is exponential. The higher the value, the stronger the risk aversion. It is a positive normalized information threshold (set to 1, 2, or 3, indicating how many standard deviations the measured value is allowed to exceed the predicted value within which it is considered normal fluctuation). It is a unit step function: when hour, ;when hour, . Ensure that When the value is 0, the item is 0. Its function is only when the normalized positive information is updated. Exceeding the threshold Only when the step function is At this time, the exponential growth term It has only just begun to take effect, making Greater than 1. If Not exceeding the threshold Then the step function is 0, leading to Its function is to ensure that the internal consistency test of EKF does not reveal any significant suspicion of positive deviation. ), optimization factor At this point, the final noise variance This is equivalent to the initial adjustment. This maintains the adjustment results from step S2 based on signal quality. However, once an unexpected positive innovation is detected (…), ), indicating the current measurement It is highly likely to be contaminated by NLOS positive bias, at this time greater than And with the degree of deviation The variance increases exponentially with the increase in noise. This leads to a sharp amplification of the final dynamically corrected UWB measurement noise variance. In the EKF update step, this extremely large final dynamically corrected UWB measurement noise variance will significantly reduce the Kalman gain, thereby greatly suppressing the impact of this highly questionable measurement data on state estimation and effectively avoiding the risk of positioning errors caused by potential biases.

[0050] This step, by introducing a second optimization factor for UWB measurement, further enhances the ability to identify and suppress common positive deviations in UWB measurement, building upon the initial adaptive adjustment of noise levels in step S2. By combining external signal feature evaluation and EKF internal consistency verification, the final dynamically corrected UWB measurement noise variance used in the EKF more comprehensively reflects the true uncertainty of the current UWB measurement, thereby significantly improving the accuracy of the AGV cooperative positioning system in complex NLOS environments.

[0051] Thus, by adjusting the risk factors based on the consistency test of the information, the deviation risk is identified, and the final dynamically corrected UWB measurement noise variance is obtained.

[0052] S4. Construct the variance matrix through dynamic variance and perform filtering update to obtain the corrected AGV state estimate and variance.

[0053] In the calculation of extended Kalman filtering, the predicted state variance is combined with the final dynamically corrected UWB measurement noise variance to recalculate the final innovation variance, and the Kalman gain is calculated based on the final innovation variance. The predicted state and predicted variance are updated by using the calculated Kalman gain and innovation data to obtain the optimal state estimate and updated state variance at the current time.

[0054] S5 achieves dynamic updates of the multi-AGV collaborative positioning system by recursively inputting optimized state estimation results into the prediction process.

[0055] After the innovative two-step dynamic adjustment of the UWB measurement noise covariance described above, and its application to the EKF measurement update, the process at time [time value missing] is completed. An optimized estimate of its own state is obtained using UWB cooperative ranging information. The updated state and variance will be used as the next time step. The input to the EKF prediction step is used to achieve recursive state estimation.

[0056] The method proposed in this invention intelligently evaluates the quality and potential bias risk of each UWB collaborative measurement data point and dynamically adjusts its corresponding measurement noise variance accordingly. This addresses the limitation of fixed noise models in adapting to changes in signal quality and further identifies and suppresses the interference of positive biases caused by NLOS and other factors on state estimation. This makes the final EKF update more robust in handling the challenges of UWB signal propagation in complex dynamic environments. When multiple or all AGVs in an AGV cluster adopt the method of this invention, by sharing more reliable state estimation and processing more accurate relative measurement information, the positioning performance of the entire cluster based on wheel odometer, IMU, and UWB collaborative measurement—including positioning accuracy, stability, and adaptability to NLOS environments—will be significantly improved. This effectively solves the problem of performance degradation in collaborative positioning systems caused by improper handling of UWB measurement errors in the prior art.

[0057] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A multi-source data fusion method based on AGV collaborative positioning, characterized in that, The method includes the following steps: Step S1: Collect and preprocess the raw data of AGV positioning through multi-source sensor fusion to obtain standardized feature values ​​and prediction parameters for collaborative updates; Step S2: By fusing physical propagation characteristics and ranging fluctuation information, the noise factor is optimized and adjusted to obtain the preliminary dynamically corrected UWB measurement noise variance; Step S3: Identify bias risks by adjusting risk factors based on the consistency test of information, and obtain the final dynamically corrected UWB measurement noise variance; Step S4: Construct the variance matrix through dynamic variance and perform filtering update to obtain the corrected AGV state estimate and variance; Step S5: By recursively inputting the optimized state estimation results into the prediction process, the dynamic update of the multi-AGV collaborative positioning system is achieved; The process of optimizing and adjusting the noise factor by fusing physical propagation characteristics and ranging fluctuation information to obtain a preliminary dynamically corrected UWB measurement noise variance specifically includes: obtaining a first adjustment term to characterize the physical propagation quality of the signal by comprehensively analyzing the degree of signal deviation from the ideal line-of-sight propagation; obtaining a second adjustment term to reflect the trend of measurement stability changes by evaluating the stability of ranging data fluctuations within a short time window; obtaining a first optimization factor for UWB measurement by fusing and evaluating the first and second adjustment terms; and dynamically adjusting the basic measurement noise variance of the UWB module using the first optimization factor for UWB measurement to obtain a preliminary dynamically corrected UWB measurement noise variance. The process of obtaining the first adjustment term includes: obtaining the first first-path power to total received power ratio feature, the first RMS spread delay feature, and the first received signal strength indication feature; subtracting the first received signal strength indication feature from a set feature threshold, and scaling it by a normalization factor, selecting the maximum value compared with zero as its deviation degree; calculating a constant 1 minus the ratio feature as a first evaluation factor, and adding the first evaluation factor to the RMS spread delay feature and the deviation degree as a second evaluation factor; multiplying the second evaluation factor by a positive adjustment coefficient and inputting it into an exponential function mapping to obtain the first adjustment term; The process of obtaining the second adjustment term includes: obtaining the set baseline ranging standard deviation and the ranging sample standard deviation within the current time window, calculating the ratio between the two and subtracting 1, and taking the maximum value with zero as the deviation factor; multiplying the deviation factor by the second positive adjustment coefficient and adding 1 to obtain the second adjustment term; The process of obtaining the preliminary dynamically corrected UWB measurement noise variance includes: multiplying the first adjustment term and the second adjustment term to obtain the calculation result as the first optimization factor of UWB measurement; obtaining the basic UWB measurement noise variance value preset by the system or obtained through offline calibration as the first basic ranging variance feature; and multiplying the first optimization factor of UWB measurement and the first basic ranging variance feature as the preliminary dynamically corrected UWB measurement noise variance.

2. The multi-source data fusion method based on AGV vehicle cooperative positioning according to claim 1, characterized in that, Based on the aforementioned acquisition and preprocessing of raw AGV vehicle positioning data through multi-source sensor fusion, standardized feature values ​​and prediction parameters for collaborative updates are obtained, specifically including: The AGV collects encoder readings from the left and right wheels using a wheel-type odometer; it also collects angular velocity and linear acceleration using an inertial measurement unit (IMU); when the AGV is within the communication range of other AGVs, it receives UWB signals from other AGVs via its UWB module and decodes them to obtain ranging data. The UWB receiving module simultaneously acquires received signal strength indication and channel impulse response data related to this measurement, the predicted state vector of another AGV, and the predicted state variance matrix associated with that AGV. The ratio of first-path power to total received power and the RMS spread delay are extracted from the channel impulse response data, and these are normalized together with the received signal strength indication data to obtain the first first-path power to total received power ratio feature, the first RMS spread delay feature, and the first received signal strength indication feature. A time window is set, and the sample standard deviation of the ranging data and the basic measurement noise variance of the UWB module are calculated based on the UWB ranging data sequence received within the most recent time window.

3. The multi-source data fusion method based on AGV vehicle cooperative positioning according to claim 1, characterized in that, Based on the aforementioned risk factor adjustment using the information consistency test, the final dynamically corrected UWB measurement noise variance is obtained, specifically including: By analyzing the difference between the current UWB measurement and the predicted measurement in real time, new information data reflecting the degree of inconsistency between measurement and prediction can be obtained; By normalizing and comparing the deviation of the new information data with its expected statistical uncertainty, a normalized deviation index is obtained to identify potential positive deviation risks. By threshold determination and exponential adjustment of the normalized deviation index, the second optimization factor for UWB measurement is obtained; The UWB measurement noise variance, which was initially dynamically corrected, was optimized and adjusted using a second optimization factor to obtain the final dynamically corrected UWB measurement noise variance.

4. The multi-source data fusion method based on AGV vehicle cooperative positioning according to claim 3, characterized in that, Based on the aforementioned method of real-time analysis of the difference between current UWB measurements and predicted measurements, new information data reflecting the degree of inconsistency between measurements and predictions is obtained, specifically including: The real-time UWB ranging data of the AGV vehicle and the measurement prediction value calculated based on the EKF prediction state are obtained. The result of subtracting the real-time UWB ranging data of the AGV vehicle from the measurement prediction value calculated based on the EKF prediction state is used as information data reflecting the degree of inconsistency between measurement and prediction.

5. The multi-source data fusion method based on AGV vehicle cooperative positioning according to claim 3, characterized in that, Based on the above, a normalized deviation index is obtained by normalizing and comparing the deviation of the new information data with its expected statistical uncertainty, which is used to identify potential positive deviation risks. Specifically, it includes: Obtain the original variance of the innovation data, and use the result of adding the original variance of the innovation data to the variance of the UWB measurement noise with the initial dynamically corrected variance as the optimized variance of the innovation data. Obtain the standard deviation of the innovation data based on the optimized variance of the innovation data. The result of dividing the new information data by the standard deviation of the new information data is compared with the constant 0, and the larger value is selected as the normalized deviation index for identifying potential positive deviation risk.

6. The multi-source data fusion method based on AGV vehicle cooperative positioning according to claim 3, characterized in that, According to the above, by performing threshold determination and exponential adjustment on the normalized deviation index, the second optimization factor for UWB measurement is obtained; The initial dynamically corrected UWB measurement noise variance is optimized and adjusted using a second optimization factor to obtain the final dynamically corrected UWB measurement noise variance, which specifically includes: The process involves: obtaining a set positive normalized innovation threshold and a set third positive adjustment coefficient; obtaining a normalized deviation index for identifying potential positive bias risks; multiplying the third positive adjustment coefficient by the normalized deviation index and performing an exponential function mapping with the natural constant as the base, then subtracting the result from a constant 1 to obtain a first optimization evaluation factor; subtracting the normalized deviation index from the positive normalized innovation threshold and mapping the result through a unit step function, then using the mapping result as a second optimization evaluation factor; multiplying the first optimization evaluation factor by the second optimization evaluation factor and adding the result to a constant 1 to obtain a second optimization factor for UWB measurement; and multiplying the second optimization factor for UWB measurement by the initially dynamically corrected UWB measurement noise variance to obtain the final dynamically corrected UWB measurement noise variance.

7. The multi-source data fusion method based on AGV vehicle cooperative positioning according to claim 1, characterized in that, Based on the aforementioned method of constructing a variance matrix through dynamic variance and performing filtering updates, the corrected AGV state estimate and variance are obtained, specifically including: In the calculation of extended Kalman filtering, the predicted state variance is combined with the final dynamically corrected UWB measurement noise variance to recalculate the final innovation variance, and the Kalman gain is calculated based on the final innovation variance. The predicted state and predicted variance are updated by using the calculated Kalman gain and innovation data to obtain the optimal state estimate and updated state variance at the current time.

Images