# Dynamics Modeling and Control of a Quadrotor with Swing Load

Authors: S. Sadr; S. Ali A. Moosavian; P. Zarafshan
Journal: Journal of Robotics
 (2014)

---

## Abstract

Nowadays, aerial robots or Unmanned Aerial Vehicles (UAV) have many applications in civilian and military fields. For example, of these applications is aerial monitoring, picking loads and moving them by different grippers. In this research, a quadrotor with a cable-suspended load with eight degrees of freedom is considered. The purpose is to control the position and attitude of the quadrotor on a desired trajectory in order to move the considered load with constant length of cable. So, the purpose of this research is proposing and designing an antiswing control algorithm for the suspended load. To this end, control and stabilization of the quadrotor are necessary for designing the antiswing controller. Furthermore, this paper is divided into two parts. In the first part, dynamics model is developed using Newton-Euler formulation, and obtained equations are verified in comparison with Lagrange approach. Consequently, a nonlinear control strategy based on dynamic model is used in order to control the position and attitude of the quadrotor. The performance of this proposed controller is evaluated by nonlinear simulations and, finally, the results demonstrate the effectiveness of the control strategy for the quadrotor with suspended load in various maneuvers.


---

## Body

## 1. Introduction

Quadrotor is a rotorcraft whose flight is based on rotation of two pairs of rotors that rotate opposite to each other. As shown in Figure1, the different movement of quadrotor is created by a difference in the velocity of rotors. If the velocity of rotor 1 (or 2) decreases and the velocity of rotor 3 (or 4) increases, then the roll (or pitch) motion is created and the quadrotor moves along the y-axis (or the x-axis). Moreover, a quadrotor is an aerial robot which has the potential to hover and take off, fly, and land in small areas. In addition, this robot has applications in different fields, among which are safety, natural risk management, environmental protection, infrastructures management, agriculture, and film protection. Moreover, a quadrotor is an underactuated system since it has six degrees of freedom and only four inputs. However, a quadrotor is inherently unstable and it can be difficult to fly. Thus, the control of this nonlinear system is a problem for both practical and theoretical interest. Many control algorithms are tested and implemented on this aerial robot in order to stabilize and move in different tasks. Among these algorithms are classic control, linear and nonlinear state feedback control, sliding mode control, back stepping control, and fuzzy and neural network control. In 2010, Vazquez and Valenzuela designed a nonlinear control system for the position and attitude control based on the classic control PID; indeed, the quadrotor altitude is controlled by a PI-action controller, [1]. In 2012, Lee et al. implemented a Linear Quadratic Regulator (LQR) controller for the position control of the quadrotor [2]. In 2004, Hoffmann [3] proposed a sliding mode method for the altitude control and an optimal control method for the attitude control. But many difficulties occurred because of motor vibrations in the high thrust and the chattering phenomena. Also, for realizing the robust control of the quadrotor, a back stepping control algorithm is proposed in [4]. This algorithm could estimate disturbances online and, so, they could improve the robustness of system. Erginer and Altug in 2012 performed dynamics modeling and control of a quadrotor. They obtained the dynamic model of the quadrotor by Newton-Euler method and controlled the quadrotor using a hybrid fuzzy-PD control algorithm, [5]. In 2008, Raffo et al. implemented a nonlinear H
∞ algorithm to control and stabilize the angular motion of the quadrotor. The simulation results show that this nonlinear algorithm can eliminate disturbances and stabilize the rotation motion of the quadrotor [6]. de Vries and Subbarao, in 2010, designed a back stepping multiloop controller for the hover flight [7].Figure 1
Scheme of quadrotor’s rotor rotation.Recently, with introduction of inexpensive micro-Unmanned Aerial Vehicles (UAV) and advanced sensors, controllers have been designed to enable these systems for many tasks, for example, offensive maneuvers [8], balancing a flying inverted pendulum [9]. Also, the progress in sensors and controllers leads very well to use of UAV for different applications. One of these applications is transportation of external loads. Different grippers for grasping and transporting of a load are designed [10–14]. Another one is cable-suspended loads which have been studied in recent years [15–17]. Cable-suspended systems are underactuated systems. Therefore, canceling or reducing oscillation of the suspended load is very necessary since oscillations of load in an industrial environment and other areas can result in damage [18–22]. So, different control methods have been proposed to control these robots since the suspended load significantly alters the flight characteristics of the quadrotor. These methods are divided into feedback and feed-forward approaches. Feedback control methods use measurements and estimations of system states to reduce the vibration, while feed-forward approaches change actuator commands for reducing the oscillation of system. The feed-forward controller can often improve the performance of feedback controller. Thus, proposing feed-forward algorithms can lead to more practical and accurate control of these systems. One effective feed-forward method is the input shaping theory which has proven to be a practical and effective approach of reducing vibrations [23, 24]. Also, several methods are proposed in order to minimize the residual vibration. Smith proposed the Posicast control of the damped oscillatory systems which is a technique to generate a nonoscillatory response from a damped system to a step input. This method breaks a step of a certain magnitude into two smaller steps, one of which is delayed in time [25]. Swigert proposed shaped torques techniques which consider the sensitivity of terminal states to variation in the model parameters [26]. Recently, in the control of overhead cranes, Mita and Kanai solved a minimum time control problem for swing free velocity profiles, which resulted in an open loop control [27]. Also, Yu proposed a nonlinear control based on the singular perturbation method [28]. In addition, Lee designed a high performance control based on the loop shaping and root locus methods [29]. Also, in 2012, Adams et al. designed input shaping control of a micro-coaxial radio-controlled helicopter carrying a suspended load [30]. Zain et al., in 2006, proposed hybrid learning control schemes with an input shaping of a flexible manipulator system [31].In this paper, the problem of the quadrotor flying with a suspended load is addressed which is widely used for different kinds of a cargo transport. The paper is organized in two parts. In the first part, a nonlinear model of an underactuated eight-degree-of-freedom quadrotor slung load system is derived on the basis of the Newton-Euler formulation. Next, this dynamic model is verified in comparison with Lagrange method. Then, a nonlinear model based control algorithm is designed for the position and attitude control of the quadrotor with the suspended load. In next part, the description of the input shaping algorithm is presented and, then, this method is implemented to the quadrotor with a suspended load. Finally, simulation results are studied to damp the oscillation of the suspended load.


## 2. Dynamics Modeling

The quadrotor slung load system is shown in Figure2. It is considered to be a system consisting of two rigid bodies connected by massless straight-line links which support only forces along the link. The system is characterized by mass and inertia parameters of rigid bodies and suspension’s attachment point locations. In this section, dynamics equations of the quadrotor slung load system are presented by Newton-Euler method. The following assumptions are made for modeling the quadrotor with a swinging load.(i)
Elastic deformation and shock of the quadrotor are ignored.
(ii)
Inertia matrix is time-invariant.
(iii)
Mass distribution of the quadrotor is symmetrical in thex-y plane.
(iv)
Drag factor and thrust factor of the quadrotor are constant.
(v)
Air density around of the quadrotor is constant.
(vi)
Thrust force and drag moment of each propellers are proportional to the square of the propeller speed.
(vii)
Both bodies are assumed to be rigid. This assumption excludes an elastic quadrotor and rotor modes such as flapping and nonrigid loads.
(viii)
The cable mass and aerodynamic effects on the load are neglected.
(ix)
The cable is considered to be inelastic.Figure 2
Free body diagram of quadrotor slung load system.These assumptions are considered to be sufficient for the realistic representation of the quadrotor with a swinging load system which is used for a nonaggressive trajectory tracking.


### 2.1. Aerodynamics of Rotor and Propeller

The aerodynamic force and moment are obtained by combining the momentum theory of the blade element [8, 9]. A quadrotor has four motors with propellers. The power applied to each motor, P, generates a torque on the rotor shaft, Q, and a force T. These torques and forces are generated by each rotor-propeller and they are proportional to the square of the propeller speed as
(1)
T
=
C
T
4
ρ
R
4
π
2
ω
m
2
,
Q
=
C
Q
4
ρ
R
5
π
3
ω
m
2
,
where ω
m is the rotor velocity, ρ is the air density, R is the propeller radius, C
T is the thrust factor, and C
Q is the momentum factor [9, 10].


### 2.2. Dynamics Equations of Motion

