# Multirobot Collaborative Navigation Algorithms Based on Odometer/Vision Information Fusion

Authors: Guohua Liu; Juan Guan; Haiying Liu; Chenlin Wang
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

Collaborative navigation is the key technology for multimobile robot system. In order to improve the performance of collaborative navigation system, the collaborative navigation algorithms based on odometer/vision multisource information fusion are presented in this paper. Firstly, the multisource information fusion collaborative navigation system model is established, including mobile robot model, odometry measurement model, lidar relative measurement model, UWB relative measurement model, and the SLAM model based on lidar measurement. Secondly, the frameworks of centralized and decentralized collaborative navigation based on odometer/vision fusion are given, and the SLAM algorithms based on vision are presented. Then, the centralized and decentralized odometer/vision collaborative navigation algorithms are derived, including the time update, single node measurement update, relative measurement update between nodes, and covariance cross filtering algorithm. Finally, different simulation experiments are designed to verify the effectiveness of the algorithms. Two kinds of multirobot collaborative navigation experimental scenes, which are relative measurement aided odometer and odometer/SLAM fusion, are designed, respectively. The advantages and disadvantages of centralized versus decentralized collaborative navigation algorithms in different experimental scenes are analyzed.


---

## Body

## 1. Introduction

With the development of the age of intelligence, the application scenarios and task requirements of intelligent mobile robots are becoming more and more complex [1, 2]. A single robot has difficulty in meeting the needs of humans for highly automated robots. Therefore, the multirobot system, which fully embodies the group wisdom, has a broad prospect. The subject has attracted extensive attention of researchers [3–5]. The premise of robot collaborative navigation operation is that each robot needs to have a certain ability of navigation and localization. Therefore, it is very important for the research of robot collaborative navigation system [6].The navigation accuracy of a single robot depends on its own navigation system, independent of other motion bodies. This navigation method is relatively simple, but due to the computing power of its own processor, sensor quality, and sensor field of vision and other factors, it limits the working range of the navigation system to a certain extent in the process of navigation and positioning. It also affects the ability of the navigation system to suppress noise, reducing errors and adapting to the complex environment. The collaborative navigation system of robot can make up for the single robot navigation system. When there is relative measurement between each robot, the relative information can be fully used to correct the navigation results. It is also possible to establish links with each other to realize the sharing of navigation resources among robots and to obtain better navigation performance [7–9].In the research of mobile robot navigation and location technology, the development of Simultaneous Localization and Mapping (SLAM) technology provides a new thought for autonomous positioning of mobile robot under complex scenes [10]. At the same time, as a significant technical means to realize relative navigation, radio navigation and visual navigation are being paid much attention. It provides important technical support for further research on the collaborative navigation of multirobots [11–13]. However, the traditional combined navigation technology is far from meeting the growing demand for the high performance of collaborative navigation.At present, most of the data fusion methods of multisource heterogeneous sensors in integrated navigation system use centralized data fusion algorithm. This algorithm has some defects in collaborative navigation of mobile robots, high communication cost, and poor robustness. However, the current research on decentralized data fusion algorithm, which is suitable for collaborative navigation environment is not mature enough [14, 15].Therefore, based on the information fusion of multisource heterogeneous sensors and related SLAM data association algorithms, this paper puts forward a new concept, model, and solution of multisource heterogeneous sensor information fusion, which can significantly improve the performance of collaborative navigation, address some problems of cooperative navigation technology, explore the key technologies that need to be solved in the collaborative navigation process of multirobot platform, and provide theoretical basis and technical support for high performance of collaborative navigation application.The structure of this paper is outlined as follows: in Section2, in the multisource information fusion collaborative navigation system model, the principle of the module involved, and the source of error are analyzed. In Section 3, the framework of centralized and decentralized collaborative navigation framework based on odometer/vision red fusion is given, and the SLAM algorithms based on vision are presented. In Sections 4 and 5, the centralized and decentralized odometer/vision collaborative navigation algorithms are derived, respectively. In Section 6, different simulation experiments are designed to verify the effectiveness of the algorithms.


## 2. Multisource Information Fusion Collaborative Navigation System Model

### 2.1. Motion Robot Model

Suppose that the mobile robot moves according to the circular arc trajectory. Figure1 shows the motion of a single mobile robot (see Figure 1).Figure 1
Motion model of a single mobile robot.


#### 2.1.1. Odometry Measurement Model

According to the principle of odometer motion calculation [16], it is assumed that the starting position of the mobile robot is x,y,θ, after ΔT of movement to the end. The end position is x+Δx,y+Δy,θ+Δθ, and the trajectory can be regarded as a circular arc ΔS with radius R. In this case, the pose of the robot (in discrete time) is given by(1)ΔSl=πD⋅ΔNLP=R−B2Δθ,ΔSr=πD⋅ΔNRP=R+B2Δθ,where ΔNL,ΔNR are increment of pulse number of odometer left and right wheel encoder. ΔSr,ΔSl is the distance between the right wheel and the left wheel of a mobile robot. P are the resolution parameters of left and right wheel encoder. The pose increment of sampling period ΔT is obtained by solving the above equations. The discrete motion equation of the mobile robot is further obtained by(2)xk≈xk−1+ΔSk−1cosθk−1+Δθk−12,yk≈yk−1+ΔSk−1sinθk−1+Δθk−12,θk=θk−1+Δθk−1.


#### 2.1.2. Odometer Error Model

There are systematic errors and random errors in odometer based positioning [17]. In the simulation experiment, the influence of random error is mainly considered. The corresponding covariance matrix Σodom is given by(3)Σodom=krΔSr00klΔSl.


### 2.2. Lidar Environmental Detection Model

As the main module to realize the SLAM, environment detection module mainly obtains the surrounding environment information through the external sensing sensor.


#### 2.2.1. Lidar Scanning Model

The two-dimensional lidar outputs the point cloud data from the surrounding environment and contains the angular information. The scanning schematic of the two-dimensional lidar is shown in Figure2 (see Figure 2).Figure 2
The scanning schematic diagram of the two-dimensional lidar.As shown in Figure2, the lidar is scanned in the sensor coordinate system along the scanning direction, with a fixed scanning angle resolution. The measured point is detected in the ith direction. Denote θi as the angle between this direction and the positive direction of the Xs-axis, and θi can be obtained by the resolution of the scanning angle with the inherent. A frame of point cloud data is obtained after a scan, which can be recorded as a set A=di,θii=1,…,n, where n is the number of scanned data points in this frame of data, di,θi is the coordinate of the measured point in the polar coordinate system. And the final point cloud data set B=xi,yii=1,…,n is obtained too.


#### 2.2.2. Lidar Error Model

In the simulation experiment, the above error can be simplified to Gaussian white noise along thex and y directions in the carrier coordinate system of the mobile robot [18]. It is assumed that the positioning errors are in the two independent directions, so the corresponding covariance matrix is given by(4)Σrad=σxb00σyb,where σxb and σyb represent the variance along the x and y directions of the observation in the carrier coordinate system of the mobile robot, respectively.


### 2.3. UWB Relative Measurement Model

The relative measurement module is an important part of the cooperative navigation network in which nodes relate to the state information of the surrounding nodes. UWB (Ultra Wide Band) can measure relative distance and lidar can measure relative position which are selected as research objects in this paper. Because the scanning model of lidar has been described in the previous section, this section only establishes the corresponding ranging model and error model.


#### 2.3.1. UWB Ranging Model

At present, there are two main ranging methods, that is, two-way ranging (TWR) method and symmetric double-sided two-way ranging (SDSTWR) method. SDSTWR method is greatly improved compared with TWR method in terms of ranging error caused by clock synchronization. Therefore, SDSTWR method is often used for distance measurement [19] (see Figure 3).Figure 3
Principle of SDSTWR.The distance expression between transceiver A and transceiver B can be obtained from Figure 3:(5)d=cTA1TB2−TB1TA2TA1+TA2+TB1+TB2,where d is the distance between the two tested transceivers. TA1=Ta2−Ta1 is the time interval between the first signal and the received signal of transceiver A. We can get TA2,TB1,TB2 in the same way.


#### 2.3.2. UWB Error Model

One of the main sources of error in UWB ranging is the error caused by the clock synchronization of the two transceivers [20]:(6)eSDS=Tfc1−kA+kB2,where kA and kB are the ratio of transceivers A and B between the actual and the expected frequency.


### 2.4. SLAM Model Based on Lidar Measurement

SLAM is widely used in the field of mobile robot navigation and positioning [21]. It is the key technology to solve the mapping problem at the same time, and it also provides a new idea for solving the path planning problem. SLAM technology mainly aims at the unknown location of the mobile robot. The environmental map is built in the form of an increasing perception of the surrounding strange environment according to the external sensor. At the same time, the state estimation of the mobile robot itself is obtained by using the built map information.The SLAM localization technology is the key technology to realize the autonomous positioning of the mobile robot under the unknown environment. It is of great research value to realized more accurate navigation and localization under the condition of reducing the limitation of moving robot (such as no GPS) [22]. By applying the technology to the framework of the collaborative navigation system of the mobile robot, the navigation performance of the whole collaborative navigation system can be effectively improved (see Figure 4).Figure 4
SLAM model diagram.It can be seen from Figure4 that a mobile robot’s localization using a SLAM navigation system is essentially a process, which is continuous estimation to approximate the true value [23]. The triangle in the graph represents the mobile robot and the circle represents the observed landmark, where the grey circle represents the estimated landmark. The solid line connecting the triangle represents the real trajectory, and the dotted line is the estimated trajectory (see Figure 4).


## 2.1. Motion Robot Model

Suppose that the mobile robot moves according to the circular arc trajectory. Figure1 shows the motion of a single mobile robot (see Figure 1).Figure 1
Motion model of a single mobile robot.


### 2.1.1. Odometry Measurement Model

According to the principle of odometer motion calculation [16], it is assumed that the starting position of the mobile robot is x,y,θ, after ΔT of movement to the end. The end position is x+Δx,y+Δy,θ+Δθ, and the trajectory can be regarded as a circular arc ΔS with radius R. In this case, the pose of the robot (in discrete time) is given by(1)ΔSl=πD⋅ΔNLP=R−B2Δθ,ΔSr=πD⋅ΔNRP=R+B2Δθ,where ΔNL,ΔNR are increment of pulse number of odometer left and right wheel encoder. ΔSr,ΔSl is the distance between the right wheel and the left wheel of a mobile robot. P are the resolution parameters of left and right wheel encoder. The pose increment of sampling period ΔT is obtained by solving the above equations. The discrete motion equation of the mobile robot is further obtained by(2)xk≈xk−1+ΔSk−1cosθk−1+Δθk−12,yk≈yk−1+ΔSk−1sinθk−1+Δθk−12,θk=θk−1+Δθk−1.


### 2.1.2. Odometer Error Model

