Novel improved Robust ESKF fusion positioning method for UWB-VIO initialization

Abstract

The invention discloses a novel UWB-VIO initialized improved Robust ESKF fusion positioning method, and the method comprises the following steps: 1, collecting UWB, IMU and camera data, carrying out the continuous estimation of a local pose of a mobile carrier, and obtaining an initial pose track defined in a VIO local coordinate system; 2, extracting a VIO track direction based on a sliding window; 3, UWB multi-anchor point direction vector construction and confidence modeling are carried out; 4, multi-anchor-point direction consistency joint initialization is carried out; and a fifth step of performing fusion positioning on the UWB-VIO based on the improved Robust ESKF. According to the method, the error accumulation and drift of VI-SLAM in a complex indoor environment are effectively inhibited, and the global consistency of the system is improved.

Description

Technical Field

[0001] This invention belongs to the field of multi-sensor fusion positioning and integrated navigation technology, specifically involving an improved Robust ESKF fusion positioning method with novel UWB-VIO initialization. Background Technology

[0002] In recent years, mobile robots have been increasingly widely used in intelligent manufacturing, unmanned delivery, security inspection, and service robots, placing higher demands on their autonomous localization and navigation accuracy. Autonomous navigation and localization of mobile robots in mixed indoor and outdoor environments is one of the important research directions in the field of intelligent robotics. Their localization accuracy and stability directly affect the performance of core functions such as robot perception, path planning, and control. The reliability and accuracy of the localization system are core elements for achieving autonomous navigation. Visual-Inertial Odometry (VIO) is widely used in autonomous mobile platforms due to its compact structure, low cost, and high localization accuracy. VIO achieves high-frequency state estimation by fusing visual and inertial information; however, it is affected by changes in ambient lighting, feature sparsity, and inertial drift, resulting in scale uncertainty and accumulated errors in its estimation results, making it difficult to maintain global consistency in long-term or large-space tasks.

[0003] To overcome the drift problem of VIO (Virtual Identification and Positioning), researchers generally employ external global positioning sources for auxiliary fusion. Currently commonly used global positioning methods include Global Navigation Satellite System (GNSS), Ultra-Wideband (UWB), lidar, and WiFi positioning. While GNSS can provide absolute position information in outdoor environments, signal obstruction is severe in areas such as inside buildings, underground parking lots, or urban canyons. Radar can achieve high-precision mapping and positioning based on environmental geometry, but it is costly, power-intensive, and sensitive to dynamic environmental changes. In contrast, UWB positioning technology offers high temporal resolution, low power consumption, and strong multipath resistance, providing stable global constraints in complex indoor environments, making it an ideal complement to VIO. Therefore, UWB and VIO fusion positioning has gradually become a research hotspot.

[0004] Existing research can be mainly divided into three categories: the first category is loosely coupled methods, which independently calculate the UWB position and VIO trajectory and then perform coordinate-level alignment and correction; the second category is tightly coupled methods, which directly introduce the UWB ranging equation into the filtering or optimization framework and solve it together with the VIO state to achieve joint estimation; the third category is joint modeling methods based on deep learning or graph optimization, which achieve nonlinear fusion by learning the scale or estimating the alignment parameters.

[0005] While existing methods have made some progress in accuracy and robustness, these fusion methods generally rely on independent coordinate system transformation and initialization steps. Since the VIO coordinate system is defined in a local inertial frame, while the UWB anchor point coordinates are in the global world coordinate system, there is an inconsistency in scale and heading. Traditional approaches typically require obtaining transformation parameters through offline calibration or external measurements, which not only increases system deployment costs but may also introduce significant initialization errors and secondary error propagation in dynamic environments. Therefore, how to solve the UWB-VIO initialization problem has become a key issue in achieving UWB-VIO fusion positioning. The essence of UWB-VIO initialization is to directly unify the two coordinate systems and determine the scale at the algorithm level. Many scholars have proposed using initialization strategies in the fusion stage to achieve coordinate system unification through scale estimation, orientation constraints, or multi-anchor point geometric information. From the perspective of research content, existing work can be broadly categorized into three types: The first type introduces UWB distance observations into the VIO backend optimization process, achieving adaptive estimation of the scale factor. A representative approach is to directly incorporate UWB ranging residuals into the VIO nonlinear optimization framework, allowing scale information and camera trajectory to be iteratively updated in the same optimization space. The second type utilizes UWB orientation constraints, such as establishing a joint constraint model using UWB signal angle of arrival and time-of-flight information, enabling the system to correct heading errors in real time during the fusion process. The third type introduces multi-anchor geometric constraints, simultaneously optimizing rotation and scale by minimizing distance errors, achieving coordinate unification without offline calibration. Current UWB-VIO fusion initialization research is gradually shifting from traditional offline calibration methods to online, real-time multi-parameter joint estimation frameworks. However, how to simultaneously achieve real-time integrated solution of scale, heading, and coordinate transformation in dynamic environments, stably acquire UWB ranging information under multipath interference and signal instability, and maintain VIO observability in feature degradation scenarios remain pressing challenges.

[0006] To address the aforementioned issues, this paper proposes a novel high-precision positioning method that fuses UWB-VIO initialization with an improved Robust Error-State Kalman Filter (Robust ESKF). To resolve the inconsistency between scale drift and heading estimation during the initialization phase, this paper introduces scale factor and heading angle as joint optimization variables, establishing a novel multi-anchor point orientation consistency constraint model for VIO and UWB for joint optimization. This algorithm unifies the two coordinate systems, satisfying real-time requirements and improving the initialization accuracy and stability of the system under dynamic environments and multipath interference. To address the vulnerability of UWB to abnormal ranging and noise interference during the fusion process, this paper designs an improved residual adaptive weighted robust filtering strategy to effectively suppress the abnormal observation effects caused by multipath and non-line-of-sight (NLOS) in UWB ranging, thereby obtaining high-precision and robust pose estimation results. Summary of the Invention

