1 Introduction

Cooperative control for multi mobile robots has been a topic of significant interest in recent years. In many applications, a group of mobile robots are required to follow a predefined trajectory while maintaining a desired spatial formation. Moving in formation has many advantages over conventional systems, for example, it can reduce the system cost, increase the robustness and efficiency of the system [1, 2]. As a result, a critical problem for cooperative control is to design appropriate algorithms so that the group of robots can converge to a desired formation. During the last decade, various formation control strategies have been developed to study the cooperative control for multi mobile robots, such as leader-follower method [3, 4], behavior-based method [5, 6] and virtual structural method [7, 8], each of whom has its own advantages and disadvantages. The leader-follower method is one of the most studied in multi robots formation. However, in this structure, if a follower fails to follow properly, no mechanism can guarantee the formation keeping. The virtual structure method has the good formation maintenance and the easy setting of the coordinated behavior for the group. This structure, however, cannot reconfigure the formation [9]. In the behavioral structure, while seeking a target, collision or obstacle avoidance are important issues, and the desired behaviors are prescribed for each agent accordingly [10].

One control approach that accommodates a general cooperative objective is model predictive control (MPC). It can be regard as a kind of optimal control methods, which employs a dynamic model of plant to forecast the future behavior of state and determines the future control action according to optimization of a certain performance target function or an operation cost function at each sampling time. It has been the target of study in multi-robot motion control in almost a decade [11–14]. The advantages of using MPC for robot motion control are: (i) it can control the robot motions and take input constraints into account so that we can steer the robot safely with a high forward velocity, and (ii) it can use future prediction to produce an predicted trajectory at each time step [14]. In [12–14], the centralized MPC strategy is proposed to study the multi mobile robots formation problem, through establishing the performance index function of all the mobile robots to force the mobile robots moving follow desired path and formation. Although the centralized MPC can handle some of the multi mobile robots formation problems, the computation and communication requirements of solving the centralized problem at every receding horizon update make it impossible to satisfy the demand growth for multi mobile robots system. An effective approach to overcome the above mentioned drawbacks of centralized MPC is to employ distributed MPC (DMPC) in which the optimal trajectory is obtained through solving a number of distributed optimization problems with lower dimensionality [15]. In the context of DMPC, each mobile robot has its own MPC controller, and each MPC controller can exchange information with its neighbor mobile robot controller [16].

In recent years, DMPC has attracted increasing research interest [17–19]. Considering the information exchange among local controllers according to different protocols, DMPC algorithms can be divided into two groups: non-iterative algorithm [20] and iterative algorithm [21, 22]. An iterative DMPC algorithm was developed in [21] based on Nash optimality for large-scale processes to tackle the state coupling between subsystems, and the relevant computation convergence and the nominal stability condition were presented for unconstrained DMPC scheme in [22]. Robust DMPC algorithms were proposed in [23–26], in which the model uncertain problem was converted into solving a linear matrix inequality optimization problem. In [27], a cooperative linear DMPC strategy was presented for any finite number of subsystems and a stabilizability condition was also presented. Due to the great advantages of DMPC, it has been widely used in formation control for multi mobile robots. In [28], a new DMPC algorithm was proposed to study the formation control problem for multi vehicle with coupling terms in the cost function, but each distributed optimal state trajectory satisfy a compatibility constraint which introduces some conservatism. In [29], a distributed nonlinear MPC algorithm was given to solve the problem of steering and coordinating multi mobile robots along given paths. Considering efficient computation, a DMPC algorithm by using an interior point method was investigated in [30] for leader-follower systems. In [31], a distributed aperiodic MPC was investigated for multi-robot systems, where each subsystem solves an optimal problem only when certain control performances cannot be guaranteed according to certain triggering rules. To the best of our knowledge, although lots of results have been available, the issue of simultaneously considering the DMPC and formation control problems has not been fully investigated to date. This motivates the present research.

In this paper, we are concerned with the design of a DMPC algorithm for multi mobile robots formation control. According to the kinematic model of the single mobile robot formulated in [32], the distributed system model is derived for multi mobile robots, where each robot is seen as a subsystem. Cooperation between subsystems can be incorporated in the distributed control problem by including the coupling terms in the cost function. Subsystems that are coupled in the cost function are referred to as neighbors. each subsystem has its own optimal control problem, and neighboring subsystems can exchange information with one another by using wireless communication. Considering the local cost function, the distributed optimal control problem is formulated and solved by using Nash-optimization iteration. Moreover, the convergence condition of the proposed algorithm is presented. Finally, an illustrative example is presented to show the effectiveness of the proposed method.

