Resilient Cooperative Localization Based on Factor Graphs for Multirobot Systems

Abstract

With the advancement of intelligent perception in multirobot systems, cooperative localization in dynamic environments has become a critical component. However, existing multirobot cooperative localization systems still fall short in meeting high-precision localization requirements in Global Navigation Satellite System (GNSS)-denied environments. In this paper, we propose a factor-graph-based resilient cooperative localization (FG-RCL) algorithm for multirobot systems. This algorithm integrates measurements from visual sensors and Ultra-WideBand (UWB) to achieve accurate cooperative state estimation—overcoming the visibility issues of visual sensors within limited fields of view. We utilize the Joint Probabilistic Data Association (JPDA) algorithm to calculate the corresponding probabilities of multiple visual detection measurements between robots and assign them to their respective edges in the factor graph, thereby addressing the data association challenges in visual detection measurements. Finally, simulation results demonstrate that the proposed algorithm significantly reduces the influence of visual detection measurement interference on the performance of cooperative localization. Experimental results indicate that the proposed algorithm outperforms UWB-based and vision-based methods in terms of localization accuracy. The system is implemented using a factor-graph-based optimization approach, and it exhibits scalability and enables plug-and-play for sensors. Furthermore, it demonstrates resilience in abnormal situations.

1. Introduction

For multirobot systems, many applications frequently encounter the challenge of cooperative localization, including formation control [1], cooperative transportation [2], and search-and-rescue missions [3]. The issue of cooperative localization in multirobot systems refers to the detection and localization of mobile robots (often with limited sensor data, such as the relative distance or bearing) relative to other robots or landmarks. This is crucial in multirobot systems as it requires cooperation among the entire team and group. Currently, state estimation for multiple robots in outdoor environments heavily relies on a Global Navigation Satellite System (GNSS) [4]. However, GNSS-based methods are restricted to outdoor environments due to limitations to satellite signals. Furthermore, these methods only provide meter-level accuracy, which falls short for tasks requiring high-precision localization of robots. Other methods involve the use of motion capture systems or traditional Ultra-WideBand (UWB) localization systems for indoor scenarios without satellite signals [5,6,7]. They rely on physical anchors or base station deployment in applications to achieve cooperative localization. Therefore, these solutions are unsuitable for unknown environments, such as large areas or indoors, where deploying or modifying infrastructure is challenging. This limitation restricts the overall performance and application potential of multirobot systems and makes their usage more difficult and expensive.

With the advancement of autonomous perception in robots, visible light sensors, LiDAR, ultrasonic sensors, and numerous other novel sensors are being applied to cooperative localization [8,9,10]. The data provided by these sensors can improve localization accuracy and enable motion tracking, providing efficient and robust state estimation for robots in both indoor and outdoor and known and unknown environments [11]. This is due to their ability to detect and understand the surrounding environment, even if their primary installation purposes might not be for localization. For instance, high-precision maps constructed using LiDAR create a digital surface model represented by discrete points, which includes spatial information required for target localization [12]. However, LiDAR, ultrasonic sensors, and optical detectors are usually costly and less easily deployable compared to camera-based visual sensors. Therefore, there is growing interest in cooperative localization methods employing cameras. Vision-based simultaneous localization and mapping (SLAM) techniques can provide relative positions of robots in the surrounding environment [13]. Additionally, other robots can also be directly detected based on the images captured by the onboard camera, and distance and bearing measurements can be extracted from visual devices. However, they suffer from the disadvantages of a small field of view, short range, and susceptibility to nearby object occlusion. Due to the limitations to the observation angle of visual sensors, relative state estimation outside the field of view of onboard cameras has become a challenging issue. Therefore, a sensor with omni-directional perception capabilities and that is easy to carry, such as UWB modules, is needed to assist the camera with relative measurements. This paper introduces a cooperative localization system based on visual and UWB technologies that can be applied to multirobot frameworks.

There are typically two modes for multirobot cooperative localization: centralized and decentralized. Compared to the centralized framework, decentralized methods reduce the complexity of computation and communication during the cooperative process. In the decentralized framework, each member runs an estimator locally: propagating its state by fusing measurements from its local onboard sensors and relative observations received from team members [14]. In multirobot systems, solving the relative distance and bearing problems in a decentralized manner is preferable and scalable. Huang et al. [15] introduced an adaptive recursive decentralized cooperative localization method for multirobot systems with varying measurement accuracy. Wang et al. [16] proposed a fully decentralized cooperative localization algorithm based on covariance union to address inconsistent state estimation caused by spurious sensor data. Ref. [17] presented a resilient decentralized localization algorithm for tracking inter-robot correlation.

The existing decentralized cooperative localization methods for multirobot systems usually utilize traditional filtering algorithms to fuse measurements from different sensors and their prior information, overcoming the limitations of individual sensor technologies to obtain more accurate estimations. For example, commonly used filtering methods include the extended Kalman filter, unscented Kalman filter, and particle filter [18]. However, with the development of multirobot systems, the nonlinearities in navigation and localization systems have become increasingly serious. Filtering methods often require appropriate modeling of the problem and are susceptible to outliers, which leads to the fact that traditional filtering techniques are inadequate for the high-precision requirements of cooperative localization systems. Factor-graph-based multi-source fusion methods have opened up new avenues for cooperative localization in multirobot systems. Unlike traditional filter-based fusion methods, they optimize the complete set of states by considering both historical measurements and system updates simultaneously. These systems can seamlessly integrate new sensors and have the feature of plug-and-play, which effectively solve the nonlinearities in cooperative localization and asynchronous issues in data transmission. Xu et al. [19] presented a decentralized visual–inertial–UWB swarm state estimation system that flexibly integrates or discards sensor measurements and provides estimations through backend graph optimization. In factor-graph-based cooperative localization systems, if a sensor becomes unavailable due to signal loss or sensor failure, the system simply avoids adding relevant factors without requiring special procedures or coordination. In this case, historical information is used to encode all measurement values and states into a factor graph, and sensor fusion is solved through iterative optimization methods. In [20], a factor-graph-assisted distributed cooperative localization algorithm was developed to address ranging localization errors in cooperative nodes. This algorithm integrated a Fisher information matrix between adjacent agent nodes to evaluate ranging performance of the agent node to adjacent nodes. In this paper, we formulate multirobot cooperative localization as a graph optimization problem to efficiently achieve accurate state estimation through a factor-graph-based decentralized cooperative localization method.