There are systematic errors and random errors in odometer based positioning [17]. In the simulation experiment, the influence of random error is mainly considered. The corresponding covariance matrix Σodom is given by(3)Σodom=krΔSr00klΔSl.


## 2.1.1. Odometry Measurement Model

According to the principle of odometer motion calculation [16], it is assumed that the starting position of the mobile robot is x,y,θ, after ΔT of movement to the end. The end position is x+Δx,y+Δy,θ+Δθ, and the trajectory can be regarded as a circular arc ΔS with radius R. In this case, the pose of the robot (in discrete time) is given by(1)ΔSl=πD⋅ΔNLP=R−B2Δθ,ΔSr=πD⋅ΔNRP=R+B2Δθ,where ΔNL,ΔNR are increment of pulse number of odometer left and right wheel encoder. ΔSr,ΔSl is the distance between the right wheel and the left wheel of a mobile robot. P are the resolution parameters of left and right wheel encoder. The pose increment of sampling period ΔT is obtained by solving the above equations. The discrete motion equation of the mobile robot is further obtained by(2)xk≈xk−1+ΔSk−1cosθk−1+Δθk−12,yk≈yk−1+ΔSk−1sinθk−1+Δθk−12,θk=θk−1+Δθk−1.


## 2.1.2. Odometer Error Model

There are systematic errors and random errors in odometer based positioning [17]. In the simulation experiment, the influence of random error is mainly considered. The corresponding covariance matrix Σodom is given by(3)Σodom=krΔSr00klΔSl.


## 2.2. Lidar Environmental Detection Model

As the main module to realize the SLAM, environment detection module mainly obtains the surrounding environment information through the external sensing sensor.


### 2.2.1. Lidar Scanning Model

The two-dimensional lidar outputs the point cloud data from the surrounding environment and contains the angular information. The scanning schematic of the two-dimensional lidar is shown in Figure2 (see Figure 2).Figure 2
The scanning schematic diagram of the two-dimensional lidar.As shown in Figure2, the lidar is scanned in the sensor coordinate system along the scanning direction, with a fixed scanning angle resolution. The measured point is detected in the ith direction. Denote θi as the angle between this direction and the positive direction of the Xs-axis, and θi can be obtained by the resolution of the scanning angle with the inherent. A frame of point cloud data is obtained after a scan, which can be recorded as a set A=di,θii=1,…,n, where n is the number of scanned data points in this frame of data, di,θi is the coordinate of the measured point in the polar coordinate system. And the final point cloud data set B=xi,yii=1,…,n is obtained too.


### 2.2.2. Lidar Error Model

In the simulation experiment, the above error can be simplified to Gaussian white noise along thex and y directions in the carrier coordinate system of the mobile robot [18]. It is assumed that the positioning errors are in the two independent directions, so the corresponding covariance matrix is given by(4)Σrad=σxb00σyb,where σxb and σyb represent the variance along the x and y directions of the observation in the carrier coordinate system of the mobile robot, respectively.


## 2.2.1. Lidar Scanning Model

The two-dimensional lidar outputs the point cloud data from the surrounding environment and contains the angular information. The scanning schematic of the two-dimensional lidar is shown in Figure2 (see Figure 2).Figure 2
The scanning schematic diagram of the two-dimensional lidar.As shown in Figure2, the lidar is scanned in the sensor coordinate system along the scanning direction, with a fixed scanning angle resolution. The measured point is detected in the ith direction. Denote θi as the angle between this direction and the positive direction of the Xs-axis, and θi can be obtained by the resolution of the scanning angle with the inherent. A frame of point cloud data is obtained after a scan, which can be recorded as a set A=di,θii=1,…,n, where n is the number of scanned data points in this frame of data, di,θi is the coordinate of the measured point in the polar coordinate system. And the final point cloud data set B=xi,yii=1,…,n is obtained too.


## 2.2.2. Lidar Error Model

In the simulation experiment, the above error can be simplified to Gaussian white noise along thex and y directions in the carrier coordinate system of the mobile robot [18]. It is assumed that the positioning errors are in the two independent directions, so the corresponding covariance matrix is given by(4)Σrad=σxb00σyb,where σxb and σyb represent the variance along the x and y directions of the observation in the carrier coordinate system of the mobile robot, respectively.


## 2.3. UWB Relative Measurement Model

The relative measurement module is an important part of the cooperative navigation network in which nodes relate to the state information of the surrounding nodes. UWB (Ultra Wide Band) can measure relative distance and lidar can measure relative position which are selected as research objects in this paper. Because the scanning model of lidar has been described in the previous section, this section only establishes the corresponding ranging model and error model.


### 2.3.1. UWB Ranging Model

At present, there are two main ranging methods, that is, two-way ranging (TWR) method and symmetric double-sided two-way ranging (SDSTWR) method. SDSTWR method is greatly improved compared with TWR method in terms of ranging error caused by clock synchronization. Therefore, SDSTWR method is often used for distance measurement [19] (see Figure 3).Figure 3
Principle of SDSTWR.The distance expression between transceiver A and transceiver B can be obtained from Figure 3:(5)d=cTA1TB2−TB1TA2TA1+TA2+TB1+TB2,where d is the distance between the two tested transceivers. TA1=Ta2−Ta1 is the time interval between the first signal and the received signal of transceiver A. We can get TA2,TB1,TB2 in the same way.


### 2.3.2. UWB Error Model

One of the main sources of error in UWB ranging is the error caused by the clock synchronization of the two transceivers [20]:(6)eSDS=Tfc1−kA+kB2,where kA and kB are the ratio of transceivers A and B between the actual and the expected frequency.


## 2.3.1. UWB Ranging Model

At present, there are two main ranging methods, that is, two-way ranging (TWR) method and symmetric double-sided two-way ranging (SDSTWR) method. SDSTWR method is greatly improved compared with TWR method in terms of ranging error caused by clock synchronization. Therefore, SDSTWR method is often used for distance measurement [19] (see Figure 3).Figure 3
Principle of SDSTWR.The distance expression between transceiver A and transceiver B can be obtained from Figure 3:(5)d=cTA1TB2−TB1TA2TA1+TA2+TB1+TB2,where d is the distance between the two tested transceivers. TA1=Ta2−Ta1 is the time interval between the first signal and the received signal of transceiver A. We can get TA2,TB1,TB2 in the same way.


## 2.3.2. UWB Error Model

One of the main sources of error in UWB ranging is the error caused by the clock synchronization of the two transceivers [20]:(6)eSDS=Tfc1−kA+kB2,where kA and kB are the ratio of transceivers A and B between the actual and the expected frequency.


## 2.4. SLAM Model Based on Lidar Measurement

SLAM is widely used in the field of mobile robot navigation and positioning [21]. It is the key technology to solve the mapping problem at the same time, and it also provides a new idea for solving the path planning problem. SLAM technology mainly aims at the unknown location of the mobile robot. The environmental map is built in the form of an increasing perception of the surrounding strange environment according to the external sensor. At the same time, the state estimation of the mobile robot itself is obtained by using the built map information.The SLAM localization technology is the key technology to realize the autonomous positioning of the mobile robot under the unknown environment. It is of great research value to realized more accurate navigation and localization under the condition of reducing the limitation of moving robot (such as no GPS) [22]. By applying the technology to the framework of the collaborative navigation system of the mobile robot, the navigation performance of the whole collaborative navigation system can be effectively improved (see Figure 4).Figure 4
SLAM model diagram.It can be seen from Figure4 that a mobile robot’s localization using a SLAM navigation system is essentially a process, which is continuous estimation to approximate the true value [23]. The triangle in the graph represents the mobile robot and the circle represents the observed landmark, where the grey circle represents the estimated landmark. The solid line connecting the triangle represents the real trajectory, and the dotted line is the estimated trajectory (see Figure 4).


## 3. Collaborative Navigation Framework Based on Odometer/Vision

When nodes in collaborative navigation network participate in collaborative navigation, different data fusion algorithms can be used for data fusion, which is obtained multisource heterogeneous sensors of different nodes. There are two data fusion algorithms generally used, centralized and decentralized data fusion algorithms [24].


### 3.1. Framework of Collaborative Navigation Algorithm

In this subsection, a corresponding data fusion framework of the centralized collaborative navigation algorithm is designed for the general odometer/vision model, and a data fusion framework of decentralized collaborative navigation algorithm is designed in the same way.In the centralized collaborative navigation structure, the measured data obtained by each node are concentrated in a data fusion center for data fusion. In a distributed collaborative navigation structure, each node shares some information with other nodes while processing its own sensor data (see Figure5).Figure 5
Collaborative navigation structure. (a) Centralized collaborative navigation. (b) Decentralized collaborative navigation.
(a)(b)According to the odometer/vision collaborative navigation model, we use the most common EKF algorithms to simulate centralized and decentralized cooperative navigation. A centralized location algorithm (CL) and a decentralized location algorithm (DCL) are designed.As shown in Figure6, in the CL algorithm corresponding to the centralized cooperative navigation structure, each node sends the information obtained by the sensor itself to the central server and realizes the data fusion through the EKF algorithm. Its state vector is a set of state vectors of each node, which is updated with the principle of track reckoning. After that, the measurement information obtained after data association in the SLAM process and the relative measurement information between nodes are selected. CL algorithm gathers the state and covariance information of all nodes, corrects them uniformly, and sends the corrected estimation results to each node. Because of participating in the joint measurement update process, the state information of each node is related to each other after the first update (see Figure 6).Figure 6
Data fusion framework of centralized collaborative navigation algorithm.This correlation is reasonably estimated based on the CL algorithm, and the task of data fusion is distributed to each node; then the DCL algorithm is proposed accordingly. Since the position of each node in the algorithm is equivalent, only one node needs to be discussed (see Figure7).In order to avoid overoptimal estimation to the greatest extent, the DCL algorithm in this paper introduces the concept of separating covariance crossover filter. The covariance of each node is divided into correlation term and irrelevant term, and the time update process is basically consistent with the time update process of a single node. The measurement update process will take two steps. Firstly, according to the measurement information of the SLAM navigation system, the state estimation results are obtained by propagating the state and integrating the related information of the auxiliary navigation system. Then, the state information sent by the adjacent nodes and the relative measurement information between the nodes can obtain another state estimation of the node. Here, relevant state information sent to neighboring nodes is used to update local maps (see Figure 7).Figure 7
Data fusion framework of decentralized collaborative navigation algorithm.


### 3.2. SLAM Algorithms Based on Vision

#### 3.2.1. Landmark State Estimation Algorithm