2 Kinematic model of mobile robot

A mobile robot together with a reference path Γ to be followed is shown in Fig. 1. {B}, {T} and {U} denote the robot body frame, the virtual vehicle frame and the world frame, respectively. Q is the centroid of the mobile robot and \(\textbf {q} = \left ({x_{\text {B}},y_{\text {B}},\alpha _{\text {B}}} \right )\) describes the position \(\left (x_{\text {B}},y_{\text {B}} \right )\) and the orientation α B of the mobile robot. P is the centroid of the virtual vehicle which is the desired tracking position on reference path Γ and \(\textbf {p}=\left ({x_{\text {T}},y_{\text {T}},\alpha _{\text {T}}} \right )\) describes the position \(\left (x_{\text {T}},y_{\text {T}}\right )\) and the orientation α T of the virtual robot. Then, the kinematic model for mobile robot and virtual robot are given as follows

$$\begin{array}{@{}rcl@{}} \left[{\begin{array}{l} {\dot x}_{\text{B}}\\ {\dot y}_{\text{B}}\\ {\dot \alpha} _{\text{B}} \end{array}} \right] = \left[{\begin{array}{ll} {\cos {\alpha_{\text{B}}}} & 0\\ {\sin {\alpha_{\text{B}}}} & 0\\ 0&1 \end{array}} \right] \left[{\begin{array}{l} v\\ \omega \end{array}} \right], \end{array} $$
(1)
$$\begin{array}{@{}rcl@{}} \left[ {\begin{array}{l} {\dot x}_{\text{T}}\\ {\dot y}_{\text{T}}\\ {\dot \alpha} _{\text{T}} \end{array}} \right] = \left[{\begin{array}{ll} {\cos {\alpha_{\text{T}}}} & 0\\ {\sin {\alpha_{\text{T}}}} & 0\\ 0&1 \end{array}} \right]\left[{\begin{array}{l} {v_{r}}\\ {\omega_{r}} \end{array}} \right], \end{array} $$
(2)

where v and ω are the control input representing the forward and angular velocities, respectively. v r and ω r are the control input representing the desired forward and angular velocities, respectively.

Fig. 1
figure 1

Illustration of the path following

Full size image

Let \(\left ({{x_{e}},{y_{e}},{\alpha _{e}}} \right )\) be the error state vector between the robot state vector q and the virtual vehicle’s state vector p. Then, we have

$$\begin{array}{@{}rcl@{}} \left[{\begin{array}{l} {{x}_{e}}\\ {{y}_{e}}\\ {{\alpha}_{e}} \end{array}}\right] = \left[ {\begin{array}{lll} {\cos {\alpha_{\text{{B}}}}}&{\sin {\alpha_{\text{{B}}}}}& 0\\ { - \sin {\alpha_{\text{{B}}}}}&{\cos {\alpha_{\text{{B}}}}}&0\\ \qquad 0 &\quad 0&1 \end{array}} \right](\textbf{{p}} - \textbf{{q}}). \end{array} $$
(3)

Then, the path following problem shown in Fig. 1 is to find a feedback control law u = [ v ω]T such that

$$ {\lim}_{t \to \infty} \;(\textbf{{p}} - \textbf{{q}}) = 0, $$
(4)

with any initial robot posture.

By taking the derivative of Eq. 3 and rearranging with Eqs. 1, 2, one obtains

$$\begin{array}{@{}rcl@{}} \left\{ \begin{array}{l} {{\dot x}_{e}} = \omega {y_{e}} - v + {v_{r}}\cos {\alpha_{e}}\\ {{\dot y}_{e}} = - \omega {x_{e}} + {v_{r}}\sin {\alpha_{e}}\\ {{\dot \alpha}_{e}} = {\omega_{r}} - \omega \end{array}. \right. \end{array} $$
(5)

Let us define

$$\begin{array}{@{}rcl@{}} x = \left[{\begin{array}{l} {{x_{e}}}\\ {{y_{e}}}\\ {{\alpha_{e}}} \end{array}} \right], u = \left[{\begin{array}{l} { - v + {v_{r}}\cos {\alpha_{e}}}\\ \,\,\,\,\,\,\,\,\,\,{\omega_{r} - \omega} \end{array}} \right]. \end{array} $$

Then, system (5) becomes

$$\begin{array}{@{}rcl@{}} \dot x = \left[ {\begin{array}{*{20}{c}} 0&\omega &0\\ { - \omega }&0&0\\ 0&0&0 \end{array}} \right]x + \left[ {\begin{array}{*{20}{c}} 0\\ {{v_r}\sin {\alpha _e}}\\ 0 \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} 1&0\\ 0&0\\ 0&1 \end{array}} \right]u. \end{array} $$
(6)