#### 2.2.1. Kinematics Equation of Quadrotor

As shown in Figure1, the quadrotor has four rotors which can generate identical thrusts and moments denoted by T
i and Q
i, for i
=
1,2
,
3,4, respectively. Let I
.
F. represent a right-hand inertia frame with the z-axis being the vertical direction to the earth. The body fixed frame is denoted by B
.
F. that center of this frame is located on the mass center of the quadrotor. The Euclidean position of the quadrotor with respect to I
.
F. is represented by the x, y, and z. Also, the Euler angle of the quadrotor with respect to I
.
F. is represented by the φ, θ, and ψ. Thus, the rotation matrix from B
.
F. to I
.
F. can be represented by R as
(2)
R
=
c
θ
c
ψ
s
φ
s
θ
c
ψ
-
c
φ
s
ψ
c
φ
s
θ
c
ψ
+
s
φ
s
ψ
c
θ
s
ψ
s
φ
s
θ
s
ψ
+
c
φ
c
ψ
c
φ
s
θ
s
ψ
-
s
φ
c
ψ
-
s
θ
c
θ
s
φ
c
θ
c
ψ
,
where c and s refer to cos
⁡ and sin function, respectively. Also, the translational and the rotational kinematics equations with respect to the inertial frame I
.
F. can be yielded as
(3)
X
˙
=
R
V
,
η
˙
=
P
ω
,
where V and ω denote the linear velocity and the angular velocity of the quadrotor with respect to the inertial frame I
.
F. expressed in the body fixed frame B
.
F.. So, the rotation velocity transfer matrix P can be given as
(4)
P
=
1
s
φ
t
θ
c
φ
t
θ
0
c
φ
-
s
φ
0
s
φ
c
θ
c
φ
c
θ
.


#### 2.2.2. Newton-Euler Equation of Quadrotor

As the free body diagram of the quadrotor slung load system shown in Figure2, the Newton-Euler equations for quadrotor in the inertia frame can be obtained as
(5)
m
X
¨
=
-
m
G
-
R
K
t
R
T
X
˙
+
R
T
-
R
F
cable
,
J
η
¨
=
-
P
P
T
η
˙
×
J
P
T
η
˙
-
P
K
r
P
T
η
˙
-
P
J
∂
P
T
∂
φ
φ
˙
+
∂
P
T
∂
θ
θ
˙
η
˙
+
P
τ
,
where m is the mass matrix of the quadrotor, J is the inertia matrix of the quadrotor, G
=
[
0
0
g
]
T is the gravity matrix, F
cable is the cable force, and K
t and K
r are the linear and angular aerodynamic friction factor, respectively. Also, T and τ matrices are given as
(6)
T
=
0
0
C
T
4
ρ
R
4
π
2
∑
i
=
1
4
ω
m
i
2
T
,
τ
=
l
C
T
4
ρ
R
4
π
2
-
ω
m
2
2
+
ω
m
4
2
l
C
T
4
ρ
R
4
π
2
-
ω
m
3
2
+
ω
m
1
2
C
p
4
ρ
R
5
π
3
∑
i
=
1
4
-
1
i
ω
m
i
2
.However, actuator forces and moments are summarized as(7)
U
1
=
T
1
+
T
2
+
T
3
+
T
4
,
U
2
=
U
φ
=
l
T
4
-
T
2
,
U
3
=
U
θ
=
l
T
3
-
T
1
,
U
4
=
U
ψ
=
-
Q
1
+
Q
2
-
Q
3
+
Q
4
,
where l is distance of two rotors opposite to each other, T
1 to T
4 are thrust forces which are generated by rotors 1 to 4, and Q
1 to Q
4 are moments which are generated by rotors 1 to 4. So, U
1 results in the motion along the z
b axis. Also, U
2, U
3, and U
4 create the roll, pitch, and yaw motion, respectively. In this system, F
cable is considered system’s input (or the actuator force for changing the cable length) which can be represented in the body frame as
(8)
F
cable
=
F
cable
sin
β
cos
⁡
γ
sin
β
sin
γ
cos
⁡
β
,
where F
cable is cable’s force magnitude. Also, the relation between F
cable and the cable length can be stated as
(9)
F
cable
=
m
p
r
¨
,
where r is the cable length, r
¨ is the acceleration of mass relative to the quadrotor in the body coordinate, and m
p is the load mass. So, motion’s equation of the load in the inertia frame can be obtained as:
(10)
m
p
X
¨
+
R
A
α
¨
+
B
+
ω
×
A
α
˙
-
m
p
R
ω
×
R
˙
b
+
m
p
G
=
R
F
cable
,
where(11a)
A
=
s
β
c
γ
-
r
s
β
s
γ
r
c
β
c
γ
s
β
s
γ
r
s
β
c
γ
r
c
β
s
γ
c
β
0
-
r
s
β
,
(11b)
α
=
r
γ
β
,
B
=
2
r
˙
β
˙
c
β
c
γ
-
2
r
˙
γ
˙
s
β
s
γ
-
2
r
β
˙
γ
˙
c
β
s
γ
-
r
β
˙
2
s
β
c
γ
-
r
γ
˙
2
s
β
c
γ
2
r
˙
β
˙
c
β
s
γ
+
2
r
˙
γ
˙
s
β
c
γ
+
2
r
β
˙
γ
˙
c
β
c
γ
-
r
β
˙
2
s
β
s
γ
-
r
γ
˙
2
s
β
s
γ
-
2
r
˙
β
˙
s
β
-
r
β
˙
2
c
β
.AndR
˙
b is the load velocity with respect to the quadrotor while it is expressed in the body frame. Equations (5) and (10) are motion equations of system with generalized coordinates as follows:
(12)
q
=
x
y
z
φ
θ
ψ
r
γ
β
T
.By considering the constant length of the cable, the system has eight degrees of freedom.


#### 2.2.3. Lagrange Equation of Quadrotor

To obtain the dynamic equations of motion by Lagrange method, generalized coordinates are defined as(13)
q
=
x
y
z
φ
θ
ψ
r
γ
β
T
.So, the kinetic energy for the quadrotor is(14)
K
q
=
1
2
X
˙
T
M
q
X
˙
+
1
2
η
˙
T
J
i
η
˙
and the swinging load’s kinetic energy can be attained as
(15)
K
p
=
1
2
X
˙
p
T
M
p
X
˙
p
=
1
2
X
˙
+
R
˙
s
T
M
q
X
˙
+
R
˙
s
,
where M
q, M
p are the mass diagonal matrix and the swinging load of the quadrotor, respectively. Also, R
˙
s is the swinging load velocity with respect to the quadrotor inertia frame (
I
.
F
.
). Moreover, J
i is the inertia moment matrix of the quadrotor in I
.
F. which can be calculated as
(16)
J
i
=
R
J
R
T
,
where J is the inertia moment matrix of the quadrotor in the body frame (
B
.
F
.
). Finally, the closed form equation of motion can be obtained as
(17)
M
q
¨
+
V
q
,
q
˙
+
G
q
=
Q
whereas, by considering (9), M
9
×
9 is the mass matrix, V
9
×
1 is the nonlinear velocity matrix, and G
9
×
1 is the gravity matrix. Also, Q is the generalized force.


#### 2.2.4. Model Verification

In this section, in order to verify the obtained dynamics model since the motion equations by Newton-Euler method are inhomogeneous unlike Lagrange equations, a desired path for the quadrotor flight is defined. Then, by solving Lagrange equations, rotors input are computed for tracking this desired path (solving the inverse dynamics). Next, these forces are exerted to Newton-Euler equations as inputs and these equations are solved (solving the forward dynamics). This procedure shows that responses of Newton-Euler and Lagrange equations are the same as the desired path. To this end, in simulations, a specified path for the quadrotor is considered and the inverse dynamic is solved to obtain the desired forces for the considered path (for Lagrange equations). By defining these forces as inputs for the Newton-Euler equations and solving these equations to find the tracked path by the quadrotor, the dynamics model can be verified. So, a hover flight is defined inz
=
2 m, and the simulation result is shown in Figure 3. As shown form this figure, both paths are the same. In next scenario, a vertical take-off flight is defined. Therefore, the desired path for this flight is
(18)
z
=
-
0.002
t
3
+
(
0.03
t
2
)
+
2
.Figure 3
Position of quadrotor in hover flight.The simulation result for this path is shown in Figure4. It is shown that both responses of these dynamic models are the same and are reasonable.Figure 4
Error of responses in vertical take-off flight.


