Cooperative Following of Multiple Autonomous Robots Based on Consensus Estimation

Abstract

When performing a specific task, a Multi-Agent System (MAS) not only needs to coordinate the whole formation but also needs to coordinate the dynamic relationship among all the agents, which means judging and adjusting their positions in the formation according to their location, velocity, surrounding obstacles and other information to accomplish specific tasks. This paper devises an integral separation feedback method for a single-agent control with a developed robot motion model; then, an enhanced strategy incorporating the dynamic information of the leader robot is proposed for further improvement. On this basis, a method of combining second-order formation control with path planning is proposed for multiple-agents following control, which uses the system dynamic of one agent and the Laplacian matrix to generate the consensus protocol. Due to a second-order consensus, the agents exchange information according to a pre-specified communication digraph and keep in a certain following formation. Moreover, an improved path planning method using an artificial potential field is developed to guide the MAS to reach the destination and avoid collisions. The effectiveness of the proposed approach is verified with simulation results in different scenarios.

1. Introduction

Robot formation control technology [1] studies the technology of motion control ideas and strategies for Multi-Agent Systems (MAS) to perform specific tasks. When multi-robots form a formation, each robot can be regarded as an agent; thus, the research on formation control is generally based on the coordinated control structure of the MAS [2,3] or the multi-robot control structure [4]. On the premise of coordinating the whole formation, the MAS also need to coordinate and deal with the dynamic relationship between all agents and judge and adjust their positions in the formation according to their position information, speed information and obstacle information [5,6,7,8,9,10,11,12]. Formation control involves making the actual state value of the agent gradually tend to the expected control value through control, which achieves the effect of convergence and stability. Relevant research mainly focuses on two aspects: one is the design of a specific control structure, and the other is the theoretical research on synergy and consensus.

Recently, the navigation and formation of MAS in unknown environments [13,14] have been developed, which aim to achieve consensus and cooperation in networked MAS and mainly focus on its consensus [15,16]. Above all, the second-order consensus in multi-agent dynamical systems is based on some necessary and sufficient conditions [17,18,19]. We need to invite a specific control structure to decide on the input control quantity (velocity and angular velocity, etc.), and most work is based on PID [20,21,22,23,24,25]. Meanwhile, for the unknown environment [26,27,28,29,30,31], we need to pay attention to the obstacle avoidance performance from the system level and guide each agent, respectively. There are many methods to realize obstacle avoidance for a single agent, and we decide to transplant similar ideas into consensus estimation following.

According to the different formation-constrained information, the formation of MAS can be divided into types based on the relative position, distance, direction, etc. In recent years, many scholars have conducted a lot of research on formation problems from the perspective of consensus based on relative position information, and they achieved a lot. The final convergence proof of relative position-based information can be transformed into a problem of consensus. However, the distance-based formation cannot be transformed into the problem of consensus because of the distance constraints between the agents, and the uniqueness of the formation needs to be considered. The formation based on distance does not restrict the direction of formation, which improves the flexibility of formation engineering applications. For example, when multiple robots carry heavy objects together, they only need to constrain the specific positions between the robots, without constraining the relative positions between the robots. The formation problem based on distance or direction information is more complex, which essentially uses some information of the relative position, that is, the norm of the relative position vector. Even if a single-agent model is a linear system, the nonlinearity of the formation constraint information will lead to the nonlinearity of the MAS. However, because of its fewer requirements for sensors and its easier implementation in engineering, it has aroused the research interest of scholars and has led the exploration of the problem of distance formation, but only the formation problem is considered, and speed tracking is not considered.

To keep a certain formation, the followers should know the positions of the leader, and the followers must maintain a relative distance from and bearing angle to the leader $\left(d^{ref}, {\phi }^{ref}\right)$. Besides that, the follower must know the linear and angular velocities for the formation control to minimize the position deviation and keep a stable formation. On this basis, this paper makes a further expansion. Taking square formation as an example, aiming at the multi-agent follow navigation strategy, this paper studies the problems of formation maintenance and speed tracking at the same time using consensus estimation. The research conducted in this paper can provide a theoretical reference for engineering problems such as multi-robot transporting and cooperative cruises. The strategy of consensus estimation can improve the flexibility of formation engineering applications. The main contributions of this article are summarized as follows: (1) an enhanced integral separation feedback following strategy incorporating the dynamic information of the leader is devised to realize the robust leader–follower control; (2) an information consensus-based formation control framework is proposed to address the more complex control problem of MAS.

