A VIO-UWB double-layer adaptive robust positioning method considering anchor error prior and residual scale alignment

Abstract

This invention discloses a VIO-UWB two-layer adaptive robust positioning method that considers anchor point error priors and residual scale alignment, applicable to environments with restricted global navigation satellite systems and non-line-of-sight interference. The method includes: constructing a VIO-UWB tightly coupled sliding window optimization model; establishing a per-anchor-point error prior to compensate for deviations and whiten noise in the ranging residuals; introducing a residual scale alignment factor to map the ranging residuals to dimensionless scale-aligned residuals comparable to visual residuals; and employing IGG3 piecewise robust weight functions and Helmert variance component estimation to achieve two-layer adaptive robust estimation. This method can mitigate the impact of anchor point differences, residual scale imbalances, and non-line-of-sight anomalies on positioning results, improving the accuracy, stability, and robustness of fusion positioning in complex environments.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation and multi-sensor fusion positioning technology, specifically involving a VIO-UWB two-layer adaptive robust positioning method that takes into account both anchor point error prior and residual scale alignment. Background Technology

[0002] For modern autonomous navigation systems, the Global Navigation Satellite System (GNSS) is one of the core means of providing absolute positioning information. However, in complex scenarios such as tunnels, underground spaces, and urban canyons, GNSS signals are easily attenuated or even unreliable due to obstruction, multipath effects, and electromagnetic interference. To solve the positioning challenges in GNSS-constrained environments, sensor multi-source fusion positioning technology has received widespread attention, and this technology has become one of the important development directions in the field of autonomous navigation and positioning. Among them, Visual-Inertial Odometry (VIO) integrates the environmental perception capabilities of a camera with the high-frequency dynamic measurement capabilities of an Inertial Measurement Unit (IMU), becoming one of the mainstream positioning solutions in GNSS-constrained environments. However, although VIO performs well in the above applications, it still faces a key problem common to relative positioning technologies: the cumulative error of positioning results over time, which can even lead to positioning drift and significantly reduce the long-term positioning accuracy of the system. To mitigate the impact of the aforementioned problems, ultra-wideband (UWB) ranging technology, due to its absolute distance constraint capability without cumulative error, has been gradually introduced into the VIO framework. Through joint optimization with visual and inertial observations, drift can be effectively suppressed.

[0003] Currently, methods applied in the field of VIO-UWB (Visual-Inertial Odometry and Ultra-Wideband) fusion localization have undergone continuous evolution. Regarding the basic framework of visual-inertial localization, Mourikis et al. proposed a multi-state constrained Kalman filter method, laying the foundation for filter-based visual-inertial fusion; Forster et al. proposed IMU pre-integration theory, providing a unified modeling tool for graph optimization-based VIO; based on this, Leutenegger et al. proposed the OKVIS system, Bloesch et al. proposed the ROVIO system, and Qin Tong et al. proposed the VINS-Mono system and its subsequent VINS-FUSION system. All of these methods significantly improve the accuracy and robustness of visual / inertial state estimation and provide an architectural foundation for introducing external absolute constraints through a general factor interface. Regarding specific VIO-UWB fusion methods, Nguyen et al. proposed a tightly coupled VIO-UWB system, verifying that a single UWB anchor point can assist monocular VIO in recovering scale and suppressing drift; Gao et al. and Cao et al. respectively achieved low-drift indoor localization and multi-robot collaborative SLAM with UWB assistance; Delama et al. proposed the UVIO method to enhance the system's robustness in occluded environments; Lu et al. proposed the GRVINS method to further unify multiple global constraints such as GNSS, UWB, and VIO into a fusion framework. Addressing non-ideal factors in practical deployments, Luo et al. and Jia et al. respectively explored assisted navigation and distributed initialization under unknown anchor point conditions; Fan et al. focused on robust fusion under abnormal observation conditions; Zhou et al. conducted in-depth research on UWB online calibration and consistency modeling, providing support for the engineering application of VIO-UWB fusion localization in complex scenarios.

