An A²CL Algorithm based on Information Optimization Strategy for MMRS

Abstract

Multiple Mobile Robots System (MMRS) has shown many attractive features in lots of real-world applications that motivate their rapid and wide diffusion. In MMRS, the Cooperative Localization (CL) is the basis and premise of its high-performance task. However, the statistical characteristics of the system noise should be already known in traditional CL algorithms, which is difficult to satisfy in actual MMRS because of the numerous of disturbances form the complex external environment. So the CL accuracy will be reduced. To solve this problem, an improved Adaptive Active Cooperative Localization (A²CL) algorithm based on information optimization strategy for MMRS is proposed in this manuscript. In this manuscript, an adaptive information fusion algorithm based on the variance component estimation under Extended Kalman filter (VCEKF) method for MMRS is introduced firstly to enhance the robustness and accuracy of information fusion by estimating the covariance matrix of the system noise or observation noise in real time. Besides, to decrease the effect of observation uncertainty on CL accuracy further, an observation optimization strategy based on information theory, the Model Predictive Control (MPC) strategy, is used here to maximize the information amount from observations. And semi-physical simulation experiments were carried out to verity the A²CL algorithm's performance finally. Results proved that the presented A²CL algorithm based on information optimization strategy for MMRS cannot only enhance the CL accuracy effectively but also have good robustness.

1. Introduction

With the improvement of the mobile robot's ability, its application areas and scopes is expanding continuously. Compared with the single mobile robot system (SMRS), multiple mobile robot system (MMRS) has more advantages, including parallelism, efficiency, flexibility and robustness, especially when facing complex tasks [1-2]. Thus, MMRS is becoming the research focus and trend of robotics [3-5].

In MMRS, the Cooperative Localization (CL) is the basis and premise of its high-performance task. And the essential of CL algorithm is multi-sensor information fusion. Since that nonlinearity existes in each real systems, so the nonlinear information fusion algorithm should be considered. The Extended Kalman Filter (EKF) transforms the nonlinear problem into linearity by Taylor series expansion of nonlinear functions and ignoring higher-order terms [6-9]. Therefore, the nonlinear CL algorithm based on EKF, named EKF-based CL algorithm for short, is widely used due to its simplicity and convenience in practical applications [9]. In EKF-based CL algorithm, the statistical characteristics of the MMRS, including the process and the observation information, should be known precisely in advance. However, this is difficult to satisfy in practical MMRS. That’s because of that there are numerous of disturbances and interference form the complex external environment, so not only the statistical characteristics can’t be known in advance but also the observation information has uncertainty. Thus, CL accuracy will be reduced, affected the performance of the whole MMRS.

Aimed at the unknown statistical characteristic problem, adaptive filters are taken into account. Although there are many kinds of adaptive filters, limitations are existed [10-14]. For example, Sage-Husa adaptive filter cannot work when the process noise and the observation noise are both unknown simultaneously. That means, it can only estimate one kind of covirance matrix when the other one is known in advance [11]. The adaptive filter with fading factor has only one optimal fading factor in each iteration, and it cannot guarantee the accuracy and reliability for a complicated system model [12]. The variance-covariance components estimation (VCE) is another common adaptive filter method based on the residual theory [13-14]. Nowadays, a great many of varietal VCE methods are used in geodesy and navigation fields, for example, the Helmert VCE method and the posteriori VCE method. In the posteriori VCE method, the adaptive iteration is ultilized to obtained the weights of different observations, implementing the system’s adaptivity. Nonetheless, in some cases, the weight matrix is non-positive definite, which leads to the decrease of the algorithm accuracy. Thus, an improved adaptive EKF based on the VCE method, named VCEKF method, was proposed for geodetic surveying and mapping, and its effectiveness was verified by using the actual experiments in [13].

So the observation uncertainty becomes a prominent problem. It is well known that the uncertainty of system states can be described by the informativeness inherently [15]. The more informativeness the states own, the smaller uncertainty of the state is [15-16]. And from the information theory, we know that both Shannon information (entropy) and Fisher information can be utilized to evaluate the informativeness contained in a probability distribution. What's more, the aim of the MMRS path planning is designing a reasonable path by using the informativeness, minimizing the CL error [17-19]. Thus, proper path planning strategy can effectively reduce the impact of uncertainty on the CL accurcy.

