Distributed Model Predictive Control with Particle Swarm Optimizer for Collision-Free Trajectory Tracking of MWMR Formation

Abstract

The distributed model predictive control (DMPC) strategy with particle swarm optimization (PSO) is applied to solve the collision-free trajectory tracking problem for the mecanum-wheeled mobile robot (MWMR) formation. Under the leader–follower framework, the predictive model is established considering the kinematics and dynamics of the MWMR with the uncertainties and external disturbances. Based on the information from itself and its neighbors, each MWMR is assigned its own finite-horizon optimal control problem, of which the objective/cost function consists of formation maintenance, trajectory tracking, and collision avoidance terms, and the control inputs of each MWMR are computed synchronously in a distributed manner. PSO serves as the fast and effective optimizer to find feasible solutions to these finite-horizon optimal control problems. Further, the feedback emendation is implemented using a double closed-loop compensator to efficiently inhibit the influence of unknown dynamics in real time. The stability of the proposed distributed formation control approach is strictly analyzed. Numerical simulations confirmed the robustness and effectiveness of the control approach in obstacle environments.

1. Introduction

In recent years, extensive research has focused on the motion control of mobile robots [1,2,3,4]. As a typical omnidirectional mobile robot, the mecanum-wheeled mobile robot (MWMR) has been widely used in family service and industrial production fields because of its flexibility in confined spaces and capability of moving to any direction without a turning radius [5]. Four specially made mecanum wheels are symmetrically mounted around the mobile robot, while each mecanum wheel is separately driven by a motor. Some rollers are angled at 45° to the hub circumference of a mecanum wheel [6], which enables the MWMR to move in sideways.

Mobile robot formation aims to control multiple mobile robots to synchronously move in a certain formation pattern, which is one of the hotspot problems in the study of robotics [7,8]. There are some typical techniques for solving the formation control problem, for example, the leader–follower method, virtual structure method, and behavior-based methods [9,10,11,12,13]. Formation control of multiple MWMRs has also attracted extensive attention from researchers. Aditya et al. [14] established the collaborative kinematics of the MWMR’s system to realize the cooperative transformation of large-scale goods in formation. Mehrez et al. [15] established a formation set-point stabilization of MWMRs using the model predictive control (MPC) method without terminal constraints. However, the MWMR actually is a complex mechanic system with kinematics, dynamics, and input saturation constraints. Treating it as a mass point or only considering the kinematics will reduce the control accuracy.

The MWMR has the complexity of nonlinearity, uncertainty, and external disturbances in its dynamics. Additionally, the input saturation constraints can also bring great challenge to the precise control of MWMRs [16,17]. Alakshendra et al. [18] derived a generalized dynamic equation of a MWMR using Newton–Euler method. Sun et al. [19] presented the integrated kinematic–dynamic model of MWMRs, and the formation control of MWMRs under changing topologies was considered [20,21]. However, uncertainties and unmodeled disturbances can usually bring up the instability in the control system [22,23]. Thus, it is highly necessary to handle the uncertainties and disturbances in the MWMRs’ formation control problem. Lu et al. [22] employed the artificial neural network to estimate various uncertain disturbances and proposed a novel adaptive sliding mode control (SMC) method to design the trajectory tracking controller of MWMR. Zhao et al. [23] employed a fuzzy approximator to approximate unknown dynamics and applied the fixed-time extended state observer (FTESO) to estimate external disturbances for improving the tracking accuracy of MWMR. Wang et al. [24] proposed a robust control strategy combing the adaptive SMC method and extended state observer (ESO) to compensate for total disturbances during the formation control process of MWMRs. However, the observer-based method can only estimate the constant and slowly varying parameters. In real contexts, uncertainties and disturbances are usually time-varying parameters, which are more suitable for feedback compensation strategies. Moreover, the collision of MWMRs with obstacles and other MWMRs should be avoided to ensure the safety of the MWMRs’ formation [25]. Potential field-based methods are often used to handle the collision avoidance [26,27], but these methods have the drawback of trapping in the local extremum. Barrier function-based finite-time SMC is also an effective control method for mobile robots under uncertainties and input saturations [28,29]. Mostafa et al. [28] proposed an optimal adaptive barrier function-based super-twisting SMC scheme for the trajectory tracking control of parallel robots with highly complex dynamics in the presence of uncertainties and external disturbances, where the global property of the controller eliminates the reaching phase, thereby guaranteeing system stability, and the barrier function-based adaptation law removes the requirement to know the upper bounds of the external disturbances. Khalid et al. [29] proposed an adaptive barrier function-based non-singular terminal SMC approach for the trajectory tracking of a quadrotor drone, where the non-singular terminal sliding surface was employed to ensure the finite time convergence of the linear sliding surfaces and tracking errors.

Distributed model predictive control (DMPC) is an effective method to handle the distributed cooperative control problem with dynamic uncertainties and disturbances [30,31], and can be applied to formation control of mobile robots [32,33,34,35]. Using the distributed framework, robots can share information with neighbors and solve their own local optimal control commands based on the prediction model. Zhao et al. [32] proposed a DMPC framework for autonomous underwater vehicles (AUV) through combining the alternating direction multiplier (ADMM) solver with feedforward compensation to deal with certain disturbances. Mario et al. [33] proposed a DMPC law for differentially driven robots and ensured system asymptotic stability. Qin et al. [34] proposed a Nash-based DMPC strategy for the formation control of multiple mobile robots, and the optimal control input is solved by a primal–dual neural network. However, there are few studies on MWMR formation control using the DMPC method. Wang et al. [24] applied linear MPC in the outer loop kinematic controller of MWMRs, but dynamics were not considered. Xiao et al. [35] proposed the self-triggered organized formation (STOF) system by combing the DMPC framework and the consensus protocol to achieve the smoother control performance for the MWMR’s formation, but the collision avoidance and obstacle avoidance were not considered.

The particle swarm optimization (PSO) algorithm is a typical swarm intelligence optimization algorithm with the advantages of being a concise concept, having convenient implementation, high precision, and fast convergence. Benefitting from a fast convergence speed and global search capability, the PSO optimizer is more efficient and stable than traditional optimization solvers. Today, there are still many recent studies applying the PSO optimizer in solving MPC problems, achieving good results [36,37,38,39,40,41].

Although lots of existing works have been available, the issue of formation control of MWMRs has not been well addressed yet when simultaneously considering various physical constraints, including kinematics/dynamics constraints, uncertainties, and disturbances, as well as collision and obstacle avoidance. In this paper, taking into account the model physical constraints and uncertainties, the collision-free trajectory tracking control of the MWMR’s formation is solved using a closed-loop DMPC strategy. Based on the leader-follower framework, the formation predictive model, which describes both the kinematics and dynamics of MWMRs with the uncertainties and external disturbances, is established. Under the distributed protocol, the MWMR can share the information among itself and its neighbors. A finite-horizon optimal control problem is assigned to each MWMR to minimize the objective/cost function consisting of formation maintenance, trajectory tracking, and collision avoidance terms. The PSO algorithm is introduced to find feasible optimal control solutions for each MWMR. Then, a novel double closed-loop disturbance feedback controller is proposed to compensate for model uncertainties and external disturbances. Theoretical analysis assures the stability of the proposed distributed formation control approach. The main contribution of the paper is described in the following part:

(1)

The leader–follower predictive model of MWMR formation, considering the kinematics and dynamics with the uncertainties and external disturbances, is established.

(2)

