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

Abstract

The invention provides a multi-agent relative positioning method and device fusing UWB and VIO, and the method and device can achieve the real-time relative positioning of multiple agents under the condition of restraining the long-term accumulated drift of VIO. Firstly, each agent obtains a relative distance measurement value with a neighbor agent by using a UWB tag, obtains a position measurement value and self displacement of the agent by using VIO, and then calculates the relative displacement between the two agents. Designing an estimator for the relative positions of the two intelligent agents according to the geometrical relationship between the measured values of the sensors, estimating the relative positions of the intelligent agents, and obtaining an estimation error as a self-adaptive cooperative positioning error between the intelligent agents; and finally, by taking the relative position between the two intelligent agents as an optimization variable, designing a target function containing a self-adaptive cooperative positioning error term, and solving a relative position estimation value between every two intelligent agents by adopting an unconstrained convex optimization problem solver.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-agent system positioning, and in particular to a multi-agent relative positioning method and device integrating ultra-wideband (UWB) and visual inertial odometer (VIO). Background Art

[0002] With the advancement of robotics, sensors and automation technology, multi-agent systems are increasingly used in various fields. Compared with a single agent, multi-agent systems show higher efficiency and stronger robustness in handling complex tasks. When multi-agent systems perform tasks such as collaborative search and formation control in a GPS-free environment, the relative positioning information between agents is crucial. Since the sensors carried by each agent have limitations in perception and communication range, agents can usually only obtain information about neighboring agents. Therefore, it is more practical to use the mutual information between neighbors for relative positioning.

[0003] Existing relative positioning methods mainly rely on external infrastructure, such as motion capture systems, UWB positioning systems with fixed anchors, etc. These methods require the installation and configuration of hardware equipment in advance in the area where the agent performs tasks, so they are not suitable for dangerous unknown and unstructured environments. In addition, the visual inertial odometry (VIO) method can provide accurate single-body positioning information in the short term, but the cumulative drift problem will become more serious during long-term operation, and it 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 device integrating UWB and VIO, which can stably and effectively suppress the long-term accumulated drift of the visual inertial odometer, and can obtain relative positioning results in real time when the multi-agent system performs tasks.

[0005] In order to solve the above technical problems, the present invention is implemented as follows.

[0006] A multi-agent relative positioning method integrating UWB and VIO, comprising:

[0007] Step 1: Each agent uses an anchor-free ultra-wideband (UWB) tag to obtain the relative distance measurement between itself and its neighboring agents at time t. The visual inertial odometry (VIO) is used to obtain the position measurement value 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, the relative distance r between the two agents at time t is calculated. ij (t) Use 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 represented by designing an adaptive collaborative positioning estimator for the relative positions of the two agents; using the information obtained in step 1, the adaptive collaborative positioning estimator is used to calculate the relative position p at time t ij (t) and determine the estimation error as the adaptive co-localization error between the agents;

[0009] Step 3: Taking the relative position between the two agents as the optimization variable, design an objective function including the adaptive collaborative positioning error term, and use an unconstrained convex optimization problem solver to solve the relative position estimation value between the two agents.

[0010] Preferably, the step 2 is:

[0011] According to the geometric relationship between the sensor measurements, the relative distance r between the two agents i and j at time t is ij (t) Use 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 expressed as:

[0012]

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

[0014]

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

[0016]

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

[0018] The adaptive collaborative localization error between agents is ∈ ij The first item of (t) is:

[0019]

[0020] Preferably, in step 3, the objective function includes the sensor measurement error term, the adaptive collaborative positioning error term obtained in step 2, and the 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 is the difference between the sensor measurement and the estimated value, E c is the difference between the measured value and the estimated value of the co-estimated term, E l is the loop estimation error;