The common used path planning methods include one-step-look-ahead strategy and multi-step look-ahead strategy [19-21]. The basic idea of one-step-look-ahead strategy is that the optimized control vector is obtained by utilizing the prior information at the present time. The performance of one-step-look-ahead strategy is not as good as multi-step look-ahead strategy which has been proved in many papers [20]. And in multi-step look-ahead strategy, a series of optimized control vectors are obtained by utilizing the prior information at the present time. However, there are many conservative assumptions in multi-step look-ahead strategy, and the optimized result is uncorrect if the model is not percisely known in advance. Model Predictive Control (MPC), which can be used in the information collecting and path planning for MMRS, has the advantages including low computational cost, closed-loop optimization and robustness [21-23]. The traditional MPC-based path planning algorithm is under the EKF framework, which will introduce localization error into actual nonlinear systems. Thus, we will propose an improved motion control strategy based on MPC.

In this paper, we aim to design a novel cooperative localization algorithm for MMRS, in order to fulfill the requirements of high-precision, self-adaption and high reliability of MMRS. The main contributions of the paper are highlighted as follows:

(1) To improve CL accuracy of MMRS, considering the uncertainty of the statistical characteristics of measurement noise, an improved CL algorithm named VCEKF is proposed in this paper based on the theory of VCE.

(2) To enhance the autonomy of MMRS, considering the characteristic that different measurement vector contains different amount of information, a novel motion control theory based on MPC is proposed in this paper based on the theory of MPC to further enhance the precision of MMRS.

The rest of the paper is organized as followed. Section 2 presented the relative basic knowledge of the nonlinear MMRS and Section 3 proposed an improved A²CL algorithm based on MPC strategy and VCEKF method. In Section 4, semi-physical simulation experiments and corresponding results and analysis were presented. And Section 5 concluded this manuscript.

2. CL Fundamentals

2.1 Nonlinear model of MMRS

It is assumed that the MMRS contains n mobile robots and m landmarks. Taken the two-dimensional position \((x, y)\ \) and the heading θ as the state variable of a single robot, the state variable of the \(i\ \)-th mobile robot is:

\(f(n)= \begin{cases} x_i(k+1)=x_i(k)v_i(k)\cdot\cos\theta_i(k) \\ y_i(k+1)=y_i(k)v_i(k)\cdot\sin\theta_i(k)\\ \theta_i(k+1)=\theta_i(k)+\omega_i(k) \end{cases} \;\)       (1)

and the state of the \(n\; \)mobile robots system is

\(x=[x_1,x_2,\cdots,x_n]^T\; \)       (2)

where \(x_i=[x_i,y_i,\theta_i]^T \; (i=1,2,\cdots,n)\; \; \)denotes the pose of mobile robot \(i\).

In MMRS, the relative range and the bearing are regarded as the observation [3]. Generally, the observation vector can be divided into two types. One observation contains the relative range and bearing information between different mobile robots while the other observation contains the relative range and bearing information between the robot and landmarks.

a. For the first type observation, which contains the relative range and bearing information between different mobile robots, the observation vector is as follows:

\(z = [r_{ij}\;\;\;\phi_{ij}]^T\)       (3)

where Eq. 3 denotes the mobile robot \(i\) observes mobile robot \(j\ \), and the relationship between these two mobile robots’ position and the observation vector are described as follows:

\(\begin{cases} tan(\theta_i +\phi_{ij})=\frac{x_i-x_j}{y_i-y_j} \\ \ r_{ij}=\sqrt{(x_i-x_j)^2+(y_i-y_j)^2} \end{cases} \; \)        (4)

b. For the second type observation, which contains relative range and bearing information between the robot and landmarks, the observation vector is as follows:

\(z(k) = [r_{il}\; \; \; \phi_{il}]^T\)       (5)

where Eq. 5 denotes the mobile robot \(i\) observes landmark \(l\), and the relationship between the relative position and the observation vector is described as follows:

\(\begin{cases} tan(\theta_i +\phi_{il})=\frac{x_i-x_l}{y_i-y_l} \\ \ r_{il}=\sqrt{(x_i-x_l)^2+(y_i-y_l)^2} \end{cases} \; \)        (6)

2.2 Frame of MMRS CL algorithm

