## 3 Methodology
### Model Formulation  
Each agent must face with many different scenarios and therefore can have complex internal states to cope with complicated tasks under different situations. At the high level of model formulation, we model each agent with three major internal states that are switching among each other to deal with different complicated situations. Figure 1 shows the three _moving_, _waiting_ and _maneuvering_ states and how a state changes to another under different situations. Most of the time, agents maintain at the _moving_ state. Once the set of trajectory nodes are calculated and returned from the algorithm, the agent follows the nodes and simultaneously predicts a new trajectory. The loop is continued until another state is switched or the target is reached. In any case during the movement, if all of the predicted trajectories are blocked by the teammates or obstacles, the agent state will be automatically switched to the _waiting_ state. The _waiting_ state refers to an agent which stops and remaining at the same position to re-plan another path. It is usually occurs when avoiding obstacles, formation merging especially when the orientations change frequently. Thus, a later agent should give up its path priority until the former agent pass. However, a deadlock may occur especially when most agents are congested in such an environment as in a blind alley. To solve such a problem, we set a constant _clock_ to prevent an agent from waiting for overtime. If _clock_ exceeds the waiting time bound, a _maneuvering_ state will be invoked, as shown in Equation (1). \[waiting()= \begin{cases}\text{stop},&\text{if }clock<t_{wait}\\ \text{switch to }\textit{maneuvering},&\text{otherwise}\end{cases}  \tag{1}\]  
In _maneuvering_ state, the agent stops and rotates its orientation in search for other possible trajectories to the target. The _maneuvering_ state is also invoked in other cases such as facing with multiple or irregular-shaped obstacles. We implement the agent in _maneuvering_ state to rotate clockwise along the orientation with a predefined angular velocity \(\omega_{nav}\), that is calculated according to Equation (2). \[ \begin{split}& v_{i}=0\\ &\theta_{i}(t)=\theta_{i}(t_{0})+t\cdot\omega_{nav}\end{split}  \tag{2}\]  
By changing the orientation of the agent by rotating, it is possible for agent to find other alternative paths when the leading agents are facing with deadlock situations or current feasible paths are blocked by all team agents. Furthermore, even waiting cannot expect to find a feasible path efficiently to resolve the deadlock. The starting point and the target (the destination point) of each agent are given initially by  
\[\begin{split}& Start_{i}=x_{i}(t_{start}),y_{i}(t_{start})\\ & Target=x_{end},y_{end} \tag{3}\]  
where \(x_{i}(t_{start}),y_{i}(t_{start})\) is the position of \(i\)th agent at the starting time \(t_{start}\). We then use the linear discrete-time model \((t_{0},t_{1},...,t_{n})\) for the kinematic equations as in Equations (4). Figure 1: State switching under different circumstances. \[ \begin{split} x_{i}(t_{n})&=x_{i}(t_{0})+\int_{t_{0}}^{t_{n }}v_{i}(t)\cdot cos\theta_{i}(t)dt\\ y_{i}(t_{n})&=y_{i}(t_{0})+\int_{t_{0}}^{t_{n}}v_{ i}(t)\cdot sin\theta_{i}(t)dt\\ \theta_{i}(t)&=\theta_{i}(t_{0})+\int_{t_{0}}^{t} \omega_{i}(\hat{t})dt\end{split}  \tag{4}\]  
for all \(i\in\)[1, 2,..., \(N\)], where \((x_{i}(t_{n}),y_{i}(t_{n}))\) is the position of \(i\)th agent, \(v_{i}(t)\) is the velocity and \(\theta_{i}(t)\) is the orientation at time \(t\). \(\theta(t)\) is determined by the angular velocity \(\omega\) at time \(\hat{t}\in[t_{0},t_{n}]\). In other words, \(v\) and \(\omega\) are two inputs which control the motion of agents at any time. In practice, the agents have the speed and angular velocity constraints as shown in Equation (5). \[ \begin{split} 0&\leq v_{i}(t)\leq v_{\max}\\ &-\omega_{\max}\leq\omega_{i}(t)\leq\omega_{\max}\end{split}