Cooperative Localization Using Fuzzy DDEIF and Broad Learning System for Uncertain Heterogeneous Omnidirectional Multi-robots

Abstract

This paper presents a novel fuzzy distributed and decentralized extended information filtering (FDDEIF) method using broad learning system (BLS), called BLS-FDDEIF, for indoor cooperative localization of a group of heterogeneous omnidirectional mobile robots (HOMRs) incorporated with their dynamic effects. A new pose initialization algorithm is proposed to estimate the robots’ initial poses. Once all the initial poses of the HOMRs have been roughly determined, a novel BLS-FDDEIF method is presented to fuse multisensory measurements for estimating more accurate poses of all the HOMRs. Comparative simulations and experimental results are conducted to show the effectiveness and superiority of the proposed method in finding accurate pose estimation of three cooperative HOMRs with unknown initial poses.

1 Introduction

Multi-robot systems have received much attention in intelligent control and smart automation communities or professional societies due to their many successful applications, such as industrial manufacturing, human servicing, smart agriculture, cooperative material handling via multi-robots, and so on. Such systems or robots have been mainly employed to reduce human interventions in doing numerous dangerous, repeated, boring, and difficult tasks. In general, interactions among multi-robots can be classified into five technical categories: collective, cooperative, collaborative, coordinative, and adversary [1]. Worthy of mention is that, in the cooperative interaction, mobile robots help each other and cooperate with each other to achieve their common or shared goal. This cooperative control method has been widely used in many applications, such as Intel’s 500 Drones, artificial satellites, and service robot in a team. Cooperative multi-robot systems have been shown to perform tasks, which cannot be done by a single mobile robot, or such task can be completed or accomplished more efficiently by exchanging information among robots.

Recently, cooperative localization of a group of mobile multi-robots has also attracted considerable attention in control and robotics communities. This technology is concerned with two technical issues: one is group pose initialization which means to find initial poses of all multi-robots in a team, and the other is group pose tracking which is related to keep track of moving poses of all multi-robots. The authors in [2] proposed a FDDEIF method with a modified graph theory for cooperative localization of a class of mobile wheeled multi-robots by addressing both issues of group pose initialization and group pose tracking. Furthermore, Tan [3] proposed a two cooperative global localization methods using fuzzy decentralized extended information filtering (FDEIF) and fuzzy decentralized extended Kalman filtering (FDEKF) for global cooperative positioning of a multi-ballbot system in the case of known maps. Yang [4] addressed the group pose initialization and tracking problems using fuzzy distributed and decentralized extended information filtering (FDDEIF) for a team of omnidirectional mobile robots. Wang and de Silva [5] presented decentralized Kalman filters to achieve cooperative transportation of multiple robots for group pose tracking. However, those methods in [1,2,3,4,5] for cooperative localization of a class of mobile multi-robots has not considered the dynamic effect of each mobile robot yet.

Neural networks have been widely used for nonlinear system identification and control; for example, recurrent fuzzy wavelet neural networks were exploited to achieve trajectory tracking of a tilting quadrotor [6]. Recently, broad learning system (BLS) has been proposed as an original flat structure established based on the random vector functional-link neural network (RVFLNN), which inherits its major features [7]; this type of system can be expanded in a wide sense [7]. Chen et al. [8] further discussed the general approximation capability and nonlinear system of BLS and some variants with their mathematical models. BLS is indeed different from some popular deep neural networks, which have high computing cost and suffer from a time consuming learning for excessive parameters, thereby providing a much faster method with high accuracy. In applying BLS, Feng and Chen [9] applied BLS with an iterative gradient decent algorithm to propose a BLS control method for a class of nonlinear dynamic system, and Tsai et al. [10] combined BLS and fractional-order nonsingular terminal siding-mode control to accomplish a nonlinear consensus formation control for a kind of heterogeneous omnidirectional robots. Motivated by [8,9,10], the BLS would be helpful in learning unknown dynamic uncertainties in real time.

Omnidirectional mobile robots (OMRs) have been widely used for our living life and industrial applications. There are two kinds of Swedish or Mecanum wheels, which are, respectively, 45° and 90° [11, 12]. Unlike conventional differential driving, OMRs have the superior flexibility to move towards any position and orientation. Owing to both wheel structures, OMRs can be made using different wheel configurations including three wheels, four wheels, car-like four wheels, etc. In [13], the authors proposed a consensus-based formation control method for heterogeneous OMRs (HOMRs) which are OMRs with different wheel configurations and distinct dynamics by finding their unified dynamic models. Inspired by [4, 8,9,10, 13], it would be theoretically and practically interesting to combine FDDEIF and BLS to address the group pose initialization, pose, and pose rate estimation problems for a team of HOMRs incorporated with uncertain dynamic effects.

Hence, the objective of this paper is to combine the graph-based FDDEIF method and BLS for developing a new BLS-FDDEIF method to address both group global pose initialization and moving pose estimation problems for a dynamic multi-HOMR system whose communication network is not fully connected. By comparing to existing methods, the presented contents are delineated in two technical contributions. One is a novel BLS-FDDEIF proposed for cooperative pose initialization and dynamic pose tracking of a group of HOMRs, where the iterative learning algorithms for each BLS have been proposed to online learn an uncertain term in the dynamic model of each HOMR incorporated with dynamic effects and uncertainties. The other is numerical and experimental verification of the proposed BLS-FDDEIF approach via computer simulations and experimental results of a physical multi-HOMR system.

The rest of the paper is outlined as follows. Section 2 briefly recalls the measurement models of HOMRs. Section 3 proposes the BLS identifier to learn the uncertain dynamic part of each HOMR with dynamic effects and uncertainties. Section 4 introduces the DDEIF method for mobile multi-robot systems, and then describes the FDDEIF algorithm with the fuzzy tuner. Section 5 proposes the cooperative pose initialization algorithm to corporately initialize the poses of all the HOMRS. Section 6 presents the proposed BLS-FDDEIF algorithm for dynamic localization of the multi-HOMR system. Computer simulations are performed in Section 7 to show the effectiveness and superiority of the proposed BLS-FDDEIF algorithm. Section 8 shows and discusses two experiment results. Finally, Section 9 concludes this paper.

2 Measurement Models of HOMRs

This section is to recall the models of the RGB-D sensor and laser scanner mounted on the three kinds of HOMRs, which are, respectively, four-wheeled OMR (FOMR), car-like OMR (COMR), and three-wheeled OMR (TOMR), as shown in Fig. 1a–c, since each HOMR is equipped with one RGB-D sensor, one laser scanner, and appropriate numbers of encoders on its driving wheels. The basic dead-reckoning or odometry of each HOMR, based on the corresponding encoders’ outputs and kinematic models, is described in [13] and not mentioned here.

Fig. 1

Three HOMRs and sensor models. a Four-wheeled OMR (FOMR). b Car-like OMR (COMR). c Three-wheeled OMR (TOMR). d Parameters defining the jth landmark. e Parameters defining the jth wall

2.1 RGB-D Sensor Model

RGB-D sensor is equipped with one RGB camera and a pair of depth sensors. The RGB-D sensor is mounted on each HOMR at its designed height, where Fig. 1a–c show the used Realsense D415 RGB-D sensor. In the measurement update step, each HOMR detects landmarks around it in order to estimate its pose. Via the RGB-D measurements and the used landmark detection procedure in [2, 4], it is easy to calculate the azimuth angle α_ij with respect to the heading direction of the ith HOMR and the distance λ_ij to the jth landmark L_j(x_L, y_L, z_L) as shown in Fig. 1d at time instant k. The value of the measurement function for the jth landmark from HOMR i is expressed by