The key of SLAM navigation algorithm lies in the process of data association. The positioning process of this SLAM navigation system is essentially a process, which is continuous estimation to approximate the true value. This kind of probability estimation problem is usually solved by introducing appropriate filter. The most common is the EKF algorithm (see Figure8).Figure 8
SLAM flow chart.Because of the high sampling frequency of the odometer selected in this paper, the lidar also has the advantages of high precision and high reliability; the EKF algorithm with better real-time performance is selected. The state estimation process of landmark information in SLAM based on EKF is described below. The observation equation of the feature information obtained by lidar is as follows:(7)zk=hXk,l,Xk,r+nk=xk,l−xk,rcosθk,r+yk,l−yk,rsinθk,r−xk,l−xk,rsinθk,r+yk,l−yk,rcosθk,r+nk,where Xk,l=xk,l,yk,lT is state vector of landmark at k time and Xk,r=xk,r,yk,r,θk,rT is state vector of mobile robot at k time. nk is measurement noise. Its variance matrix is Rk, which can be denoted as nk∼N0,Rk. Since the landmarks are static, the state estimation of k−1 time landmark can be regarded as a priori estimation of k time landmark state. The measurement update process based on EKF is as follows:Step 1: calculating the innovation and the filter gain:(8)vk=zk−hXk−1,l,Xk,r−,Kk=Σk−1HkTHkΣk−1HkT+Rk−1.Step 2: updating the state estimation and the corresponding covariance:(9)Xk,l=Xk−1,l+Kkvk,Σk=I−KkHkΣk−1,where Σk is the covariance matrix for state estimation of landmark at k time and Hk is the measurement matrix at k time.Remark 1.
Any observed landmark information can be position corrected by the above method, and it is noted that such position correction is limited to the landmark in the local map observed at thek time.


#### 3.2.2. Data Association Algorithm

In a SLAM navigation system, the process of data association is an important prerequisite for state estimation. Incorrect data association is likely to lead to serious deviation of estimation results [25, 26].At present, there are two data association algorithms commonly used in SLAM technology, that is, the nearest neighbor data association algorithm (NN, nearest neighbor) [27] and joint compatibility body & body data association algorithm (JCBB) [28]. The NN algorithm has less computation, but it is easy to form wrong data association when the density of feature information is large, which leads to the divergence of SLAM results, so it is only suitable for the environment where the density of feature information is small and the system error is small. JCBB is the improvement of the NN algorithm, which extends the association of single features in the NN to all observed feature information, which is more binding and more reliable. The JCBB algorithm can obtain more credible association assumptions than the NN algorithm and exclude some wrong association assumptions. However, the computation amount is obviously increased, which to some extent affects the real-time performance of SLAM navigation system.To ensure the accuracy of data association in the process of the SLAM, reduce the amount of computation as much as possible, and enhance the real-time performance of SLAM algorithm, this subsection describes an optimized data association algorithm. The classification method mentioned in [29] is used to divide the related feature information set; finally the appropriate feature information set in the local map and the preprocessed observation feature information set are selected to form the association space.First, the collection of feature information in the local map is divided as follows:(10)Dxm,ym,xk,yk≤Δd,xk,yk∈Fk,Dxm,ym,xk,yk>Δd,xk,yk∉Fk,where Dxm,ym,xk,yk is the relative distance between the feature information xk,yk of the local map and other feature information xm,ym.Then, the observation feature information set is preprocessed and divided. In the actual navigation process, the observation feature information obtained by lidar contains noise information. The purpose of preprocessing is to filter out some noise information, improve the accuracy of data association, and reduce the amount of computation at the same time. The judgment process is as follows:(11)fi,j=1,Dxi,yj,xi,yj<ΔD,0,Dxi,yj,xi,yj≥ΔD,where ΔD is the threshold, which is determined by the performance of the laser sensor. When the relative distance between the two observation feature information is less than the threshold, the observation feature information is considered to be the feature point; otherwise the noise point does not participate in the subsequent calculation.When the set is divided, the set of observed feature information is sorted according to the order of observation. Based on the process of the local map feature information set above, the subset is divided in turn, and all points are not involved in the division repeatedly.Finally, we select the appropriate association set to execute the data association algorithm. The subset of feature information of each local map and the subset of observed feature information at the current time are joint compatibility test, and the feature information with the best test results is selected to form a new subset as the data association object.


## 3.1. Framework of Collaborative Navigation Algorithm

In this subsection, a corresponding data fusion framework of the centralized collaborative navigation algorithm is designed for the general odometer/vision model, and a data fusion framework of decentralized collaborative navigation algorithm is designed in the same way.In the centralized collaborative navigation structure, the measured data obtained by each node are concentrated in a data fusion center for data fusion. In a distributed collaborative navigation structure, each node shares some information with other nodes while processing its own sensor data (see Figure5).Figure 5
Collaborative navigation structure. (a) Centralized collaborative navigation. (b) Decentralized collaborative navigation.
(a)(b)According to the odometer/vision collaborative navigation model, we use the most common EKF algorithms to simulate centralized and decentralized cooperative navigation. A centralized location algorithm (CL) and a decentralized location algorithm (DCL) are designed.As shown in Figure6, in the CL algorithm corresponding to the centralized cooperative navigation structure, each node sends the information obtained by the sensor itself to the central server and realizes the data fusion through the EKF algorithm. Its state vector is a set of state vectors of each node, which is updated with the principle of track reckoning. After that, the measurement information obtained after data association in the SLAM process and the relative measurement information between nodes are selected. CL algorithm gathers the state and covariance information of all nodes, corrects them uniformly, and sends the corrected estimation results to each node. Because of participating in the joint measurement update process, the state information of each node is related to each other after the first update (see Figure 6).Figure 6
Data fusion framework of centralized collaborative navigation algorithm.This correlation is reasonably estimated based on the CL algorithm, and the task of data fusion is distributed to each node; then the DCL algorithm is proposed accordingly. Since the position of each node in the algorithm is equivalent, only one node needs to be discussed (see Figure7).In order to avoid overoptimal estimation to the greatest extent, the DCL algorithm in this paper introduces the concept of separating covariance crossover filter. The covariance of each node is divided into correlation term and irrelevant term, and the time update process is basically consistent with the time update process of a single node. The measurement update process will take two steps. Firstly, according to the measurement information of the SLAM navigation system, the state estimation results are obtained by propagating the state and integrating the related information of the auxiliary navigation system. Then, the state information sent by the adjacent nodes and the relative measurement information between the nodes can obtain another state estimation of the node. Here, relevant state information sent to neighboring nodes is used to update local maps (see Figure 7).Figure 7
Data fusion framework of decentralized collaborative navigation algorithm.


## 3.2. SLAM Algorithms Based on Vision

### 3.2.1. Landmark State Estimation Algorithm

The key of SLAM navigation algorithm lies in the process of data association. The positioning process of this SLAM navigation system is essentially a process, which is continuous estimation to approximate the true value. This kind of probability estimation problem is usually solved by introducing appropriate filter. The most common is the EKF algorithm (see Figure8).Figure 8
SLAM flow chart.Because of the high sampling frequency of the odometer selected in this paper, the lidar also has the advantages of high precision and high reliability; the EKF algorithm with better real-time performance is selected. The state estimation process of landmark information in SLAM based on EKF is described below. The observation equation of the feature information obtained by lidar is as follows:(7)zk=hXk,l,Xk,r+nk=xk,l−xk,rcosθk,r+yk,l−yk,rsinθk,r−xk,l−xk,rsinθk,r+yk,l−yk,rcosθk,r+nk,where Xk,l=xk,l,yk,lT is state vector of landmark at k time and Xk,r=xk,r,yk,r,θk,rT is state vector of mobile robot at k time. nk is measurement noise. Its variance matrix is Rk, which can be denoted as nk∼N0,Rk. Since the landmarks are static, the state estimation of k−1 time landmark can be regarded as a priori estimation of k time landmark state. The measurement update process based on EKF is as follows:Step 1: calculating the innovation and the filter gain:(8)vk=zk−hXk−1,l,Xk,r−,Kk=Σk−1HkTHkΣk−1HkT+Rk−1.Step 2: updating the state estimation and the corresponding covariance:(9)Xk,l=Xk−1,l+Kkvk,Σk=I−KkHkΣk−1,where Σk is the covariance matrix for state estimation of landmark at k time and Hk is the measurement matrix at k time.Remark 1.
Any observed landmark information can be position corrected by the above method, and it is noted that such position correction is limited to the landmark in the local map observed at thek time.


### 3.2.2. Data Association Algorithm

In a SLAM navigation system, the process of data association is an important prerequisite for state estimation. Incorrect data association is likely to lead to serious deviation of estimation results [25, 26].At present, there are two data association algorithms commonly used in SLAM technology, that is, the nearest neighbor data association algorithm (NN, nearest neighbor) [27] and joint compatibility body & body data association algorithm (JCBB) [28]. The NN algorithm has less computation, but it is easy to form wrong data association when the density of feature information is large, which leads to the divergence of SLAM results, so it is only suitable for the environment where the density of feature information is small and the system error is small. JCBB is the improvement of the NN algorithm, which extends the association of single features in the NN to all observed feature information, which is more binding and more reliable. The JCBB algorithm can obtain more credible association assumptions than the NN algorithm and exclude some wrong association assumptions. However, the computation amount is obviously increased, which to some extent affects the real-time performance of SLAM navigation system.To ensure the accuracy of data association in the process of the SLAM, reduce the amount of computation as much as possible, and enhance the real-time performance of SLAM algorithm, this subsection describes an optimized data association algorithm. The classification method mentioned in [29] is used to divide the related feature information set; finally the appropriate feature information set in the local map and the preprocessed observation feature information set are selected to form the association space.First, the collection of feature information in the local map is divided as follows:(10)Dxm,ym,xk,yk≤Δd,xk,yk∈Fk,Dxm,ym,xk,yk>Δd,xk,yk∉Fk,where Dxm,ym,xk,yk is the relative distance between the feature information xk,yk of the local map and other feature information xm,ym.Then, the observation feature information set is preprocessed and divided. In the actual navigation process, the observation feature information obtained by lidar contains noise information. The purpose of preprocessing is to filter out some noise information, improve the accuracy of data association, and reduce the amount of computation at the same time. The judgment process is as follows:(11)fi,j=1,Dxi,yj,xi,yj<ΔD,0,Dxi,yj,xi,yj≥ΔD,where ΔD is the threshold, which is determined by the performance of the laser sensor. When the relative distance between the two observation feature information is less than the threshold, the observation feature information is considered to be the feature point; otherwise the noise point does not participate in the subsequent calculation.When the set is divided, the set of observed feature information is sorted according to the order of observation. Based on the process of the local map feature information set above, the subset is divided in turn, and all points are not involved in the division repeatedly.Finally, we select the appropriate association set to execute the data association algorithm. The subset of feature information of each local map and the subset of observed feature information at the current time are joint compatibility test, and the feature information with the best test results is selected to form a new subset as the data association object.


## 3.2.1. Landmark State Estimation Algorithm