By linearizing system (6) about the equilibrium point \(\left (x=0, u=0\right )\), one has

$$\begin{array}{@{}rcl@{}} \dot x = {A_{p}}x + {B_{p}}u, \end{array} $$
(7)

where \({A_{p}} = \left [ {\begin {array}{lll} 0&{{\omega _{r}}}&0\\ { - {\omega _{r}}}&0&{{v_{r}}}\\ 0&0&0 \end {array}} \right ]\), \({B_{p}} = \left [ {\begin {array}{ll} 1&0\\ 0&0\\ 0&1 \end {array}} \right ]\). For simplicity, the reference path Γ considered in this paper is a circular path. Then, the controllability of system (7) can be easily checked.

3 Path following and formation

The coordinated path following problem for multi mobile robots is stated as that each mobile robot is required to follow its own predetermined reference path while keeping a desired inter-vehicle formation pattern with its neighbors in time [29]. Considering the formation problem for N, N≥2 mobile robots, a set of N mobile robots and a set of N expected path Γ i , i=1,2,…,N are required. According to Eq. 7, the model of mobile robot i follows path Γ i can be described as follows

$$\begin{array}{@{}rcl@{}} {\dot x_{i}} = {A_{pi}}{x_{i}} + {B_{pi}}{u_{i}} \end{array} $$
(8)

where \({x_{i}} = \left [{\begin {array}{ll} {{x_{e,i}}}\\ {{y_{e,i}}}\\ {{\alpha _{e,i}}} \end {array}} \right ]\), \({A_{pi}} = \left [ {\begin {array}{lll} 0&{{k_{p,i}}{v_{r,i}}}&0\\ { - {k_{p,i}}{v_{r,i}}}&0&{{v_{r,i}}}\\ 0&0&0 \end {array}} \right ]\), \({B_{pi}} = \left [ {\begin {array}{ll} 1&0\\ 0&0\\ 0&1 \end {array}} \right ]\), \({u_{i}} = \left [ {\begin {array}{ll} { - v_{i} + {v_{r,i}}\cos {\alpha _{e,i}}}\\ {{\omega _{r,i}} - \omega _{i}} \end {array}} \right ]\).

Considered the sampling period of the sensors onboard mobile robots is T, then the discrete-time model can be derived by discretizing (8) with sampling period T

$$ {x_{i}}\left({k + 1} \right) = {A_{i}}{x_{i}}\left(k \right) + {B_{i}}{u_{i}}\left(k \right) $$
(9)

where A i =e ApiT, \({B_{i}} = {{\int }_{0}^{T}} {{e^{{A_{pi}}\tau } }d\tau } {B_{pi}}\).

Formation configuration and information exchange can be generalized as a graph. The formation graph is defined as \(G = \left ( {V,\varepsilon } \right )\), where V=1, ⋯, N is the set of mobile robots and \(\varepsilon \subset V \times V\) is the set of relative vectors between mobile robots. Two mobile robots i and j are called neighbors if \(\left ( {i,j} \right ) \in \varepsilon \), and the set of neighbors of mobile robot i is denoted by η i ∈V. For convenience, we define Γ i as the expected path for mobile robot i and Γ as the base reference path for the whole system. The radius of Γ i and Γ are denoted as R i and R, respectively. Let \({s_{i}}\left (k \right )\) be the position of mobile robot i projected on the expected pace Γ i and \({s_{j}}\left (k \right )\) be the position of mobile robot j projected on the expected pace Γ j at time instance k. Denoted \({s_{i}}^{\prime } \left ( k \right )\) and \({s_{j}}^{\prime } \left ( k \right )\) be the projective position on the whole system base reference path Γ of mobile robots i and j, respectively. Then, the arc length between \({s_{i}}^{\prime } \left ( k \right )\) and \({s_{j}}^{\prime } \left ( k \right )\) is denoted as s i j (k). Since the reference paths are circle paths, the arc length can be described as follows