[0004] Although the aforementioned VIO-UWB tightly coupled fusion scheme significantly improves system positioning accuracy, existing mainstream methods still face three common problems in handling joint optimization of heterogeneous sensors, which restrict the overall system performance. First, the inherent hardware differences between anchor points and the influence of local environments are often oversimplified. Existing frameworks mostly use a uniform Gaussian error model, assuming that all UWB anchor points have consistent statistical characteristics. However, hardware parameters such as antenna delay of UWB nodes introduce device-specific quasi-static biases, and complex multipath propagation indoors causes anchor points at different spatial locations to exhibit different error distributions. Under the combined effects of manufacturing differences, antenna group delay characteristics, and local propagation conditions, each anchor point usually exhibits stable but inconsistent error statistical characteristics. If the per-anchor-point bias is not explicitly modeled, the system is prone to introducing systematic errors that are difficult to eliminate. Second, in the joint optimization of multi-source information, visual reprojection residuals and UWB ranging residuals differ significantly in terms of dimensions and numerical scales. The former is usually measured in pixels and involves a large number of observations, while the latter is a meter-level physical quantity and involves a relatively small number of observations. During factor graph optimization, a large number of visual factors may dominate the Hessian matrix, thus affecting the numerical conditions of the system and weakening the contribution of UWB constraints to global scale recovery and drift suppression. Finally, existing methods generally lack online adaptive mechanisms to cope with time-varying interference. Existing robust methods such as Switchable Constraints, dynamic covariance scaling, and asymptotic nonconvexity mainly reduce the weight of local outliers at the measurement level, which has a good suppression effect on single non-line-of-sight (NLOS) observations. However, when the carrier is in an extremely dynamic, weakly textured, or complex occlusion environment, causing the overall observation quality of a certain type of sensor to degrade, the above-mentioned single measurement-level robust methods cannot achieve dynamic allocation of group-level weights, and the performance of traditional fusion systems using fixed covariance or empirical weights will be significantly limited.

[0005] To address the common problems in current VIO-UWB fusion localization methods, such as the lack of explicit modeling of anchor point error differences, heterogeneous residual scale imbalance, and the lack of multi-level online adaptive robustness mechanisms, this invention proposes a two-layer adaptive robust localization method that takes into account both anchor point error priors and residual scale alignment. The method mainly consists of four parts: tightly coupled VIO-UWB modeling, anchor point-by-anchor-point error prior construction, residual scale alignment, and two-layer adaptive robust estimation. In the tightly coupled modeling section, this invention constructs a sliding window joint optimization model for the raw observation data of the camera, inertial measurement unit, and ultra-wideband tag, including marginalization prior factors, visual reprojection factors, inertial pre-integration factors, and ultra-wideband ranging factors. In the anchor-point error prior construction section, this invention establishes an anchor-point error model through offline calibration, explicitly models the constant deviation and noise scale of different anchor points, and introduces ranging deviation compensation and residual whitening processing. In the residual scale alignment section, this invention proposes a scale alignment strategy based on the camera's equivalent focal length and pixel-derived noise, mapping meter-level UWB ranging residuals to a dimensionless scale comparable to visual residuals to alleviate the gradient suppression problem caused by heterogeneous residual scale imbalance. In the two-layer adaptive robust estimation section, this invention introduces a piecewise robust weight function (Institute of Geodesy and Geophysics III, IGG3) at the measurement level to suppress single anomalous observations, and introduces Helmert variance component estimation at the observation group level. HVCE (High-Volume Estimation) adjusts the global weights of the visual, inertial, and UWB observation groups in real time to enhance the system's positioning accuracy, stability, and robustness in complex environments. Summary of the Invention

[0006] To address the common problems of current VIO-UWB fusion positioning methods in complex environments, such as the lack of explicit modeling of anchor point error differences, imbalance of heterogeneous residual scales, and the lack of multi-level online adaptive robustness mechanisms, this invention proposes a VIO-UWB two-layer adaptive robust positioning method that takes into account both prior anchor point error and residual scale alignment. This method effectively improves the positioning accuracy, stability, and robustness of VIO-UWB fusion positioning methods in complex environments.

[0007] A VIO-UWB two-layer adaptive robust localization method that takes into account both prior anchor point error and residual scale alignment includes the following steps:

[0008] Step 1: Acquire the raw observation data of the camera, inertial measurement unit and ultra-wideband tag, perform visual feature tracking, inertial pre-integration and ultra-wideband ranging preprocessing, and construct a tightly coupled factor graph optimization model containing marginalization prior factors, visual reprojection factors, inertial pre-integration factors and ultra-wideband ranging factors within a sliding window;

[0009] Step 2: Establish a prior model for each anchor point error for different ultra-wideband anchor points. Use the constant deviation prior and noise scale prior corresponding to each anchor point to perform deviation compensation and residual whitening on the ranging observation, and obtain the ultra-wideband ranging residual that takes into account the differences of anchor points.

