An adaptive UWB-SLAM loose coupling fusion method and system based on graph optimization

Abstract

The application provides an adaptive UWB-SLAM loose coupling fusion method based on graph optimization, comprising: S1: a space-time synchronization module acquires a mobile robot pose estimation value output by a front-end SLAM system and original measurement data generated by communication between a UWB anchor and a UWB tag, unifies the time and coordinate system of the pose estimation value and the original measurement data to a world coordinate system, realizes time and coordinate system alignment, and obtains a coordinate system transformation matrix; S2: an online statistical quality evaluator module acquires UWB distance measurement values, signal strength indication values and precision estimation factors between a UWB tag and a plurality of UWB anchors deployed in an environment, obtains a comprehensive quality score, generates a dynamic information matrix based on the comprehensive quality score; S3: a factor graph optimization module weights UWB distance measurement values in the original measurement values by using the dynamic information matrix, and outputs a robot global path, a current real-time pose of a mobile robot and a UWB anchor coordinate.

Description

Technical Field

[0001] This invention relates to the field of robot autonomous navigation technology, and in particular to an adaptive UWB-SLAM loosely coupled fusion method and system based on graph optimization. Background Technology

[0002] The core technology of autonomous navigation is Simultaneous Localization and Mapping (SLAM), which allows robots to localize themselves while building a map. Mainstream vision-based or LiDAR-based SLAM methods are based on incremental estimation, leading to an unavoidable core flaw: accumulated error (drift). As runtime or distance increases, tiny errors accumulate, causing the map and trajectory to deviate from reality. This problem worsens dramatically in environments containing numerous repetitive structures (such as uniform corridors or warehouses) or reflective surfaces like mirrors / glass. These environments lack distinguishable features, causing sensors (vision and LiDAR) to acquire incorrect or false information, leading to frequent errors in data association and loop closure detection modules. The direct consequence is the rapid accumulation of localization errors, ultimately resulting in catastrophic trajectory drift, causing the robot to become completely lost. To address this challenge, one existing solution is the introduction of Ultra-Wideband (UWB) technology. As an independent global absolute positioning source, it provides continuous, high-precision absolute position information to the SLAM system, effectively correcting accumulated errors and ensuring long-term stable navigation of the robot in challenging environments.

[0003] Most existing UWB-SLAM fusion solutions employ a "tightly coupled" integration approach, which presents significant challenges at the systems engineering level. First, it demands a deep understanding of the main navigation system's internal workings, sometimes even requiring modification of its source code for complex, system-specific customization, leading to high non-repetitive engineering costs. Second, this system-specific approach lacks portability and cannot be applied to other SLAM systems, resulting in severe "vendor lock-in." Finally, tightly coupled systems impose a heavy maintenance burden; any software update to the main system can break the fusion interface, forcing developers to perform costly regression testing and code refactoring. For example, one R&D team chose to tightly couple UWB with a mainstream open-source SLAM system (such as VINS-Mono). They first had to invest months and senior engineers to deeply modify the VINS-Mono underlying nonlinear optimizer source code, incurring substantial development costs. When the team wanted to reuse this mature solution for a new project using LiDAR SLAM (such as FAST-LIO2), they found that because the underlying architecture and data structure of the two systems were completely different, all the previous fusion code was not portable and had to be started from scratch, resulting in a serious technical lock-in. Moreover, even if they only maintained the original project, once the VINS-Mono open-source community released a major version update of the optimization algorithm, the team's original UWB fusion code would immediately become invalid, forcing them to invest in a new round of adaptation, development, and regression testing, making the maintenance burden extremely heavy.

[0004] Even if the system engineering challenges posed by the aforementioned tight coupling are resolved, the inherent physical limitations of UWB technology itself present new and equally severe challenges in the actual fusion process. Theoretically, UWB can achieve centimeter-level ranging by measuring the time-of-flight (ToF) of signals, providing an ideal, drift-free global position reference for SLAM systems. However, UWB is extremely sensitive to non-line-of-sight (NLOS) and multipath propagation effects. When the signal path between the robot and the UWB anchor point is blocked by obstacles (such as walls, devices, or people), the signal will propagate around obstacles, resulting in a measured distance significantly greater than the actual distance. If this data, containing severe NLOS errors, is indiscriminately adopted by the fusion system, it not only fails to correct SLAM drift but also contaminates the entire state estimation, severely compromising the final positioning accuracy.

[0005] Furthermore, anchor point calibration is a major challenge in the deployment of UWB sensors. Before practical application, most solutions require the use of specialized equipment such as laser rangefinders or total stations to manually measure and pre-set the precise three-dimensional coordinates of all UWB anchor points in a time-consuming and tedious process. This step greatly increases deployment costs and complexity, severely limiting the deployment efficiency and flexibility of UWB-assisted navigation systems in large-scale scenarios.

[0006] In summary, there is an urgent need in this field for a novel technical solution that can combine the local accuracy of SLAM with the global drift-free characteristics of UWB, while overcoming the systemic drawbacks of existing tightly coupled integration methods, such as high cost, low flexibility, and poor portability. At the same time, it can solve the problems of dynamic quality assessment of UWB signals and convenient deployment of anchor points. Summary of the Invention

[0007] In view of the shortcomings of the prior art described above, the purpose of this invention is to provide an adaptive UWB-SLAM loosely coupled fusion method based on graph optimization to solve the technical problem of inaccurate positioning.

[0008] To achieve the above objectives, this invention provides an adaptive UWB-SLAM loosely coupled fusion method based on graph optimization, comprising:

[0009] S1: The spatiotemporal synchronization module acquires the mobile robot pose estimation value output by the front-end SLAM system and the raw measurement data generated by the communication between UWB anchors and UWB tags. It unifies the time and coordinate system of the pose estimation value with the raw measurement data into the world coordinate system, realizes time and coordinate system alignment, and obtains the coordinate system transformation matrix.

