Multi-agent relative positioning method and device integrating UWB and VIO

Abstract

This disclosure provides a multi-agent relative localization method and apparatus that integrates UWB and VIO, enabling real-time relative localization of multiple agents while suppressing long-term cumulative drift of VIO. First, each agent uses a UWB tag to acquire relative distance measurements with neighboring agents and uses VIO to acquire position measurements and its own displacement, then calculates the relative displacement between the two agents. Based on the geometric relationship between the sensor measurements, an estimator for the relative position of the two agents is designed to estimate the relative position and obtain the estimation error, which serves as the adaptive cooperative localization error between the agents. Finally, using the relative position between the two agents as the optimization variable, an objective function including an adaptive cooperative localization error term is designed, and an unconstrained convex optimization problem solver is used to solve for the estimated relative position between each pair of agents.

Description

Technical Field

[0001] This invention relates to the field of multi-agent system positioning technology, specifically to a multi-agent relative positioning method and apparatus that integrates ultra-wideband (UWB) and visual inertial odometry (VIO). Background Technology

[0002] With advancements in robotics, sensors, and automation technologies, multi-agent systems are finding increasingly widespread applications across various fields. Compared to single agents, multi-agent systems exhibit higher efficiency and greater robustness when handling complex tasks. In GPS-free environments, the relative positioning information between agents is crucial for multi-agent systems performing tasks such as cooperative search and formation control. Because the sensors carried by each agent have limited sensing and communication range, agents typically only acquire information from their neighbors. Therefore, utilizing mutual information between neighbors for relative positioning is more practically meaningful.

[0003] Existing relative positioning methods primarily rely on external infrastructure, such as motion capture systems and UWB positioning systems with fixed anchor points. These methods require the pre-installation and configuration of hardware within the area where the agent performs its task, making them unsuitable for dangerous, unknown, and unstructured environments. Furthermore, while visual inertial odometry (VIO) methods can provide accurate individual positioning information in the short term, the cumulative drift problem becomes increasingly severe over long-term operation, and they cannot provide relative positioning information between agents. Summary of the Invention

[0004] In view of this, the present invention provides a multi-agent relative positioning method and apparatus that integrates UWB and VIO, which can stably and effectively suppress long-term cumulative drift of visual inertial odometry, and can obtain relative positioning results in real time when the multi-agent system performs tasks.

[0005] To solve the above-mentioned technical problems, the present invention is implemented as follows.

[0006] A multi-agent relative localization method integrating UWB and VIO includes:

[0007] Step 1: Each agent uses an anchorless ultra-wideband (UWB) tag to obtain the relative distance measurement between itself and its neighboring agents at time t. Visual inertial odometry (VIO) is used to obtain the position measurement of the agent in the VIO local coordinate system at time t. and its own displacement φ at time t i (t); Calculate the relative displacement φ between agent i and agent j. ij (t);

[0008] Step 2: Based on the geometric relationship between the sensor measurements, determine the relative distance r between the two agents at time t. ij (t) using the relative distance r at time t-1 ij (t-1), relative position p ij (t-1), and the relative displacement φ at time t. ij (t) is used to represent the relative position of the two agents, and an adaptive cooperative localization estimator is designed to estimate the relative position p at time t. Using the information obtained in step 1, the adaptive cooperative localization estimator is used to estimate the relative position p at time t. ij Estimate (t) and determine the estimation error as the adaptive cooperative localization error between agents;

[0009] Step 3: Using the relative positions between the two agents as optimization variables, design an objective function that includes the adaptive cooperative localization error term, and use an unconstrained convex optimization problem solver to solve for the estimated relative positions between the two agents.

[0010] Preferably, step 2 is as follows:

[0011] Based on the geometric relationship between sensor measurements, the relative distance r between two agents i and j at time t is calculated. ij (t) using the relative distance r at time t-1 ij (t-1), relative position p ij (t-1), and the relative displacement φ at time t. ij (t) can be represented as:

[0012]

[0013] The recursive weighted least squares estimator of formula (1) is designed to calculate the relative positions between agents.

[0014]

