# Trajectory optimisation for a quadrotor helicopter considering energy consumption

Fouad Yacef, Nassim Rizoug, Laid Degaa, Omar Bouhali, Mustapha Hamerlain

► To cite this version:

Fouad Yacef, Nassim Rizoug, Laid Degaa, Omar Bouhali, Mustapha Hamerlain. Trajectory optimisation for a quadrotor helicopter considering energy consumption. 2017 4th International Conference on Control, Decision and Information Technologies (CoDIT), Apr 2017, Barcelona, Spain. pp.1030-1035, 10.1109/codit.2017.8102734. hal-04577392

HAL Id: hal-04577392

https://hal.science/hal-04577392

Submitted on 16 May 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Trajectory optimisation for a quadrotor helicopter considering energy consumption

Fouad Yacef, Nassim Rizoug, Laid Degaa, Omar Bouhali, Mustapha Hamerlain

► To cite this version:

Fouad Yacef, Nassim Rizoug, Laid Degaa, Omar Bouhali, Mustapha Hamerlain. Trajectory optimisation for a quadrotor helicopter considering energy consumption. 2017 4th International Conference on Control, Decision and Information Technologies (CoDIT), Apr 2017, Barcelona, Spain. pp.1030-1035, 10.1109/codit.2017.8102734 . hal-04577392

HAL Id: hal-04577392

https://hal.science/hal-04577392v1

Submitted on 16 May 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Trajectory Optimisation for a Quadrotor Helicopter Considering Energy Consumption

Conference Paper · April 2017

DOI: 10.1109/CoDIT.2017.8102734

CITATIONS

22

READS

535

5 authors, including:

<figure><img src="image_2.png" /></figure>

Yacef Fouad

Centre Algérien de Développement des Technologies Avancées

38 PUBLICATIONS

401

CITATIONS

SEE PROFILE

<figure><img src="image_3.png" /></figure>

Nassim Rizoug

Ecole Supérieure des Techniques Aéronautiques et de Construction Automobile

34 PUBLICATIONS

450

CITATIONS

SEE PROFILE

<figure><img src="image_4.png" /></figure>

Laid Degaa

Ecole Supérieure des Techniques Aéronautiques et de Construction Automobile

25 PUBLICATIONS

123

CITATIONS

SEE PROFILE

<figure><img src="image_5.png" /></figure>

Omar Bouhali

University of Jijel

74 PUBLICATIONS

857

CITATIONS

SEE PROFILE
# Trajectory Optimisation for a Quadrotor Helicopter Considering Energy Consumption

Conference Paper · April 2017

DOI: 10.1109/CoDIT.2017.8102734

CITATIONS

22

READS

535

5 authors, including:

<figure><img src="image_2.png" /></figure>

Yacef Fouad
Centre Algérien de Développement des Technologies Avancées

38 PUBLICATIONS 401 CITATIONS

SEE PROFILE

<figure><img src="image_3.png" /></figure>

Nassim Rizoug
Ecole Supérieure des Techniques Aéronautiques et de Construction Automobile

34 PUBLICATIONS 450 CITATIONS

SEE PROFILE

<figure><img src="image_4.png" /></figure>

Laid Degaa
Ecole Supérieure des Techniques Aéronautiques et de Construction Automobile

25 PUBLICATIONS 123 CITATIONS

SEE PROFILE

<figure><img src="image_5.png" /></figure>

Omar Bouhali
University of Jijel

74 PUBLICATIONS 857 CITATIONS

SEE PROFILE
# Trajectory Optimisation for a Quadrotor Helicopter
Considering Energy Consumption

Fouad Yacef*†, Nassim Rizoug†, Laid Degaa‡, Omar Bouhali§ and Mustapha Hamerlain*

*Centre de Développement des Technologies Avancées, Algeria
Email: fyacef@cdta.dz, mhamerlain@cdta.dz

†Ecole Supérieure des Techniques Aéronautiques et de Construction Automobile, France
Email: nassim.rizoug@estaca.fr, fouad.yacef@estaca.fr

‡École nationale polytechnique d'Oran, Algeria
Email: laid Degaa@enp-oran.dz

§Laboratoire MécaTronique (LMT), Université de Jijel, Algeria
Email: bouhali_omar@univ-jijel.dz

**Abstract**—In this paper we deals with the limitation of embedded energy for quadrotor unmanned aerial vehicles. Quadrotor UAVs are flying machines that use lift generated by several rotors, and because of this, a large proportion of their available energy is consumed by rotors in order to maintain the vehicle in the air. In this concept, two optimal control problems are formulated and solved. For the first problem, minimum-energy control effort is computed for desired initial and final configurations with respect to the angular velocity of rotors. Where in the second, minimum-time control effort is computed for a desired energy. The proposed method is illustrated by simulation experiment for a quadrotor UAV.

## I. INTRODUCTION

### A. Motivation and related work

In the last few years, rotary wings unmanned aerial vehicles UAVs have attract more interest due to the wide range of applications that can be addressed with such a vehicle. Recently some promising new applications have been emerged like package delivery, cinematography, agriculture surveillance and aerial manipulation; however theses applications still restricted since the embedded energy whose source is LiPo battery provide a flight time between 15 and 45 minutes, which limited the class of mission that can be carried out successfully by the UAV. To undertake this problem, many efforts have made in reduction of rotary wings UAVs weight by the use of carbon fibre airframe and high energy density intelligent-soft and in the improvement of power to weight rate. These solutions success to reduced operating in energy-starved regimes; nevertheless, no significant technological progress is made until now.

Many studies have been proposed recently contributing towards save energy and increased endurance. These contributions have mainly focused on the design of automated battery charging/replacing system. A battery swapping system for multiple small-scale UAVs have been proposed in [1]. In addition to the battery swapping mechanism, the system includes an online algorithm that can supervise replenishment of many UAVs operating simultaneously, determine when the vehicle require replenishment and perform a precision landing onto the battery swapping mechanism's landing platform. In

[2] the design, test and construction of an autonomous ground recharge station for battery-powered quadrotor helicopter was presented. An energy management algorithm was implemented for a multi-agent system where the priority is given to the group to ensure the optimality of the solution regardless of number, position and density of the environment. Where in [3], an autonomous battery maintenance mechatronic system to extends the operational time of battery powered small-scaled UAVs have been developed.

Other studies have introduce endurance estimation model, in [4] a simple model is proposed to estimate the endurance of an indoor hovering quadrotor, whereas in [5] a characterization of the power consumption of rotorcraft supplied by LiPo battery and an accurate endurance estimation model have been introduced. In order to extends UAVs operating time a battery state of charge based altitude controller for an six-rotor aircraft was proposed in [6], where a battery monitoring system was designed in order to estimate state of charge (SOC) and then use it to calculate the designed controller.

In view of path following control with minimum energy consumption, the authors in [7] evaluate the relationship between navigation speed and energy consumption in a miniature quadrotor helicopter, which travels over a desired path through experimental test. Then, a novel path-following controller is proposed in which the speed of the rotorcraft is a dynamic profile that varies with the geometric requirements of the desired path.

The energy optimal path planning problem for rotary wings UAVs has gain less interest in the unmanned aerial systems literature. In [8] an approach has been proposed to solve near-minimum-energy tours for an hexarotor on a multi-target mission using the generalized travelling salesman problem with Neighbourhoods and a heuristic algorithm with 4-DOF dynamic model for cost function calculation. Where in [9] minimum-energy paths were obtained between given initial and final configurations for a 6-DOF quadrotor UAV by solving an optimal control problem, also a minimum-time and/or minimum-control-effort trajectory was calculated by solving a related optimal control problem.
# Trajectory Optimisation for a Quadrotor Helicopter
Considering Energy Consumption

Fouad Yacef*†, Nassim Rizoug†, Laid Degaa‡, Omar Bouhali§ and Mustapha Hamerlain*

*Centre de Développement des Technologies Avancées, Algeria
Email: fyacef@cdta.dz, mhamerlain@cdta.dz

†Ecole Supérieure des Techniques Aéronautiques et de Construction Automobile, France
Email: nassim.rizoug@estaca.fr, fouad.yacef@estaca.fr

‡École nationale polytechnique d'Oran, Algeria
Email: laid Degaa@enp-oran.dz

§Laboratoire MécaTronique (LMT), Université de Jijel, Algeria
Email: bouhali_omar@univ-jijel.dz

***Abstract***—In this paper we deals with the limitation of embedded energy for quadrotor unmanned aerial vehicles. Quadrotor UAVs are flying machines that use lift generated by several rotors, and because of this, a large proportion of their available energy is consumed by rotors in order to maintain the vehicle in the air. In this concept, two optimal control problems are formulated and solved. For the first problem, minimum-energy control effort is computed for desired initial and final configurations with respect to the angular velocity of rotors. Where in the second, minimum-time control effort is computed for a desired energy. The proposed method is illustrated by simulation experiment for a quadrotor UAV.

## I. INTRODUCTION

### A. Motivation and related work

