# Development of Path Planning Tool for Unmanned System Considering Energy Consumption
## 4 Mathematical Modeling of the UAV and Battery Pack  
### Uav  
The configuration and governing control inputs of the quadrotor are described in Figure 6, where \(X_{E}\), \(Y_{E}\), and \(Z_{E}\) represent earth coordinates, \(x_{q}\), \(y_{q}\), and \(z_{q}\) represent quadrotor coordinates, and \(F_{i}\) represents a thrust force generated by the \(i\)-th rotor in the indicated direction.  
To extract current and voltage profiles along the predetermined path obtained from the previous section, an enhanced quadrotor dynamic model can be represented as  
\[\begin{array}{l}w_{1}^{2}=b\Big{(}T+\tau_{x}+\tau_{y}+\tau_{z}\Big{)}^{-1} \\ w_{2}^{2}=lb(-\tau_{x}+\tau_{z})\\ w_{3}^{2}=lb\Big{(}-T+\tau_{y}\Big{)}\\ w_{4}^{2}=d\Big{(}-T+\tau_{x}-\tau_{y}+\tau_{z}\Big{)}\end{array} \tag{16}\]  
Figure 5: Flowchart of the energy-efficient path planning (EEPP) algorithm.  
Figure 6: Quadrotor configuration.  
where \(T\) is the total upward thrust of the UAV (N), \(b\) is the thrust factor, and \(w_{i}\) is the \(i\)-th rotor speed (rad/s), \(l\) is the lever length from the center of gravity (CG) to rotors (m), \(d\) is the drag factor, and \(\tau_{x}\), \(\tau_{y}\), and \(\tau_{z}\) are torques applied on the UAV's body along \(x\)-, \(y\)-, and \(z\)-axes (N\(\cdot\)m) [21; 22].  
Here, \(T\), \(\tau_{x}\), \(\tau_{y}\), and \(\tau_{z}\) can be calculated using proportional-integral-derivative (PID) controllers as  
\[\begin{split}& T=k_{p_{T}}(z^{\star}-z)+k_{i_{T}}\int\limits_{t_{1}} ^{t_{2}}(z^{\star}-z)dt+k_{d_{T}}\big{(}\dot{z}^{\star}-\dot{z}\big{)}+\sqrt{W/4 b},\\ &\tau_{x}=k_{p_{x}}(\phi^{\star}-\phi)+k_{i_{x}}\int\limits_{t_{1 }}^{t_{2}}(\phi^{\star}-\phi)dt+k_{d_{x}}\Big{(}\dot{\phi}^{\star}-\dot{\phi} \Big{)},\\ &\tau_{y}=k_{p_{y}}(\phi^{\star}-\theta)+k_{i_{y}}\int\limits_{t_ {1}}^{t_{2}}(\theta^{\star}-\theta)dt+k_{d_{y}}\Big{(}\dot{\theta}^{\star}- \dot{\theta}\Big{)},\\ &\tau_{z}=k_{p_{z}}(\psi^{\star}-\psi)+k_{i_{z}}\int\limits_{t_{1 }}^{t_{2}}(\psi^{\star}-\psi)dt+k_{d_{z}}\Big{(}\dot{\psi}^{\star}-\dot{\psi} \Big{)},\end{split} \tag{17}\]  
where \(k_{p}\), \(k_{i}\), and \(k_{d}\) are PID controller gains, \(\phi\), \(\theta\), and \(\psi\) are roll, pitch, and yaw angles (rad), uppercase letter * represents reference data, and \(W\) is the UAV weight (kg).  
The full quadrotor UAV dynamic model can be written as  
\[\begin{split}&\ddot{x}=(\cos\phi\sin\theta\cos\psi+\sin\phi\sin \psi)\frac{1}{m}b\Big{(}w_{1}^{2}+w_{2}^{2}+w_{3}^{2}+w_{4}^{2}\Big{)},\\ &\ddot{y}=(\cos\phi\sin\theta\sin\psi-\sin\phi\cos\psi)\frac{1}{m }b\Big{(}w_{1}^{2}+w_{2}^{2}+w_{3}^{2}+w_{4}^{2}\Big{)},\\ &\ddot{z}=(\cos\phi\cos\theta)\frac{1}{m}b\Big{(}w_{1}^{2}+w_{2}^ {2}+w_{3}^{2}+w_{4}^{2}\Big{)}-g,\\ &\ddot{\phi}=\dot{\phi}\dot{\psi}\Big{(}\frac{l_{y}-l_{y}}{l_{x}} \Big{)}-\frac{l_{x}}{l_{y}}\dot{\phi}(w_{1}+w_{3}-w_{2}-w_{4})+\frac{l_{y}}{l_{ y}}b\Big{(}w_{3}^{2}-w_{1}^{2}\Big{)},\\ &\ddot{\psi}=\dot{\phi}\dot{\phi}\Big{(}\frac{l_{x}-l_{y}}{l_{x}} \Big{)}+\frac{1}{l_{z}}d\Big{(}w_{2}^{2}+w_{4}^{2}-w_{1}^{2}-w_{3}^{2}\Big{)}. \end{split} \tag{18}\]  
Here, \(J_{r}\) is the rotor inertia (kg\(\cdot\)m\({}^{2}\)).  
Using the fact that power can be calculated by multiplying the thrust and velocity, one can calculate the required propulsive thrust power from the total upward thrust \(T\) in Equation (15) as  
\[P=T\cdot\|v_{0}\|. \tag{19}\]  
Then, the load current can be calculated using  
\[\frac{P}{v_{t}}=i_{0} \tag{20}\]  
where \(v_{t}\) is the terminal voltage (V), and \(i_{0}\) is the load current (A).  
The above equations are used for the operation of flight simulation, as shown in Figure 7.  
Figure 7: Flowchart of the EEPP algorithm.