$$\begin{aligned} z_{{\text{RGBD}_{ij} }} (k) & = h_{{\text{RGBD}_{ij} }} \left( {X(k), L_{i} } \right) = \left[ {\begin{array}{*{20}l} {\alpha_{ij} } \\ {\lambda_{ij} } \\ \end{array} } \right] \\ & = \left[ {\begin{array}{*{20}l} {\tan^{ - 1} \frac{{y_{{L_{j} }} - y_{i} \left(k \right)}}{{x_{{L_{j} }} - x_{i} \left(k \right)}} - \theta_{i} \left( k \right)} \\ {\sqrt {\left( {x_{{L_{j} }} - x_{i} \left( k \right)} \right)^{2} + \left( {y_{{L_{j} }} - y_{i} \left( k \right)} \right)^{2} + \left( {z_{{L_{j} }} - z_{i} \left( k \right)} \right)^{2} } } \\ \end{array} } \right] + V_{{\text{RGBD}_{ij} }} (k) \\ \end{aligned}$$

(1)

where x_i(k), y_i(k), z_i(k), θ_i(k) represents the pose coordinates of the ith HOMR in the world frame; note that $V_{{{\text{RGBD}}_{ij} }} (k)$ is the zero-mean measurement noise vector with zero mean and diagonal covariance matrix $R_{{{\text{RGBD}}_{ij} }} (k)$. Note that the azimuth angle α_ij is negative in the clockwise direction.

2.2 Laser Scanner

In order to localize each HOMR by the laser scanner, UST-10LX, from Hokuyo, we here adopt the well-known minimalistic environment model, which is composed of a set of walls, m_j i.e., ${\mathbf{M}} = \left\{ {m_{j} ,j = 1, \ldots ,\overline{M} } \right\}$. Figure 1e illustrates the parameters which define the jth wall, where (x_e, y_e) and (x_s, y_s) are the ending and starting points of the jth wall, l_j is the length of the jth wall, ρ ^m _j is the perpendicular distance from the origin in the world frame to the jth wall, and β ^m _j stands for the orientation of the jth wall. Distance measurements can be obtained from the laser ranging data in one whole scanning cycle. Therefore, the measurement model of the laser scanner for HOMR i is expressed by [2, 4]

$$z_{\text{laser}i} (k) = \bar{h}_{\text{laser}i} (X_{i} (k), \, {\mathbf{M}}) + V_{li} (k)$$

(2)

where $\bar{h}_{\text{laser}} (X_{i} , \, {\mathbf{M}}) = \left[ {\begin{array}{*{20}l} {\overline{h}_{{{\text{laser}}_{i1} }} \left( {X_{i} ,m_{1} } \right)}\ldots {\overline{h}_{{{\text{laser}}_{{i\overline{M} }} }} \left( {X_{i} ,m_{{\overline{M} }} } \right)} \\ \end{array} } \right]^{\text{T}}$, $X_{i} (k) = [x_{i} \, y_{i} (k) \, \theta_{i} (k)]^{\text{T}}$, $V(k) = \left[ {\begin{array}{*{20}l} {v_{1} (k)} & {v_{2} (k)} & \ldots & {v_{{\overline{M} }} (k)} \\ \end{array} } \right]^{T}$, and $\overline{M}$ denotes the number of the total walls. Note that V_li(k) denote is the measurement noise vector with zero mean and covariance matrix $R_{{{l}i}} (k) = {\text{diag}}\left\{ {\delta_{1}^{2} ,\delta_{2}^{2} , \ldots ,\delta_{{\overline{M} }}^{2} } \right\}.$ Moreover, the measurement function $\overline{h}_{{{\text{laser}}_i}} (X_{i} ,m_{j} )$ is described by

$$\bar{h}_{{\text{laser}_{ij} }} \left( {X_{i} ,m_{j} } \right) = \left[ {\begin{array}{*{20}c} {\rho_{ij} } \\ {\beta_{ij} } \\ \end{array} } \right]$$

(3)

where

$$\begin{aligned} \rho_{ij} & = \rho_{j}^{m} - \sqrt {x_{i}^{2} + y_{i}^{2} } \cos \left( {\alpha_{j}^{m} - \psi } \right) = \rho_{j}^{m} - \left( {x_{i} \cos \alpha_{j}^{m} + y_{i} \sin \alpha_{j}^{m} } \right) \\ \beta_{ij} &= \beta_{j}^{m} - \theta_{i} ,\quad \, j = 1, \, 2, \, 3, \ldots , \, \overline{M} . \\ \end{aligned}$$

3 BLS Identifier for Dynamic Model of Each HOMR with Dynamic Effect and Uncertainties

This section will introduce the nonlinear discrete-time stochastic system model of the multi-HOMR system with one set of measurement models, and construct a BLS identifier to online learn the dynamic term of each HOMR with dynamic effects and uncertainties. The unified dynamic model of HOMR proposed in [13] will be used. In particular, the iterative learning algorithm of a BLS identifier will be derived based on the well-known gradient descent method.

3.1 Modeling of Multi-HOMRs with Dynamic Effects

Since the multi-HOMR system is composed of N HOMRs, the dynamic behavior model of the ith HOMR with one RGB-D sensor and one laser scanner is modeled by a nonlinear discrete-time stochastic system model with one measurement model expressed by [13]

$$\left[ {\begin{array}{*{20}c} {x_{i} \left( {k + 1} \right)} \\ {v_{i} \left( {k + 1} \right)} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {x_{i} \left( k \right) + Tv_{i} \left( k \right)} \\ {T \cdot \left( { - g_{i} \left( k \right) + \xi_{i} \left( k \right)} \right) + Tu_{i} (k)} \\ \end{array} } \right] \,$$

(4)

or in a vector–matrix form

$$X_{i} \left( {k + 1} \right) = f_{i} \left( {X_{i} (k), u (k)} \right) + \left[ {\begin{array}{*{20}l} 0 \\ {T\xi_{i} \left( k \right)} \\ \end{array} } \right]$$

(5)

$$Z_{i} \left( k \right) = h_{i} \left( {X_{i} \left( k \right),{\mathbf{L}},{\mathbf{M}},k} \right) + V_{i} \left( k \right)$$

(6)

where L denotes the set of all landmarks; x_i(k) ∈ R³, v_i(k) ∈ R³; X_i(k + 1) = [x_i(k + 1) v_i(k + 1)]^T ∈ R⁶; h_i(·) ∈ R^P are nonlinear, twice differentiable functions of the system state; u_i(k) ∈ R³ means the input of the ith HOMR. The stochastic process vectors, $\xi_{i} \left( k \right)$ ~ N(0, Q_i(k)) and V_i(k) ~ N(0, R_i(k)) are the process noise and the measurement noise, respectively. Note that h_i (X_i(k), L, M, k) is the combination of the measurement models of the RGB-D camera and laser scanner. Since the nonlinear term, g_i(k), is unknown, the BLS will be used to online learn the nonlinear term, g_i(k), based on the gradient descent method in the coming subsection.

3.2 BLS Approximation

This subsection will propose iterative learning algorithms for BLS to learn online and approximate the nonlinear term, $g_{i} (k)$, in (4). The proposed iterative learning algorithms are based on the gradient descent method; the algorithms first find the gradients of all the updating parameters in the used BLS as shown in Fig. 2, and then use the deepest descent approach to obtaining the iterative parameter learning rules or algorithms. In doing so, suppose that there are $n$ mapping groups with $K_{i}$ feature nodes in the $i{\text{th}}$ group, and there are one group of $m$ enhancement nodes, the input vector is denoted ${\mathbf{X}}_{l}$, l = 1, …, M, and the BLS output vector is denoted as $\hat{y}$.