The key of SLAM navigation algorithm lies in the process of data association. The positioning process of this SLAM navigation system is essentially a process, which is continuous estimation to approximate the true value. This kind of probability estimation problem is usually solved by introducing appropriate filter. The most common is the EKF algorithm (see Figure8).Figure 8
SLAM flow chart.Because of the high sampling frequency of the odometer selected in this paper, the lidar also has the advantages of high precision and high reliability; the EKF algorithm with better real-time performance is selected. The state estimation process of landmark information in SLAM based on EKF is described below. The observation equation of the feature information obtained by lidar is as follows:(7)zk=hXk,l,Xk,r+nk=xk,l−xk,rcosθk,r+yk,l−yk,rsinθk,r−xk,l−xk,rsinθk,r+yk,l−yk,rcosθk,r+nk,where Xk,l=xk,l,yk,lT is state vector of landmark at k time and Xk,r=xk,r,yk,r,θk,rT is state vector of mobile robot at k time. nk is measurement noise. Its variance matrix is Rk, which can be denoted as nk∼N0,Rk. Since the landmarks are static, the state estimation of k−1 time landmark can be regarded as a priori estimation of k time landmark state. The measurement update process based on EKF is as follows:Step 1: calculating the innovation and the filter gain:(8)vk=zk−hXk−1,l,Xk,r−,Kk=Σk−1HkTHkΣk−1HkT+Rk−1.Step 2: updating the state estimation and the corresponding covariance:(9)Xk,l=Xk−1,l+Kkvk,Σk=I−KkHkΣk−1,where Σk is the covariance matrix for state estimation of landmark at k time and Hk is the measurement matrix at k time.Remark 1.
Any observed landmark information can be position corrected by the above method, and it is noted that such position correction is limited to the landmark in the local map observed at thek time.


## 3.2.2. Data Association Algorithm

In a SLAM navigation system, the process of data association is an important prerequisite for state estimation. Incorrect data association is likely to lead to serious deviation of estimation results [25, 26].At present, there are two data association algorithms commonly used in SLAM technology, that is, the nearest neighbor data association algorithm (NN, nearest neighbor) [27] and joint compatibility body & body data association algorithm (JCBB) [28]. The NN algorithm has less computation, but it is easy to form wrong data association when the density of feature information is large, which leads to the divergence of SLAM results, so it is only suitable for the environment where the density of feature information is small and the system error is small. JCBB is the improvement of the NN algorithm, which extends the association of single features in the NN to all observed feature information, which is more binding and more reliable. The JCBB algorithm can obtain more credible association assumptions than the NN algorithm and exclude some wrong association assumptions. However, the computation amount is obviously increased, which to some extent affects the real-time performance of SLAM navigation system.To ensure the accuracy of data association in the process of the SLAM, reduce the amount of computation as much as possible, and enhance the real-time performance of SLAM algorithm, this subsection describes an optimized data association algorithm. The classification method mentioned in [29] is used to divide the related feature information set; finally the appropriate feature information set in the local map and the preprocessed observation feature information set are selected to form the association space.First, the collection of feature information in the local map is divided as follows:(10)Dxm,ym,xk,yk≤Δd,xk,yk∈Fk,Dxm,ym,xk,yk>Δd,xk,yk∉Fk,where Dxm,ym,xk,yk is the relative distance between the feature information xk,yk of the local map and other feature information xm,ym.Then, the observation feature information set is preprocessed and divided. In the actual navigation process, the observation feature information obtained by lidar contains noise information. The purpose of preprocessing is to filter out some noise information, improve the accuracy of data association, and reduce the amount of computation at the same time. The judgment process is as follows:(11)fi,j=1,Dxi,yj,xi,yj<ΔD,0,Dxi,yj,xi,yj≥ΔD,where ΔD is the threshold, which is determined by the performance of the laser sensor. When the relative distance between the two observation feature information is less than the threshold, the observation feature information is considered to be the feature point; otherwise the noise point does not participate in the subsequent calculation.When the set is divided, the set of observed feature information is sorted according to the order of observation. Based on the process of the local map feature information set above, the subset is divided in turn, and all points are not involved in the division repeatedly.Finally, we select the appropriate association set to execute the data association algorithm. The subset of feature information of each local map and the subset of observed feature information at the current time are joint compatibility test, and the feature information with the best test results is selected to form a new subset as the data association object.


## 4. Centralized Collaborative Navigation Algorithm

### 4.1. Time Update

First of all, the state space model should be established. The state vector of a single mobile robot with three degrees of freedom contains position and heading angle information. Suppose the number of nodes isN , the state space of collaborative navigation system in centralized framework contains the state vectors of all mobile robots in the group, and the state vector of mobile robot i is Xki and the state of system is Xk. Then the state space equation of the system can be expressed as follows:(12)Xk=Xk1Xk2⋮XkN=f1Xk−11,uk−11f2Xk−12,uk−12⋮fNXk−1N,uk−1N+wk−11wk−12⋮wk−1N≜ΦXk−1,uk−1+wk−1,where the function fiX,u describes the kinematic characteristics of the mobile robot, uk−1i=ΔSr,k−1ΔSl,k−1T represents the input required by the mobile robot i to calculate the track at time k, wk−1i is the system noise, and wk−1i∼N0,Qk−1i.It is assumed that the motion of any node is not affected by any other node, and each node moves independently without being controlled by other nodes. Therefore, the state transfer matrix for centralized collaborative positioning is given by(13)Fk−1=JXk−110⋯00JXk−12⋯0⋮⋮⋱⋮00⋯JXk−1N,where Juk−1i and JXk−1i are the Jacobian matrices of function f for state vectors and control inputs, respectively. The system noise variance matrix of collaborative navigation system in centralized framework is as follows:(14)Qk−1=Qk−110⋯00Qk−12⋯0⋮⋮⋱⋮00⋯Qk−1N,where Qk−1i=Juk−1iΣuJuk−1iT and Σu is the covariance matrix for controlling input. Then, the time update process of collaborative navigation system in centralized framework can be deduced:(15)Xk−=ΦXk−1+,uk−1,Pk−=Fk−1Pk−1Fk−1T+Qk−1≜P11,k−P12,k−⋯P1N,k−P21,k−P22,k−⋯P2N,k−⋮⋮⋱⋮PN1,k−PN2,k−⋯PNN,k−,where(16)Pii,k−=JXk−1iPii,k−1JXk−1iT+Qk−1i,Pij,k−=JXk−1iPij,k−1JXk−1jT.


### 4.2. Single-Node Measurement Update

In this section, the measurement updating process involving only one node in the centralized framework is described. The aided navigation system selected is SLAM navigation system, which integrates the landmark information of the surrounding environment measured by lidar. In this paper, a measurement model based on this navigation system is built, and the process of measurement updating based on EKF is described.


#### 4.2.1. Measurement Model Based on SLAM

The measurement model based on SLAM is the measurement model after data association. In this paper, the position information of landmarks obtained by lidar is taken as the observation equation.(17)zki=xlbylb=hiXki,Xlw+nki=xlw−xkicosθki+ylw−ykisinθki−xlw−xkisinθki+ylw−ykicosθki+nki,where xlb,ylb is position information for landmarks obtained by lidar. xlw,ylw,θki is the coordinates of landmarks in the world coordinate system. xki,yki, is the state of the mobile robot at the time of k. nki is the measurement noise and its variance matrix is Rki, which can be denoted as nki∼N0,Rki. After linearization and state extension, the observation equations of the whole system can be obtained:(18)zki=HkiXk+hiXki,−−∇hiXki,−+nki,where(19)Hki=0⋯∇hi⋯02×3N,and ∇hii is Jacobian matrices of function hiXki.


#### 4.2.2. Measurement and Update Based on EKF

Combined with the basic principle of Kalman filter, the measurement and update process of the aided navigation system for a single node can be obtained as follows:Step 1: calculating the innovation and the filter gain:(20)v=zki−hiXki,−,Si=HkiPk−HkiT+Rki,Si=HkiPk−HkiT+RkiStep 2: updating the state estimation and the corresponding covariance:(21)Xk+=Xk−+Kiv,Pk=Pk−−Pk−HkiTSi−1HkiPk−.


### 4.3. Relative Measurement Update among Nodes

The standard observation model can be divided into two types: the measurement model based on the relative distance and the measurement model based on the relative position.


#### 4.3.1. Measurement Model Based on Relative Distance

The observation of mobile roboti to mobile robot j at k time can be denoted by zkij; then the observation equation is given by(22)zkij=hijXki,Xkj+nkij=xki−xkj2+yki−ykj2+nkij,where nkij is the measurement noise, its variance matrix is Rkij=σUWB, which can be denoted as nkij∼N0,Rkij, and σUWB is the variance for UWB ranging.After linearization and state extension, the observation equations of the whole system can be obtained:(23)zkij=HkijXk+hijXki,−,Xkj,−−∇hiijXki,−−∇hjijXkj,−+nkij,where(24)Hkij=0⋯∇hiij⋯∇hjij⋯02×3N,and ∇hiij and ∇hjij are Jacobian matrices of function hijXi,Xj, respectively.


#### 4.3.2. Measurement Model Based on Relative Position

Using lidar as the sensor to realize the relative observation among nodes can be divided into two kinds: direct method and indirect method. The direct method is to measure the relative position between the two nodes directly; the indirect method is to use lidar to observe the nearest landmark between the two nodes. The relative position between the two nodes is obtained by correlation calculation.The state of mobile roboti is denoted by xki,yki,θki, at time k and the state of mobile robot j is denoted by xki,yki,θki. The coordinates of landmark L1 adjacent to mobile robot i in the world coordinate system are xl1w,yl1w, the coordinates in the mobile robot i coordinate system are xl1i,yl1i, the coordinates of landmark L2 adjacent to mobile robot j in the world coordinate system are xl2w,yl2w, and the coordinates in the coordinate system of mobile robot j are xl2i,yiw. The specific solution process of the indirect method is as follows (see Figure 9):(25)xkj−xkiykj−yki=xl2jcosθkj−yl2jsinθkjxl2jsinθkj+yl2jcosθkj−xl1icosθki−yl1isinθki+xl1w−xl2wxl1isinθki+yl1icosθki+yl1w−yl2w,when mobile robot i observe mobile robot j at k time, the coordinates of mobile robot j in the mobile robot i coordinate system as the observation. The observation equations are as follows:(26)zkij=xkj,iykj,i=xkj−xkicosθki+ykj−ykisinθki−xkj−xkisinθki+ykj−ykicosθki+nkij,where nkij is the measurement noise, its variance matrix is Rkij, which can be denoted as nkij∼N0,Rkij, and xkj,i,ykj,i is the coordinate of mobile robot j in the coordinate system of mobile robot i at k time.Figure 9
Indirect observation schematic diagram.


#### 4.3.3. Measurement Update Based on EKF

Similarly, we can finally get the measurement update process for the relative observation between nodes.


## 4.1. Time Update

