UWB-IMU-ODOM sequential fusion positioning algorithm based on AUKF, storage medium and equipment

Abstract

The invention discloses a UWB-IMU-ODOM sequential fusion positioning algorithm based on AUKF, a storage medium and equipment, and relates to the technical field of indoor positioning, and the algorithm comprises the steps: completing the preliminary calculation of UWB data and a track plotting result in respective time steps; performing state prediction by a central UKF (Unscented Kalman Filter) based on a CTRV motion model, and eliminating abnormal measurement values by comparing a residual range with a chi-square distribution value so as to reduce the influence of noise peaks and burst errors on the positioning precision; through an adaptive noise adjustment mechanism, a sliding window is adopted to calculate the moving average of the residual error, and the noise covariance is dynamically adjusted according to the residual error change amplitude to adapt to the change of noise characteristics in a complex dynamic environment. A centralized sequential fusion framework is adopted, independence of UWB and IMU / ODOM data is reserved, sensor data at different moments are fused through a central UKF filter, and the method has low calculation complexity and good robustness and is suitable for an embedded system with limited resources.

Description

Technical Field

[0001] The present invention relates to the field of indoor positioning technology, and in particular to an AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm. Background Art

[0002] As building sizes expand, the demand for indoor positioning technology in areas like underground garages and warehouses is increasing. Ultrawideband (UWB) technology, which precisely transmits and receives radio pulses, is widely used in robotic positioning due to its high resolution and low cost. However, UWB positioning systems can suffer from factors such as non-line-of-sight (NLOS) in complex environments, leading to reduced positioning accuracy and stability.

[0003] When UWB sensors are obstructed, an inertial measurement unit (IMU) and wheel-mounted odometry (ODOM) can form a dead-reckoning system, providing high-precision position and pose information in a short period of time. However, the accuracy of an IMU decays over time, and ODOM errors can accumulate due to factors such as measurement errors, actuator errors, and sideslip, affecting positioning accuracy. Multi-sensor fusion technology, however, combines the advantages of three low-cost sensors: UWB, IMU, and ODOM, providing a low-cost and effective solution for mobile robot positioning in complex indoor environments. Furthermore, compared with other fusion algorithms, centralized sequential fusion, which updates the system state using data from each sensor in a chronological order, offers low computational complexity and good robustness, making it suitable for resource-constrained embedded systems. However, while existing multi-sensor fusion approaches improve positioning accuracy, UWB signals are susceptible to interference in complex and dynamic environments, resulting in reduced positioning accuracy. Furthermore, errors in IMUs and ODOMs accumulate over time, affecting positioning stability. Furthermore, existing fusion algorithms struggle with time-varying noise and are unable to dynamically adjust the noise covariance matrix, limiting the robustness and accuracy of the filter. Summary of the Invention

[0004] The present invention aims to provide an AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm to address the prior art issues presented in the aforementioned background art, such as the susceptibility of UWB signals to interference in complex dynamic environments, resulting in decreased positioning accuracy. Errors in the IMU and ODOM accumulate over time, affecting positioning stability. Furthermore, the algorithm cannot dynamically adjust the noise covariance matrix when processing time-varying noise, resulting in limited filter robustness and accuracy.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a UWB-IMU-ODOM sequential fusion positioning algorithm based on AUKF, comprising:

[0006] The UWB data and dead reckoning results are initially solved within their respective time steps;

[0007] The central UKF filter performs state prediction based on the CTRV motion model and eliminates abnormal measurement values by comparing the residual range with the chi-square distribution value to reduce the impact of noise spikes and burst errors on positioning accuracy;

[0008] Through the adaptive noise adjustment mechanism, a sliding window is used to calculate the moving average of the residuals, and the noise covariance is dynamically adjusted according to the amplitude of the residual changes to adapt to the changes in noise characteristics in complex dynamic environments.

[0009] Preferably, the specific step of dead reckoning result is S1 based on the motion state equation of CTRV; specifically, it is as follows:

[0010] The target turning rate and speed are kept constant by setting the CTRV model in the secondary motion model; the CTRV model uses the following state variables:

[0011]

[0012] Among them, x and y represent the position coordinates of the target in the two-dimensional plane; v represents the target's movement speed; φ represents the target's heading angle, that is, the target's current movement direction; Indicates the target's turning rate, i.e., angular velocity;