Vision-based multirobot cooperative localization often requires accurate recognition of robots to track associated metrics for estimation of the association states [21]. However, real-world environments are far from perfect and often contain various sources of interference that make the task of correct data association challenging. Xunt et al. [22] proposed a system that focuses on relative pose estimation for multirobot systems but established data association through special patterns formed by infrared LED boards. Nguyen et al. [23] proposed a cooperative localization method using a coupled probabilistic data association filter to handle nonlinear measurements. However, this method is implemented based on a filtering structure. While considerable work has been done on multirobot cooperative localization, there is limited research on factor-graph-based methods using visual information for cooperative localization. To solve the problem of association uncertainty in cooperative localization, some researches have explored the use of data association algorithms in factor graphs. Gulati et al. [24] utilized data association filters to calculate the correspondence probabilities between multiple topologies of two robot states, which reduced interference in topological factors. Xu et al. [25] introduced a cooperative localization method based on omni-directional cameras, UWB sensors, and maps from multiple unmanned aerial vehicles that associates visual data through the global nearest neighbor algorithm. Although these methods can effectively track a single target, they face challenges in achieving satisfactory results when dealing with data association for multiple targets. Compared to [25], we use monocular camera and do not have the computational burden of dealing with a large number of distorted images. Meanwhile, in this paper, the Joint Probabilistic Data Association (JPDA) algorithm is used to solve the data association problem in multirobot cooperative localization. It can track multiple targets simultaneously with high tracking performance, and a target can be correlated with multiple measurements in the association gate. By fusing the measurements from the front-end with the back-end through graph optimization based on a factor graph, accurate localization is achieved while improving the resilience of the system in the face of abnormal situations.

We propose a factor-graph-based resilient cooperative localization algorithm (FG-RCL) for multirobot system. The main contributions of this paper are as follows:

• We propose a resilient and accurate cooperative localization system that fuses visual information and UWB measurements without relying on external onboard sensors or computing power.
• The system can handle data association problems in the visual measurement process of multirobot cooperative localization.
• The accuracy and robustness of the proposed algorithm are demonstrated through simulation results and experiments on a public dataset.

The remaining content of this paper is organized as follows. Section 2 first provides some basic concepts and system frameworks. Then, construction of the cost factors and the use of the JPDA algorithm to solve the data association problem of visual detection measurements are introduced. Section 3 presents simulation and experimental results. Finally, Section 4 and Section 5 discuss and summarize our work.

2. Materials and Methods

2.1. Notation

In this paper, we use $\mathbb{R}$, $\mathbb{N}$, and ${\mathbb{N}}^{+}$ to represent the sets of natural numbers, non-negative integers, and positive integers, respectively. ${\mathbb{R}}^m$ represents the m-dimensional real vector space of $m\in \mathbb{N}$. For a vector or matrix, we use ${(·)}^{\top }$ to represent its transpose. For vector $\mathbf{x}\in {\mathbb{R}}^m$, $∥\mathbf{x}∥$ represents its Euclidean norm. For matrix $\mathbf{A}$, ${\mathbf{brc}}_m\left(\mathbf{A}\right)$ represents the $m\times m$ matrix block in the bottom right corner of matrix $\mathbf{A}$, e.g, ${\mathbf{brc}}_1\left[\begin{array}{cc}a& b\\ c& d\end{array}\right]=\left[d\right]$. We use ${⌊·⌋}_{\times }$ to represent an antisymmetric matrix, e.g, for $\mathbf{w}={\left[w_x,w_y,w_z\right]}^{\top }\in {\mathbb{R}}^3$, there is

$${⌊\mathbf{w}⌋}_{\times }=\left[\begin{array}{ccc}0& -w_z& w_y\\ w_z& 0& -w_x\\ -w_y& w_x& 0\end{array}\right].$$

(1)

We add a left superscript ${ }^{\mathrm{A}}$ to $\mathbf{x}$ to indicate that the vector $\mathbf{x}$ is relative to the reference frame $\left\{\mathrm{A}\right\}$, e.g, ${ }^{\mathrm{A}}\mathbf{x}$. $\mathbf{R}$ and $\mathbf{T}$, along with the the left superscript and subscript, represent the rotation matrix and transformation matrix, respectively, between two frames. For example, ${ }_{\mathrm{B}}^{\mathrm{A}}\mathbf{R}$ and ${ }_{\mathrm{B}}^{\mathrm{A}}\mathbf{T}$ are the rotation matrix and transformation matrix, respectively, from frame $\left\{\mathrm{B}\right\}$ to $\left\{\mathrm{A}\right\}$.

Quaternions can also be used to represent rotation. A quaternion $\mathbf{q}$ is a four-dimensional complex number composed of one real part and three imaginary parts:

$$\begin{array}{c}\mathbf{q}=q_w+\mathbf{i}{q}_x+\mathbf{j}{q}_y+\mathbf{k}{q}_z=\left(q_w,{\mathbf{q}}_v\right),\\ q_w,q_x,q_y,q_z\in \mathbb{R},{\mathbf{q}}_v=\left(q_x,q_y,q_z\right)\in {\mathbb{R}}^3.\end{array}$$

(2)

The multiplication of rotation matrices represents the rotation between reference frames, and the corresponding quaternion operations are as follows:

$$\begin{array}{c}{ }_{\mathrm{C}}^{\mathrm{A}}\mathbf{R}={ }_{\mathrm{B}}^{\mathrm{A}}\mathbf{R}{ }_{\mathrm{C}}^{\mathrm{B}}\mathbf{R}\iff { }_{\mathrm{B}}^{\mathrm{A}}\mathbf{q}\circ { }_{\mathrm{C}}^{\mathrm{B}}\mathbf{q}={⌊{ }_{\mathrm{B}}^{\mathrm{A}}\mathbf{q}⌋}_l{ }_{\mathrm{C}}^{\mathrm{B}}\mathbf{q}={⌊{ }_{\mathrm{C}}^{\mathrm{B}}\mathbf{q}⌋}_r{ }_{\mathrm{B}}^{\mathrm{A}}\mathbf{q},\end{array}$$

(3)

$$\begin{array}{c}{⌊\mathbf{q}⌋}_l=\left[\begin{array}{cc}{q}_w& -{\mathbf{q}}_v^{\top }\\ {\mathbf{q}}_{\mathbf{v}}& q_w\mathrm{I}+{⌊{\mathbf{q}}_v⌋}_{\times }\end{array}\right],\end{array}$$

(4)

$$\begin{array}{c}{⌊\mathbf{q}⌋}_r=\left[\begin{array}{cc}{q}_w& -{\mathbf{q}}_v^{\top }\\ {\mathbf{q}}_{\mathbf{v}}& q_w\mathrm{I}-{⌊{\mathbf{q}}_v⌋}_{\times }\end{array}\right].\end{array}$$

(5)

We denote $\left\{\mathrm{B}\right\}$ as the body reference frame of the robot, $\left\{\mathrm{W}\right\}$ as the world reference frame, and $\left\{\mathrm{C}\right\}$ as the camera reference frame. Without specifying it, the left subscript ${ }_{\mathrm{B}}\left(·\right)$ body reference frame is implicit: for example, ${ }^{\mathrm{C}}{\mathbf{q}}_{\mathbf{k}}$ represents the rotation quaternion from the camera frame reference to the body frame $\left\{\mathrm{B}\right\}$ at time k, i.e., ${ }_{\mathrm{B}}^{\mathrm{C}}{\mathbf{q}}_{\mathbf{k}}$.

2.2. System Overview

We assume a multirobot system containing N robots, where the i-th robot is denoted as Robot i, $i=1,2,3,\cdots N$. For simplicity, unless otherwise specified, Robot i will be discussed in the following content. If the self-motion estimation of Robot j works normally and can communicate reliably with Robot i, then Robot j is within the available set of Robot i.

For the team of robots described in this paper, the sensors of each robot include cameras and UWBs. The cooperative localization system is formulated as a nonlinear optimization problem. Figure 1 shows the proposed factor graph structure. Hollow circles serve as vertices, which represent the poses to be optimized. There are three types of edges connecting poses: namely, (1) the local state estimation factor, which refers to incremental estimation of the own motion state of the robot through VO, thereby forming pose constraints for the robot at two moments before and after; (2) the UWB factor, which represents the relative distance constraint between two robots at the same time; (3) the visual detection factor, which represents the relative visual measurement constraint of one robot being detected by another robot. For each edge, the measurement, information matrix, and the Jacobian matrix of the error function relative to the state variables should be implemented.

Usually, $\mathrm{SE}\left(3\right)$ is used to describe a set of rigid body motions in 3D space. The pose of a robot on $\mathrm{SE}\left(3\right)$ is represented as ${ }^{\mathrm{W}}\mathbf{T}=\left[\begin{array}{cc}{ }^{\mathrm{W}}\mathbf{R}& { }^{\mathrm{W}}\mathbf{t}\\ {\mathbf{0}}^T& 1\end{array}\right]=\left[\begin{array}{cc}\mathcal{R}\left({ }^{\mathrm{W}}\mathbf{q}\right)& { }^{\mathrm{W}}\mathbf{p}\\ \mathbf{0}& 1\end{array}\right]$, where ${ }^{\mathrm{W}}\mathbf{q}$ and ${ }^{\mathrm{W}}\mathbf{p}$ represent the orientation quaternion and the position of the body frame relative to the world frame, respectively. $\mathcal{R}\left({ }^{\mathrm{W}}\mathbf{q}\right)$ represents the transformation of quaternions into rotation matrices, which can be seen in [26]. Obviously, the main goal of our work is to estimate the transform ${ }^{\mathrm{W}}\mathbf{T}$.

The proposed framework is divided into a front-end and a back-end that run independently on each robot. At the front-end, the raw data are processed separately through local state estimation (LE), visual detection (VD), and UWB modules. On the back-end, state estimation is performed using a graph optimization method. We use sliding windows to construct cost functions based on the measurements of LE, VD, and UWB at each new time step. The state of Robot i to be estimated includes the robot’s orientation quaternion ${\mathbf{q}}_i^k$ and position ${\mathbf{p}}_i^k$ at time step k. We define the state estimate of Robot i at each time step k as ${\widehat{\mathbf{x}}}_i^k$, so the optimized full state vector of Robot i in the sliding window is defined as:

$$\begin{array}{c}{\widehat{\mathbf{X}}}_i^k=\left[{\widehat{\mathbf{x}}}_1^k,\cdots ,{\widehat{\mathbf{x}}}_1^{k+m},{\widehat{\mathbf{x}}}_2^k,\cdots ,{\widehat{\mathbf{x}}}_2^{k+m},\cdots ,{\widehat{\mathbf{x}}}_N^k,\cdots ,{\widehat{\mathbf{x}}}_N^{k+m},\right]\end{array}$$

(6)

$$\begin{array}{c}{\widehat{\mathbf{x}}}_i^k=\left[{\mathbf{q}}_i^k,{\mathbf{p}}_i^k\right].\end{array}$$

(7)

where N is the total number of robots, and m is the number of keyframes in the sliding window.