$$\hat{y} = \sum\limits_{i = 1}^{n} {\sum\limits_{k = 1}^{{K_{i} }} {w_{k}^{i} } } F_{k}^{i} + \sum\limits_{j = 1}^{m} {w_{j} \xi_{j} } \left( {\sum\limits_{i = 1}^{n} {\sum\limits_{k = 1}^{{K_{i} }} {w_{jk}^{i} } } F_{k}^{i} + b_{j} } \right)$$

(7)

where $F_{k}^{i} = \phi_{k}^{i} \left( {\sum\nolimits_{l = 1}^{M} {\left( {w_{{f_{kl} }}^{i} {\mathbf{X}}_{l} + b_{{f_{k} }}^{i} } \right)} } \right)$ denotes the output of the $k{\text{th}}$ feature node in the $i{\text{th}}$ mapping group, $\phi_{k}^{i} ( \bullet )$ and $\xi_{j} ( \bullet )$ is the activation function. We denote $W_{fi} \, = { [}w_{fkl}^{i} ]_{{M \times K_{i} }}$, $\beta_{fi} = [b_{fk}^{i} ]_{{1 \times K_{i} }}$, $W_{e} \, = { [}w_{jk}^{i} ]_{{\left( {K_{1} + K_{2} + \cdots + K_{n} } \right) \times m}}$, $\beta_{e} = [b_{j} ]_{1 \times m}$, where $w_{{f_{kl} }}^{i}$ is the weight connecting the $l{\text{th}}$ input $x_{l}$ to the $k{\text{th}}$ feature node in the $i{\text{th}}$ mapping group, $b_{{f_{k} }}^{i}$ is the bias term associated to the $k{\text{th}}$ feature node in the $i{\text{th}}$ mapping group, $w_{jk}^{i}$ is the weight connecting the $k{\text{th}}$ feature node of the $i{\text{th}}$ mapping group to the $j{\text{th}}$ enhancement node, and $b_{j}$ is the bias term associated to the $j{\text{th}}$ enhancement node. We define the weight matrix connecting the outputs of feature nodes and enhancement nodes to the output neuron as $W = \left[ {w_{{_{1} }}^{1} ,\ldots w_{{K_{1} }}^{1} ,\ldots ,w_{{_{n} }}^{n} ,\ldots w_{{K_{n} }}^{n} ,w_{1} ,\ldots ,w_{m} } \right]^{\text{T}}$ where $w_{k}^{i}$ is the weight connecting the $k{\text{th}}$ feature node in the $i{\text{th}}$ mapping group to the output neuron, and $w_{j}$ is the weight connecting the $j{\text{th}}$ enhancement node to the output neuron.

Fig. 2

Structure of the BLS

Next, derive the learning laws for the six types of parameters,$w_{{f_{kl} }}^{i}$, $b_{{f_{k} }}^{i}$, $w_{jk}^{i}$, $b_{j}$, $w_{k}^{i}$, $w_{j}$, according to the gradient descent method in the following six steps. First, the learning law for $w_{{f_{kl} }}^{i}$ is

$$w_{fkl}^{i} (k + 1) = w_{fkl}^{i} (k) - \eta \frac{\partial E(k)}{{\partial w_{fkl}^{i} }} = w_{fkl}^{i} (k) + \eta \left( y(k) - \hat{y}(k) \right) \frac{{\partial \hat{y}(k)}}{{\partial w_{fkl}^{i} }}$$

(8)

where $E(k) = \left( {y(k) - \hat{y}(k)} \right)^{2} /2$,

$$\hat{y} = \sum\limits_{i = 1}^{n} {\sum\limits_{k = 1}^{{K_{i} }} {w_{k}^{i} } F_{k}^{i} } + \sum\limits_{j = 1}^{m} {w_{j} } \xi_{j} \left( {\sum\limits_{i = 1}^{n} {\sum\limits_{k = 1}^{{K_{i} }} {w_{jk}^{i} } F_{k}^{i} + b_{j} } } \right),$$

and

$$\begin{aligned} \frac{{\partial \hat{y}(k)}}{{\partial w_{fkl}^{i} }} & = \frac{{\partial \sum\nolimits_{i = 1}^{n} {} \sum\nolimits_{k = 1}^{{K_{i} }} {w_{k}^{i} } F_{k}^{i} }}{{\partial F_{k}^{i} (k)}}\frac{{\partial F_{k}^{i} (k)}}{{\partial \left( \sum\nolimits_{l = 1}^{{K_{i} }} {w_{fkl}^{i} } x_{l} + b_{fk}^{i} \right) }}\frac{{\partial \left(\sum\nolimits_{l = 1}^{{K_{i} }} {w_{fkl}^{i} } x_{l} + b_{fk}^{i}\right)}}{{\partial w_{fkl}^{i} }} \\ & \quad + \sum\limits_{j = 1}^{m} {w_{j} } \frac{{\partial \xi_{j} (k)}}{{\partial \left(\sum\nolimits_{i = 1}^{n} {} \sum\nolimits_{k = 1}^{{K_{i} }} {w_{jk}^{i} } F_{k}^{i} + b_{j} \right) }}\frac{{\partial \left( \sum\nolimits_{i = 1}^{n} {} \sum\nolimits_{k = 1}^{{K_{i} }} {w_{jk}^{i} } F_{k}^{i} + b_{j} \right) }}{{\partial F_{k}^{i} (k)}}\frac{{\partial F_{k}^{i} (k)}}{{\partial \left(\sum\nolimits_{l = 1}^{{K_{i} }} {w_{fkl}^{i} } x_{l} + b_{fk}^{i} \right)}} \\ & \frac{{\partial \left(\sum\nolimits_{l = 1}^{{K_{i} }} {w_{fkl}^{i} } x_{l} + b_{fk}^{i} \right)}}{{\partial w_{fkl}^{i} }} = w_{k}^{i} \dot{\phi }_{k}^{i} x_{l} + \sum\limits_{j = 1}^{m} {w_{j} \dot{\xi }_{j} w_{jk}^{i} } \dot{\phi }_{k}^{i} x_{l} = \left(w_{k}^{i} + \sum\limits_{j = 1}^{m} {w_{j} \dot{\xi }_{j} w_{jk}^{i} } \right)\dot{\phi }_{k}^{i} x_{l} \\ \end{aligned}$$

Second, the learning law for $b_{{f_{k} }}^{i}$ is

$$b_{fk}^{i} (k + 1) = b_{fk}^{i} (k) - \eta \frac{\partial E(k)}{{\partial b_{fk}^{i} }} = w_{k}^{i} (k) + \eta \left( {y(k) - \hat{y}(k)} \right)\frac{{\partial \hat{y}(k)}}{{\partial b_{fk}^{i} }}$$

(9)

where