The rest of this work is organized as follows: Section 2 depicts the leader–follower model designs to simplify the simulation; Section 3 illustrates the mobile robots following a control algorithm; an information consensus-based formation control strategy is elaborated on in Section 4; concluding remarks are given in Section 5.

2. Robot Motion Model

This paper uses differentially driven wheeled mobile robots, with a universal wheel equipped in the front of the chassis and two drive wheels at the rear, as shown in Figure 1. Several identical robots are used to form the MAS.

The horizontal motion of this robot model can be expressed as:

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\alpha }\end{array}\right]=\left[\begin{array}{cc}cos\alpha & 0\\ sin\alpha & 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ w\end{array}\right]$$

(1)

where $x$ and $y$ stand for the robot’s longitudinal and lateral positions; $\alpha$ stands for the robot’s heading; $v$ and $w$ stand for the linear and angular velocities of the robot, respectively. The relationship between $v, w$ and the speed of the two drive wheels (without sliding) is

$$\left[\begin{array}{c}v\\ w\end{array}\right]=\left[\begin{array}{cc}1/2& 1/2\\ 1/b& -1/b\end{array}\right]\left[\begin{array}{c}{v}_r\\ v_l\end{array}\right]=\left[\begin{array}{cc}1/2& 1/2\\ 1/b& -1/b\end{array}\right]\left[\begin{array}{c}{\beta }_r{N}_r\\ {\beta }_l{N}_l\end{array}\right]$$

(2)

where $b$ represents the distance between the left and right wheels, $v_r$ and $v_l$ represent the left and right wheel speeds, respectively, $N_r$ and $N_l$ represent the number of pulses of the encoder per unit time and ${\beta }_r$ and ${\beta }_l$ represent the corresponding speed conversion coefficients.

The turning radius of the robot under this model can be expressed as:

$$R=\frac{b{v}_r}{v_r-v_l}-\frac{b}{2}.$$

(3)

when $v_l=v_r$, the turning radius tends to be ∞, and the robot travels along a straight line; when $v_l=-v_r$, the turning radius is 0, and the robot turns around in situ.

3. Single-Agent Following Control Strategy

The autonomous target following of intelligent vehicles means processing the target information and the information obtained by the agent’s sensors at the same time and driving the corresponding actuators (in this paper, two servos of the two-wheel differential robot, mainly) to reach and stabilize at a fixed position around the target. Based on the ROS (Robot Operating System) routine turtle following case, the main implementation process of the following control algorithm in this paper selects the controlling method and gradually optimizes the control algorithm to realize a continuous and stable formation control.

3.1. Leader–Follower Formation Control

Two intelligent vehicles are taken as an example with the following system framework (Figure 2).

The host node acts as the data center in charge of data storage and most of the data processing. The laser data of the leader and follower vehicles are matched in the map library of the host node and fused with their respective motion information to obtain an accurate location which will be sent to their respective nodes; in addition, the follower node needs to subscribe to the position information of the master vehicle for following. The host and follower nodes are respectively responsible for publishing corresponding motion control commands to keep the gap between the leader and follower vehicle to a given value, forming a fixed following formation. The follower calculates its velocity command based on its relative position to the leader, including both angular and linear velocity.

$${\omega }_z=k_1{\tan }^{-1}\frac{y}{x},$$

(4)

$$v_x=k_2·\sqrt{x^2+y^2}.$$

(5)

This coordinate system is established with the follower as the origin and the leader as the reference of $\left(x,y\right)$. The control coefficients of angular and linear velocity in the turtle routine are $k_1=4,k_2=0.5$. Given that the leader travels at a fixed speed, the inverse tangent calculation, based on the angular velocity, will gradually align the follower with the leader. When the follower is farther away from the leader, the calculated linear velocity $v_x$ will be greater than the leader’s speed and less than the leader’s speed when they are closer than a certain distance.