[0015] Among them, W ij (t) is a diagonal weight matrix, Γ ij (t) is the update matrix at time t, I3 is a 3x3 identity matrix, α is an adjustable weight parameter, ∈ ij (t) is the error generated by equation (1), defined as:

[0016]

[0017] in, Take the estimated value of the relative position at time t-1;

[0018] The adaptive cooperative localization error between agents is taken as ∈ ij The first term of (t) is:

[0019]

[0020] Preferably, in step 3, the objective function includes a sensor measurement error term, an adaptive cooperative positioning error term obtained in step 2, and a loop estimation error term; the physical meaning of the loop estimation error is that the sum of the L relative positions forming a loop is 0.

[0021] Preferably, the objective function is expressed as:

[0022]

[0023] Among them, E r and E o It is the difference between the sensor's measured value and the estimated value, E. c It is the difference between the measured value and the estimated value of the collaborative estimator, E. l It is the loop estimation error;

[0024] These represent the distance measurement set, the visual-inertial odometry measurement set, and the cooperative estimation measurement set, respectively. They include UWB measurement data, VIO measurement data, and error data calculated by the adaptive cooperative localization estimator for all agents at time t; γ and β are adjustable gains. and Let Σ represent the covariance respectively. o and Σ r Mahalanobis distance; ρ(·) represents the loss function; This represents the relative displacement between agent L and agent 1. Let X(t) represent the relative displacement between agent i and agent i+1; X(t) is the optimization variable, which includes the relative position between any two agents at time t. f i f represents the offset of the UWB tag of agent i relative to the origin of the machine. j This represents the offset of the UWB label of agent j relative to the origin of the machine. Let VIO of agent i at time t contain the position measurement value with cumulative drift. Let VIO of agent j at time t contain the position measurement value with cumulative drift.

[0025] Alternatively, when the UWB measurement data uses the average of multiple UWB labels, the objective function is expressed as:

[0026]

[0027] Among them, E r and E oIt is the difference between the sensor's measured value and the estimated value, E. c It is the difference between the measured value and the estimated value of the collaborative estimator, E. l It is the loop estimation error;

[0028] These represent the distance measurement set, the visual-inertial odometry measurement set, and the cooperative estimation measurement set, respectively. They include UWB measurement data, VIO measurement data, and error data calculated by the adaptive cooperative localization estimator for all agents at time t; γ and β are adjustable gains. and Let Σ represent the covariance respectively. o and Σ r Mahalanobis distance; ρ(·) represents the loss function; This represents the relative displacement between agent L and agent 1. Let X(t) represent the relative displacement between agent i and agent i+1; X(t) is the optimization variable, which includes the relative position between any two agents at time t. Each agent is equipped with m UWB tags. Let be the offset of the g-th UWB tag of agent i relative to the origin of the machine. This represents the offset of the h-th UWB tag of agent j relative to the origin of the machine. Let VIO position measurement of agent i at time t, including cumulative drift. Let VIO be the position measurement of agent j at time t, which includes cumulative drift.

[0029] Preferably, ρ(·) adopts the Huber loss function.

[0030] Preferably, the unconstrained convex optimization problem solver used in step 3 is the Ceres Solver solver.

[0031] Preferably, after step 3, the method further includes: using a Savitzky-Golay filter to smooth the optimization result to obtain the final value of the relative position.

[0032] Preferably, each agent is equipped with m UWB tags, then the relative distance measurement value The mean of the distance measurements is used:

[0033]

[0034] in, This represents the distance measurement data between the g-th UWB tag of agent i and the h-th UWB tag of agent j at time t.

[0035] The present invention also provides a multi-agent relative positioning device that integrates UWB and VIO. The device includes an anchorless ultra-wideband (UWB) tag and a visual inertial odometry (VIO) configured for each agent, and also includes a positioning calculation unit. The positioning calculation unit includes a data processing module, an adaptive cooperative positioning error determination module, and a relative position optimization module.

[0036] The UWB tag acquires the relative distance measurement between agent i and neighboring agent j at time t. The data is sent to the positioning calculation unit; the VIO obtains the position measurement value of the agent i at time t in the VIO local coordinate system. and its own displacement φ at time t i (t), is sent to the positioning calculation unit;