[0024] They represent the distance measurement set, the visual inertial odometer measurement set, and the collaborative estimation measurement set, which respectively include the UWB measurement data, VIO measurement data, and error data calculated by the adaptive collaborative localization estimator of all agents at time t; γ, β are adjustable gains, and Respectively represent the covariance as Σ o and Σ r Mahalanobis distance; ρ(·) represents the loss function; represents the relative displacement between agent L and agent 1, represents the relative displacement between agent i and agent i+1; X(t) is the optimization variable, including the relative position between any two agents at time t. f i represents the offset of the UWB tag of agent i relative to the origin of the fuselage, f j represents the offset of the UWB tag of agent j relative to the origin of the fuselage, is the position measurement value of the VIO of agent i at time t containing the accumulated drift is the position measurement of agent j’s VIO at time t containing the accumulated drift.

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

[0026]

[0027] Among them, E r and E ois the difference between the sensor measurement and the estimated value, E c is the difference between the measured value and the estimated value of the co-estimated term, E l is the loop estimation error;

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

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

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

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

[0032] Preferably, each intelligent agent is equipped with m UWB tags, then the relative distance measurement value Take the mean of the distance measures:

[0033]

[0034] in, 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 integrating UWB and VIO, the device comprising an ultra-wideband (UWB) tag and a visual inertial odometer (VIO) without anchor points configured for each agent, and a positioning calculation unit; the positioning calculation unit comprises a data processing module, an adaptive collaborative positioning error determination module and a relative position optimization module;

[0036] The UWB tag obtains the relative distance measurement value between the intelligent agent i and the neighboring intelligent agent j at time t Send to the positioning calculation unit; the VIO obtains the position measurement value of the intelligent agent i in the VIO local coordinate system at time t and its own displacement φ at time t i (t), 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 collaborative positioning error determination module is used to calculate the relative distance r of the two agents at time t according to the geometric relationship between the sensor measurement values. ij (t) Use 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 represented by designing an adaptive collaborative positioning estimator for the relative position of the two agents; using the information from each agent, the adaptive collaborative positioning estimator is used to calculate the relative position p between the agents at time t ij (t) and determine the estimation error as the adaptive co-localization error between agents;

[0039] The relative position optimization module is used to design an objective function including the adaptive collaborative positioning error term using the relative position between two agents as the optimization variable, and to solve the relative position estimation value between two agents using an unconstrained convex optimization problem solver.

[0040] Beneficial effects:

[0041] (1) The present invention effectively combines the omnidirectional ranging capability of UWB and the high-precision short-term state estimation capability of VIO by fusing the ranging data of UWB and the displacement and local position information of VIO. Through the design of optimization algorithm and adaptive collaborative positioning estimator, the relative positioning error of the agent in the process of executing the task is significantly reduced, the accumulated drift error of VIO in long-term operation is effectively suppressed, and the overall positioning accuracy of the multi-agent system is improved.

[0042] (2) The solution designed by the present invention first uses an estimator based on geometric relationships to solve the relative position of the agent. However, since the estimator is very sensitive to the noise of distance measurement data and VIO measurement data, only 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 equipment. It can achieve the relative positioning of the multi-agent system by relying solely on the onboard sensors of the agents. It is particularly suitable for unknown, dangerous or unstructured environments, supports collaborative task execution between agents, and has high robustness and environmental adaptability.

[0044] (4) The present invention installs multiple UWB tags on each intelligent body 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) The present invention uses the Ceres Solver to solve nonlinear convex optimization problems, ensuring the efficiency of the calculation process and the global optimality of the solution. During the operation of the multi-agent system, the relative position can be calculated in real time, providing timely position information support for the control and path planning of the multi-agent system. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 This is a flow chart of the multi-agent relative positioning method that integrates ultra-wideband and visual inertial odometer in the present invention.

[0047] Figure 2 Measuring networks for sensors.

[0048] Figure 3 Schematic diagram illustrating loop estimation.

[0049] Figure 4 is the two-dimensional trajectory of the multi-agent system.