We know that the optimal estimation is divided into two parts: time update and observation update. The time update accomplishes the system state prediction while the observation update can achieve the error correction and state estimation by utilizing observations. And the frequency of the observation update depends on the ability to obtain observations. However in many cases, the frequency of the observation update is lower than the one of the time update since that the observations from images or other sources should be analyzed and processed further. In other words, the frequency of obtaining observation is out of sync with the one of the MMRS sampling. So in the MMRS CL algorithm, after time update, the observation update is executed only when the observation is obtained. Otherwise, the time update is executed only. In addition, taken the nonlinearity of the MMRS into account, the EKF is used. Therefore, the CL algorithm based on EKF for MMRS is shown as Algorithm 1.

Algorithm 1 CL algorithm based on EKF

In Algorithm 1, \(\hat{x}_i(k,k)\)  and ˆ \(\hat{P}_i(k,k)\) are optimal estimations of the state variate and covariance matrix while \(z_i(k)\) is the observation; F (⋅) and H( ⋅) indicate Jacobian matrices of nonlinear state transfer function and observation transfer function, respectively; \(K(k)\) denotes the Kalman gain; \(Q_i(k)\)and  \(R_i(k) \) are covariance matrices of the system process and the observation noises, respectively. In addition, the previous state variate \(\hat{x}_i(k,k-1)\; \) and covariance matrix \(\hat{P}_i(k-1,k-1)\; \)should be already known in this algorithm; y denotes the innovation matrix;I denotes the identity matrix.

The pseudo-code illustrates a MMRS CL algorithm based on EKF. In this algorithm, the observation update procedure accomplishes when the observation is available. Otherwise, the time update procedure is approximated as the observation update procedure. This CL algorithm is simple and effective, but it has two obvious flaws. Firstly, the observation cannot be evaluated in the observation update procedure, causing that the observation uncertainty is brought into the system, which degrades the estimation performance. secondly, it is assumed that the statistical charateristics of the process noise is known as priori knowledge and do not change over time. But actually this assumption is hard to valid in actual MMRS CL. If the inappropriate noise model is chosen, the performance of MMRS CL will also be degraded.

3. An improved A² CL algorithm based on information optimization strategy

As mentioned previously, the statistical characteristics of the process and observation noise have not been taken into account seriously in traditional CL algorithms which restricts the performance of the MMRS CL. Therefore, this paper will propose an improved active CL algorithm based on VCEKF method and MPC strategy. Firstly, in order to solve the effect of observation uncertainty on the performance of the MMRS CL, an active CL algorithm for MMRS oriented the minimization of the observation information uncertainty is proposed. Here the evaluation method of the observation information uncertainty is built up based on entropy. On this basis, the mathematical model between the control vector and the observation information uncertainty is established by utilizing the MPC strategy, so that an optimal path has been planned to minimize the observation uncertainty. In addition, in order to solve the effect of process uncertainty on the performance of the MMRS CL, an adaptive CL algorithm based on VCEKF is proposed.

3.1 Active CL algorithm for MMRS

A. Active CL model based on MPC strategy

Controlling vector which determined by motion control strategy is the key to achieve the goal of active CL algorithm. One-step look-ahead strategy and multi-step look-ahead strategy are both common motion control strategies [21]. One-step look-ahead strategy is quite simple and suitable for real-time system. However, the performance is not as good as multi-step look-ahead strategy which is proved by many researchers [24-28]. Although multi-step look-ahead strategy has better performance, it works under some conservative hypotheses the performance turns bad when the model is uncorrect. Besides, it is also supposed that the dimension of the observation vector is fixed which is not impeccable. As a result, some other alternating motion control strategies are concerned and in this section the MPC method is introduced [21].

The controlling vector sequence \(u_0,\; u_1, \; \cdots, \;u_{N-1}\; \) by utilizing the N -step look-ahead strategy at the initial time and \(u_0\;\) is used as the initial controlling vector. At time step 1, \(\hat{x}_{1\mid1}\) and \(\hat{P}_{1\mid1}\) is updated by utilizing \(z_1\) and the controlling vector sequence \(u_0,\; u_1, \; \cdots, \;u_{N-1}\; \) by utilizing the N −1-step look-ahead strategy and \(u_1\)is used as the controlling vector at time step 1. The controlling vector uN−1 is determined by utilizing and \(\hat{x}_{N-1\mid N-1}\) and \(\hat{P}_{N-1\mid N-1}\) based on the one-step look-ahead strategy at time step N −1. The pseudo code of motion control strategy based on MPC is described as Algorithm 2.