$$\begin{aligned} \frac{{\partial \hat{y}(k)}}{{\partial b_{fk}^{i} }} & = \frac{{\partial \sum\nolimits_{i = 1}^{n} {\sum\nolimits_{k = 1}^{{K_{i} }} {w_{k}^{i} } F_{k}^{i} } }}{{\partial F_{k}^{i} (k)}}\frac{{\partial F_{k}^{i} (k)}}{{\partial \left( {\sum\nolimits_{l = 1}^{{K_{i} }} {w_{fkl}^{i} } x_{l} + b_{fk}^{i} } \right)}}\frac{{\partial \left( {\sum\nolimits_{l = 1}^{{K_{i} }} {w_{fkl}^{i} } x_{l} + b_{fk}^{i} } \right)}}{{\partial b_{fk}^{i} }} \\ & \quad + \sum\nolimits_{j = 1}^{m} {w_{j} } \frac{{\partial \xi_{j} (k)}}{{\partial \left( {\sum\nolimits_{i = 1}^{n} {\sum\nolimits_{k = 1}^{{K_{i} }} {w_{jk}^{i} } F_{k}^{i} + b_{j} } } \right)}}\frac{{\partial \left( {\sum\nolimits_{i = 1}^{n} {\sum\nolimits_{k = 1}^{{K_{i} }} {w_{jk}^{i} } F_{k}^{i} + b_{j} } } \right)}}{{\partial F_{k}^{i} (k)}}\frac{{\partial F_{k}^{i} (k)}}{{\partial \left( {\sum\nolimits_{l = 1}^{{K_{i} }} {w_{fkl}^{i} } x_{l} + b_{fk}^{i} } \right)}} \\ & \frac{{\partial \left( {\sum\nolimits_{l = 1}^{{K_{i} }} {w_{fkl}^{i} } x_{l} + b_{fk}^{i} } \right)}}{{\partial b_{fk}^{i} }} = w_{k}^{i} \dot{\phi }_{k}^{i} + \sum\nolimits_{j = 1}^{m} {w_{j} \dot{\xi }_{j} w_{jk}^{i} } \dot{\phi }_{k}^{i} = \left( {w_{k}^{i} + \sum\nolimits_{j = 1}^{m} {w_{j} \dot{\xi }_{j} w_{jk}^{i} } } \right)\dot{\phi }_{k}^{i} \\ \end{aligned}$$

Third, the learning law for $w_{jk}^{i}$ is

$$w_{jk}^{i} (k + 1) = w_{jk}^{i} (k) - \eta \frac{\partial E(k)}{{\partial w_{jk}^{i} }} = w_{jk}^{i} (k) + \eta \left( {y(k) - \hat{y}(k)} \right)\frac{{\partial \hat{y}(k)}}{{\partial w_{jk}^{i} }}$$

(10)

where

$$\frac{{\partial \hat{y}(k)}}{{\partial w_{jk}^{i} }} = \frac{{\partial \hat{y}}}{{\partial \xi_{j} }} \cdot \frac{{\partial \xi_{j} }}{{\partial w_{jk}^{i} }} = w_{j} \dot{\xi }_{j} \left( {\sum\limits_{i = 1}^{n} {\sum\limits_{k = 1}^{K_i} {w_{jk}^{i} } F_{k}^{i} + b_{j} } } \right)F_{k}^{i}$$

Fourth, the learning law for $b_{j}$ is

$$w_{k}^{i} (k + 1) = w_{k}^{i} (k) - \eta \frac{\partial E(k)}{{\partial w_{k}^{i} }} = w_{k}^{i} (k) + \eta \left( {y(k) - \hat{y}(k)} \right)\frac{{\partial \hat{y}(k)}}{{\partial w_{k}^{i} }}$$

(11)

where

$$\, \frac{{\partial \hat{y}(k)}}{{\partial w_{k}^{i} }} = F_{k}^{i} {\text{ and }}\frac{\partial E(k)}{{\partial w_{k}^{i} }} = \frac{\partial E}{{\partial \hat{y}}} \cdot \frac{{\partial \hat{y}}}{{\partial w_{k}^{i} }} = (\hat{y} - y)F_{k}^{i}$$

Finally, the learning law for $w_{j}$ is

$$w_{j} (k + 1) = w_{j} (k) - \eta \frac{\partial E(k)}{{\partial w_{j} }} = w_{j} (k) + \eta \left( {y(k) - \hat{y}(k)} \right)\frac{{\partial \hat{y}(k)}}{{\partial w_{j} }}$$

(12)

where

$$\frac{{\partial \hat{y}(k)}}{{\partial w_{j} }} = \xi_{j} \left( {\sum\limits_{i = 1}^{n} {\sum\limits_{k = 1}^{K_i} {w_{jk}^{i} } F_{k}^{i} + b_{j} } } \right).$$

Remark 1

Let $\xi_{j} (x) = \tan \;h\;(x) = \frac{{e^{x} - e^{ - x} }}{{e^{x} + e^{ - x} }}$, which gives $\dot{\xi }_{j} (x) = 1 - \xi_{j}^{2} (x).$

Remark 2

Let $\phi_{k}^{i} (x) = x$, which obtains $\dot{\phi }_{k}^{i} (x) = 1.$

In what follows, the Lyapunov function is proposed to investigate the convergent condition of the BLS identifier. Let $\eta (k)$ be the learning rate at discrete time k for the BLS parameters. The BLS identifier is asymptotically convergent provided that $\eta$ satisfies the following condition:

$$0 < \eta < \frac{2}{{\mathop {{\text{max}}}\limits_k \left\| {\left( {\frac{{\partial \hat y(k)}}{{\partial P(k)}}} \right)} \right\|_2^2}}$$

(13)

where $\, P(k) = \left[ {\begin{array}{*{20}l} {w_{fkl}^{i} } & {b_{fk}^{i} } & {w_{k}^{i} } & {w_{j} } & {w_{jk}^{i} } & {b_{j} } \\ \end{array} } \right]^{\text{T}}$, and $\left\| {\left( {\frac{{\partial \hat y(k)}}{{\partial P(k)}}} \right)} \right\|_2^2$ is given as below.

$$\begin{aligned} \left\| {\left( {\frac{{\partial \hat{y}(k)}}{{\partial P(k)}}} \right)} \right\|_{2}^{2} & = \sum\limits_{{i = 1}}^{n} {\sum\limits_{{k = 1}}^{{K_{i} }} {\left( {\left( {\frac{{\partial \hat{y}(k)}}{{\partial w_{{fkl}}^{i} }}} \right)^{2} + \left( {\frac{{\partial \hat{y}(k)}}{{\partial b_{{fk}}^{i} }}} \right)^{2} } \right)} } \\ & \quad + \sum\limits_{{j = 1}}^{m} {\left( {\left( {\frac{{\partial \hat{y}(k)}}{{\partial b_{j} }}} \right)^{2} + \left( {\frac{{\partial \hat{y}(k)}}{{\partial w_{j} }}} \right)^{2} } \right)} + \sum\limits_{{i = 1}}^{n} {\sum\limits_{{k = 1}}^{{K_{i} }} {\left( {\left( {\frac{{\partial \hat{y}(k)}}{{\partial w_{k}^{i} }}} \right)^{2} + \left( {\frac{{\partial \hat{y}(k)}}{{\partial w_{{jk}}^{i} }}} \right)^{2} } \right)} } \\ \end{aligned}$$

(14)

Using the previous BLS learning algorithms, one obtains