LE, VD, and UWB provide observations coupling two continuous states in a sliding window. Using all the observations obtained from LE, VD, and UWB, the cost function can be constructed at time step k as

$$\underset{{\widehat{\mathbf{X}}}_i^k}{min}\left\{\sum _{\left(i,k\right)\in L}{∥{\mathbf{r}}_L\left({\widehat{\mathbf{Z}}}_{L,i}^k,{\widehat{\mathbf{x}}}_i^k\right)∥}_{{\mathbf{P}}_L^{-1}}^2+\sum _{\left(i,j,k\right)\in V}{∥{\mathbf{r}}_V\left({\widehat{\mathbf{Z}}}_{VD,i\to j}^k,{\widehat{\mathbf{x}}}_i^k\right)∥}_{{\mathbf{P}}_V^{-1}}^2+\sum _{\left(i,j,k\right)\in U}{∥{\mathbf{r}}_U\left({\widehat{\mathbf{Z}}}_{UWB,i\to j}^k,{\widehat{\mathbf{x}}}_i^k\right)∥}_{{\mathbf{P}}_U^{-1}}^2\right\},$$

(8)

where L is the set of all local state estimation factors; V is the set of all visual detection factors; U is the set of all UWB factors; ${\mathbf{r}}_L\left(·\right)$ denotes the residuals of the local state estimation factors, which ensures local consistency of the state of Robot i; ${\mathbf{r}}_V\left(·\right)$ denotes the residuals of the visual detection factors; ${\mathbf{r}}_U\left(·\right)$ denotes the residuals of the UWB factors; and ${\mathbf{P}}_L$, ${\mathbf{P}}_V$, and ${\mathbf{P}}_U$ are the covariance matrices of the measurement errors.

2.3. Construction of the Cost Factors

Our proposed FG-RCL algorithm divides the measurements into two stages: (1) a local measurement stage, where local state estimation results are obtained through visual odometry (VO) or proprioceptive sensors; (2) a relative measurement stage, where the relative measurement results of available robots are obtained by cameras and UWBs and are then used with local state estimation results for fusion based on graph optimization.

2.3.1. Local State Estimation Factor

In local state estimation, the VO algorithm is used. The transformation matrix ${ }^{\mathrm{W}}{\widehat{\mathbf{T}}}_i^k=\left[\begin{array}{cc}\mathcal{R}\left({ }^{\mathrm{W}}{\mathbf{q}}_{\mathbf{i}}^{\mathbf{k}}\right)& { }^{\mathrm{W}}{\mathbf{p}}_{\mathbf{i}}^{\mathbf{k}}\\ \mathbf{0}& 1\end{array}\right]$ can be obtained based on the state vector ${ }^{\mathrm{W}}{\mathbf{x}}_i^k$ of Robot i in the world reference frame at time step k, so measurement of the transformation matrix is obtained as

$$\Delta { }^{\mathrm{W}}{\widehat{\mathbf{T}}}_i^k={\left({ }^{\mathrm{C}}{\mathbf{T}}_i^k\right)}^{-1}\Delta { }_{\mathrm{C}}^{\mathrm{W}}{\widehat{\mathbf{T}}}_i^k.$$

(9)

where ${ }^{\mathrm{C}}{\mathbf{T}}_i^k$ is the transformation matrix of Robot i from the world reference frame to the camera reference frame.

Then, we can use the Gaussian model to represent the local measurements as

$${\widehat{\mathbf{Z}}}_{L,i}^k=\Delta { }^{\mathrm{W}}{\widehat{\mathbf{T}}}_i^k+{\mathbf{n}}_L,$$

(10)

where ${\mathbf{n}}_L$ is the Gaussian white noise conforming to $N\sim (0,{\sigma }_L^2)$.

The measurement noise is modeled as an independent zero-mean Gaussian distribution with the following covariance:

$${\mathbf{P}}_{L,i}=Cov\left(\left[\begin{array}{c}{\mathbf{n}}_{L_{\Delta \mathbf{q}}}\\ {\mathbf{n}}_{L_{\Delta \mathbf{p}}}\end{array}\right]\right)=diag\left({\sigma }_{L_1}^2,{\sigma }_{L_2}^2,\dots ,{\sigma }_{L_6}^2\right).$$

(11)

In this paper, the residual of the local measurement denotes the residual of the VO result, as shown in Equation (12). The multiplication of ${\left({ }^W{\widehat{\mathbf{T}}}_i^k\right)}^{-1}$ and ${ }^W{\widehat{\mathbf{T}}}_i^{k-1}$ denotes the transformation matrix of Robot i from time step $k-1$ to k in the world reference frame. The result is then multiplied with the local measurement ${\widehat{\mathbf{Z}}}_{L,i}^k$ to denote the residual of the local measurement.

$${\mathbf{r}}_{L,i}={\widehat{\mathbf{Z}}}_{L,i}^k\left({\left({ }^{\mathrm{W}}{\widehat{\mathbf{T}}}_i^k\right)}^{-1}{ }^{\mathrm{W}}{\widehat{\mathbf{T}}}_i^{k-1}\right).$$

(12)

We divide the measurement residual ${\mathbf{r}}_{L,i}$ into two parts according to the relative rotation $\Delta \mathbf{q}$ and displacement $\Delta \mathbf{p}$ and find the Jacobi matrix for the rotational component ${\mathbf{r}}_{\Delta \mathbf{q},i}$ and the translational component ${\mathbf{r}}_{\Delta \mathbf{p},i}$, respectively.

The variable ${\mathbf{r}}_{\Delta \mathbf{q},i}$ takes the partial derivatives of ${ }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^{k-1}$, ${ }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^k$, ${ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^{k-1}$, and ${ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k$ to obtain

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^{k-1}}^{{\mathbf{r}}_{\Delta \mathbf{q},i}}=-{\mathbf{brc}}_3\left({⌊\Delta { }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^{k-1}⌋}_r{⌊{\left({ }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^k\right)}^{-1}\circ { }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^{k-1}⌋}_l\right),$$

(13)

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^k}^{{\mathbf{r}}_{\Delta \mathbf{q},i}}={\mathbf{brc}}_3\left({⌊{\left(\Delta { }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^{k-1}\right)}^{-1}\circ {\left({ }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^k\right)}^{-1}\circ { }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^{k-1}⌋}_l\right),$$