[0013] Set the time corresponding to the discrete time step k to t k , the time corresponding to time step k+1 is t k+1 , the time difference between the two is Δt=t k+1 -t k ; Considering the influence of noise vector on the whole process, then When , the state transition formula of the CTRV model can be expressed as:

[0014]

[0015] when When , x and y in the original state transfer formula will tend to infinity, making the formula unusable; but the actual motion of the target is linear motion, so the state transfer formula can be simplified to a constant speed model:

[0016]

[0017] Among them, the velocity and angular velocity in the CTRV model are considered constant, and the higher-order quantity is used as the noise disturbance term, and the linear acceleration v is set a,k and yaw acceleration The mean is 0 and the variances are and Gaussian distribution.

[0018] Preferably, the specific steps of UWB data are the S2 system measurement equation; specifically as follows:

[0019] The two-dimensional position component (x) is directly extracted from the state vector by the UWB measurement model. k ,y k ), we get the linear mapping relationship from the system state to the measurement space:

[0020] Z k =HX k +W k

[0021] Among them, the measurement vector Measurement Matrix Measurement noise Measurement noise covariance matrix σ x 2 , σ y 2 Represents the measurement variance of UWB in the X and Y directions respectively; the dead reckoning measurement model composed of IMU and odometer is mainly responsible for the continuity estimation of short-term motion; the measurement vector contains three components: path length ρ, heading angle φ, path speed The measurement equation of the system can be expressed as:

[0022] Z k =h(X k )+W k

[0023] Among them, the measurement vector Nonlinear measurement function Measurement noise Measurement noise covariance matrix σ ρ 2 , σ φ 2 、 represent the variance of the measurement noise of path length, heading angle, and path velocity, respectively, and they are independent of each other.

[0024] Preferably, the state prediction by the central UKF filter based on the CTRV motion model and the elimination of abnormal measurement values by comparing the residual range with the chi-square distribution value specifically include the following steps:

[0025] S3 Unscented Kalman Filter based on Sequential Fusion: The distribution of the system state X is set to Gaussian distribution UKF constructs a Gaussian distribution that approximates the true distribution through sampling points selected by unscented transformation to achieve state estimation; UKF sampling points are distributed around the state mean, and sigma points are generated by the following formula:

[0026] χ (0) =μ

[0027]

[0028] Where χ represents the sigma point matrix, the first point is the mean μ, and the remaining sigma points are scaled according to the scale of the covariance matrix Σ; λ is an adjustable expansion coefficient used to control the distribution range of the sigma points;

[0029] Each sigma point is assigned a weight according to its importance in the distribution. The weight calculation formula is as follows:

[0030]

[0031] Among them, w m is the mean weight, the sum of all representative point weights is equal to 1; w c is the mean square error weight; λ = α 2 (n+k)-n is the diffusion parameter, α controls the distribution range of the points, k is used for further fine-tuning, and β is used to introduce prior knowledge of the distribution, constituting the four key parameters for adjusting the unscented transform;

[0032] The generated sigma point is mapped to the current moment through the state transfer equation to obtain the state sigma point at the predicted moment:

[0033]

[0034] Combined with process noise:

[0035]

[0036] Get the mean and covariance of the state prediction values:

[0037]

[0038] By integrating the state transition model and process noise, the prior estimation of the system state is completed.

[0039] Preferably, to further improve the accuracy of state estimation, the predicted system state is corrected in combination with the sensor's measurement value, the steps are as follows:

[0040] Map the state sigma point to the measurement space through the measurement equation h(x) to obtain the measurement sigma point

[0041]

[0042] Combined with the measurement noise covariance matrix R k , get the mean of the measured predicted values Covariance S k and the covariance matrix P between the state prediction and the measurement prediction xz :

[0043]

[0044] Calculate the Kalman gain K k :

[0045]

[0046] During each update of sequential fusion, only the current sensor's measurement value and the state estimate of the previous moment are combined to update the state, and the state mean and covariance update of sensor i are obtained:

[0047]

[0048] Get the global state estimate of the central processor at time k+1 and the estimated error covariance matrix P k+1|N ; The predicted state and measurement information are integrated to complete the state estimation at the current moment.

[0049] Preferably, the adaptive noise adjustment mechanism includes:

[0050] S4 Adaptive Noise Processing: Adopts an adaptive noise adjustment method based on the statistical characteristics of residuals to improve the robustness and accuracy of the filter by dynamically adjusting the noise covariance matrix; and

[0051] S5 outlier removal: Calculate using the Mahalanobis distance method to avoid scale inconsistencies that may occur when relying solely on Euclidean distance.

[0052] Preferably, the S4 adaptive noise processing specifically includes the following:

[0053] The moving average of the residual is calculated by introducing a sliding window. The size of the sliding window is set to N, and a double-ended queue is used to store the most recent r i The residual vector is calculated at each filtering iteration, and the moving average r of the sliding window is calculated. avg :

[0054]

[0055] When the sliding window exceeds the set size, the earliest residual is removed so that the calculation is always based on the latest N residual data; after obtaining the residual statistics, its norm || r is calculated avg||To measure the overall change trend of the current residual;

[0056] Set the dynamic adjustment factor dynamic_factor = 1.0 + β·||r avg ||-threshold| is used to control the adjustment range of the noise covariance, where β is a proportional factor used to control the weight of the influence of residual changes on the adjustment factor; threshold is a dynamic threshold that distinguishes the normal range from the abnormal range of residuals; the adjustment of noise covariance is based on the dynamic factor and is divided into two positioning models: UWB and dead reckoning;

[0057] The update formula for the UWB noise covariance matrix is:

[0058]

[0059] The update formula for the dead reckoning noise covariance matrix is:

[0060]

[0061] At the same time, in order to avoid numerical instability caused by too small values of the covariance matrix, the minimum noise limit matrix min_radar_noise is set, and the updated result is:

[0062] R uwb =max(R uwb , R min )

[0063] R imu+odom =max(R imu+odom ,R min ).

[0064] Preferably, the specific calculation formula for the S5 outlier removal by the Mahalanobis distance method is:

[0065]

[0066] For n-dimensional data, the square of the Mahalanobis distance follows a chi-square distribution with n degrees of freedom under the Gaussian distribution assumption. By finding its cumulative distribution function, the critical value at a specific confidence level is determined. A fixed chi-square distribution threshold is difficult to adapt to the changing characteristics of measurement noise in a dynamic environment. By combining the uncertainty information of the system, the threshold is dynamically adjusted. The threshold is defined as:

[0067] τ0+k·Tr(Σ)

[0068] Where τ0 is the initial reference value; Tr(Σ) represents the trace of the state covariance matrix; k is the adjustment factor; the greater the uncertainty of the system, the wider the threshold will be to avoid eliminating possible valid measurements; and when the uncertainty is smaller, the threshold will be tightened to enhance the ability to detect outliers.

[0069] A storage medium readable by an electronic device stores a computer program / instruction, which, when executed by a processor, executes the above-mentioned AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm.

[0070] An electronic device, comprising:

[0071] processor;

[0072] A memory stores a computer program, which is used to execute the above-mentioned AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm when the computer program is run by the processor.

[0073] Compared with the prior art, the present invention has the following beneficial effects:

[0074] By introducing an adaptive noise adjustment mechanism and an outlier rejection strategy based on the Mahalanobis distance, this method dynamically adjusts the noise covariance matrix, improving the robustness and accuracy of the filter. Furthermore, it adopts a centralized sequential fusion architecture, preserving the independence of UWB and IMU / ODOM data. A centralized UKF filter fuses sensor data at different times, resulting in low computational complexity and good robustness, making it suitable for resource-constrained embedded systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] The accompanying drawings, as part of this disclosure, are intended to provide a further understanding of the disclosure. The exemplary embodiments of the disclosure and their descriptions are intended to explain the disclosure and do not constitute undue limitations thereon. Obviously, the drawings described below are merely examples, and those skilled in the art can derive other drawings based on these drawings without inventive effort.

[0076] Figure 1 Schematic diagram of the CTRV motion model of the present invention. DETAILED DESCRIPTION