First of all, the state space model should be established. The state vector of a single mobile robot with three degrees of freedom contains position and heading angle information. Suppose the number of nodes isN , the state space of collaborative navigation system in centralized framework contains the state vectors of all mobile robots in the group, and the state vector of mobile robot i is Xki and the state of system is Xk. Then the state space equation of the system can be expressed as follows:(12)Xk=Xk1Xk2⋮XkN=f1Xk−11,uk−11f2Xk−12,uk−12⋮fNXk−1N,uk−1N+wk−11wk−12⋮wk−1N≜ΦXk−1,uk−1+wk−1,where the function fiX,u describes the kinematic characteristics of the mobile robot, uk−1i=ΔSr,k−1ΔSl,k−1T represents the input required by the mobile robot i to calculate the track at time k, wk−1i is the system noise, and wk−1i∼N0,Qk−1i.It is assumed that the motion of any node is not affected by any other node, and each node moves independently without being controlled by other nodes. Therefore, the state transfer matrix for centralized collaborative positioning is given by(13)Fk−1=JXk−110⋯00JXk−12⋯0⋮⋮⋱⋮00⋯JXk−1N,where Juk−1i and JXk−1i are the Jacobian matrices of function f for state vectors and control inputs, respectively. The system noise variance matrix of collaborative navigation system in centralized framework is as follows:(14)Qk−1=Qk−110⋯00Qk−12⋯0⋮⋮⋱⋮00⋯Qk−1N,where Qk−1i=Juk−1iΣuJuk−1iT and Σu is the covariance matrix for controlling input. Then, the time update process of collaborative navigation system in centralized framework can be deduced:(15)Xk−=ΦXk−1+,uk−1,Pk−=Fk−1Pk−1Fk−1T+Qk−1≜P11,k−P12,k−⋯P1N,k−P21,k−P22,k−⋯P2N,k−⋮⋮⋱⋮PN1,k−PN2,k−⋯PNN,k−,where(16)Pii,k−=JXk−1iPii,k−1JXk−1iT+Qk−1i,Pij,k−=JXk−1iPij,k−1JXk−1jT.


## 4.2. Single-Node Measurement Update

In this section, the measurement updating process involving only one node in the centralized framework is described. The aided navigation system selected is SLAM navigation system, which integrates the landmark information of the surrounding environment measured by lidar. In this paper, a measurement model based on this navigation system is built, and the process of measurement updating based on EKF is described.


### 4.2.1. Measurement Model Based on SLAM

The measurement model based on SLAM is the measurement model after data association. In this paper, the position information of landmarks obtained by lidar is taken as the observation equation.(17)zki=xlbylb=hiXki,Xlw+nki=xlw−xkicosθki+ylw−ykisinθki−xlw−xkisinθki+ylw−ykicosθki+nki,where xlb,ylb is position information for landmarks obtained by lidar. xlw,ylw,θki is the coordinates of landmarks in the world coordinate system. xki,yki, is the state of the mobile robot at the time of k. nki is the measurement noise and its variance matrix is Rki, which can be denoted as nki∼N0,Rki. After linearization and state extension, the observation equations of the whole system can be obtained:(18)zki=HkiXk+hiXki,−−∇hiXki,−+nki,where(19)Hki=0⋯∇hi⋯02×3N,and ∇hii is Jacobian matrices of function hiXki.


### 4.2.2. Measurement and Update Based on EKF

Combined with the basic principle of Kalman filter, the measurement and update process of the aided navigation system for a single node can be obtained as follows:Step 1: calculating the innovation and the filter gain:(20)v=zki−hiXki,−,Si=HkiPk−HkiT+Rki,Si=HkiPk−HkiT+RkiStep 2: updating the state estimation and the corresponding covariance:(21)Xk+=Xk−+Kiv,Pk=Pk−−Pk−HkiTSi−1HkiPk−.


## 4.2.1. Measurement Model Based on SLAM

The measurement model based on SLAM is the measurement model after data association. In this paper, the position information of landmarks obtained by lidar is taken as the observation equation.(17)zki=xlbylb=hiXki,Xlw+nki=xlw−xkicosθki+ylw−ykisinθki−xlw−xkisinθki+ylw−ykicosθki+nki,where xlb,ylb is position information for landmarks obtained by lidar. xlw,ylw,θki is the coordinates of landmarks in the world coordinate system. xki,yki, is the state of the mobile robot at the time of k. nki is the measurement noise and its variance matrix is Rki, which can be denoted as nki∼N0,Rki. After linearization and state extension, the observation equations of the whole system can be obtained:(18)zki=HkiXk+hiXki,−−∇hiXki,−+nki,where(19)Hki=0⋯∇hi⋯02×3N,and ∇hii is Jacobian matrices of function hiXki.


## 4.2.2. Measurement and Update Based on EKF

Combined with the basic principle of Kalman filter, the measurement and update process of the aided navigation system for a single node can be obtained as follows:Step 1: calculating the innovation and the filter gain:(20)v=zki−hiXki,−,Si=HkiPk−HkiT+Rki,Si=HkiPk−HkiT+RkiStep 2: updating the state estimation and the corresponding covariance:(21)Xk+=Xk−+Kiv,Pk=Pk−−Pk−HkiTSi−1HkiPk−.


## 4.3. Relative Measurement Update among Nodes

The standard observation model can be divided into two types: the measurement model based on the relative distance and the measurement model based on the relative position.


### 4.3.1. Measurement Model Based on Relative Distance

The observation of mobile roboti to mobile robot j at k time can be denoted by zkij; then the observation equation is given by(22)zkij=hijXki,Xkj+nkij=xki−xkj2+yki−ykj2+nkij,where nkij is the measurement noise, its variance matrix is Rkij=σUWB, which can be denoted as nkij∼N0,Rkij, and σUWB is the variance for UWB ranging.After linearization and state extension, the observation equations of the whole system can be obtained:(23)zkij=HkijXk+hijXki,−,Xkj,−−∇hiijXki,−−∇hjijXkj,−+nkij,where(24)Hkij=0⋯∇hiij⋯∇hjij⋯02×3N,and ∇hiij and ∇hjij are Jacobian matrices of function hijXi,Xj, respectively.


### 4.3.2. Measurement Model Based on Relative Position

Using lidar as the sensor to realize the relative observation among nodes can be divided into two kinds: direct method and indirect method. The direct method is to measure the relative position between the two nodes directly; the indirect method is to use lidar to observe the nearest landmark between the two nodes. The relative position between the two nodes is obtained by correlation calculation.The state of mobile roboti is denoted by xki,yki,θki, at time k and the state of mobile robot j is denoted by xki,yki,θki. The coordinates of landmark L1 adjacent to mobile robot i in the world coordinate system are xl1w,yl1w, the coordinates in the mobile robot i coordinate system are xl1i,yl1i, the coordinates of landmark L2 adjacent to mobile robot j in the world coordinate system are xl2w,yl2w, and the coordinates in the coordinate system of mobile robot j are xl2i,yiw. The specific solution process of the indirect method is as follows (see Figure 9):(25)xkj−xkiykj−yki=xl2jcosθkj−yl2jsinθkjxl2jsinθkj+yl2jcosθkj−xl1icosθki−yl1isinθki+xl1w−xl2wxl1isinθki+yl1icosθki+yl1w−yl2w,when mobile robot i observe mobile robot j at k time, the coordinates of mobile robot j in the mobile robot i coordinate system as the observation. The observation equations are as follows:(26)zkij=xkj,iykj,i=xkj−xkicosθki+ykj−ykisinθki−xkj−xkisinθki+ykj−ykicosθki+nkij,where nkij is the measurement noise, its variance matrix is Rkij, which can be denoted as nkij∼N0,Rkij, and xkj,i,ykj,i is the coordinate of mobile robot j in the coordinate system of mobile robot i at k time.Figure 9
Indirect observation schematic diagram.


### 4.3.3. Measurement Update Based on EKF

Similarly, we can finally get the measurement update process for the relative observation between nodes.


## 4.3.1. Measurement Model Based on Relative Distance

The observation of mobile roboti to mobile robot j at k time can be denoted by zkij; then the observation equation is given by(22)zkij=hijXki,Xkj+nkij=xki−xkj2+yki−ykj2+nkij,where nkij is the measurement noise, its variance matrix is Rkij=σUWB, which can be denoted as nkij∼N0,Rkij, and σUWB is the variance for UWB ranging.After linearization and state extension, the observation equations of the whole system can be obtained:(23)zkij=HkijXk+hijXki,−,Xkj,−−∇hiijXki,−−∇hjijXkj,−+nkij,where(24)Hkij=0⋯∇hiij⋯∇hjij⋯02×3N,and ∇hiij and ∇hjij are Jacobian matrices of function hijXi,Xj, respectively.


## 4.3.2. Measurement Model Based on Relative Position

Using lidar as the sensor to realize the relative observation among nodes can be divided into two kinds: direct method and indirect method. The direct method is to measure the relative position between the two nodes directly; the indirect method is to use lidar to observe the nearest landmark between the two nodes. The relative position between the two nodes is obtained by correlation calculation.The state of mobile roboti is denoted by xki,yki,θki, at time k and the state of mobile robot j is denoted by xki,yki,θki. The coordinates of landmark L1 adjacent to mobile robot i in the world coordinate system are xl1w,yl1w, the coordinates in the mobile robot i coordinate system are xl1i,yl1i, the coordinates of landmark L2 adjacent to mobile robot j in the world coordinate system are xl2w,yl2w, and the coordinates in the coordinate system of mobile robot j are xl2i,yiw. The specific solution process of the indirect method is as follows (see Figure 9):(25)xkj−xkiykj−yki=xl2jcosθkj−yl2jsinθkjxl2jsinθkj+yl2jcosθkj−xl1icosθki−yl1isinθki+xl1w−xl2wxl1isinθki+yl1icosθki+yl1w−yl2w,when mobile robot i observe mobile robot j at k time, the coordinates of mobile robot j in the mobile robot i coordinate system as the observation. The observation equations are as follows:(26)zkij=xkj,iykj,i=xkj−xkicosθki+ykj−ykisinθki−xkj−xkisinθki+ykj−ykicosθki+nkij,where nkij is the measurement noise, its variance matrix is Rkij, which can be denoted as nkij∼N0,Rkij, and xkj,i,ykj,i is the coordinate of mobile robot j in the coordinate system of mobile robot i at k time.Figure 9
Indirect observation schematic diagram.


## 4.3.3. Measurement Update Based on EKF

Similarly, we can finally get the measurement update process for the relative observation between nodes.


## 5. Decentralized Collaborative Navigation Algorithm

The state and covariance information of each node under the decentralized collaborative navigation algorithm is, respectively, calculated. In order to avoid overoptimal estimation to the maximum extent, the concept of the covariance intersection filter is introduced and the covariance of each node is divided into related and irrelevant items.


### 5.1. Covariance Intersection Filter