3.2. Basic Following Control Strategy

A MATLAB simulation of the two-vehicle following model is carried out with the following point set to the leading vehicle itself and the leading vehicle located at $\left(0,0\right)$ initially, heading in the direction of $\frac{\pi }{2}$ and continuing to move at the speed of 30 m/s. The following vehicle is located at$\left(20,0\right),$ with an orientation angle of $\frac{\pi }{2}$, and is stopped there initially. The simulation results (Experiment 1) of the following process are shown in Figure 3 and

Table 1. Simulation results (angular velocity control coefficient $k_1$ = 4.0, initial distance between the two vehicles = 20 m, facing the same direction).

Main Vehicle Speed V (m/s)	Linear Speed Control Coefficient ${\mathit{k}}_2$	Settling Distance (m)	Settling Time t (s)
30	0.25	119.9800	34.75
30	0.5	59.9900	17.34
30	1	29.9950	8.62
30	2	14.9975	4.24
15	0.5	29.9900	15.94

, which show that the final formation configuration is not only determined by the speed control coefficient $k_2$ but is also related to the speed of the leading vehicle, and the final form is not stable, i.e., the settling distance is uncontrollable in the event that the leader’s speed is variable.

It can be seen that the coefficient$k_2$ in Equation (6) proportionally controls the deviation signal

$$e_d=\sqrt{x^2+y^2}.$$

(6)

when deviations occur, the following control model immediately serves as a controller to reduce the deviation, which means it only regulates through proportional control, so the deviations remain after stability. Thus, the PID control is introduced to eliminate the steady-state error and align the stationary point of the follower with the preset following point with integral factors and a speed up system response with differential factors.

3.3. Integral Separation Feedback Following Control Strategy

The conventional PID control algorithm consists of three parts: proportional, integral and differential elements, where the actual output value c(t) deviates from the given value r(t) as $e\left(t\right)=r\left(t\right)-c\left(t\right)$. Considering the overshoot or even oscillation resulting from the accumulation of deviant integrals of PID operation caused by the large inconsensus of the systemic output at the very beginning of this part, this paper adopts the integral separation method to improve system stability and control accuracy. The integral control is activated only when the absolute value of deviation is less than a predetermined threshold value $\mathsf{\epsilon }$ in order to ensure the achievement of error tracking. To meet the needs of the simulation, the PID is discretized incrementally, and its integral separation control algorithm can be expressed as:

$$u\left(k\right)=K_{p}e\left(k\right)+T\cdot \beta k_i{{\displaystyle \sum }}_{j=0}^{k}e\left(j\right)+k_d\frac{e\left(k\right)-e\left(k-1\right)}{T},$$

(7)