In the last few years, rotary wings unmanned aerial vehicles UAVs have attract more interest due to the wide range of applications that can be addressed with such a vehicle. Recently some promising new applications have been emerged like package delivery, cinematography, agriculture surveillance and aerial manipulation; however theses applications still restricted since the embedded energy whose source is LiPo battery provide a flight time between 15 and 45 minutes, which limited the class of mission that can be carried out successfully by the UAV. To undertake this problem, many efforts have made in reduction of rotary wings UAVs weight by the use of carbon fibre airframe and high energy density intelligent-soft and in the improvement of power to weight rate. These solutions success to reduced operating in energy-starved regimes; nevertheless, no significant technological progress is made until now.

Many studies have been proposed recently contributing towards save energy and increased endurance. These contributions have mainly focused on the design of automated battery charging/replacing system. A battery swapping system for multiple small-scale UAVs have been proposed in [1]. In addition to the battery swapping mechanism, the system includes an online algorithm that can supervise replenishment of many UAVs operating simultaneously, determine when the vehicle require replenishment and perform a precision landing onto the battery swapping mechanism's landing platform. In

[2] the design, test and construction of an autonomous ground recharge station for battery-powered quadrotor helicopter was presented. An energy management algorithm was implemented for a multi-agent system where the priority is given to the group to ensure the optimality of the solution regardless of number, position and density of the environment. Where in [3], an autonomous battery maintenance mechatronic system to extends the operational time of battery powered small-scaled UAVs have been developed.

Other studies have introduce endurance estimation model, in [4] a simple model is proposed to estimate the endurance of an indoor hovering quadrotor, whereas in [5] a characterization of the power consumption of rotorcraft supplied by LiPo battery and an accurate endurance estimation model have been introduced. In order to extends UAVs operating time a battery state of charge based altitude controller for an six-rotor aircraft was proposed in [6], where a battery monitoring system was designed in order to estimate state of charge (SOC) and then use it to calculate the designed controller.

In view of path following control with minimum energy consumption, the authors in [7] evaluate the relationship between navigation speed and energy consumption in a miniature quadrotor helicopter, which travels over a desired path through experimental test. Then, a novel path-following controller is proposed in which the speed of the rotorcraft is a dynamic profile that varies with the geometric requirements of the desired path.

The energy optimal path planning problem for rotary wings UAVs has gain less interest in the unmanned aerial systems literature. In [8] an approach has been proposed to solve near-minimum-energy tours for an hexarotor on a multi-target mission using the generalized travelling salesman problem with Neighbourhoods and a heuristic algorithm with 4-DOF dynamic model for cost function calculation. Where in [9] minimum-energy paths were obtained between given initial and final configurations for a 6-DOF quadrotor UAV by solving an optimal control problem, also a minimum-time and/or minimum-control-effort trajectory was calculated by solving a related optimal control problem.
## B. Contribution and organization

Motivated by the previous discussion, in this paper we introduced two energy optimal control problem, where the objective is to minimise the energy consumed by the quadrotor vehicle at the end of the mission while the quadrotor aircraft has to satisfy boundary conditions and feasibility constraints on the states of aircraft and control inputs. Another objective is to compute the minimum time sped by the vehicle to travelled the trajectory during the mission with fixed energy. An efficiency function was identified, in order to modelled motor efficiency and have an energetic model close to reality.

The rest of this paper is organized as follows. In section 2, we presented actuator model. Then, in section 3 dynamic model of the quadrotor vehicle is presented. In section 4, the problem of trajectory optimisation with minimum energy is presented. The trajectory optimisation minimum time problem is illustrated in section 5. In section 6 nonlinear programming method and simulation results are presented. Conclusions and remarks are drawn in section 7.

## II. ACTUATOR MODEL

A quadrotor UAV actuator system is typically consist of a LiPo battery, a brushless direct current (BLDC) motor and an control stage to control the angular velocity (RPM) of the motor. Electrical DC motors are well modelled by a circuit containing a resistor, inductor, and voltage generator in series [10].

$$v(t) = Ri(t) + L\frac{\partial i(t)}{\partial t} + \frac{\omega(t)}{k_v} \quad (1)$$

where R is the motor internal resistance, L is the inductance, $\omega(t)$ is the rotational rat of the motor, and $k_v$ is the voltage constant of the motor, expressed in rad/s/volt. Also, the motor torque $\tau$ can be modelled as being proportional to the current $i(t)$ through the torque constant, $k_t$, expressed in Nm/A.

$$\tau(t) = k_t i(t) \quad (2)$$

The motor dynamics are modelled as a simple first order differential equation (3) where $\dot{\omega}$ is driven by the motor torque and the load friction torque $Q_f(\omega(t))$. The inertia, J includes the motor and the propeller, the motor torque comes from the voltage generator, and the load friction torque results from the propeller drag $Q_f(\omega(t)) = \kappa_\tau \omega^2(t)$, $\kappa_\tau$ is the drag coefficient.

$$J \frac{\partial \omega(t)}{\partial t} = \tau(t) - Q_f(\omega(t)) - D_v \omega(t) \quad (3)$$

where $D_v$ is the viscous damping coefficient of the motor Nms/rad. Typically, the inductance of small, DC motors is neglected compared to the physical response of the system and so can be ignored. Under steady-state conditions, the current $i(t)$ is constant, and equation (1) reduces to :

$$v(t) = Ri(t) + \frac{\omega(t)}{k_v} \quad (4)$$

where the term $\frac{1}{k_v}\omega(t)$ represent the electromotive force of the motor. Table I shows the motor coefficients for the BLDC motor used in our study.

<figure><img src="image_6.png" /><figcaption>Fig. 1. Quadrotor scheme</figcaption></figure>

## III. QUADROTOR DYNAMIC MODEL