## 2.1. Aerodynamics of Rotor and Propeller

The aerodynamic force and moment are obtained by combining the momentum theory of the blade element [8, 9]. A quadrotor has four motors with propellers. The power applied to each motor, P, generates a torque on the rotor shaft, Q, and a force T. These torques and forces are generated by each rotor-propeller and they are proportional to the square of the propeller speed as
(1)
T
=
C
T
4
ρ
R
4
π
2
ω
m
2
,
Q
=
C
Q
4
ρ
R
5
π
3
ω
m
2
,
where ω
m is the rotor velocity, ρ is the air density, R is the propeller radius, C
T is the thrust factor, and C
Q is the momentum factor [9, 10].


## 2.2. Dynamics Equations of Motion

### 2.2.1. Kinematics Equation of Quadrotor

As shown in Figure1, the quadrotor has four rotors which can generate identical thrusts and moments denoted by T
i and Q
i, for i
=
1,2
,
3,4, respectively. Let I
.
F. represent a right-hand inertia frame with the z-axis being the vertical direction to the earth. The body fixed frame is denoted by B
.
F. that center of this frame is located on the mass center of the quadrotor. The Euclidean position of the quadrotor with respect to I
.
F. is represented by the x, y, and z. Also, the Euler angle of the quadrotor with respect to I
.
F. is represented by the φ, θ, and ψ. Thus, the rotation matrix from B
.
F. to I
.
F. can be represented by R as
(2)
R
=
c
θ
c
ψ
s
φ
s
θ
c
ψ
-
c
φ
s
ψ
c
φ
s
θ
c
ψ
+
s
φ
s
ψ
c
θ
s
ψ
s
φ
s
θ
s
ψ
+
c
φ
c
ψ
c
φ
s
θ
s
ψ
-
s
φ
c
ψ
-
s
θ
c
θ
s
φ
c
θ
c
ψ
,
where c and s refer to cos
⁡ and sin function, respectively. Also, the translational and the rotational kinematics equations with respect to the inertial frame I
.
F. can be yielded as
(3)
X
˙
=
R
V
,
η
˙
=
P
ω
,
where V and ω denote the linear velocity and the angular velocity of the quadrotor with respect to the inertial frame I
.
F. expressed in the body fixed frame B
.
F.. So, the rotation velocity transfer matrix P can be given as
(4)
P
=
1
s
φ
t
θ
c
φ
t
θ
0
c
φ
-
s
φ
0
s
φ
c
θ
c
φ
c
θ
.


### 2.2.2. Newton-Euler Equation of Quadrotor

As the free body diagram of the quadrotor slung load system shown in Figure2, the Newton-Euler equations for quadrotor in the inertia frame can be obtained as
(5)
m
X
¨
=
-
m
G
-
R
K
t
R
T
X
˙
+
R
T
-
R
F
cable
,
J
η
¨
=
-
P
P
T
η
˙
×
J
P
T
η
˙
-
P
K
r
P
T
η
˙
-
P
J
∂
P
T
∂
φ
φ
˙
+
∂
P
T
∂
θ
θ
˙
η
˙
+
P
τ
,
where m is the mass matrix of the quadrotor, J is the inertia matrix of the quadrotor, G
=
[
0
0
g
]
T is the gravity matrix, F
cable is the cable force, and K
t and K
r are the linear and angular aerodynamic friction factor, respectively. Also, T and τ matrices are given as
(6)
T
=
0
0
C
T
4
ρ
R
4
π
2
∑
i
=
1
4
ω
m
i
2
T
,
τ
=
l
C
T
4
ρ
R
4
π
2
-
ω
m
2
2
+
ω
m
4
2
l
C
T
4
ρ
R
4
π
2
-
ω
m
3
2
+
ω
m
1
2
C
p
4
ρ
R
5
π
3
∑
i
=
1
4
-
1
i
ω
m
i
2
.However, actuator forces and moments are summarized as(7)
U
1
=
T
1
+
T
2
+
T
3
+
T
4
,
U
2
=
U
φ
=
l
T
4
-
T
2
,
U
3
=
U
θ
=
l
T
3
-
T
1
,
U
4
=
U
ψ
=
-
Q
1
+
Q
2
-
Q
3
+
Q
4
,
where l is distance of two rotors opposite to each other, T
1 to T
4 are thrust forces which are generated by rotors 1 to 4, and Q
1 to Q
4 are moments which are generated by rotors 1 to 4. So, U
1 results in the motion along the z
b axis. Also, U
2, U
3, and U
4 create the roll, pitch, and yaw motion, respectively. In this system, F
cable is considered system’s input (or the actuator force for changing the cable length) which can be represented in the body frame as
(8)
F
cable
=
F
cable
sin
β
cos
⁡
γ
sin
β
sin
γ
cos
⁡
β
,
where F
cable is cable’s force magnitude. Also, the relation between F
cable and the cable length can be stated as
(9)
F
cable
=
m
p
r
¨
,
where r is the cable length, r
¨ is the acceleration of mass relative to the quadrotor in the body coordinate, and m
p is the load mass. So, motion’s equation of the load in the inertia frame can be obtained as:
(10)
m
p
X
¨
+
R
A
α
¨
+
B
+
ω
×
A
α
˙
-
m
p
R
ω
×
R
˙
b
+
m
p
G
=
R
F
cable
,
where(11a)
A
=
s
β
c
γ
-
r
s
β
s
γ
r
c
β
c
γ
s
β
s
γ
r
s
β
c
γ
r
c
β
s
γ
c
β
0
-
r
s
β
,
(11b)
α
=
r
γ
β
,
B
=
2
r
˙
β
˙
c
β
c
γ
-
2
r
˙
γ
˙
s
β
s
γ
-
2
r
β
˙
γ
˙
c
β
s
γ
-
r
β
˙
2
s
β
c
γ
-
r
γ
˙
2
s
β
c
γ
2
r
˙
β
˙
c
β
s
γ
+
2
r
˙
γ
˙
s
β
c
γ
+
2
r
β
˙
γ
˙
c
β
c
γ
-
r
β
˙
2
s
β
s
γ
-
r
γ
˙
2
s
β
s
γ
-
2
r
˙
β
˙
s
β
-
r
β
˙
2
c
β
.AndR
˙
b is the load velocity with respect to the quadrotor while it is expressed in the body frame. Equations (5) and (10) are motion equations of system with generalized coordinates as follows:
(12)
q
=
x
y
z
φ
θ
ψ
r
γ
β
T
.By considering the constant length of the cable, the system has eight degrees of freedom.


### 2.2.3. Lagrange Equation of Quadrotor

To obtain the dynamic equations of motion by Lagrange method, generalized coordinates are defined as(13)
q
=
x
y
z
φ
θ
ψ
r
γ
β
T
.So, the kinetic energy for the quadrotor is(14)
K
q
=
1
2
X
˙
T
M
q
X
˙
+
1
2
η
˙
T
J
i
η
˙
and the swinging load’s kinetic energy can be attained as
(15)
K
p
=
1
2
X
˙
p
T
M
p
X
˙
p
=
1
2
X
˙
+
R
˙
s
T
M
q
X
˙
+
R
˙
s
,
where M
q, M
p are the mass diagonal matrix and the swinging load of the quadrotor, respectively. Also, R
˙
s is the swinging load velocity with respect to the quadrotor inertia frame (
I
.
F
.
). Moreover, J
i is the inertia moment matrix of the quadrotor in I
.
F. which can be calculated as
(16)
J
i
=
R
J
R
T
,
where J is the inertia moment matrix of the quadrotor in the body frame (
B
.
F
.
). Finally, the closed form equation of motion can be obtained as
(17)
M
q
¨
+
V
q
,
q
˙
+
G
q
=
Q
whereas, by considering (9), M
9
×
9 is the mass matrix, V
9
×
1 is the nonlinear velocity matrix, and G
9
×
1 is the gravity matrix. Also, Q is the generalized force.


### 2.2.4. Model Verification