[0077] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0078] A UWB-IMU-ODOM sequential fusion positioning algorithm based on AUKF is proposed. The algorithm includes: preliminary calculation of UWB data and dead reckoning results within their respective time steps; a central UKF filter performs state prediction based on the CTRV motion model, and eliminates abnormal measurement values by comparing the residual range with the chi-square distribution value to reduce the impact of noise spikes and burst errors on positioning accuracy; an adaptive noise adjustment mechanism is used to calculate the moving average of the residuals using a sliding window, and the noise covariance is dynamically adjusted according to the residual change amplitude to adapt to the changes in noise characteristics in complex dynamic environments.

[0079] The specific implementation includes the following steps: S1 motion state equation based on CTRV, S2 system measurement equation, S3 unscented Kalman filter based on sequential fusion, S4 adaptive noise processing and S5 outlier removal; specifically as follows:

[0080] S1 CTRV-based motion state equation: In complex dynamic scenarios such as non-linear motion, traditional linear motion models are difficult to accurately describe the motion characteristics of the system, so a more targeted dynamic model is needed. By setting the target turning rate and speed to be constant in the CTRV model in the secondary motion model; Figure 1 As shown, the CTRV model uses the following state variables:

[0081]

[0082] Among them, x and y represent the position coordinates of the target in the two-dimensional plane; v represents the target's movement speed; φ represents the target's heading angle, that is, the target's current movement direction; Indicates the target's turning rate, i.e., angular velocity;

[0083] Set the time corresponding to the discrete time step k to t k , the time corresponding to time step k+1 is t k+1 , then the time difference between the two is Δt=t k+1 -t k ; Considering the influence of noise vector on the whole process, then When , the state transition formula of the CTRV model can be expressed as:

[0084]

[0085] when When , x and y in the original state transfer formula will tend to infinity, making the formula unusable; but the actual motion of the target is linear motion, so the state transfer formula can be simplified to a constant speed model:

[0086]

[0087] Among them, the velocity and angular velocity in the CTRV model are considered constant, and the higher-order quantity is used as the noise disturbance term, and the linear acceleration v is set a,k and yaw acceleration The mean is 0 and the variances are and Gaussian distribution.

[0088] S2 system measurement equation: In the fusion framework, UWB provides the absolute position information of the target in the global coordinate system, which is a linear measurement model. The two-dimensional position component (x k ,y k ), we get the linear mapping relationship from the system state to the measurement space:

[0089] Z k =HX k +W k

[0090] Among them, the measurement vector Measurement Matrix Measurement noise Measurement noise covariance matrix σ x 2 , σ y 2 Represents the measurement variance of UWB in the X and Y directions respectively; the dead reckoning measurement model composed of IMU and odometer is mainly responsible for the continuity estimation of short-term motion; the measurement vector contains three components: path length ρ, heading angle φ, path speed The measurement equation of the system can be expressed as:

[0091] Z k =h(X k )+W k

[0092] Among them, the measurement vector Nonlinear measurement function Measurement noise Measurement noise covariance matrix σ ρ 2 , σ φ 2 、 represent the variance of the measurement noise of path length, heading angle and path velocity, respectively, and they are independent of each other.

[0093] S3 Unscented Kalman Filter based on Sequential Fusion: The distribution of the system state X is set to Gaussian distribution UKF constructs a Gaussian distribution that approximates the true distribution through sampling points selected by unscented transformation to achieve state estimation; UKF sampling points are distributed around the state mean, and sigma points are generated by the following formula:

[0094] χ (0) =μ

[0095]

[0096] Where χ represents the sigma point matrix, the first point is the mean μ, and the remaining sigma points are scaled according to the scale of the covariance matrix Σ; λ is an adjustable expansion coefficient used to control the distribution range of the sigma points;

[0097] Each sigma point is assigned a weight according to its importance in the distribution. The weight calculation formula is as follows:

[0098]

[0099] Among them, w m is the mean weight, the sum of all representative point weights is equal to 1; w c is the mean square error weight; λ = α 2 (n+k)-n is the diffusion parameter, α controls the distribution range of the points, k is used for further fine-tuning, and β is used to introduce prior knowledge of the distribution, constituting the four key parameters for adjusting the unscented transform;

[0100] The generated sigma point is mapped to the current moment through the state transfer equation to obtain the state sigma point at the predicted moment:

[0101]

[0102] Combined with process noise:

[0103]

[0104] Get the mean and covariance of the state prediction values:

[0105]

[0106] By integrating the state transition model and process noise, the prior estimation of the system state is completed.