[0010] S2: The online statistical quality evaluator module acquires the UWB distance measurement value, signal strength indicator value, and accuracy estimation factor between the mobile UWB tag and multiple UWB anchor points deployed in the environment; it evaluates the UWB distance measurement value, signal strength indicator value, and accuracy estimation factor respectively to obtain the quality score, RSSI quality score, and EOP quality score of the statistical UWB distance measurement value; the fusion module multiplies and fuses the quality score, RSSI quality score, and EOP quality score of the statistical UWB distance measurement value to obtain a comprehensive quality score; and a dynamic information matrix is ​​generated based on the comprehensive quality score.

[0011] S3: The factor graph optimization module obtains the mobile robot pose estimation value and UWB anchor point coordinates in the world coordinate system, as well as the UWB distance measurement value, coordinate system transformation matrix and dynamic information matrix. The factor graph optimization module uses the dynamic information matrix to weight the UWB distance measurement value in the original measurement value and outputs the robot global path, the mobile robot's current real-time pose, and the UWB anchor point coordinates.

[0012] Optionally, in step S2, the quality score, RSSI quality score, and EOP quality score of the statistical UWB distance measurement are obtained, respectively, including:

[0013] In the first stage, the online statistical quality evaluator module identifies the UWB distance measurement value and finally outputs a quality score for the UWB distance measurement value to determine whether the UWB distance measurement is an outlier.

[0014] In the second stage, the online statistical quality evaluator module first calculates the predicted value of the received signal strength indication based on the UWB distance measurement, then calculates the residual between the predicted value and the measured value of the received signal strength indication, and finally generates the RSSI quality score reflecting the possibility of non-line-of-sight interference. ;

[0015] In the third stage, the line statistical quality evaluator module will extract data from the original measurement data. The value is calculated using a piecewise function to determine the EOP quality score. .

[0016] Optionally, the first stage includes calculating the predicted UWB distance based on the current UWB anchor point coordinates and the pose estimation value output by the front-end SLAM system; then, constructing an innovation vector from the difference between the measured UWB distance and the predicted UWB distance. Then calculate the covariance matrix of the innovation vector. Finally, the squared Mahalanobis distance of the innovation vector is calculated. The calculated square Mahalanobis distance Will with critical value Compare, if Greater than this critical value At this point, the mass fraction of the UWB distance measurement value is output. ; and if Less than the critical value The mass fraction of this UWB distance measurement value. .

[0017] Optionally, the received signal strength indicator is a predicted value. The calculation formula is as follows:

[0018]

[0019] In the formula, It is at a known, close-range reference distance. The strength of the reference signal measured at the location; It is the path loss exponent, which describes the rate at which signal strength decays with distance; It is the current distance between the UWB anchor point and the UWB tag; It is a Gaussian random variable with a mean of 0 and a standard deviation of σ.

[0020] Optionally, RSSI quality score The calculation formula is:

[0021]

[0022] In the formula, It is a natural exponential function. The RSSI residual is the absolute value of the difference between the predicted RSSI value and the measured RSSI value. This is an adjustable parameter that represents the standard deviation of the RSSI residuals.

[0023] Optionally, in the third stage, if Value less than or equal to preset threshold If the quality score is 1.0, then the quality score is 1.0; if it is greater than or equal to the preset threshold... If the value is zero, the quality score is 0; if the value is between these two, the quality score will increase accordingly. The value decreases linearly as it increases.

[0024] Optionally, EOP quality score The calculation formula is as follows:

[0025]

[0026] In the formula, For the extracted accuracy estimation factor, The accuracy estimation factor for achieving the target, This is an accuracy estimation factor that does not meet the standard.

[0027] The present invention also provides a graph-optimized adaptive UWB-SLAM loosely coupled fusion system, including multiple UWB anchor points deployed in an indoor environment, as well as UWB tags, a main navigation system, and a processing unit mounted on a robot, the processing unit being used to execute the method described above.

[0028] Optionally, the processing unit includes:

[0029] The spatiotemporal synchronization module is used to unify the time and coordinate system of the pose estimation values ​​with the original measurement data to the world coordinate system, thereby achieving time and coordinate system alignment.

[0030] The online statistical quality evaluator module consists of a first module, a second module, a third module, and a fusion module. The first module is used to identify UWB distance measurements and ultimately output a quality score for the UWB distance measurement to determine whether it is an outlier. The second module is used to generate an RSSI quality score that reflects the possibility of non-line-of-sight interference. The second module is used to calculate the EOP quality score. The fusion module is used to multiply and fuse the quality scores of statistical UWB distance measurements, RSSI quality scores, and EOP quality scores to obtain a comprehensive quality score.

[0031] The factor graph optimization module is used to optimize the robot pose and UWB anchor coordinates to minimize the global error function, and outputs the robot's global path, the current real-time pose of the mobile robot, and the UWB anchor coordinates.

[0032] The beneficial effects of this invention are as follows: This invention aims to solve the inherent cumulative drift problem of existing SLAM / VIO systems and overcome the challenges of traditional fusion schemes in system integration, deployment, and robustness. The core of this invention is a lightweight, pluggable UWB fusion module, which acts as a post-processing optimizer, working loosely with any primary navigation system that provides pose and covariance outputs without modifying the original system. This invention includes two key innovations: First, an online statistical quality evaluator that dynamically evaluates measurement quality and assigns corresponding weights based on real-time metadata (such as RSSI and EOP) of the UWB signal, thereby effectively suppressing interference from non-line-of-sight (NLOS) errors and enhancing system robustness; Second, a UWB anchor point adaptive self-calibration mechanism that jointly optimizes the spatial coordinates of the robot's trajectory and UWB anchor points while the robot performs its task, eliminating the tedious work of manually pre-calibrating anchor points. This invention ultimately outputs a globally corrected, drift-free robot trajectory and an accurate self-calibrated anchor point map, providing a localization enhancement solution with universal compatibility, high robustness, and ease of deployment. Attached Figure Description