[0007] An improved Robust ESKF fusion localization method with novel UWB-VIO initialization includes the following steps:

[0008] Step 1: Based on the synchronized image and IMU data, construct a visual inertial odometry system to continuously estimate the local pose of the moving vehicle and obtain the initial pose trajectory defined in the VIO local coordinate system.

[0009] Step 2: Based on the VIO trajectory in Step 1, a sliding window mechanism is introduced to statistically process the displacement vectors of adjacent poses within the window and extract smooth and stable local motion direction vectors to reduce the impact of instantaneous visual noise and inertial drift on direction estimation.

[0010] Step 3: Based on the known spatial coordinates of multiple UWB base stations and the corresponding ranging information, construct ranging direction vectors from each base station to the mobile tag, and assign confidence weights to each direction vector according to the UWB signal strength or ranging quality index to reduce the impact of abnormal ranging on subsequent estimation.

[0011] Step 4: Based on the consistency of direction between the VIO trajectory direction vector in Step 2 and the UWB ranging direction vector in Step 3, construct a joint optimization model, with the scale factor and heading angle as parameters to be estimated.

[0012] Step 5: After initialization, based on the improved Robust ESKF UWB–VIO fusion localization model, the fusion state is represented as an error correction relative to the VIO estimate. The error state is then time-predicted based on the kinematic model.

[0013] And update the error covariance simultaneously.

[0014] Step 2 introduces a sliding window mechanism based on the VIO trajectory described in Step 1. This mechanism statistically processes the displacement vectors of adjacent poses within the window to extract smooth and stable local motion direction vectors, thereby reducing instantaneous visual noise and inertial drift.

[0015] The impact on the estimate;

[0016] The specific steps are as follows:

[0017] In VIO systems, visual feature matching and inertial integration are susceptible to transient noise, illumination changes, and motion blur, leading to random fluctuations in trajectory direction estimation between adjacent frames. Directly using the raw VIO output for UWB-assisted initialization introduces inconsistencies in direction, affecting the accuracy and stability of heading angle estimation. To reduce this type of local direction noise, this paper introduces a sliding window mechanism for local direction estimation of the VIO trajectory, obtaining a temporally continuous and geometrically consistent motion direction vector. Let p be the three-dimensional position output by the VIO system in the time series. i At time k, a sliding window of length L is W. k ={p k-L ,p k-L+1 ,...,p k}, in window W k Within, the unit direction between adjacent frames is calculated by analyzing continuous trajectories.

[0018] The vector is averaged to obtain the local principal direction:

[0019]

[0020] This method effectively suppresses high-frequency noise components caused by visual errors and inertial drift while preserving the local geometric features of the VIO trajectory.

[0021] Step 3 involves constructing a ranging direction vector from each base station to the mobile tag based on the known spatial coordinates of multiple UWB base stations and the corresponding ranging information. A confidence weight is then assigned to each direction vector based on the UWB signal strength or ranging quality index to reduce the impact of abnormal ranging on subsequent estimations.

[0022] The specific steps are as follows:

[0023] Since the orientation distribution of UWB anchor points is an absolute geometric structure defined in the world coordinate system, while the VIO forward direction is defined in the local IMU coordinate system, the heading angle determines the relative rotation of these two coordinate systems in the horizontal plane. Therefore, as long as there are more than two anchor points, the distribution is not perfectly symmetrical, and the robot's motion path changes, the heading angle will be geometrically deterministic and observable. After obtaining the robot's own motion direction given by the VIO at each moment, the UWB ranging direction is calculated.

[0024] A good quality heading angle can be obtained by minimizing the VIO direction and the UWB ranging direction, based on the anchor point position.

[0025] By applying VIO trajectory estimation, the unit direction vector from the anchor point to the label position is obtained as follows:

[0026]

[0027] In the formula, s is the scaling factor, and R z (θ) is a rotation matrix with respect to the heading angle θ. It is the location of the j-th UWB base station, and This represents the unit direction vector from the UWB base station to the robot in the i-th frame;

[0028] Because UWB ranging accuracy is susceptible to signal attenuation and multipath effects, the spatial distribution of the UWB ranging direction vector exhibits random shifts and instabilities, thus interfering with the joint estimation of scale and heading angles. To suppress the impact of abnormal ranging values ​​caused by UWB signal attenuation or multipath reflection on the joint estimation of scale and heading angles, this paper constructs confidence weights based on the UWB received signal strength to ensure the statistical stability and reliability of the UWB ranging direction vector. The confidence weights are constructed based on the received signal strength as follows:

[0029]

[0030] In the formula, RSS max This represents the maximum signal strength.

[0031] The consistency of direction between the VIO trajectory direction vector based on step 2 and the UWB ranging direction vector based on step 3, as described in step 4, is used as a constraint to construct a joint optimization model, with the scale factor and heading angle as parameters to be estimated.

[0032] The specific steps are as follows:

[0033] After obtaining the VIO trajectory direction and UWB direction after sliding window smoothing, the alignment of the two coordinate systems at the global level is achieved by minimizing the angle difference between the VIO direction vector and the UWB direction vector. Geometrically, when the heading angle is correctly estimated, the VIO trajectory direction should be consistent with the distance distribution of multiple anchor points after rotation to the UWB coordinate system, thereby achieving the unification of the two coordinate systems. To improve the global observability of heading angle and scale, a global kernel matrix is ​​constructed to constrain the consistency between the trajectory direction and the geometric structure of the anchor points, thereby enhancing the stable estimation capability of heading angle and scale.

[0034] Assuming that at least two UWB anchor points are used in the initialization phase, when the anchor point spatial distribution is reasonable, the joint optimization constitutes a least squares problem with global geometric constraints in the heading angle dimension, thereby avoiding the single anchor point degradation problem. To simplify the calculation, this paper introduces a structural consistency kernel matrix to describe the geometric constraint strength of the anchor point direction on the overall heading angle and scale estimation.

[0035] Define the VIO directional covariance tensor:

[0036]

[0037] This tensor describes the distribution of the main motion directions of the VIO trajectory within the sliding window and the correlation between these directions. It is a statistical representation of the trajectory directions in local space. Geometrically, Q... vio The principal eigenvectors represent the main motion trends of the VIO trajectory, while the eigenvalue distribution reflects the degree of dispersion of the motion direction;

[0038] Define the direction tensor kernel matrix corresponding to each UWB base station as follows:

[0039]

[0040] This tensor reflects the overall distribution and geometric consistency of the UWB ranging direction in space, and has stable global reference properties. In order to achieve global alignment between the VIO direction and the UWB ranging direction in a statistical sense, the Frobenius norm between the two directional structure tensors can be minimized to simplify the calculation:

[0041]

[0042] Expanding the Frobenius norm:

[0043]

[0044] For scale *s*, the function is a quadratic concave function; therefore, for any given θ, the optimal solution for scale *s* is:

[0045]

[0046] Substituting equation (8) into equation (7), we get:

[0047]

[0048] This step decouples the scale and angle optimization variables, transforming the problem into a single-variable nonlinear optimization problem. Furthermore, minimizing L... * (θ) is equivalent to maximizing the trace. This paper solves the remaining nonlinear least squares problem using Ceres Solver. By employing Ceres' automatic differentiation and trust region optimization strategy (Levenberg–Marquardt, LM), efficient joint estimation of heading angle and scale parameters is achieved. This approach can significantly improve convergence speed while ensuring numerical stability, meeting the real-time and accuracy requirements of the mobile robot initialization phase.

[0049] After initialization, as described in step 5, the UWB–VIO fusion localization model based on the improved Robust ESKF is used to represent the fusion state as an error correction relative to the VIO estimate. The error state is then predicted in time based on the kinematic model, and the error covariance is updated synchronously.

[0050] The specific steps are as follows:

[0051] After the UWB-VIO initialization phase, the system has obtained an initial state with uniform scale and heading. At this point, it can enter the dynamic fusion phase to achieve global robust positioning. This paper utilizes an improved residual-weighted Robust ESKF to reduce the impact of outlier observations during the filtering update phase. Using VIO as a reference benchmark, Kalman filtering only estimates the deviation relative to VIO, and UWB ranging is used to correct the cumulative drift of VIO. This method employs an error state modeling approach, where the state vector represents the error correction amount estimated relative to VIO.

[0052] δX k =[δx k ,δy k ,δz k ,δv x ,δv y ,δv z ] T (10)

[0053] In the formula, δp k =[δx k ,δy k ,δz k ] T δv is the position error correction relative to the VIO estimate. k =[δvx ,δv y ,δv z ] T This indicates the amount of speed error correction;

[0054] Unlike traditional KF, ESKF does not update directly in the state space, but instead performs linearization and updates in the error space, and then corrects the master state through error injection. A constant-velocity motion model is used for the correction, and the discrete-time state transition equation is as follows:

[0055]

[0056] State transition matrix:

[0057]

[0058] Error covariance propagation:

[0059]

[0060] The initialization phase has completed the unification of scale and heading angle, allowing for the transformation from VIO to UWB coordinate system:

[0061]

[0062] The UWB ranging equation is defined as:

[0063] ρ i =h i (X k )+v i =||p k -r i ||+v i (15)

[0064] In the formula r i For the coordinates of the i-th UWB base station, during filtering and updating, linearization is performed at the fusion position of the previous time step to obtain a stable Jacobian matrix:

[0065]

[0066] To mitigate the impact of NLOS and gross error ranging on filter stability, this paper introduces a robust weighting function based on residual magnitude within the ESKF framework. First, the innovation vector is constructed, and the residual of the innovation vector is calculated:

[0067]

[0068] Calculate the residual covariance:

[0069]

[0070] An improved weighting function is used to dynamically adjust the measurement noise covariance through a robust factor: Robust factor ω i Defined as:

[0071]

[0072] In the formula, k0 and k1 are threshold parameters;

[0073] This mechanism enables adaptive weighting of observation variance, which automatically reduces or eliminates outlier ranging measurements, significantly enhancing the system's robustness to NLOS environments.

[0074] Recalculate the new information covariance:

[0075]

[0076] Calculate the filter gain parameters and update the state:

[0077]

[0078] The final fused position is obtained by subtracting the correction amount from the Kalman filter estimate from the VIO estimate:

[0079]

[0080] In the formula p k Position estimation after VIO transformation to UWB coordinate system This is the position correction amount estimated by the Kalman filter.

[0081] Beneficial effects of this invention:

[0082] 1. This invention proposes a UWB-VIO joint initialization method with multi-anchor point orientation consistency constraints, which simultaneously solves the scale and heading angle at the optimization level, eliminating the dependence of traditional methods on external coordinate alignment steps.