$$\begin{aligned} \, \Delta P(k) & = P(k + 1) - P(k) \hfill \\ &= \eta (y(k) - \hat{y}(k))\left[ {\begin{array}{*{20}l} {\frac{{\partial \hat{y}(k)}}{{\partial w_{fkl}^{i} (k)}}} & {\frac{{\partial \hat{y}(k)}}{{\partial b_{fk}^{i} (k)}}} & {\frac{{\partial \hat{y}(k)}}{{\partial w_{k}^{i} (k)}}} & {\frac{{\partial \hat{y}(k)}}{{\partial w_{j} (k)}}} & {\frac{{\partial \hat{y}(k)}}{{\partial w_{jk}^{i} (k)}}} & {\frac{{\partial \hat{y}(k)}}{{\partial b_{j} (k)}}} \\ \end{array} } \right]^{\text{T}} \\ &= \eta e(k)\frac{{\partial \hat{y}(k)}}{\partial P(k)} \\ \end{aligned}.$$

(15)

Proof

To prove the asymptotical convergence of the proposed BLS identifier, we choose the following Lyapunov function.

$$L(k) = \left( {y(k) - \hat{y}(k)} \right)^{2} = e^{2} (k)$$

(16)

Then the time difference of the Lyapunov function is given by

$$\Delta {L}_{M} (k) = {L}_{M} (k + 1) - {L}_{M} (k) = \Delta e(k) \cdot \left[ {2e(k) + \Delta e(k)} \right]$$

(17)

where

$$\begin{aligned} \Delta e(k) & = \left( {\frac{\partial e(k)}{\partial P(k)}} \right)^{\text{T}} \Delta P(k) = \eta \left( {\frac{{\partial (y(k) - \hat{y}(k))}}{\partial P(k)}} \right)^{\text{T}} \left( {y(k) - \hat{y}(k)} \right)\frac{{\partial \hat{y}(k)}}{\partial P(k)} \\ & = - \eta \frac{{\partial \hat{y}(k)}}{\partial P(k)}^{\text{T}} e(k)\frac{{\partial \hat{y}(k)}}{\partial P(k)} = - \eta e(k)\frac{{\partial \hat{y}(k)}}{\partial P(k)}^{\text{T}} \frac{{\partial \hat{y}(k)}}{\partial P(k)} \\ & = - \eta e(k)\left\| {\frac{{\partial \hat{y}(k)}}{\partial P(k)}} \right\|_{2}^{2} \\ \end{aligned}$$

(18)

Thus,

$$\begin{aligned} \Delta {L}_{M} (k) &= - \eta e(k)\left\| {\frac{{\partial \hat{y}(k)}}{\partial P(k)}} \right\|_{2}^{2} \cdot \left[ {2e(k) - \eta e(k)\left\| {\frac{{\partial \hat{y}(k)}}{\partial P(k)}} \right\|_{2}^{2} } \right] \\ &= - \eta e^{2} (k)\left\| {\frac{{\partial \hat{y}(k)}}{\partial P(k)}} \right\|_{2}^{2} \cdot \left[ {2 - \eta \left\| {\frac{{\partial \hat{y}(k)}}{\partial P(k)}} \right\|_{2}^{2} } \right] \\ \end{aligned}$$

(19)

Obviously, $\Delta {L}_{M} (k) < 0$ is negative-definite if and only if the sufficient condition (13) is satisfied. This completes the proof.

Moreover, the best learning rate can be found by considering at the time when $\mathop {\hbox{max} }\limits_{k} \left\| {\frac{{\partial \hat{y}(k)}}{\partial P(k)}} \right\|_{2}^{2}$ occurs. Therefore, it follows that

$$\Delta {{L}_M}(k) = - {e^2}(k)\mathop {{\text{max}}}\limits_k \left\| {\frac{{\partial \hat y(k)}}{{\partial P(k)}}} \right\|_2^2 \cdot \left[ {2\eta - {\eta ^2}\mathop {{\text{max}}}\limits_k \left\| {\frac{{\partial \hat y(k)}}{{\partial P(k)}}} \right\|_2^2} \right]$$

(20)

To find the best convergent rate, one finds

$$\frac{{\partial \Delta {{L}_M}(k)}}{{\partial \eta }}{|_{\eta = {\eta ^*}}} = - {\text{ }}{e^2}(k)\mathop {{\text{max}}}\limits_k \left\| {\frac{{\partial \hat y(k)}}{{\partial P(k)}}} \right\|_2^2 \cdot \left[ {2 - 2{\eta ^*}\mathop {{\text{max}}}\limits_k \left\| {\frac{{\partial \hat y(k)}}{{\partial P(k)}}} \right\|_2^2} \right] = 0$$

(21)

The best convergent rate can be obtained from (21).

$$\eta^{*} = \frac{1}{{\mathop {\hbox{max} }\limits_{k} \left\| {\frac{{\partial \hat{y}(k)}}{\partial P(k)}} \right\|_{2}^{2} }}.$$

(22)

4 Fuzzy Distributed and Decentralized EIF Algorithm for HOMR Multi-robots

This section will develop the novel algorithm of the fuzzy distributed and decentralized extended information filtering (FDDEIF) method for HOMRs whose communication topology is not fully connected; this algorithm is different from the previous algorithm introduced in [2] for mobile multi-robot systems with a fully connected communication topology. In doing so, this section starts with the system model description of each mobile robot and graph-based model of the multi-robot system, and derive the FDDEIF algorithm.

4.1 Distributed and Decentralized Extended Information Filtering Algorithm Using a Modified Graph Theory

To cope with the cooperative localization problem of the nonlinear estimation problem from the multi-HOMR system whose communication topology is not fully connected, this subsection introduces the DDEIF algorithm with modified graph theory. Like the DEIF algorithm, the DDEIF linearizes the system state and measurement models in (4), (6) using Taylor’s series expansion. In principle, the DDEIF filter takes the advantages of low computational load and ease of initialization procedure. The modified graph-based DDEIF algorithm is described as below. Suppose that interconnection topology of n HOMRs is a directed graph G, and n HOMRs can be regarded as n nodes. The relevant modified adjacency matrix is denoted as A = [a_ij], a_ij ≥ 0, ∀i, j ∈ {1,2 ,…, n}. Moreover we assume a_ii = 1, namely that the ith HOMR receives the localization information from its own sensors.

The modified graph-based DDEIF algorithm of the ith mobile robot is summarized as below:

• (i) One-step-ahead prediction:

  $$\hat{y}_{i} (k|k - 1) = Y_{i} (k|k - 1)f_{i} \left( {\hat{X}_{i} (k - 1|k - 1),u_{i} (k - 1),(k - 1)} \right)$$

  (23)
  $$Y_{i} (k|k - 1) = \left[ {\nabla f_{{X_{i} }} (k - 1)Y_{i}^{ - 1} (k - 1|k - 1)\nabla f_{{X_{i} }}^{T} (k - 1) + Q_{i} (k - 1)} \right]^{ - 1}$$

  (24)

  where $Y_{i} (k|k - 1)$ and $\, \hat{y}_{i} (k|k - 1)$ are, respectively, the information matrix and information state vector of the ith mobile robot.