$$\begin{array}{@{}rcl@{}} \begin{array}{l} {s_{ij}}\left(k \right) = {s_{i}}^{\prime} \left(k \right) - {s_{j}}^{\prime} \left(k \right)\\ \;\;\, = {s_{i}}\left(k \right)R/{R_{i}} - {s_{j}}\left(k \right)R/{R_{j}}. \end{array} \end{array} $$
(10)

It should be noted that s i j (k)=−s j i (k).

In order to keep a desired inter-vehicle formation pattern, s i j (k) is expected to close to the desired value in time. Furthermore, the radius angle between two mobile robots \({\theta _{ij}}\left (k\right )\) is also expected to close to the desired value, and satisfies \({s_{ij}}(k) = R{\theta _{ij}}\left ( k \right )\). As an example, the formation configuration for three mobile robots is shown in Fig. 2. In this example, the position of virtual mobile robot 1, mobile robot 2 and mobile robot 3 on the expected path Γ1, Γ2 and Γ3 are \({s_{1}}\left ( k \right )\), \({s_{2}}\left ( k \right )\) and \({s_{3}}\left ( k \right )\), respectively. The path Γ2 for mobile robot 2 is assumed as the base reference path Γ. Then, we can obtain the arc length as follows

$$\begin{array}{@{}rcl@{}} \begin{array}{l} {s_{12}}\left(k \right) = {s_{1}}\left(k \right){R_{2}}/{R_{1}} - {s_{2}}\left(k \right)\\ {s_{23}}\left(k \right) = {s_{2}}\left(k \right) - {s_{3}}\left(k \right){R_{2}}/{R_{3}}\,. \end{array} \end{array} $$
Fig. 2
figure 2

Straight line formation of three mobile robots

Full size image

By the aforementioned discussion, it can be known that the arc length is a constraint for the formation. Distributed MPC is a popular technique for the control of large-scale systems [17–19]. Its essential feature renders the MPC approach very appropriate to incorporate the constraints into the online optimization. Therefore, the formation problem is to design a stabilizing local controller \({u_{i}}\left (k\right )\) for multi mobile robots system by distributed MPC strategy.

4 Distributed MPC

The main idea of the distributed MPC algorithm is an online optimization of MPC. For multi mobile robots, the optimization formulation can be decomposed into a number of small-scale optimizations, and each mobile robot is treated as a subsystem. In the formation problem, each mobile robot can communication with its neighbors and exchange their state information without delay by wireless network [33, 34]. The communication topology is shown in Fig. 3. In order to guarantee the multi mobile robots to work at given references and a desired inter-vehicle formation pattern, we propose a local optimization performance index for the mobile robot as follows

$$\begin{array}{@{}rcl@{}} \begin{array}{l} {J_{i}}\left(k \right) = \sum\limits_{j = 1}^{P} {{x_{i}^{T}}\left({k + j|k} \right){Q_{i}}{x_{i}}\left({k + j|k} \right)} \\ \;\;\;\;\;\;\;\;\;\;\;\; + \sum\limits_{j = 0}^{M - 1} {{u_{i}^{T}}\left({k + j|k} \right){S_{i}}{u_{i}}\left({k + j|k} \right)} \\ \;\;\;\;\;\;\;\;\;\;\;\; + \sum\limits_{j \in {\eta_{i}}} {{w_{i,j}}{{\left({\frac{{{s_{i}}\left({k + 1|k} \right)R}}{{{R_{i}}}} - \frac{{{s_{j}}\left({k + 1|k} \right)R}}{{{R_{j}}}} - s_{i,j}^{R}} \right)}^{2}}}. \end{array} \end{array} $$
(11)

where \({x_{i}}\left ( {k + j|k} \right )\) and \({u_{i}}\left ( {k + j|k} \right )\) are the predicted value of the variables \({x_{i}}\left ( {k + j} \right )\) and \({u_{i}}\left ( {k + j} \right )\) based on the measurements of time instance k, respectively. P and M are the prediction horizon and control horizon, respectively, Q i and S i are the given positive definite weight matrices with appropriate dimension, w i,j is the given positive scalar. R is the radius of the base reference Γ, \(s_{i,j}^{R}\) is the expected arc length between two position points for the position of mobile robot i and robot j projected on Γ. \({s_{i}}\left ( {k + 1|k} \right )\) and \({s_{j}}\left ( {k + 1|k} \right )\) are the predicted value of the variables \({s_{i}}\left ( {k + 1} \right )\) and \({s_{j}}\left ( {k + 1} \right )\) for mobile robot i and robot j based on the measurements of time instance k, respectively. η i is the set of neighbors for mobile robot i. The third term on the right side of the Eq. 11 is a coupling cost between mobile robots i and j, which involves the information from the neighbors.