[0083] 2. The present invention proposes an improved residual weighted Robust ESKF fusion framework, which dynamically adjusts the observation noise covariance during the filtering update stage to enhance the robustness of the system in complex indoor environments.

[0084] 3. This invention proposes and implements a field experimental system for a robot platform. Multiple sets of comparative experiments were conducted in a narrow corridor scenario where both UWB and vision were interfered with. The results show that the method in this paper is significantly better than the existing schemes in terms of root mean square error, average error, and maximum error. Attached Figure Description

[0085] Figure 1 This is a flowchart of an improved Robust ESKF fusion positioning method for novel UWB-VIO initialization according to the present invention;

[0086] Figure 2 This is a flowchart illustrating step 2 of one embodiment of the present invention.

[0087] Figure 3 This is a flowchart illustrating step 3 of one embodiment of the present invention;

[0088] Figure 4 This is a flowchart illustrating step 4 of one embodiment of the present invention.

[0089] Figure 5 This is a flowchart illustrating step 5 of one embodiment of the present invention;

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

[0091] Figure 7 This is a trajectory comparison diagram of the present invention's solution and total station data according to one embodiment of the present invention;

[0092] Figure 8 This is an error comparison diagram of one embodiment of the present invention. Detailed implementation method:

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

[0094] In this embodiment of the invention, a novel improved Robust ESKF fusion localization method with UWB-VIO initialization is described, such as... Figure 1 As shown, it includes the following steps:

[0095] Step 1: Based on the synchronized image and IMU data, construct a visual inertial odometry system to continuously estimate the local pose of the moving vehicle and obtain the initial pose trajectory defined in the VIO local coordinate system.

[0096] Step 2: Based on the VIO trajectory in Step 1, a sliding window mechanism is introduced to statistically process the displacement vectors of adjacent poses within the window and extract smooth and stable local motion direction vectors to reduce the impact of instantaneous visual noise and inertial drift on direction estimation.

[0097] The specific steps are as follows:

[0098] In VIO systems, visual feature matching and inertial integration are susceptible to transient noise, illumination changes, and motion blur, leading to random fluctuations in trajectory direction estimation between adjacent frames. Directly using the raw VIO output for UWB-assisted initialization introduces inconsistencies in direction, affecting the accuracy and stability of heading angle estimation. To reduce this type of local direction noise, this paper introduces a sliding window mechanism for local direction estimation of the VIO trajectory, obtaining a temporally continuous and geometrically consistent motion direction vector. Let p be the three-dimensional position output by the VIO system in the time series. i At time k, a sliding window of length L is W. k ={p k-L ,p k-L+1 ,…,p k}, in window W k Within this framework, the local principal direction is obtained by calculating the unit direction vector between adjacent frames for continuous trajectories and averaging them.

[0099]

[0100] This method effectively suppresses high-frequency noise components caused by visual errors and inertial drift while preserving the local geometric features of the VIO trajectory.

[0101] Step 3: Based on the known spatial coordinates of multiple UWB base stations and the corresponding ranging information, construct ranging direction vectors from each base station to the mobile tag, and assign confidence weights to each direction vector according to the UWB signal strength or ranging quality index to reduce the impact of abnormal ranging on subsequent estimation.

[0102] The specific steps are as follows:

[0103] Since the orientation distribution of UWB anchor points is an absolute geometric structure defined in the world coordinate system, while the VIO forward direction is defined in the local IMU coordinate system, the heading angle determines the relative rotation of these two coordinate systems in the horizontal plane. Therefore, as long as there are more than two anchor points, the distribution is not perfectly symmetrical, and the robot's motion path changes, the heading angle will be geometrically deterministic and observable. After obtaining the robot's own motion direction given by the VIO at each moment, the UWB ranging direction vector is calculated. By minimizing the VIO direction and the UWB ranging direction, a good quality heading angle can be obtained. Based on the anchor point position and VIO trajectory estimation, the unit direction vector from the anchor point to the tag position is obtained as follows:

[0104]

[0105] In the formula, s is the scaling factor, and R z (θ) is a rotation matrix with respect to the heading angle θ. It is the location of the j-th UWB base station, and This represents the unit direction vector from the UWB base station to the robot in the i-th frame;

[0106] Because UWB ranging accuracy is susceptible to signal attenuation and multipath effects, the spatial distribution of the UWB ranging direction vector exhibits random shifts and instabilities, thus interfering with the joint estimation of scale and heading angles. To suppress the impact of abnormal ranging values ​​caused by UWB signal attenuation or multipath reflection on the joint estimation of scale and heading angles, this paper constructs confidence weights based on the UWB received signal strength to ensure the statistical stability and reliability of the UWB ranging direction vector. The confidence weights are constructed based on the received signal strength as follows:

[0107]

[0108] In the formula, RSS max This represents the maximum signal strength.

[0109] Step 4: Based on the consistency of direction between the VIO trajectory direction vector in Step 2 and the UWB ranging direction vector in Step 3, construct a joint optimization model, with the scale factor and heading angle as parameters to be estimated.

[0110] The specific steps are as follows:

[0111] After obtaining the VIO trajectory direction and UWB direction after sliding window smoothing, the alignment of the two coordinate systems at the global level is achieved by minimizing the angle difference between the VIO direction vector and the UWB direction vector. Geometrically, when the heading angle is correctly estimated, the VIO trajectory direction should be consistent with the distance distribution of multiple anchor points after rotation to the UWB coordinate system, thereby achieving the unification of the two coordinate systems. To improve the global observability of heading angle and scale, a global kernel matrix is ​​constructed to constrain the consistency between the trajectory direction and the geometric structure of the anchor points, thereby enhancing the stable estimation capability of heading angle and scale.

