# Formation Control and Distributed Goal Assignment for Multi-Agent Non-Holonomic Systems
## 2 Control Algorithm  
The kinematic model of the \(i\)-th differentially-driven mobile robot \(R_{i}\) (\(i=1\ldots N\), \(N\)--number of robots) is given by the following equation:  
\[q_{i}=\left[ \begin{array}{cc}\cos\theta_{i}&0\\ \sin\theta_{i}&0\\ 0&1\end{array} \right]u_{i} \tag{1}\]  
where vector \(q_{i}=[x_{i}\;\;y_{i}\;\;\theta_{i}]^{\top}\) denotes the pose and \(x_{i}\), \(y_{i}\), \(\theta_{i}\) are the position coordinates and orientation of the robot with respect to a global, fixed coordinate frame. Vector \(u_{i}= \begin{bmatrix}v_{i}&\omega_{i}\end{bmatrix} ^{\top}\) is the control vector with \(v_{i}\) denoting the linear velocity and \(\omega_{i}\) denoting the angular velocity of the platform. The task of the formation is to follow the virtual leader that moves with desired linear and angular velocities \([v_{l}\;\;\omega_{l}]^{T}\). The robots are expected to imitate the motion of the virtual leader. They should have the same velocities as the virtual leader. The position coordinates \([x_{l}\;\;y_{l}]^{T}\) of the virtual leader are used as a reference position for the individual robots but each of them has different displacement with respect to the leader:  
\[x_{id}=x_{l}+d_{ix}\;\;\;\;\;y_{id}=y_{l}+d_{iy}, \tag{2}\]  
where \([d_{ix}\;\;\;d_{iy}]^{T}\) is desired displacement of the \(i\)-th robot. As the robots position converge to the desired values their orientations \(\theta_{i}\) converge to the orientation of the virtual leader \(\theta_{l}\). The collision avoidance behaviour is based on the APF. This concept was originally proposed in [1]. All robots are surrounded by APFs that raise to infinity near objects border \(r_{j}\) (\(j\)--number of the robots/obstacles) and decreases to zero at some distance \(R_{j}\), \(R_{j}>r_{j}\). One can introduce the following function [6]:  
\[B_{aij}(l_{ij})=\left\{ \begin{array}{ccc}0&\text{for}&l_{ij}<r_{j}\\ \frac{l_{ij}-r_{j}}{l_{ij}-R_{j}}&\text{for}&r_{j}\leq l_{ij}<R_{j}\\ 0&\text{for}&l_{ij}\geq R_{j}\end{array} \right., \tag{3}\]  
that gives output \(B_{aij}(l_{ij})\in\langle 0,1\rangle\). The distance between the \(i\)-th robot and the \(j\)-th robot is defined as the Euclidean length \(l_{ij}=\left\|[x_{j}\;y_{j}]^{\top}-[x_{i}\;y_{i}]^{\top}\right\|\). Scaling the function given by Equation (3) within the range \(\langle 0,\infty\rangle\) can be given as follows:  
\[V_{aij}(l_{ij})=\frac{B_{aij}(l_{ij})}{1-B_{aij}(l_{ij})}, \tag{4}\]  
that is used later to avoid collisions. In further description, the terms 'collision area' or 'collision region' are used for locations fulfilling the condition \(l_{ij}<r_{j}\). The range \(r_{j}<l_{ij}<R_{j}\) is called 'collision avoidance area' or 'collision avoidance region' (Figure 1). The goal of the control is to drive the formation along the desired trajectory avoiding collisions between agents. It is equivalent to bringing the following quantities to zero:  
\[p_{ix} =x_{id}-x_{i} \tag{5}\] \[p_{iy} =y_{id}-y_{i}\] \[p_{i\theta} =\theta_{l}-\theta_{i}.\]  
**Assumption 1**. : \(\forall\{i,j\},i\
eq j,\ ||[x_{id}\ \ y_{id}]^{\mathrm{T}}-[x_{id}\ \ y_{id}]^{ \mathrm{T}}||>R_{j}\)_._  
**Assumption 2**. : _If robot \(i\) gets into the collision avoidance region of any other robot \(j\), \(j\
eq i\) its desired trajectory is temporarily frozen (\(\dot{x}_{id}=0\), \(\dot{y}_{id}=0\)). If the robot leaves the avoidance area its desired coordinates are immediately updated. As long as the robot 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._  
Assumption 1 comes down to the statement that desired paths of individual robots are planned in such a way that in steady state all robots are out of the collision avoidance regions of other robots. Assumption 2 means that the tracking process is temporarily suspended because collision avoidance has a higher priority.