[0107] As a further improvement: To further improve the accuracy of state estimation, the predicted system state can be corrected based on the sensor measurements. The steps are as follows:

[0108] Map the state sigma point to the measurement space through the measurement equation h(x) to obtain the measurement sigma point

[0109]

[0110] Combined with the measurement noise covariance matrix R k , get the mean of the measured predicted values Covariance S k and the covariance matrix P between the state prediction and the measurement prediction xz :

[0111]

[0112] Calculate the Kalman gain K k :

[0113]

[0114] Because in each update process of sequential fusion, only the measurement value of the current sensor and the state estimate of the previous moment are combined to update the state, the state mean and covariance update of sensor i are obtained:

[0115]

[0116] Then we get the global state estimate of the central processor at the k+1th moment and the estimated error covariance matrix P k+1|N ; The predicted state and measurement information are integrated to complete the state estimation at the current moment.

[0117] It should be noted that the adaptive noise adjustment mechanism includes:

[0118] S4 Adaptive Noise Processing: Adopts an adaptive noise adjustment method based on the statistical characteristics of residuals to improve the robustness and accuracy of the filter by dynamically adjusting the noise covariance matrix; and

[0119] S5 Outlier Removal: Mahalanobis distance is a covariance-based measurement scale that can accurately measure the degree of deviation in multidimensional data. It is calculated using the Mahalanobis distance method to avoid scale inconsistencies that may occur when relying solely on Euclidean distance.

[0120] As a further illustration, S4 adaptive noise processing specifically includes the following:

[0121] The moving average of the residual is calculated by introducing a sliding window. The size of the sliding window is set to N, and a double-ended queue is used to store the most recent r i The residual vector is calculated at each filtering iteration, and the moving average r of the sliding window is calculated. avg :

[0122]

[0123] When the sliding window exceeds the set size, the earliest residual is removed so that the calculation is always based on the latest N residual data; after obtaining the residual statistics, its norm || r is calculated avg ||To measure the overall change trend of the current residual;

[0124] Set the dynamic adjustment factor dynamic_factor = 1.0 + β·||r avg ||-threshold| is used to control the adjustment range of the noise covariance, where β is a proportional factor used to control the weight of the influence of residual changes on the adjustment factor; threshold is a dynamic threshold that distinguishes the normal range from the abnormal range of residuals; the adjustment of noise covariance is based on the dynamic factor and is divided into two positioning models: UWB and dead reckoning;

[0125] The update formula for the UWB noise covariance matrix is:

[0126]

[0127] The update formula for the dead reckoning noise covariance matrix is:

[0128]

[0129] At the same time, in order to avoid numerical instability caused by too small values of the covariance matrix, the minimum noise limit matrix min_radar_noise is set, and the updated result is:

[0130] R uwb =max(R uwb , R min )

[0131] R imu+odom =max(R imu+odom ,R min ).

[0132] As a further explanation, the specific calculation formula for S5 outlier removal using the Mahalanobis distance method is:

[0133]

[0134] For n-dimensional data, the square of the Mahalanobis distance follows a chi-square distribution with n degrees of freedom under the Gaussian distribution assumption. By finding its cumulative distribution function, the critical value at a specific confidence level is determined. A fixed chi-square distribution threshold is difficult to adapt to the changing characteristics of measurement noise in a dynamic environment. By combining the uncertainty information of the system, the threshold is dynamically adjusted. The threshold is defined as:

[0135] τ0+k·Tr(Σ)

[0136] Where τ0 is the initial reference value; Tr(Σ) represents the trace of the state covariance matrix; k is the adjustment factor; the greater the uncertainty of the system, the wider the threshold will be to avoid eliminating possible valid measurements; and when the uncertainty is smaller, the threshold will be tightened to enhance the ability to detect outliers.

[0137] A storage medium readable by an electronic device stores a computer program / instruction, which, when executed by a processor, executes the above-mentioned AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm.

[0138] An electronic device, comprising:

[0139] processor;

[0140] A memory stores a computer program, which is used to execute the above-mentioned AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm when the processor runs the computer program.

[0141] It should be noted that in actual applications, the algorithm's average RMSE has been reduced from 0.077 meters for the UWB module and 0.079 meters for dead reckoning to 0.060 meters, improving accuracy by approximately 22.1% and 24.1%, respectively. Furthermore, in interference scenarios, the algorithm effectively suppresses extreme errors and maintains good continuous positioning.