[0033] Figure 1 This is a schematic diagram of the system hardware architecture of one embodiment of the present invention;

[0034] Figure 2 This is a flowchart of an embodiment of the present invention;

[0035] Figure 3 This is a flowchart illustrating the data acquisition process in an embodiment of the present invention;

[0036] Figure 4 This is a flowchart of the online statistical quality evaluator module described in an embodiment of the present invention;

[0037] Figure 5 This is a flowchart illustrating the coordinate system and time synchronization process performed by the spatiotemporal synchronization module according to an embodiment of the present invention.

[0038] Figure 6 This is a flowchart of the factor graph optimization module described in this embodiment of the invention;

[0039] Figure 7 This is a flowchart of the system output data proposed in the embodiments of the present invention. Detailed Implementation

[0040] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention.

[0041] This invention aims to improve the serious positioning drift or loss problem of existing front-end SLAM systems in environments containing a large number of repetitive structures (such as uniform corridors and warehouses) or reflective surfaces such as mirrors / glass, while overcoming the challenges of traditional solutions in terms of system integration, ease of deployment and environmental robustness.

[0042] Reference Figure 1 In this embodiment, the mobile robot is a robot, meaning the method is applied to a mobile robot, specifically a RANGER MINI 2.0. Furthermore, the hardware involved in this method includes multiple UWB anchors, UWB tags, and an independent main navigation system deployed in the indoor environment. The mobile robot is equipped with a processing unit to execute this method.

[0043] The main navigation system primarily outputs a series of timestamped estimates of the robot's position and orientation. and the covariance matrix used to represent the uncertainty of this estimate. In other words, the output is a data stream containing timestamps, six-DOF pose estimates (3D position and quaternion pose) and their corresponding 6×6 covariance matrices, which is a standardized message type widely used in mainstream development environments such as Robot Operating System (ROS).

[0044] The main navigation system is a sensor array used to provide relative motion estimation for the robot. In some preferred embodiments, the sensor array is a visual inertial unit (VIO) consisting of a depth camera (such as an Intel RealSense D435) and an inertial measurement unit (IMU) capable of outputting high-frequency six-degree-of-freedom (6-DoF) pose. In other embodiments, it may be a combination of LiDAR and IMU, or a separate wheeled odometry system. The main navigation system is connected to the processing unit via a USB cable. In this example, the main navigation system is a front-end SLAM system.

[0045] The UWB tag is a UWB communication module, which is rigidly fixed at a known relative position on the robot body and is used for ranging communication with the UWB anchor point.

[0046] like Figure 2 As shown, this embodiment provides an adaptive UWB-SLAM loosely coupled fusion method based on graph optimization, including:

[0047] S1: The spatiotemporal synchronization module acquires the mobile robot pose estimation value output by the front-end SLAM system and the raw measurement data generated by the communication between UWB anchors and UWB tags. It unifies the time and coordinate system of the pose estimation value with the raw measurement data into the world coordinate system, realizes time and coordinate system alignment, and obtains the coordinate system transformation matrix.

[0048] S2: The online statistical quality evaluator module acquires the UWB distance measurement value, signal strength indicator value, and accuracy estimation factor between the mobile UWB tag and multiple UWB anchor points deployed in the environment; it evaluates the UWB distance measurement value, signal strength indicator value, and accuracy estimation factor respectively to obtain the quality score, RSSI quality score, and EOP quality score of the statistical UWB distance measurement value; the fusion module multiplies and fuses the quality score, RSSI quality score, and EOP quality score of the statistical UWB distance measurement value to obtain a comprehensive quality score; and a dynamic information matrix is ​​generated based on the comprehensive quality score.

[0049] S3: The factor graph optimization module obtains the mobile robot pose estimation value and UWB anchor point coordinates in the world coordinate system, as well as the UWB distance measurement value, coordinate system transformation matrix and dynamic information matrix. The factor graph optimization module uses the dynamic information matrix to weight the UWB distance measurement value in the original measurement value and outputs the robot global path, the mobile robot's current real-time pose, and the UWB anchor point coordinates.

[0050] like Figure 3As shown, before step S1, the pose estimation value of the mobile robot itself and its associated covariance matrix are obtained through the front-end SLAM system; multiple raw measurement values ​​between the UWB tag on the mobile robot and multiple UWB anchors deployed in the environment are obtained through UWB anchors. That is, the data is obtained asynchronously from two independent data sources: the front-end SLAM system and the UWB tag.

[0051] The UWB tag on the robot communicates wirelessly with UWB anchors in the environment at high frequency to acquire various raw measurement data between them. The UWB tag then sends this data to the onboard processing unit via a serial port protocol for the fusion module to read and process. The raw measurement data generated by the communication between the UWB anchor and the UWB tag mainly includes the coordinates of the UWB tag and the UWB anchor, the UWB distance measurement between the UWB tag and the UWB anchor, the actual received signal strength indication measurement (actual RSSI measurement), the accuracy estimation factor (EOP), and other physical layer information.

[0052] Because the front-end SLAM system, UWB anchors, and UWB tags each have independent clocks and initial coordinate systems, two fundamental problems exist before effectively fusing data from the front-end SLAM system and UWB anchors: inconsistent time bases and inconsistent spatial coordinate systems. If fusion is forced without unifying the spatiotemporal bases, the fusion module will incorrectly correlate data from different timestamps or spatial reference systems. This incorrect correlation not only fails to correct the accumulated errors of the front-end SLAM system but also introduces significant data conflicts, severely polluting state estimation and potentially causing the entire fusion algorithm to fail or diverge. Therefore, a unified spatiotemporal base is needed to provide precisely aligned data input for subsequent fusion processing. To achieve this goal, this invention sets up a "spatiotemporal synchronization module," referring to... Figure 5 The specific steps to achieve this are as follows:

[0053] like Figure 5 As shown, the core task of the spatiotemporal synchronization module is to align the data stream of the front-end SLAM system to the spatiotemporal reference of the UWB sensor. The spatiotemporal synchronization module acquires the pose estimation values ​​output by the front-end SLAM system and the raw measurement data generated by the communication between the UWB anchors and UWB tags, and performs two key operations: First, it converts the timestamp of the front-end SLAM system to the time reference of the UWB sensor; second, since the front-end SLAM system automatically establishes an initial coordinate system upon startup, defined as (V), and a coordinate system is also defined between the UWB tags and UWB anchors, which we define as the world coordinate system (W), it is necessary to convert all robot poses represented in the front-end SLAM coordinate system (V) to the world coordinate system (W).

[0054] In this example, synchronizing the time of the front-end SLAM system and the UWB sensor specifically involves online estimation of the time relationship, correcting for unknown initial offsets and frequency drifts in the time of the front-end SLAM system and the UWB sensor, and converting the time of the front-end SLAM system to the time of the UWB sensor. The time relationship between the two is as follows:

[0055]

[0056] In the formula, For UWB anchor point time; is the time of the message from the front-end SLAM system; a is the scale factor, used to correct the frequency difference or drift rate between the two clocks; b is the offset, representing the initial time difference when the two clocks start.

[0057] After completing the online estimation of time relationships a and b, the spatiotemporal synchronization module can then access the timestamps of all front-end SLAM systems. Time base mapped to UWB However, since the front-end SLAM system and the UWB sensor are asynchronously triggered, it is almost impossible for data pairs with perfectly overlapping timestamps after mapping to exist. Therefore, a data interpolation step is required: the system uses the timestamp of each UWB measurement... Based on this, a SLAM pose strictly aligned with the UWB measurement moment is synthesized from the front-end SLAM pose sequence already mapped to the same time domain using a linear interpolation algorithm. In this way, the spatiotemporal synchronization module obtains SLAM pose estimates and UWB distance measurements that are truly "at the same time," and begins to perform coordinate system unification on them, as follows:

[0058] After completing time synchronization, coordinate system one was established for the pose estimation values ​​output by the front-end SLAM system and the distance measurement values ​​between UWB anchor points and tags at the same time, as follows:

[0059] First, this invention uses a rigid body transformation to describe the spatial transformation relationship between the front-end SLAM coordinate system and the global world coordinate system jointly defined by the UWB anchor points and UWB tags. Mathematically, this transformation belongs to a special Euclidean group SE(3), and is usually represented by a 4×4 homogeneous coordinate transformation matrix. To describe:

[0060]

[0061] In the formula, It is a 3×3 rotation matrix that describes the orientation of the front-end SLAM coordinate system (V) relative to the world coordinate system (W); It is a 3×1 translation vector that describes the position of the origin of the front-end SLAM coordinate system (V) in the world coordinate system (W);

[0062] Then, all robot poses represented in the front-end SLAM system coordinate system (V) are transformed to the world coordinate system (W). The transformation formula is as follows:

[0063]

[0064] In the formula, Let W be the robot's pose in the world coordinate system (W). The robot pose is represented in the coordinate system (V) of the front-end SLAM system, using a 4×4 matrix.

[0065] After the spatiotemporal reference (time and coordinates) are aligned, the spatiotemporal synchronization module will correctly correlate the front-end SLAM output pose with the corresponding UWB anchor point and the distance measurement between the coordinates in the same time and coordinate system, laying the foundation for subsequent fusion processing.

[0066] Furthermore, the reliability of the UWB measurement data itself also significantly impacts the fusion results. To ensure the system's robustness in complex real-world environments filled with signal interference, this invention introduces an 'online statistical quality evaluator module'. This module acquires raw UWB measurement data in real time, then filters and judges the data quality of each distance measurement between the UWB tag and all UWB anchor points in the environment, dynamically evaluating its measurement quality score. The online statistical quality evaluator module ultimately outputs a 'dynamic information matrix', which is used in subsequent fusion steps to dynamically weight the UWB distance measurement, thereby intelligently suppressing the interference of unreliable data on the final positioning result.

[0067] like Figure 4 As shown, the "Online Statistical Quality Evaluator Module" employs a three-stage evaluation model to perform real-time, multi-dimensional quality assessment of measurement data. The module acquires raw measurement data, comprehensively evaluates the statistical consistency, non-line-of-sight (NLOS) interference probability, and geometric configuration reliability of each measurement value, and outputs a dynamic information matrix for dynamically weighting UWB distance measurements, including:

[0068] The first stage involves identifying UWB distance measurements and ultimately outputting a quality score for each UWB distance measurement to determine whether it is an outlier. The quality score of the UWB distance measurement is quantified as a binary statistical test score; if the measurement passes the test, then... This indicates that the measured value is statistically consistent; if it fails the test, then... This indicates that the measured value is considered an outlier, and its weight in subsequent fusion will become zero, thus removing the outlier. This step is based on the chi-square test within the Extended Kalman Filter (EKF) framework, aiming to statistically identify and remove outliers that are significantly inconsistent with the current state prediction of the system, in order to assess the statistical consistency of the UWB distance measurements.

[0069] Specifically, outlier measurement data that does not match the current state prediction value is identified and removed from the original UWB distance measurements. This includes: using an extended Kalman filter based on the current UWB anchor point coordinates. pose estimation values ​​output by the front-end SLAM system Calculate the predicted UWB distance. Then, the difference between the measured UWB distance and the predicted UWB distance is used to construct an innovation vector. Then calculate the covariance matrix of the innovation vector. The covariance matrix represents the innovation vector. The uncertainty, which combines the uncertainty of the output pose of the front-end SLAM system and the uncertainty of UWB measurement itself, is expressed as follows:

[0070]