• (ii) Estimation (measurement update):

  At time k, the ith HOMR not only has its own measurements, but also receives the measurements of other HOMRs via the communication architecture. Hence, the measurement updates of the ith mobile robot are given by

  $$\hat{y}_{i} (k|k) = \hat{y}_{i} (k|k - 1) + \sum\limits_{j = 1}^{N} {a_{ij} {\mathbf{i}}_{j} (k)}$$

  (25)
  $$Y_{i} (k|k) = Y_{i} (k|k - 1) + \sum\limits_{j = 1}^{N} {a_{ij} {\mathbf{I}}_{j} (k)}$$

  (26)

  where I_j(k) is the measurement covariance and i_j(k) is the measurement vector.

  $${\mathbf{i}}_{j} (k) = \nabla h_{{X_{j} }}^{T} (k)R_{j}^{ - 1} (k) \left[r_{j} (k) + \nabla h_{{X_{j} }} (k)\hat{X}(k|k - 1) \right]$$

  (27)
  $${\mathbf{I}}_{j} (k) = \nabla h_{{X_{j} }}^{T} (k)R_{j}^{ - 1} (k)\nabla h_{{X_{j} }} (k)$$

  (28)

  Notice that ∇f_xj(k − 1) is the Jacobian matrix of f_j evaluated at $X_{j} = \hat{X}_{j} (k|k - 1)$, ∇h_xj(k) is the Jacobian matrix of the output h_j evaluated at X_j, and the innovation r_j(k) is given by

  $$r_{j} (k) = Z_{j} (k) - h_{j} \left(\hat{X}_{j} (k|k - 1) \right)$$

  (29)

  The $\nabla f_{{X_{j} }} (k - 1)$ and $\nabla h_{{X_{j} }} (k)$ denote by

  $$\nabla f_{{X_{j} }} (k - 1) = \left. {\frac{{\partial f_{j} }}{\partial X}} \right|_{{X = \hat{X}_{j} (k - 1|k - 1)}}$$

  (30)
  $$\nabla h_{{X_{j} }} (k) = \left. {\frac{{\partial h_{j} }}{\partial X}} \right|_{{X = \hat{X}_{j} (k|k - 1)}}.$$

  (31)

Remark 1

If the communication topology is fully connected, then all the elements of a_ij are unity. For this case, modified graph-based DDEIF algorithm is reduced to the DDEIF algorithm.

4.2 Fuzzy DDEIF (FDDEIF)

The essential part of the FDDEIF is that this filter consists of N fuzzy tuners to automatically adjust its exponential weights for the measurement noise covariance matrices. Given the state Eq. (5) and measurement Eq. (6), we assume that the measurement noise covariance matrices are described by

$$R_{j} (k) = R_{j} \cdot \gamma_{j}^{ - \kappa (k + 1)}, \quad j = 1, \ldots ,N$$

(32)

where γ_j and κ are, respectively, the weighting factor and the variation of exponential function of the ith measurement model. Note that κ is set by 2 in the paper, and γ_j is near one, and the constant matrices R_j are constant values of right dimensions. Given an initial state estimate $\;\hat{X}_{i} (0/0)$ and an initial information matrix Y_i(0/0), the proposed FDDEIF whose prediction and estimation equations are modified from the exponential weighted EIF augmented with fuzzy logics in [14], which can be easily described as follows:

• (i) One-step prediction:

  $$\hat{y}_{i} (k|k - 1) = Y_{i} (k|k - 1)\; \times f_{i} \left( {\hat{X}_{i} (k - 1|k - 1),U_{i} (k - 1),(k - 1)} \right)$$

  (33)
  $$Y_{i} (k|k - 1) = \left[ \begin{aligned} & \gamma_{i}^{\kappa } \nabla f_{{X_{i} }} (k - 1)Y_{i}^{ - 1} (k - 1|k - 1)\; \times \nabla f_{{X_{i} }}^{T} (k - 1) \\ & + Q_{i} (k - 1) \\ \end{aligned} \right]^{ - 1}$$

  (34)

• (ii) Estimation (measurement update):

  $$\begin{aligned} \hat{y}_{i} (k|k) &= \hat{y}_{i} (k|k - 1) \\ & \quad + \sum\limits_{j = 1}^{N} {a_{ij} \nabla h_{{X_{j} }}^{T} \left( k \right)\left( {\frac{{R_{j} }}{{\gamma_{j}^{\kappa } }}} \right)^{ - 1} \times \left[ \begin{aligned} & r_{j} \left( k \right) \\ & + \nabla h_{{X_{j} }} \left( k \right)\hat{X}\left( {k|k + 1} \right) \\ \end{aligned} \right]} \\ \end{aligned}$$

  (35)
  $$Y_{i} (k|k) = Y_{i} (k|k - 1) + \sum\limits_{j = 1}^{N} {a_{ij} \nabla h_{{X_{j} }}^{T} \left( k \right)\left( {{\raise0.7ex\hbox{${R_{j} }$} \!\mathord{\left/ {\vphantom {{R_{j} } {\gamma_{j}^{\kappa } }}}\right.\kern-0pt} \!\lower0.7ex\hbox{${\gamma_{j}^{\kappa } }$}}} \right)^{ - 1} \times \nabla h_{{X_{j} }} \left( k \right)}$$

  (36)

Note that if $\gamma_{j} = 1,j = 1, \ldots ,N$, and then this FDDEIF becomes a regular DDEIF.

4.3 Fuzzy Tuner

Each fuzzy tuner for $\gamma_{j}$ is employed to monitor the innovation to avoid divergence of the FDDEIF and to tune the value of each $\gamma_{j}$ in (34)–(36). There are three inputs and one output for the $j{\text{th}}$ fuzzy tuner. The mean value, the covariance, and covariance slew rate of the innovation of the $j{\text{th}}$ fuzzy tuner are considered as the inputs to monitor the degree of filter divergence. Similar to the fuzzy tuner in [14], the three inputs of the fuzzy tuner are given as follows:

• 1. Statistical mean of the innovation:

  $$\bar{r}_{i} \cong \sum\limits_{k = 1}^{l} {r_{i} (k)} /l$$

  (37)

• 2. Statistical second-order moment of the innovation

  $$\sigma_{{r_{i} }}^{2} \cong \sum\limits_{k = 1}^{l} {r_{i}^{2} (k)} /l$$

  (38)

• 3. The variation of the innovation second-order moment

  $$\text{Slew rate} = \sigma_{{r_{i} }}^{2} (k) - \sigma_{{r_{i} }}^{2} (k - k_{1} ).$$

  (39)

  where the parameter k₁ is positive and can be chosen by designers. Throughout the paper, we set k₁ = 50 and l = 500.

The jth fuzzy tuner uses trapezoidal membership functions for the three input variables and the output variable, and also employs five and three linguistic sets, respectively, for the statistical mean and variance of each innovation process, but adopts three linguistic sets for the output. The slew rate of the innovation variance is divided into three cases: positive, zero, and negative. Hence, 45 fuzzy rules in [14] are used for each sensor model.

5 Cooperative Pose Initialization

5.1 Landmark-Based Pose Initialization

This subsection presents a map-based initialization approach for global pose localization of one HOMR, i.e., the ith HOMR in the team using the measurements from the RGB-D sensor and laser scanner. For pose initialization, all the HOMRs are initially assumed to get lost in their working space, namely that all their postures of these HOMRs are unknown from the outset. To start up with the initialization procedure, all the HOMRs use their laser scanner and RGB-D sensor to accomplish collision and obstacle avoidance, and randomly navigate around the working space until any one feature or artificial landmark embedded in the environment is found by any one of the HOMRs, i.e., the ith HOMR. Note that the position of the feature or artificial landmark is known at a prespecified location. Once the feature or artificial landmark has been detected and then recognized by the ith HOMR, the position measurement, $(x_{mi} ,y_{mi} ,z_{mi} )$, of the feature or landmark with respect to the RGB-D sensor mounted on the ith HOMR are expressed by

$$x_{mi} = x_{L} - x_{i} + v_{1i} , \quad y_{mi} = y_{L} - y_{i} + v_{2i} , \quad z_{mi} = z_{L} - z_{i} + v_{3i}$$

(40)