[0010] Step 3: Construct a residual scale alignment factor to map the whitened ultrawideband ranging residual into a dimensionless scale alignment residual comparable to the numerical scale of the visual residual, so as to enhance the effective influence of ultrawideband constraints in joint optimization.

[0011] Step 4: Construct a two-layer adaptive robust estimation mechanism. At the measurement level, the IGG3 piecewise robust weight function is used to reduce the weight of abnormal ranging observations. At the observation group level, the Helmert variance component estimation method is used to adaptively update the global weights of the visual observation group, the inertial observation group, and the ultra-wideband observation group. The pose, velocity, and zero-bias state estimation results of the carrier at the current moment are output through nonlinear joint optimization.

[0012] Step 1 involves acquiring the raw observation data from the camera, inertial measurement unit, and ultra-wideband tag, performing visual feature tracking, inertial pre-integration, and ultra-wideband ranging preprocessing, and constructing a tightly coupled factor graph optimization model within a sliding window that includes marginalization prior factors, visual reprojection factors, inertial pre-integration factors, and ultra-wideband ranging factors.

[0013] The specific steps are as follows:

[0014] Step 1-1: Acquire camera images, observe angular velocity and acceleration output by the inertial measurement unit, and measure the distance from the ultra-wideband tag to each anchor point. The camera, inertial measurement unit, and ultra-wideband tag are fixed to the same carrier platform.

[0015] Steps 1-2: Perform feature extraction and cross-frame tracking on camera images to construct visual reprojection residual factors;

[0016] Steps 1-3: Perform pre-integration on continuous observations of the inertial measurement unit to construct the inertial pre-integration residual factor between adjacent time points;

[0017] Steps 1-4: Combine the ranging observations between the ultra-wideband tag and each anchor point to construct the ultra-wideband ranging residual factor;

[0018] Steps 1-5: Using the pose, velocity, inertial measurement unit bias, and sensor extrinsic parameters at each moment within the sliding window as the state variables to be estimated, the marginalization prior factor, visual reprojection factor, inertial pre-integration factor, and ultra-wideband ranging factor are uniformly incorporated into the same nonlinear least squares objective function for tightly coupled joint optimization.

[0019] Step 2 describes establishing a priori error model for each anchor point for different ultra-wideband anchor points, using the constant deviation prior and noise scale prior corresponding to each anchor point to perform deviation compensation and residual whitening of ranging observations, and obtaining ultra-wideband ranging residuals that take into account the differences of anchor points.

[0020] The specific steps are as follows:

[0021] Step 2-1: Given the spatial coordinates of each ultra-wideband anchor point, calculate the first... The set of calibration residuals for each anchor point ;

[0022] Step 2-2: Based on the calibration residual set Construct the first Prior constant deviation of each anchor point and noise scale prior ,as follows:

[0023]

[0024]

[0025] in, To calibrate the residual set The residual elements in;

[0026] Steps 2-3: Utilize the aforementioned constant deviation prior. Original distance measurement observation Perform bias compensation to obtain the compensated distance measurement observations. ,as follows:

[0027]

[0028] Steps 2-4: Based on the compensated ranging observations and the current state, predict the first... Ultra-wideband ranging residuals corresponding to each anchor point ,as follows:

[0029]

[0030] in, The predicted geometric distance is calculated using the noise scale prior. The ultra-wideband ranging residual is subjected to weighted whitening processing.

[0031] Step 3 describes the construction of a residual scale alignment factor, which maps the whitened ultrawideband ranging residual to a dimensionless scale alignment residual comparable to the numerical scale of the visual residual, thereby enhancing the effective influence of ultrawideband constraints in joint optimization.

[0032] The specific steps are as follows:

[0033] Step 3-1: Analyze the differences in physical dimensions and numerical scale between visual reprojection residuals and ultra-wideband ranging residuals, and establish the numerical balance relationship of heterogeneous residuals;

[0034] Step 3-2: Let the equivalent focal length of the camera be... Noise extraction for planar pixels is Construct a dimensionless residual scaling alignment factor ,as follows:

[0035]

[0036] Step 3-3: Calculate the ultra-wideband ranging residual. Alignment factor with the residual scale Multiply to obtain the aligned residuals ,as follows:

[0037]

[0038] Steps 3-4: Combining the noise scale prior from step 2 The dimensionless standard whitening residual was calculated. ,as follows:

[0039]

[0040] The standard whitened residuals are then incorporated into the sliding window joint optimization to mitigate gradient imbalance and Hessian matrix contribution imbalance.