[0071] In the formula, This is the predicted UWB distance value. Robot pose estimation value The partial derivatives; It is the UWB distance prediction value. anchor point coordinates The partial derivatives, It is the pose estimate. The covariance matrix, It is the covariance matrix of the UWB anchor point coordinates. This is the inherent noise of the UWB sensor itself;

[0072] Finally, the squared Mahalanobis distance of the innovation vector is calculated. Its calculation expression is as follows:

[0073]

[0074] in, It is the inverse matrix of the covariance matrix of the innovation vector; It is an innovation vector The transpose of the matrix; This represents the critical value for the chi-square test, a threshold pre-set based on the statistical chi-square distribution. The subscript "1" indicates that the system has 1 degree of freedom, because the UWB distance measurement is a scalar; the subscript... The significance level is a preset probability value (e.g., 0.05 or 0.01) used to define the rigor of the test.

[0075] Calculated square Mahalanobis distance Will with critical value Compare, if Greater than this critical value This indicates that the current UWB measurement value is significantly inconsistent with the current state prediction and will be considered a statistical outlier. In this case, the quality score of the current UWB distance measurement value will be output. This means that the measured value is discarded and will not participate in subsequent optimization; while if Less than the critical value If this is true, then the UWB distance measurement is statistically consistent, meaning it passes the test and the quality score of the UWB distance measurement is output. .

[0076] In the second stage, the online statistical quality evaluator module utilizes the physical layer attribute of the signal, namely the Signal Strength Indication (RSSI), to detect the possibility of non-line-of-sight (NLOS) propagation. This value is a physical layer data actually measured and acquired by the UWB tag during communication. Because signal strength naturally attenuates with increasing distance, and when the signal path is blocked by an obstacle, i.e., NLOS propagation occurs, the signal will experience additional and significant attenuation. Based on this principle, in this stage, the online statistical quality evaluator module first calculates the predicted value of the received signal strength indication based on the UWB distance measurement value. Then, the residuals between the predicted Received Signal Strength Indication (RSSI) value and the measured Received Signal Strength Indication (RSSI) value are calculated. Finally, an RSSI quality score reflecting the likelihood of non-line-of-sight interference is generated. .

[0077] The formula for calculating the RSSI predicted value is as follows:

[0078]

[0079] In the formula, It is at a known, close-range reference distance. The strength of the reference signal measured at the location; It is the path loss exponent, which describes the rate at which signal strength decays with distance; It is the current distance between the UWB anchor point and the UWB tag; It is a Gaussian random variable with a mean of 0 and a standard deviation of σ;

[0080] The final output result is the RSSI quality score. The calculation formula is:

[0081]

[0082] In the formula, It is a natural exponential function. The RSSI residual is the absolute value of the difference between the predicted RSSI value and the measured RSSI value. This is an adjustable parameter that represents the standard deviation of the RSSI residuals.

[0083] In the third stage, the online statistical quality evaluator module uses the Accuracy Estimation Factor (EOP), derived from the field of satellite navigation, to quantify the geometric reliability of the current measurement. The magnitude of the EOP value reflects the quality of the relative geometric configuration between the robot's current pose and the visible UWB anchor point. Generally speaking, the smaller the EOP value, the better the geometric configuration and the higher the achievable positioning accuracy. The online statistical quality evaluator module extracts the EOP from the raw measurement data... The value is calculated using a piecewise function to determine the EOP quality score. .like Value less than or equal to the "good" threshold If the quality score is 1.0, then the quality score is 1.0; if it is greater than or equal to the "difference" threshold... If the value is zero, the quality score is 0; if the value is between these two, the quality score will increase accordingly. The value decreases linearly as it increases. and These are all preset values.

[0084] Among them, EOP quality score The calculation formula is as follows:

[0085]

[0086] In the formula, For the extracted accuracy estimation factor, The accuracy estimation factor for achieving the target, This is an accuracy estimation factor that does not meet the standard.

[0087] Subsequently, the fusion module acquires a series of timestamped robot pose estimates and their associated covariance matrix from the front-end SLAM system, as well as various raw measurements between the UWB tags on the mobile robot and multiple UWB anchors deployed in the environment.

[0088] The fusion module of this invention is designed as an independent software algorithm unit. It interacts loosely with the pose data output by the front-end SLAM system and the measurement data provided by the UWB tag through a well-defined and standardized data interface. This is a modular "plug-and-play" software architecture.

[0089] The fusion module combines the outputs of the three stages into a single comprehensive quality score. That is, the quality score of the statistical UWB distance measurement value. RSSI quality score and EOP quality score Combined into a single comprehensive quality score This invention employs a multiplicative fusion strategy to ensure that failure in the evaluation of any dimension will lead to a significant reduction in the final weight. The calculation formula is as follows:

[0090]

[0091] Finally, a dynamic information matrix is ​​generated for each UWB distance measurement based on the comprehensive quality score. The calculation formula is as follows:

[0092]

[0093] In the formula, It represents the inherent standard deviation of the measurement between the UWB anchor and the UWB tag under conditions of no interference, such as no NLOS, geometric configuration differences, etc.

[0094] After completing the quality assessment of the UWB measurement data and generating a corresponding dynamic information matrix for each UWB distance measurement, the fusion module obtains reliable, dynamically weighted UWB distance measurement data.

[0095] To address the anchor point calibration challenge in UWB sensor deployment, the joint and simultaneous optimization steps further include: assigning an initial estimated spatial location to all anchor points during system initialization, and continuously refining the spatial locations of these anchor points by solving the factor graph optimization problem, thereby achieving adaptive self-calibration of the anchor points.

[0096] A factor graph optimization problem is constructed, which takes the robot pose sequence and the three-dimensional spatial position of UWB anchor points as variables to be optimized. By solving the factor graph optimization problem, the robot's motion trajectory and the spatial coordinates of the UWB anchor points are jointly optimized to minimize the global error. Finally, a robot motion trajectory with global correction and a UWB anchor point map with adaptive calibration are output.