(14)

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^{k-1}}^{{\mathbf{r}}_{\Delta \mathbf{q},i}}=0,$$

(15)

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k}^{{\mathbf{r}}_{\Delta \mathbf{q},i}}=0.$$

(16)

The variable ${\mathbf{r}}_{\Delta \mathbf{p},i}$ takes the partial derivatives of ${ }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^{k-1}$, ${ }^{\mathrm{W}}{\widehat{\mathbf{q}}}_i^k$, ${ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^{k-1}$, and ${ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k$ to obtain

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\mathbf{q}}_i^{k-1}}^{{\mathbf{r}}_{\Delta \mathbf{p},i}}\simeq {⌊{\left({ }^{\mathrm{W}}{\widehat{\mathbf{R}}}_i^{k-1}\right)}^{\top }\left({ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^{k-1}\right)⌋}_{\times },$$

(17)

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\mathbf{q}}_i^k}^{{\mathbf{r}}_{\Delta \mathbf{p},i}}=0,$$

(18)

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^{k-1}}^{{\mathbf{r}}_{\Delta \mathbf{p},i}}\simeq -{\left({ }^{\mathrm{W}}{\widehat{\mathbf{R}}}_i^{k-1}\right)}^{\top },$$

(19)

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k}^{{\mathbf{r}}_{\Delta \mathbf{p},i}}\simeq {\left({ }^{\mathrm{W}}{\widehat{\mathbf{R}}}_i^{k-1}\right)}^{\top }.$$

(20)

After completing local state estimation in the first stage, it is also necessary to detect and track the relative measurements between robots in the second stage.

2.3.2. Visual Detection Factor

Relative measurements can be obtained by detecting and tracking targets using vision sensors. In this paper, a monocular camera is employed for target detection. When Robot j appears in the field of view of Robot i, Robot i utilizes an object detection algorithm (such as YOLO) to detect Robot j. It is assumed that the center coordinate of the detection bounding box in the pixel coordinate frame of the camera is ${\mathbf{p}}_b=\left[x_I,y_I\right]$. The position information of the detected Robot j is broadcast by the communication module and then received by Robot i. The visual detection measurement can be calculated as the direction relative to the detected robot.

$${\widehat{\mathbf{Z}}}_{VD,i\to j}^k={{(}_{ }^{\mathrm{C}}{\mathbf{R}}_i^k)}^{-1}{f}_{VD}\left({\mathbf{p}}_b\right)+{\mathbf{n}}_{VD},$$

(21)

where $f_{VD}\left(·\right)$ denotes the function that obtains the direction relative to the detected robot from the coordinates of the bounding box center, and its specific form can be referenced in [27]. ${ }^{\mathrm{C}}{\mathbf{R}}_i^k$ is the rotation matrix of Robot i from the world reference frame to the camera reference frame. The relative vector is frame invariant, and ${\mathbf{n}}_{VD}$ is the Gaussian white noise that conforms to $N\sim (0,{\sigma }_{VD}^2)$.

The measurement noise is modeled as an independent zero-mean Gaussian distribution with the following covariance:

$${\mathbf{P}}_{VD,i}=Cov\left({\mathbf{n}}_{VD}\right)=diag\left({\sigma }_{V{D}_1}^2,{\sigma }_{V{D}_2}^2,{\sigma }_{V{D}_3}^2\right).$$

(22)

The residual of the visual detection measurements is