[0050] Figure 5 Positioning results and error curves of the multi-agent system.

[0051] Figure 6 Schematic diagram of a multi-agent relative positioning device integrating ultra-wideband and visual inertial odometer according to the present invention. DETAILED DESCRIPTION

[0052] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0053] The present invention provides a multi-agent relative positioning solution integrating ultra-wideband (UWB) and visual inertial odometry (VIO). The solution uses anchor-free 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 the relative position between any two agents.

[0054] It can be seen that the present invention effectively combines the omnidirectional ranging capability of UWB and the high-precision short-term state estimation capability of VIO. Through the design of optimization algorithm and adaptive collaborative positioning estimator, the relative positioning error of the agent in the process of executing the task is significantly reduced, the accumulated drift error of VIO in long-term operation is effectively suppressed, and the overall positioning accuracy of the multi-agent system is improved.

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

[0056] Step 1: Each agent uses an anchor-free ultra-wideband (UWB) tag to obtain the relative distance measurement value between itself and its neighboring agents in real time. Visual inertial odometry (VIO) is used to obtain the relative displacement φ of the agent itself within a time period. i (t), and its own position measurement in the local coordinate system of the visual inertial odometer Since the size and weight of sensors that intelligent agents can carry are limited, easily accessible physical quantities are usually selected as measurement information, such as relative distance, self-displacement, etc.

[0057] After each agent completes the measurement, it exchanges the measurement data with the central agent, which further uses φ i (t) Calculate the relative displacement φ between agent i and agent j ij (t).

[0058] Preferably, m UWB tags are loaded on each intelligent body to obtain the relative distance between intelligent bodies. The offset of the g-th UWB tag of intelligent body i relative to the origin of the fuselage is The position of the UWB tag in the world coordinate system is: where p i (t) is the position of the origin of the body coordinate system in the world coordinate system at time t. It has been verified by actual measurement that the UWB noise n r ~(0,σ r ) obeys a Gaussian distribution, where σ r is the noise variance. The distance measurement can be modeled as

[0059]

[0060] in 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 is Take the average of all label measurements to reduce the effect of noise:

[0062]

[0063] A VIO is installed on each agent to obtain the position of the VIO in the local coordinate system and the displacement between two consecutive positions in the local coordinate system. The displacement can be modeled as a first-order Markov process with a noise n O ~(0,Σ O ) obeys Gaussian distribution, where Σ O is the noise variance, which can be modeled as

[0064]

[0065] where φ i (t) is the displacement of agent i at time t. The sensor measurement network of the 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 collaborative 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 collaborative localization error between agents.

[0069] First, consider the geometric relationship between the sensor measurements and transform the relative distance r between the two agents at time t into ij (t) Use 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 expressed as:

[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 estimated result is recorded as

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

[0073]

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

[0075]

[0076] in is the distance measurement value of UWB. In order to reduce the influence of noise, the average value of all tag measurements is used.

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

[0078] Step three, design an optimized UWB-VIO fusion full-state collaborative relative positioning estimation algorithm.

[0079] In this step, the relative position between the two agents is used as the optimization variable, and the objective function including the adaptive collaborative positioning error term is designed. The unconstrained convex optimization problem solver is used to solve the relative position estimation value between the two agents.

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

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

[0082] In a preferred embodiment, the measurement data of UWB and VIO are fused based on an optimization method, and the objective function is designed to include the sensor measurement error term, the adaptive collaborative positioning error term obtained in step 2, and the loop estimation error term.

[0083] The specific expression of the objective function is:

[0084]

[0085] There are 4 items in the objective function, E r and E o is the difference between the sensor measurement and the estimated value, E c is the difference between the measured value and the estimated value of the co-estimated term, E l is the loop estimation error, and its physical meaning is 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, such as Figure 3 shown.

[0086] in, They represent the distance measurement set, the odometer measurement set, and the collaborative estimation measurement set, respectively, which include the UWB measurement data, VIO measurement data, and error data calculated by the adaptive collaborative positioning estimator of all agents at time t. γ, β are adjustable gains. and Respectively represent the covariance as Σ o and Σ r Mahalanobis distance, which can eliminate the data differences caused by different variable dimensions in the objective function and the interference of correlation between different variables. ρ(·) represents the loss function, preferably the Huber loss function, which has a good filtering effect on UWB measurement outliers. l middle represents the relative displacement between agent L and agent 1, represents the relative displacement between agent i and agent i+1; each agent is equipped with m UWB tags, is the offset of the g-th UWB tag of agent i relative to the origin of the fuselage, represents the offset of the h-th UWB tag of agent j relative to the origin of the fuselage; is the position measurement value with accumulated drift measured by the VIO of agent i at time t It is the position measurement value with accumulated drift measured by the VIO of agent j at time t.

[0087] The above formula is applicable to the case where each intelligent agent is equipped with m UWB tags. If the intelligent agent is equipped with only one UWB tag, then Simplified to:

[0088]

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

[0090] After receiving UWB and VIO data each time, the optimization problem of formula (3) is constructed and solved 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 of step three will fluctuate around the true value due to the influence of sensor measurement noise. Therefore, the introduction of 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 integrating UWB and VIO, such as Figure 6 As shown, the device includes an anchor-free ultra-wideband (UWB) tag and a visual inertial odometer (VIO) configured for each intelligent agent, and also includes a positioning calculation unit, which can be configured on a central intelligent agent or on the cloud. Figure 6 The scheme configured in the central agent is shown.

[0094] The agent uses the UWB tag to obtain the relative distance measurement value between the agent i and the neighboring agent j at time t. Sent to the positioning calculation unit.

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

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

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

[0098] The adaptive collaborative positioning error determination module executes step 2 to calculate the relative distance r between the two agents at time t based on the geometric relationship between the sensor measurements. ij (t) Use 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 represented by designing an adaptive collaborative positioning estimator for the relative position of the two agents; using the information from each agent, the adaptive collaborative positioning estimator is used to calculate the relative position p between the agents at time t ij (t) and determine the estimation error as the adaptive co-localization error between the agents.

[0099] The relative position optimization module executes step 3 to design an objective function including the adaptive collaborative positioning error term using the relative position between the two agents as the optimization variable, and uses an unconstrained convex optimization problem solver to solve the relative position estimation value between the two agents.

[0100] In a preferred embodiment, the relative position optimization module further executes step 4, using a Savitzky-Golay filter to perform smoothing filtering on the optimization result to obtain a final value of the relative position.

[0101] The following simulation experiment is conducted on 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 the three agents are circular, "M" and "8" shaped. The xy direction motion trajectories of the three agents are as follows Figure 4 As shown in Figure 1, the orange curve is the actual position, and the other curves are the results of VIO cumulative drift. Under the positioning method proposed in the present invention, the obtained xy direction positioning results and error curves are shown in Figure 1. Figure 5 As shown, it can be seen that it significantly reduces the cumulative error of VIO, and the error curve fluctuates around zero.

[0102] The above specific embodiments only describe the design principle of the present invention. The shapes and names of the components in the description may be different and are not limited. Therefore, those skilled in the art in the field of the present invention may modify or replace the technical solutions recorded in the above embodiments; and these modifications and replacements do not deviate from the creative purpose 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 anchor-free ultra-wideband (UWB) tag to obtain the relative distance measurement between itself and its neighboring agents at time t. The visual inertial odometry (VIO) is used to obtain the position measurement value 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); Step 2: Based on the geometric relationship between the sensor measurements, the relative distance r between the two agents at time t is calculated. ij (t) Use 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) Represent and design an adaptive co-localization estimator for the relative positions of two agents; Using the information obtained in step 1, the adaptive co-localization estimator is used to calculate the relative position p at time t. ij (t) and determine the estimation error as the adaptive co-localization error between agents; Step 3: Taking the relative position between the two agents as the optimization variable, design an objective function including the adaptive collaborative positioning error term, and use an unconstrained convex optimization problem solver to solve the relative position estimation value between the two agents.