[0097] When solving the factor graph optimization problem, the dynamic information matrix is ​​used to weight the corresponding UWB distance constraints.

[0098] The spatial positions of these anchor nodes are then refined by solving the factor graph optimization problem. The iterative algorithm for solving the factor graph optimization problem is the Gauss-Newton algorithm.

[0099] The factor graph optimization problem includes a first set of variables to be optimized and a second set of variables to be optimized, wherein the first set of variables to be optimized represents the robot pose that changes over time, and the second set of variables to be optimized represents the three-dimensional spatial position of the plurality of UWB anchor points.

[0100] By solving the factor graph optimization problem, the first set of variables to be optimized and the second set of variables to be optimized are jointly and simultaneously optimized to minimize the global error function; outputting a globally corrected robot trajectory and an optimized map of the 3D spatial positions of UWB anchor points. That is, outputting the globally corrected complete robot trajectory from the start of the task to the current moment; outputting the robot's real-time six-DOF pose at the current moment after UWB data correction; and outputting a set of calibrated anchor point coordinates formatted as standard 3D point cloud data.

[0101] The odometry factor, which is composed of pose data output by the main navigation system, constrains the relative pose transformation between robot pose nodes.

[0102] The UWB distance factor, composed of UWB measurement data, constrains the Euclidean distance between the robot pose node and the UWB anchor node.

[0103] The more accurate 3D spatial position of the UWB anchor point obtained after optimization will be used as the initial value for the anchor point position in the next round of factor graph optimization.

[0104] like Figure 6 As shown, factor graph optimization is a back-end optimization method that uses a graph structure to represent state estimation problems. The unknowns to be estimated (state variables, such as robot pose, UWB anchor positions, etc.) are represented as nodes in the graph, and the measurement constraints from sensor observations are represented as factors connecting the nodes. By constructing the residual functions of each factor and weighting them according to their variances, the problem can be transformed into a nonlinear least squares problem. Solving this problem yields a globally consistent solution that minimizes the sum of squared weighted residuals; this solution represents the unknowns being sought.

[0105] After all input data has been preprocessed and spatiotemporally aligned—that is, after the raw measurement data between the robot pose, UWB anchor points, and labels output by the front-end SLAM has been preprocessed and spatiotemporally aligned—the factor graph optimization module begins its work. This module is the computational core of the fusion framework.

[0106] The specific states to be estimated are treated as "nodes" in the factor graph, including: the robot pose in the world coordinate system; and the coordinate system transformation matrix. UWB anchor point coordinates;

[0107] Measurements from different sensors are used as “factors (measurement constraints)” to connect these nodes, including: odometry factor, extrinsic alignment factor, and UWB ranging factor provided by the front-end SLAM system;

[0108] The residual functions of various factors in the factor graph constructed in this invention are defined as follows:

[0109] Odometry factor: This factor is composed of pose measurements output by the front-end SLAM system and is used to constrain the pose nodes at two consecutive time points. and The residual function of this factor is defined as follows:

[0110]

[0111] in, This refers to the two consecutive poses output by the front-end SLAM system. and The calculated relative pose transformation, This refers to the two poses estimated from the factor graph. and The relative pose transformation is calculated from the pose optimization value output by the front-end SLAM system. Ideally, if the two transformation values ​​are exactly equal, the residual function will result in zero. Therefore, by minimizing this residual factor, the estimated... and It satisfies the relative motion constraints given by the front-end SLAM system.

[0112] Extrinsic alignment factor: This factor is used to constrain the transformation matrix between the SLAM coordinate system (V) and the world coordinate system (W). It outputs the raw pose of the front-end SLAM. (In SLAM coordinate system) Estimated pose in graph optimization (In the world coordinate system) correlated. The residual function of this factor is defined as:

[0113]

[0114] in, This represents the transformation matrix being optimized in the factor graph. Ideally, equal At this point, the residual function calculation result is zero. Therefore, by minimizing this factor residual, the optimizer can be forced to solve for an optimal rotation matrix. This makes the original pose output by the SLAM system... It can "align" to the optimized estimated pose with minimal error. superior.

[0115] UWB Distance Factor: This factor is calculated from the distance measurement between the UWB tag and the anchor point. The structure is used to constrain the robot's pose nodes at time k. and UWB anchor point coordinate nodes The residual function of this factor is defined as follows:

[0116]

[0117] in, It is the offset vector of the UWB tag installation position relative to the robot's geometric center. It is based on the current optimized pose The predicted position of the UWB label in the world coordinate system (W). This represents the position of anchor point j in the world coordinate system (W). This residual is a scalar and ideally should be zero. Therefore, by minimizing this residual factor, the estimated robot pose and anchor point coordinates can be forced to satisfy all true ranging constraints of UWB.

[0118] After constructing the factor graph, the robot pose, coordinate transformation matrix, and UWB anchor point position in the world coordinate system are jointly solved by minimizing the weighted sum of squares of all factor residuals. The solution steps are as follows:

[0119] The objective function for optimization is to minimize the weighted sum of squares of all factor errors (Mahanobis distance), i.e., to find an optimal set of robot poses. Anchor point location Rotation matrix This minimizes the sum of the Mahalanobis distances of all factor errors. Mathematically, this is expressed as solving a nonlinear least squares problem, with the following formula:

[0120]

[0121] in, , , These are information matrices for the odometer factor, UWB ranging factor, and extrinsic alignment factor, respectively. For the output of the online statistical quality evaluator module, Then a reasonable value needs to be manually set based on the average performance of the front-end SLAM system. It is the pose output by the front-end SLAM system. The inverse of the associated covariance matrix. The information matrix acts as a weighting factor, quantifying our level of confidence in the corresponding measurements.