[0112] Assuming that at least two UWB anchor points are used in the initialization phase, when the anchor point spatial distribution is reasonable, the joint optimization constitutes a least squares problem with global geometric constraints in the heading angle dimension, thereby avoiding the single anchor point degradation problem. To simplify the calculation, this paper introduces a structural consistency kernel matrix to describe the geometric constraint strength of the anchor point direction on the overall heading angle and scale estimation.

[0113] Define the VIO directional covariance tensor:

[0114]

[0115] This tensor describes the distribution of the main motion directions of the VIO trajectory within the sliding window and the correlation between these directions. It is a statistical representation of the trajectory directions in local space. Geometrically, Q...vio The principal eigenvectors represent the main motion trends of the VIO trajectory, while the eigenvalue distribution reflects the degree of dispersion of the motion direction;

[0116] Define the direction tensor kernel matrix corresponding to each UWB base station as follows:

[0117]

[0118] This tensor reflects the overall distribution and geometric consistency of the UWB ranging direction in space, and has stable global reference properties. In order to achieve global alignment between the VIO direction and the UWB ranging direction in a statistical sense, the Frobenius norm between the two directional structure tensors can be minimized to simplify the calculation:

[0119]

[0120] Expanding the Frobenius norm:

[0121]

[0122] For scale *s*, the function is a quadratic concave function; therefore, for any given θ, the optimal solution for scale *s* is:

[0123]

[0124] Substituting equation (8) into equation (7), we get:

[0125]

[0126] This step decouples the scale and angle optimization variables, transforming the problem into a single-variable nonlinear optimization problem. Furthermore, minimizing L... * (θ) is equivalent to maximizing the trace. This paper solves the remaining nonlinear least squares problem using Ceres Solver. By employing Ceres' automatic differentiation and trust region optimization strategy (Levenberg–Marquardt, LM), efficient joint estimation of heading angle and scale parameters is achieved. This approach can significantly improve convergence speed while ensuring numerical stability, meeting the real-time and accuracy requirements of the mobile robot initialization phase.

[0127] Step 5: After initialization, based on the improved Robust ESKF UWB–VIO fusion localization model, the fusion state is represented as the error correction amount relative to the VIO estimate. The error state is predicted in time based on the kinematic model, and the error covariance is updated synchronously.

[0128] The specific steps are as follows:

[0129] After the UWB-VIO initialization phase, the system has obtained an initial state with uniform scale and heading. At this point, it can enter the dynamic fusion phase to achieve global robust positioning. This paper utilizes an improved residual-weighted Robust ESKF to reduce the impact of outlier observations during the filtering update phase. Using VIO as a reference benchmark, Kalman filtering only estimates the deviation relative to VIO, and UWB ranging is used to correct the cumulative drift of VIO. This method employs an error state modeling approach, where the state vector represents the error correction amount estimated relative to VIO.

[0130] δX k =[δx k ,δy k ,δz k ,δv x ,δv y ,δv z ] T (10)

[0131] In the formula, δp k =[δx k ,δy k ,δz k ] T δv is the position error correction relative to the VIO estimate. k =[δv x ,δv y ,δv z ] T This indicates the amount of speed error correction;

[0132] Unlike traditional KF, ESKF does not update directly in the state space, but instead performs linearization and updates in the error space, and then corrects the master state through error injection. A constant-velocity motion model is used for the correction, and the discrete-time state transition equation is as follows:

[0133]

[0134] State transition matrix:

[0135]

[0136] Error covariance propagation:

[0137]

[0138] The initialization phase has completed the unification of scale and heading angle, allowing for the transformation from VIO to UWB coordinate system:

[0139]

[0140] The UWB ranging equation is defined as:

[0141] ρ i =h i (X k )+v i =||p k -r i ||+v i (15)

[0142] In the formula r i For the coordinates of the i-th UWB base station, during filtering and updating, linearization is performed at the fusion position of the previous time step to obtain a stable Jacobian matrix:

[0143]

[0144] To mitigate the impact of NLOS and gross error ranging on filter stability, this paper introduces a robust weighting function based on residual magnitude within the ESKF framework. First, the innovation vector is constructed, and the residual of the innovation vector is calculated:

[0145]

[0146] Calculate the residual covariance:

[0147]

[0148] An improved weighting function is used to dynamically adjust the measurement noise covariance through a robust factor: Robust factor ω i Defined as:

[0149]

[0150] In the formula, k0 and k1 are threshold parameters;

[0151] This mechanism enables adaptive weighting of observation variance, which automatically reduces or eliminates outlier ranging measurements, significantly enhancing the system's robustness to NLOS environments.

[0152] Recalculate the new information covariance:

[0153]

[0154] Calculate the filter gain parameters and update the state:

[0155]

[0156] The final fused position is obtained by subtracting the correction amount from the Kalman filter estimate from the VIO estimate:

[0157]

[0158] In the formula p kPosition estimation after VIO transformation to UWB coordinate system This is the position correction amount estimated by the Kalman filter.