Fig. 3
figure 3

Communication topology for multi mobile robots

Full size image

Remark 1

It should be noted that the position of mobile robot \({s_{i}}\left ( k \right )\) is a nonlinear function. For simplicity, we use the linearization method to approximate \({s_{i}}\left ( k \right )\). In order to reduce the linearization error in the prediction horizon, one step predictive value \({s_{i}}\left ( {k + 1|k} \right )\) is used to explain the coupling terms of the performance index (11) for the multi mobile robots.

Considering the linearization method for the mobile robot model, the position of mobile robot i can be expressed as the following recursive formulation

$$\begin{array}{@{}rcl@{}} {s_{i}}\left({k + 1} \right) = {s_{i}}\left(k \right) + T{R_{i}}{\omega_{i}}\left(k \right)\,. \end{array} $$
(12)

Since the reference paths are concentric circles, it can be known that all mobile robots have the same angular velocity, such as ω r,i =ω r for all mobile robots. By the definition of u i (k), (12) can be rewritten as follows

$$\begin{array}{@{}rcl@{}} {s_{i}}\left({k + 1} \right) = {s_{i}}\left(k \right) + T{R_{i}}\left[ {{\omega_{r}} - F{u_{i}}\left(k \right)} \right] \end{array} $$
(13)

where \(F = \left [ {\begin {array}{ll} 0&1 \end {array}} \right ]\).

Let

\({\tilde Q_{i}} = diag\underbrace {\left ( {{Q_{i}},...,{Q_{i}}} \right )}_{P \times {Q_{i}}}\), \({\tilde S_{i}} = diag\underbrace {\left ( {{S_{i}},...,{S_{i}}} \right )}_{M \times {S_{i}}}\), \(\tilde F = \left [ {\begin {array}{ll} 0&1 \end {array}} \right .\;\;\underbrace {\left . {\begin {array}{lll} 0& {\cdots } &0 \end {array}} \right ]}_{2\left ( {M - 1} \right ) \times 0}\), \({\tilde x_{i}}\left ( k \right ) = \left [ {\begin {array}{l} {{x_{i}}\left ( {k + 1|k} \right )}\\ {\vdots } \\ {{x_{i}}\left ( {k + P|k} \right )} \end {array}} \right ]\), \({\tilde u_{i}}\left ( k \right ) = \left [ {\begin {array}{l} {{u_{i}}\left ( {k|k} \right )}\\ {\vdots } \\ {{u_{i}}\left ( {k + M - 1|k} \right )} \end {array}} \right ]\).

Then, local performance index (11) can be rewritten as follows

$$\begin{array}{@{}rcl@{}} \begin{array}{l} {J_{i}}\left(k \right) = {\tilde x_{i}^{T}}\left(k \right){{\tilde Q}_{i}}{{\tilde x}_{i}}\left(k \right) + {\tilde u_{i}^{T}}\left(k \right){{\tilde S}_{i}}{{\tilde u}_{i}}\left(k \right)\\ \;\;\;\;\;\;\;\;\;\;\;\; + \sum\limits_{j \in {\eta_{i}}} {{w_{i,j}}\left({ - RT\tilde F{{\tilde u}_{i}}\left(k \right) + \frac{{R{s_{i}}\left(k \right)}}{{{R_{i}}}}} \right.} \\ \;\;\;\;\;\;\;\;\;\;\;\;{\left. { - \frac{{R{s_{j}}\left(k \right)}}{{{R_{j}}}} + RT\tilde F{{\tilde u}_{j}}\left(k \right) - s_{i,j}^{R}} \right)^{2}}\\ \quad \quad \;\, = {\tilde x_{i}^{T}}\left(k \right){{\tilde Q}_{i}}{{\tilde x}_{i}}\left(k \right) + {\tilde u_{i}^{T}}\left(k \right){{\tilde S}_{i}}{{\tilde u}_{i}}\left(k \right)\\ \;\;\;\;\;\;\;\;\;\;\;\; + \sum\limits_{j \in {\eta_{i}}} {{w_{i,j}}{{\left({\Psi}{{\tilde u}_{i}}\left(k \right) + {{\Phi}_{i,j}} \right)}^{2}}} \end{array} \end{array} $$
(14)