[0122] Since the objective function to be solved is highly nonlinear, an iterative optimization method is required. The solution steps are as follows:

[0123] The factor graph optimization module starts with an initial estimate and, in each iteration, calculates an approximation of the objective function with respect to the Jacobian and Hessian matrices of all variables to be optimized. It then solves a system of linear equations to obtain an optimal update increment. This increment is then applied to the current state estimate (robot pose in world coordinates; coordinate transformation matrix). (UWB anchor point location): ;

[0124] This iterative process is repeated until the convergence condition is met (the update increment is small enough or the maximum number of iterations is reached). Upon convergence, the system obtains a globally consistent set of optimal solutions: the updated robot pose, anchor point positions, and coordinate transformation matrices. These updated parameters will be used in the next optimization loop and distributed to downstream modules.

[0125] Reference Figure 7 The downstream data publishing module is responsible for publishing the fused, optimized, high-precision, and globally consistent results in a standardized format. Under the Robot Operating System (ROS), this data exchange is accomplished through a "Topic" mechanism. The topics published by this data publishing module include:

[0126] Robot global path: The entire robot motion trajectory from the start of the task to the current moment, which has been completely smoothed and optimized by the backend factor graph, i.e., the pose of each moment is superimposed.

[0127] Current real-time pose: The robot's pose estimate in the world coordinate system (W) at the current moment, after global correction using UWB data;

[0128] UWB anchor point coordinates: This output contains all UWB anchor points in the environment, and the optimized high-precision three-dimensional spatial coordinates are obtained. At this point, the adaptive calibration of the UWB sensor is also completed.

[0129] UWB Anchor Point Cloud: This is a visualization of "anchor point coordinates". The system collects all the 3D coordinates of the calibrated anchor points and formats them into a standard 3D point cloud.

[0130] This invention proposes a loosely coupled fusion method for UWB absolute positioning and SLAM relative positioning within a graph optimization framework. This method is not limited to a specific SLAM front-end (vision and LiDAR are both applicable) nor to a specific type of mobile robot platform. Its core idea lies in maximizing the advantages of two complementary positioning technologies (SLAM and UWB) through an adaptive back-end optimization system. This not only helps improve robot positioning in environments containing numerous repetitive structures (such as uniform corridors or warehouses) or reflective surfaces like mirrors / glass, but also overcomes the core pain points of existing technologies in system integration, deployment, and maintenance.

[0131] Through the above description of the embodiments, those skilled in the art can clearly understand that this application can be implemented in hardware or by means of software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solution of this application can be embodied in the form of a software product. The software product can be stored in a non-volatile storage medium (such as CD-ROM, USB flash drive, mobile hard drive, etc.) and includes several instructions to cause a computer device (such as personal computer, server, or network device, etc.) to execute the methods described in the various implementation scenarios of this application.

[0132] In the embodiments provided in this application, it should be understood that the disclosed robots and methods can also be implemented in other ways. The robot and method embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, program segment, or part of code, which includes one or more executable instructions for implementing a specified logical function. It should also be noted that each block in a block diagram and / or flowchart, and combinations of blocks in block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions. Furthermore, the functional modules in the various embodiments of this application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0133] This invention aims to address the inherent cumulative drift problem in existing SLAM / VIO systems and overcome the challenges of traditional fusion schemes in system integration, deployment, and robustness. The core of this invention is a lightweight, pluggable UWB fusion module that acts as a post-processing optimizer, working loosely with any primary navigation system that provides pose and covariance outputs without modifying the original system. This invention includes two key innovations: First, an online statistical quality evaluator that dynamically assesses measurement quality and assigns corresponding weights based on real-time metadata (such as RSSI and EOP) of the UWB signal, effectively suppressing interference from non-line-of-sight (NLOS) errors and enhancing system robustness; Second, a UWB anchor point adaptive self-calibration mechanism that jointly optimizes the spatial coordinates of the robot's trajectory and UWB anchor points while the robot performs its task, eliminating the tedious work of manually pre-calibrating anchor points. This invention ultimately outputs a globally corrected, drift-free robot trajectory and an accurate self-calibrated anchor point map, providing a universally compatible, highly robust, and easily deployable localization enhancement solution.

[0134] The core of the technical solution provided in this application is a lightweight, pluggable, UWB fusion modular architecture. As a post-processing optimizer, it works loosely coupled with any SLAM system that provides standard pose and covariance outputs, without requiring modification to the original system and possessing universal compatibility. Simultaneously, through an integrated online statistical quality evaluator and a UWB anchor point adaptive self-calibration mechanism, it can intelligently suppress interference such as NLOS and eliminate the tedious work of manually pre-calibrating anchor points. This invention fundamentally solves the core pain points of existing technologies in system integration, deployment, and robustness, providing a high-precision, highly robust, and easily deployable positioning enhancement solution.

[0135] Example 2

[0136] An adaptive UWB-SLAM loosely coupled fusion system based on graph optimization includes multiple UWB anchor points deployed in an indoor environment, as well as UWB tags, a main navigation system, and a processing unit mounted on a robot. The processing unit is used to execute the method described in Embodiment 1.

[0137] The processing unit includes:

[0138] The spatiotemporal synchronization module is used to unify the time and coordinate system of the pose estimation values ​​with the original measurement data to the world coordinate system, thereby achieving time and coordinate system alignment.

[0139] The online statistical quality assessment module consists of a first module, a second module, a third module, and a fusion module. The first module is used to identify UWB distance measurements and finally outputs a quality score for the UWB distance measurement to determine whether it is an outlier. The second module is used to generate an RSSI quality score that reflects the likelihood of non-line-of-sight interference. The second module is used to calculate the EOP quality score. The fusion module is used to multiply and fuse the quality scores of statistical UWB distance measurements, RSSI quality scores, and EOP quality scores to obtain a comprehensive quality score.