[0159] In this invention example, a combined positioning mobile platform was constructed. The platform moved along a pre-designed route, simultaneously collecting visual, inertial, and UWB data, with a total travel distance of approximately 250m. A four-wheeled skid-steer robot data acquisition platform was built based on the Songling SCOUT2.0 wheeled robot. The robot chassis adopted a differential drive structure, and the UWB module used a LinkTrack-P series ultra-wideband positioning system. Eight fixed base stations were set up and installed in the corridor of the teaching building, at a height of approximately 2.0m. The mobile terminal was installed on the top of the robot. The UWB ranging frequency was 5Hz, and ranging was performed using a two-way time-of-flight method. The visual-inertial module used a Basler Aca2500-20gc camera and an Xsens MTI-670 IMU module to acquire synchronized images and inertial data. The image resolution was 648×512, the frame rate was 10Hz, and the IMU sampling frequency was 200Hz. A Leica TS50 auto-tracking total station was used to provide a reference trajectory for the experiment. The experiment was conducted in a long corridor of a university teaching building, only 3 meters wide. Anchor points were deployed along the corridor, resulting in UWB anchor points being almost collinear with the robot's direction of movement. This deployment failed to achieve stable distance constraints, leading to reduced observability in vertical positioning. Furthermore, the presence of walls, metal doors, and glass windows caused UWB signals to be susceptible to attenuation and multipath interference, affecting overall fusion accuracy. The long corridor environment is a typical visual degradation scenario, characterized by sparse feature point distribution, highly repetitive structures, and monotonous textures, significantly reducing the robustness of visual feature extraction and matching. In addition, uneven lighting and localized insufficient illumination within the corridor reduced the image signal-to-noise ratio and made feature tracking prone to loss, further exacerbating inertial drift and scale uncertainty. In such environments with weak texture, repetitive structures, and low illumination, the positioning accuracy and long-term stability of the VIO system significantly degrade. To comprehensively evaluate the long-distance positioning accuracy and stability of the proposed fusion method in complex and variable environments, poor local lighting, and weak texture scenes, the robot moved along the corridor from the starting point to the end point, completing a continuous motion trajectory with a total distance of approximately 250m.

[0160] Figure 7The trajectory comparison diagram of the mobile robot localization results shows that the novel UWB-VIO initialization and improved Robust ESKF fusion localization method proposed in this invention exhibits good stability and continuity throughout the entire process. Its trajectory almost perfectly matches the reference trajectory, with no significant drift or oscillation in regions a–e. This result demonstrates that the proposed method can effectively suppress coordinate system synchronization errors and scale drift, and maintain high-precision global consistency even in environments with insufficient illumination and multipath interference. The proposed method outperforms the comparative algorithms in terms of trajectory continuity, global consistency, and trajectory accuracy. Especially in long-distance operation and multipath interference environments, the proposed orientation consistency initialization and improved Robust ESKF fusion framework can simultaneously improve the system's accuracy and robustness. This framework significantly reduces the impact of scale drift and heading deviation on subsequent fusion optimization by introducing orientation consistency constraints during the initialization phase; during the filtering update process, it dynamically suppresses abnormal observations using an improved residual weighted robustness mechanism, thereby maintaining the stability of state estimation in complex and variable environments, poor local illumination, and long-distance scenes with weak textures. Experimental results show that this method not only effectively suppresses the trajectory misalignment and attitude jump problems existing in VIO and traditional UWB-VIO, but also achieves smooth and high-precision trajectory estimation in UWB signal occlusion, weak texture and local low illumination environments, fully verifying the robustness and generalization ability of the algorithm.

[0161] To fully verify the positioning performance of the algorithm proposed in this invention, this experiment uses experimental data collected by a mobile robot to perform positioning calculations and comparative analysis. The positioning error is calculated using the reference trajectory provided by the total station and the calculated mobile robot position information. The corresponding error curves are shown below. Figure 8 As shown, VIO's overall error level is relatively low, but short-term fluctuations still occur in areas with varying lighting or sparse textures, mainly due to unstable visual feature matching and accumulated IMU integration errors. Once the system reacquires sufficient feature points, the error quickly recovers, exhibiting typical error recovery characteristics of visual-inertial systems. For example, in the middle section of the corridor, due to localized dim lighting and sparse features, the number of visual feature matches for VIO decreases, leading to short-term fluctuations in the error curve. Meanwhile, UWB positioning in this section is affected by the near-collinearity of anchor points, resulting in generally low UWB positioning accuracy, large overall error amplitude, and drastic fluctuations, reaching a maximum of over 4m. This indicates that it is significantly affected by geometric degradation and multipath interference in narrow corridor environments. Especially in areas where anchor points are nearly collinear, the vertical positioning constraint weakens, leading to obvious peaks and irregular jumps in the error curve. The proposed method achieves error compensation under geometric degradation conditions through joint optimization of UWB and VIO, ensuring the trajectory remains continuous and stable in this area, maintaining high accuracy.

[0162] For UWB-VIO, the overall error level is significantly lower than that of a single system, indicating that the complementarity of the two sensors plays a positive role in global positioning constraints. However, at turning positions... Figure 5 In region 3 of the model, poor lighting conditions and drastic changes in motion direction lead to a decrease in the number of visual feature matches for VIO, resulting in significant fluctuations in the error curve. Traditional UWB–VIO fusion methods are susceptible to coordinate transformation errors and inconsistent heading estimates, causing local oscillations in the error curve. Furthermore, this method exhibits trajectory misalignment in the initial stage, causing the error curve to rise rapidly in the early stages, and occasional slight oscillations occur during the fusion process. In contrast, our proposed method introduces a direction consistency constraint, significantly reducing the impact of scale drift and heading deviation on subsequent fusion optimization. Moreover, an improved residual weighted robustness mechanism is introduced in the filtering stage, which adaptively weakens the influence of abnormal ranging, enabling the system to maintain smooth error changes even under conditions of varying lighting and multipath interference.