where \({\Psi } = - RT\tilde F\), \({{\Phi }_{i,j}} = \frac {{R{s_{i}}\left ( k \right )}}{{{R_{i}}}} - \frac {{R{s_{j}}\left ( k \right )}}{{{R_{j}}}} + RT\tilde F{\tilde u_{j}}\left ( k \right ) - s_{i,j}^{R}\).

Therefore, the formation control target of mobile robot i can be described as the following minimization problem

$$\begin{array}{@{}rcl@{}} {\min}_{{u_{i}}\left(k \right),...,{u_{i}}\left({k + M - 1} \right)} {J_{i}}\left(k \right)\,. \end{array} $$
(15)

Remark 2

It can be seen that the local optimization problem as in Eq. 15 for mobile robot i contains all the inputs \({u_{j}}\left ( k \right )\) and position information \({s_{j}}\left ( k \right )\), j∈η i of its neighbors. The position information and control input exchange by wireless communication network.

Such local optimization problem (15) with different goals can be solved by means of Nash optimal concept [21]. Each robot optimizes its local objective only using its own control decision assuming that the neighbors optimal solutions are known. It is obviously that the minimize problem (15) is a QP problem. By using the first order KKT condition, the solution to Eq. 15 is obtained as follows

$$\begin{array}{@{}rcl@{}} \begin{array}{l} {{\tilde u}_{i}}\left(k \right) = - {\left({{\tilde B}_i^T{{\tilde Q}_{i}}{{\tilde B}_{i}} + {{\tilde S}_{i}} + \sum\limits_{j \in {\eta_{i}}} {{w_{i,j}}{\Psi}^{T}{\Psi}}} \right)^{- 1}}\\ \;\;\;\;\;\;\;\;\;\;\;\; \times \left({{\tilde B_{i}^{T}}{{\tilde Q}_{i}}{{\tilde A}_{i}}{x_{i}}\left(k \right) + \sum\limits_{j \in {\eta_{i}}} {{w_{i,j}}{\Psi}^{T}{{\Phi}_{i,j}}}} \right) \end{array} \end{array} $$
(16)

where \({\tilde A_{i}} = \left [ {\begin {array}{l} {{A_{i}}}\\ {\vdots } \\ {{A_{i}^{P}}} \end {array}} \right ]\), \({\tilde B_{i}} = \left [ {\begin {array}{llll} {{B_{i}}}&0& {\cdots } &0\\ {{A_{i}}{B_{i}}}&{{B_{i}}}& {\cdots } &0\\ {\vdots } & {\vdots } & {\cdots } & {\vdots } \\ {A_{i}^{P-1}{B_{i}}} & {A_{i}^{P - 2}B} & {\cdots } &{A_{i}^{P - M}B} \end {array}} \right ]\).

The Nash optimization algorithm is one of the iterative algorithms, which is developed to seek the local optimal control decision for each subsystem at each sampling time. If the algorithm is convergent, all the terminal conditions of all the subsystems will be satisfied, and then the iteration will be terminated; otherwise, each subsystem transfer the newly computed control decision to its neighbors, and resolves its local problem with the updated values for neighbors. The overall control system will converge at Nash equilibrium. However, the Nash optimal solution to the local optimization problem may not equal to the global optimal control decision. In formation of multiple mobile robots path following problem, the goal of performing communication and exchanging solutions among controllers is to achieve the optimal solution of the each mobile robot in an iterative fashion. The algorithm is summarized as follows. Algorithm 1:

  • S1 : Initialization: At time instant k=0, iterations p=0, the state of mobile robot \(i,i \in \left ( {1,...,N} \right )\) are \({x_{i}}\left ( k \right )\) and \({s_{i}}\left ( k \right )\), and the control input \({\tilde u_{i}^{p}}\left ( k \right )\).

  • S2 : Communication: Mobile robot i obtain its neighbors position information \({s_{j}}\left ( k \right )\) and control input \({\tilde u_{j}^{p}}\left ( k \right )\), \(\left ( {i,j} \right ) \in \eta \), and send its position information \({s_{i}}\left ( k \right )\) and control input \({\tilde u_{i}^{p}}\left ( k \right )\) to its neighbors by using wireless communication.

  • S3 : Local optimization: Each subsystem resolves its local optimization problem described in Eq. 15, and then calculates the local optimal input \(\tilde u_{i}^{p + 1}\left ( k \right )\) by Eq. 16.

  • S4 : Checking converge: Given error accuracy ε i , if all subsystems satisfy the terminal condition \(||\tilde u_{i}^{p + 1}\left ( k \right ) - {\tilde u_{i}^{p}}\left ( k \right )|| \le {\varepsilon _{i}}\), \(i \in \left ( {1,...,N} \right )\), then end the iteration, set the local optimal control input \(\tilde u_{i}^{*}\left ( k \right ) = {\tilde u_{i}^{p}}\left ( k \right )\), go to S5, otherwise, let p=p+1, each subsystem communicates to exchange the new information \({\tilde u_{i}^{p}}\left ( k \right )\) with its neighbors, and go to S2.

  • S5 : Implementation: Apply the first element \(u_{i}^{*}\left ( {k|k} \right )\) from the control input \(\tilde u_{i}^{*}\left ( {k} \right )\) to its neighbors. Set the initial control input for the next sampling time \({\tilde u_{i}^{0}}\left ( {k + 1} \right ) = \tilde u_{i}^{*}\left ( k \right )\).

  • S6 : Receding horizon: p=0,k=k+1, move horizon to the next sampling time, go to S1.