By combining information from itself and its neighbors, each MWMR is given its own finite-horizon optimal control problem within the proposed framework of DMPC, whose objective/cost function compromises of formation maintenance, trajectory tracking, and collision avoidance terms simultaneously, is established.

(3)

A PSO-based control solver is used to find the feasible solutions to these finite-horizon optimal control problems, which shows better performance than traditional solvers.

(4)

A novel double closed-loop feedback compensation controller is developed to compensate for model uncertainties and external disturbances in real-time.

The rest of this paper is constructed as follows. Section 2 describes the problem description and definition of the MWMR’s formation. Section 3 presents the proposed PSO-based DMPC controller for the MWMR’s formation in detail, including the DMPC framework, PSO optimizer, double closed-loop feedback compensator, as well as the stability analysis. In Section 4, numerical results demonstrate the superiority and effectiveness of our proposed strategy. Section 5 gives the concluding remarks finally.

2. Problem Description

2.1. MWMR Model

In this section, both the kinematic and dynamic models of MWMRs with uncertainties and time-varying external disturbances are presented. Kinematics can depict the relationship between the position and velocity of the robot, and then, dynamics further establish the relationship between the control inputs and velocity of the robot.

2.1.1. Kinematics

In order to set up the mathematics model of kinematics for the MWMR, two coordinate frame systems are constructed firstly. As is shown in Figure 1, $X_a{O}_a{Y}_a$ denotes the world/global frame, $X_b{O}_b{Y}_b$ denotes the local/body frame $\mathit{x}={\left[x,y,\theta \right]}^T$, and ${\mathit{x}}_b={\left[x_b,y_b,\theta \right]}^T$ denotes the posture vectors defined in $X_a{O}_a{Y}_a$ and $X_b{O}_b{Y}_b$, respectively, and:

$$\dot{\mathit{x}}=T(\theta ){\dot{\mathit{x}}}_b$$

(1)

where $T(\theta )$ is the rotation matrix of the local frame with respect to global frame, as follows:

$$T(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

(2)

${\propto }_i^w$ represents the angle between axes of the i-th wheel rollers and the mecanum wheel frame $x_i^w{O}_i^w{y}_i^w$, i = 1,2,3,4, and ${\propto }_i^w=\pi /4$; R_w is the wheel radius; a and b are half the length and width of the MWMR, respectively.

As to the i-th MWMR, the inverse kinematics can be represented as:

$${\dot{\mathit{q}}}_i=J{\dot{\mathit{x}}}_{ib}$$

(3)

$$J=\frac{1}{R_w}\left[\begin{array}{ccc}1& -1& -(a+b)\\ 1& 1& a+b\\ 1& 1& -(a+b)\\ 1& -1& a+b\end{array}\right]$$

(4)

where $x_{ib}={\left[x_{ib},y_{ib},{\theta }_{ib}\right]}^T$ is the state vector of MWMR-i in the body frame; ${\dot{\mathit{q}}}_i={\left[{\dot{q}}_{i1},{\dot{q}}_{i2},{\dot{q}}_{i3},{\dot{q}}_{i4}\right]}^T$ is the angular velocities of four mecanum wheels of MWMR-i.

Define J⁺ as the Moore–Penrose inverse of J that is shown in the following equation:

$$J^{+}={\left(J^{T}J\right)}^{-1}{J}^T$$

(5)

Therefore, the kinematic model of MWMR-i is:

$${\dot{\mathit{x}}}_{ib}=J^{+}{\dot{\mathit{q}}}_i$$

(6)

where J⁺ can be represented as:

$$J^{+}=\frac{R_w}{4}\left[\begin{array}{cccc}1& 1& 1& 1\\ -1& 1& 1& -1\\ -\frac{1}{a+b}& \frac{1}{a+b}& -\frac{1}{a+b}& \frac{1}{a+b}\end{array}\right]$$

(7)

Combining (1) and (6), the kinematic model of MWMR-i with respect to the global frame can be expressed as follows:

$${\dot{\mathit{x}}}_i=T({\theta }_i)J^{+}{\dot{\mathit{q}}}_i$$

(8)

where ${\mathit{\theta }}_i$ represents the heading angle of MWMR-i.

2.1.2. Dynamics

The mathematics model of the MWMR dynamics is developed using the Lagrange’s equation. For the MWMR-i, the kinematic energy equation can be expressed as:

$$K_i=\frac{1}{2}m({\dot{x}}_i^2+{\dot{y}}_i^2)+\frac{1}{2}{J}_z{\dot{\theta }}_i^2+\frac{1}{2}{J}_w({\dot{q}}_{i1}^2+{\dot{q}}_{i2}^2+{\dot{q}}_{i3}^2+{\dot{q}}_{i4}^2)$$

(9)

where m is the total mass of the MWMR, $J_z=m\frac{{(2a)}^2+{(2b)}^2}{12}$ is the moment of inertia of MWMR around the z_b-axis, m_w is the mass of the mecanum wheel, and $J_w=\frac{m_w{R}_w^2}{2}$ is the moment of inertia of wheels around the center of revolution. Further, the loss energy due to the viscous friction is:

$$D=\frac{1}{2}{\dot{\mathit{q}}}_i^T{H}_n{\dot{\mathit{q}}}_i$$

(10)

where $H_n=diag({\lambda }_1,{\lambda }_2,{\lambda }_3,{\lambda }_4)$ denotes the coefficient of the wheel’s viscous friction.

Let the Lagrangian function be

$$L_i=K_i-V_i$$

(11)

where V_i is the potential energy of MWMR-i on the horizontal plane and equals to zero. Using the Lagrangian equation, the following equation can be yielded:

$$\frac{d}{dt}\frac{\partial }{\partial {\dot{\mathit{q}}}_i}{L}_i-\frac{\partial }{\partial {\mathit{q}}_i}{L}_i={\mathit{u}}_i-\frac{\partial }{\partial {\dot{\mathit{q}}}_i}D{R}_w^2-F({\dot{\mathit{q}}}_i)R_w$$

(12)

where ${\mathit{u}}_i={\left[u_{i1},u_{i2},u_{i3},u_{i4}\right]}^T$ denotes the driving torque vector by the four motors of MWMR-i. The static friction $F({\dot{\mathit{q}}}_i)$ is denoted by $F({\dot{\mathit{q}}}_i)={\left[f\mathrm{sgn}({\dot{\mathit{q}}}_{i1}),f\mathrm{sgn}({\dot{\mathit{q}}}_{i2}),f\mathrm{sgn}({\dot{\mathit{q}}}_{i3}),f\mathrm{sgn}({\dot{\mathit{q}}}_{i4})\right]}^T$. $f=\mu m_{w}g$ represents the maximum static friction and $\mu$ is the frictional coefficient. $g=9.8m/s^2$ is the gravity acceleration.

Manipulating (12), the dynamic model of MWMR-i can be represented as:

$${\mathit{u}}_i=M_n{\ddot{\mathit{q}}}_i+H_n{\dot{\mathit{q}}}_i{R}_w^2+F({\dot{\mathit{q}}}_i)R_w$$

(13)

where

$$M_n=\left[\begin{array}{cccc}{J}_A+J_B+J_w& -J_A& J_A& J_B-J_A\\ -J_A& J_A+J_B+J_w& J_B-J_A& J_A\\ J_A& J_B-J_A& J_A+J_B+J_w& -J_A\\ J_B-J_A& J_A& -J_A& J_A+J_B+J_w\end{array}\right]$$

(14)

where $J_A=\frac{J_Z{R}_w{}^2}{16{(a+b)}^2}$, $J_B=\frac{m{R}_w{}^2}{8}$. Then, substituting (8) into (13), the second-order differential equation for MWMR-i can be obtained as follows:

$${\ddot{\mathit{x}}}_i=T({\theta }_i)J^{+}{M}_n{}^{-1}{\mathit{u}}_i-R_{w}T({\theta }_i)J^{+}{M}_n{}^{-1}F({\dot{\mathit{q}}}_i)-[T({\theta }_i){\dot{T}}^{-1}({\theta }_i)+R_w{}^{2}T({\theta }_i)J^{+}{M}_n{}^{-1}{H}_{n}J{T}^{-1}({\theta }_i)]{\dot{\mathit{x}}}_i$$

(15)

2.1.3. Model under Uncertainties and Disturbances

Considering wheel slippage, flaws of mechanical structure, and measurement errors, both the model uncertainties and external disturbances can affect the actual motion of MWMRs. Therefore, adding the uncertainty and disturbance factors into Equation (8), the kinematic model can be rewritten as follows:

$${\dot{\mathit{x}}}_i=T({\theta }_i)(J^{+}+\Delta J){\dot{\mathit{q}}}_i$$

(16)

where ΔJ is the uncertainties term of the kinematic equation. Then, the dynamic equation in (13) can also be rewritten as follows:

$${\mathit{u}}_i+{\mathit{u}}_d=(M_n+\Delta M){\ddot{\mathit{q}}}_i+(H_n+\Delta H){\dot{\mathit{q}}}_i{R}_w^2+(F({\dot{\mathit{q}}}_i)+\Delta F)R_w$$

(17)

where ΔM, ΔH, and ΔF represent the uncertain dynamic terms and ${\mathit{u}}_d$ denotes the unknown input disturbances.

Combining (16) and (17), the second-order dynamic equation with model uncertainties and disturbances can be derived as follows:

$${\ddot{\mathit{x}}}_i=-(T({\theta }_i){\dot{T}}^{-1}+R_w^{2}T({\theta }_i)A^{+}B{T}^{-1}({\theta }_i)){\dot{\mathit{x}}}_i+T({\theta }_i)A^{+}{\mathit{u}}_i+T({\theta }_i)A^{+}{\mathit{u}}_d-R_{w}T({\theta }_i)A^{+}(F({\dot{\mathit{q}}}_i)+\Delta F)$$

(18)

where $A=(M_n+\Delta M){(J^{+}+\Delta J)}^{+}$, $B=(H_n+\Delta H){(J^{+}+\Delta J)}^{+}$, and A⁺ and (J⁺ + ΔJ)⁺ are the Moore–Penrose inverse of A and (J⁺ + ΔJ), respectively.

2.2. Formation Model

In this section, the distributed formation trajectory-tracking model for a team of M MWMRs under the leader–follower framework is established. Given the reference trajectory ${\mathit{x}}_r$ and the desired formation pattern ${\mathit{P}}^d\in R^{2\times M}$, each MWMR-i in the formation should use its own state ${\mathit{x}}_i$ and the neighbor state ${\mathit{x}}_j={\left[x_j,y_j,{\theta }_j\right]}^T$ to form the desired formation shape and track the reference trajectory. At the same time, the collision avoidance and obstacle avoidance are also required.

The desire formation pattern is defined by ${\mathit{P}}^d=\left[p_1^d,\mathrm{\cdots },p_M^d\right]$, where ${\mathit{p}}_i^d={\left[h_{xi},h_{yi}\right]}^T$ is the desired position defined by an orthogonal frame X_fO_fY_f so that the center of the formation pattern is located at the frame origin. ${\mathit{x}}_r={\left[x_r,y_r,{\theta }_r\right]}^T$ represents the reference trajectory, which is assumed to be continuously differentiable at any points. ${\mathit{x}}_i^d={\left[x_i^d,y_i^d,{\theta }_i^d\right]}^T$ is the desired state of MWMR-i as follows:

$${\mathit{x}}_i^d=\frac{1}{N_i+d_i}\left({\displaystyle \sum _{j\in {\mathcal{N}}_i}({\mathit{x}}_j+{\mathit{h}}_i-{\mathit{h}}_j)+d_i({\mathit{x}}_r+{\mathit{h}}_i)}\right)$$

(19)

where ${\mathcal{N}}_i$ is the set of neighbors within the detection range R_det of MWMR-i and N_i is the number of MWMRs belonging to ${\mathcal{N}}_i$. ${\mathit{h}}_i={\left[h_{xi},h_{yi},0\right]}^T$ is the desired formation state of MWMR-i in X_fO_fY_f. $d_i=1$ is for the leader robot and $d_i=0$ is for follower robots. An example of three MWMRs’ formation trajectory-tracking is shown in Figure 2, where the dashed box represents the leader robot, while the solid boxes are follower robots. Since follower robots MWMR-2 and MWMR-3 cannot perceive the desired trajectory, they compute the distributed consensus error ${\displaystyle {\sum }_{j\in {\mathcal{N}}_i}\left({\mathit{x}}_i-{\mathit{x}}_j-\left({\mathit{h}}_i-{\mathit{h}}_j\right)\right)}$ with neighbors. For the leader robot, since it can perceive the trajectory, it needs to add another tracking term with respect to the relative position of x_r.

Assumption 1.

The reference trajectory can be manually determined and can be received by the leader MWMRs but cannot be received by the follower MWMRs.

Assumption 2.

There always exists a directed spanning tree in the time-varying communication graph G(t) within MWMRs.

Definition 1 (Problem statement).

A set of M MWMRs with the communication graph $G$ is supposed to satisfy Assumption 2. There exists a set $ℒ$ of l fixed obstacles in the environment. Given displacement vectors ${\mathit{P}}^d$ defining the desired state of the MWMRs’ formation, for MWMR-i, the formation tracking problem is to compute the optimal control input to track the reference trajectory with a certain desired formation while avoiding collisions and obstacles, which can be formulated as follows:

$$\underset{k\to \infty }{\lim }({\mathit{x}}_i(k)-{\mathit{x}}_i^d)=0$$

(20)

$$\underset{k\to \infty }{\lim }({\mathit{x}}_i(k)-{\mathit{x}}_j(k))={\mathit{h}}_{ij},\forall i,j\in \mathcal{N}, i\ne j$$

(21)

$$\left|\right|{\mathit{p}}_i(k)-{\mathit{p}}_j(k)\left|\right|\ge R_{coll}\forall i,j\in \mathcal{N}, i\ne j$$

(22)

$$\left|\right|{\mathit{p}}_l^o-{\mathit{p}}_i(k)\left|\right|\ge R_{obs}\forall l\in ℒ$$

(23)

where ${\mathit{h}}_{ij}={\mathit{h}}_j-{\mathit{h}}_i$ represents the ideal vector for formation between MWMRs. ${\mathit{p}}_i(k)={\left[x_i(k),y_i(k)\right]}^T$ is the position vector of MWMR-i. R_coll is a safe distance which guarantees the absence of collisions between MWMRs. ${\mathit{p}}_l^o={\left[x_l^o,y_l^o\right]}^T$ is the closest position of obstacle l and R_obs is the safe distance which guarantees obstacle avoidance.

3. Formation Control Based on DMPC with PSO Optimizer

3.1. DMPC Framework

The proposed DMPC scheme for MWMR formation is shown in Figure 3. At the k-th timestep, the control inputs of MWMRs are initialized by using the previous predicted optimal control trajectories. Then, each MWMR-i receives the assumed control trajectory from its neighbors and meanwhile transmits its assumed control trajectory to the neighbors. Based on the estimated state and control trajectories from neighbors, each MWMR evaluates its own cost function and finds the optimal predicted control trajectory over the current prediction horizon. Finally, the optimal control trajectory on the first prediction horizon is implemented to update the states of the MWMRs.

Set the predictive step length as N_p in order to describe the DMPC scheme; the following state and control input trajectories of MWMR-i within the prediction horizon [k, k + N_p] at timestep k are as follows:

(1)

${\mathit{x}}_i(k)$: the current state trajectories;

(2)

${\mathit{u}}_i(k),{\mathit{u}}_i^{*}(k)$: the predicted and optimal predicted control sequences;

(3)

${\mathit{x}}_i^{*}(k+s|k),{\mathit{u}}_i^{*}(k+s|k)$: the optimal predicted state and control sequences;

(4)

${\widehat{\mathit{x}}}_i(k+s|k),{\widehat{\mathit{u}}}_i(k+s|k)$: the assumed state and control sequences;

(5)

${\mathit{x}}_i(k+s|k),{\mathit{u}}_i(k+s|k)$: the actual predicted state and control sequences;

(6)

${\mathit{p}}_i(k+s|k)$: the predicted position; and

(7)

${\widehat{\mathit{p}}}_i(k+s|k)$: the assumed position;

where $s\in \left[0,N_p-1\right]$ denotes the future prediction timesteps. The assumed control sequence ${\widehat{\mathit{u}}}_j(k+s|k)$, $j\in {\mathcal{N}}_i$ over the prediction horizon $\left[k,k+N_p\right]$ contains two individual parts. The first part is the same as the previous optimal control sequence ${\mathit{u}}_j^{*}(k+s|k-1),j\in {\mathcal{N}}_i$, $s\in \left[0,N_p-1\right)$, and the second part follows the last control input in the optimal control sequence. To be specific, the assumed control sequence ${\widehat{\mathit{u}}}_j(k+s|k)$ can be derived as follows:

$${\widehat{\mathit{u}}}_j(k+s|k)=\{\begin{array}{ll}{\mathit{u}}_j^{*}(k+s|k-1),\hfill & s\in \left[0,N_p-1\right)\hfill \\ {\mathit{u}}_j^{*}(k+N_p-2|k-1),\hfill & s=N_p-1\hfill \end{array}$$

(24)

Thus, the corresponding assumed state trajectories of MWMR-j, $j\in {\mathcal{N}}_i$ can also be computed as:

$${\widehat{\mathit{x}}}_j(k+s+1|k)=f\left({\widehat{\mathit{x}}}_j(k+s|k),{\widehat{u}}_j(k+s|k)\right) s\in \left[0,N_p-1\right]$$

(25)

where ${\widehat{\mathit{x}}}_j(k+s+1|k)$ is the predicted states of neighbor j at timestep k + s + 1. $f(\cdot )$ is the prediction model based on Equation (15).

Based on the assumed control sequence and state trajectory, the DMPC optimization problem for MWMR-i at the timestep $k\ge 0$ is formulated as follows:

$$J_i=\underset{u_i(k+s|k)}{\min }{\displaystyle \sum _{s=0}^{N_p-1}\left(J_i^f({\mathit{x}}_i,{\mathit{u}}_i,{\widehat{\mathit{x}}}_j,{\widehat{\mathit{u}}}_j)+J_i^c({\mathit{x}}_i,{\mathit{u}}_i,{\widehat{\mathit{x}}}_j,{\widehat{\mathit{u}}}_j)+J_i^o({\mathit{x}}_i,{\mathit{u}}_i)\right)}$$

(26)

s.t.

$${\mathit{x}}_i(k+s+1|k)=f({\mathit{x}}_i(k+s|k),{\mathit{u}}_i(k+s|k))$$

(27)

$${\mathit{u}}_i(k+s|k)\in \mathcal{U}$$

(28)

$$\left|\right|{\mathit{x}}_i(k+N_p|k)-{\mathit{x}}_i^d(k+N_p)|{|}_Q^2=0$$

(29)

where Equation (27) is the prediction model based on Equation (15); the allowed control variable space $\mathcal{U}$ is limited within $\left[u_{\min },u_{\max }\right]$; Equation (29) is the terminal constraint to ensure stability. $J_i$ is the distributed cost function for MWMR-i, which includes three parts, given as follows.

(1) The cost function of formation $J_i^f$:

$$J_i^f({\mathit{x}}_i,{\mathit{u}}_i,{\widehat{\mathit{x}}}_j,{\widehat{\mathit{u}}}_j)=\left|\right|{\mathit{x}}_i(k+s+1|k)-{\mathit{x}}_i^d(k+s+1)|{|}_Q^2+\left|\right|{\mathit{u}}_i(k+s|k)|{|}_R^2$$

(30)

where Q and R are the positive definite symmetric matrices. Similar to (19), ${\mathit{x}}_i^d(k+s+1)$ is the desired state at timestep k + s + 1, given as:

$${\mathit{x}}_i^d(k+s+1)=\frac{1}{N_i+d_i}\left({\displaystyle \sum _{j\in {\mathcal{N}}_i}({\widehat{\mathit{x}}}_j(k+s+1)+{\mathit{h}}_i-{\mathit{h}}_j)+d_i({\mathit{x}}_r(k)+{\mathit{h}}_i)}\right)$$

(31)

where ${\mathit{x}}_r(k)={\left[x_r(k),y_r(k),{\theta }_r(k)\right]}^T$ denotes the reference trajectory point at timestep k.

(2) The cost function of collision avoidance $J_i^c$:

$$\begin{array}{l}{J}_i^c({\mathit{x}}_i,{\mathit{u}}_i,{\widehat{\mathit{x}}}_j,{\widehat{\mathit{u}}}_j)\\ =\left(\begin{array}{cc}0,& \left|\right|{\mathit{p}}_i(k+s+1|k)-{\widehat{\mathit{p}}}_j(k+s+1|k)\left|\right|>R_{coll}\\ -K_{coll}\left(\left|\right|{\mathit{p}}_i(k+s+1|k)-{\widehat{\mathit{p}}}_j(k+s+1|k)\left|\right|-R_{coll}\right),& \left|\right|{\mathit{p}}_i(k+s+1|k)-{\widehat{\mathit{p}}}_j(k+s+1|k)\left|\right|\le R_{coll}\end{array}\right)\end{array}$$

(32)

where $K_{coll}$ is the positive penalty gain number of collision avoidance.

(3) The cost function of obstacle avoidance $J_i^o$:

$$J_i^o({\mathit{x}}_i,{\mathit{u}}_i)=\left(\begin{array}{cc}0,& \left|\right|{\mathit{p}}_i(k+s+1|k)-{\mathit{p}}_l^o||>R_{obs}\\ -K_{obs}\left(\left|\right|{\mathit{p}}_i(k+s+1|k)-{\mathit{p}}_l^o||-R_{obs}\right),& \left|\right|{\mathit{p}}_i(k+s+1|k)-{\mathit{p}}_l^o||\le R_{obs}\end{array}\right)$$

(33)

where $K_{obs}$ is a positive penalty gain number of obstacle avoidance.

The schematic diagram of collision avoidance and obstacle avoidance is shown in Figure 4, where R₀ is the radius of the obstacle. Equations (32) and (33) ensure that the distances from each MWMR to the neighbors as well as the obstacles are strictly greater than the critical distance in the entire prediction domain.

Pseudo-code of the designed DMPC scheme is shown as Algorithm 1.

Algorithm 1: Pseudo-code of the DMPC scheme for each MWMR-i, $i\in \mathrm{1,2},\dots ,\mathit{M}$.
1:	// Initialization: at timestep $k=0$
2:	Initialize the state of its own and neighbors: ${\mathit{x}}_i(0),{\mathit{x}}_j(0)$
3:	Set ${\widehat{\mathit{u}}}_i(s|0)=0,{\widehat{\mathit{u}}}_j(s|0)=0,s\in \left[0,N_p-1\right]$ and solve (26)–(29) for MWMR-i, yielding the optimal control sequence ${\mathit{u}}_i^{*}(s|0),s\in \left[0,N_p-1\right]$
4:	Apply the first control input ${\mathit{u}}_i^{*}(0|0)$
5:	Main loop: at any timestep $k>0$
6:	Measure the current state ${\mathit{x}}_i(k)$
7:	Transmit ${\widehat{\mathit{u}}}_i(k+s|k),s\in [0,N_p-1]$ to every neighbor j
8:	Receive ${\widehat{\mathit{u}}}_j(k+s|k),s\in [0,N_p-1]$ from every neighbor j and compute assumed trajectory ${\widehat{\mathit{x}}}_j(k+s+1|k),s\in [0,N_p-1]$
9:	Solve (26)–(29) using Algorithm 2 for MWMR-i, yielding the optimal control sequence ${\mathit{u}}_i^{*}(k+s|k),s\in \left[0,N_p-1\right]$
10:	Apply the first control input ${\mathit{u}}_i^{*}(k|k)$ // Results
11:	Validate the state constraints and terminal conditions

3.2. PSO Optimizer

PSO is a powerful swarm intelligence optimization method which has better global search ability than traditional optimization methods such as the interior point method, sequential quadratic programming (SQP) method, active set method, ADMM method, etc. In our proposed DMPC-based formation controller, PSO is used to serve as the control input optimizer to achieve the optimal control sequences.

The population size of the PSO is set as M_pop. Given the D-dimensional space, each particle i (i = 1,2, …, M_pop) can be represented by a position vector x_i = (x_i,1, x_i,2,…, x_i,D) and a velocity vector v_i = (p_i,1, p_i,2,…, p_i,D). Let Pbest_i = (Pbest_i,1, Pbest_i,2,…, Pbest_i,D) be the personal best position of the particle i, and Gbest = (Gbest₁, Gbest₂,…, Gbest_D) be the global best position of the whole particle population. At the nc-th iteration, particle-i updates its velocity and position according to the current velocity and distance from the personal and global best positions, as follows:

$${\mathit{v}}_i(nc+1)=w\cdot {\mathit{v}}_i(nc)+c_1\cdot r_1\cdot \left({\mathit{Pbest}}_i(nc)-{\mathit{x}}_i(nc)\right)+c_2\cdot r_2\cdot \left(\mathit{Gbest}(nc)-{\mathit{x}}_i(nc)\right)$$

(34)

$${\mathit{x}}_i(nc+1)={\mathit{x}}_i(nc)+{\mathit{v}}_i(nc+1)$$

(35)

where c₁ and c₂ are the cognitive factor and social learning factor; r₁, r₂ are random numbers uniformly distributed within [0, 1]; w is the dynamic inertia weight, which decreases linearly as follows:

$$w=\frac{(w_{\max }-w_{\min })(N{c}_{\max }-nc)}{N{c}_{\max }}+w_{\min }$$

(36)

where w_max and w_min are the maximum and minimum values of the inertia weight and Nc_max is the maximum iteration number.

Moreover, considering the search space range, the position and velocity which are out of the range will be modified as follows:

$$v_{i,j}=\{\begin{array}{cc}{v}_{\max }& \mathrm{if} v_{i,j}>v_{\max }\\ v_{\min }& \mathrm{if} v_{i,j}<v_{\min }\end{array}$$

(37)

$$x_{i,j}=\{\begin{array}{cc}{x}_{\max }& \mathrm{if} x_{i,j}>x_{\max }\\ x_{\min }& \mathrm{if} x_{i,j}<x_{\min }\end{array}$$

(38)

where x_i,j and v_i,j are the position and velocity of particle i in the jth dimension, respectively. x_min and x_max are the lower and upper limits of the particle position; v_min and v_max are the lower and upper limits of the particle velocity.

As is shown in Figure 5, the cost function for each particle i in iteration nc is defined as $J_i(nc)$. The optimal unknown parameters are contained in the position vector that relates to the cost function. Therefore, to solve the DMPC problem using PSO at timestep k, for MWMR-i we just need to initialize prediction control input sequence ${\mathit{u}}_i(k+s|k), s=0,\mathrm{\cdots },N_p-1$ in the prediction horizon $N_p$ randomly with the number equal to the population size $M_{pop}$. The lower and upper bounds of each element in ${\mathit{u}}_i(k+s|k), s=0,\mathrm{\cdots },N_p-1$ are $u_{\min }$ and $u_{\max }$, respectively. Then, by taking each element in ${\mathit{u}}_i(k)$ into the cost function (26) and solving the PSO algorithm until it reaches the maximum number of iterations, the global best position $\mathit{Gbest}(N{c}_{\max })$ is the optimal control input sequence ${\mathit{u}}_i^{*}(k)$. The pseudo-code of the PSO optimizer is listed in Algorithm 2.

Algorithm 2: Pseudo-code of the PSO optimizer in solving the control sequence of MWMR-i

1:      Compute ${\widehat{\mathit{x}}}_j(k+s|k),s\in [0,N_p]$ of its every neighbor j at timestep k

2:    Initialize the parameters in population vector ${\mathit{x}}_i(1)$ and ${\mathit{v}}_i(1)$ randomly within $[u_{\min },u_{\max }]$ and $\left[v_{\min },v_{\max }\right]$ at nc = 1

3:      for $i=1:M_{pop}$ do

4:            Evaluate cost function using Equations (26)–(29)

5:            if $J_i(1)<J({\mathit{Pbest}}_i)$

6:                  ${\mathit{Pbest}}_i\leftarrow {\mathit{x}}_i(1)$ 7:                  $J_i(1)\leftarrow J({\mathit{Pbest}}_i)$

8:            end if

9:      end for

10:    Calculate $\mathit{Gbest}(1)$ at $nc=1$

11:    while $nc<N{c}_{\max }$

12:          nc = nc + 1

13:          for $i=1:M_{pop}$ do

14:                Evaluate cost function using Equations (26)–(29)

15:                if $J_i(nc)<J({\mathit{Pbest}}_i)$

16:                      ${\mathit{Pbest}}_i\leftarrow {\mathit{x}}_i(nc)$ 17:                      $J_i(nc)\leftarrow J({\mathit{Pbest}}_i)$

18:                end if

19:                Determine the current global best position $\mathit{Gbest}(nc)$

20:                Update ${\mathit{v}}_i(nc+1)$ using Equation (34)

21:                Update ${\mathit{x}}_i(nc+1)$ using Equation (35)

22:                for j = 1: D do

23:                      if $x_{i,j}\ge u_{\max }$

24:                            $x_{i,j}\leftarrow u_{\max }$

25:                      end if

26:                      if $x_{i,j}\le u_{\min }$

27:                            $x_{i,j}\leftarrow u_{\min }$

28:                      end if

29:                end for

30:          end for

31:    end while

32:    ${\mathit{u}}_i^{*}(k|k)\leftarrow \mathit{Gbest}(N{c}_{\max })$

3.3. Disturbance Compensation

The model uncertainties and input disturbances are time-varying parameters which cannot be solved well using the ESO or traditional feedback compensation techniques. Therefore, we propose a double closed-loop disturbance feedback compensation controller, which divides the system into an external loop and inner loop through two proportional feedback compensations.

As is shown in Figure 6, due to the uncertain kinematics, the optimal velocity vector in $X_b{O}_b{Y}_b$ satisfying Equation (3) cannot be obtained accurately; therefore, in the external loop, the measured actual state output ${\mathit{x}}_i$ and ideal state output ${\mathit{x}}_i^{*}$ of MWMR-i in the global frame obtained from the DMPC optimization are the input quantities, then the output quantity of the external loop can be obtained as:

$${\dot{\mathit{x}}}_{ib}^{*}=K_1({\mathit{x}}_i-{\mathit{x}}_i^{*})$$

(39)

where ${\dot{\mathit{x}}}_{ib}^{*}$ is the optimal output velocity vector in the local frame. $K_1$ is the feedback gain matrix with a size of 3 × 3. Through the inverse kinematic model without uncertainties, the optimal rotational speed of the four wheels of MWMR-i ${\dot{\mathit{q}}}_i^{*}={\left[{\dot{q}}_{i1}^{*},{\dot{q}}_{i2}^{*},{\dot{q}}_{i3}^{*},{\dot{q}}_{i4}^{*}\right]}^T$ can be obtained as:

$${\dot{\mathit{q}}}_i^{*}=J{\dot{x}}_{ib}^{*}$$

(40)

As for the inner loop, taking the measured actual rotational speed of the four wheels ${\dot{\mathit{q}}}_i$ and optimal rotational speed of the four wheels ${\dot{\mathit{q}}}_i^{*}$ as the input quantities, then the output quantity of the inner loop can be obtained as:

$${\mathit{u}}_c=K_2({\dot{\mathit{q}}}_i-{\dot{\mathit{q}}}_i^{*})$$

(41)

where ${\mathit{u}}_c$ is the compensation for the torque of the four wheels and $K_2$ is a gain matrix with the size of 4 × 4. Therefore, the actual control input of MWMR-i can be obtained as:

$${\mathit{u}}_a={\mathit{u}}_c+{\mathit{u}}_i^{*}(k|k)$$

(42)

In this way, by choosing appropriate values of $K_1$ and $K_2$, the uncertainties and disturbances can be compensated in a real-time manner.

3.4. Stability Analysis

Proposition 1 (Recursive feasibility).:

If (26)–(29) optimization is feasible at initial time k = 0, then it is feasible at any future point in time.

Proposition 2 (Uniform asymptotic stability).:

If communication graph G(t) satisfies Assumption 2 and at least one MWMR can perceive the reference trajectory, then the consensus error of the formation system via DMPC is uniformly asymptotically stable.

Proposition 3 (Collision avoidance guarantee).:

Given any initial cost function value satisfying J_i(0) < g, no inter-robot collisions and collisions with obstacles occur at any future point in time.

Proof of Proposition 1.

Define the error state and optimal error state at timestep k + s, respectively, as follows:

$$\begin{array}{l}{\mathit{e}}_i(k+s|k)={\mathit{x}}_i(k+s|k)-{\mathit{x}}_i^d(k+s|k)\\ {\mathit{e}}_i^{*}(k+s|k)={\mathit{x}}_i^{*}(k+s|k)-{\mathit{x}}_i^d(k+s|k)\end{array}$$

(43)

The optimal control input and error state sequences at time k are chosen by

$$\begin{array}{l}{\mathit{u}}_i^{*}(k)={\left[{\mathit{u}}_i^{*}(k|k),{\mathit{u}}_i^{*}(k+1|k),\mathrm{\cdots },{\mathit{u}}_i^{*}(k+N_p-1|k)\right]}^T\\ {\mathit{e}}_i^{*}(k)={\left[{\mathit{e}}_i^{*}(k|k),{\mathit{e}}_i^{*}(k+1|k),\mathrm{\cdots },{\mathit{e}}_i^{*}(k+N_p-1|k),0\right]}^T\end{array}$$

(44)

The corresponding state and control sequences at k + 1 can be derived as:

$$\begin{array}{l}{\mathit{u}}_i(k+1)={\left[{\mathit{u}}_i(k+1|k+1),{\mathit{u}}_i(k+2|k+1),\mathrm{\cdots },{\mathit{u}}_i(k+N_p-1|k+1),{\mathit{u}}_i(k+N_p|k+1)\right]}^T\\ ={\left[{\mathit{u}}_i^{*}(k+1|k),{\mathit{u}}_i^{*}(k+2|k),\mathrm{\cdots },{\mathit{u}}_i^{*}(k+N_p-1|k),0\right]}^T\end{array}$$

(45)

$$\begin{array}{l}{\mathit{e}}_i(k+1)={\left[{\mathit{e}}_i(k+1|k+1),{\mathit{e}}_i(k+2|k+1),\mathrm{\cdots },{\mathit{e}}_i(k+N_p|k+1),{\mathit{e}}_i(k+N_p+1|k+1)\right]}^T\\ ={\left[{\mathit{e}}_i^{*}(k+1|k),{\mathit{e}}_i^{*}(k+2|k),\mathrm{\cdots },{\mathit{e}}_i^{*}(k+N_p-1|k),0,0\right]}^T\end{array}$$

(46)

The above derivations indicate that there exists at least one set of solutions satisfying all constraints, and the optimization (26)–(29) is feasible at all prediction steps. □

Proof of Proposition 2.

After the terminal equality constraint in (29), we consider the total cost function of all MWMRs $J(k)$, which becomes

$$J(k)={\displaystyle \sum _{i=1}^M[{\displaystyle \sum _{s=0}^{N_p-1}(||{\mathit{e}}_i(k+s|k)|{|}_Q^2+||{\mathit{u}}_i(k+s|k)|{|}_R^2)]}}$$

(47)

Select the candidate for the Lyapunov function $V(k)$ provided via the optimal function $J^{*}(k)$:

$$J^{*}(k)={\displaystyle \sum _{i=1}^M[{\displaystyle \sum _{s=0}^{N_p-1}(||{\mathit{e}}_i^{*}(k+s|k)|{|}_Q^2+||{\mathit{u}}_i^{*}(k+s|k)|{|}_R^2)]}}$$

(48)

The selected Lyapunov function is positive definite and decrescent. Thus, the objective function $J(k+1)$ at timestep k + 1 yields

$$\begin{array}{l}J(k+1)={\displaystyle \sum _{i=1}^M[{\displaystyle \sum _{s=0}^{N_p-1}(||{\mathit{e}}_i(k+s+1|k+1)|{|}_Q^2+||{\mathit{u}}_i(k+s+1|k+1)|{|}_R^2)]}}\\ =J^{*}(k)-{\displaystyle \sum _{i=1}^M}(||{\mathit{e}}_i^{*}(k|k)|{|}_Q^2+||{\mathit{u}}_i^{*}(k|k)|{|}_R^2)\end{array}$$

(49)

and

$$\begin{array}{c}V(k+1)-V(k)=J^{*}(k+1)-J^{*}(k)\le J(k+1)-J^{*}(k)\\ \le -{\displaystyle \sum _{i=1}^M(||{\mathit{e}}_i^{*}(k|k)|{|}_Q^2+||{\mathit{u}}_i^{*}(k|k)|{|}_R^2)}\end{array}$$

(50)

It indicates that the Lyapunov function is decreasing, and uniform asymptotical stability of the formation system is confirmed. On the one hand, G(t) satisfying Assumption 2 indicates that no individual MWMR will be disconnected; on the other hand, the quadratic form indicates that any item in the cost function is greater than or equal to zero, i.e., the cost function containing both trajectory-tracking and formation will asymptotically converge to zero, which means that the MWMRs’ system can achieve formation trajectory-tracking. □

Proof of Proposition 3.

Following (50), without considering the collision avoidance and obstacle avoidance penalty terms, we have $J_i(k+1)<J_i(k)$, $\forall k$ and thus $J_i(k)<g$, $\forall k$ when $J_i(1)<g$. Hence,

$$J_i(k)<g\forall k>0$$

(51)

Considering the penalty terms, the penalty gains $K_{coll}$ and $K_{obs}$ are large enough, $J_i(k)\gg g$, which contradicts Equation (51). Therefore, no inter-robot collision and collisions with obstacles occurs, nor for all future points in time. □

4. Experimental Results

In order to evaluate the effectiveness of our proposed PSO-DMPC-based control method, three simulation scenes for formation trajectory-tracking of the MWMRs considering the various physical constraints are performed in the MATLAB environment. The control effect of PSO-DMPC is compared with the SQP-DMPC and traditional PID controllers. SQP is a typical optimizer that is often used to solve MPC problems in recent research [42,43,44,45], while PID is the typical and widely-used control method [46,47,48]. Each MWMR uses the DC brushless gear motor to drive the four mecanum wheels. The definition and values of the main parameters of the MWMR are shown in

Table 1. Parameter values of the multi-robot formation system.

Definition	Symbol	Value	Unit
Mass of robots	m	5	kg
Sampling period	$t_s$	0.025	s
Radius of wheels	Rw	0.05	m
Moment of inertia of robots	Jz	0.01875	kg·m2
Moment of inertia of wheels	Jw	0.000625	kg·m2
Longitudinal distance of wheels to center	a	0.145	m
Lateral distance of wheels to center	b	0.145	m
Viscous friction coefficient of wheels	${\lambda }_i$	0.2	None
Static friction coefficient of wheels	$\mu$	0.1	None

.

The parameters in the uncertain terms ΔM, ΔJ, and ΔH are given as [a_ij]_3×4, [b_ij]_3×4, [c_ij]_3×4, |a_ij| < 0.1, |b_ij| < 0.1, |c_ij| < 0.1; the uncertain terms of static friction ΔF are given as [d_ij]_4×1, |d_ij| < 0.1, and the unknown time-varying input disturbances are given as ${\mathit{\tau }}_d={[e_{ij}]}_{4\times 1},\left|e_{ij}\right|<0.5$. The detection range of each robot is given as R_det = 5 m. Parameters of the DMPC are set as $N_p=7$, $Q={10}^5{I}_1$, and $R=I_2$. The feedback compensation gain matrixes are chosen as $K_1=-{10}^3{I}_3$, $K_2=-I_4$, where $I_1$, $I_2$, $I_3$, and $I_4$ are identity matrices of appropriate dimensions. The penalty gains of collision avoidance and obstacle avoidance are chosen as $K_{obs}=K_{coll}={10}^7$. The maximum and minimum output torque of the motor are set as u_max = 5 N∙m and u_min = 5 N∙m. The parameters in the PSO optimizer are set as M_pop = 50, Nc_max = 5, w_max = 0.9, w_min = 0.4, c₁ = 2, and c₂ = 2. The parameters of the SQP optimizer are set as the initial guess of ${\mathit{u}}_i^{*}$(k): [u_ij]_28×1, |u_ij| < u_max; the maximum number of iterations I_max = 30; and termination error ϵ = 10⁻²⁰. The parameters of the PID controller are set as: K_p = 15, K_i = 0.1, and K_d = 0.3.

To quantitatively illustrate the average tracking errors of the MWMRs’ formation, the following indices are introduced to evaluate the formation average tracking performance:

$$||e|{|}_{RMS}=\sqrt{\frac{1}{N_t}{\displaystyle \sum _{i=1}^M\left|\right|e|{|}^2}}$$

(52)

where N_t is the total number of sampling steps. $\left|\right|e_x|{|}_{RMS}$, $\left|\right|e_y|{|}_{RMS}$, and $\left|\right|e_{\theta }|{|}_{RMS}$ are average tracking performance along the x and y directions, as well as the heading angle, respectively.

Further, to illustrate the formation performance based on the relative position of MWMRs, the position consensus error is defined as follows:

$$e_h=\frac{1}{M}{\displaystyle \sum _{i=1}^M{\displaystyle \sum _{j\in {\mathcal{N}}_i}\left|\right|{\mathit{p}}_i-{\mathit{p}}_j-({\mathit{p}}_i^d-{\mathit{p}}_j^d)|{|}^2}}$$

(53)

4.1. Example 1 (Straight Line Formation Tracking)

In the first numerical simulation an example is used to test its ability to generate and maintain the desired formation without a change of reference direction using the proposed PSO-DMPC method. The target shape of the formation is designed as a square with a side length of 1 m.

Table 2. Initial states of four MWMRs (Example 1).

MWMR	(x, y) (m)	θ (rad)	(vx, vy) (m/s)	w (rad/s)
MWMR-1	(2, 2)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-2	(2, 3)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-3	(2, 1.5)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-4	(2, 1)	$\pi /3$	(1, 1)	$\pi /4$

shows the initial states of four MWMRs.

Table 3. Parameter settings of obstacles (Example 1).

Obstacle	Position (m)	Ro (m)
Obstacle-1	(5.5, 3.5)	1
Obstacle-2	(8, 10)	1

shows the parameter settings of the obstacles. The reference trajectory x_r is assumed to be available only to MWMR-1, and the desired line trajectory is given as:

$$\{\begin{array}{ll}{x}_r=2+t\hfill & \hfill \\ y_r=2+t\hfill & \hfill \\ {\theta }_r=\pi /4\hfill & \hfill \end{array}$$

(54)

The formation trajectory-tracking results of example 1 using the traditional PID controller, the SQP-DMPC controller and PSO-DMPC controller in the x–y plane are presented in Figure 7. It can be seen that by implementing the PSO-DMPC, the formation is restored immediately after the obstacle avoidance is completed, while the SQP-DMPC and PID controllers have a transition process before restoring the initial formation. This is because PSO has a better processing ability for nonlinear obstacle avoidance and collision avoidance constraints with input limitation and has a stronger ability to find the global optimal solution. The relative distances of the three methods are presented in Figure 8. It can be seen from Figure 8 that the PSO-DMPC has the smallest mutual distance fluctuations when avoiding obstacles, which further shows that the quality of the optimal solution obtained via PSO is the best, and the deformation of the formation can be effectively reduced. The tracking performance along the x and y directions are shown in Figure 9a,b, respectively, and Figure 9c presents the heading angle tracking results of the MWMRs. It can be seen that the DMPC method has good tracking performance but that the PID controller has large steady-state tracking errors in the x and y coordinates. This is because the recede horizon optimization of the DMPC has a stronger ability to predict and correct nonlinear systems than the PID controller. The PSO-DMPC has a faster convergence rate than the SQP-DMPC and PID controllers. The RMS errors are show in

Table 4. RMS values of tracking error for example 1. (unit: m).

RMS Value	PID	SQP-MPC	PSO-MPC
$\left|\right|e_x|{|}_{RMS}$	0.1166	0.1200	0.0883
$\left|\right|e_y|{|}_{RMS}$	0.1448	0.1102	0.0845
$\left|\right|e_{\theta }|{|}_{RMS}$	0.0145	0.0103	0.0125

. It can be observed that both the tracking error of the position and heading angle of the PSO-DMPC have the highest precision. The comparison of position consensus errors is presented in Figure 10. It can be observed that position consensus errors of the three methods converge to zero after about 2.5 s, and when avoiding obstacles, the PSO-DMPC have smaller consensus errors and faster formation convergence rates than the SQP-DMPC and PID controllers. The speeds of the MWMRs are shown in Figure 11.

4.2. Example 2 (Circle Line Formation Tracking)

In this example, a numerical simulation is performed further to test the ability to generate and maintain the desired formation with the time-varying heading angle using the proposed PSO-DMPC method. The desired formation shape is designed as an equilateral triangle with a side length of 1 m.

Table 5. Initial states of three MWMRs (Example 2).

MWMR	(x, y) (m)	θ (rad)	(vx, vy) (m/s)	w (rad/s)
MWMR-1	(12.5, 7.5)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-2	(12.5, 8)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-3	(12.5, 9)	$\pi /3$	(1, 1)	$\pi /4$

shows the initial states of three MWMRs.

Table 6. Parameter settings of the obstacle (Example 2).

Obstacle	Position (m)	Ro (m)
Obstacle-1	(7, 11.5)	1

shows the obstacle settings. The equation of the desired circle trajectory is given as:

$$\{\begin{array}{l}{x}_r=7.5+5\cos 0.035t\\ y_r=7.5+5\sin 0.035t\\ {\theta }_r=\pi /2+0.035t\end{array}$$

(55)

Figure 12 shows the formation trajectory-tracking results of the PSO-DMPC, SQP-DMPC, and traditional PID controller of example 2, and the relative distances of the MWMRs are shown in Figure 13. From Figure 12 and Figure 13, it can be seen that when the critical distance to the obstacle is reached, the formation deformations of the two DMPC methods are quite small, while the PID controller has a large formation deformation due to the insufficient ability to deal with constraints and disturbances. When PSO-DMPC ends the obstacle avoidance process, the SQP-DMPC and PID controllers still have larger formation deformation due to the insufficient ability to find global optimal solutions. The tracking performance along the x and y direction and the heading angle are shown in Figure 14a–c, respectively. It can be seen that the DMPC–based controller has a bigger tracking error when avoiding obstacles than the PID controllers but has a smaller tracking error during the formation maintaining process. To further illustrate the tracking performance, the RMS errors are show in

Table 7. RMS values of tracking error for example 2. (unit: m).

RMS Value	PID	SQP-MPC	PSO-MPC
$\left|\right|e_x|{|}_{RMS}$	0.0513	0.0673	0.0408
$\left|\right|e_y|{|}_{RMS}$	0.0460	0.0575	0.0458
$\left|\right|e_{\theta }|{|}_{RMS}$	0.1135	0.0922	0.0881

. It can be observed that both the tracking error of the position and heading angle of the PSO-DMPC have the highest precision. The comparison of the position consensus errors is presented in Figure 15. It can be observed that all three methods have the capability of generating and maintaining the desired formation swiftly, and when avoiding obstacles, the PSO-DMPC has a smaller position consensus error and faster formation convergence rate than the SQP-DMPC and PID controllers. In Figure 16, the speeds of the MWMRs are shown.

4.3. Example 3 (Curve Formation Tracking with Formation Reconstruction)

Example 3 is performed to test the formation maintenance and reconstruction ability of our proposed PSO-DMPC method. The desired trajectory is an irregular segmented trajectory containing two semicircles. The desired formation shape to be maintained is designed as an isosceles triangle with a bottom length of 2 m and a height length of 1 m. The desired formation shape to be reconstructed is designed as a rectangle with both the length and width as 1 m.

Table 8. Initial states of six MWMRs (Example 3).

MWMR	(x, y) (m)	θ (rad)	(vx, vy) (m/s)	w (rad/s)
MWMR-1	(3, 13)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-2	(2.5, 13.5)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-3	(2.5, 12.5)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-4	(2, 14)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-5	(2, 13)	$\pi /3$	(1, 1)	$\pi /4$
MWMR-6	(2, 12)	$\pi /3$	(1, 1)	$\pi /4$

shows the initial states of six MWMRs.

Table 9. Parameter settings of the obstacles (Example 3).

Obstacle	Position (m)	Length (m)	Width (m)
Obstacle-1	(5.3, 4)	1	5
Obstacle-2	(8.3, 9)	4	1

shows the parameter settings of the obstacles.

Figure 17 shows the formation trajectory-tracking results of the PSO-DMPC, SQP-DMPC, and traditional PID controller of example 3; we can see that the six MWMRs have good performance in generating, maintaining the desired formation, and tracking the desired trajectory. Figure 18 shows the relative distances between each of the two MWMRs in the formation. It can be found that the distances can maintain the desired initial values, and no collision between MWMRs occurs. Figure 19a–c shows the tracking performance along the x and y direction and the heading angle, respectively. The comparison of the position consensus errors is presented in Figure 20. From Figure 19 and Figure 20, it can be seen that the three methods perform equally well. Therefore, to further illustrate the tracking performance, the RMS errors are show in

Table 10. RMS values of tracking error for example 3. (unit: m).

RMS Value	PID	SQP-MPC	PSO-MPC
$\left|\right|e_x|{|}_{RMS}$	0.1536	0.1552	0.1531
$\left|\right|e_y|{|}_{RMS}$	0.0974	0.1019	0.0989
$\left|\right|e_{\theta }|{|}_{RMS}$	0.1199	0.1208	0.1188

. It can be observed that although the tracking error in the y direction of the PSO-DMPC controller is slightly less than that of the PID controller, both the tracking error of the x direction and heading angle of the PSO-DMPC have the highest precision. In Figure 21 the speeds of the MWMRs are shown.

From the above simulation results, it can be concluded that the proposed PSO-DMPC controller has the better and more robust capability in terms of convergence time, tracking error, and consensus error than both the SQP-DMPC and PID controllers. Moreover, the simulation results also reflect the adaptability of the PSO-DMPC, which makes the designed closed-loop compensator have much better adaptive ability for the MWMRs in the presence of uncertainties and external disturbances. Therefore, the PSO-DMPC control method is effective and feasible for formation trajectory-tracking control of MWMRs with physical constraints and collision/obstacle avoidance simultaneously.

5. Conclusions

In this paper, the collision-free trajectory tracking formation control problem for multiple MWMRs under complexed obstacle environments via the DMPC scheme and real-time disturbance compensation is studied. The formation model for MWMRs is developed using a consensus strategy with the integrated kinematics and dynamics under the leader–follower framework. The DMPC scheme for the MWMRs’ formation is constructed, in which prediction information are shared among neighbors. Collision avoidance and obstacle avoidance are guaranteed by adding the penalty into the optimal control cost function. The constrained optimal control problems are solved online by the PSO algorithm. Then, the double closed-loop disturbance compensation controller is introduced to achieve real-time compensation for uncertainties and disturbances. Simulation results show that our proposed method can remove the effects of external disturbances and model uncertainties and achieve the formation trajectory-tracking with obstacle avoidance and collision avoidance accurately. Comparative studies indicate that the designed PSO-DMPC controller has better formation performance than the SQP-DMPC controller and the PID controller. Our future work will focus on the cooperative formation reconfiguration, including split and merge, under the unknown dynamic environment. In addition, the formation recursive self-healing and cooperative intent recognition under time-varying communication topology are also our following research interests.