[0163] Traditional algorithms exhibit varying degrees of error fluctuation under different scenarios, while the novel UWB-VIO initialization-based improved Robust ESKF fusion positioning method proposed in this paper demonstrates the most stable error variation trend throughout the entire operation. Its error curve exhibits a low-amplitude oscillation, indicating that the algorithm maintains stable convergence even under multi-source uncertainties. Thanks to the VIO and UWB coordinate system unification achieved in the initialization phase, and the improved residual weighted robustness mechanism introduced in the filtering phase, the system can adaptively mitigate the influence of abnormal ranging and significantly suppress the impact of multipath interference. Even under complex conditions such as uneven illumination and UWB occlusion, the proposed method maintains trajectory smoothness and error stability, fully validating the accuracy advantage and robustness of the proposed algorithm in complex indoor environments.

[0164] 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 substitutions that can be understood by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A novel improved Robust ESKF fusion positioning method with UWB-VIO initialization, characterized in that, Includes the following steps: Step 1: Based on the synchronized image and IMU data, construct a visual inertial odometry system to continuously estimate the local pose of the moving vehicle and obtain the initial pose trajectory defined in the VIO local coordinate system. Step 2: Based on the VIO trajectory in Step 1, a sliding window mechanism is introduced to statistically process the displacement vectors of adjacent poses within the window and extract smooth and stable local motion direction vectors to reduce the impact of instantaneous visual noise and inertial drift on direction estimation. Step 3: Based on the known spatial coordinates of multiple UWB base stations and the corresponding ranging information, construct ranging direction vectors from each base station to the mobile tag, and assign confidence weights to each direction vector according to the UWB signal strength or ranging quality index to reduce the impact of abnormal ranging on subsequent estimation. Step 4: Based on the consistency of direction between the VIO trajectory direction vector in Step 2 and the UWB ranging direction vector in Step 3, construct a joint optimization model, with the scale factor and heading angle as parameters to be estimated. Step 5: After initialization, based on the improved Robust ESKF UWB–VIO fusion localization model, the fusion state is represented as the error correction amount relative to the VIO estimate. The error state is predicted in time based on the kinematic model, and the error covariance is updated synchronously.

2. The improved Robust ESKF fusion positioning method for novel UWB-VIO initialization according to claim 1, characterized in that, Step 1 describes the construction of a visual inertial odometry system based on synchronized images and IMU data to continuously estimate the local pose of the moving vehicle and obtain the initial pose trajectory defined in the VIO local coordinate system.

3. The improved Robust ESKF fusion positioning method for novel UWB-VIO initialization according to claim 1, characterized in that, Step 2 introduces a sliding window mechanism based on the VIO trajectory in step 1 to perform statistical processing on the displacement vectors of adjacent poses within the window, and extract smooth and stable local motion direction vectors to reduce the influence of instantaneous visual noise and inertial drift on direction estimation. In VIO systems, visual feature matching and inertial integration are susceptible to transient noise, illumination changes, and motion blur, leading to random fluctuations in trajectory direction estimation between adjacent frames. Directly using the raw VIO output for UWB-assisted initialization introduces inconsistencies in direction, affecting the accuracy and stability of heading angle estimation. To reduce this type of local direction noise, this paper introduces a sliding window mechanism for local direction estimation of the VIO trajectory, obtaining a temporally continuous and geometrically consistent motion direction vector. Let p be the three-dimensional position output by the VIO system in the time series. i At time k, a sliding window of length L is W. k ={p k-L ,p k-L+1 ,...,p k }, in window W k Within this framework, the local principal direction is obtained by calculating the unit direction vector between adjacent frames for continuous trajectories and averaging them. This method effectively suppresses high-frequency noise components caused by visual errors and inertial drift while preserving the local geometric features of the VIO trajectory.

4. The improved Robust ESKF fusion positioning method for novel UWB-VIO initialization according to claim 1, characterized in that, Step 3 involves constructing a ranging direction vector from each base station to the mobile tag based on the known spatial coordinates of multiple UWB base stations and the corresponding ranging information. A confidence weight is then assigned to each direction vector based on the UWB signal strength or ranging quality index to reduce the impact of abnormal ranging on subsequent estimations. The specific steps are as follows: Since the orientation distribution of UWB anchor points is an absolute geometric structure defined in the world coordinate system, while the VIO forward direction is defined in the local IMU coordinate system, the heading angle determines the relative rotation of these two coordinate systems in the horizontal plane. Therefore, as long as there are more than two anchor points, the distribution is not perfectly symmetrical, and the robot's motion path changes, the heading angle will be geometrically deterministic and observable. After obtaining the robot's own motion direction given by the VIO at each moment, the UWB ranging direction vector is calculated. By minimizing the VIO direction and the UWB ranging direction, a good quality heading angle can be obtained. Based on the anchor point position and VIO trajectory estimation, the unit direction vector from the anchor point to the tag position is obtained as follows: In the formula, s is the scaling factor, and R z (θ) is a rotation matrix with respect to the heading angle θ. It is the location of the j-th UWB base station, and This represents the unit direction vector from the UWB base station to the robot in the i-th frame; Because UWB ranging accuracy is susceptible to signal attenuation and multipath effects, the spatial distribution of the UWB ranging direction vector exhibits random shifts and instabilities, thus interfering with the joint estimation of scale and heading angles. To suppress the impact of abnormal ranging values ​​caused by UWB signal attenuation or multipath reflection on the joint estimation of scale and heading angles, this paper constructs confidence weights based on the UWB received signal strength to ensure the statistical stability and reliability of the UWB ranging direction vector. The confidence weights are constructed based on the received signal strength as follows: In the formula, RSS max This represents the maximum signal strength.