In this section, in order to verify the obtained dynamics model since the motion equations by Newton-Euler method are inhomogeneous unlike Lagrange equations, a desired path for the quadrotor flight is defined. Then, by solving Lagrange equations, rotors input are computed for tracking this desired path (solving the inverse dynamics). Next, these forces are exerted to Newton-Euler equations as inputs and these equations are solved (solving the forward dynamics). This procedure shows that responses of Newton-Euler and Lagrange equations are the same as the desired path. To this end, in simulations, a specified path for the quadrotor is considered and the inverse dynamic is solved to obtain the desired forces for the considered path (for Lagrange equations). By defining these forces as inputs for the Newton-Euler equations and solving these equations to find the tracked path by the quadrotor, the dynamics model can be verified. So, a hover flight is defined inz
=
2 m, and the simulation result is shown in Figure 3. As shown form this figure, both paths are the same. In next scenario, a vertical take-off flight is defined. Therefore, the desired path for this flight is
(18)
z
=
-
0.002
t
3
+
(
0.03
t
2
)
+
2
.Figure 3
Position of quadrotor in hover flight.The simulation result for this path is shown in Figure4. It is shown that both responses of these dynamic models are the same and are reasonable.Figure 4
Error of responses in vertical take-off flight.


## 2.2.1. Kinematics Equation of Quadrotor

As shown in Figure1, the quadrotor has four rotors which can generate identical thrusts and moments denoted by T
i and Q
i, for i
=
1,2
,
3,4, respectively. Let I
.
F. represent a right-hand inertia frame with the z-axis being the vertical direction to the earth. The body fixed frame is denoted by B
.
F. that center of this frame is located on the mass center of the quadrotor. The Euclidean position of the quadrotor with respect to I
.
F. is represented by the x, y, and z. Also, the Euler angle of the quadrotor with respect to I
.
F. is represented by the φ, θ, and ψ. Thus, the rotation matrix from B
.
F. to I
.
F. can be represented by R as
(2)
R
=
c
θ
c
ψ
s
φ
s
θ
c
ψ
-
c
φ
s
ψ
c
φ
s
θ
c
ψ
+
s
φ
s
ψ
c
θ
s
ψ
s
φ
s
θ
s
ψ
+
c
φ
c
ψ
c
φ
s
θ
s
ψ
-
s
φ
c
ψ
-
s
θ
c
θ
s
φ
c
θ
c
ψ
,
where c and s refer to cos
⁡ and sin function, respectively. Also, the translational and the rotational kinematics equations with respect to the inertial frame I
.
F. can be yielded as
(3)
X
˙
=
R
V
,
η
˙
=
P
ω
,
where V and ω denote the linear velocity and the angular velocity of the quadrotor with respect to the inertial frame I
.
F. expressed in the body fixed frame B
.
F.. So, the rotation velocity transfer matrix P can be given as
(4)
P
=
1
s
φ
t
θ
c
φ
t
θ
0
c
φ
-
s
φ
0
s
φ
c
θ
c
φ
c
θ
.


## 2.2.2. Newton-Euler Equation of Quadrotor

As the free body diagram of the quadrotor slung load system shown in Figure2, the Newton-Euler equations for quadrotor in the inertia frame can be obtained as
(5)
m
X
¨
=
-
m
G
-
R
K
t
R
T
X
˙
+
R
T
-
R
F
cable
,
J
η
¨
=
-
P
P
T
η
˙
×
J
P
T
η
˙
-
P
K
r
P
T
η
˙
-
P
J
∂
P
T
∂
φ
φ
˙
+
∂
P
T
∂
θ
θ
˙
η
˙
+
P
τ
,
where m is the mass matrix of the quadrotor, J is the inertia matrix of the quadrotor, G
=
[
0
0
g
]
T is the gravity matrix, F
cable is the cable force, and K
t and K
r are the linear and angular aerodynamic friction factor, respectively. Also, T and τ matrices are given as
(6)
T
=
0
0
C
T
4
ρ
R
4
π
2
∑
i
=
1
4
ω
m
i
2
T
,
τ
=
l
C
T
4
ρ
R
4
π
2
-
ω
m
2
2
+
ω
m
4
2
l
C
T
4
ρ
R
4
π
2
-
ω
m
3
2
+
ω
m
1
2
C
p
4
ρ
R
5
π
3
∑
i
=
1
4
-
1
i
ω
m
i
2
.However, actuator forces and moments are summarized as(7)
U
1
=
T
1
+
T
2
+
T
3
+
T
4
,
U
2
=
U
φ
=
l
T
4
-
T
2
,
U
3
=
U
θ
=
l
T
3
-
T
1
,
U
4
=
U
ψ
=
-
Q
1
+
Q
2
-
Q
3
+
Q
4
,
where l is distance of two rotors opposite to each other, T
1 to T
4 are thrust forces which are generated by rotors 1 to 4, and Q
1 to Q
4 are moments which are generated by rotors 1 to 4. So, U
1 results in the motion along the z
b axis. Also, U
2, U
3, and U
4 create the roll, pitch, and yaw motion, respectively. In this system, F
cable is considered system’s input (or the actuator force for changing the cable length) which can be represented in the body frame as
(8)
F
cable
=
F
cable
sin
β
cos
⁡
γ
sin
β
sin
γ
cos
⁡
β
,
where F
cable is cable’s force magnitude. Also, the relation between F
cable and the cable length can be stated as
(9)
F
cable
=
m
p
r
¨
,
where r is the cable length, r
¨ is the acceleration of mass relative to the quadrotor in the body coordinate, and m
p is the load mass. So, motion’s equation of the load in the inertia frame can be obtained as:
(10)
m
p
X
¨
+
R
A
α
¨
+
B
+
ω
×
A
α
˙
-
m
p
R
ω
×
R
˙
b
+
m
p
G
=
R
F
cable
,
where(11a)
A
=
s
β
c
γ
-
r
s
β
s
γ
r
c
β
c
γ
s
β
s
γ
r
s
β
c
γ
r
c
β
s
γ
c
β
0
-
r
s
β
,
(11b)
α
=
r
γ
β
,
B
=
2
r
˙
β
˙
c
β
c
γ
-
2
r
˙
γ
˙
s
β
s
γ
-
2
r
β
˙
γ
˙
c
β
s
γ
-
r
β
˙
2
s
β
c
γ
-
r
γ
˙
2
s
β
c
γ
2
r
˙
β
˙
c
β
s
γ
+
2
r
˙
γ
˙
s
β
c
γ
+
2
r
β
˙
γ
˙
c
β
c
γ
-
r
β
˙
2
s
β
s
γ
-
r
γ
˙
2
s
β
s
γ
-
2
r
˙
β
˙
s
β
-
r
β
˙
2
c
β
.AndR
˙
b is the load velocity with respect to the quadrotor while it is expressed in the body frame. Equations (5) and (10) are motion equations of system with generalized coordinates as follows:
(12)
q
=
x
y
z
φ
θ
ψ
r
γ
β
T
.By considering the constant length of the cable, the system has eight degrees of freedom.


## 2.2.3. Lagrange Equation of Quadrotor

To obtain the dynamic equations of motion by Lagrange method, generalized coordinates are defined as(13)
q
=
x
y
z
φ
θ
ψ
r
γ
β
T
.So, the kinetic energy for the quadrotor is(14)
K
q
=
1
2
X
˙
T
M
q
X
˙
+
1
2
η
˙
T
J
i
η
˙
and the swinging load’s kinetic energy can be attained as
(15)
K
p
=
1
2
X
˙
p
T
M
p
X
˙
p
=
1
2
X
˙
+
R
˙
s
T
M
q
X
˙
+
R
˙
s
,
where M
q, M
p are the mass diagonal matrix and the swinging load of the quadrotor, respectively. Also, R
˙
s is the swinging load velocity with respect to the quadrotor inertia frame (
I
.
F
.
). Moreover, J
i is the inertia moment matrix of the quadrotor in I
.
F. which can be calculated as
(16)
J
i
=
R
J
R
T
,
where J is the inertia moment matrix of the quadrotor in the body frame (
B
.
F
.
). Finally, the closed form equation of motion can be obtained as
(17)
M
q
¨
+
V
q
,
q
˙
+
G
q
=
Q
whereas, by considering (9), M
9
×
9 is the mass matrix, V
9
×
1 is the nonlinear velocity matrix, and G
9
×
1 is the gravity matrix. Also, Q is the generalized force.


## 2.2.4. Model Verification