[0041] The two-layer adaptive robust estimation mechanism described in step 4 uses the IGG3 piecewise robust weight function to reduce the weight of abnormal ranging observations at the measurement level, and the Helmert variance component estimation method is used at the observation group level to adaptively update the global weights of the visual observation group, the inertial observation group, and the ultra-wideband observation group. The pose, velocity, and zero-bias state estimation results of the inertial measurement unit at the current moment are output through nonlinear joint optimization.

[0042] The specific steps are as follows:

[0043] Step 4-1: In the measurement-level robust weighting stage, the dimensionless residuals obtained after deviation compensation and residual whitening in Step 2 and scale alignment in Step 3 are used. As input variables;

[0044] Step 4-2: Calculate the corresponding measurement-level robust weights using the IGG3 piecewise robust weight function. This is applied to the ultra-wideband ranging residual term, where the set segmentation threshold is... and ,as follows:

[0045]

[0046] Step 4-3: In the observation group-level adaptive update phase, calculate the weighted sum of squared post-hoc residuals for the visual observation group, inertial observation group, and ultra-wideband observation group within the current sliding window. ,in ;

[0047] Step 4-4: Based on the post-hoc residual statistics of each observation group and the effective number of observations after equivalent reweighting Calculate the unit weighted variance estimate for each observation group. ,as follows:

[0048]

[0049] Steps 4-5: Adaptively update the global weights of each observation group using a multiplicative iterative approach. The update format is as follows:

[0050]

[0051] in, For dimensionless unit variance benchmark, superscript Indicates the number of iterations. This is a truncation function. and Truncation constraints are applied to prevent weight divergence, including upper and lower limits.

[0052] Steps 4-6: The two-layer adaptive robust estimation is performed using an inner and outer layer iterative method. The inner layer uses the current measurement-level robust weights and the observation group-level global weights to perform sliding window nonlinear optimization. The outer layer updates the global weights of each observation group based on the post-hoc residual statistics. The positioning result is output after iterating until the convergence condition is met or the preset number of iterations is reached.

[0053] Beneficial effects of this invention:

[0054] 1. To mitigate the impact of inherent hardware and local environmental differences among various UWB anchor points on the fusion positioning results, this invention proposes a per-anchor-point error prior modeling method. This method constructs constant deviation priors and noise scale priors for each anchor point through offline calibration, and explicitly incorporates them into the UWB ranging factor to achieve deviation compensation and residual whitening at the measurement level. Compared to existing unified error model processing methods, this invention can more accurately characterize the error statistical characteristics of different anchor points, effectively reducing systematic errors caused by anchor point differences, thereby improving the accuracy and stability of VIO-UWB fusion positioning.

[0055] 2. To address the issue of insufficient contribution of ultrawideband constraints in joint optimization due to the inconsistency in dimensions and numerical scale between visual reprojection residuals and ultrawideband ranging residuals, this invention proposes a residual scale alignment method. By constructing a scale alignment factor based on the equivalent focal length of the camera and the noise extracted from image plane pixels, meter-level ultrawideband ranging residuals are mapped to a dimensionless scale comparable to visual residuals. Compared to existing direct joint optimization methods, this invention effectively alleviates the gradient suppression and Hessian matrix contribution imbalance caused by heterogeneous residual scale imbalance, enhancing the role of ultrawideband observations in global scale recovery and long-term drift suppression.

[0056] 3. To improve the system's adaptability to abnormal observations and time-varying degradation in complex environments, this invention establishes a two-layer adaptive robust estimation mechanism. At the measurement level, the IGG3 piecewise robust weight function is used to reduce the weight of single abnormal ranging observations. At the observation group level, the Helmert variance component estimation method is used to adjust the global weights of the visual observation group, inertial observation group, and ultra-wideband observation group in real time. Compared with existing methods that rely solely on a single-layer robust kernel function or fixed covariance settings, this invention can not only suppress the impact of single non-line-of-sight or multipath abnormal observations, but also dynamically allocate the weights of each observation group when the overall observation quality of a certain type of sensor degrades, thereby significantly improving the system's robustness and environmental adaptability in complex environments.

[0057] 4. This invention integrates anchor-point error prior modeling, residual scale alignment, and two-layer adaptive robust estimation into a tightly coupled VIO-UWB sliding window optimization framework. This enables the system to simultaneously address local anomaly suppression, adaptive global weight adjustment, and efficient collaborative utilization of multi-source observation information. Compared to existing VIO-UWB fusion positioning methods, this invention exhibits higher positioning accuracy, better long-term stability, and stronger robustness in tunnels, underground spaces, urban canyons, and other complex GNSS-constrained environments, resulting in more stable, reliable, and accurate fusion positioning results. Attached Figure Description