$${\mathbf{r}}_{V,i\to j}=\frac{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k}{{∥{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k∥}_2}-{\widehat{\mathbf{Z}}}_{VD,i\to j}^k.$$

(23)

The Jacobian matrices of the measurement residual based on visual detection over ${ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k$ and ${ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k$ are

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k}^{{\mathbf{r}}_{VD,i}}=\frac{1}{{{∥{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k∥}_2}^3}\left(-{{∥{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k∥}_2}^2+\left({ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k\right){\left({ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k\right)}^T{\mathbf{I}}_{3\times 1}\right),$$

(24)

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k}^{{\mathbf{r}}_{VD,i}}=\frac{1}{{{∥{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k∥}_2}^3}\left({{∥{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k∥}_2}^2-\left({ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k\right){\left({ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k\right)}^T{\mathbf{I}}_{3\times 1}\right).$$

(25)

2.3.3. UWB Factor

From the UWB module, the relative distance between each pair of nodes can be obtained. The relative distance measurements between Robot i and Robot j can also be modeled using Gaussian noise as

$${\widehat{\mathbf{Z}}}_{UWB,i\to j}^k=d_{ij}^k+{\mathbf{n}}_{UWB},$$

(26)

where ${\mathbf{n}}_{UWB}$ is the Gaussian white noise conforming to $N\sim (0,{\sigma }_{UWB}^2)$.

The measurement noise is modeled as an independent zero-mean Gaussian distribution with the following covariance:

$${\mathbf{P}}_{\mathbf{UWB}}=Cov\left({\mathbf{n}}_{UWB}\right)={\sigma }_{UWB}^2.$$

(27)

The residual of the relative distance measurements between Robot i and Robot j is

$${\mathbf{r}}_{U,i\to j}={∥{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k∥}_2-{\widehat{\mathbf{Z}}}_{UWB,i\to j}^k.$$

(28)

The Jacobian matrices of the residual of relative distance measurements over ${ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k$ and ${ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k$ are

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k}^{{\mathbf{r}}_{UWB,i}}=\frac{-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k}{{∥{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k∥}_2},$$

(29)

$${\mathbf{J}}_{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k}^{{\mathbf{r}}_{UWB,i}}=\frac{{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k}{{∥{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_j^k-{ }^{\mathrm{W}}{\widehat{\mathbf{p}}}_i^k∥}_2}.$$

(30)

2.4. Data Association for Visual Detection Measurements

The JPDA is a well-known Bayesian target tracking filter. Instead of using a single measurement to update the state, it takes into account all measurements within the associated region of the predicted measurements of the target. JPDA considers each measurement within the cross-correlation region as potentially coming from clutter or multiple targets at the intersection and then assigns weights to the validated measurements [28]. Here, we only provide a brief overview of calculating the corresponding probabilities of multiple visual detection measurements at a time step.

Assume that the set of validated measurements is represented as

$$\mathbf{Z}\left(k\right)={\left\{z_j^k\right\}}_{j=1}^{m^k},$$

(31)

where $z_j^k$ is the j-th validated measurement, and $m^k$ is the number of measurements in the validation region at time step k.

The cumulative set of measurements before time step k is

$${\mathbf{Z}}^k={\left\{\mathbf{Z}\left(k\right)\right\}}_{i=1}^k.$$

(32)

All possible sources for each measurement can be indicated by creating a validation matrix. Define the validation matrix as

$$\Omega =\stackrel{t=0,1,\dots ,N_t}{\overbrace{\left.\left[\begin{array}{ccc}1& \dots & {\omega }_{1N_t}\\ ⋮& \ddots & ⋮\\ 1_{m^{k}0}& \dots & {\omega }_{m^k{N}_t}\end{array}\right]\right\}}}j=1,2,\dots ,m^k,$$

(33)

where $\omega$ is a binary element 0 or 1. If the j-th visual measurement is associated with the target Robot t, then the corresponding validation matrix element ${\omega }_{jt}$ is 1; otherwise, it is 0. Hence, there is

$${\omega }_{jt}=\left\{\begin{array}{c}\begin{array}{cc}1,& \mathrm{if}\mathrm{visual}\mathrm{detection}\end{array}\hfill \\ \begin{array}{cc}0,& \mathrm{otherwise}\end{array}\hfill \end{array}\right.j=1,2,\dots ,m^k.$$

(34)

The main purpose of the JPDA is to calculate the probability of associating each validated measurement with a target that could have generated it. There are the following assumptions: (1) each measurement can only come from one target source; (2) each target can only generate at most one true measurement.

According to the above rules, we can get the feasible matrix ${\widehat{\Omega }}_l\left(k\right)\left(l=1,2,\dots ,N_e\right)$, where $N_e$ is the number of feasible matrices. Calculating the probability of a joint event requires scanning each feasible matrix row by row. If ${\omega }_{jt}=1$, then the Gaussian distribution function of the validated measurements is

$$P\left\{\theta \left(k\right)|{\mathbf{Z}}^k\right\}=\frac{1}{c}\frac{\varphi !}{m\left(k\right)!}{\mu }_F\left(\varphi \right)V^{-\varphi }\prod _j{\left\{f\left[z_j^k\right]\right\}}^{{\tau }_j}\prod _t{\left(P_D\right)}^{{\delta }_t}{\left(1-P_D\right)}^{1-{\delta }_t},$$

(35)

where $\theta$ denotes the joint event based on the feasible matrix ${\widehat{\Omega }}_l\left(k\right)$, c is a normalization constant, $\varphi$ is the number of false measurements, ${\mu }_F$ is the probability mass function of the number of false measurements, $f\left[z_j^k\right]$ is the probability density distribution function of the true measurement, ${\tau }_j$ is the association indicator of the measurement, and ${\delta }_t$ is the detection indicator of the target. $P_D$ denotes the probability of the detection of the target and is set manually according to the actual situation.

The association probability between measurement j and target t is obtained from the joint association probability, i.e.:

$$\begin{array}{cc}\hfill {\beta }_{jt}& \stackrel{\Delta }{=}P\left\{{\theta }_{jt}|{\mathbf{Z}}^k\right\}\hfill \\ \hfill & =\sum _{\theta }P\left\{\theta |{\mathbf{Z}}^k\right\}{\widehat{\omega }}_{jt}\left(\theta \right)\hfill \\ \hfill & =\sum _{\theta :{\theta }_{jt}\in \theta }P\left\{\theta |{\mathbf{Z}}^k\right\}.\hfill \end{array}$$

(36)

The final state estimate ${\widehat{\mathbf{x}}}_t^{k|k}$ at time step k in terms of ${\beta }_{jt}$ can be written as

$${\widehat{\mathbf{x}}}_t^{k|k}=\sum _{j=0}^{m^k}{\widehat{\mathbf{x}}}_{jt}^{k|k}{\beta }_{jt},$$

(37)

where ${\widehat{\mathbf{x}}}_{jt}^{k|k}$ is the updated state of target t based on the events associated with the j-th validated measurement.

Therefore, using the JPDA filter, we can calculate the sum of the probabilities of all feasible events at time step k to obtain the association probability ${\beta }_{jt}$ of the validated measurement $z_j^k$ relative to the target t and then assign weights to the measurements based on the association probabilities.

To solve the problem of data association in visual detection measurement, we implement JPDA next to the factor graph. Thus, the probabilities obtained from the JPDA filter are used as weights for the factors.

We assume that ${\beta }_{sj}$ is the probability of the s-th validated measurement ${\widehat{\mathbf{z}}}_{V{D}_s,i\to j}^k$ of Robot i with respect to Robot j; normalizing it yields

$${\beta }_{sj}^{*}=\frac{{\beta }_{sj}}{{\displaystyle \sum _{s=0}^{m^k}}{\beta }_{sj}}.$$

(38)

For $m^k$ measurements of Robot i relative to Robot j, each measurement has a certain probability. We can rewrite the visual detection measurement as

$${\tilde{\mathbf{Z}}}_{VD,i\to j}^k=\sum _{s=1}^{m^k}{\beta }_{sj}^{*}{\widehat{\mathbf{z}}}_{V{D}_s,i\to j}^k.$$

(39)

Therefore, the corresponding visual detection factor is

$$\sum _{\left(i,j,k\right)\in V}{∥{\mathbf{r}}_V\left({\tilde{\mathbf{Z}}}_{VD,i\to j}^k,{\widehat{\mathbf{x}}}_i^k\right)∥}_{{\Sigma }_V^{-1}}^2.$$

(40)

Data association based on visual detection can be understood from Figure 2. When Robot i detects both Robot j and Robot l, there may be multiple object detection bounding boxes or clutter measurements relative to a single target. Therefore, Robot i has multiple relative measurements relative to Robot j or Robot l. In this case, it is necessary to perform data association on these measurements. We use JPDA to enable Robot i to track measurements relative to multiple target robots. Firstly, a validation region is given for these measurements to exclude some invalid measurements. Robot i has four valid measurements relative to Robot j and three valid measurements relative to Robot l. The variables ${\widehat{\mathrm{Z}}}_{V{D}_{i\to l}}^k$ and ${\widehat{\mathrm{Z}}}_{V{D}_{i\to j}}^k$ are the predictions of visual detection measurements of Robot i relative to Robot l and Robot j, respectively. The variables ${\mathrm{Z}}_{V{D}_{i\to l}}^k$ and ${\mathrm{Z}}_{V{D}_{i\to j}}^k$ are the validation measurements in the validation region of Robot i relative to Robot l and Robot j, respectively. In particular, ${\mathrm{Z}}_{VD,3}^k$ is in the intersection portion of the validation region, indicating that it may come from Robot i relative to Robot l or Robot j. Then, JPDA is used to calculate the probability for each validated measurement. In Figure 2, the size of the rectangle in the validation region represents the probability of measurement. The probabilities of these calculations are used in the above equation. Therefore, only validated relative measurements can contribute to a final solution based on weights.

3. Results

3.1. Simulation Results

The simulations are implemented using three robots traveling 200 steps on the same ground plane. Each robot carries a camera and UWB. Assume that the mean Gaussian noise of all sensors is zero, and the covariances of VO and UWB are diag [0.01, 0.01] and diag [0.1, 0.1], and their measurement frequencies are 20 Hz and 80 Hz, respectively. We randomly generate 1–4 visual detection measurements for each robot with a covariance of diag [0.1, 0.1] and measurement frequency of 20 Hz consistent with VO. The initial estimates of all parameters are zero when solving the optimization problem using the Gauss–Newton method.

We can measure the performance of the cooperative localization algorithm by calculating the root mean square deviation (RMSE). There are four cases as follows:

(1)

Ground truth trajectory;

(2)

No visual detection measurement interference: the estimated trajectory by fusing VO, visual detection, and UWB measurements;

(3)

Visual detection measurement interference without data association: the estimated trajectory by fusing VO and UWB measurements and multiple visual detection factors for each state (assuming there are visual detection measurement interferences) without using the data association algorithm for visual detection measurement interference;

(4)

The proposed FG-RCL algorithm: the estimated trajectory by fusing VO and UWB measurements and multiple visual detection factors for each state (assuming there is visual detection measurement interference) with probability assignment using the JPDA.

The ground truths of the three robots and their estimated trajectories in various situations are shown in Figure 3. It can be seen that the trajectories of the proposed algorithm (Case 4) are closest to the ground truth. However, this case performs slightly worse compared to Case 2, which uses only one visual detection measurement with a probability of one. But Case 2 also assumes that there is no visual detection measurement interference, which is not true in real environments. Case 3 does not use data association to cope with the interference in the visual detection measurements, and it can be seen from Figure 3 that the estimated trajectories are the worst: even resulting in extreme deviations from normal values.

As shown in Figure 4, the localization performance of the system is further verified by the RMSEs of three robots in each case. In Case 3, the RMSE is the highest, and some outliers even deviate significantly from the normal range of values, with the location error of Robot 3 being far more than 18 m at 63–67 epochs. This is because visual detection measurement interference can add false information to the fusion process of the factor graph, which significantly increases the localization error. Although Case 2 has the smallest RMSE and the best execution effect, it cannot reflect the real environment because it does not add any false information to the factor graph. Therefore, it can be seen from the RMSE of Case 4 in Figure 4 that using JPDA to handle visual detection measurement interference is an effective solution for real environments.

Figure 5 compares the total number of visual detection measurements for all epochs of three robots with the number of visual detection measurements selected by JPDA. The visual detection measurements processed by data association provide more effective measurement information and avoid the interference of visual detection measurement that provides false information to the factor graph, thus allowing accurate estimation of the final state.

3.2. Experimental Results

In this section, we use the AirMuseum dataset to evaluate the proposed FG-RDL algorithm [29]. We first describe the dataset and then compare the localization performance of the algorithms in indoor scenes.

The AirMuseum dataset was obtained from the former Air Museum at ONERA Meudon, France. An overview of the experimental site for a 40 m × 80 m warehouse is shown in Figure 6a. Three ground robots (referred to as Robot A, Robot B, and Robot C) are used in the dataset, as shown in Figure 6b. Each robot is equipped with a stereo camera and an inertial measurement unit (IMU). The stereo camera is equipped with a 4 mm focal length lens and a 26 cm baseline and uses a resolution of 640 × 512 pixels with a frame rate of 20 images per second. The robots are connected together via WiFi in the 5 GHz band. Clock synchronization is performed using the Public Network Time Protocol (NTP) reference. Data are collected directly as .bag files using the ROS. The dataset uses Structure-from-Motion algorithms to establish an accurate reconstruction of the environment and to extract a reliable ground truth from it.

Robot B and Robot C are both equipped with a backward-facing AprilTag marker for direct observation between robots. Each AprilTag marker can be detected in an undistorted image using detection algorithms. If the tag is detected by a robot, it obtains inter-robot measurements, which can be directly used as an additional constraint for the relative poses between robots.

We use Scenario 2 of this dataset to validate the algorithm proposed in this paper. Images from the camera cam101 at a rate of 20 Hz are utilized for visual odometry state estimation of individual robots using ORB-SLAM2 [30]. Since the dataset does not contain UWB measurements, we generate them using the ground truth and taking into account actual features. Specifically, we simulate a UWB network wherein each robot carries a UWB node. Subsequently, visual measurements from the detection module and distance measurements from the UWB module are used together for the estimation of the relative measurement stage. It is assumed that information transfer between the robots is performed through communication modules broadcasting data with synchronized timestamps at a frequency of 100 Hz.

To verify the effectiveness of the proposed sensor fusion method, we constructed different localization scenarios that discarded UWB measurement and visual detection measurement data. Figure 7 shows the ground truths and estimated trajectories of the three robots. It can be seen that the estimated trajectory of the FG-RCL algorithm, which integrates visual detection measurement and UWB measurement, is closest to the ground truth. The estimated trajectories relying only on visual detection measurement are the worst. This is due to the presence of many obstacles in the scene used for the dataset, which resulted in a small number of visual detection measurements being acquired between the robots.

Figure 8 and Figure 9 show a comparison of the position error and orientation error, respectively, between the proposed algorithm and the ground truth.

Table 1. Comparison of average RMSEs using the proposed algorithm.

	Metrics	Robot A	Robot B	Robot C
Trajectory	Length (m)	116	141	162
Without UWB	ARMSE (m)	1.04	1.63	1.13
	ARMSE (deg)	8.13	5.11	7.79
Without Detection	ARMSE (m)	0.65	0.70	0.61
	ARMSE (deg)	2.77	2.89	4.01
Proposed Algorithm	ARMSE (m)	0.64	0.54	0.55
	ARMSE (deg)	2.80	2.43	3.53

shows the average RMSEs of the position and orientation. In the three-robot scenario, the average RMSE for the position when using the proposed algorithm can reach 0.6 m. With the dataset used in this section, the cooperative localization method [5], which relies only on UWB, has an average RMSE of 1.27 m in the horizontal position and an average RMSE of 3.22° in the orientation angle for the three robots. Compared with the UWB-only based method, the FG-RCL algorithm improves localization accuracy by 54.41%. On the other hand, the vision-only-based cooperative localization in [8] has an average RMSE of 0.65 m in the horizontal position and an average RMSE of 7.01° in the orientation angle for the three robots. Compared to the vision-only-based method, the FG-RCL algorithm improves the localization accuracy by 11.67%. FG-RCL not only has higher accuracy, but it also has the ability to handle situations wherein other robots are located outside the field of view. The proposed algorithm outperforms the other two methods in terms of both relative estimation reliability and localization accuracy.

In addition, Figure 10 presents the relative states of other robots estimated by Robot A to demonstrate the consistency of our proposed algorithm. The decentralized vision–UWB cooperative localization system based on a factor graph is not limited by FoV or GNSS, which means it may be widely adopted by multirobot applications in different environments.

4. Discussion

This paper proposes a resilient cooperative localization algorithm based on a factor graph. In this method, a factor graph model of the resilient cooperative localization system is established, and the mathematical model for constructing the cost function is derived in detail. The measurement, information matrix, error function, and Jacobian matrix of the error function relative to relevant state variables are provided. This method solves the problem of visual detection measurement data association in factor graphs by using the JPDA algorithm. This paper divides the front-end into a local measurement stage and a relative measurement stage, and it implements optimized fusion based on a factor graph in the back-end. Each robot operates independently to achieve decentralized cooperative localization. Therefore, this solution can realize plug-and-play of sensors, scalability, and resilience in complex environments.

The FG-RCL algorithm is applicable to multirobot cooperative localization in GNSS-denied environments. Specifically, when visual detection measurements have interference, erroneous visual information can affect the optimization of the global function during the factor graph fusion process, leading to an unsatisfactory final estimation state. Although this paper uses the JPDA algorithm to solve the problem of multi-target data association during visual detection measurement, it is still based on the correct recognition of targets. When the cooperative localization system fails to acquire visual information for a long time or is subjected to strong environmental interference, it can cause deviation in the localization results. For example, when the system is in a low-light environment, insufficient lighting seriously weakens the recognition efficiency of the visual object detection module; meanwhile, small targets cannot be recognized effectively, making the system miss part of the relative visual measurement results.

In the future, the FG-RCL algorithm can be combined with sensor selection algorithms, such as consistency-checking-based sensor optimization methods [31], to address the problems of sensor deviation and noise model updates in the factor graph framework during the fusion process.

In addition, for cooperative localization systems, a robust communication network is essential. In a perfect communication scenario, wireless communication networks can be established through point-to-point methods to achieve localization at lower cost and complexity. However, if the system suffers from occasional message loss due to external events, this situation can lead to communication failure within the network. It is best to use corresponding resilient measures to make the cooperative localization system more robust in this situation.

5. Conclusions

In this paper, we propose an FG-RCL algorithm for the fusion of visual and UWB measurements. We derive the mathematical model for the construction of the cost function in detail and use the JPDA algorithm to solve the data association problem in visual detection measurements. The front-end of the system is divided into local measurement and relative measurement stages, and then optimized fusion based on a factor graph is implemented in the back-end. Each robot operates independently to achieve decentralized cooperative localization. This solution enables plug-and-play of sensors along with scalability and resilience to complex environments. The simulation results show that interference from visual detection measurements degrade the performance of cooperative localization algorithms. However, through the data association against this interference performed by the proposed algorithm, its RMSE is better than that of a system without weighted visual detection factors. We also verify the effectiveness of the proposed algorithm through real datasets, and the estimation results for position and orientation show that it can accurately achieve multirobot cooperative localization.