Given the state estimation vectorX and corresponding covariance matrix P, assuming that P∗ is the covariance of the error between the state estimate X and the state real value X∗, it can be expressed as follows:(27)P∗=EX−X∗X−X∗T.Consistency is a characteristic of the covariance matrix of the estimation [30]. When the covariance matrix of the state estimation is not less than the real covariance, it is said that the estimation satisfies the consistency, that is, no overoptimal estimation is produced. Suppose two state estimates X1 and X2 are independent and satisfy the consistency, the corresponding covariances are P1 and P2. If there is a correlation between the two estimates, the Kalman filter may produce inconsistent results; in other words, it leads to overoptimal estimation:(28)P1=P1dw+P1i,(29)P2=P2dw+P2i,(30)P−1=P1−1+P2−1,(31)X=PP1−1X1+P2−1X2,(32)Pi=PP1−1P1iP1−1+P2−1P2iP2−1,(33)Pd=P−Pi,where the covariance corresponding to two state estimates X1 and X2 is P1d+P1i and P2d+P2i, respectively. P1d and P2d are the correlation covariance components corresponding to the maximum correlation between the two state estimates. P1i and P2i are independent covariance components corresponding to absolute independence of two state estimates, W within interval 0,1. It is an optimization parameter that minimizes the covariance after fusion, and the w in this interval can ensure the consistency of the fusion results.


### 5.2. Time Update

Before describing the time update process of DCL algorithm, it is necessary to decompose the state information of the system in the framework of centralized collaborative navigation, which can be expressed as(34)EG=XG,PG=X1,P1;X2,P2⋯XN,PN=X1,P1d+P1i;X2,P2d+P2i⋯XN,PNd+PNi,where EG is the set of states under the centralized collaborative navigation framework, and XG and PG are the state space and the corresponding covariance matrix under the centralized collaborative navigation framework, respectively.The state propagation process under the decentralized collaborative navigation framework is the state propagation process of a single node, and the propagation process of covariance can be expressed as(35)Pki,−=JXk−1iPk−1iJXk−1iT+Juk−1iΣuJuk−1iT,Pk,ii,−=JXk−1iPk−1,iiJXk−1iT+Juk−1iΣuJuk−1iT,where Pki,− is the one-step prediction covariance matrix of the mobile robot i at time k and Pk,ii,− is the covariance. The independent covariance components of the matrix, JXk−1i and Juk−1i, are Jacobian matrices of function fiX,u for state vector and control input and Σu is the error matrix of the control input.


### 5.3. Single Node Measurement Update

The measurement and updating process of a single node only involves the aided navigation system of a single node, so there is no need to estimate the correlation, that is, the process of saving formulas (28) and (29). Similar to the single node measurement update process in centralized collaborative navigation, the single node measurement update process in distributed collaborative navigation can be expressed asStep 1: calculate the innovation and the filtering gain:(36)v=zki−∇hiXki,−,S=∇hiPki,−∇hiT+Rki,K=Pki,−∇hiTS−1.Step 2: update the state estimation and the corresponding covariance:(37)Xki,+=Xki,−+Kv,Pki=I−K∇hiPki,−,Pk,ii=I−K∇hiPk,ii,−I−K∇hiT+KRkiKT,Pk,di=Pki−Pk,ii.


### 5.4. Collaborative Measurement Update among Nodes

In the framework of decentralized collaborative navigation, the state estimation results of a single node aided navigation system and the state estimation results based on information sharing among nodes are integrated in the process of internode collaborative measurement updating, and the corrected state information is derived.For the decentralized collaborative navigation framework, any node can clearly estimate the state of other nodes. In order to save the communication cost and reduce the computation of a single mobile robot platform, this paper sets up that the information exchange among the mobile robots is only taking place between the two adjacent mobile robot nodes.Assuming that the mobile roboti performs relative observations of the mobile robot j at the k time and shares its own state and covariance information with the mobile robot j, the state of the mobile robot j can be clearly expressed with information received state of the mobile robot i and the relative measurement information between the two nodes:(38)Xkcoj=xkcojykcoj=xki+xkj,icosθki−ykj,isinθkiyki+xkj,isinθki+ykj,icosθki,where xkcoj,ykcoj is the partial state estimation of the mobile robot j obtained by the information sharing between the mobile robot i and the mobile robot j at k time, xki,xki,θki is the state vector shared by the mobile robot j in the k direction of the mobile robot i, and urel=xkj,i,ykj,i is the relative measurement information of the two nodes in the i coordinate system of the mobile robot.If there is a direct relative observation between the two nodes, the relative measurement information can be obtained directly by the sensor that carries on the relative observation. If the relative observation between the two nodes is with the help of the indirect relative observation of the surrounding landmark information, then the relative measurement information needs to be solved to a certain extent, and the concrete solution method can be combined (25) and then converted into the mobile robot.Finally, based on the principle of covariance intersection filter, the updating process of collaborative measurement among nodes in the framework of decentralized collaborative navigation can be obtained.


## 5.1. Covariance Intersection Filter

Given the state estimation vectorX and corresponding covariance matrix P, assuming that P∗ is the covariance of the error between the state estimate X and the state real value X∗, it can be expressed as follows:(27)P∗=EX−X∗X−X∗T.Consistency is a characteristic of the covariance matrix of the estimation [30]. When the covariance matrix of the state estimation is not less than the real covariance, it is said that the estimation satisfies the consistency, that is, no overoptimal estimation is produced. Suppose two state estimates X1 and X2 are independent and satisfy the consistency, the corresponding covariances are P1 and P2. If there is a correlation between the two estimates, the Kalman filter may produce inconsistent results; in other words, it leads to overoptimal estimation:(28)P1=P1dw+P1i,(29)P2=P2dw+P2i,(30)P−1=P1−1+P2−1,(31)X=PP1−1X1+P2−1X2,(32)Pi=PP1−1P1iP1−1+P2−1P2iP2−1,(33)Pd=P−Pi,where the covariance corresponding to two state estimates X1 and X2 is P1d+P1i and P2d+P2i, respectively. P1d and P2d are the correlation covariance components corresponding to the maximum correlation between the two state estimates. P1i and P2i are independent covariance components corresponding to absolute independence of two state estimates, W within interval 0,1. It is an optimization parameter that minimizes the covariance after fusion, and the w in this interval can ensure the consistency of the fusion results.


## 5.2. Time Update

Before describing the time update process of DCL algorithm, it is necessary to decompose the state information of the system in the framework of centralized collaborative navigation, which can be expressed as(34)EG=XG,PG=X1,P1;X2,P2⋯XN,PN=X1,P1d+P1i;X2,P2d+P2i⋯XN,PNd+PNi,where EG is the set of states under the centralized collaborative navigation framework, and XG and PG are the state space and the corresponding covariance matrix under the centralized collaborative navigation framework, respectively.The state propagation process under the decentralized collaborative navigation framework is the state propagation process of a single node, and the propagation process of covariance can be expressed as(35)Pki,−=JXk−1iPk−1iJXk−1iT+Juk−1iΣuJuk−1iT,Pk,ii,−=JXk−1iPk−1,iiJXk−1iT+Juk−1iΣuJuk−1iT,where Pki,− is the one-step prediction covariance matrix of the mobile robot i at time k and Pk,ii,− is the covariance. The independent covariance components of the matrix, JXk−1i and Juk−1i, are Jacobian matrices of function fiX,u for state vector and control input and Σu is the error matrix of the control input.


## 5.3. Single Node Measurement Update

The measurement and updating process of a single node only involves the aided navigation system of a single node, so there is no need to estimate the correlation, that is, the process of saving formulas (28) and (29). Similar to the single node measurement update process in centralized collaborative navigation, the single node measurement update process in distributed collaborative navigation can be expressed asStep 1: calculate the innovation and the filtering gain:(36)v=zki−∇hiXki,−,S=∇hiPki,−∇hiT+Rki,K=Pki,−∇hiTS−1.Step 2: update the state estimation and the corresponding covariance:(37)Xki,+=Xki,−+Kv,Pki=I−K∇hiPki,−,Pk,ii=I−K∇hiPk,ii,−I−K∇hiT+KRkiKT,Pk,di=Pki−Pk,ii.


## 5.4. Collaborative Measurement Update among Nodes

In the framework of decentralized collaborative navigation, the state estimation results of a single node aided navigation system and the state estimation results based on information sharing among nodes are integrated in the process of internode collaborative measurement updating, and the corrected state information is derived.For the decentralized collaborative navigation framework, any node can clearly estimate the state of other nodes. In order to save the communication cost and reduce the computation of a single mobile robot platform, this paper sets up that the information exchange among the mobile robots is only taking place between the two adjacent mobile robot nodes.Assuming that the mobile roboti performs relative observations of the mobile robot j at the k time and shares its own state and covariance information with the mobile robot j, the state of the mobile robot j can be clearly expressed with information received state of the mobile robot i and the relative measurement information between the two nodes:(38)Xkcoj=xkcojykcoj=xki+xkj,icosθki−ykj,isinθkiyki+xkj,isinθki+ykj,icosθki,where xkcoj,ykcoj is the partial state estimation of the mobile robot j obtained by the information sharing between the mobile robot i and the mobile robot j at k time, xki,xki,θki is the state vector shared by the mobile robot j in the k direction of the mobile robot i, and urel=xkj,i,ykj,i is the relative measurement information of the two nodes in the i coordinate system of the mobile robot.If there is a direct relative observation between the two nodes, the relative measurement information can be obtained directly by the sensor that carries on the relative observation. If the relative observation between the two nodes is with the help of the indirect relative observation of the surrounding landmark information, then the relative measurement information needs to be solved to a certain extent, and the concrete solution method can be combined (25) and then converted into the mobile robot.Finally, based on the principle of covariance intersection filter, the updating process of collaborative measurement among nodes in the framework of decentralized collaborative navigation can be obtained.


## 6. Simulation Results

### 6.1. Simulated Experimental Environment

In this section, the nodes of the mobile robot network involved in collaborative navigation are 3. Assuming that the area of the moving2D environment is 25m×25m when the mobile robot population group works together, each node in the environment is assigned to an initial position, and each node can follow random trajectory motion in this area. Assuming that all nodes follow the same simulation trajectory, only the initial position is different. The maximum speed of the mobile robot in the straight line is 0.3125m/s and the angular velocity at the bend is 0.1°/s. It is assumed that the environment around the simulated trajectory of this rectangle can be extracted by lidar scanning to 88 landmarks (environmental feature points) for the SLAM-assisted navigation system (see Figure 10).Figure 10
Simulation trajectory diagram.During this simulation, mobile robots as carriers can carry different types of sensors, including odometers, UWB, and lidar. Suitable sensors are selected according to the requirements of positioning accuracy, among which Time Domain P410 UWB sensors are used to measure the relative distance, and lidar is selected from LMS291 series 2D lidar produced by a German company. Based on the relevant parameters of these sensors, which are shown in Table1, a simulation model for mobile robots carrying different types of sensors is built using MATLAB.Table 1
Relevant parameters of sensors.
TypeMeasureFrequency (Hz)ErrorOdometerLinear velocity of the two wheels204 cm/sUWBRelative distance103 cmLidarRelative position ofX direction102 cmRelative position ofY direction102 cm