[0142] In the description of this specification, the reference terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0143] It should be understood that various parts of the present invention can be implemented using hardware, software, firmware, or a combination thereof. In the above-described embodiments, multiple steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0144] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0145] In addition, the functional units in the various embodiments of the present invention may be integrated into a single processing module, each unit may exist physically separately, or two or more units may be integrated into a single module. The aforementioned integrated modules may be implemented in the form of hardware or in the form of software functional modules. If the integrated modules are implemented in the form of software functional modules and sold or used as independent products, they may also be stored in a computer-readable storage medium.

[0146] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

Claims

1. A UWB-IMU-ODOM sequential fusion positioning algorithm based on AUKF, characterized by: include: The UWB data and dead reckoning results are initially solved within their respective time steps; The central UKF filter performs state prediction based on the CTRV motion model and eliminates abnormal measurement values by comparing the residual range with the chi-square distribution value to reduce the impact of noise spikes and burst errors on positioning accuracy; Through the adaptive noise adjustment mechanism, a sliding window is used to calculate the moving average of the residuals, and the noise covariance is dynamically adjusted according to the amplitude of the residual changes to adapt to the changes in noise characteristics in complex dynamic environments.

2. The AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to claim 1, characterized in that: The specific steps of dead reckoning are as follows: S1 is based on the CTRV's motion equation; The target turning rate and speed are kept constant by setting the CTRV model in the secondary motion model; the CTRV model uses the following state variables: Among them, x and y represent the position coordinates of the target in the two-dimensional plane; v represents the target's movement speed; φ represents the target's heading angle, that is, the target's current movement direction; Indicates the target's turning rate, i.e., angular velocity; Set the time corresponding to the discrete time step k to t k , the time corresponding to time step k+1 is t k+1 , the time difference between the two is Δt=t k+1 -t k ; Considering the influence of noise vector on the whole process, then When , the state transition formula of the CTRV model can be expressed as: when When , x and y in the original state transfer formula will tend to infinity, making the formula unusable; but the actual motion of the target is linear motion, so the state transfer formula can be simplified to a constant speed model: Among them, the velocity and angular velocity in the CTRV model are considered constant, and the higher-order quantity is used as the noise disturbance term, and the linear acceleration v is set a,k and yaw acceleration The mean is 0 and the variances are and Gaussian distribution.

3. The AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to claim 1, characterized in that: The specific steps for UWB data are the S2 system measurement equation; specifically, they are as follows: The two-dimensional position component (x) is directly extracted from the state vector by the UWB measurement model. k ,y k ), we get the linear mapping relationship from the system state to the measurement space: Z k =HX k +W k Among them, the measurement vector Measurement Matrix Measurement noise Measurement noise covariance matrix Represents the measurement variance of UWB in the X and Y directions respectively; the dead reckoning measurement model composed of IMU and odometer is mainly responsible for the continuity estimation of short-term motion; the measurement vector contains three components: path length ρ, heading angle φ, path speed The measurement equation of the system can be expressed as: Z k =h(X k )+W k Among them, the measurement vector Nonlinear measurement function Measurement noise Measurement noise covariance matrix represent the variance of the measurement noise of path length, heading angle, and path velocity, respectively, and they are independent of each other.

4. The AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to claim 3, characterized in that: The central UKF filter performs state prediction based on the CTRV motion model and eliminates abnormal measurement values by comparing the residual range with the chi-square distribution value. The specific steps include the following: S3 Unscented Kalman Filter based on Sequential Fusion: The distribution of the system state X is set to Gaussian distribution UKF constructs a Gaussian distribution that approximates the true distribution through sampling points selected by unscented transformation to achieve state estimation; UKF sampling points are distributed around the state mean, and sigma points are generated by the following formula: Where χ represents the sigma point matrix, the first point is the mean μ, and the remaining sigma points are scaled according to the scale of the covariance matrix Σ; λ is an adjustable expansion coefficient used to control the distribution range of the sigma points; Each sigma point is assigned a weight according to its importance in the distribution. The weight calculation formula is as follows: Among them, w m is the mean weight, the sum of all representative point weights is equal to 1; w c is the mean square error weight; λ = α 2 (n+k)-n is the diffusion parameter, α controls the distribution range of the points, k is used for further fine-tuning, and β is used to introduce prior knowledge of the distribution, constituting the four key parameters for adjusting the unscented transform; The generated sigma point is mapped to the current moment through the state transfer equation to obtain the state sigma point at the predicted moment: Combined with process noise: Get the mean and covariance of the state prediction values: By integrating the state transition model and process noise, the prior estimation of the system state is completed.