[0140] The factor graph optimization module is used to optimize the robot pose and UWB anchor coordinates to minimize the global error function, and outputs the robot's global path, the current real-time pose of the mobile robot, and the UWB anchor coordinates.

[0141] Example 3

[0142] A non-transitory computer-readable storage medium storing instructions that, when executed by a processor, cause the processor to perform the method described in Embodiment 1.

[0143] The above embodiments are merely illustrative of the principles and effects of the present invention and are not intended to limit the invention. Any person skilled in the art can modify or alter the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or alterations made by those skilled in the art without departing from the spirit and technical concept disclosed in the present invention should still be covered by the claims of the present invention.

Claims

1. An adaptive UWB-SLAM loosely coupled fusion method based on graph optimization, characterized in that, include: S1: The spatiotemporal synchronization module acquires the mobile robot pose estimation value output by the front-end SLAM system and the raw measurement data generated by the communication between UWB anchors and UWB tags. It unifies the time and coordinate system of the pose estimation value with the raw measurement data into the world coordinate system, realizes time and coordinate system alignment, and obtains the coordinate system transformation matrix. S2: The online statistical quality evaluator module acquires the UWB distance measurement value, signal strength indicator value, and accuracy estimation factor between the mobile UWB tag and multiple UWB anchor points deployed in the environment; it evaluates the UWB distance measurement value, signal strength indicator value, and accuracy estimation factor respectively to obtain the quality score, RSSI quality score, and EOP quality score of the statistical UWB distance measurement value; the fusion module multiplies and fuses the quality score, RSSI quality score, and EOP quality score of the statistical UWB distance measurement value to obtain a comprehensive quality score; and a dynamic information matrix is ​​generated based on the comprehensive quality score. S3: The factor graph optimization module obtains the mobile robot pose estimation value and UWB anchor point coordinates in the world coordinate system, as well as the UWB distance measurement value, coordinate system transformation matrix and dynamic information matrix. The factor graph optimization module uses the dynamic information matrix to weight the UWB distance measurement value in the original measurement value and outputs the robot global path, the mobile robot's current real-time pose, and the UWB anchor point coordinates.

2. The method according to claim 1, characterized in that, In step S2, the quality scores, RSSI quality scores, and EOP quality scores of the statistical UWB distance measurements are obtained, including: In the first stage, the online statistical quality evaluator module identifies the UWB distance measurement value and finally outputs a quality score for the UWB distance measurement value to determine whether the UWB distance measurement is an outlier. In the second stage, the online statistical quality evaluator module first calculates the predicted value of the received signal strength indication based on the UWB distance measurement, then calculates the residual between the predicted value and the measured value of the received signal strength indication, and finally generates the RSSI quality score reflecting the possibility of non-line-of-sight interference. ; In the third stage, the line statistical quality evaluator module will extract data from the original measurement data. The value is used to calculate the EOP quality score through a piecewise function. .

3. The method according to claim 2, characterized in that, The first stage includes calculating the predicted UWB distance based on the current UWB anchor point coordinates and the pose estimation value output by the front-end SLAM system; then, the difference between the measured UWB distance and the predicted UWB distance is used to construct an innovation vector. Then calculate the covariance matrix of the innovation vector. Finally, the squared Mahalanobis distance of the innovation vector is calculated. The calculated square Mahalanobis distance Will with critical value Compare, if Greater than this critical value At this point, the mass fraction of the UWB distance measurement value is output. ; and if Less than the critical value The mass fraction of this UWB distance measurement value. .

4. The method according to claim 2, characterized in that, Received signal strength indication prediction value The calculation formula is as follows: In the formula, It is at a known, close-range reference distance. The strength of the reference signal measured at the location; It is the path loss exponent, which describes the rate at which signal strength decays with distance; It is the current distance between the UWB anchor point and the UWB tag; It is a Gaussian random variable with a mean of 0 and a standard deviation of σ.

5. The method according to claim 4, characterized in that, RSSI quality score The calculation formula is: In the formula, It is a natural exponential function. The RSSI residual is the absolute value of the difference between the predicted RSSI value and the measured RSSI value. This is an adjustable parameter that represents the standard deviation of the RSSI residuals.

6. The method according to claim 2, characterized in that, In the third stage, if Value less than or equal to preset threshold If the quality score is 1.0, then the quality score is 1.0; if it is greater than or equal to the preset threshold... If the value is zero, the quality score is 0; if the value is between these two, the quality score will increase accordingly. The value decreases linearly as it increases.

7. The method according to claim 6, characterized in that, EOP quality score The calculation formula is as follows: In the formula, For the extracted accuracy estimation factor, The accuracy estimation factor for achieving the target, This is an accuracy estimation factor that does not meet the standard.

8. An adaptive UWB-SLAM loosely coupled fusion system based on graph optimization, characterized in that, It includes multiple UWB anchor points deployed in an indoor environment, as well as UWB tags, a main navigation system, and a processing unit mounted on the robot, the processing unit being used to perform the method described in any one of claims 1-7.

9. The system according to claim 8, characterized in that, The processing unit includes: The spatiotemporal synchronization module is used to unify the time and coordinate system of the pose estimation values ​​with the original measurement data to the world coordinate system, thereby achieving time and coordinate system alignment. The online statistical quality evaluator module consists of a first module, a second module, a third module, and a fusion module. The first module is used to identify UWB distance measurements and ultimately output a quality score for the UWB distance measurement to determine whether it is an outlier. The second module is used to generate an RSSI quality score that reflects the possibility of non-line-of-sight interference. The second module is used to calculate the EOP quality score. The fusion module is used to multiply and fuse the quality scores of statistical UWB distance measurements, RSSI quality scores, and EOP quality scores to obtain a comprehensive quality score. The factor graph optimization module is used to optimize the robot pose and UWB anchor coordinates to minimize the global error function, and outputs the robot's global path, the current real-time pose of the mobile robot, and the UWB anchor coordinates.

Images