where (x_L, y_L, z_L)^T denotes the true position of the landmark; (x_i, y_i, z_i)^T represents the current position of the ith HOMR at the sampling instant k; v_1i, v_2i, and v_3i are three independent, Gaussian measurement noise processes with zero means . In order to solve the unknown position (k − 1|k − 1) and orientation of the HOMR, it is easy to measure the relative positions of the feature or landmark using the RGB-D sensor and obtain three sets of n measured data for each feature or landmark, i.e., x_mi(k), y_mi(k), z_mi(k), k = 1 ,…, l. Thus, the current position of the HOMR can be estimated by the following simple averaging method.

$$\begin{aligned} x_{i} & = x_{L} - \frac{1}{l}\sum\limits_{k = 1}^{n} {x_{mi} \left( k \right)}, \quad y_{i} = y_{L} - \frac{1}{l}\sum\limits_{k = 1}^{n} {y_{mi} \left( k \right)}, \\ z_{i} & = z_{L} - \frac{1}{l}\sum\limits_{k = 1}^{n} {z_{mi} \left( k \right)} \\ \end{aligned}.$$

(41)

5.2 Cooperative Pose Initialization

This subsection is aimed at finding the initial poses of other n − 1 HOMRs if the ith HOMR is self-localized by recognizing and measuring a RGB-D-based feature, landmark, and its surrounding environment model. In the cooperative localization scenario, if the jth HOMR is detected by the laser scanner in the ith HOMR, then it is easy to find that the distance between both HOMRs is d_ij, and the angle between the line-of-sight and the heading of the ith HOMR is ϕ_ij (positive in the counterclockwise (CCW) direction and negative in the clockwise (CW) direction. Therefore, then the position of the jth HOMR is calculated by the following equations:

$$x_{j} = x_{i} + d_{ij} \cos (\phi_{ij} + \theta_{i} ),y_{j} = y_{i} + d_{ij} \sin (\phi_{ij} + \theta_{i} )$$

(42)

Once the position of the jth HOMR has been determined, then the heading of the jth HOMR can be found by using the following cases:

Case 1

If the jth HOMR can observe and identify the L neighboring walls using its own laser scanner and map, then orientation of the jth HOMR is averagely computed by

$$\theta_{j} = \left( {\sum\limits_{k = 1}^{L} {\beta_{k}^{m} - \beta_{k} /L} } \right)$$

(43)

Case 2

If a RGB-D-based landmark can be found by the jth HOMR, then its orientation is easily calculated by

$$\theta_{j} = {\text{tan2}}^{ - 1} \frac{{y_{L} - y_{j} }}{{x_{L} - x_{j} }} - \alpha_{j}$$

(44)

where $\alpha_{j} = \tan 2^{ - 1} (y_{j} /x_{j} )$ is the azimuth angle between the detected landmark and RGB-D sensor.

Remark 2

Assume that the communication network is connected. The proposed cooperative initialization method based on the two cases is shown effective in finding all the initial pose estimations of all the robots.

6 BLS-FDDEIF for Dynamic Localization

This section is devoted to describing the main ideas of the proposed cooperative BLS-FDDEIF localization method for the moving multi-HOMR system. By considering the directed communication network among these multi-robots, this section will apply the BLS-FDDEIF algorithm to the multi-HOMR system including three kinds of omnidirectional wheeled mobile robots. By including the previous cooperative pose initialization algorithm, this section will develop a discrete-time BLS-FDDEIF dynamic pose estimation method to obtain the best pose estimate of each HOMR when the multiple HOMRs navigate around their indoor working environments and some of embedded landmarks can be observed by HOMRs. Figure 3 depicts the block diagram of the BLS-FDDEIF algorithm for each HOMR. The discrete-time BLS-FDDEIF dynamic pose estimation is described in the following six steps.

Fig. 3

Block diagram of the BLS-FDDEIF algorithm for each HOMR

• Step 1: Measure the relative position of one landmark using the RGB-D sensor and obtain one set of n measured data for each landmark, i.e., $x_{mi} (l),y_{mi} (l),z_{mi} (l),$$l = 1, \ldots ,n$.

• Step 2: Use (41) and cooperative pose initialization method to solve for the start-up position $\left( {x_{i} (k),y_{i} (k),z_{i} (k)} \right)^{\text{T}}$ and initial orientation $\theta_{i} (k)$ of the ith HOMR.

• Step 3: Select the initial values of each information state $\hat{y}_{i} (0|0)$ and information matrix $Y_{i} (0|0)$ at time k$k = 0$ for each HOMR.

• Step 4: Do the one-step prediction by setting each optimal information state estimate and the information matrix at time $k - 1$ to be $\hat{y}_{i} (k - 1|k - 1)$ and $Y_{i} (k - 1|k - 1)$ using (32, 33) and use each BLS to learn each ${\mathbf{g}}_{i}$ in real time.

• Step 5: Obtain the updating estimate $\hat{y}_{i} (k|k)$ and the information matrix $Y_{i} (k|k)$ at time $k$ using (35, 36) where the fuzzy tuner is used and each HOMR reads the N-pairs measurement data $Z_{j} (k)$, $j = 1,\ldots,N$.

• Step 6: Repeat Steps 4 and 5.

7 Simulations and Discussion

In this section, three simulations are performed to examine the performance and merits of the proposed cooperative pose initialization and BLS-FDDEIF dynamic pose tracking method for the multi-HOMR system in [13]. Each HOMR is equipped with encoders amounted on the driving wheels, one laser scanner and one RGB-D sensor. The first simulation verifies the proposed cooperative pose initialization algorithm, and the second simulation aims to accomplish the BLS-FDDEIF algorithm for dynamic pose tracking. The third simulation is employed to compare the superiority of the BLS-FDDEIF by comparing to BLS-DDEIF. Three simulations are done by using MATLAB/Simulink under the case in which the three HOMRs navigate in a flat terrain in structured environments where all the landmarks are well recognized. In three simulations, both two landmarks in the surrounding space are, respectively, installed at (x_L1, y_L1, z_L1) = (5 m, 2 m, 2 m) and (x_L2, y_L2, z_L2) = (5 m, 3 m, 2 m); the parameters of the walls are given by (ρ ^m₁  = 0, β ^m₁  = 90°), (ρ ^m₂  = 5, β ^m₂  = 0°), (ρ ^m₃  = 5, β ^m₃  = 90°).

In the first simulation, the true poses of the three HOMRs are, respectively, set (2 m, 1.5 m, 1 m, 0°), (3 m, 2.5 m, 1 m, 0°), (3.5 m, 3.5 m, 1 m, 0°). Figure 4 depicts the simulation results where the black circles are the true positions and the green circles denote the estimated position. The results in Fig. 4a reveal that the initial posture estimates are very close to their true values. In the first simulations, we assume that the first HOMR can recognize two landmarks, the three walls, and use FDDEIF to fuse these data to obtain the estimated pose. The second HOMR cannot detect one of the landmarks, so the first HOMR can transmit that information for the second HOMRs. The third HOMR is localized by the second HOMR using the laser scanner to measure the distance and angle between the two HOMRs. Figure 4b shows that when high noise occurs during the measurements of each HOMR, BLS-FDDEIF can avoid the divergence of pose estimation of each HOMRs. Table 1 displays the root mean-square errors (RMSEs) of the initial posture estimates of the three HOMRs for the high noise case, where the performance index, RMSE, is defined by ${\text{RMSE = }}\sqrt {{{\sum\nolimits_{i}^{n} { (x_{ei}^{2} + y_{ei}^{2} + \theta_{ei}^{2} )} } \mathord{\left/ {\vphantom {{\sum\limits_{i}^{n} { (x_{ei}^{2} + y_{ei}^{2} + \theta_{ei}^{2} )} } n}} \right. \kern-0pt} n}}$.

Fig. 4

Environment settings and results of the first simulation for high noise case: a little measurement noise; b big measurement noise

Table 1 List of the root mean-square errors (RMSEs) of the initial posture estimates of the three HOMRs for high noise case

The second simulation aims to investigate the effectiveness of the proposed BLS-FDDEIF method for dynamic pose tracking of the three HOMRs where they are moving 1 m along the x-axis and the simulation environment is identical to Simulation 1, as shown in Fig. 4a. The red HOMR uses the car-like omnidirectional dynamic model, the blue HOMR adopts the three-wheeled omnidirectional dynamic model, and the pink HOMR exploits the four-wheeled omnidirectional dynamic model. All the three HOMRs are moving 1 m along the x-axis. Figure 5 depicts the simulation results of the three HOMRS when the measurement data are corrupted with little noise, thereby showing the effectiveness of the proposed BLS-FDDEIF localization method. Table 2 compares the RMSEs of estimated poses of the two algorithms, BLS-FDDEIF and FDDEIF, for dynamic pose tracking in the second simulation.

Fig. 5

Dynamic pose tracking simulation of the cooperative BLS-FDDEIF localization algorithm

Table 2 List of the RMSEs of pose tracking in second simulation

The results in Table 2 indicate that the BLS-FDDEIF is superior to the FDDEIF in terms of RMSEs. Table 3 compares the RMSEs of the estimated velocities from the two algorithms, BLS-FDDEIF and FDDEIF, in the second simulation, Table 3 reveals that the BLS-FDDEIF is still superior to FDDEIF in terms of RMSEs of estimated velocities of the three HOMRs.

Table 3 List of the RMSEs of the estimated velocities of the three HOMRS in the second simulation

The third simulation is used to compare the BLS-DDEIF and BLS-FDDEIF methods in estimated position accuracy of the three HOMRs where they are moving 0.5 m along the x-axis and the big measurement noise as shown in Fig. 6. The same color lines display the estimated positions using the BLS-DDEIF algorithm, while the green lines illustrate the estimated positions using BLS-FDDEIF. Tables 4 and 5 present the comparative RMSEs of the pose tracking and velocity tracking errors using the two algorithms in the third simulation for the high noise case. Obviously, the results reveal that BLS-FDDEIF significantly outperforms BLS-DDEIF in terms of improvement percentages.

Fig. 6

Comparative time evolutions of the true poses and BLS-DDEIF and BLS-FDDEIF estimated positions of the three HOMRs for high noise case

Table 4 List the RMSEs of the pose tracking errors of the three HOMRs in the third simulation for high noise case

Table 5 Comparison of the RMSEs of estimating velocities the three HOMRs using the two algorithms, BLS-FDDEIF and BLS-DDEIF in the third simulation

8 Experimental Results and Discussion

This section conducts two experiments to verify the real performance and applicability of the proposed cooperative BLS-FDDEIF localization algorithm. Figure 7 illustrates the three experimental HOMRs and the experimental environment with one landmark was installed at (x_L1, y_L1, z_L1) = (2 m, 1.17 m, 0.25 m), and three walls were given by (ρ ^m₁  = 0, β ^m₁  = 90°), (ρ ^m₂  = 5, β ^m₂  = 0°), (ρ ^m₃  = 5, β ^m₃  = 90°), respectively. In the beginning, the first robot was stopped at (1 m, 1.17 m), the second robot was initialized at (0.5 m, 1.67 m), and the third robot got started at (0.5 m, 0.67 m). Note that all the three robots did not have their initial true poses with respect to their surroundings. Therefore, the Realsense RGB-D sensor was used to detect the surrounding landmark, and the laser scanner was exploited to measure the perpendicular distances and orientations of the walls.

Fig. 7

Three experimental HOMRs and experimental environment

For landmark detection, one used color feature extractions to locate its center point. Figure 8 illustrates the color and depth image from the RealSense RGB-D sensor, and Fig. 9 displays the extracted blue blocks by removing other colors by using the HSV color space. Moreover, Fig. 10 indicates the contour of the chosen biggest blue block.

Fig. 8

The color and depth image from Realsense RGB-D sensor

Fig. 9

Extracted blue blocks from the Realsense RGB-D sensor

Fig. 10

Illustration of the largest selected blue block from the Realsense

In the first experiment when all the HOMRS were stopped at their initial points, each HOMR used its onboard RGB-D camera to detect the relative position between the landmark and itself, and, meanwhile, utilized its onboard laser scanner to measure the perpendicular distance ρ_j and orientation β_j of each wall in the robot frame. By taking the BLS-FDDEIF algorithm to fuse the readings from these RGB-D sensors and laser scanners, the estimated positions of the three HOMRS were, respectively, found by (1.005 m, 1.1734 m), (0.4964 m, 1.6663 m), and (0.4978 m, 0.6787 m). Moreover, the statistically estimated errors of the three corresponding HOMRs were given by (0.0005 m, 0.0034 m), (0.0036 m, 0.0037 m), and (0.0022 m, 0.0087 m), respectively. These results indicate that the proposed BLS-FDDEIF algorithm worked well for this scenario.

The second experiment is carried out to explore the real performance of the proposed cooperative BLS-FDDEIF dynamic localization method for the three HOMRs. The experimental set-up was identical to the first experiment but all the three HOMRs moved along a slant line. The BLS-FDDEIF algorithm was coded by standard C programming language. Figure 11 shows the experimental pictures during the second experimental process in which all the three HOMRs moved 0.4 m along the x-axis defined in the world frame. Figure 12 demonstrates the experimental estimates of dynamic poses of the three moving HOMRs. Moreover, Table 6 displays their RMSEs, respectively. Through the experiment results in Table 6, the proposed BLS-FDDEIF algorithm has been shown applicable and practicable in achieving cooperative localization of the multi-HOMR system.

Fig. 11

Experimental pictures of the cooperative dynamic localization of the three moving HOMRs

Fig. 12

Experimental results of the cooperative dynamic pose estimates of the three moving HOMRs

Table 6 List of the RMSEs of the dynamic pose estimates of the three moving HOMRs

9 Conclusions and Future work

This paper has presented a novel FDDEIF method augmented by broad learning system (BLS), called BLS-FDDEIF, for cooperative pose initialization and dynamic pose tracking of a team of heterogeneous omnidirectional mobile robots (HOMRs). Each BLS with its iterative gradient decent algorithm has been proposed to approximate the dynamic effects and uncertainty vectors in the dynamic model of each HOMR in real time. By fusing measurement information taken from the encssoders, RGB-D sensor, and laser scanner, the proposed static global pose initialization and BLS-FDDEIF dynamic pose estimation methods have been shown capable of obtaining accurate pose and pose rate estimates of the HOMRs. Comparative simulation results have demonstrated the effectiveness and superiority of the proposed BLS-FDDEIF method for a group of HOMRs. The practicability of the proposed BLS-FDDEIF method has been well demonstrated by conducting static and dynamic experiments using the three cooperating HOMRs. An interesting topic for future research would be to develop a fuzzy distributed decentralized extended Kalman filtering (FDDEKF) localization using BLS, abbreviated as BLS-FDDEKF, for cooperative pose initialization and dynamic pose tracking of the multi-HOMR system owing to direct estimation of the physical state variables instead of information state variables.