It is known that the overall control system will converge at Nash equilibrium if Algorithm 1 is convergent. Then, we will give the convergence analysis of the proposed distributed algorithm. For the sake of clarity of exposition, we shall use the following definitions

$$\begin{array}{@{}rcl@{}} {G_{i}} &=& {\tilde B_{i}^{T}}{\tilde Q_{i}}{\tilde B_{i}} + {\tilde S_{i}} + \sum\nolimits_{j \in {\eta_{i}}} {{w_{i,j}}{{\Psi}^{T}}{\Psi}},\quad {H_{i}} = {\tilde B_{i}^{T}}{\tilde Q_{i}}{\tilde A_{i}}, \\{{\Pi}_{i,j}} \!&\,=\,&\! (\!R{s_{i}}(k)/{R_{i}} \,-\, R{s_{j}}(k)/{R_{j}}\,-\,s_{i,j}^{R}){w_{i,j}}{{\Psi}^{T}}\!, {\mathrm{T}_{i,j}} = {w_{i,j}}{{\Psi}^{T}}RT\tilde F. \end{array} $$

Then, the local MPC controller (16) can be rewritten as follows

$$\begin{array}{@{}rcl@{}} {\tilde u_{i}}(k) = - G_{i}^{- 1}\left[ {{H_{i}}{x_{i}}(k) + \sum\limits_{j \in {\eta_{i}}} { {{{\Pi}_{i,j}} + {\mathrm{T}_{i,j}}{{\tilde u}_{j}}(k)}} } \right]\, . \end{array} $$
(17)

From Algorithm 1 and Eq. 17, the relationship of control decision for the ith subsystem between iteration p and iteration p+1 can be derived as follows

$$\begin{array}{@{}rcl@{}} \tilde u_{i}^{p + 1}(k) \,=\, - G_{i}^{- 1}\left[ \!{{H_{i}}{x_{i}}(k) + \sum\limits_{j \in {\eta_{i}}} { {{{\Pi}_{i,j}} + {\mathrm{T}_{i,j}}{\tilde u_{j}^{p}}(k)}}} \!\right]. \end{array} $$
(18)

From Eq. 18, we can obtain the control decision of the whole system as follows

$$\begin{array}{@{}rcl@{}} {U^{p + 1}}(k) = - {G^{- 1}}\left[ {Hx(k) + {\Pi} + \mathrm{T}{U^{p}}(k)} \right] \end{array} $$
(19)