[0037] The data processing module is used to utilize the displacement φ of each intelligent agent. i (t), calculate the relative displacement φ between agent i and agent j. ij (t);

[0038] The adaptive cooperative positioning error determination module is used to determine the relative distance r between the two agents at time t based on the geometric relationship between the sensor measurements. ij (t) using the relative distance r at time t-1 ij (t-1), relative position p ij (t-1), and the relative displacement φ at time t. ij (t) is used to represent the relative positions of the two agents, and an adaptive cooperative localization estimator is designed to estimate the relative positions p between the agents at time t. Using information from each agent, the adaptive cooperative localization estimator is used to estimate the relative positions p between the agents at time t. ij Estimate (t) and determine the estimation error as the adaptive cooperative localization error between agents;

[0039] The relative position optimization module is used to design an objective function that includes the adaptive cooperative localization error term, using the relative position between the two agents as the optimization variable, and employing an unconstrained convex optimization problem solver to solve for the estimated relative position between the two agents.

[0040] Beneficial effects:

[0041] (1) This invention effectively combines the omnidirectional ranging capability of UWB with the high-precision short-term state estimation capability of VIO by fusing ranging data from UWB and displacement and local position information from VIO. Through the design of optimized algorithms and adaptive cooperative localization estimators, the relative positioning error of the agent during task execution is significantly reduced, the cumulative drift error of VIO over long-term operation is effectively suppressed, and the overall positioning accuracy of the multi-agent system is improved.

[0042] (2) The solution scheme designed in this invention first uses an estimator based on geometric relationships to solve the relative position of the agents. However, since the estimator is very sensitive to noise in distance measurement data and VIO measurement data, the estimation error of the estimator is used to construct the objective function, thereby improving the overall positioning accuracy of the multi-agent system.

[0043] (3) The multi-agent relative positioning method of the present invention does not rely on external devices, but only on the onboard sensors of the agents to realize the relative positioning of the multi-agent system. It is particularly suitable for unknown, dangerous or unstructured environments, supports the collaborative task execution between agents, and has high robustness and environmental adaptability.

[0044] (4) The present invention installs multiple UWB tags on each smart agent, and reduces the total measurement error of the UWB sensor by using multiple sets of measurement information between different tags, while reducing the impact of possible abnormal measurement values ​​on positioning accuracy.

[0045] (5) This invention uses Ceres Solver to solve nonlinear convex optimization problems, ensuring the efficiency of the calculation process and the global optimality of the solution. The relative position can be calculated in real time during the operation of the multi-agent system, providing timely position information support for the control and path planning of the multi-agent system. Attached Figure Description

[0046] Figure 1 This is a flowchart of the multi-agent relative positioning method that integrates ultra-wideband and visual inertial odometry according to the present invention.

[0047] Figure 2 For sensor measurement networks.

[0048] Figure 3 This is a schematic diagram illustrating loop estimation.

[0049] Figure 4 The trajectory is a two-dimensional plane trajectory for a multi-agent system.

[0050] Figure 5 The localization results and error curves for the multi-agent system are shown.

[0051] Figure 6 This is a schematic diagram of the multi-agent relative positioning device that integrates ultra-wideband and visual inertial odometry according to the present invention. Detailed Implementation

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

[0053] This invention provides a multi-agent relative positioning scheme that integrates ultra-wideband (UWB) and visual inertial odometry (VIO). The scheme uses anchorless UWB tags to obtain the relative distance between agents, uses visual inertial odometry to obtain the local coordinate system position of a single agent, and uses the designed multi-agent relative positioning algorithm to solve for the relative position between any two agents.

[0054] As can be seen, this invention effectively combines the omnidirectional ranging capability of UWB with the high-precision short-term state estimation capability of VIO. Through the optimization of the algorithm and the design of the adaptive cooperative localization estimator, the relative positioning error of the agent during task execution is significantly reduced, the cumulative drift error of VIO over long-term operation is effectively suppressed, and the overall positioning accuracy of the multi-agent system is improved.

[0055] Figure 1 The flowchart of the multi-agent relative positioning method integrating UWB and VIO according to an embodiment of the present invention is shown. This embodiment considers the 3D spatial motion of a multi-agent system composed of N agents, such as... Figure 1 As shown, the method includes the following steps:

[0056] Step 1: Each agent uses an anchorless ultra-wideband (UWB) tag to obtain real-time measurements of its relative distance to neighboring agents. Visual inertial odometry (VIO) is used to obtain the relative displacement φ of the agent within one time period. i (t), and its own position measurement in the visual inertial odometry local coordinate system. Since the size and weight of the sensors that intelligent agents can carry are limited, they usually choose easily obtainable physical quantities as measurement information, such as relative distance and their own displacement.

[0057] After each agent completes its measurement, it exchanges the measurement data with the central agent. The central agent then further utilizes φ in this step. i (t) Calculate the relative displacement φ between agent i and agent j. ij (t).

[0058] Preferably, m UWB tags are loaded onto each agent to obtain the relative distance between agents. The offset of the g-th UWB tag of agent i relative to the origin of the aircraft is: The location of this UWB label in the world coordinate system is: Where p i (t) represents the position of the origin of the body coordinate system in the world coordinate system at time t. Through actual measurement verification, the UWB noise n r ~(0,σ r ) follows a Gaussian distribution, where σ r This represents the noise variance. Distance measurements can be modeled as...

[0059]

[0060] in This represents the distance measurement data between the g-th UWB tag of agent i and the h-th UWB tag of agent j at time t.

[0061] Then the relative distance measurement between agents i and j The average of all label measurements is used to reduce the impact of noise.

[0062]

[0063] Each agent is equipped with a VIO (Voice over Inverter) to obtain its position in the VIO's local coordinate system and its self-displacement between two adjacent positions in the local coordinate system. The self-displacement can be modeled as a first-order Markov process with noise n. O ~(0,Σ O ) follows a Gaussian distribution, where Σ O The noise variance can be modeled as follows:

[0064]

[0065] Where φ i (t) represents the displacement of agent i at time t. The sensor measurement network of a multi-agent system is as follows: Figure 2 As shown. Define the relative displacement φ between agents i and j. ij (t):

[0066] φ ij (t)=φ i (t)-φ j (t)

[0067] Step 2: Design an adaptive cooperative relative positioning estimator.

[0068] This step uses the information obtained in step 1, considers the geometric relationship between sensor measurements, and designs an iterative weighted least squares algorithm to obtain the adaptive cooperative localization error between agents.

[0069] First, considering the geometric relationship between sensor measurements, the relative distance r between the two agents at time t is... ij (t) using the relative distance r at time t-1 ij (t-1), relative position p ij (t-1), and the relative displacement φ at time t. ij (t) is used to represent:

[0070]

[0071] Where r ij (t) and r ij (t-1) represents the relative distance between agents i and j at time t and time t-1. In the above formula, the parameter to be estimated is the relative position p of agents i and j. ij (t), the estimation result is denoted as

[0072] A recursive weighted least squares estimator can be designed to implement p. ij The estimation of (t) is used, but the estimation result of this step is not used as the final result. However, since the estimator is very sensitive to the noise of the distance measurement data and VIO measurement data, the estimation error of the adaptive cooperative positioning estimator is used to construct the objective function of step three.

[0073]

[0074] Among them W ij (t) is a diagonal weight matrix, Γ ij (t) is the update matrix, α is the adjustable weight parameter, ∈ ij (t) is the error generated by equation (1), defined as:

[0075]

[0076] in For UWB distance measurements, the average of all tag measurements is used to reduce the impact of noise.

[0077] Equation (2) will be used in the optimization algorithm of step three to design the objective function of the optimization problem. Define ∈ ij The first term of (t) is

[0078] Step 3: Design an optimized UWB-VIO fusion full-state cooperative relative positioning estimation algorithm.

[0079] This step uses the relative positions between the two agents as the optimization variable, designs an objective function that includes the adaptive cooperative localization error term, and employs an unconstrained convex optimization problem solver to solve for the estimated relative positions between the two agents.

[0080] In practice, the Ceres Solver can be used to solve this nonlinear convex optimization problem and obtain the relative positions between each group of agents.

[0081] The optimization variables are selected as follows: This variable includes the relative positions between any two agents at time t.