Algorithm 2 Motion control strategy based on MPC

Algorithm 2 illustrates the mathematical model between the control vector and the observation information uncertainty based on the entropy evaluation method and MPC, which is the fondermental of active CL algorithm. And in this algorithm, \(\max _{\boldsymbol{u}(k-1)} \log \operatorname{det}\left[\frac{\boldsymbol{P}^{-1}[k \mid \boldsymbol{u}(k-1)]}{\boldsymbol{P}^{-1}(k-1)}\right]\) is the evaluation method of the observation information uncertainty based on Shannon entropy.

B. Active CL algorithm based MPC and EKF

Combining the motion control strategy and CL algorithm, an active CL algorithm based on MPC strategy and EKF method is proposed here. And the corresponding pseudo code is described as Algorithm 3. Since the mutual information is maximized at each time step in the active CL algorithm, the active CL algorithm has better performance than that of the common CL algorithm theoretically from the view point of information theory.

Algorithm 3 Active CL algorithm based on MPC and EKF

In Algorithm 3, COOPERATIVE(⋅) indicates the CL algorithm based on EKF shown in Algorithm 1 while MPC(⋅) indicates the motion control strategy based on MPC shown in Algorithm 2.

3.2 Adaptive filter algorithm based on the VCEKF

The fundamental theory of the adaptive filter is that estimating the state variables and the noise statistical characteristics in real time at the same time, reducing the estimation errors and improving the filtering accuracy. In VCE method, decomposing system innovation vectors by using residual vectors, variance components of system process and the observation noises can be estimated [13-14, 29-30]. On this basis, the corresponding weighting matrices can be adjusted, achieving the adjustment of the state variable. Taken the nonlinearity of the actual MMRS into account, the VCEKF method is presented here.

Taken the standard nonlinear system model into account, the system model is [31-32]:

\(\begin{cases} x(k)=f(x(k-1))+w(k-1) \\ \ z(k)=h(x(k))+\vartriangle(k) \end{cases} \; \)         (7)

where \(x(k)\) and \(z(k) \) indicate the state vector and observation vector; \(f(\cdot)\) and \(h(\cdot)\) denote the nonlinear state function and observation function; \(w(k) \) and Δ(\(k\)) are process noise vector and observation noise vector, whose covariance matrices are expressed as symbols \(Q(k) \) and \(R(k) \) .

From estimation theory, we know that the system model and the noise characteristic should be exactly known in the EKF estimation framework. However, it is fairly hard to satisfy in actual systems due to the sensor error and the external random interference. Normally system noises are set based on the prior knowledge, and errors will be introduced inevitably into the MMRS. Thus, estimating the noise in real time by utilizing the adaptive filter is an ideal solution.

Based on the theory of the adaptive VCE method [13], three pseudo observation vectors are defined as:

\(\begin{cases} l_x(k) = f(x(k-1)) \\ l_w(k) = w(k-1)\\ l_z(k) = z(k-1) \end{cases} \; \)       (8)

wherein \(l_x\) , \(l_w\) and \(l_z\) indicate the process noise, the observation noise and the predicted states noise, respectively.

On the basis of the residual theory, the residual equation of the nonlinear system can be expressed

\(\begin{cases} v_{l_x}(k) =\hat{x}(k-1)+\hat{w}(k-1)-l_x(k) \\ v_{l_w}(k) = \hat{w}(k-1)-l_w(k)\\ v_{l_z} (k) = h(\hat{x}(k))-l_z(k) \end{cases} \; \)        (9)

So in the nonlinear system, the corresponding covariance matrices are:

\(\begin{cases} p_{l_x{l_x}}(k) = F(k-1)P(k-1)F^T (k-1) \\ p_{l_w{l_w}}(k) = Q(k-1) \\ p_{l_z{l_z}} (k) = R(k) \end{cases} \; \)        (10)

Therefore, as long as covariance matrices shown as Eq.(10) are calculated, the covariance matrix of the system process noise will be estimated in real time. Taken the nonlinearity into accunt, the estimation of the residual vectors based on EKF are as belows:

\(\begin{cases} v_{l_x{l_x}}(k) = P_{l_x{l_x}}(k)P^{-1}(k,k-1)G(k)d(k) \\ v_{l_w{l_w}}(k) = Q(k-1)P^{-1}(k,k-1)G(k)d(k) \\ v_{l_z{l_z}} (k) = [H(k)K(k)-I]d(k) \end{cases} \; \)        (11)

where F and H mean Jacobian matrices of the state equation and observation equation, respectively; \(K(k)\) and \(d(k)\) indicate filter gain and filter innovation at epoch k . According to the EKF frame, their variance matrices are:

\(\begin{cases} P_{v_{l_x{l_x}}}(k) = F(k-1)P(k-1)F^T(k-1)\cdot H^T(k)P^{-1}_{dd}(k)H(k)F(k-1)\cdot P(k-1)F^T(k-1)\\ P_{v_{l_w{l_w}}}(k) = Q(k-1)H^T (k)P^{-1}_{dd}(k)H(k)\cdot Q(k-1) \\ P_{v_{l_z{l_z}}} (k) = {I-H(k)G(k)}R(k) \end{cases} \; \)       (12)

where \(P_{dd}\) is the intermediate matrix and

\(P_{dd}(k) = H(k)P(k,k-1)H^T(k)+R(k) \)

Simplicity without loss of generality, all components of observation vectors \(I_z(k) \) and \(I_w(k) \) are supposed as uncorrelated, so the covariance of process noise and observation noise \((R(k), Q(k))\) are diagonal matrices. Thus, the redundant observation component of the pseudo observation vectors expressed in Eq.8 are as follows:

\(\begin{cases} {r_x(k) = trace[F(k-1)P(k)F^T(k-1)\cdot H^T(k)P^{-1}_{dd}(k)H(k)]\\ r_w(k) = trace[Q(k)H^T (k)P^{-1}_{dd}(k)]H(k)\\ r_z(k) = trace[I-H(k)G(k)} \end{cases} \; \)        (13)

Defined that the covariance component factor \(\hat{\sigma}\) represents the relationship between the residual vector v and the reduandant observation component r , so we can get:

\(\hat{\sigma}_{ii}^2 = \frac{v_{ii}^T v_{ii}}{r_{ii}}\)        (14)

where i is the row or column of the matrix.

Hence, the covariance factors of \(l_z(k)\) and \(l_w(k)\) can be expressed as follows:

\(\begin{cases} \hat{\sigma}_{z_{i}}^2 =\frac {v_{z_{i}}^2 (k)}{r_{zi}(k)}\\ \hat{\sigma}_{w_{j}}^2 =\frac {v_{w_{j}}^2 (k)}{r_{wj}(k)} \end{cases} \; \)        (15)

Correspondingly, covariance matrices of the process noise and observation noise can be estimated as follows:

\(Q(k)= \begin{bmatrix} \hat{\sigma}_{w_{1}}^2 (k) & \ & \\ & \cdots & \\\ & & \hat{\sigma}_{w_{m}}^2 (k) \end{bmatrix}\\ \)       (16)

\(R(k)=\begin{bmatrix} \hat{\sigma}_{z_{1}}^2 (k) & \ & \\ & \cdots & \\\ & & \hat{\sigma}_{z_{p}}^2 (k) \end{bmatrix} \)       (17)

3.3 A²CL algorithm based on MPC and VCEKF

To improve the accuracy and adaptability of MMRS CL system, an A²CL algorithm based on MPC strategy and VCEKF method is proposed in this section. The uncertainty of process noise and observation noise is fully considered and the observations are evaluated by utilizing the entropy theory in this novel algorithm. The algorithm achieves the goal of the overall positioning accuracy by planning the trajectories of each mobile robot. The pseudo code of the A2 CL algorithm is shown as Algorithm 4.

Algorithm 4 A²CL algorithm based on MPC and VCEKF

In Algorithm 4, COOPERATIVE(⋅) indicates the CL algorithm based on EKF method shown in Algorithm 1, MPC(⋅) denotes the motion control strategy based on MPC shown in Algorithm 2 and VCEKF(⋅) means the adaptive filter algorithm based on VCEKF shown in Section 3.2.

4. Experiments and analysis

4.1 Experiments set

To test the performance of the A²CL algorithm, the open source Multi-Robot Cooperative Localization and Mapping Dataset from Autonomous Space Robotics Lab (ASRL) of UTIAS, University of Toronto is used to build up the semi-physical experiment environment firstly. There are 5 individual mobile robots and 15 landmarks in the experiment environment. 9 independent datasets are established after the test. Each dataset includes velocity and angular rate of each mobile robot, and the relative range and bearing information. Besides, the dataset also includes the accurate relative range collected from VICON motion capture system as reference. For more details, please visit http://asrl.utias.utoronto.ca/datasets/mrclam/ index.html.

Fig. 1. Equipments and environment of the ASRL experiments

The general idea of the semi-physical experiment is described as follows. Firsty, the observation information of the each mobile robots is processed and the relative range and bearing error information is extracted. Then the observation error is added into the mobile robot simulation environment to lead to the mobile robot in accordance with the error distribution of the real sensor.

4.2 Observation noise extraction and analysis

Before testing the effectiveness of the novel A² CL algorithm, the statistical characteristics of observation error is analyzed. The method of observation noise extraction is introduced firstly.

A. Relative range error statistical method

The relative range error between the mobile robot and landmark is represented as:

\(\delta R=R^a - R^t\)       (18)

where a \(R^a\) denotes the observed range value between the mobile robot and landmark; t \(R^t\) denotes the theoretical range value and it can be expressed as

\(R^t = \sqrt{(x_{true}-x_{lm_{i}})^2+(y_{true}-y_{lm_{i}})}\; \)       (19)

( \(x_{true},y_{true}\) ) denotes the theoretical position of mobile robot which obtained from VICON motion capture system while \((x_{lm},y_{lm_{i}})\)denotes the theoretical position of landmark \(i\) .

B. Relative bearing error statistical method

The relative range error between the mobile robot and landmark is represented as:

\(\delta B= B^a - B^t\)       (20)

where a \(B^a\) denotes the observed range value between the mobile robot and landmark; t \(B^t\) denotes the theoretical range value and it can be expressed as

\(B^t = arctan\frac{y_{true}-y_{lm_{i}}}{x_{true}-x_{lm_{i}}}-H^t\)       (21)

where \(H^t\) denotes the theoretical heading.

One experiment data was randomly selected in the dataset. The observation noise extracted based on the above method are statistically analyzed. The statistical curves are fitted as Fig. 2. It is obvious from Fig. 2 that the observation error is approximate normal distribution.

Fig. 2. Fitting curves of range and bearing error

4.3 Semi-physical simulation experiments environment establishment

To evaluate the performance of the A²CL algorithm, a 2D simulation environment of MMRS is established. Three mobile robots locate in different places at initial time in the simulation environment and their initial position coordinates are shown in Table 2. The simulation environment at the initial time is shown as Fig. 3.

Table 2. Applications in each class

Fig. 3. The simulation environment at the initial time

In the experiment, It is assumed that mobile robots move with a constant speed and the value is set as 0.6m/s, therefore the controling vector just includes the angular rate. Each mobile robot has the basic function of obstacle avoidance to avoid the collisions with other mobile robots and edge. The mobile robots also have the ability of data collection and recording the relative range and bearing information.

In this manuscript, four simulation tests are designed and the schemes were designed respectively as follows:

TEST 1 CL algorithm based on EKF: Three mobile robot moves randomly in the simulation environment. Their speed keeps 0.6m/s and their angular rate is a random value which ranges from -5°/s to 5°/s. The EKF based CL algorithm is utilized to calculate the localization information of all the mobile robots.

TEST 2 Adaptive CL algorithm based on VCEKF: Three mobile robot moves randomly in the simulation environment. Their speed keeps 0.6m/s and their angular rate is a random value which ranges from -5°/s to 5°/s. The VCEKF based CL algorithm is utilized to calculate the localization information of all the mobile robots.

TEST 3 Active CL algorithm based on MPC and EKF: Three mobile robot moves randomly in the simulation environment. Their speed keeps 0.6m/s and their angular rate is obtained by the motion control strategy based on MPC which also ranges from -5°/s to 5°/s. The EKF based CL algorithm is utilized to calculate the localization information of all the mobile robots.

TEST 4 A²CL algorithm based on MPC and VCEKF: Three mobile robot moves randomly in the simulation environment. Their speed keeps 0.6m/s and their angular rate is obtained by the motion control strategy based on MPC which also ranges from -5°/s to 5°/s. The VCEKF based CL algorithm is utilized to calculate the localization information of all the mobile robots.

TEST 1, TEST 2 and TEST 3 are as the reference of TEST 4. In these semi-physical simulation experiments, the initial state vector \(x(0)\), the initial state variance matrix \(P(0)\) and the process noise variance matrix \(Q(0)\) are set as follows:

\(x(0)=\begin{bmatrix} x_1(0) & \ & \\ & x_2(0) & \\\ & & x_3(0) \end{bmatrix} \)          (22)

\(P(0)=\begin{bmatrix} P_1(0)& \ & \\ & P_2(0) & \\\ & & P_3(0) \end{bmatrix} \)          (23)

\(Q(0)=\begin{bmatrix} Q_1(0) & \ & \\ & Q_2(0) & \\\ & & Q_3(0) \end{bmatrix} \)         (24)

where

\(x_1(0)=diag(-10,-11,-1.5); x_2(0)=diag(-3.,-4,1.5);\\ x_3(0) - diag(4, 4, 1.6)\\ P_1(0)=P_2(0)=P_3(0)=diag(1.4,1.4,1.1)\\ Q_1(0)=Q_2(0)=Q_3(0)=diag(0.7^2,0.7^2,0.7^2). \)

Besides, to verify the adapability of the proposed A²CL algorithm based on MPC strategy and VCEKF method further, the observation noise variance matrix is different from the real observation greatly. Table 1 presented the statistic characteristics of the real observation. And in these semi-physical simulation experiments, the observation noise variance matrix is set as follows:

\(R= \begin{bmatrix} R_1& \ & \\ & R_2 & \\\ & & R_3 \end{bmatrix} \)        (25)

where

\(R_1=R_2=R_3=diag(0.9,0.9). \)

Table 1. Fitting parameters of the observation noise

4.4 Results and analysis

To verify the effectiveness and repeatability of the presented A² CL algorithm proposed in this manuscript, 40 times semi-physical simulation tests are finished by utilizing 40 groups of different observation noise from real sensors. And one of the trajectories and error curves by utilizing different algorithms are expressed as Fig. 4 to Fig. 8, respectively. It is obviously known from Fig. 4 that the localization information can be estimated by all four different CL algorithms. It is also known from Fig. 5 to Fig. 8 that all four different CL algorithms keep stability of the estimation error.

Fig. 4. Trajectories by utilizing different estimated algorithms for a typical MMRS. The purple crosses denote the landmarks; The red solid lines denote the ideal trajectories; The blue dash lines denote the estimated trajectories; The triangles denote the poses of each mobile robots.

Fig. 5. Estimation error curves by utilizing different CL algorithms of Robot 1

Fig. 6. Estimation error curves by utilizing different CL algorithms of Robot 2.

Fig. 7. Estimation error curves by utilizing different CL algorithms of Robot 3

Fig. 8. Simulation results and comparison. The Blue bar denotes the mean of positioning error covariance. The brick red bar denotes the standard deviation of positioning error covariance.

To further verify the superiority of the proposed A²CL algorithm, covariances of the 40 times simulation tests localization error were calculated, and the statistical analysis results were shown in Fig. 8. It is obviously known from Fig. 8 that the mean and standard deviation (STD) of the localization error covariance with the VCEKF-based A²CL algorithm are about 0.26m and 0.02m while the ones with the standard EKF-based algorithm are 0.51m and 0.11m, respectively. Therefore, we can know that the VCEKF-based A²CL algorithm has better performance both in localization accuracy and error convergence stability compared with the other three CL algorithms under the same condition.

5. Conclusion

To decrease the effect caused by the system uncertainty on CL accuracy for MMRS, an A²CL algorithm based on MPC strategy and VCEKF method was proposed in this manuscript. The proposed A²CL algorithm based on information optimization strategy can not only reduce the influence caused by the unknown statistical characteristics of system noise on state estimation availably, but also increase the active CL accuracy for MMRS effectively by maximizing the observation information with MPC strategy. Semi-physical simulation experiments were carried out to verify the performance of the proposed A²CL algorithm based on MPC strategy and VCEKF method. And the results proved that the proposed algorithm is more effective and superior both in accuracy and consistency than other traditional localization algorithms.