# Differentially-Driven Robots Moving in Formation--Leader-Follower Approach
## 4 Control Algorithm  
The goal of the formation control is to mimic motion of the virtual leader maintaining desired displacements between robots and avoiding collisions. It is equivalent to bringing the following quantities to zero:  
\[p_{ix} =x_{i-1}-x_{i}-d_{x}^{i-1,i}\] \[p_{iy} =y_{i-1}-y_{i}-d_{y}^{i-1,i}\] \[p_{i\theta} =\theta_{i-1}-\theta_{i} \tag{5}\]  
where \(d_{x}^{i-1,i}\) and \(d_{y}^{i-1,i}\) are components of the spatial displacements between robots \(i-1\) and \(i\) (Figure 2). **Assumption 1**. : \(\forall\{i,j\},i\
eq j\)_, \(\left\|\sum_{i=1}^{N}\left[ \begin{array}{c}d_{x}^{i-1,i}\\ d_{y}^{i-1,i}\end{array} \right]-\sum_{j=1}^{N}\left[ \begin{array}{c}d_{x}^{j- 1,j}\\ d_{y}^{j-1,j}\end{array} \right]\right\|>\bar{R}_{j}\)._  
This means that the formation is designed in such a way that, when the robots are moving with zero errors, none of them is in the collision avoidance region of other robots. Figure 1: An example of APF (indexes omitted for simplicity). **Assumption 2**. : _If robot \(i\) gets into the collision avoidance region of any other mobile platform \(j\), \(j\
eq i\) its desired trajectory is temporarily frozen. If the robot leaves the avoidance area its desired coordinates are immediately updated. As long as the mobile platform remains in the avoidance region, its desired coordinates are periodically updated at certain discrete instants of time. The time period \(t_{u}\) of this update process is relatively large in comparison to the main control loop sample time._  
A detailed explanation of this assumption is given in reference [20]. Position and orientation errors expressed with respect to the local coordinate frame fixed to the robot is given by equation:  
\[\left[ \begin{array}{c}e_{ix}\\ e_{iy}\\ e_{i\theta}\end{array} \right]=\left[ \begin{array}{ccc}\cos(\theta_{i})&\sin (\theta_{i})&0\\ -\sin(\theta_{i})&\cos(\theta_{i})&0\\ 0&0&1\end{array} \right]\left[ \begin{array}{c}p_{ix}\\ p_{iy}\\ p_{i\theta}\end{array} \right]. \tag{6}\]  
Accordingly to given model of the robot in Equation (1) the non-holonomic constraint is given by formula:  
\[\dot{y}_{i}\cos(\theta_{i})-\dot{x}_{i}\sin(\theta_{i})=0. \tag{7}\]  
Using Equations (6) and (7) the error dynamics between robots \(R_{i-1}\) (the leader) and \(R_{i}\) (the follower) can be expressed as follows:  
\[\dot{e}_{ix} =e_{iy}\omega_{i}-v_{i}+v_{i-1}\cos(e_{i\theta})\] \[\dot{e}_{iy} =-e_{ix}\omega_{i}+v_{i-1}\sin(e_{i\theta})\] \[\dot{e}_{i\theta} =\omega_{i-1}-\omega_{i}. \tag{8}\]  
Position correction variables are introduced that consist of position error and collision avoidance components of control, computed using spatial derivatives of the APFs:  
\[P_{ix} =p_{ix}-\sum_{j=1,j\
eq i}^{N}\frac{\partial V_{aij}}{\partial x_ {i}}\] \[P_{iy} =p_{iy}-\sum_{j=1,j\
eq i}^{N}\frac{\partial V_{aij}}{\partial y_ {i}}. \tag{9}\]  
\(V_{aij}\) is function of coordinate variables \(x_{i}\) and \(y_{i}\) according to Equations (2) and (3). To take into account collision avoidance, the error components in the original algorithm from [21] will be replaced by these position correction variables. **Assumption 3**. : _There are no obstacles other than robots (enumerated \(i=1,\ldots,N\)) in the taskspace._  
The position correction variables can be expressed in local coordinate frame fixed in the geometric center of the mobile platform:  
\[\left[ \begin{array}{c}E_{ix}\\ E_{iy}\\ e_{i\theta}\end{array} \right]=\left[ \begin{array}{ccc}\cos(\theta_{i})&\sin (\theta_{i})&0\\ -\sin(\theta_{i})&\cos(\theta_{i})&0\\ 0&0&1\end{array} \right]\left[ \begin{array}{c}P_{ix}\\ P_{iy}\\ p_{i\theta}\end{array} \right]. \tag{10}\]  
As shown in reference [24] gradient of the APF can be transformed to be expressed with respect to the local coordinate frame fixed to the \(i\)-th mobile robot:  
\[\left[ \begin{array}{c}\frac{\partial V_{aij}}{\partial x_{ix}}\\ \frac{\partial V_{aij}}{\partial e_{iy}}\end{array} \right]=\left[ \begin{array}[ ]{ccc}-\cos(\theta_{i})&-\sin(\theta_{i})\\ \sin(\theta_{i})&-\cos(\theta_{i})\end{array} \right]\left[