5. The improved Robust ESKF fusion positioning method for novel UWB-VIO initialization according to claim 1, characterized in that, The consistency of direction between the VIO trajectory direction vector based on step 2 and the UWB ranging direction vector based on step 3, as described in step 4, is used as a constraint to construct a joint optimization model, with the scale factor and heading angle as parameters to be estimated. The specific steps are as follows: After obtaining the VIO trajectory direction and UWB direction after sliding window smoothing, the alignment of the two coordinate systems at the global level is achieved by minimizing the angle difference between the VIO direction vector and the UWB direction vector. Geometrically, when the heading angle is correctly estimated, the VIO trajectory direction should be consistent with the distance distribution of multiple anchor points after rotation to the UWB coordinate system, thereby achieving the unification of the two coordinate systems. To improve the global observability of heading angle and scale, a global kernel matrix is ​​constructed to constrain the consistency between the trajectory direction and the geometric structure of the anchor points, thereby enhancing the stable estimation capability of heading angle and scale. Assuming that at least two UWB anchor points are used in the initialization phase, when the anchor point spatial distribution is reasonable, the joint optimization constitutes a least squares problem with global geometric constraints in the heading angle dimension, thereby avoiding the single anchor point degradation problem. To simplify the calculation, this paper introduces a structural consistency kernel matrix to describe the geometric constraint strength of the anchor point direction on the overall heading angle and scale estimation. Define the VIO directional covariance tensor: This tensor describes the distribution of the main motion directions of the VIO trajectory within the sliding window and the correlation between these directions. It is a statistical representation of the trajectory directions in local space. Geometrically, Q... vio The principal eigenvectors represent the main motion trends of the VIO trajectory, while the eigenvalue distribution reflects the degree of dispersion of the motion direction; Define the direction tensor kernel matrix corresponding to each UWB base station as follows: This tensor reflects the overall distribution and geometric consistency of the UWB ranging direction in space, and has stable global reference properties. In order to achieve global alignment between the VIO direction and the UWB ranging direction in a statistical sense, the Frobenius norm between the two directional structure tensors can be minimized to simplify the calculation: Expanding the Frobenius norm: For scale *s*, the function is a quadratic concave function; therefore, for any given θ, the optimal solution for scale *s* is: Substituting equation (8) into equation (7), we get: This step decouples the scale and angle optimization variables, transforming the problem into a single-variable nonlinear optimization problem. Furthermore, minimizing L... * (θ) is equivalent to maximizing the trace. This paper solves the remaining nonlinear least squares problem using Ceres Solver. By employing Ceres' automatic differentiation and trust region optimization strategy (Levenberg–Marquardt, LM), efficient joint estimation of heading angle and scale parameters is achieved. This approach can significantly improve convergence speed while ensuring numerical stability, meeting the real-time and accuracy requirements of the mobile robot initialization phase.

6. The improved Robust ESKF fusion positioning method for novel UWB-VIO initialization according to claim 1, characterized in that, After initialization, as described in step 5, the UWB–VIO fusion localization model based on the improved Robust ESKF is used to represent the fusion state as an error correction relative to the VIO estimate. The error state is then predicted in time based on the kinematic model, and the error covariance is updated synchronously. The specific steps are as follows: After the UWB-VIO initialization phase, the system has obtained an initial state with uniform scale and heading. At this point, it can enter the dynamic fusion phase to achieve global robust positioning. This paper utilizes an improved residual-weighted Robust ESKF to reduce the impact of outlier observations during the filtering update phase. Using VIO as a reference benchmark, Kalman filtering only estimates the deviation relative to VIO, and UWB ranging is used to correct the cumulative drift of VIO. This method employs an error state modeling approach, where the state vector represents the error correction amount estimated relative to VIO. In the formula, δp k =[δx k ,δy k ,δz k ] T δv is the position error correction relative to the VIO estimate. k =[δv x ,δv y ,δv z ] T This indicates the amount of speed error correction; Unlike traditional KF, ESKF does not update directly in the state space, but instead performs linearization and updates in the error space, and then corrects the master state through error injection. A constant-velocity motion model is used for the correction, and the discrete-time state transition equation is as follows: State transition matrix: Error covariance propagation: The initialization phase has completed the unification of scale and heading angle, allowing for the transformation from VIO to UWB coordinate system: The UWB ranging equation is defined as: ρ i =h i (X k )+v i =||p k -r i ||+v i (15) In the formula r i For the coordinates of the i-th UWB base station, during filtering and updating, linearization is performed at the fusion position of the previous time step to obtain a stable Jacobian matrix: To mitigate the impact of NLOS and gross error ranging on filter stability, this paper introduces a robust weighting function based on residual magnitude within the ESKF framework. First, the innovation vector is constructed, and the residual of the innovation vector is calculated: Calculate the residual covariance: An improved weighting function is used to dynamically adjust the measurement noise covariance through a robust factor: Robust factor ω i Defined as: In the formula, k0 and k1 are threshold parameters; This mechanism enables adaptive weighting of observation variance, which automatically reduces or eliminates outlier ranging measurements, significantly enhancing the system's robustness to NLOS environments. Recalculate the new information covariance: Calculate the filter gain parameters and update the state: The final fused position is obtained by subtracting the correction amount from the Kalman filter estimate from the VIO estimate: In the formula p k Position estimation after VIO transformation to UWB coordinate system This is the position correction amount estimated by the Kalman filter.

Images