### 6.2. Relative Measurement Aided Odometer Collaborative Navigation

In the experiment, all three mobile robots are equipped with odometer capable of moving monitoring, UWB capable of measuring relative distance, or lidar capable of measuring relative position.From Figure11, it can be seen that the collaborative navigation system which realizes relative information sharing has significant advantages over the case of not sharing any information in positioning accuracy. Besides, the improvement of group navigation performance of mobile robots is affected by the type of shared relative information. When the relative position information is shared, the growth of the error can be effectively limited; relatively speaking, when the relative distance information is shared, the position error is still growing slowly, which only reduces the growth rate of the error (see Figure 11).Figure 11
Comparative diagram of navigation error under the condition of sharing different relative information. (a) Position error. (b) Angle error.
(a)(b)The analysis shows that the relative distance information is weakly constrained, so sharing this information cannot effectively realize the navigation and localization of mobile robots. In contrast, the sharing of relative position information includes the solution to mobile robot navigation and information. Information accuracy is significantly improved. It can even be increased by more than60% at some time. This difference is more obvious in the angle error diagram (see Figure 11).In this paper, two observation methods, direct relative measurement and indirect relative measurement, are mentioned in the description of the measurement model based on relative position. Based on this experimental scene, scenario I is three mobile robots to observe the relative position information directly through lidar. Scenario II is three mobile robots to extract the surrounding landmark information through lidar, and based on this solution, the relative position information is calculated. In the above experimental scenario, the centralized collaborative navigation algorithm is used to solve the navigation problem. The two relative position measurement methods are compared through the above simulation experimental scenarios. The comparison results are shown in Figure12 (see Figure 12).Figure 12
Comparison of navigation errors under different relative position measuring methods. (a) Position error. (b) Angle error.
(a)(b)Through Figure12, it is clear that the collaborative navigation and positioning accuracy of relative position measurement using the direct method are better than those of indirect method. However, cost calculation cannot be ignored while navigation performance is considered. The direct method requires that the measurement range of lidar includes the activity range of the whole mobile robot population group while the measurement range of lidar required by indirect method only needs to include the surrounding landmarks. This greatly reduces the cost calculation. Considering that the accuracy of collaborative navigation and positioning using the two relative position measurement methods is not much different, it is obvious that the indirect method is more suitable for practical application (see Figure 12).The difference of the decentralized collaborative navigation framework compared with the centralized collaborative navigation framework is that the correlation among the different node states is accurately calculated in the centralized collaborative navigation framework, and this correlation cannot be used in the decentralized collaborative navigation framework. In order to better reflect the impact of this correlation, the navigation errors of the two collaborative navigation algorithms in the odometer collaborative navigation system are shown in Figure13 (see Figure 13).Figure 13
Comparison of navigation errors under different collaborative navigation algorithms. (a) Position error. (b) Angle error.
(a)(b)To compare the two algorithms, 20 experiments are carried out in this paper, and the root mean square error (RMS) and formulas of the two collaborative navigation algorithms are calculated as shown in the following formula:(39)RMS=1n∑i=1nxi−xi¯2,where n is the total number of samples, xi is the actual value, and xi¯ is the estimated value. RMS parameters for the odometer collaborative navigation are shown in Table 2.As can be seen from Figure13 and Table 2, the error of the centralized collaborative navigation algorithm is smaller than that of the decentralized collaborative navigation algorithm. This can be predicted because the correlation among node states in the centralized collaborative navigation algorithm can be calculated accurately, which is estimated in the decentralized collaborative navigation algorithm. However, the improved accuracy of the navigation is at the expense of high computing power and high quality data communication. Therefore, although the performance of centralized collaborative navigation framework is better than that of distributed collaborative navigation framework, the centralized collaborative navigation framework is not applicable in some practical scenarios (see Figure 13).Table 2
Odometer collaborative navigation RMS parameters.
Algorithm typePosition error (m)Angle error (°)CL0.16290.74625DCL0.363421.3762


### 6.3. Odometer/Vision SLAM Collaborative Navigation

In the odometer/vision collaborative navigation model, scenario I is designed that all the mobile robots are equipped with an odometer which can monitor the motion. One of the mobile robots is equipped with SLAM-aided navigation system and can work properly.Firstly, the mobile robot with SLAM-aided navigation system is studied, and it only runs its own integrated navigation algorithm without sharing the relative position information. Using the centralized collaborative navigation algorithm, the navigation error of nodes with SLAM-aided navigation system is shown in Figure14 (see Figure 14).Figure 14
The navigation error map of the node with the SLAM-aided navigation system. (a) Position error. (b) Angle error.
(a)(b)Figure14 fully verifies the correctness of a centralized collaborative navigation algorithm based on the odometer/vision collaborative navigation model. The SLAM-assisted navigation system is based on the relative observation. The position estimation of the node itself and the position estimation of the landmark have the error accumulation, but the association algorithm of the SLAM is combined, the centralized collaborative navigation algorithm makes the position estimation of the landmark closer to the real value while the positioning accuracy of the mobile robot is improved, the data association is more reliable, further correcting the state estimation of the mobile robot itself. Therefore, the algorithm is very obvious to improve the navigation accuracy of mobile robot (see Figure 14).Then, the mobile robots without the SLAM-aided navigation system in the experiment are studied. In order to fully reflect the influence of the SLAM-aided navigation information on the navigation performance of other nodes, Scenario II is designed that all mobile robots are equipped with odometer which can monitor the motion, and two of them are equipped with SLAM-aided navigation system and can work properly. The navigation error of other nodes without SLAM-aided navigation system is shown in Figure15 (see Figure 15).Figure 15
Some nodes are equipped with SLAM-aided navigation system. (a) Position error. (b) Angle error.
(a)(b)As shown in Figure15, the mobile robot with SLAM-aided navigation system performs loop-back detection in about 320 seconds and data associated with the local map created at the initial location, thus eliminating most of the accumulated errors. The unique superior performance of the SLAM-aided navigation system is transmitted to other nodes in the group through the process of information sharing in the process of collaborative navigation, so that it can also eliminate most of the accumulated errors in the vicinity of the time, which is an important advantage of the collaborative navigation system (see Figure 15).To verify the NN algorithm, JBCC algorithm, and the optimized data association algorithm on the navigation performance of nodes without SLAM-aided navigation system, the experimental scene is designed that all mobile robots are equipped with odometer which can carry out motion monitoring. One of the mobile robots is equipped with SLAM-aided navigation system and can work normally, and the CL algorithm is run. The navigation error of nodes without SLAM-aided navigation system is shown in Figure16 (see Figure 16).Figure 16
Comparison diagram of navigation error for fusion of single node SLAM information under different SLAM data association algorithms. (a) Position error. (b) Angle error.
(a)(b)The performance of the centralized collaborative navigation algorithm under the three SLAM data association algorithms is shown in Table 3.From Figure16 and Table 3, it can be seen that the navigation performance of nodes without SLAM-aided navigation system is affected by the SLAM data association algorithm used by nodes carrying SLAM-aided navigation system. Running the NN algorithm, the matching accuracy of feature information is not high, so that the navigation accuracy is poor. Running the JCBB algorithm, the correct rate of data association is the highest, but the operation time is the longest. Running optimization data association algorithm, the navigation accuracy is slightly reduced, but the operation time is less, which can meet the real-time requirements (see Figure 16).Table 3
Performance comparison of centralized collaborative navigation algorithms under different SLAM data association algorithms.
Algorithm typePosition error (m)Angle error (°)Relative timeNN2.832310.79194JCBB0.03220.162312Optimization0.55872.24761In this subsection, to compare the performance of collaborative navigation systems in odometer/vision collaborative navigation systems under centralized and decentralized collaborative navigation algorithms, we run the CL and DCL algorithms separately under the experimental scenario I. The navigation errors of the two collaborative navigation algorithms are compared as shown in Figure17. Under the experimental scenario II of this subsection, we run the CL algorithm and the DCL algorithm, respectively. The navigation errors of the two collaborative navigation algorithms are compared as shown in Figure 18 (see Figures 17 and 18).Figure 17
Comparative diagram of navigation error for fusion of single node SLAM information under different collaborative navigation algorithms. (a) Position error. (b) Angle error.
(a)(b)Figure 18
Comparison diagram of navigation error for fusion of multinode SLAM information under different collaborative navigation algorithms. (a) Position error. (b) Angle error.
(a)(b)After 20 experiments, the RMS parameters of collaborative navigation with single node SLAM information are shown in Table 4.The RMS parameters of the coordinated navigation with the fused multinode SLAM information are shown in Table 5.As can be seen from Figures17 and 18 in conjunction with Tables 4 and 5, in the odometer/vision collaborative navigation system, the error of the centralized collaborative navigation algorithm is less than the distributed collaborative navigation algorithm; after the landmark information collected by the single node or the multinode is fused, there is a small gap between the two algorithms. In other words, the distributed collaborative navigation algorithm based on the odometer/vision collaborative navigation model can well estimate the correlation of the internode information (see Figures 17 and 18).Table 4
Collaborative navigation RMS parameters for fusion of single node SLAM information.
Algorithm typePosition error (m)Angle error (°)CL0.03220.1623DCL0.06690.2094Table 5
Collaborative navigation RMS parameters for fusion of multinode SLAM information.
Algorithm typePosition error (m)Angle error (°)CL0.02430.0524DCL0.04380.1265Considering the high requirement of the centralized collaborative navigation algorithm to the computing power and the communication level, the application scenarios of the two algorithms are analyzed in combination with the abovementioned collaborative navigation experiment: the centralized collaborative navigation algorithm is suitable for the case that there are few nodes and the nodes are not equipped with additional aided navigation system, the decentralized collaborative navigation algorithm is suitable for the large number of nodes and the large amount of information shared, and some nodes are equipped with additional aided navigation systems, especially in the case of SLAM-aided navigation system.


## 6.1. Simulated Experimental Environment

In this section, the nodes of the mobile robot network involved in collaborative navigation are 3. Assuming that the area of the moving2D environment is 25m×25m when the mobile robot population group works together, each node in the environment is assigned to an initial position, and each node can follow random trajectory motion in this area. Assuming that all nodes follow the same simulation trajectory, only the initial position is different. The maximum speed of the mobile robot in the straight line is 0.3125m/s and the angular velocity at the bend is 0.1°/s. It is assumed that the environment around the simulated trajectory of this rectangle can be extracted by lidar scanning to 88 landmarks (environmental feature points) for the SLAM-assisted navigation system (see Figure 10).Figure 10
Simulation trajectory diagram.During this simulation, mobile robots as carriers can carry different types of sensors, including odometers, UWB, and lidar. Suitable sensors are selected according to the requirements of positioning accuracy, among which Time Domain P410 UWB sensors are used to measure the relative distance, and lidar is selected from LMS291 series 2D lidar produced by a German company. Based on the relevant parameters of these sensors, which are shown in Table1, a simulation model for mobile robots carrying different types of sensors is built using MATLAB.Table 1
Relevant parameters of sensors.
TypeMeasureFrequency (Hz)ErrorOdometerLinear velocity of the two wheels204 cm/sUWBRelative distance103 cmLidarRelative position ofX direction102 cmRelative position ofY direction102 cm