In this section, in order to verify the obtained dynamics model since the motion equations by Newton-Euler method are inhomogeneous unlike Lagrange equations, a desired path for the quadrotor flight is defined. Then, by solving Lagrange equations, rotors input are computed for tracking this desired path (solving the inverse dynamics). Next, these forces are exerted to Newton-Euler equations as inputs and these equations are solved (solving the forward dynamics). This procedure shows that responses of Newton-Euler and Lagrange equations are the same as the desired path. To this end, in simulations, a specified path for the quadrotor is considered and the inverse dynamic is solved to obtain the desired forces for the considered path (for Lagrange equations). By defining these forces as inputs for the Newton-Euler equations and solving these equations to find the tracked path by the quadrotor, the dynamics model can be verified. So, a hover flight is defined inz
=
2 m, and the simulation result is shown in Figure 3. As shown form this figure, both paths are the same. In next scenario, a vertical take-off flight is defined. Therefore, the desired path for this flight is
(18)
z
=
-
0.002
t
3
+
(
0.03
t
2
)
+
2
.Figure 3
Position of quadrotor in hover flight.The simulation result for this path is shown in Figure4. It is shown that both responses of these dynamic models are the same and are reasonable.Figure 4
Error of responses in vertical take-off flight.


## 3. Controller Design

### 3.1. Position and Attitude Control of Quadrotor

The main aim of this section is to design a model based control scheme for a full control of a quadrotor. This control method is a compensation of nonlinear terms based on the accurate knowledge of the dynamics system. Thus, dynamic equations of motion based on the accurate knowledge of the dynamics system can be stated as(19)
τ
=
M
-
q
q
¨
+
V
-
q
,
q
˙
+
G
-
q
,
where M
¯, V
¯, and G
¯ are the mass matrix, the nonlinear velocity matrix, and the gravity matrix, respectively. These matrices are obtained based on the physical knowledge and the geometrical dimensions. In Figure 5, the block diagram of this control algorithm is shown. According to this diagram, the control law can be calculated as
(20)
τ
c
c
=
V
¯
q
,
q
˙
+
G
¯
q
+
M
¯
q
¨
d
+
K
p
e
+
K
d
e
˙
.Figure 5
Model based control algorithm scheme.Also, this control torque can be applied to the below dynamics equations of the considered system as(21)
τ
=
M
q
q
¨
+
V
q
,
q
˙
+
G
q
.Moreover, by considering what is well known about the dynamic parameters of the system, it can be concluded that(22)
M
¯
≜
M
,
V
¯
≜
V
,
G
¯
≜
G
.So, by substituting (20) into (21) and considering the assumption in (22), it yields
(23)
M
e
¨
+
K
p
e
+
K
d
e
˙
=
0
.AsM is the positive definition matrix, so it can be written as
(24)
e
¨
+
K
p
e
+
K
d
e
˙
=
0
which confirms the error convergence by choosing the proper controller gains K
p and K
d.Using this algorithm and by choosing optimal gains for the designed controller, the position and attitude of the quadrotor are controlled. So, the model can be viewed as two independent subsystems, which are the transitional movement subsystem and the angular movement subsystem. The transitional motion does not affect the angular motion, but the angular motion affects the transitional motion. However, for designing a control algorithm in order to take full control of degrees of freedom, quadrotor’s dynamics equations must be divided into two subsystems: the transitional subsystem and the rotation subsystem. Therefore, the control algorithm is designed in two parts: the position controller and the attitude controller. Figure6 shows this control algorithm scheme.Figure 6
Block diagram of the considered control system.Based on quadrotor’s operation in different flights, it is obvious that the quadrotor does not have any actuator force which directly creates a movement along thex- and the y-axis. Thus, this robot can fly in the x and the y direction by creation of the pitch and the roll motion. Based on this principle, control forces can be obtained using the transitional subsystem as
(25)
c
φ
s
θ
c
ψ
+
s
φ
s
ψ
U
1
=
F
x
,
c
φ
s
θ
s
ψ
-
s
φ
c
ψ
U
1
=
F
y
,
c
θ
c
φ
U
1
=
F
z
,
where F
x, F
y, and F
z are created forces motion along the x-, y-, and z-axis which are calculated according to the desired trajectory. Also, the vector of control forces F
c
c
=
[
F
x
,
F
y
,
F
z
]
T can be calculated according to the transitional subsystem (transitional motion equation of the quadrotor and swing load) as
(26)
F
c
c
=
M
t
q
¨
d
+
K
p
t
e
+
K
d
t
e
˙
+
V
t
q
,
q
˙
+
G
t
q
,
where M
t
(
6
×
6
), V
t
(
6
×
1
), and G
t
(
6
×
1
) are the mass matrix, the nonlinear velocity matrix, and the gravity matrix of the transitional dynamic subsystem, respectively. Also, q
¨
d is the desired acceleration and e
=
q
d
-
q (where q
=
x
,
y
,
z
,
r
,
γ
,
β) is the position error. Moreover, K
p
t and K
d
t are controller gains. By solving three equations of (25), simultaneously, control outputs U
1
-ctrl, φ
ctrl, and θ
ctrl which create motion along the x-, y-, and z-axis are calculated as
(27)
U
1
-ctrl
=
F
z
c
θ
c
φ
,
φ
ctrl
=
arcsin
1
U
1
F
x
s
ψ
d
-
F
y
c
ψ
d
,
θ
ctrl
=
arcsin
1
U
1
c
φ
ctrl
F
x
c
ψ
d
+
F
y
s
ψ
d
,
where ψ
d is the desired value of the yaw angle. In the same way, the attitude controller is designed according to the rotational subsystem. Therefore, to perform this control propose, the torque control vector or τ
c
c
=
[
U
2
,
U
3
,
U
4
]
T can be considered:
(28)
τ
c
c
=
M
r
q
¨
d
+
K
p
r
e
+
K
d
r
e
˙
+
V
r
q
,
q
˙
+
G
r
(
q
)
,
where M
r, V
r, and G
r are the mass matrix, the nonlinear velocity matrix, and the gravity matrix of the rotational dynamic subsystem (rotational motion of the quadrotor), respectively. Also, q
=
φ
,
θ
,
ψ and K
p
r, K
d
r are rotation controller’s gains.


### 3.2. Simulation Results of Designed Position and Attitude Controller

To verify the effectiveness and the application effect of the proposed control method, the simulation study has been carried on the quadrotor with a swinging load. In this section, simulation results of the Model Based Algorithm controller (MBA controller) are compared to a PID controller in different maneuvers. It should be noted that the gains of PID controller are calculated by try and error method. So, controller gains and the system parameters of simulations are described in Tables1 and 2. In the first case study, a linear path in the x-y plane at z
=
3 m is defined. The equation of desired path is defined as follows:
(29)
z
=
3
;
x
=
0.2
t
+
1
;
y
=
0.2
t
+
1
.Table 1
MBA and PID controller gains values.
Controllers gains
Value
K
p
t = K
p
r
100
K
d
t = K
d
r
80
K
p
x = K
p
y
50
K
d
x = K
d
y
8
K
i
x = K
i
y
8
K
p
z
40
K
d
z
14
K
i
z
20
K
p
φ = K
p
θ = K
p
ψ
80
K
i
φ = K
i
θ = K
i
ψ
10
K
d
φ = K
d
θ
8
K
d
ψ
6Table 2
Physical parameters of the system.
Parameter
Value
m (Kg)
0.65
m
p (Kg)
0.3
l (m)
0.232
r (m)
1
J
x (Kg/m2)
7.5 × 1
0
-
3
J
y (Kg/m2)
7.5 × 1
0
-
3
J
z (Kg/m2)
1.3 × 1
0
-
2
C
T
0.07428
C
Q
0.10724
K
r = K
t
10 × 1
0
-
15Simulation results of the designed controller are shown in Figures7–9. As shown in Figure 7, the controller performance in comparison with a PID controller in this maneuver is good. As seen from Figure 8, the quadrotor finds the desired path by the MBA controller earlier than the PID controller. Also, Figure 9 shows the maximum error of the x and the y position, so that the considered MBA controller has the better performance than the PID controller.Figure 7
Linear path of quadrotor slung load system.Figure 8
Altitude of quadrotor in linear flight.Figure 9
Position error of quadrotor in linear flight.The last case study is a circular path for the quadrotor flight (Figure10). In this scenario, the superior performance of the proposed MBA controller is compared to the PID controller (Figures 11 and 12). The position error of the quadrotor is converged to the zero after a few seconds by the designed model based control algorithm, while by the PID controller this error has oscillations about zero and it is not zero.Figure 10
Circle path flight of quadrotor slung load system.Figure 11
Position error of quadrotor in circle flight with MBA controller.Figure 12
Position error of quadrotor in circle flight with PID controller.


