# Real-Time Swarm Search Method for Real-World Quadcopter Drones
## 2 The Proposed Swarm Search Method
### The Drone Position Update Mechanism
#### 2.1.2 The Dynamic Model of the Drones for the Mechanism  
The update mechanism should reflect the way in which the drones approach the next destination based on the real-world environment (i.e., the dynamic model). To establish the dynamic model, first, the kinematic model of a drone is necessary, as shown in Figure 1.  
From this kinematic model, the rotation matrix \(R\) for mapping the vector from the body frame to the inertial frame can be derived as  
\[R=\left[ \begin{array}{ccc}c_{\phi}c_{\psi}-c_{\theta}s_{\phi}s_{\psi}&-c_{ \psi}s_{\phi}-c_{\phi}c_{\theta}s_{\psi}&s_{\theta}s_{\psi}\\ c_{\theta}c_{\psi}s_{\phi}+c_{\phi}s_{\psi}&c_{\phi}c_{\phi}c_{\psi}-s_{\phi}s_{ \psi}&-c_{\psi}s_{\theta}\\ s_{\phi}s_{\theta}&c_{\phi}s_{\theta}&c_{\theta}\end{array} \right], \tag{4}\]  
where \(c_{\phi}\) and \(s_{\phi}\) represent \(cos(\phi)\) and \(sin(\phi)\), respectively. Then, from [PERSON]'s equation, the linear motion can be derived as  
\[m\tilde{x}=\left[ \begin{array}{c}0\\ 0\\ -mg\end{array} \right]+RT_{B}+F_{D}, \tag{5}\]  
where \(x\) is the position of the drone, \(g\) is the acceleration due to gravity, \(F_{D}\) is the drag force, and \(T_{B}\) is the thrust vector in the body frame. For simplicity, in this paper, the drag force is regarded as 0, and \(T_{B}\) is calculated based on [21]. Additionally, from [PERSON]'s equation, the angular motion can be derived as  
\[I\dot{w}=\tau-w\times(Iw),\]  
Figure 1: The physical model of a quadcopter drone. where \(w\) is the angular velocity vector, \(I\) is the inertia matrix, and \(\tau\) is a vector of external torques. In this paper, \(I\) and \(\tau\) are calculated based on [22; 23]. Finally, based on these motion equations, the final state space equations for the dynamic model can be derived as  
\[ \begin{split}\dot{x_{1}}&=x_{2},\\ \dot{x_{2}}&=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]+\frac{RT_{B}}{m}+\frac{F_{D}}{m},\\ \dot{x_{3}}&=\left[\begin{array}{ccc}1&0&-s_{\theta}\\ 0&c_{\phi}&c_{\theta}s_{\phi}\\ 0&-s_{\phi}&c_{\theta}c_{\phi}\end{array}\right]^{-1}\cdot x_{4},\\ \dot{x_{4}}&=I^{-1}\cdot[\tau-x_{3}\times(Ix_{3})],\end{split}  \tag{7}\]  
where \(x_{1}\) is the velocity vector, \(x_{2}\) is the acceleration vector, \(x_{3}\) is the angular velocity vector, and \(x_{4}\) is the angular acceleration vector. The model parameters used in this paper are listed in Table 1.  
As shown in Figure 2, the control system of the drone can be designed based on a well-known proportional-integral-derivative (PID) control scheme [24]. Note that the control system is not for the low-level motor actuation control, but for the high-level control of the commands transmitted to each drone. In addition, in simulation, the sensor system and the environment can be replaced by the dynamic model derived above. First, the position error at the \(n\)-th control period is calculated. Then, the linear PID system yields the desired displacement \(\hat{\mathbf{v}}_{n+1}\), and the next destination posture \(\theta_{n+1}^{*}\) can be calculated by multiplying a scaling matrix, since it is assumed that the drone is in a piecewise hovering state. Lastly, the angular PID system yields the posture displacement vector \(\hat{\theta}_{n+1}\), and the actuation system executes the corresponding throttle commands for the motors. As a result, the drone can gradually approach the next destination \(\mathbf{x}_{t+1}^{*}\).