[0082] In a preferred embodiment, an optimized method is used to fuse UWB and VIO measurement data. The objective function is designed to include a sensor measurement error term, an adaptive cooperative positioning error term obtained in step two, and a loop estimation error term.

[0083] The objective function is specifically expressed as follows:

[0084]

[0085] The objective function has 4 terms, E r and E o It is the difference between the sensor's measured value and the estimated value, E. c It is the difference between the measured value and the estimated value of the collaborative estimator, E. l This is the loop estimation error, which physically means that the sum of the L relative positions forming a loop should be 0, such as p. 12 (t)+p 23 (t)+p 31 (t) = 0, as Figure 3 As shown.

[0086] in, These represent the distance measurement set, odometry measurement set, and cooperative estimation measurement set, respectively. They include UWB measurement data, VIO measurement data, and error data calculated by the adaptive cooperative localization estimator for all agents at time t. γ and β are adjustable gains. and Let Σ represent the covariance respectively. o and Σ r Mahalanobis distance can eliminate data discrepancies caused by different units of measurement of variables in the objective function, as well as interference from correlations between different variables. ρ(·) represents the loss function, preferably the Huber loss function, which has a good filtering effect on outliers in UWB measurements. E l middle This represents the relative displacement between agent L and agent 1. This represents the relative displacement between agent i and agent i+1; each agent is loaded with m UWB tags. Let be the offset of the g-th UWB tag of agent i relative to the origin of the machine. This represents the offset of the h-th UWB tag of agent j relative to the origin of the machine. The position measurement value containing cumulative drift measured by agent i at time t using VIO. Let VIO be the position measurement value containing cumulative drift of agent j at time t.

[0087] The above formula applies when each agent loads m UWB tags. If the agent loads only one UWB tag, then the above formula... Simplified to:

[0088]

[0089] By introducing UWB relative distance measurements and collaborative estimator errors, the cumulative drift of VIO can be compensated, using information in the global coordinate system to eliminate the cumulative drift error. Equation (3) is an unconstrained nonlinear convex optimization problem that can be solved using the Ceres Solver solver.

[0090] After receiving UWB and VIO data each time, construct the optimization problem of equation (3) and solve it to obtain the current relative position.

[0091] Step 4: Use the Savitzky-Golay filter to smooth the optimization results and obtain the final value of the relative position, which can be used in real time for the control and planning of multi-agent systems.

[0092] The solution in step three will fluctuate around the true value due to the influence of sensor measurement noise. Therefore, introducing a Savitzky-Golay filter to smooth the relative position over a period of time can significantly reduce the influence of measurement noise.

[0093] Based on the above method, the present invention also provides a multi-agent relative positioning device that integrates UWB and VIO, such as... Figure 6 As shown, the device includes anchorless ultra-wideband (UWB) tags and visual inertial odometry (VIO) configured for each agent, as well as a positioning computing unit that can be configured on the central agent or in the cloud. Figure 6 The scheme configured in the central agent is shown.

[0094] The agent uses UWB tags to obtain the relative distance measurement between its own agent i and its neighbor agent j at time t. Send it to the positioning calculation unit.

[0095] VIO obtains the position measurement of agent i in the VIO local coordinate system at time t. and its own displacement φ at time t i (t), is sent to the positioning calculation unit;

[0096] The positioning calculation unit includes a data processing module, an adaptive cooperative positioning error determination module, and a relative position optimization module; among which,

[0097] The data processing module is used to utilize the displacement φ of each intelligent agent. i (t), calculate the relative displacement φ between agent i and agent j. ij (t).

[0098] The adaptive cooperative localization error determination module executes step two, which determines the relative distance r between the two agents at time t based on the geometric relationship between the sensor measurements. ij (t) using the relative distance r at time t-1 ij (t-1), relative position p ij (t-1), and the relative displacement φ at time t. ij (t) is used to represent the relative positions of the two agents, and an adaptive cooperative localization estimator is designed to estimate the relative positions p between the agents at time t. Using information from each agent, the adaptive cooperative localization estimator is used to estimate the relative positions p between the agents at time t. ij Estimate (t) and determine the estimation error as the adaptive cooperative localization error between agents.