[0058] Figure 1 is a flowchart of a VIO-UWB two-layer adaptive robust positioning method that takes into account the prior anchor point error and residual scale alignment according to the present invention.

[0059] Figure 2 is a detailed flowchart of step 1 of one embodiment of the present invention;

[0060] Figure 3 is a detailed flowchart of step 2 of one embodiment of the present invention;

[0061] Figure 4 is a detailed flowchart of step 3 of one embodiment of the present invention;

[0062] Figure 5 is a detailed flowchart of step 4 of one embodiment of the present invention;

[0063] Figure 6 is a summary flowchart of one embodiment of the present invention;

[0064] Figure 7 shows the three-dimensional trajectory diagrams of the method of the present invention and two other methods in the eee_01 scenario;

[0065] Figure 8 shows the error analysis diagram of the method of the present invention and two other methods in the eee_01 scenario;

[0066] Figure 9 shows the error statistics distribution of the method of the present invention and two other methods in the eee_01 scenario; Detailed Implementation

[0067] An embodiment of the present invention will be further described below with reference to the accompanying drawings.

[0068] In this embodiment of the invention, a VIO-UWB two-layer adaptive robust localization method that takes into account both prior anchor point error and residual scale alignment is provided, such as... Figure 1 As shown, it includes the following steps:

[0069] Step 1: Acquire the raw observation data of the camera, inertial measurement unit and ultra-wideband tag, perform visual feature tracking, inertial pre-integration and ultra-wideband ranging preprocessing, and construct a tightly coupled factor graph optimization model containing marginalization prior factors, visual reprojection factors, inertial pre-integration factors and ultra-wideband ranging factors within a sliding window;

[0070] Step 1-1: Acquire camera images, observe angular velocity and acceleration output by the inertial measurement unit, and measure the distance from the ultra-wideband tag to each anchor point. The camera, inertial measurement unit, and ultra-wideband tag are fixed to the same carrier platform.

[0071] Steps 1-2: Perform feature extraction and cross-frame tracking on camera images to construct visual reprojection residual factors;

[0072] Steps 1-3: Perform pre-integration on continuous observations of the inertial measurement unit to construct the inertial pre-integration residual factor between adjacent time points;

[0073] Steps 1-4: Combine the ranging observations between the ultra-wideband tag and each anchor point to construct the ultra-wideband ranging residual factor;

[0074] Steps 1-5: Using the pose, velocity, inertial measurement unit bias, and sensor extrinsic parameters at each moment within the sliding window as the state variables to be estimated, the marginalization prior factor, visual reprojection factor, inertial pre-integration factor, and ultra-wideband ranging factor are uniformly incorporated into the same nonlinear least squares objective function for tightly coupled joint optimization.

[0075] Step 2: Establish a prior model for each anchor point error for different ultra-wideband anchor points. Use the constant deviation prior and noise scale prior corresponding to each anchor point to perform deviation compensation and residual whitening on the ranging observation, and obtain the ultra-wideband ranging residual that takes into account the differences of anchor points.

[0076] Step 2-1: Given the spatial coordinates of each ultra-wideband anchor point, calculate the first... The set of calibration residuals for each anchor point ;

[0077] Step 2-2: Based on the calibration residual set Construct the first Prior constant deviation of each anchor point and noise scale prior ,as follows:

[0078]

[0079]

[0080] in, To calibrate the residual set The residual elements in;

[0081] Steps 2-3: Utilize the aforementioned constant deviation prior. Original distance measurement observation Perform bias compensation to obtain the compensated distance measurement observations. ,as follows:

[0082]

[0083] Steps 2-4: Based on the compensated ranging observations and the current state, predict the first... Ultra-wideband ranging residuals corresponding to each anchor point ,as follows:

[0084]

[0085] in, The predicted geometric distance is calculated using the noise scale prior. The ultra-wideband ranging residual is subjected to weighted whitening processing.

[0086] Step 3: Construct a residual scale alignment factor to map the whitened ultrawideband ranging residual into a dimensionless scale alignment residual comparable to the numerical scale of the visual residual, so as to enhance the effective influence of ultrawideband constraints in joint optimization.

[0087] Step 3-1: Analyze the differences in physical dimensions and numerical scale between visual reprojection residuals and ultra-wideband ranging residuals, and establish the numerical balance relationship of heterogeneous residuals;

[0088] Step 3-2: Let the equivalent focal length of the camera be... Noise extraction for planar pixels is Construct a dimensionless residual scaling alignment factor ,as follows:

[0089]

[0090] Step 3-3: Calculate the ultra-wideband ranging residual. Alignment factor with the residual scale Multiply to obtain the aligned residuals ,as follows:

[0091]

[0092] Steps 3-4: Combining the noise scale prior from step 2 The dimensionless standard whitening residual was calculated. ,as follows:

[0093]

[0094] The standard whitened residuals are then incorporated into the sliding window joint optimization to mitigate gradient imbalance and Hessian matrix contribution imbalance.

[0095] Step 4: Construct a two-layer adaptive robust estimation mechanism. At the measurement level, the IGG3 piecewise robust weight function is used to reduce the weight of abnormal ranging observations. At the observation group level, the Helmert variance component estimation method is used to adaptively update the global weights of the visual observation group, the inertial observation group, and the ultra-wideband observation group. The pose, velocity, and zero-bias state estimation results of the carrier at the current moment are output through nonlinear joint optimization.

[0096] Step 4-1: In the measurement-level robust weighting stage, the dimensionless residuals obtained after deviation compensation and residual whitening in Step 2 and scale alignment in Step 3 are used. As input variables;

[0097] Step 4-2: Calculate the corresponding measurement-level robust weights using the IGG3 piecewise robust weight function. This is applied to the ultra-wideband ranging residual term, where the set segmentation threshold is... and ,as follows:

[0098]

[0099] Step 4-3: In the observation group-level adaptive update phase, calculate the weighted sum of squared post-hoc residuals for the visual observation group, inertial observation group, and ultra-wideband observation group within the current sliding window. ,in ;

[0100] Step 4-4: Based on the post-hoc residual statistics of each observation group and the effective number of observations after equivalent reweighting Calculate the unit weighted variance estimate for each observation group. ,as follows:

[0101]

[0102] Steps 4-5: Adaptively update the global weights of each observation group using a multiplicative iterative approach. The update format is as follows:

[0103]

[0104] in, For dimensionless unit variance benchmark, superscript Indicates the number of iterations. This is a truncation function. and Truncation constraints are applied to prevent weight divergence, including upper and lower limits.

[0105] Steps 4-6: The two-layer adaptive robust estimation is performed using an inner and outer layer iterative method. The inner layer uses the current measurement-level robust weights and the observation group-level global weights to perform sliding window nonlinear optimization. The outer layer updates the global weights of each observation group based on the post-hoc residual statistics. The positioning result is output after iterating until the convergence condition is met or the preset number of iterations is reached.

[0106] To verify the actual performance of the VIO-UWB two-layer adaptive robust localization method of this invention, which takes into account both anchor point error prior and residual scale alignment, this embodiment uses the publicly available open-source dataset NTU-VIRAL for experimental verification. The experiment selects three types of scene sequences from this dataset: eee, nya, and sbs. The eee scene was collected from the School of EEE central carpark, the nya scene from inside the Nanyang Auditorium, and the sbs scene from the plaza in front of the School of Bio.Science. To avoid coupling between parameter setting and test results, this embodiment uses the _02 sequences from each scene, namely eee_02, nya_02, and sbs_02, for parameter evaluation and setting; and uses the _01 and _03 sequences from each scene, namely eee_01, eee_03, nya_01, nya_03, sbs_01, and sbs_03, for performance verification of the method of this invention. The above scenarios differ in terms of environmental structure, dynamic interference, and observation conditions, and can be used to verify the positioning accuracy, stability, and robustness of the method of the present invention under different complex environments.

[0107] like Figure 7 The image shows a comparison of the 3D trajectories of the proposed method, VINS-Fusion, and VIR-SLAM methods in the eee_01 scene sequence. To verify the actual performance of the VIO-UWB two-layer adaptive robust localization method of this invention, which considers both anchor point error priors and residual scale alignment, the eee_01 scene sequence from the NTU-VIRAL open-source dataset was selected for experimental verification. The proposed method was then compared with VINS-Fusion and VIR-SLAM methods. The eee_01 scene was collected from the School of EEE central carpark; its structure has a certain degree of repetition, which can easily lead to accumulated localization drift during long-term operation. Figure 7 It can be intuitively seen that, compared with VINS-Fusion and VIR-SLAM, the positioning trajectory obtained by the method of the present invention is more closely aligned with the reference trajectory as a whole, and maintains better global consistency during trajectory extension. In contrast, the comparative methods exhibit varying degrees of local offset and cumulative drift, indicating that the method of the present invention can more effectively suppress error accumulation and improve trajectory estimation stability in this scenario.

