# Formation Control and Distributed Goal Assignment for Multi-Agent Non-Holonomic Systems
## 2 Control Algorithm
Once the robot is outside the collision detection region, it updates the reference to the new values. In addition, when the robot is in the collision avoidance region its reference is periodically updated. This low-frequency process supports leaving the unstable equilibrium points that occur e.g., when one robot is located exactly between the other robots and its goal. The system error expressed with respect to the coordinate frame fixed to the robot is described below:  
\[\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}\]  
Figure 1: Artificial potential functions (APF) as a function of distance to the centre of the robot (indexes omitted for simplicity). Using the above equations and non-holonomic constraint \(\dot{y}_{i}\cos(\theta_{i})-\dot{x}_{i}\sin(\theta_{i})=0\) the error dynamics between the leader and the follower are as follows:  
\[\dot{e}_{ix} =e_{iy}\omega_{i}-v_{i}+v_{l}\cos e_{i\theta}\] \[\dot{e}_{iy} =-e_{ix}\omega_{i}+v_{l}\sin e_{i\theta}\] \[\dot{e}_{i\theta} =\omega_{l}-\omega_{i}. \tag{7}\]  
One can introduce the position correction variables that consist of position error and collision avoidance terms:  
\[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{8}\]  
\(V_{aij}\) depends on \(x_{i}\) and \(y_{i}\) according to Equation (5). It is assumed that the robots avoid collisions with each other and there are no other obstacles in the taskspace. The correction variables can be transformed to the local coordinate frame fixed in the mass centre of the robot:  
\[\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{9}\]  
Differentiating the first two equations of (5) with respect to the \(p_{ix}\) and \(p_{iy}\) respectively one obtains:  
\[\frac{\partial x_{i}}{\partial p_{ix}}=-1\hskip 14.226378 pt\frac{\partial y_{i}}{ \partial p_{iy}}=-1. \tag{10}\]  
Using (10) one can write:  
\[\frac{\partial V_{aij}}{\partial p_{ix}} =\frac{\partial V_{aij}}{\partial x_{i}}\frac{\partial x_{i}}{ \partial p_{ix}}=-\frac{\partial V_{aij}}{\partial x_{i}}\] \[\frac{\partial V_{aij}}{\partial p_{iy}} =\frac{\partial V_{aij}}{\partial y_{i}}\frac{\partial y_{i}}{ \partial p_{iy}}=-\frac{\partial V_{aij}}{\partial y_{i}}. \tag{11}\]  
Taking into account Equations (8) and (9) the gradient of the APF can be expressed with respect to the local coordinate frame fixed to the \(i\)-th robot:  
\[\left[ \begin{array}{c}\frac{\partial V_{aij}}{\partial e_{ix}}\\ \frac{\partial V_{aij}}{\partial e_{iy}}\end{array} \right]=\left[ \begin{array} []{cc}\cos\theta_{i}&\sin\theta_{i}\\ -\sin\theta_{i}&\cos\theta_{i}\end{array} \right]\left[ \begin{array}{c}\frac {\partial V_{aij}}{\partial p_{ix}}\\ \frac{\partial V_{aij}}{\partial p_{iy}}\end{array} \right]. \tag{12}\]  
Equation (12) can be verified easily by taking partial derivatives of \(V_{aij}(d_{ix}-p_{ix},d_{iy}-p_{iy})=V_{aij}(d_{ix}-p_{ix}(e_{ix},e_{iy}),d_{iy} -p_{iy}(e_{ix},e_{iy}))\) with respect to \(e_{ix}\), \(e_{iy}\) and taking into account the inverse transformation of the first two equations of Equation (6). Using Equation (11), the above equation can be written as follows:  
\[\left[ \begin{array}{c}\frac{\partial V_{aij}}{\partial e_{ix}}\\ \frac{\partial V_{aij}}{\partial e_{iy}}\end{array} \right]=\left[ \begin{array} []{cc}-\cos\theta_{i}&-\sin\theta_{i}\\ \sin\theta_{i}&-\cos\theta_{i}\end{array} \right]\left[ \begin{array}{c}\frac {\partial V_{aij}}{\partial y_{i}}\\ \frac{\partial V_{aij}}{\partial y_{i}}\end{array} \right].