5. The AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to claim 4, characterized in that: To further improve the accuracy of state estimation, the predicted system state can be corrected based on the sensor measurements. The steps are as follows: Map the state sigma point to the measurement space through the measurement equation h(x) to obtain the measurement sigma point Combined with the measurement noise covariance matrix R k , get the mean of the measured predicted values Covariance S k and the covariance matrix P between the state prediction and the measurement prediction xz : Calculate the Kalman gain K k : During each update of sequential fusion, only the current sensor's measurement value and the state estimate of the previous moment are combined to update the state, and the state mean and covariance update of sensor i are obtained: Get the global state estimate of the central processor at time k+1 and the estimated error covariance matrix P k+1|N ; The predicted state and measurement information are integrated to complete the state estimation at the current moment.

6. The AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to any one of claims 1 to 5, characterized in that: The adaptive noise adjustment mechanism includes: S4 Adaptive Noise Processing: Adopts an adaptive noise adjustment method based on the statistical characteristics of residuals to improve the robustness and accuracy of the filter by dynamically adjusting the noise covariance matrix; and S5 outlier removal: Calculate using the Mahalanobis distance method to avoid scale inconsistencies that may occur when relying solely on Euclidean distance.

7. The AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to claim 6, characterized in that: The S4 adaptive noise processing specifically includes the following: The moving average of the residual is calculated by introducing a sliding window. The size of the sliding window is set to N, and a double-ended queue is used to store the most recent r i The residual vector is calculated at each filtering iteration, and the moving average r of the sliding window is calculated. avg : When the sliding window exceeds the set size, the earliest residual is removed so that the calculation is always based on the latest N residual data; after obtaining the residual statistics, its norm || r is calculated avg ||To measure the overall change trend of the current residual; Set the dynamic adjustment factor dynamic_factor = 1.0 + β·||r avg ||-threshold| is used to control the adjustment range of the noise covariance, where β is a proportional factor used to control the weight of the influence of residual changes on the adjustment factor; threshold is a dynamic threshold that distinguishes the normal range from the abnormal range of residuals; the adjustment of noise covariance is based on the dynamic factor and is divided into two positioning models: UWB and dead reckoning; The update formula for the UWB noise covariance matrix is: The update formula for the dead reckoning noise covariance matrix is: At the same time, in order to avoid numerical instability caused by too small values of the covariance matrix, the minimum noise limit matrix min_radar_noise is set, and the updated result is: R uwb =max(R uwb ,R min ) R imu+odom =max(R imu+odom ,R min )。 8. The AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to claim 1, characterized in that: The specific calculation formula for the S5 outlier removal is calculated by the Mahalanobis distance method: For n-dimensional data, the square of the Mahalanobis distance follows a chi-square distribution with n degrees of freedom under the Gaussian distribution assumption. By finding its cumulative distribution function, the critical value at a specific confidence level is determined. A fixed chi-square distribution threshold is difficult to adapt to the changing characteristics of measurement noise in a dynamic environment. By combining the uncertainty information of the system, the threshold is dynamically adjusted. The threshold is defined as: τ0+k·Tr(Σ) Where τ0 is the initial reference value; Tr(Σ) represents the trace of the state covariance matrix; k is the adjustment factor; the greater the uncertainty of the system, the wider the threshold will be to avoid eliminating possible valid measurements; and when the uncertainty is smaller, the threshold will be tightened to enhance the ability to detect outliers.

9. An electronic device readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instruction is executed by a processor, the AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to any one of claims 1 to 8 is executed.

10. An electronic device, characterized in that: The electronic device comprises: processor; A memory having a computer program stored thereon, wherein the computer program is executed by the processor to execute the AUKF-based UWB-IMU-ODOM sequential fusion positioning algorithm according to any one of claims 1 to 8.

Images