where \(x(k) = {\left [ {\begin {array}{lll} {{x_{1}^{T}}(k)}&\cdots &{{x_{N}^{T}}(k)} \end {array}} \right ]^{T}}\), \(U(k) = {\left [ {\begin {array}{llll} {{\tilde u_{1}^{T}}(k)}& {\cdots } &{{\tilde u_{N}^{T}}(k)} \end {array}} \right ]^{T}}\), \(G = diag\left \{ {{G_{1}},\; {\cdots } ,\;{G_{N}}} \right \}\), \(H = diag\left \{ {{H_{1}},\; {\cdots } ,\;{H_{N}}} \right \}\), \({\Pi } (i,j) = \left \{ {\begin {array}{l} {{{\Pi }_{i,j}},\;i \ne j}\\ {0,\;\;\;\;i = j} \end {array}} \right .\), \(\mathrm {T}(i,j) = \left \{ {\begin {array}{l} {{\mathrm {T}_{i,j}},\;i \ne j}\\ {0,\;\;\;\;i = j} \end {array}} \right .\).

Since x i (k) and s i (k) are known in advance at time k, it can be known that H x(k)+π is a constant term at each iteration. Then, it can be easily concluded that if the eigenvalues of G −1T are all in the unite circle, \(\left | {\lambda ({G^{- 1}}\mathrm {T})} \right | < 1\), the proposed distributed algorithm is convergent.

5 Illustrative example

In this section, path following of three mobile robots on two concentric circles path is used to illustrative the effectiveness of the proposed algorithm. The expected formation of these three mobile robots is shown in Fig. 4. In this formation, mobile robot 1 and mobile robot 3 are expected to follow reference path Γ1 , which is a circle with radius of 2m, while mobile robot 2 is expected to follow reference path Γ2=Γ, which is a circle with radius of 1m. Moreover, the angle and arc length between two mobile robots also should be satisfied constraints. Specifically, the expected angle between mobile robot 1 and mobile robot 2 is \({\theta _{12}} = \frac {\pi } {6}\), and the arc length that between the projected position of mobile robot 1 and mobile robot 2 on Γ is expected as \({s_{12}} = \frac {\pi } {3}\); the expected angle between mobile robot 2 and mobile robot 3 is \({\theta _{23}} = \frac {\pi } {6}\), and the arc length that between the projected position of mobile robot 2 and mobile robot 3 on Γ is expected as \({s_{23}} = \frac {\pi } {3}\).

Fig. 4
figure 4

The expected formation for three mobile robots

Full size image

It is assumed that the expected angular velocity of three mobile robots is 1.5 rad/s and the systems is discretized with sampling period T=0.15 s. In order to drive the mobile robots toward the desired formation, the proposed distributed model predictive control is applied to the robot system. For the distributed model predictive control, the prediction horizon and control horizon are P=3 and M=1, respectively. The weight matrices in the performance index are chosen as Q i =100×I 3, S i =I 2, w i =100, \(i \in \left ( {1,...N} \right )\), where I i shows the identity matrix with dimension i. Then, by using the proposed method in Algorithm 1, the distributed controllers are obtained and the tracking performances are shown in Figs. 5, 6 and 7, where the solid curve denotes the error of the horizontal direction, the dash-and-dot one denotes the error of the vertical direction and the dash one denotes the angle deviation. It can be seen from Figs. 5, 6 and 7 that the mobile robot 1 and mobile robot 3 converge to the reference path Γ1 and mobile robot 2 converges to reference Γ2 finally. The constraints, arc length on Γ between two robots, is shown in Fig. 8. It is shown that the arc length that between the projected position of mobile robots 1 and mobile robot 2 on Γ is converge to s 12=𝜃 12×R 2≈0.523, that means the angle between mobile robot 1 and mobile robot 2 converge to \({\theta _{12}} = \frac {\pi } {6}\); the arc length that between the projected position of mobile robots 2 and mobile robot 3 on Γ is converge to s 23=𝜃 23×R 2≈0.523, and the angle between mobile robot 2 and mobile robot 3 converge to \({\theta _{23}} = \frac {\pi } {6}\). Figure 9 shows the real trajectory of three mobile robots under the proposed control algorithm, where the circle denotes the initial position of the robot, and the triangle denotes the position of the robot at time 45 s. As can be seen from the simulation results, the designed distributed controller based on Nash optimization algorithm performs well for multiple mobile robots formation problem.

Fig. 5
figure 5

The states of mobile robot 1

Full size image
Fig. 6
figure 6

The states of mobile robot 2

Full size image
Fig. 7
figure 7

The states of mobile robot 3

Full size image
Fig. 8
figure 8

The angular separation for mobile robots

Full size image
Fig. 9
figure 9

The trajectory of three mobile robots

Full size image

6 Conclusion

In this paper, the distributed model predictive control problem was addressed for multi mobile robots systems. According to the kinematic model of single robot, the distributed system model was obtained for multi mobile robots. By including the coupling terms in the cost function, the distributed optimal control problem was formulated by the local cost function. The distributed controller was obtained by using Nash-optimization iteration algorithm, and the convergence analysis of the proposed algorithm was also presented. Finally, the effectiveness of the proposed method was verified by the simulations.