### 3.3. Antiswing Control of Quadrotor

In the first step of the antiswing controller design procedure, it should be better to find commands that move systems without vibration. So, it is helpful to start with the simplest command. It is known that, by giving an impulse to the system, it will cause vibration. However, if a second impulse is applied to the system in the right moment, the vibration included by the first impulse is cancelled [23]. This concept is shown in Figure 13. Subsequently, input shaping theory is implemented by convolving the reference command with a sequence of impulses. This process is illustrated in Figure 14. The impulse amplitudes and time locations of their sequence are calculated in order to obtain a stair-like command to reduce the detrimental effects of system oscillations. The amplitude and time locations of impulses are calculated by estimation of system’s natural frequency and the damping ratio [30].Figure 13
Response of dynamic system to two sequent impulses.Figure 14
Input shaping process with ZV shaper.If estimations of system’s natural frequency,ω
n, and the damping ratio, ζ, for vibration modes of the system which must be canceled are known, then the residual vibration that results from the sequence of impulses can be described as
(30)
ν
ω
n
,
ζ
=
e
-
ζ
ω
n
t
N
C
ω
n
,
ζ
2
+
S
ω
n
,
ζ
2
,
where
(31)
C
ω
n
,
ζ
=
∑
i
=
1
N
A
i
e
ζ
ω
n
t
i
cos
⁡
ω
d
t
i
,
S
ω
n
,
ζ
=
∑
i
=
1
N
A
i
e
ζ
ω
n
t
i
sin
ω
d
t
i
,
where A
i and t
i are the amplitude and the time locations of impulses, N is the number of impulses in the impulse sequence, and ω
d
=
ω
n
(
1
-
ζ
2
). To avoid the trivial solution of all zero valued impulses and to obtain a normalized result, it is necessary to satisfy the below equation:
(32)
∑
i
=
1
N
A
i
=
1
.In the modeling of a suspended load, the load damping orζ can be neglected and the frequency of the linear model can be considered:
(33)
ω
n
=
g
l
,
where l is length of the cable. When the amplitude of impulses is considered positive and the estimation of system’s parameters is accurate, the simplest shaper or the ZV shaper can be proposed with two impulses in two times t
1 and t
2. Convolving this shaper to the step input is shown in Figure 14. The ZV shaper can cancel the oscillation of systems whose natural frequency and damping ratio are specific and known. In this case, this assumption can be considered to drive dynamic equations. The cable is considered inelastic and the aerodynamic force on the suspended load is neglected. So, the exact estimation of natural frequency and the damping ratio does not exist. In 1993, Bohlke proposed a method to increase the robustness of the ZV shaper against errors of system’s parameter estimation [23]. In this proposed method, the derivation of the residual vibration to the natural frequency and the damping ratio must be zero as
(34)
d
d
ω
n
ν
ω
n
,
ζ
=
0
,
d
d
ζ
ν
ω
n
,
ζ
=
0
.These two equations can be written as(35)
∑
i
=
1
N
A
i
t
i
e
ζ
ω
n
t
i
cos
⁡
ω
d
t
i
=
0
,
∑
i
=
1
N
A
i
t
i
e
ζ
ω
n
t
i
sin
ω
d
t
i
=
0
.Due to adding these two constraints, two variables are required to be added by an impulse with theA
3 amplitude at the time t
3 to the ZV shaper. The result of this addition is the ZVD shaper with 3 impulses and 5 unknown A
1, A
2, A
3, t
2, and t
3 (with the assumption t
1
=
0) for this shaper. Thus, the following equations can be obtained:
(36)
A
1
+
A
2
e
ζ
ω
n
t
2
cos
⁡
ω
d
t
2
+
A
3
e
ζ
ω
n
t
3
cos
⁡
ω
d
t
3
=
0
,
A
2
e
ζ
ω
n
t
2
sin
ω
d
t
2
+
A
3
e
ζ
ω
n
t
3
sin
ω
d
t
3
=
0
,
A
2
t
2
e
ζ
ω
n
t
2
cos
⁡
ω
d
t
2
+
A
3
t
3
e
ζ
ω
n
t
3
cos
⁡
ω
d
t
3
=
0
,
A
2
t
2
e
ζ
ω
n
t
2
sin
ω
d
t
2
+
A
3
t
3
e
ζ
ω
n
t
3
sin
ω
d
t
3
=
0
.By solving these four equations, unknown parameters of the input shaper are calculated as(37)
t
2
=
T
d
2
,
t
3
=
T
d
,
A
1
=
1
1
+
2
K
+
K
2
,
A
2
=
2
K
1
+
2
K
+
K
2
,
A
3
=
K
2
1
+
2
K
+
K
2
,
where
(38)
K
=
exp
⁡
-
ζ
π
1
-
ζ
2
.For the quadrotor with the suspended load, the cable length is assumed asr
=
1 m. By estimating the natural frequency and the damping ratio, input shaper’s parameters are obtained as
(39)
t
2
=
1.00322
,
t
3
=
2.00644
,
A
1
=
1
4
,
A
2
=
1
2
,
A
3
=
1
4
.However, the transfer function of the designed ZVD shaper is(40)
G
i
s
=
A
1
e
-
t
1
s
+
A
2
e
-
t
2
s
+
A
3
e
-
t
3
s
.For the small oscillation, suspended load’s motion is caused by the horizontal motion of the quadrotor. So, it is necessary that the shaper is implemented on the part of the desired path. If the part of the desired path for the shaper is defined as(41)
X
-
d
=
x
d
,
y
d
,
ψ
d
,
v
x
_
d
,
v
y
_
d
,
ψ
˙
d
,
a
x
_
d
,
a
y
_
d
,
ψ
¨
d
T
,
consequently, the shaped path for the quadrotor is
(42)
X
¯
d
*
t
=
Γ
Z
V
D
·
X
¯
d
t
,
where Γ
Z
V
D is the transfer function matrix of the ZVD shaper in the time domain. By considering the shaped path for the quadrotor controller, the controller performance is improved and the oscillation of the swing load is reduced. In next section, simulation results of the implementing shaper on input commands are shown and discussed.


### 3.4. Simulation Results of Designed Antiswing Controller

To verify the effectiveness and the application effect of the control method for canceling the oscillation of the suspended load, the simulation routine has been carried on the quadrotor with the suspended load. In addition, for estimating the natural frequency and the damping ratio, this system is compared to a cart-pendulum system. So, the natural frequency of system is assumed as (33). Also, the damping ratio of oscillation mode of the swing load is related to the aerodynamics drag on the load and suspension system and, so, the damping ratio is zero according to the small scale of system consideration. Moreover, the system parameters used in the simulation are listed in Table 2. Moreover, the desired path is defined as the line-curve-line and simulation results are shown in Figure 15. By comparing Figure 15 with Figure 16, it can be concluded that the designed feed-forward controller can effectively cancel the oscillation of the suspended load. In addition, Figure 17 shows the position error of the quadrotor both with ZVD shaper and without ZVD shaper. As seen from this figure, the error is converged to zero after a few seconds when input shaping is implemented on desired command, and the performance of the controller is better. Also, it is clear that, by implementing input shaping on a desired path, the required time for tracking the desired path is increased. So, the quadrotor requires more time for tracking the desired path.Figure 15
3D flight of quadrotor with input shaping controller.Figure 16
3D flight of quadrotor without input shaping controller.Position error of quadrotor: (a) without input shaper, (b) with input shaper.
(a)
(b)In the second senario, the square path is designed for the quadrotor flight. In Figures18 and 19, simulation results are shown both with ZVD shaper and without it. As can be seen from these figures, the feed-forward controller can cancel the oscillation. Also, Figure 20 shows the position error of the quadrotor with input shaping. As shown from this figure, error is zero after few seconds.Figure 18
Square path of quadrotor with input shaping controller.Figure 19
Square path of quadrotor without input shaping controller.Figure 20
Position Error of quadrotor with input shaping.