## 6.2. Relative Measurement Aided Odometer Collaborative Navigation

In the experiment, all three mobile robots are equipped with odometer capable of moving monitoring, UWB capable of measuring relative distance, or lidar capable of measuring relative position.From Figure11, it can be seen that the collaborative navigation system which realizes relative information sharing has significant advantages over the case of not sharing any information in positioning accuracy. Besides, the improvement of group navigation performance of mobile robots is affected by the type of shared relative information. When the relative position information is shared, the growth of the error can be effectively limited; relatively speaking, when the relative distance information is shared, the position error is still growing slowly, which only reduces the growth rate of the error (see Figure 11).Figure 11
Comparative diagram of navigation error under the condition of sharing different relative information. (a) Position error. (b) Angle error.
(a)(b)The analysis shows that the relative distance information is weakly constrained, so sharing this information cannot effectively realize the navigation and localization of mobile robots. In contrast, the sharing of relative position information includes the solution to mobile robot navigation and information. Information accuracy is significantly improved. It can even be increased by more than60% at some time. This difference is more obvious in the angle error diagram (see Figure 11).In this paper, two observation methods, direct relative measurement and indirect relative measurement, are mentioned in the description of the measurement model based on relative position. Based on this experimental scene, scenario I is three mobile robots to observe the relative position information directly through lidar. Scenario II is three mobile robots to extract the surrounding landmark information through lidar, and based on this solution, the relative position information is calculated. In the above experimental scenario, the centralized collaborative navigation algorithm is used to solve the navigation problem. The two relative position measurement methods are compared through the above simulation experimental scenarios. The comparison results are shown in Figure12 (see Figure 12).Figure 12
Comparison of navigation errors under different relative position measuring methods. (a) Position error. (b) Angle error.
(a)(b)Through Figure12, it is clear that the collaborative navigation and positioning accuracy of relative position measurement using the direct method are better than those of indirect method. However, cost calculation cannot be ignored while navigation performance is considered. The direct method requires that the measurement range of lidar includes the activity range of the whole mobile robot population group while the measurement range of lidar required by indirect method only needs to include the surrounding landmarks. This greatly reduces the cost calculation. Considering that the accuracy of collaborative navigation and positioning using the two relative position measurement methods is not much different, it is obvious that the indirect method is more suitable for practical application (see Figure 12).The difference of the decentralized collaborative navigation framework compared with the centralized collaborative navigation framework is that the correlation among the different node states is accurately calculated in the centralized collaborative navigation framework, and this correlation cannot be used in the decentralized collaborative navigation framework. In order to better reflect the impact of this correlation, the navigation errors of the two collaborative navigation algorithms in the odometer collaborative navigation system are shown in Figure13 (see Figure 13).Figure 13
Comparison of navigation errors under different collaborative navigation algorithms. (a) Position error. (b) Angle error.
(a)(b)To compare the two algorithms, 20 experiments are carried out in this paper, and the root mean square error (RMS) and formulas of the two collaborative navigation algorithms are calculated as shown in the following formula:(39)RMS=1n∑i=1nxi−xi¯2,where n is the total number of samples, xi is the actual value, and xi¯ is the estimated value. RMS parameters for the odometer collaborative navigation are shown in Table 2.As can be seen from Figure13 and Table 2, the error of the centralized collaborative navigation algorithm is smaller than that of the decentralized collaborative navigation algorithm. This can be predicted because the correlation among node states in the centralized collaborative navigation algorithm can be calculated accurately, which is estimated in the decentralized collaborative navigation algorithm. However, the improved accuracy of the navigation is at the expense of high computing power and high quality data communication. Therefore, although the performance of centralized collaborative navigation framework is better than that of distributed collaborative navigation framework, the centralized collaborative navigation framework is not applicable in some practical scenarios (see Figure 13).Table 2
Odometer collaborative navigation RMS parameters.
Algorithm typePosition error (m)Angle error (°)CL0.16290.74625DCL0.363421.3762


## 6.3. Odometer/Vision SLAM Collaborative Navigation

In the odometer/vision collaborative navigation model, scenario I is designed that all the mobile robots are equipped with an odometer which can monitor the motion. One of the mobile robots is equipped with SLAM-aided navigation system and can work properly.Firstly, the mobile robot with SLAM-aided navigation system is studied, and it only runs its own integrated navigation algorithm without sharing the relative position information. Using the centralized collaborative navigation algorithm, the navigation error of nodes with SLAM-aided navigation system is shown in Figure14 (see Figure 14).Figure 14
The navigation error map of the node with the SLAM-aided navigation system. (a) Position error. (b) Angle error.
(a)(b)Figure14 fully verifies the correctness of a centralized collaborative navigation algorithm based on the odometer/vision collaborative navigation model. The SLAM-assisted navigation system is based on the relative observation. The position estimation of the node itself and the position estimation of the landmark have the error accumulation, but the association algorithm of the SLAM is combined, the centralized collaborative navigation algorithm makes the position estimation of the landmark closer to the real value while the positioning accuracy of the mobile robot is improved, the data association is more reliable, further correcting the state estimation of the mobile robot itself. Therefore, the algorithm is very obvious to improve the navigation accuracy of mobile robot (see Figure 14).Then, the mobile robots without the SLAM-aided navigation system in the experiment are studied. In order to fully reflect the influence of the SLAM-aided navigation information on the navigation performance of other nodes, Scenario II is designed that all mobile robots are equipped with odometer which can monitor the motion, and two of them are equipped with SLAM-aided navigation system and can work properly. The navigation error of other nodes without SLAM-aided navigation system is shown in Figure15 (see Figure 15).Figure 15
Some nodes are equipped with SLAM-aided navigation system. (a) Position error. (b) Angle error.
(a)(b)As shown in Figure15, the mobile robot with SLAM-aided navigation system performs loop-back detection in about 320 seconds and data associated with the local map created at the initial location, thus eliminating most of the accumulated errors. The unique superior performance of the SLAM-aided navigation system is transmitted to other nodes in the group through the process of information sharing in the process of collaborative navigation, so that it can also eliminate most of the accumulated errors in the vicinity of the time, which is an important advantage of the collaborative navigation system (see Figure 15).To verify the NN algorithm, JBCC algorithm, and the optimized data association algorithm on the navigation performance of nodes without SLAM-aided navigation system, the experimental scene is designed that all mobile robots are equipped with odometer which can carry out motion monitoring. One of the mobile robots is equipped with SLAM-aided navigation system and can work normally, and the CL algorithm is run. The navigation error of nodes without SLAM-aided navigation system is shown in Figure16 (see Figure 16).Figure 16
Comparison diagram of navigation error for fusion of single node SLAM information under different SLAM data association algorithms. (a) Position error. (b) Angle error.
(a)(b)The performance of the centralized collaborative navigation algorithm under the three SLAM data association algorithms is shown in Table 3.From Figure16 and Table 3, it can be seen that the navigation performance of nodes without SLAM-aided navigation system is affected by the SLAM data association algorithm used by nodes carrying SLAM-aided navigation system. Running the NN algorithm, the matching accuracy of feature information is not high, so that the navigation accuracy is poor. Running the JCBB algorithm, the correct rate of data association is the highest, but the operation time is the longest. Running optimization data association algorithm, the navigation accuracy is slightly reduced, but the operation time is less, which can meet the real-time requirements (see Figure 16).Table 3
Performance comparison of centralized collaborative navigation algorithms under different SLAM data association algorithms.
Algorithm typePosition error (m)Angle error (°)Relative timeNN2.832310.79194JCBB0.03220.162312Optimization0.55872.24761In this subsection, to compare the performance of collaborative navigation systems in odometer/vision collaborative navigation systems under centralized and decentralized collaborative navigation algorithms, we run the CL and DCL algorithms separately under the experimental scenario I. The navigation errors of the two collaborative navigation algorithms are compared as shown in Figure17. Under the experimental scenario II of this subsection, we run the CL algorithm and the DCL algorithm, respectively. The navigation errors of the two collaborative navigation algorithms are compared as shown in Figure 18 (see Figures 17 and 18).Figure 17
Comparative diagram of navigation error for fusion of single node SLAM information under different collaborative navigation algorithms. (a) Position error. (b) Angle error.
(a)(b)Figure 18
Comparison diagram of navigation error for fusion of multinode SLAM information under different collaborative navigation algorithms. (a) Position error. (b) Angle error.
(a)(b)After 20 experiments, the RMS parameters of collaborative navigation with single node SLAM information are shown in Table 4.The RMS parameters of the coordinated navigation with the fused multinode SLAM information are shown in Table 5.As can be seen from Figures17 and 18 in conjunction with Tables 4 and 5, in the odometer/vision collaborative navigation system, the error of the centralized collaborative navigation algorithm is less than the distributed collaborative navigation algorithm; after the landmark information collected by the single node or the multinode is fused, there is a small gap between the two algorithms. In other words, the distributed collaborative navigation algorithm based on the odometer/vision collaborative navigation model can well estimate the correlation of the internode information (see Figures 17 and 18).Table 4
Collaborative navigation RMS parameters for fusion of single node SLAM information.
Algorithm typePosition error (m)Angle error (°)CL0.03220.1623DCL0.06690.2094Table 5
Collaborative navigation RMS parameters for fusion of multinode SLAM information.
Algorithm typePosition error (m)Angle error (°)CL0.02430.0524DCL0.04380.1265Considering the high requirement of the centralized collaborative navigation algorithm to the computing power and the communication level, the application scenarios of the two algorithms are analyzed in combination with the abovementioned collaborative navigation experiment: the centralized collaborative navigation algorithm is suitable for the case that there are few nodes and the nodes are not equipped with additional aided navigation system, the decentralized collaborative navigation algorithm is suitable for the large number of nodes and the large amount of information shared, and some nodes are equipped with additional aided navigation systems, especially in the case of SLAM-aided navigation system.


## 7. Conclusion

In order to improve the performance of cooperative navigation system, a multirobot collaborative navigation algorithm based on odometer/vision multisource information fusion is studied. On the basis of establishing the multisource information fusion collaborative navigation system model, the centralized collaborative navigation of odometer/vision fusion, the decentralized collaborative navigation framework, and the vision-based SLAM are given, and the centralized and decentralized odometer/vision collaborative navigation algorithms are derived, respectively. The effectiveness of the proposed algorithm is verified by the simulation experiments, which has some theoretical value and application value in high performance collaborative navigation applications.



---

*Source: 5819409-2020-08-27.xml*