In this section, we will first introduce the quadrotor unmanned aerial flying vehicles coordinate system as depicted in Fig. 1. To study the system motion dynamics, two frames are used: an inertial frame attached to the earth defined by $E^a(e_{a1}, e_{a2}, e_{a3})$ and a body-fixed frame $E^b(e_{b1}, e_{b2}, e_{b3})$ fixed to the centre of mass of the quadrotor. The absolute position of the quadrotor is described by $p = [x, y, z]^T$ and its attitude by the Euler angles $\eta = [\phi, \theta, \psi]^T$. The attitude angles are respectively Yaw angle ($\psi$ rotation around z-axis), Pitch angle (($\theta$ rotation around y-axis), and roll angle ($\phi$ rotation around x-axis) [11]. The dynamic model for quadrotor vehicle can be derived as

$$ \begin{aligned} m\ddot{x} &= (\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi)T \\ m\ddot{y} &= (\cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi)T \\ m\ddot{z} &= (\cos \phi \cos \theta)T - mg \\ I_x \ddot{\phi} &= (I_y - I_z)\dot{\theta}\dot{\psi} + J\dot{\theta}\varpi + lu_1 \\ I_y \ddot{\theta} &= (I_z - I_x)\dot{\phi}\dot{\psi} - J\dot{\phi}\varpi + lu_2 \\ I_z \ddot{\psi} &= (I_x - I_y)\dot{\phi}\dot{\theta} + u_3 \end{aligned} \quad (5) $$

where $\varpi = \omega_1 - \omega_2 + \omega_3 - \omega_4$. where J is the rotor inertia, m, $I_x$, $I_y$ and $I_z$ denotes the mass of the quadrotor flying vehicle and inertia, l is the distance from the centre of mass to the rotor shaft, $\kappa_b$ is the thrust factor and $\omega_j$ j = 1, ..., 4 is the motor speed, $g = 9.81m/s^2$ is the acceleration due to gravity.

<table><caption>TABLE I
MOTOR COEFFICIENTS</caption><thead><tr><td>Parameter</td><td>Value</td></tr></thead><tbody><tr><td>J (kg.m<sup>2</sup>)</td><td>4.1904e<sup>-5</sup></td></tr><tr><td>k<sub>t</sub> (N.m/A)</td><td>0.0104e<sup>-3</sup></td></tr><tr><td>k<sub>v</sub> (rad/s/volt)</td><td>96.342</td></tr><tr><td>D<sub>v</sub> (Nms/rad)</td><td>0.2e<sup>-3</sup></td></tr><tr><td>R (Ohm)</td><td>0.2</td></tr></tbody></table>
## B. Contribution and organization

Motivated by the previous discussion, in this paper we introduced two energy optimal control problem, where the objective is to minimise the energy consumed by the quadrotor vehicle at the end of the mission while the quadrotor aircraft has to satisfy boundary conditions and feasibility constraints on the states of aircraft and control inputs. Another objective is to compute the minimum time sped by the vehicle to travelled the trajectory during the mission with fixed energy. An efficiency function was identified, in order to modelled motor efficiency and have an energetic model close to reality.

The rest of this paper is organized as follows. In section 2, we presented actuator model. Then, in section 3 dynamic model of the quadrotor vehicle is presented. In section 4, the problem of trajectory optimisation with minimum energy is presented. The trajectory optimisation minimum time problem is illustrated in section 5. In section 6 nonlinear programming method and simulation results are presented. Conclusions and remarks are drawn in section 7.

## II. ACTUATOR MODEL

A quadrotor UAV actuator system is typically consist of a LiPo battery, a brushless direct current (BLDC) motor and an control stage to control the angular velocity (RPM) of the motor. Electrical DC motors are well modelled by a circuit containing a resistor, inductor, and voltage generator in series [10].

$$v(t) = Ri(t) + L\frac{\partial i(t)}{\partial t} + \frac{\omega(t)}{k_v} \quad (1)$$

where R is the motor internal resistance, L is the inductance, $\omega(t)$ is the rotational rat of the motor, and $k_v$ is the voltage constant of the motor, expressed in rad/s/volt. Also, the motor torque $\tau$ can be modelled as being proportional to the current $i(t)$ through the torque constant, $k_t$, expressed in Nm/A.

$$\tau(t) = k_t i(t) \quad (2)$$

The motor dynamics are modelled as a simple first order differential equation (3) where $\dot{\omega}$ is driven by the motor torque and the load friction torque $Q_f(\omega(t))$. The inertia, J includes the motor and the propeller, the motor torque comes from the voltage generator, and the load friction torque results from the propeller drag $Q_f(\omega(t)) = \kappa_{\tau}\omega^2(t)$, $\kappa_{\tau}$ is the drag coefficient.

$$J \frac{\partial \omega(t)}{\partial t} = \tau(t) - Q_f(\omega(t)) - D_v \omega(t) \quad (3)$$

where $D_v$ is the viscous damping coefficient of the motor Nms/rad. Typically, the inductance of small, DC motors is neglected compared to the physical response of the system and so can be ignored. Under steady-state conditions, the current $i(t)$ is constant, and equation (1) reduces to :

$$v(t) = Ri(t) + \frac{\omega(t)}{k_v} \quad (4)$$

where the term $\frac{1}{k_v}\omega(t)$ represent the electromotive force of the motor. Table I shows the motor coefficients for the BLDC motor used in our study.

<figure><img src="image_6.png" /><figcaption>Fig. 1. Quadrotor scheme</figcaption></figure>

## III. QUADROTOR DYNAMIC MODEL

In this section, we will first introduce the quadrotor unmanned aerial flying vehicles coordinate system as depicted in Fig. 1. To study the system motion dynamics, two frames are used: an inertial frame attached to the earth defined by $E^a(e_{a1}, e_{a2}, e_{a3})$ and a body-fixed frame $E^b(e_{b1}, e_{b2}, e_{b3})$ fixed to the centre of mass of the quadrotor. The absolute position of the quadrotor is described by $p = [x, y, z]^T$ and its attitude by the Euler angles $\eta = [\phi, \theta, \psi]^T$. The attitude angles are respectively Yaw angle ($\psi$ rotation around z-axis), Pitch angle (($\theta$ rotation around y-axis), and roll angle ($\phi$ rotation around x-axis) [11]. The dynamic model for quadrotor vehicle can be derived as

$$ \begin{aligned} m\ddot{x} &= (\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi)T \\ m\ddot{y} &= (\cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi)T \\ m\ddot{z} &= (\cos \phi \cos \theta)T - mg \\ I_x\ddot{\phi} &= (I_y - I_z)\dot{\theta}\dot{\psi} + J\dot{\theta}\varpi + lu_1 \\ I_y\ddot{\theta} &= (I_z - I_x)\dot{\phi}\dot{\psi} - J\dot{\phi}\varpi + lu_2 \\ I_z\ddot{\psi} &= (I_x - I_y)\dot{\phi}\dot{\theta} + u_3 \end{aligned} \quad (5) $$

where $\varpi = \omega_1 - \omega_2 + \omega_3 - \omega_4$. where J is the rotor inertia, m, $I_x$, $I_y$ and $I_z$ denotes the mass of the quadrotor flying vehicle and inertia, l is the distance from the centre of mass to the rotor shaft, $\kappa_b$ is the thrust factor and $\omega_j$ j = 1, ..., 4 is the motor speed, $g = 9.81m/s^2$ is the acceleration due to gravity.

<table><caption>TABLE I
MOTOR COEFFICIENTS</caption><thead><tr><td>Parameter</td><td>Value</td></tr></thead><tbody><tr><td>J (kg.m<sup>2</sup>)</td><td>4.1904e<sup>-5</sup></td></tr><tr><td>k<sub>t</sub> (N.m/A)</td><td>0.0104e<sup>-3</sup></td></tr><tr><td>k<sub>v</sub> (rad/s/volt)</td><td>96.342</td></tr><tr><td>D<sub>v</sub> (Nms/rad)</td><td>0.2e<sup>-3</sup></td></tr><tr><td>R (Ohm)</td><td>0.2</td></tr></tbody></table>
The control inputs are given as follows:

$$
\left\{
\begin{aligned}
T &= \kappa_b(\omega_1^2 + \omega_2^2 + \omega_3^2 + \omega_4^2) \\
u_1 &= \kappa_b(\omega_2^2 - \omega_4^2) \\
u_2 &= \kappa_b(\omega_3^2 - \omega_1^2) \\
u_3 &= \kappa_\tau(\omega_1^2 - \omega_2^2 + \omega_3^2 - \omega_4^2)
\end{aligned}
\right.
$$

Remark 1. The Euler angles roll and pitch are assumed to be limited to $-\pi/2 < \phi < \pi/2$, $-\pi/2 < \theta < \pi/2$. This assumption is common in practice since the quadrotor vehicle does not perform aggressive manoeuvres over free flight.

IV. TRAJECTORY OPTIMISATION WITH MINIMUM ENERGY

The trajectory optimisation problem seeks to find the tra-
jectory for quadrotor helicopter that minimize the consumed
energy while satisfying a set of constraints on states and
control inputs. In view of optimal control, we try to compute
an open-loop solution to an optimal control problem. Firsts let
define the energy consumed by the vehicle during the mission.

$$
E_c = \int_{t_0}^{t_f} \sum_{j=1}^{4} \tau_j(t) \omega_j(t) dt \quad (6)
$$

with $\tau_j(t)$ is the torque generated by motor $j$ and $\omega_j(t)$ is rotor speed at time $t$. By using equation (3) for the four motors, equation (6) can be rewrite as follow:

$$
E_c = \int_{t_0}^{t_f} \sum_{j=1}^{4} \left( \dot{\omega}_j(t) + \kappa_\tau \omega_j^2(t) + D_v \omega_j(t) \right) \omega_j(t) dt \quad (7)
$$

A. Motor efficiency

In order to work with an energetic model close to reality,
an efficiency function is identified and added to energy func-
tion (7). The efficiency of the brushless dc motor used for
actuate quadrotor helicopter is function of motor torque and
rotor speed $f_r(\tau(t), \omega(t))$. We have used polynomial interpo-
lation for efficiency function identification, thus $f_r(\tau(t), \omega(t))$
can be formulated as follow:

$$
f_r(\tau(t), \omega(t)) = a(\omega(t))\tau^3(t) + b(\omega(t))\tau^2(t) \\
\qquad + c(\omega(t))\tau(t) + d(\omega(t)) \quad (8)
$$

and

$$
a(\omega(t)) = a_1 \omega^2(t) + b_1 \omega(t) + c_1
$$

$$
b(\omega(t)) = a_2 \omega^2(t) + b_2 \omega(t) + c_2
$$

$$
c(\omega(t)) = a_3 \omega^2(t) + b_3 \omega(t) + c_3
$$

$$
d(\omega(t)) = a_4 \omega^2(t) + b_4 \omega(t) + c_4
$$

The parameters of the polynomial are calculated using Matlab
for the four motors (we assume that the motors are identical).

$$
a_1 = -1.72 \ 10^{-5} \qquad b_1 = 0.014 \qquad c_1 = -0.8796
$$

$$
a_2 = 1.95 \ 10^{-5} \qquad b_2 = -0.0157 \qquad c_2 = 0.3385
$$

$$
a_3 = -6.98 \ 10^{-6} \qquad b_3 = 5.656 \ 10^{-3} \qquad c_3 = 0.2890
$$

$$
a_4 = 4.09 \ 10^{-7} \qquad b_4 = -3.908 \ 10^{-4} \qquad c_4 = 0.1626
$$

Then the consumed energy (7) can be rewrite as

$$
E_c = \int_{t_0}^{t_f} \sum_{j=1}^{4} \frac{(\dot{\omega}_j(t) + \kappa_\tau \omega_j^2(t) + D_v \omega_j(t))}{f_{r,j}(\tau_j(t), \omega_j(t))} \omega_j(t) dt \quad (9)
$$

B. Problem statement

Now the problem of optimal energy trajectory planning
can be formulated as a minimisation problem, by which the
final consumed energy $E_c(tf)$ is used as the cost function. In
addition the state variables in $[x, \dot{x}, y, \dot{y}, z, \dot{z}, \phi, \dot{\phi}, \theta, \dot{\theta}, \psi, \dot{\psi}]^T$
and control variables in $[\omega_1, \omega_2, \omega_3, \omega_4]^T$ are constrained to
satisfy the vehicle dynamics (5) and boundary conditions. The
mission is to fly between specified initial and final positions
during a time interval $[t_0, tf]$ where $t_0$ and $tf$ are given.

Based on the above description, the optimal control problem
can be formulated as:

$$
\min_{(\omega_j, \tau_j)} E_c(t_f) \qquad (10)
$$

subject to

$$
m\ddot{x} = (\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi)T
$$

$$
m\ddot{y} = (\cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi)T
$$

$$
m\ddot{z} = (\cos \phi \cos \theta)T - mg
$$

$$
I_x \ddot{\phi} = (I_y - I_z) \dot{\theta} \dot{\psi} + J \dot{\theta} \dot{\varpi} + lu_1
$$

$$
I_y \ddot{\theta} = (I_z - I_x) \dot{\phi} \dot{\psi} - J \dot{\phi} \dot{\varpi} + lu_2
$$

$$
I_z \ddot{\psi} = (I_x - I_y) \dot{\phi} \dot{\theta} + u_3 \quad (11)
$$

and

$$
|\phi| \le \frac{\pi}{2}, \quad |\theta| \le \frac{\pi}{2}, \quad |\dot{\psi}| \le \dot{\Psi}
$$

$$
\omega_{\min} \leq \omega_j \leq \omega_{\max}
$$

$$
0 \le T \le T_{\max}, \quad |u_k| \le u_{\max}, \quad k = 1, 2, 3 \qquad (12)
$$

with boundary conditions:

$$
\begin{align}
[x(t_0), y(t_0), z(t_0), \phi(t_0), \theta(t_0), \psi(t_0)]^T = \nonumber \\
& [x_0, y_0, z_0, \phi_0, \theta_0, \psi_0]^T \nonumber \\
& [x(t_f), y(t_f), z(t_f)]^T = [x_f, y_f, z_f]^T \tag{13}
\end{align}
$$

The additional constraints in (12) are associated with vehicle
dynamics where $\omega_{\min}$ and $\omega_{\max}$ are the minimum and maxi-
mum feasible velocity of the aircraft rotors, respectively. The
roll and pitch angles, $\phi$, $\theta$, have to satisfy $|\phi| \le \frac{\pi}{2}$, $|\theta| \le \frac{\pi}{2}$
based on their physical definition, and $|\dot{\psi}| \le \dot{\Psi}$ is required
to generate a smooth trajectory where $\dot{\Psi}$ is the maximum
changing rate of the heading angle.

V. TRAJECTORY OPTIMISATION WITH MINIMUM TIME

The problem of minimum time path generation is a sub-
problem in optimal control, the objective is to calculate the
minimum time spent by the aircraft while travelling between
initial and final position with fixed energy. The state variable
are [x, ˙x, y, ˙y, z, ˙z, φ, ˙φ, θ, ˙θ, ψ, ˙ψ, ˙E<sub>c</sub>]<sup>T</sup> and control variables
[ω<sub>1</sub>, ω<sub>2</sub>, ω<sub>3</sub>, ω<sub>4</sub>]<sup>T</sup>, while the mission is the same as in the first
problem except the final time t<sub>f</sub> which is free in this case.
The control inputs are given as follows:

$$
\left\{
\begin{aligned}
T &= \kappa_b(\omega_1^2 + \omega_2^2 + \omega_3^2 + \omega_4^2) \\
u_1 &= \kappa_b(\omega_2^2 - \omega_4^2) \\
u_2 &= \kappa_b(\omega_3^2 - \omega_1^2) \\
u_3 &= \kappa_\tau(\omega_1^2 - \omega_2^2 + \omega_3^2 - \omega_4^2)
\end{aligned}
\right.
$$

Remark 1. The Euler angles roll and pitch are assumed to be limited to $-\pi/2 < \phi < \pi/2$, $-\pi/2 < \theta < \pi/2$. This assumption is common in practice since the quadrotor vehicle does not perform aggressive manoeuvres over free flight.

IV. TRAJECTORY OPTIMISATION WITH MINIMUM ENERGY

The trajectory optimisation problem seeks to find the tra-
jectory for quadrotor helicopter that minimize the consumed
energy while satisfying a set of constraints on states and
control inputs. In view of optimal control, we try to compute
an open-loop solution to an optimal control problem. Firsts let
define the energy consumed by the vehicle during the mission.

$$
E_c = \int_{t_0}^{t_f} \sum_{j=1}^{4} \tau_j(t) \omega_j(t) dt \quad (6)
$$

with $\tau_j(t)$ is the torque generated by motor $j$ and $\omega_j(t)$ is rotor speed at time $t$. By using equation (3) for the four motors, equation (6) can be rewrite as follow:

$$
E_c = \int_{t_0}^{t_f} \sum_{j=1}^{4} \left( \dot{\omega}_j(t) + \kappa_\tau \omega_j^2(t) + D_v \omega_j(t) \right) \omega_j(t) dt \quad (7)
$$

A. *Motor efficiency*

In order to work with an energetic model close to reality,
an efficiency function is identified and added to energy func-
tion (7). The efficiency of the brushless dc motor used for
actuate quadrotor helicopter is function of motor torque and
rotor speed $f_r(\tau(t), \omega(t))$. We have used polynomial interpo-
lation for efficiency function identification, thus $f_r(\tau(t), \omega(t))$
can be formulated as follow:

$$
f_r(\tau(t), \omega(t)) = a(\omega(t))\tau^3(t) + b(\omega(t))\tau^2(t) \\
\qquad + c(\omega(t))\tau(t) + d(\omega(t)) \quad (8)
$$

and

$$
a(\omega(t)) = a_1 \omega^2(t) + b_1 \omega(t) + c_1
$$

$$
b(\omega(t)) = a_2 \omega^2(t) + b_2 \omega(t) + c_2
$$

$$
c(\omega(t)) = a_3 \omega^2(t) + b_3 \omega(t) + c_3
$$

$$
d(\omega(t)) = a_4 \omega^2(t) + b_4 \omega(t) + c_4
$$

The parameters of the polynomial are calculated using Matlab
for the four motors (we assume that the motors are identical).

$$
a_1 = -1.72 \ 10^{-5} \qquad b_1 = 0.014 \qquad c_1 = -0.8796
$$

$$
a_2 = 1.95 \ 10^{-5} \qquad b_2 = -0.0157 \qquad c_2 = 0.3385
$$

$$
a_3 = -6.98 \ 10^{-6} \qquad b_3 = 5.656 \ 10^{-3} \qquad c_3 = 0.2890
$$

$$
a_4 = 4.09 \ 10^{-7} \qquad b_4 = -3.908 \ 10^{-4} \qquad c_4 = 0.1626
$$

Then the consumed energy (7) can be rewrite as

$$
E_c = \int_{t_0}^{t_f} \sum_{j=1}^{4} \frac{(\dot{\omega}_j(t) + \kappa_\tau \omega_j^2(t) + D_v \omega_j(t))}{f_{r,j}(\tau_j(t), \omega_j(t))} \omega_j(t) dt \quad (9)
$$

B. Problem statement

Now the problem of optimal energy trajectory planning
can be formulated as a minimisation problem, by which the
final consumed energy $E_c(tf)$ is used as the cost function. In
addition the state variables in $[x, \dot{x}, y, \dot{y}, z, \dot{z}, \phi, \dot{\phi}, \theta, \dot{\theta}, \psi, \dot{\psi}]^T$
and control variables in $[\omega_1, \omega_2, \omega_3, \omega_4]^T$ are constrained to
satisfy the vehicle dynamics (5) and boundary conditions. The
mission is to fly between specified initial and final positions
during a time interval $[t_0, tf]$ where $t_0$ and $tf$ are given.

Based on the above description, the optimal control problem
can be formulated as:

$$
\min_{(\omega_j, \tau_j)} E_c(t_f) \qquad (10)
$$

subject to

$$
m\ddot{x} = (\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi)T
$$

$$
m\ddot{y} = (\cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi)T
$$

$$
m\ddot{z} = (\cos \phi \cos \theta)T - mg
$$

$$
I_x \ddot{\phi} = (I_y - I_z) \dot{\theta} \dot{\psi} + J \dot{\theta} \dot{\varpi} + lu_1
$$

$$
I_y \ddot{\theta} = (I_z - I_x) \dot{\phi} \dot{\psi} - J \dot{\phi} \dot{\varpi} + lu_2
$$

$$
I_z \ddot{\psi} = (I_x - I_y) \dot{\phi} \dot{\theta} + u_3 \quad (11)
$$

and

$$
|\phi| \le \frac{\pi}{2}, \quad |\theta| \le \frac{\pi}{2}, \quad |\dot{\psi}| \le \dot{\Psi}
$$

$$
\omega_{\min} \le \omega_j \le \omega_{\max}
$$

$$
0 \le T \le T_{\max}, \quad |u_k| \le u_{\max}, \quad k = 1, 2, 3 \qquad (12)
$$

with boundary conditions:

$$
\begin{align}
[x(t_0), y(t_0), z(t_0), \phi(t_0), \theta(t_0), \psi(t_0)]^T &= \nonumber \\
&[x_0, y_0, z_0, \phi_0, \theta_0, \psi_0]^T \nonumber \\
[x(t_f), y(t_f), z(t_f)]^T &= [x_f, y_f, z_f]^T \tag{13}
\end{align}
$$

The additional constraints in (12) are associated with vehicle
dynamics where $\omega_{\min}$ and $\omega_{\max}$ are the minimum and maxi-
mum feasible velocity of the aircraft rotors, respectively. The
roll and pitch angles, $\phi$, $\theta$, have to satisfy $|\phi| \le \frac{\pi}{2}$, $|\theta| \le \frac{\pi}{2}$
based on their physical definition, and $|\dot{\psi}| \le \dot{\Psi}$ is required
to generate a smooth trajectory where $\dot{\Psi}$ is the maximum
changing rate of the heading angle.

V. TRAJECTORY OPTIMISATION WITH MINIMUM TIME

The problem of minimum time path generation is a sub-
problem in optimal control, the objective is to calculate the
minimum time spent by the aircraft while travelling between
initial and final position with fixed energy. The state variable
are [x, ˙x, y, ˙y, z, ˙z, φ, ˙φ, θ, ˙θ, ψ, ˙ψ, ˙E<sub>c</sub>]<sup>T</sup> and control variables
[ω<sub>1</sub>, ω<sub>2</sub>, ω<sub>3</sub>, ω<sub>4</sub>]<sup>T</sup>, while the mission is the same as in the first
problem except the final time t<sub>f</sub> which is free in this case.
The optimal control problem can be formulated as:

$$ \min_{(\omega_j, \tau_j)} t_f \qquad (14) $$

subject to

$$ m\ddot{x} = (\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi)T $$

$$ m\ddot{y} = (\cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi)T $$

$$ m\ddot{z} = (\cos \phi \cos \theta)T - mg $$

$$ I_x \ddot{\phi} = (I_y - I_z) \dot{\theta} \dot{\psi} + J \dot{\theta} \dot{\varpi} + lu_1 $$

$$ I_y \ddot{\psi} = (I_z - I_x) \dot{\phi} \dot{\psi} - J \dot{\phi} \dot{\varpi} + lu_2 $$

$$ I_z \ddot{\psi} = (I_x - I_y) \dot{\phi} \dot{\theta} + u_3 $$

$$ \dot{E}_c = \sum_{j=1}^{4} \frac{1}{f_{r,j}(\tau_j, \omega_j)} (\dot{\omega}_j + \kappa_{\tau} \omega_j^2 + D_v \omega_j) \omega_j \quad (15) $$

and

$$ |\phi| \le \frac{\pi}{2}, \quad |\theta| \le \frac{\pi}{2}, \quad |\dot{\psi}| \le \dot{\Psi} $$

$$ \omega_{\min} \le \omega_j \le \omega_{\max} $$

$$ 0 \le T \le T_{\max}, \quad |u_k| \le u_{\max}, \quad k = 1, 2, 3 \qquad (16) $$

with boundary conditions:

$$ \begin{aligned} [x(t_0), y(t_0), z(t_0), \phi(t_0), \theta(t_0), \psi(t_0), E_c(t_0)]^T &= [x_0, y_0, z_0, \phi_0, \theta_0, \psi_0, E_{c,0}]^T \\ [x(t_f), y(t_f), z(t_f), E_c(t_f)]^T &= [x_f, y_f, z_f, E_{c,f}]^T \end{aligned} \quad (17) $$

Note that the energy function $E_c$ has been treated as a state in this problem, with boundary conditions $E_c(t_0) = E_{c,0}$ and $E_c(t_f) = E_{c,f}$.

## VI. NONLINEAR PROGRAMMING AND SIMULATION RESULTS

### A. Nonlinear programming method

The optimal control problems presented in the previous sections (10)-(13) and (14)-(V) are a complex nonlinear optimization problems. The general approach to solve this problem is the direct collocation method. The basic idea of direct collocation is to discretize a continuous solution to a problem represented by state and control variables by using linear interpolation to satisfy the differential equations. In this way an optimal control problem is transformed into a nonlinear programming problem (NLPP).

In our study, the proposed optimal control problems have been numerically solved using a Matlab software called GPOPS-II [12]. The software employs a Legendre-Gauss-Radau (LGR) [13][14] quadrature orthogonal collocation method where the continuous-time optimal control problem is transcribed to a large sparse nonlinear programming problem (NLP). It used an adaptive mesh refinement method that determines the number of mesh intervals and the degree of the approximating polynomial within each mesh interval to achieve a specified accuracy. The software allows the use of two nonlinear programming (NLP) solver used to solve the NLPP. The first is the open-source NLP solver IPOPT (Interior Point OPTimizer) [15], where the second is the NLP solver SNOPT (Sparse Nonlinear OPTimizer) [16].

<figure><img src="image_7.png" /><figcaption>Fig. 2. Quadrotor position (a) and linear velocity during the trajectory (b)</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 3. Quadrotor attitude (a) and angular velocity (b)</figcaption></figure>

### B. Simulation Results

Problem (10)-(13) and (14)-(17) were solved using the open-source NLP solver IPOPT in second derivative (full Newton) mode with the publicly available multi-frontal massively parallel sparse direct linear solver MUMPS [17]. All results were obtained using the implicit integration form of the Radau collocation method and various forms of the aforementioned ph mesh refinement method using default NLP solver settings and the automatic scaling routine in GPOPS-II.

In our tests we considered the DJI Phantom 2 quadrotor [18] with multi-rotor propulsion system (2212/920KV motors). The physical parameters of the Phantom 2 used in the simulation experiment, are reported in Table II.

The problem (10)-(13) was numerically solved to find the minimum energy control inputs $\omega_j(t)$ that allows quadrotor to fly from the initial position $[x, y, z]^T = [0, 0, 0]^T$ at time $t_0 = 0$ to the final one $[x, y, z]^T = [4, 5, 6]^T$ at time $t_f = 10s$, with initial condition $[x_0, y_0, z_0, \phi_0, \theta_0, \psi_0]^T = [0_{1 \times 6}]^T$ and final condition $[x_f, y_f, z_f, \phi_f, \theta_f, \psi_f]^T = [4, 5, 6, 0, 0, 0]^T$. Null initial angular and linear velocities were considered $[\dot{x}_0, \dot{y}_0, \dot{z}_0, \dot{\phi}_0, \dot{\theta}_0, \dot{\psi}_0]^T = [0_{1 \times 6}]^T$ and the same for final angular and linear velocities. With respect to constraints in (12) the initial guess for inputs control is given by $w_s = 912, 32$ rad/s which means that the total thrust is $T = 12, 75$ N which corresponds the thrust necessary to counterbalance the gravity acceleration. Fig. 2 shows the time evolutions of the vehicle position and linear velocity, were Fig. 3 shows the time evolutions of the vehicle attitude and angular velocity. Fig. 4 reports The optimal trajectory in $(x - y - z)$ space and the control inputs $\omega_j(t)$. The energy consumed by the quadrotor to ravel this trajectory is $E_c(t_f) = 10.48$ kJ.

In order to evaluate the energy consumed by the vehicle
The optimal control problem can be formulated as:

$$ \min_{(\omega_j, \tau_j)} t_f \qquad (14) $$

subject to

$$ m\ddot{x} = (\cos \phi \sin \theta \cos \psi + \sin \phi \sin \psi)T $$

$$ m\ddot{y} = (\cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi)T $$

$$ m\ddot{z} = (\cos \phi \cos \theta)T - mg $$

$$ I_x \ddot{\phi} = (I_y - I_z) \dot{\theta} \dot{\psi} + J \dot{\theta} \dot{\varpi} + lu_1 $$

$$ I_y \ddot{\theta} = (I_z - I_x) \dot{\phi} \dot{\psi} - J \dot{\phi} \dot{\varpi} + lu_2 $$

$$ I_z \ddot{\psi} = (I_x - I_y) \dot{\phi} \dot{\theta} + u_3 $$

$$ \dot{E}_c = \sum_{j=1}^{4} \frac{1}{f_{r,j}(\tau_j, \omega_j)} (\dot{\omega}_j + \kappa_{\tau} \omega_j^2 + D_v \omega_j) \omega_j \quad (15) $$

and

$$ |\phi| \le \frac{\pi}{2}, \quad |\theta| \le \frac{\pi}{2}, \quad |\dot{\psi}| \le \dot{\Psi} $$

$$ \omega_{\min} \le \omega_j \le \omega_{\max} $$

$$ 0 \le T \le T_{\max}, \quad |u_k| \le u_{\max}, \quad k = 1, 2, 3 \qquad (16) $$

with boundary conditions:

$$ \begin{aligned} [x(t_0), y(t_0), z(t_0), \phi(t_0), \theta(t_0), \psi(t_0), E_c(t_0)]^T &= [x_0, y_0, z_0, \phi_0, \theta_0, \psi_0, E_{c,0}]^T \\ [x(t_f), y(t_f), z(t_f), E_c(t_f)]^T &= [x_f, y_f, z_f, E_{c,f}]^T \end{aligned} \quad (17) $$

Note that the energy function $E_c$ has been treated as a state in this problem, with boundary conditions $E_c(t_0) = E_{c,0}$ and $E_c(t_f) = E_{c,f}$.

# VI. NONLINEAR PROGRAMMING AND SIMULATION RESULTS

## A. Nonlinear programming method

The optimal control problems presented in the previous sections (10)-(13) and (14)-(V) are a complex nonlinear optimization problems. The general approach to solve this problem is the direct collocation method. The basic idea of direct collocation is to discretize a continuous solution to a problem represented by state and control variables by using linear interpolation to satisfy the differential equations. In this way an optimal control problem is transformed into a nonlinear programming problem (NLPP).

In our study, the proposed optimal control problems have been numerically solved using a Matlab software called GPOPS-II [12]. The software employs a Legendre-Gauss-Radau (LGR) [13][14] quadrature orthogonal collocation method where the continuous-time optimal control problem is transcribed to a large sparse nonlinear programming problem (NLP). It used an adaptive mesh refinement method that determines the number of mesh intervals and the degree of the approximating polynomial within each mesh interval to achieve a specified accuracy. The software allows the use of two nonlinear programming (NLP) solver used to solve the NLPP. The first is the open-source NLP solver IPOPT (Interior Point OPTimizer) [15], where the second is the NLP solver SNOPT (Sparse Nonlinear OPTimizer) [16].

<figure><img src="image_7.png" /><figcaption>Fig. 2. Quadrotor position (a) and linear velocity during the trajectory (b)</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 3. Quadrotor attitude (a) and angular velocity (b)</figcaption></figure>

## B. Simulation Results

Problem (10)-(13) and (14)-(17) were solved using the open-source NLP solver IPOPT in second derivative (full Newton) mode with the publicly available multi-frontal massively parallel sparse direct linear solver MUMPS [17]. All results were obtained using the implicit integration form of the Radau collocation method and various forms of the aforementioned ph mesh refinement method using default NLP solver settings and the automatic scaling routine in GPOPS-II.

In our tests we considered the DJI Phantom 2 quadrotor [18] with multi-rotor propulsion system (2212/920KV motors). The physical parameters of the Phantom 2 used in the simulation experiment, are reported in Table II.

The problem (10)-(13) was numerically solved to find the minimum energy control inputs $\omega_j(t)$ that allows quadrotor to fly from the initial position $[x, y, z]^T = [0, 0, 0]^T$ at time $t_0 = 0$ to the final one $[x, y, z]^T = [4, 5, 6]^T$ at time $t_f = 10s$, with initial condition $[x_0, y_0, z_0, \phi_0, \theta_0, \psi_0]^T = [0_{1 \times 6}]^T$ and final condition $[x_f, y_f, z_f, \phi_f, \theta_f, \psi_f]^T = [4, 5, 6, 0, 0, 0]^T$. Null initial angular and linear velocities were considered $[\dot{x}_0, \dot{y}_0, \dot{z}_0, \dot{\phi}_0, \dot{\theta}_0, \dot{\psi}_0]^T = [0_{1 \times 6}]^T$ and the same for final angular and linear velocities. With respect to constraints in (12) the initial guess for inputs control is given by $w_s = 912, 32 rad/s$ which means that the total thrust is $T = 12, 75 N$ which corresponds the thrust necessary to counterbalance the gravity acceleration. Fig. 2 shows the time evolutions of the vehicle position and linear velocity, were Fig. 3 shows the time evolutions of the vehicle attitude and angular velocity. Fig. 4 reports The optimal trajectory in $(x - y - z)$ space and the control inputs $\omega_j(t)$. The energy consumed by the quadrotor to ravel this trajectory is $E_c(t_f) = 10.48 kJ$.

In order to evaluate the energy consumed by the vehicle
<figure><img src="image_9.png" /><figcaption>Fig. 4. The optimal trajectory in (x - y - z) space (a), and the control inputs (b)</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>Fig. 5. Quadrotor position (a) and linear velocity during the trajectory (b)</figcaption></figure>

and have an idea about the saving energy with the proposed approach, we have compared the energy consumed by the quadrotor vehicle under our optimal control approach and the energy consumed under a classical control approach.

The classical control approach consiste of two controller, a low-level controller and the high-level control algorithm for quadrotor vehicle. The low-level control module consist of an adaptive fuzzy backstepping controller[19] [20]. Where the high-level control approach consist of a high order polynomial trajectory planning algorithm with initial and final conditions on velocity.

The adaptive fuzzy backstepping controller is composed of three terms , the fuzzy adaptive control term which is designed to approximate a model-based backstepping control law $u_{f,j} = \Theta_j^T \varphi_j(x_s)$, where $\Theta_j^T$ is the vector of fuzzy basis functions, $\varphi_j(x_s)$ is the vector of adjustable parameters of the fuzzy logic system and $x_s$ is the state vector. The bounded robust control term $u_{r,j} = \hat{\delta}_j \tanh(\frac{e_{2j}}{\epsilon_j})$ employed to compensate the fuzzy approximation error. Finally, $u_{p,j} = k_{2j}e_{2j}$ the proportional derivative term.

Before proceeding the robust control scheme, we define the tracking errors as:

$$e_{1j} = x_{1j,d} - x_{1j}, \quad j = 1, \dots, 6 \quad (18)$$

where $x_{1j,d}$ is the position and attitude desired signals. Using position controller $u_5$ and $u_6$ (19), the desired roll and pitch signals can be calculated as $\theta_d = atan(\frac{u_5 \cos \psi + u_6 \sin \psi}{g})$, $\phi_d = atan(\frac{u_5 \sin \psi - u_6 \cos \psi}{g} \cos \theta_d)$. The backstepping second tracking errors signals is defined as

$$e_{2j} = v_j - x_{2j} \quad (19)$$

with the virtual control law $v_i = \dot{x}_{1j,d} + k_{1j}e_{1j}$ and $k_{1j} > 0$. Then, we introduce the following tracking control algorithm.

$$u_j(t) = \Theta_j^T \varphi_j(x_s) + \hat{\delta}_j \tanh \left( \frac{e_{2j}}{\epsilon_j} \right) + k_{2j} e_{2j} \quad (20)$$

$$\dot{\Theta}_j = \gamma_j e_{2i} \varphi_j(x_s) \quad (21)$$

$$\dot{\hat{\delta}}_j = \eta_j e_{2j} \tanh \left( \frac{e_{2j}}{\epsilon_j} \right) \quad (22)$$

where $k_{2j} > 0$, $\epsilon_j > 0$, $\gamma_j > 0$, $\eta_j > 0$ are design parameters.

<figure><img src="image_11.png" /><figcaption>Fig. 6. Quadrotor attitude (a) and angular velocity (b)</figcaption></figure>

The high-level control algorithm consist of a third degree polynomial, $q(t) = \alpha_0 + \alpha_1 t + \alpha_2 t^2 + \alpha_3 t^3$, with initial and final conditions on position and velocity $q(t_0) = q_0$, $q(t_f) = q_f$, $\dot{q}(t_0) = \dot{q}_0$, $\dot{q}(t_f) = \dot{q}_f$ we can calculate parameters of the polynomial. The consumed energy obtained with the classical approach is $E_c(t_f) = 10.49 \text{ kJ}$, compared to the energy consumed by the proposed approach, its increase with 0.1% from the total energy, which is not a good saving quantity of energy. To improve the saving quantity of energy we must make the model more energetic, which will be the objective of future work.

<table><caption>TABLE II
QUADROTOR PARAMETERS</caption><thead><tr><th>Parameter</th><th>Value</th></tr></thead><tbody><tr><td>$l$ (m)</td><td>0.175</td></tr><tr><td>$m$ (kg)</td><td>1.3</td></tr><tr><td>$I_x$ (kgm<sup>2</sup>)</td><td>0.081</td></tr><tr><td>$I_y$ (kgm<sup>2</sup>)</td><td>0.081</td></tr><tr><td>$I_z$ (kgm<sup>2</sup>)</td><td>0.142</td></tr><tr><td>$\kappa_b$ (N/rad/s)</td><td>3.8305e<sup>-6</sup></td></tr><tr><td>$\kappa_\tau$ (Nm/rad/s)</td><td>2.2518e<sup>-8</sup></td></tr></tbody></table>

The problem (14)-(17) was numerically solved to find the minimum time control inputs $\omega_j(t)$ that allows quadrotor to fly from the initial position $[x, y, z]^T = [0, 0, 0]^T$ at time $t_0 = 0$ to the final one $[x, y, z]^T = [4, 5, 6]^T$ with final energy $E_c(t_f) = 3.5 \text{ kJ}$, with same initial and final conditions as the previous problem and $E_c(t_0) = 0 \text{ kJ}$. Simulation results for this test are reported in Fig. 5, Fig. 6 and Fig. 7. Its clear from Fig. 7b that the control inputs $\omega_j(t)$ are saturated and it is evident since the actuator are more solicited to allows the vehicle travelling the trajectory in a minimum time. The optimal time spend to travel the trajectory with respect to constraints in (12) is 3.06s.
<figure><img src="image_9.png" /><figcaption>Fig. 4. The optimal trajectory in (x - y - z) space (a), and the control inputs (b)</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>Fig. 5. Quadrotor position (a) and linear velocity during the trajectory (b)</figcaption></figure>

and have an idea about the saving energy with the proposed approach, we have compared the energy consumed by the quadrotor vehicle under our optimal control approach and the energy consumed under a classical control approach.

The classical control approach consiste of two controller, a low-level controller and the high-level control algorithm for quadrotor vehicle. The low-level control module consist of an adaptive fuzzy backstepping controller[19] [20]. Where the high-level control approach consist of a high order polynomial trajectory planning algorithm with initial and final conditions on velocity.

The adaptive fuzzy backstepping controller is composed of three terms , the fuzzy adaptive control term which is designed to approximate a model-based backstepping control law $u_{f,j} = \Theta_j^T \varphi_j(x_s)$, where $\Theta_j^T$ is the vector of fuzzy basis functions, $\varphi_j(x_s)$ is the vector of adjustable parameters of the fuzzy logic system and $x_s$ is the state vector. The bounded robust control term $u_{r,j} = \hat{\delta}_j \tanh(\frac{e_{2j}}{\epsilon_j})$ employed to compensate the fuzzy approximation error. Finally, $u_{p,j} = k_{2j}e_{2j}$ the proportional derivative term.

Before proceeding the robust control scheme, we define the tracking errors as:

$$e_{1j} = x_{1j,d} - x_{1j}, \quad j = 1, \dots, 6 \quad (18)$$

where $x_{1j,d}$ is the position and attitude desired signals. Using position controller $u_5$ and $u_6$ (19), the desired roll and pitch signals can be calculated as $\theta_d = atan(\frac{u_5 \cos \psi + u_6 \sin \psi}{g})$, $\phi_d = atan(\frac{u_5 \sin \psi - u_6 \cos \psi}{g} \cos \theta_d)$. The backstepping second tracking errors signals is defined as

$$e_{2j} = v_j - x_{2j} \quad (19)$$

with the virtual control law $v_i = \dot{x}_{1j,d} + k_{1j}e_{1j}$ and $k_{1j} > 0$. Then, we introduce the following tracking control algorithm.

$$u_j(t) = \Theta_j^T \varphi_j(x_s) + \hat{\delta}_j \tanh \left( \frac{e_{2j}}{\epsilon_j} \right) + k_{2j} e_{2j} \quad (20)$$

$$\dot{\Theta}_j = \gamma_j e_{2i} \varphi_j(x_s) \quad (21)$$

$$\dot{\hat{\delta}}_j = \eta_j e_{2j} \tanh \left( \frac{e_{2j}}{\epsilon_j} \right) \quad (22)$$

where $k_{2j} > 0$, $\epsilon_j > 0$, $\gamma_j > 0$, $\eta_j > 0$ are design parameters.

<figure><img src="image_11.png" /><figcaption>Fig. 6. Quadrotor attitude (a) and angular velocity (b)</figcaption></figure>

The high-level control algorithm consist of a third degree polynomial, $q(t) = \alpha_0 + \alpha_1 t + \alpha_2 t^2 + \alpha_3 t^3$, with initial and final conditions on position and velocity $q(t_0) = q_0$, $q(t_f) = q_f$, $\dot{q}(t_0) = \dot{q}_0$, $\dot{q}(t_f) = \dot{q}_f$ we can calculate parameters of the polynomial. The consumed energy obtained with the classical approach is $E_c(t_f) = 10.49 \text{ kJ}$, compared to the energy consumed by the proposed approach, its increase with 0.1% from the total energy, which is not a good saving quantity of energy. To improve the saving quantity of energy we must make the model more energetic, which will be the objective of future work.

<table><caption>TABLE II
QUADROTOR PARAMETERS</caption><thead><tr><th>Parameter</th><th>Value</th></tr></thead><tbody><tr><td>$l$ (m)</td><td>0.175</td></tr><tr><td>$m$ (kg)</td><td>1.3</td></tr><tr><td>$I_x$ (kgm<sup>2</sup>)</td><td>0.081</td></tr><tr><td>$I_y$ (kgm<sup>2</sup>)</td><td>0.081</td></tr><tr><td>$I_z$ (kgm<sup>2</sup>)</td><td>0.142</td></tr><tr><td>$\kappa_b$ (N/rad/s)</td><td>3.8305e<sup>-6</sup></td></tr><tr><td>$\kappa_\tau$ (Nm/rad/s)</td><td>2.2518e<sup>-8</sup></td></tr></tbody></table>

The problem (14)-(17) was numerically solved to find the minimum time control inputs $\omega_j(t)$ that allows quadrotor to fly from the initial position $[x, y, z]^T = [0, 0, 0]^T$ at time $t_0 = 0$ to the final one $[x, y, z]^T = [4, 5, 6]^T$ with final energy $E_c(t_f) = 3.5 \text{ kJ}$, with same initial and final conditions as the previous problem and $E_c(t_0) = 0 \text{ kJ}$. Simulation results for this test are reported in Fig. 5, Fig. 6 and Fig. 7. Its clear from Fig. 7b that the control inputs $\omega_j(t)$ are saturated and it is evident since the actuator are more solicited to allows the vehicle travelling the trajectory in a minimum time. The optimal time spend to travel the trajectory with respect to constraints in (12) is 3.06s.
<figure><img src="image_12.png" /><figcaption>Fig. 7. The optimal trajectory in (x - y - z) space (a), and the control inputs (b)</figcaption></figure>

VII. CONCLUSION AND FUTURE WORK

Motivated by the fact that the problem of trajectory op-
timisation with minimum energy for rotary wings UAVs
applications have received less interest by unmanned aerial
systems research community, in this paper two optimal control
problems have been introduced and solved using an optimal
control software. The first problem seeks to find the trajectory
that minimize the consumed energy for a quadrotor helicopter
during a simple mission. Where in the second, minimum time
trajectory is computed for a desired energy. An efficiency
function was identified, in order to have an energetic model
close to reality. The numerical experiments illustrated the
solutions of the proposed optimal control problem, and the
comparative study provide quantisation of energy that can be
saved in a mission.

In future work, we will incorporate a dynamic model for
LiPo battery, and we will introduce an energy optimization
problem with respect to battery life. We are also going to
advise an experimental procedure to to highlight the effect of
path planning of energy consumption.

REFERENCES

[1] K. A. Swieringa, C. B. Hanson, J. R. Richardson, J. D. White, Z. Hasan, E. Qian, and A. Girard, "Autonomous battery swapping system for small-scale helicopters," in *Robotics and Automation (ICRA), IEEE International Conference on*, 2010, pp. 3335-3340.

[2] J. Leonard, A. Savvaris, and A. Tsourdos, "Energy management in swarm of unmanned aerial vehicles," *Journal of Intelligent & Robotic Systems*, vol. 74, no. 1-2, pp. 233–250, 2014.

[3] N. K. Ure, G. Chowdhary, T. Toksoz, J. P. How, M. A. Vavrina, and J. Vian, "An automated battery management system to enable persistent missions with multiple aerial vehicles," *Mechatronics, IEEE/ASME Transactions on*, vol. 20, no. 1, pp. 275–286, 2015.

[4] J. F. Roberts, J.-C. Zufferey, and D. Floreano, "Energy management for indoor hovering robots," in *Intelligent Robots and Systems (IROS), IEEE/RSJ International Conference on*, 2008, pp. 1242–1247.

[5] A. Abdilla, A. Richards, and S. Burrow, "Power and endurance mod-
elling of battery-powered rotorcraft," in *Intelligent Robots and Systems
(IROS), 2015 IEEE/RSJ International Conference on*, 2015, pp. 675-
680.

[6] M. Podhradský, C. Coopmans, and A. Jensen, "Battery state-of-charge based altitude controller for small, low cost multirotor unmanned aerial vehicles," *Journal of Intelligent & Robotic Systems*, vol. 74, no. 1-2, pp. 193–207, 2014.

[7] D. C. Gandolfo, L. R. Salinas, A. Brando, and J. M. Toibero, "Sta-
ble path-following control for a quadrotor helicopter considering en-
ergy consumption," *IEEE Transactions on Control Systems Technology*,
vol. PP, no. 99, pp. 1–8, 2016.

[8] K. Vicencio, T. Korras, K. A. Bordignon, and I. Gentilini, "Energy-optimal path planning for six-rotors on multi-target missions," in *Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on*, 2015, pp. 2481–2487.

[9] F. Morbidi, R. Cano, and D. Lara, "Minimum-energy path generation for a quadrotor uav," in *Robotics and Automation (ICRA), IEEE International Conference on*, 2016.

[10] M. Cutler, N. K. Ure, B. Michini, and J. P. How, "Comparison of fixed and variable pitch actuators for agile quadrotors," in *AIAA Conference on Guidance, Navigation and Control*, 2011.

[11] T. Madani and A. Benallegue, "Sliding mode observer and backstepping control for a quadrotor unmanned aerial vehicles," in *American Control Conference (ACC)*, 2007, pp. 5887–5892.

[12] M. A. Patterson and A. V. Rao, "Gpops-ii: A matlab software for solving multiple-phase optimal control problems using hp-adaptive gaussian quadrature collocation methods and sparse nonlinear programming," *ACM Trans. Math. Softw.*, vol. 41, no. 1, pp. 1:1–1:37, Oct. 2014. [Online]. Available: http://doi.acm.org/10.1145/2558904

[13] D. Garg, W. W. Hager, and A. V. Rao, "Pseudospectral methods for solving infinite-horizon optimal control problems," *Automatica*, vol. 47, no. 4, pp. 829 – 837, 2011. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S0005109811001002

[14] M. A. Patterson and A. Rao, "Exploiting sparsity in direct collocation pseudospectral methods for solving optimal control problems," *Journal of Spacecraft and Rockets*, vol. 49, no. 2, pp. 354–377, 2012.

[15] L. T. Biegler, O. Ghattas, M. Heinkenschloss, and B. van Bloemen Waanders, "Large-scale pde-constrained optimization: an introduction," in *Large-Scale PDE-Constrained Optimization*. Springer, 2003, pp. 3–13.

[16] P. E. Gill, W. Murray, and M. A. Saunders, "Snopt: An sqp algorithm for large-scale constrained optimization," *SIAM review*, vol. 47, no. 1, pp. 99–131, 2005.

[17] P. R. Amestoy, I. S. Duff, J. Koster, and J.-Y. L'Excellent, "A fully asynchronous multifrontal solver using distributed dynamic scheduling," *SIAM Journal on Matrix Analysis and Applications*, vol. 23, no. 1, pp. 15–41, 2001.

[18] "Dji phantom 2," http://www.dji.com/fr/phantom-2, accessed: 2016-02-25.

[19] F. Yacef, O. Bouhali, and M. Hamerlain, "Adaptive fuzzy backstepping control for trajectory tracking of unmanned aerial quadrotor," in *Unmanned Aircraft Systems (ICUAS), 2014 International Conference on*, 2014, pp. 920–927.

[20] F. Yacef, O. Bouhali, M. Hamerlain, and N. Rizoug, "Observer-based adaptive fuzzy backstepping tracking control of quadrotor unmanned aerial vehicle powered by li-ion battery," *Journal of Intelligent & Robotic Systems*, vol. 84, no. 1, pp. 179–197, 2016.

<figure><img src="image_13.png" /></figure>
<figure><img src="image_12.png" /><figcaption>Fig. 7. The optimal trajectory in (x - y - z) space (a), and the control inputs (b)</figcaption></figure>

VII. CONCLUSION AND FUTURE WORK

Motivated by the fact that the problem of trajectory op-
timisation with minimum energy for rotary wings UAVs
applications have received less interest by unmanned aerial
systems research community, in this paper two optimal control
problems have been introduced and solved using an optimal
control software. The first problem seeks to find the trajectory
that minimize the consumed energy for a quadrotor helicopter
during a simple mission. Where in the second, minimum time
trajectory is computed for a desired energy. An efficiency
function was identified, in order to have an energetic model
close to reality. The numerical experiments illustrated the
solutions of the proposed optimal control problem, and the
comparative study provide quantisation of energy that can be
saved in a mission.

In future work, we will incorporate a dynamic model for
LiPo battery, and we will introduce an energy optimization
problem with respect to battery life. We are also going to
advise an experimental procedure to to highlight the effect of
path planning of energy consumption.

REFERENCES

[1] K. A. Swieringa, C. B. Hanson, J. R. Richardson, J. D. White, Z. Hasan, E. Qian, and A. Girard, "Autonomous battery swapping system for small-scale helicopters," in *Robotics and Automation (ICRA), IEEE International Conference on*, 2010, pp. 3335-3340.

[2] J. Leonard, A. Savvaris, and A. Tsourdos, "Energy management in swarm of unmanned aerial vehicles," *Journal of Intelligent & Robotic Systems*, vol. 74, no. 1-2, pp. 233–250, 2014.

[3] N. K. Ure, G. Chowdhary, T. Toksoz, J. P. How, M. A. Vavrina, and J. Vian, "An automated battery management system to enable persistent missions with multiple aerial vehicles," *Mechatronics, IEEE/ASME Transactions on*, vol. 20, no. 1, pp. 275–286, 2015.

[4] J. F. Roberts, J.-C. Zufferey, and D. Floreano, "Energy management for indoor hovering robots," in *Intelligent Robots and Systems (IROS), IEEE/RSJ International Conference on*, 2008, pp. 1242–1247.

[5] A. Abdilla, A. Richards, and S. Burrow, "Power and endurance mod-
elling of battery-powered rotorcraft," in *Intelligent Robots and Systems
(IROS), 2015 IEEE/RSJ International Conference on*, 2015, pp. 675-
680.

[6] M. Podhradský, C. Coopmans, and A. Jensen, "Battery state-of-charge based altitude controller for small, low cost multirotor unmanned aerial vehicles," *Journal of Intelligent & Robotic Systems*, vol. 74, no. 1-2, pp. 193–207, 2014.

[7] D. C. Gandolfo, L. R. Salinas, A. Brando, and J. M. Toibero, "Sta-
ble path-following control for a quadrotor helicopter considering en-
ergy consumption," *IEEE Transactions on Control Systems Technology*,
vol. PP, no. 99, pp. 1–8, 2016.

[8] K. Vicencio, T. Korras, K. A. Bordignon, and I. Gentilini, “Energy-optimal path planning for six-rotors on multi-target missions,” in *Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on*, 2015, pp. 2481–2487.

[9] F. Morbidi, R. Cano, and D. Lara, "Minimum-energy path generation for a quadrotor uav," in *Robotics and Automation (ICRA), IEEE International Conference on*, 2016.

[10] M. Cutler, N. K. Ure, B. Michini, and J. P. How, "Comparison of fixed and variable pitch actuators for agile quadrotors," in *AIAA Conference on Guidance, Navigation and Control*, 2011.

[11] T. Madani and A. Benallegue, "Sliding mode observer and backstepping control for a quadrotor unmanned aerial vehicles," in *American Control Conference (ACC)*, 2007, pp. 5887–5892.

[12] M. A. Patterson and A. V. Rao, "Gpops-ii: A matlab software for solving multiple-phase optimal control problems using hp-adaptive gaussian quadrature collocation methods and sparse nonlinear programming," *ACM Trans. Math. Softw.*, vol. 41, no. 1, pp. 1:1–1:37, Oct. 2014. [Online]. Available: http://doi.acm.org/10.1145/2558904

[13] D. Garg, W. W. Hager, and A. V. Rao, “Pseudospectral methods for solving infinite-horizon optimal control problems,” *Automatica*, vol. 47, no. 4, pp. 829 – 837, 2011. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S0005109811001002

[14] M. A. Patterson and A. Rao, “Exploiting sparsity in direct collocation pseudospectral methods for solving optimal control problems,” *Journal of Spacecraft and Rockets*, vol. 49, no. 2, pp. 354–377, 2012.

[15] L. T. Biegler, O. Ghattas, M. Heinkenschloss, and B. van Bloemen Waanders, “Large-scale pde-constrained optimization: an introduction,” in *Large-Scale PDE-Constrained Optimization*. Springer, 2003, pp. 3–13.

[16] P. E. Gill, W. Murray, and M. A. Saunders, “Snopt: An sqp algorithm for large-scale constrained optimization,” *SIAM review*, vol. 47, no. 1, pp. 99–131, 2005.

[17] P. R. Amestoy, I. S. Duff, J. Koster, and J.-Y. L'Excellent, "A fully asynchronous multifrontal solver using distributed dynamic scheduling," *SIAM Journal on Matrix Analysis and Applications*, vol. 23, no. 1, pp. 15-41, 2001.

[18] "Dji phantom 2," http://www.dji.com/fr/phantom-2, accessed: 2016-02-25.

[19] F. Yacef, O. Bouhali, and M. Hamerlain, "Adaptive fuzzy backstepping control for trajectory tracking of unmanned aerial quadrotor," in *Unmanned Aircraft Systems (ICUAS), 2014 International Conference on*, 2014, pp. 920–927.

[20] F. Yacef, O. Bouhali, M. Hamerlain, and N. Rizoug, "Observer-based adaptive fuzzy backstepping tracking control of quadrotor unmanned aerial vehicle powered by li-ion battery," *Journal of Intelligent & Robotic Systems*, vol. 84, no. 1, pp. 179–197, 2016.

<figure><img src="image_13.png" /></figure>