[0108] like Figure 8The figure shows the localization error analysis of the proposed method, VINS-Fusion, and VIR-SLAM methods in the eee_01 scene sequence. The error trend shows that throughout the localization process, the error curve of the proposed method is generally lower than that of VINS-Fusion and VIR-SLAM, and the change is more stable over time. Even in stages where the error increases significantly, the proposed method maintains good error control, while the comparative methods are more prone to continuous increases or large fluctuations. Especially in the later stages of the scene, the proposed method maintains a lower error level, indicating that through anchor-point error prior modeling, residual scale alignment, and two-layer adaptive robust estimation, the proposed method can effectively reduce the impact of anchor-point differences, heterogeneous residual scale imbalance, and abnormal ranging interference on the localization results, thereby improving the robustness and stability of localization in complex environments.

[0109] like Figure 9 The figure shows the localization error statistics of the proposed method, VINS-Fusion, and VIR-SLAM methods in the eee_01 scene sequence. The statistical results show that the proposed method outperforms VINS-Fusion and VIR-SLAM in multiple statistical indicators, including root mean square error, mean, median, standard deviation, and maximum value. This indicates that the proposed method not only has a smaller overall localization error but also a more concentrated error distribution, with more effective suppression of abnormal deviations. Compared to the comparative methods, the proposed method exhibits better error convergence characteristics and stronger environmental adaptability in the eee_01 scene sequence, further validating the effectiveness of the proposed anchor-point error prior, residual scale alignment, and two-layer adaptive robust estimation mechanism.

[0110] The above description is merely the most basic specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions, improvements, and modifications made by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope defined in the claims.

Claims

1. A VIO-UWB two-layer adaptive robust positioning method that takes into account prior anchor point error and residual scale alignment, characterized in that, Includes the following steps: Step 1: Acquire the raw observation data of the camera, inertial measurement unit and ultra-wideband tag, perform visual feature tracking, inertial pre-integration and ultra-wideband ranging preprocessing, and construct a tightly coupled factor graph optimization model containing marginalization prior factors, visual reprojection factors, inertial pre-integration factors and ultra-wideband ranging factors within a sliding window; Step 2: Establish a prior model for each anchor point error for different ultra-wideband anchor points. Use the constant deviation prior and noise scale prior corresponding to each anchor point to perform deviation compensation and residual whitening on the ranging observation, and obtain the ultra-wideband ranging residual that takes into account the differences of anchor points. Step 3: Construct a residual scale alignment factor to map the whitened ultrawideband ranging residual into a dimensionless scale alignment residual comparable to the numerical scale of the visual residual, so as to enhance the effective influence of ultrawideband constraints in joint optimization. Step 4: Construct a two-layer adaptive robust estimation mechanism. At the measurement level, the IGG3 piecewise robust weight function is used to reduce the weight of abnormal ranging observations. At the observation group level, the Helmert variance component estimation method is used to adaptively update the global weights of the visual observation group, the inertial observation group, and the ultra-wideband observation group. The pose, velocity, and zero-bias state estimation results of the carrier at the current moment are output through nonlinear joint optimization.

2. The VIO-UWB two-layer adaptive robust positioning method considering prior anchor point error and residual scale alignment as described in claim 1, characterized in that, Step 1 involves acquiring the raw observation data from the camera, inertial measurement unit, and ultra-wideband tag, performing visual feature tracking, inertial pre-integration, and ultra-wideband ranging preprocessing, and constructing a tightly coupled factor graph optimization model within a sliding window that includes marginalization prior factors, visual reprojection factors, inertial pre-integration factors, and ultra-wideband ranging factors. The specific steps are as follows: Step 1-1: Acquire camera images, observe angular velocity and acceleration output by the inertial measurement unit, and measure the distance from the ultra-wideband tag to each anchor point. The camera, inertial measurement unit, and ultra-wideband tag are fixed to the same carrier platform. Steps 1-2: Perform feature extraction and cross-frame tracking on camera images to construct visual reprojection residual factors; Steps 1-3: Perform pre-integration on continuous observations of the inertial measurement unit to construct the inertial pre-integration residual factor between adjacent time points; Steps 1-4: Combine the ranging observations between the ultra-wideband tag and each anchor point to construct the ultra-wideband ranging residual factor; Steps 1-5: Using the pose, velocity, inertial measurement unit bias, and sensor extrinsic parameters at each moment within the sliding window as the state variables to be estimated, the marginalization prior factor, visual reprojection factor, inertial pre-integration factor, and ultra-wideband ranging factor are uniformly incorporated into the same nonlinear least squares objective function for tightly coupled joint optimization.