$$\beta =\left\{\begin{array}{c} 1 \left|e\left(k\right)\right|\le \epsilon \\ 0 \left|e\left(k\right)\right|>\epsilon \end{array}\right.$$

(8)

where $T$ represents the sampling time, and $\beta$ represents the switching coefficient of the integral item. The corresponding process is shown in Figure 4.

The control variables of the intelligent vehicle in this paper are the motion speed $v_x$ of the longitudinal loop control variable and the steering speed $w_z$ of the lateral loop control variable. It is the longitudinal loop control variable that leads to the steady-state error, so the improvement of the control principle can be achieved by adjusting $v_x$. The integral separation PID control is thus introduced, and the calculation of the linear speed is modified as follows:

$$v_x=K_p{e}_d\left(k\right)+T\cdot \beta k_i{{\displaystyle \sum }}_{j=0}^k{e}_d\left(j\right)+k_d\frac{e_d\left(k\right)-e_d\left(k-1\right)}{T}$$

(9)

Under the initial conditions of Experiment 1, adjusting the formation to ensure a stable following process at a relatively quick speed, set $K_p=2$,$k_i=1$,$k_d=0.2$, $\epsilon$= 3 m and the sampling time T = 0.01 s; the simulation results (Experiment 2) are shown in Figure 5.

Compared to Figure 3, with only proportional factors, the above-shown simulation results use the integral separation PID control strategy, which shows that the follower has been nearly settling at 2.5 s to 3 s, which is significantly faster than 4.24 s, and steady-state errors are eliminated to reach the desired position. The integral separation PID control strategy chooses to introduce the integral action only when the deviation is less than a certain value. This ensures no deviation tracking and reduced overshoot.

For the convenience of observation, the following point is set at 1 m to the right of the leader, and the simulation results are obtained in combination with the DWA (Dynamic-Window Approach) [32] local obstacle avoidance algorithm, as shown in Figure 6. It indicates that the two vehicles can form a stable formation when there is no obstacle, avoid obstacles separately when they exist on the predicted trajectory and finally reach the destination in the form of formation.

3.4. Enhanced Following Control with Dynamic Information

The problem in the following control can be divided into two parts: the global control part determined by the exact following control model and the local control part that eliminates deviations and achieves stability. The following control model determined by Equations (4) and (5) can achieve real-time tracking of the leader with the help of distance and angle information, but if the formation is to be maintained, it can only rely on stable and stepwise PID control to narrow the follower’s position deviation to zero. Such a control method is not optimal because the velocity information of the leader is not considered in the following model, which results in a certain lag.

In this work, we propose taking the sensed dynamic leader’s speed, distance and angle information into consideration, as shown in Figure 7.

$d^{ref}\in \left(0,+\infty \right)$ and${\phi }^{ref}\in \left(-\frac{\pi }{2},\frac{\pi }{2}\right)$ are the expected relative distance and bearing angle of the following process of the two-vehicles scenario. If the follower $i$ wants to follow the leader $j$, whose motion speed is $v_j$, the bearing angle is ${\beta }_{ji}={\theta }_j-{\theta }_i$; at a desired settling position ${\left(d_{ji}^{ref},{\phi }_{ji}^{ref}\right)}_{ }$, the following control model is given as:

$$v_i=v_j\frac{\cos \left({\beta }_{ji}-{\phi }_{ji}^{ref}\right)}{cos{\phi }_{ji}^{ref}},$$

(10)

$${\omega }_i=v_j\frac{sin{\beta }_{ji}}{d_{ji}^{ref}cos{\phi }_{ji}^{ref}}.$$

(11)

The following control model determined by the equation above is stable [1], and further design will be made to decrease the position deviation of the follower to zero. The associated deviation is defined in Figure 8, as shown below.

$$E_{\tau }=d_{ji}cos{\phi }_{ji}-d_{ji}^{ref}cos{\phi }_{ji}^{ref},$$

(12)

$$E_v=d_{ji}sin{\phi }_{ji}-d_{ji}^{ref}sin{\phi }_{ji}^{ref}.$$

(13)

The following control model is modified after taking these errors into consideration.

$$v_i=v_j\frac{\cos \left({\beta }_{ji}-{\phi }_{ji}^{ref}\right)}{cos{\phi }_{ji}^{ref}}+E_{\tau }{K}_v$$

(14)

$${\omega }_i=v_j\frac{sin{\beta }_{ji}}{d_{ji}^{ref}cos{\phi }_{ji}^{ref}}+E_v{K}_{\omega }.$$

(15)

Set the desired following position ${\left(d_{ji}^{ref},{\phi }_{ji}^{ref}\right)}_{ }$ to ${\left(2 m,0^{°}\right)}_{ }$, which is 2 m away from the leading vehicle on the right, and the leading vehicle is initially located at (0,0) with the direction of $\frac{\pi }{2}$ and the speed of 4 m/s. The following vehicle, located at (4,0), follows the leader with an angle direction of $\frac{\pi }{2}$. The simulation results (experiment 3) after the minor adjustment of the parameters are shown in Figure 9 and Figure 10. The formation distance and bearing angle are also recorded to show the improvements compared with the PID following control.

If the PID model from Experiment 2 is used, assume that the following process is under the same conditions as those in Experiment 3, as shown in Figure 11 and Figure 12.

The total error E(m) is specified as a measure of the following effect and is calculated as follows.

$$E=\frac{{{\displaystyle \sum }}_{k=1}^N{E}_k}{N},$$

(16)

$$E_k=\sqrt{E_{\tau }^2+E_v^2}.$$

(17)

N = 500 represents how many times the simulation runs, and it is when it has run 500 times (N = 500) that a stable following has been achieved. The optimized following control model is proven to be more effective, as the total error E during the simulation of experiment 3 is 0.3334 m, which is 57.6% lower than that of experiment 2, conducted under the same initial conditions, 0.7863 m.

When there are only two agents, one leader and one follower, the follower can achieve unbiased and stable tracking at the desired position. When the number of followers is increased, although the desired following positions of the remaining agents can be set around the leader to coordinate the whole formation, such a top-down control method with only a single reference point fails to coordinate the dynamic relationships among all agents, which means that, during the formation process, followers cannot take advantage of the local information of their adjacent agents, largely reducing the efficiency of the formation of the multi-agents.

4. Information Consensus-based Formation Control Strategy

The consensus algorithms can not only orchestrate the whole formation of the MAS but also coordinate the dynamic relationships among all the agents. In the study of multi-agent systems of consensus, the main focus is on aligning the state of each intelligent body, i.e., coherence needs to be achieved. In order to initially solve the problem of the coordination and collaboration of multiple robots, this section uses the consensus algorithm for formation control to achieve a more efficient multi-robot formation control compared to the control strategy that only uses PID.

4.1. Principle of Information Consensus

Consensus theory can describe MAS formation systems concisely and effectively, and the study of related problems requires graph theory tools, which investigate the representation, transformation and properties of graphs through linear algebraic theory, etc. Therefore, the knowledge of graph theory and matrix theory and the basic concepts of the consensus system and its stability will be introduced at first.

4.1.1. Graph Theory

Assume $G=\left(V,E\right)$ is the Nth-order directed graph used to represent the communication topology graph of an MAS, where $V=\left\{ v_1,v_2,\dots ,v_N \right\}$ represents the set of nodes. The directed edges in this network $e_{ij}$ are represented by the ordered pairs of nodes $(v_i,v_j)$, with the set of directed edges being $E\subset V\times V$. $\mathrm{deg}\left(v_i\right)$ represents the degrees of the node $v_i$, which are divided into outdegree and indegree.

A directed graph contains a directed spanning tree if and only if its sub-graph is a directed spanning tree.

The degree matrix $D\in R^N$ is a diagonal matrix that represents the number of links connected to each node or vertex:

$$D_{ij}=\left\{\begin{array}{c}\mathrm{deg}\left(v_i\right) ,i=j\\ 0 ,i\ne j\end{array}\right..$$

(18)

The adjacency matrix A $\in R^N$ is a square matrix that represents the information flow or connectivity among different nodes:

$$A=\left(\begin{array}{ccc}{a}_{11}& \cdots & a_{1n}\\ ⋮& \ddots & ⋮\\ a_{n1}& \cdots & a_{nn}\end{array}\right),$$

(19)

$$a_{ij}=\left\{\begin{array}{c} k ,(v_i,v_j)\in E\\ 0 ,(v_i,v_j)\notin E\end{array}\right..$$

(20)

$a_{ij}$ represents the weight of the edge $(v_i,v_j)$. If the node $v_i$ can receive the information of the node $v_j$, then $a_{ij}>0$, representing the weight of the edge; otherwise, $a_{ij}=0$. When k is not constantly 1, such a graph is called a weighted graph, and A serves as a weighted adjacency matrix.

The Laplacian matrix, $L=D-A$, is also a matrix used to describe the topology of graph G.

4.1.2. Second-Order Consensus Systems

There is a system containing n agents, where the dynamic model for each of them can be represented by the following equation.

$$\left\{\begin{array}{c}{\dot{x}}_i=v_i \\ {\dot{v}}_i=u_i \end{array}\right..$$

(21)

where $x_i\in R^m$ denotes the information state of the intelligent agent $i$, which can include m state information including position, velocity, etc., and $v_i\in R^m$ represents the derivative of the information state of agent $i$, i.e., the rate of change, while $u_i\in R^m$ is set as the control principle imposed by $i$.

In this second-order multi-agent system, the basic control law is designed as follows.

$$u_i=-\sum a_{ij}\left[\left(x_i-x_j\right)+\left(v_i-v_j\right)\right].$$

(22)

The stability of the system depends on the positive integer $\eta$. In the vehicle formation model, the $x_i,v_i,u_i$ can be used directly to represent the position, velocity and thrust given to the ith vehicle.

Set $x={[x_1,x_2,\dots ,x_n]}^T$ and $v={[v_1,v_2,\dots ,v_n]}^T$ The closed-loop system equation for this model under the control law is as follows. $\otimes$is cross-product symbol.

$$\left[\begin{array}{c}\dot{x}\\ \dot{v}\end{array}\right]=\left[\left(\begin{array}{cc}{0}_{n\times n}& I_n\\ -L& -\mathsf{\eta }L\end{array}\right)\otimes I_m\right]\left[\begin{array}{c}x\\ v\end{array}\right].$$

(23)

4.2. Formation Control for a Second-Order System and Improved Path Planning

In the model of vehicle formation, take $\eta =1$ and ensure that the system is stable [2], as follows.

$$u\left(t\right)=-Lx\left(t\right)-Lv\left(t\right)$$

(24)

For a consensus protocol, the following condition must be satisfied, i.e., all agents must converge to the same state as their neighboring agents. All $i,j=1,2,3\dots N$ comply with:

$$\underset{t\to \infty }{\lim }\left|\left|x_i\left(t\right)-x_j\left(t\right)\right|\right|=0,$$

(25)

$$\underset{t\to \infty }{\lim }\left|\left|v_i\left(t\right)-v_j\left(t\right)\right|\right|=0.$$

(26)

The second-order consensus protocol can be written as follows.

$$\left\{\begin{array}{c}{\dot{x}}_i\left(t\right)=v_i\left(t\right)\\ {\dot{v}}_i\left(t\right)=-\alpha {\displaystyle {\displaystyle \sum }_{j=1}^n}{L}_{ij}{x}_j\left(t\right)-\beta {\displaystyle {\displaystyle \sum }_{j=1}^n}{L}_{ij}{v}_j\left(t\right)\\ i=1,2,\dots N\end{array}\right.$$

(27)

Let $y={(x^T,v^T)}^T$, and put it in the form of a composite matrix:

$$\dot{y}\left(t\right)=\left(L\otimes I_n\right)y$$

(28)

4.2.1. Establish a Continuous-Time Model for Formation Control

Design the same dynamic model for N vehicles in a formation.

$${\dot{x}}_i=A{x}_i+B{u}_i ,i=1,2\dots N,x_i\in R^{2n}$$

(29)

Vector $x_i$ is used to store the N state parameters of these vehicles, and vector $u_i$ stores the input quantities of control. For the sake of simplicity, in the subsequent simulations, let $A=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]$, $B=I_n\otimes \left(\begin{array}{c}0\\ 1\end{array}\right)$, which means that, in this dynamic system, the position is only determined by the velocity, which only depends on the input of control.

To control the velocity and acceleration separately, let$x_p={({\left(x_p\right)}_1,\dots ,{\left(x_p\right)}_N)}^T$, $x_v={({\left(x_v\right)}_1,\dots ,{\left(x_v\right)}_N)}^T$ to represent the position and velocity of vector$x$:

$$x=x_p\otimes \left(\begin{array}{c}1\\ 0\end{array}\right)+x_v\otimes \left(\begin{array}{c}0\\ 1\end{array}\right)$$

(30)

Define the formation vector $h=h_p\otimes \left(\begin{array}{c}1\\ 0\end{array}\right)\in R^{2nN}$, the vector of the N vehicle in the formation $h: q,w\in R^n$, to represent ${\left(x_p\right)}_i\left(t\right)-{\left(h_p\right)}_i=q$, ${\left(x_p\right)}_i\left(t\right)=w$. Assume that all trolleys establish communication between each other. The Laplace matrix is used to represent the way the small vehicles are linked by communication, and $J_i$ is defined as the set of neighbors of the ith vehicle. To further simplify, control $u_i$ to utilize the feedback control law, i.e., each vehicle can use relative information about its neighbors.

Combining the relevant information to define the output function $y_i$, an equation is obtained by calculating the average of the relative displacements and velocities of adjacent vehicles, as shown below.

$$y_i=\left(x_i-h_i\right)-\frac{1}{\left|J_i\right|}{{\displaystyle \sum }}_{j\in J_i}\left(x_j-h_j\right),i=1,\dots ,N.$$

(31)

Since it may happen that the vehicles do not receive the information, some modifications are still needed. Such a case might happen if one of the vehicles is the designated leader and the other vehicles should adjust their respective movements. Define the output function $z_i$:

$$z_i={\displaystyle \sum }_{j\in J_i}\left(\left(x_i-h_i\right)-\left(x_j-h_j\right)\right),i=1,\dots ,N.$$

(32)

The corresponding output vector can be written as $z=L\left(x-h\right)$, where $L=L_G\otimes I_{2n}. L_G$ is the Laplace matrix of the directed formation graph G. When grouping all equations into one system, there are:

$$\left\{\begin{array}{c}\dot{x}=Ac\ast x+Bc\ast u\\ z=L\left(x-h\right)\end{array}\right.,$$

(33)

where $Ac=I_N\otimes A$, $Bc=I_N\otimes B$. The overall dynamics of the formation in continuous time are modeled as:

$$\dot{x}=Ac\ast x+Bc\ast L\left(x-h\right).$$

(34)

4.2.2. Digitization

For simulation and practical use, a discretization of the continuous-time state space is required. Since $\dot{x}$ is the differential state of the current state of all carts, the differential equation can only be solved by using the ODE45 function in MATLAB. This is a continuous equation that is difficult to track and apply to real cases. Suppose that a continuous-time state-space system is obtained.

$$\left\{\begin{array}{c}\dot{x}\left(t\right)=Ax\left(t\right)+Bu\left(t\right)\\ y\left(t\right)=Cx\left(t\right)+Du\left(t\right)\end{array}\right. .$$

(35)

Then, the state space expression obtained after its exact discretization is:

$$\left\{\begin{array}{c}x\left(\left(k+1\right)T\right)=G\left(T\right)x\left(kT\right)+H\left(T\right)u\left(kT\right)\\ y\left(kT\right)=Cx\left(kT\right)+Du\left(kT\right)\end{array}\right..$$

(36)

The input changes only at discrete sampling intervals. So, this problem can be solved if matrixes G and H, independent of t or k, can be found, which leads to a discrete-time model of the system.

According to conclusions in textbooks about modern control theory:

$$\left\{\begin{array}{c}G\left(T\right)=\Phi \left(T\right)=e^{AT}\\ H\left(T\right)=\left({{\displaystyle \int }}_0^T\mathsf{\Phi }\left(t\right)dt\right)B=\left({{\displaystyle \int }}_0^T{e}^{At}dt\right)B\end{array}\right.,$$

(37)

Calculate the value of state x at time $kT$ and $\left(k+1\right)T$.

$$x\left(\left(k+1\right)T\right)=e^{A\left(k+1\right)T}x\left(0\right)+e^{A\left(k+1\right)T}{{\displaystyle \int }}_0^{\left(k+1\right)T}{e}^{-A\tau }Bu\left(\tau \right)d\tau .$$

(38)

Set Ad, the discrete state model of the matrix A, as follows: $\mathrm{Ad}=e^{AT}$. From the Taylor expansion, we have:

$$e^{AT}=I+TA+\frac{1}{2!}{A}^2{T}^2+\dots$$

(39)

Assume that the sampling period T is small such that the higher-order terms can be omitted; then, we obtain $Ad=G=I+TA$ as well as $Bd=H=BT$.

Thus, the discretization of the continuous-time model will be:

$$x\left(\left(k+1\right)T\right)=Ad\ast x\left(kT\right)+Bd\ast u\left(kT\right) .$$

(40)

4.2.3. Improved Path Planning Algorithm

Consider the position vector of the vehicle’s dynamics model in discrete time.

$$P_r=\left\{\begin{array}{c}{P}_r\left(1,i+1\right)=P_r\left(1,i\right)+\lambda \ast cos\gamma ,x coordinate \\ P_r\left(2,i+1\right)=P_r\left(2,i\right)+\lambda \ast sin\gamma ,y coordinate\end{array}\right. .$$

(41)

In the equation, $\lambda$ indicates the step length. When the distance between the detected obstacle and the robot is less than the threshold, the robot enters obstacle avoidance mode. Set the potential field located at the target point of $P_g$, playing the role of attracting the leader $P_r$, and use the conventional gravitational potential field:

$$U_a=w_a\ast {\left(P_g-P_r\right)}^2.$$

(42)

For the obstacle rejection potential field, instead of using a conventional potential field, a Gaussian-like function is used as the potential barrier.

$$U_{ob,i}=w_2\ast exp(-\frac{1}{{\sigma }^2}\ast [{\left(x_r-x_{ob,i}\right)}^2+{\left(y_r-y_{ob,i}\right)}^2-R^2-r^2]) ,$$

(43)

where $x_r,y_r$ represent the leader’s current position; $x_{ob,i}, y_{ob,i}$ represent the central position of the ith obstacle; $\sigma$ represents the standard deviation of the obstacle’s potential energy; $w_2$ represents a coefficient of the range of the obstacle’s potential energy; R represents the collision radius of the leader; r is the collision radius of the obstacle.

A general approach to path planning with a simplified artificial potential field is depicted in Figure 13.

4.2.4. Simulation Results

In this section, simulations are performed through MATLAB. Assume there are four vehicles in the formation.

$$\dot{x}=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]x+\left[\begin{array}{c}0\\ 1\end{array}\right]u$$

(44)

The four vehicles, $x_1, x_2, x_3, x_4$ ($x_4$ is the leader), have the following communication topology diagram (shown in Figure 14 )and Laplace matrix.

$$L_G=\left[\begin{array}{cccc}3& -1& -1& -1\\ -1& 3& -1& -1\\ -1& -1& 3& -1\\ 0& 0& 0& 0\end{array}\right]$$

(45)

At this point, the leader does not follow a consistent protocol, its state is determined by predefined inputs and all states of each cart are determined accordingly. To build the continuous-time model, we use the KRON function to generate the Kronecker product by multiplying the unit matrix I with the matrix A and the Kronecker product by multiplying the matrix L with the matrix B in MATLAB. The discrete-time model will also be produced through digitization.

After identifying the control part for the second-order consensus, develop the overall control protocol for all vehicles, which is shown in Figure 15.

After applying the above control method, simulate a four-vehicle rectangular formation simulation under MATLAB with a formation of $20\times 20$squares and set the formation’s vector$h=\left(\begin{array}{cccccccc}20& 0& 0& 0& 0& 0& 20& 0\\ 0& 0& 0& 0& 20& 0& 20& 0\end{array}\right)$. The results are shown in Figure 16 and Figure 17.

It can be seen that the formation can form and stabilize even when the leader is moving, avoid obstacles normally according to the preset avoidance logic, resume the formation immediately after the avoidance and keep moving to the end.

5. Conclusions

This paper focuses on the cooperative following control function of mobile robots. In terms of multi-robot following, the leader-follower formation model is developed, and a basic following control method is analyzed in simulation experiments. Then, the enhanced integral separation feedback following control strategy incorporating the dynamic information is proposed to realize the fast completion of formation and stable following. In the two-robots motion simulation, the optimized following control model is proved to be more effective, as the total error E during the simulation of experiment 3 is 0.3334 m, 57.6% lower than that of experiment 2, which was conducted under the same initial conditions, 0.7863 m. Considering the following control in the case of a multi-robot system, an information consensus-based formation control framework is proposed for the multi-agent system with validated effectiveness. The consensus algorithms can not only orchestrate the whole formation of the MAS but also coordinate the dynamic relationships among all the agents. In the study of multi-agent systems of consensus, the main focus is on aligning the state of each intelligent body.