2. The method according to claim 1, characterized in that The step 2 is: According to the geometric relationship between the sensor measurements, the relative distance r between the two agents i and j at time t is ij (t) Use 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 expressed as: The recursive weighted least squares estimator of formula (1) is designed to solve the relative positions between agents. Among them, W ij (t) is the diagonal weight matrix, Γ ij (t) is the update matrix at time t, I3 is the 3*3 identity matrix, α is the adjustable weight parameter, ∈i j (t) is the error generated by equation (1) and is defined as: in, Take the estimated value of the relative position at time t-1; The adaptive collaborative localization error between agents is ∈i j The first item of (t) is:

3. The method according to claim 1, characterized in that In step 3, the objective function includes the sensor measurement error term, the adaptive collaborative positioning error term obtained in step 2, and the 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.

4. The method according to claim 2, characterized in that The objective function is expressed as: Among them, E r and E o is the difference between the sensor measurement and the estimated value, E c is the difference between the measured value and the estimated value of the co-estimated term, E l is the loop estimation error; They represent the distance measurement set, the visual inertial odometer measurement set, and the collaborative estimation measurement set, which respectively include the UWB measurement data, VIO measurement data, and error data calculated by the adaptive collaborative localization estimator of all agents at time t; γ and β are adjustable gains. and Respectively represent the covariance as ∑ o and∑ r Mahalanobis distance; ρ(·) represents the loss function; represents the relative displacement between agent L and agent 1, represents the relative displacement between agent i and agent i+1; X(t) is the optimization variable, including the relative position between any two agents at time t. f i represents the offset of the UWB tag of agent i relative to the origin of the fuselage, f j represents the offset of the UWB tag of agent j relative to the origin of the fuselage, is the position measurement value of the VIO of agent i at time t containing the accumulated drift is the position measurement of agent j’s VIO at time t containing the accumulated drift.

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

6. The method according to claim 4 or 5, characterized in that ρ(·) adopts the Huber loss function.

7. The method according to claim 4 or 5, characterized in that The unconstrained convex optimization problem solver used in step 3 is: Ceres Solver.

8. The method according to claim 1, characterized in that After step 3, the method further includes: using a Savitzky-Golay filter to perform smoothing filtering on the optimization result to obtain a final value of the relative position.

9. The method according to claim 1, characterized in that Each intelligent agent is equipped with m UWB tags, then the relative distance measurement value Take the mean of the distance measures: in, 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 an anchor-free ultra-wideband (UWB) tag and a visual inertial odometer (VIO) configured for each intelligent agent, and also includes a positioning calculation unit; the positioning calculation unit includes a data processing module, an adaptive collaborative positioning error determination module and a relative position optimization module; The UWB tag obtains the relative distance measurement value between the intelligent agent i and the neighboring intelligent agent j at time t Send to the positioning calculation unit; the VIO obtains the position measurement value of the intelligent agent i in the VIO local coordinate system at time t and its own displacement φ at time t i (t), 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 collaborative positioning error determination module is used to calculate the relative distance r of the two agents at time t according to the geometric relationship between the sensor measurement values. ij (t) Use 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 represented by designing an adaptive collaborative positioning estimator for the relative position of the two agents; using the information from each agent, the adaptive collaborative positioning estimator is used to calculate the relative position p between the agents at time t ij (t) and determine the estimation error as the adaptive co-localization error between agents; The relative position optimization module is used to design an objective function including the adaptive collaborative positioning error term using the relative position between two agents as the optimization variable, and to solve the relative position estimation value between two agents using an unconstrained convex optimization problem solver.

Images