## 3.1. Position and Attitude Control of Quadrotor

The main aim of this section is to design a model based control scheme for a full control of a quadrotor. This control method is a compensation of nonlinear terms based on the accurate knowledge of the dynamics system. Thus, dynamic equations of motion based on the accurate knowledge of the dynamics system can be stated as(19)
τ
=
M
-
q
q
¨
+
V
-
q
,
q
˙
+
G
-
q
,
where M
¯, V
¯, and G
¯ are the mass matrix, the nonlinear velocity matrix, and the gravity matrix, respectively. These matrices are obtained based on the physical knowledge and the geometrical dimensions. In Figure 5, the block diagram of this control algorithm is shown. According to this diagram, the control law can be calculated as
(20)
τ
c
c
=
V
¯
q
,
q
˙
+
G
¯
q
+
M
¯
q
¨
d
+
K
p
e
+
K
d
e
˙
.Figure 5
Model based control algorithm scheme.Also, this control torque can be applied to the below dynamics equations of the considered system as(21)
τ
=
M
q
q
¨
+
V
q
,
q
˙
+
G
q
.Moreover, by considering what is well known about the dynamic parameters of the system, it can be concluded that(22)
M
¯
≜
M
,
V
¯
≜
V
,
G
¯
≜
G
.So, by substituting (20) into (21) and considering the assumption in (22), it yields
(23)
M
e
¨
+
K
p
e
+
K
d
e
˙
=
0
.AsM is the positive definition matrix, so it can be written as
(24)
e
¨
+
K
p
e
+
K
d
e
˙
=
0
which confirms the error convergence by choosing the proper controller gains K
p and K
d.Using this algorithm and by choosing optimal gains for the designed controller, the position and attitude of the quadrotor are controlled. So, the model can be viewed as two independent subsystems, which are the transitional movement subsystem and the angular movement subsystem. The transitional motion does not affect the angular motion, but the angular motion affects the transitional motion. However, for designing a control algorithm in order to take full control of degrees of freedom, quadrotor’s dynamics equations must be divided into two subsystems: the transitional subsystem and the rotation subsystem. Therefore, the control algorithm is designed in two parts: the position controller and the attitude controller. Figure6 shows this control algorithm scheme.Figure 6
Block diagram of the considered control system.Based on quadrotor’s operation in different flights, it is obvious that the quadrotor does not have any actuator force which directly creates a movement along thex- and the y-axis. Thus, this robot can fly in the x and the y direction by creation of the pitch and the roll motion. Based on this principle, control forces can be obtained using the transitional subsystem as
(25)
c
φ
s
θ
c
ψ
+
s
φ
s
ψ
U
1
=
F
x
,
c
φ
s
θ
s
ψ
-
s
φ
c
ψ
U
1
=
F
y
,
c
θ
c
φ
U
1
=
F
z
,
where F
x, F
y, and F
z are created forces motion along the x-, y-, and z-axis which are calculated according to the desired trajectory. Also, the vector of control forces F
c
c
=
[
F
x
,
F
y
,
F
z
]
T can be calculated according to the transitional subsystem (transitional motion equation of the quadrotor and swing load) as
(26)
F
c
c
=
M
t
q
¨
d
+
K
p
t
e
+
K
d
t
e
˙
+
V
t
q
,
q
˙
+
G
t
q
,
where M
t
(
6
×
6
), V
t
(
6
×
1
), and G
t
(
6
×
1
) are the mass matrix, the nonlinear velocity matrix, and the gravity matrix of the transitional dynamic subsystem, respectively. Also, q
¨
d is the desired acceleration and e
=
q
d
-
q (where q
=
x
,
y
,
z
,
r
,
γ
,
β) is the position error. Moreover, K
p
t and K
d
t are controller gains. By solving three equations of (25), simultaneously, control outputs U
1
-ctrl, φ
ctrl, and θ
ctrl which create motion along the x-, y-, and z-axis are calculated as
(27)
U
1
-ctrl
=
F
z
c
θ
c
φ
,
φ
ctrl
=
arcsin
1
U
1
F
x
s
ψ
d
-
F
y
c
ψ
d
,
θ
ctrl
=
arcsin
1
U
1
c
φ
ctrl
F
x
c
ψ
d
+
F
y
s
ψ
d
,
where ψ
d is the desired value of the yaw angle. In the same way, the attitude controller is designed according to the rotational subsystem. Therefore, to perform this control propose, the torque control vector or τ
c
c
=
[
U
2
,
U
3
,
U
4
]
T can be considered:
(28)
τ
c
c
=
M
r
q
¨
d
+
K
p
r
e
+
K
d
r
e
˙
+
V
r
q
,
q
˙
+
G
r
(
q
)
,
where M
r, V
r, and G
r are the mass matrix, the nonlinear velocity matrix, and the gravity matrix of the rotational dynamic subsystem (rotational motion of the quadrotor), respectively. Also, q
=
φ
,
θ
,
ψ and K
p
r, K
d
r are rotation controller’s gains.


## 3.2. Simulation Results of Designed Position and Attitude Controller

To verify the effectiveness and the application effect of the proposed control method, the simulation study has been carried on the quadrotor with a swinging load. In this section, simulation results of the Model Based Algorithm controller (MBA controller) are compared to a PID controller in different maneuvers. It should be noted that the gains of PID controller are calculated by try and error method. So, controller gains and the system parameters of simulations are described in Tables1 and 2. In the first case study, a linear path in the x-y plane at z
=
3 m is defined. The equation of desired path is defined as follows:
(29)
z
=
3
;
x
=
0.2
t
+
1
;
y
=
0.2
t
+
1
.Table 1
MBA and PID controller gains values.
Controllers gains
Value
K
p
t = K
p
r
100
K
d
t = K
d
r
80
K
p
x = K
p
y
50
K
d
x = K
d
y
8
K
i
x = K
i
y
8
K
p
z
40
K
d
z
14
K
i
z
20
K
p
φ = K
p
θ = K
p
ψ
80
K
i
φ = K
i
θ = K
i
ψ
10
K
d
φ = K
d
θ
8
K
d
ψ
6Table 2
Physical parameters of the system.
Parameter
Value
m (Kg)
0.65
m
p (Kg)
0.3
l (m)
0.232
r (m)
1
J
x (Kg/m2)
7.5 × 1
0
-
3
J
y (Kg/m2)
7.5 × 1
0
-
3
J
z (Kg/m2)
1.3 × 1
0
-
2
C
T
0.07428
C
Q
0.10724
K
r = K
t
10 × 1
0
-
15Simulation results of the designed controller are shown in Figures7–9. As shown in Figure 7, the controller performance in comparison with a PID controller in this maneuver is good. As seen from Figure 8, the quadrotor finds the desired path by the MBA controller earlier than the PID controller. Also, Figure 9 shows the maximum error of the x and the y position, so that the considered MBA controller has the better performance than the PID controller.Figure 7
Linear path of quadrotor slung load system.Figure 8
Altitude of quadrotor in linear flight.Figure 9
Position error of quadrotor in linear flight.The last case study is a circular path for the quadrotor flight (Figure10). In this scenario, the superior performance of the proposed MBA controller is compared to the PID controller (Figures 11 and 12). The position error of the quadrotor is converged to the zero after a few seconds by the designed model based control algorithm, while by the PID controller this error has oscillations about zero and it is not zero.Figure 10
Circle path flight of quadrotor slung load system.Figure 11
Position error of quadrotor in circle flight with MBA controller.Figure 12
Position error of quadrotor in circle flight with PID controller.


## 3.3. Antiswing Control of Quadrotor