[0099] The relative position optimization module executes step three, which involves designing an objective function that includes the adaptive cooperative localization error term, using the relative position between the two agents as the optimization variable. An unconstrained convex optimization problem solver is then used to solve for the estimated relative position between each pair of agents.

[0100] In a preferred embodiment, the relative position optimization module further performs step four, using a Savitzky-Golay filter to smooth the optimization result and obtain the final value of the relative position.

[0101] The following simulation experiment demonstrates the proposed UWB-VIO fusion multi-agent relative positioning method. Let the number of agents be N = 3, and the UWB noise variance be σ. r =0.02m, VIO noise variance is Σ o =0.005m, gain parameter γ=2, ==1.5, given the trajectories of three agents as circular, "M" shaped, and figure-eight shaped respectively. The xy-direction motion trajectories of the three agents are as follows: Figure 4 As shown, the orange curve represents the true position, and the other curves represent the cumulative VIO drift. Under the positioning method proposed in this invention, the obtained xy-direction positioning results and error curves are as follows: Figure 5 As shown, it significantly reduces the cumulative error of VIO, with the error curve fluctuating around zero.

[0102] The specific embodiments described above only illustrate the design principles of the present invention. The shapes and names of the components in this description may differ and are not limited. Therefore, those skilled in the art can modify or make equivalent substitutions to the technical solutions described in the foregoing embodiments; and these modifications and substitutions do not depart from the inventive spirit and technical solutions of the present invention, and should all fall within the protection scope of the present invention.

Claims

1. A multi-agent relative positioning method integrating UWB and VIO, characterized in that, include: Step 1: Each agent uses an anchorless ultrawideband UWB tag to obtain the relative distance measurement between itself and its neighboring agents at time t. The position measurement of the agent in the VIO local coordinate system at time t is obtained using a visual inertial odometry (VIO). and its own displacement φ at time t i (t); Calculate the relative displacement φ between agent i and agent j. ij (t); Step 2: Based on the geometric relationship between the sensor measurements, determine the relative distance r between the two agents at time t. ij (t) using the relative distance r at time t-1 ij (t-1), relative position p ij (t-1), and the relative displacement φ at time t. ij Using (t) as the representation, design an adaptive cooperative localization estimator for the relative position of two agents; Using the information obtained in step 1, the adaptive cooperative localization estimator is used to determine the relative position p at time t. ij Estimate (t) and determine the estimation error as the adaptive cooperative localization error between agents; Step 3: Using the relative positions between the two agents as optimization variables, design an objective function that includes the adaptive cooperative localization error term, and use an unconstrained convex optimization problem solver to solve for the estimated relative positions between the two agents.

2. The method as described in claim 1, characterized in that, Step 2 is as follows: Based on the geometric relationship between sensor measurements, the relative distance r between two agents i and j at time t is calculated. ij (t) using the relative distance r at time t-1 ij (t-1), relative position p ij (t-1), and the relative displacement φ at time t. ij (t) can be represented as: The recursive weighted least squares estimator of formula (1) is designed to calculate the relative positions between agents. Among them, W ij (t) is a diagonal weight matrix, Γ ij (t) is the update matrix at time t, I3 is a 3x3 identity matrix, α is an adjustable weight parameter, ∈ ij (t) is the error generated by equation (1), defined as: in, Take the estimated value of the relative position at time t-1; The adaptive cooperative localization error between agents is taken as ∈ ij The first term of (t) is:

3. The method as described in claim 1, characterized in that, In step 3, the objective function includes a sensor measurement error term, an adaptive cooperative positioning error term obtained in step 2, and a loop estimation error term; the physical meaning of the loop estimation error is that the sum of the L relative positions that form a loop is 0.