3. The VIO-UWB two-layer adaptive robust positioning method considering prior anchor point error and residual scale alignment as described in claim 1, characterized in that, Step 2 describes establishing a priori error model for each anchor point for different ultra-wideband anchor points, using the constant deviation prior and noise scale prior corresponding to each anchor point to perform deviation compensation and residual whitening of ranging observations, and obtaining ultra-wideband ranging residuals that take into account the differences of anchor points. The specific steps are as follows: Step 2-1: Given the spatial coordinates of each ultra-wideband anchor point, calculate the first... The set of calibration residuals for each anchor point ; Step 2-2: Based on the calibration residual set Construct the first Prior constant deviation of each anchor point and noise scale prior ,as follows: ， ,in, To calibrate the residual set The residual elements in; Steps 2-3: Utilize the aforementioned constant deviation prior. Original distance measurement observation Perform bias compensation to obtain the compensated distance measurement observations. ,as follows: ， Steps 2-4: Based on the compensated ranging observations and the current state, predict the first... Ultra-wideband ranging residuals corresponding to each anchor point ,as follows: ,in, The predicted geometric distance is calculated using the noise scale prior. The ultra-wideband ranging residual is subjected to weighted whitening processing.

4. The VIO-UWB two-layer adaptive robust positioning method considering prior anchor point error and residual scale alignment as described in claim 1, characterized in that, Step 3 describes the construction of a residual scale alignment factor, which maps the whitened ultrawideband ranging residual to a dimensionless scale alignment residual comparable to the numerical scale of the visual residual, thereby enhancing the effective influence of ultrawideband constraints in joint optimization. The specific steps are as follows: Step 3-1: Analyze the differences in physical dimensions and numerical scale between visual reprojection residuals and ultra-wideband ranging residuals, and establish the numerical balance relationship of heterogeneous residuals; Step 3-2: Let the equivalent focal length of the camera be... Noise extraction for planar pixels is Construct a dimensionless residual scaling alignment factor ,as follows: Step 3-3: Calculate the ultra-wideband ranging residual. Alignment factor with the residual scale Multiply to obtain the aligned residuals ,as follows: ， Steps 3-4: Combining the noise scale prior from step 2 The dimensionless standard whitening residual was calculated. ,as follows: Furthermore, the standard whitened residuals are introduced into the sliding window joint optimization to mitigate gradient imbalance and Hessian matrix contribution imbalance.

5. The VIO-UWB two-layer adaptive robust positioning method considering prior anchor point error and residual scale alignment as described in claim 1, characterized in that, The two-layer adaptive robust estimation mechanism described in step 4 uses the IGG3 piecewise robust weight function to reduce the weight of abnormal ranging observations at the measurement level, and the Helmert variance component estimation method is used at the observation group level to adaptively update the global weights of the visual observation group, the inertial observation group, and the ultra-wideband observation group. The pose, velocity, and zero-bias state estimation results of the inertial measurement unit at the current moment are output through nonlinear joint optimization. The specific steps are as follows: Step 4-1: In the measurement-level robust weighting stage, the dimensionless residuals obtained by deviation compensation and residual whitening in step 2 and scale alignment in step 3 are used as input variables. Step 4-2: Calculate the corresponding measurement-level robust weights using the IGG3 piecewise robust weight function. This is applied to the ultra-wideband ranging residual term, where the set segmentation threshold is... and ,as follows: Step 4-3: In the observation group-level adaptive update phase, calculate the weighted sum of squared post-hoc residuals for the visual observation group, inertial observation group, and ultra-wideband observation group within the current sliding window. ,in ; Step 4-4: Based on the post-hoc residual statistics of each observation group and the effective number of observations after equivalent reweighting Calculate the unit weighted variance estimate for each observation group. ,as follows: Steps 4-5: Adaptively update the global weights of each observation group using a multiplicative iterative approach. The update format is as follows: ,in, For dimensionless unit variance benchmark, superscript Indicates the number of iterations. This is a truncation function. and Truncation constraints are applied to prevent weight divergence, including upper and lower limits. Steps 4-6: The two-layer adaptive robust estimation is performed using an inner and outer layer iterative method. The inner layer uses the current measurement-level robust weights and the observation group-level global weights to perform sliding window nonlinear optimization. The outer layer updates the global weights of each observation group based on the post-hoc residual statistics. The positioning result is output after iterating until the convergence condition is met or the preset number of iterations is reached.

Images