In the first step of the antiswing controller design procedure, it should be better to find commands that move systems without vibration. So, it is helpful to start with the simplest command. It is known that, by giving an impulse to the system, it will cause vibration. However, if a second impulse is applied to the system in the right moment, the vibration included by the first impulse is cancelled [23]. This concept is shown in Figure 13. Subsequently, input shaping theory is implemented by convolving the reference command with a sequence of impulses. This process is illustrated in Figure 14. The impulse amplitudes and time locations of their sequence are calculated in order to obtain a stair-like command to reduce the detrimental effects of system oscillations. The amplitude and time locations of impulses are calculated by estimation of system’s natural frequency and the damping ratio [30].Figure 13
Response of dynamic system to two sequent impulses.Figure 14
Input shaping process with ZV shaper.If estimations of system’s natural frequency,ω
n, and the damping ratio, ζ, for vibration modes of the system which must be canceled are known, then the residual vibration that results from the sequence of impulses can be described as
(30)
ν
ω
n
,
ζ
=
e
-
ζ
ω
n
t
N
C
ω
n
,
ζ
2
+
S
ω
n
,
ζ
2
,
where
(31)
C
ω
n
,
ζ
=
∑
i
=
1
N
A
i
e
ζ
ω
n
t
i
cos
⁡
ω
d
t
i
,
S
ω
n
,
ζ
=
∑
i
=
1
N
A
i
e
ζ
ω
n
t
i
sin
ω
d
t
i
,
where A
i and t
i are the amplitude and the time locations of impulses, N is the number of impulses in the impulse sequence, and ω
d
=
ω
n
(
1
-
ζ
2
). To avoid the trivial solution of all zero valued impulses and to obtain a normalized result, it is necessary to satisfy the below equation:
(32)
∑
i
=
1
N
A
i
=
1
.In the modeling of a suspended load, the load damping orζ can be neglected and the frequency of the linear model can be considered:
(33)
ω
n
=
g
l
,
where l is length of the cable. When the amplitude of impulses is considered positive and the estimation of system’s parameters is accurate, the simplest shaper or the ZV shaper can be proposed with two impulses in two times t
1 and t
2. Convolving this shaper to the step input is shown in Figure 14. The ZV shaper can cancel the oscillation of systems whose natural frequency and damping ratio are specific and known. In this case, this assumption can be considered to drive dynamic equations. The cable is considered inelastic and the aerodynamic force on the suspended load is neglected. So, the exact estimation of natural frequency and the damping ratio does not exist. In 1993, Bohlke proposed a method to increase the robustness of the ZV shaper against errors of system’s parameter estimation [23]. In this proposed method, the derivation of the residual vibration to the natural frequency and the damping ratio must be zero as
(34)
d
d
ω
n
ν
ω
n
,
ζ
=
0
,
d
d
ζ
ν
ω
n
,
ζ
=
0
.These two equations can be written as(35)
∑
i
=
1
N
A
i
t
i
e
ζ
ω
n
t
i
cos
⁡
ω
d
t
i
=
0
,
∑
i
=
1
N
A
i
t
i
e
ζ
ω
n
t
i
sin
ω
d
t
i
=
0
.Due to adding these two constraints, two variables are required to be added by an impulse with theA
3 amplitude at the time t
3 to the ZV shaper. The result of this addition is the ZVD shaper with 3 impulses and 5 unknown A
1, A
2, A
3, t
2, and t
3 (with the assumption t
1
=
0) for this shaper. Thus, the following equations can be obtained:
(36)
A
1
+
A
2
e
ζ
ω
n
t
2
cos
⁡
ω
d
t
2
+
A
3
e
ζ
ω
n
t
3
cos
⁡
ω
d
t
3
=
0
,
A
2
e
ζ
ω
n
t
2
sin
ω
d
t
2
+
A
3
e
ζ
ω
n
t
3
sin
ω
d
t
3
=
0
,
A
2
t
2
e
ζ
ω
n
t
2
cos
⁡
ω
d
t
2
+
A
3
t
3
e
ζ
ω
n
t
3
cos
⁡
ω
d
t
3
=
0
,
A
2
t
2
e
ζ
ω
n
t
2
sin
ω
d
t
2
+
A
3
t
3
e
ζ
ω
n
t
3
sin
ω
d
t
3
=
0
.By solving these four equations, unknown parameters of the input shaper are calculated as(37)
t
2
=
T
d
2
,
t
3
=
T
d
,
A
1
=
1
1
+
2
K
+
K
2
,
A
2
=
2
K
1
+
2
K
+
K
2
,
A
3
=
K
2
1
+
2
K
+
K
2
,
where
(38)
K
=
exp
⁡
-
ζ
π
1
-
ζ
2
.For the quadrotor with the suspended load, the cable length is assumed asr
=
1 m. By estimating the natural frequency and the damping ratio, input shaper’s parameters are obtained as
(39)
t
2
=
1.00322
,
t
3
=
2.00644
,
A
1
=
1
4
,
A
2
=
1
2
,
A
3
=
1
4
.However, the transfer function of the designed ZVD shaper is(40)
G
i
s
=
A
1
e
-
t
1
s
+
A
2
e
-
t
2
s
+
A
3
e
-
t
3
s
.For the small oscillation, suspended load’s motion is caused by the horizontal motion of the quadrotor. So, it is necessary that the shaper is implemented on the part of the desired path. If the part of the desired path for the shaper is defined as(41)
X
-
d
=
x
d
,
y
d
,
ψ
d
,
v
x
_
d
,
v
y
_
d
,
ψ
˙
d
,
a
x
_
d
,
a
y
_
d
,
ψ
¨
d
T
,
consequently, the shaped path for the quadrotor is
(42)
X
¯
d
*
t
=
Γ
Z
V
D
·
X
¯
d
t
,
where Γ
Z
V
D is the transfer function matrix of the ZVD shaper in the time domain. By considering the shaped path for the quadrotor controller, the controller performance is improved and the oscillation of the swing load is reduced. In next section, simulation results of the implementing shaper on input commands are shown and discussed.


## 3.4. Simulation Results of Designed Antiswing Controller

To verify the effectiveness and the application effect of the control method for canceling the oscillation of the suspended load, the simulation routine has been carried on the quadrotor with the suspended load. In addition, for estimating the natural frequency and the damping ratio, this system is compared to a cart-pendulum system. So, the natural frequency of system is assumed as (33). Also, the damping ratio of oscillation mode of the swing load is related to the aerodynamics drag on the load and suspension system and, so, the damping ratio is zero according to the small scale of system consideration. Moreover, the system parameters used in the simulation are listed in Table 2. Moreover, the desired path is defined as the line-curve-line and simulation results are shown in Figure 15. By comparing Figure 15 with Figure 16, it can be concluded that the designed feed-forward controller can effectively cancel the oscillation of the suspended load. In addition, Figure 17 shows the position error of the quadrotor both with ZVD shaper and without ZVD shaper. As seen from this figure, the error is converged to zero after a few seconds when input shaping is implemented on desired command, and the performance of the controller is better. Also, it is clear that, by implementing input shaping on a desired path, the required time for tracking the desired path is increased. So, the quadrotor requires more time for tracking the desired path.Figure 15
3D flight of quadrotor with input shaping controller.Figure 16
3D flight of quadrotor without input shaping controller.Position error of quadrotor: (a) without input shaper, (b) with input shaper.
(a)
(b)In the second senario, the square path is designed for the quadrotor flight. In Figures18 and 19, simulation results are shown both with ZVD shaper and without it. As can be seen from these figures, the feed-forward controller can cancel the oscillation. Also, Figure 20 shows the position error of the quadrotor with input shaping. As shown from this figure, error is zero after few seconds.Figure 18
Square path of quadrotor with input shaping controller.Figure 19
Square path of quadrotor without input shaping controller.Figure 20
Position Error of quadrotor with input shaping.


## 4. Conclusions

In this paper, the problem of the quadrotor flying is addressed with a suspended load which is widely used for different kinds of cargo transportation. The suspended load is also known as either the slung load or the sling load. Also, flying with a suspended load can be a very challenging and sometimes hazardous task because the suspended load significantly alters the flight characteristics of the quadrotor. So, many different control algorithms have been proposed to control these systems. To this end, dynamic model of this system was obtained and verified by comparing two Newton-Euler and Lagrange methods. Next, a control algorithm was presented for the position and attitude of the quadrotor. In this algorithm, swinging object’s oscillation may cause danger in the work space and it can make instability in the quadrotor flight. Using comprehensive simulation routine, it was shown that this designed controller could control the robot motion on the desired path but could not reduce the load oscillation in noncontinuous and nondifferentiable paths. To deal with this issue, a feed-forwarded control algorithm was introduced for reducing or canceling swinging load’s oscillation. This controller was designed by implementing the input shaping theory which convolves the reference command with a sequence of impulses. Finally, it was shown that the feed-forward controller could actively improve the performance of the feedback controller.



---

*Source: 265897-2014-11-17.xml*