4. The method as described in claim 2, characterized in that, The objective function is expressed as: Among them, E r and E o It is the difference between the sensor's measured value and the estimated value, E. c It is the difference between the measured value and the estimated value of the collaborative estimator, E. l It is the loop estimation error; These represent the distance measurement set, the visual-inertial odometry measurement set, and the cooperative estimation measurement set, respectively. They include UWB measurement data, VIO measurement data, and error data calculated by the adaptive cooperative localization estimator for all agents at time r; γ and β are adjustable gains. and Let Σ represent the covariance respectively. o and Σ r Mahalanobis distance; ρ(·) represents the loss function; This represents the relative displacement between agent L and agent 1. Let X(t) represent the relative displacement between agent i and agent i+1; X(t) is the optimization variable, which includes the relative position between any two agents at time t. f i f represents the offset of the UWB tag of agent i relative to the origin of the machine. j This represents the offset of the UWB label of agent j relative to the origin of the machine. Let VIO of agent i at time t contain the position measurement value with cumulative drift. Let VIO of agent j at time t contain the position measurement value with cumulative drift.

5. The method as described in claim 2, characterized in that, The objective function is expressed as: Among them, E r and E o It is the difference between the sensor's measured value and the estimated value, E. c It is the difference between the measured value and the estimated value of the collaborative estimator, E. l It is the loop estimation error; These represent the distance measurement set, the visual-inertial odometry measurement set, and the cooperative estimation measurement set, respectively. They include UWB measurement data, VIO measurement data, and error data calculated by the adaptive cooperative localization estimator for all agents at time t; γ and β are adjustable gains. and Let Σ represent the covariance respectively. o and Σ r Mahalanobis distance; ρ(·) represents the loss function; This represents the relative displacement between agent L and agent 1. Let X(t) represent the relative displacement between agent i and agent i+1; X(t) is the optimization variable, which includes the relative position between any two agents at time t. Each agent is equipped with m UWB tags. Let be the offset of the g-th UWB tag of agent i relative to the origin of the machine. This represents the offset of the h-th UWB tag of agent j relative to the origin of the machine. Let VIO position measurement of agent i at time t, including cumulative drift. Let VIO position measurement of agent j at time t, including cumulative drift. This represents the distance measurement data between the g-th UWB tag of agent i and the h-th UWB tag of agent j at time t.

6. The method as described in claim 4 or 5, characterized in that, ρ(·) uses the Huber loss function.

7. The method as described in claim 4 or 5, characterized in that, The unconstrained convex optimization problem solver used in step 3 is the Ceres Solver solver.

8. The method as described in claim 1, characterized in that, Following step 3, the method further includes: smoothing the optimization results using a Savitzky-Golay filter to obtain the final value of the relative position.

9. The method as described in claim 1, characterized in that, If each agent loads m UWB tags, then the relative distance measurement value The mean of the distance measurements is used: in, This represents the distance measurement data between the g-th UWB tag of agent i and the h-th UWB tag of agent j at time t.

10. A multi-agent relative positioning device integrating UWB and VIO, characterized in that, The device includes anchorless ultra-wideband UWB tags and visual inertial odometry (VIO) configured for each agent, and also includes a positioning calculation unit; the positioning calculation unit includes a data processing module, an adaptive cooperative positioning error determination module, and a relative position optimization module; The UWB tag acquires the relative distance measurement between agent i and neighboring agent j at time t. The data is sent to the positioning calculation unit; the VIO obtains the position measurement value of the agent i at time t in the VIO local coordinate system. and its own displacement φ at time t i (t), is sent to the positioning calculation unit; The data processing module is used to utilize the displacement φ of each intelligent agent. i (t), calculate the relative displacement φ between agent i and agent j. ij (t); The adaptive cooperative positioning error determination module is used to determine the relative distance r between the two agents at time t based on the geometric relationship between the sensor measurements. ij (t) using the relative distance r at time t-1 ij (t-1), relative position p ij (t-1), and the relative displacement φ at time t. ij (t) is used to represent the relative positions of the two agents, and an adaptive cooperative localization estimator is designed to estimate the relative positions p between the agents at time t. Using information from each agent, the adaptive cooperative localization estimator is used to estimate the relative positions p between the agents at time t. ij Estimate (t) and determine the estimation error as the adaptive cooperative localization error between agents; The relative position optimization module is used to design an objective function that includes the adaptive cooperative localization error term, using the relative position between the two agents as the optimization variable, and employing an unconstrained convex optimization problem solver to solve for the estimated relative position between the two agents.

Images