OSVC_Meta-Data based Simulation Automation to overcome Verification Challenge...
Lesson_11_Low-level_Flight_Control hexa copt best ppt.pdf
1. Introduction to Multicopter
Design and Control
Quan Quan , Associate Professor
qq_buaa@buaa.edu.cn
BUAA Reliable Flight Control Group, http://rfly.buaa.edu.cn/
Beihang University, China
Lesson 11 Low-level Flight Control
2. 2016/12/25 2
How is a multicopter controlled by motors to
achieve a desired position？
Preface
3. Outline
1. Framework of Low-level Flight Control
2. Model Simplification
3. Position Control
4. Attitude Control
5. Control Allocation
6. Motor Control
7. Comprehensive Simulation
8. Conclusion
2016/12/25 3
4. 1. Framework of Low-level Flight Control
2016/12/25 4
The low-level flight control of multicopters
includes four parts:
Position control : desired position ->
desired roll angle , pitch angle and
total thrust
Attitude control : desired attitude angles
-> desired moment
Control allocation : -> desired
propeller angular speeds
Motor control : -> desired throttle
command of each motor
Figure 11.1 Framework of low-level flight
control of multicopters
d
p
d
d
f
d
τ
d d
, f
τ
d, , 1,2, ,
k k n
d,k
d, , 1,2, ,
k k n
d
d d d
, ,
Motor control
Desired
pitch angle,
roll angle
Control allocation
Desired
moments
Desired propeller
angular speeds
Desired
thrust
Desired
trajectory
Desired
yaw angle
Attitude control
Position control
Desired motor throttle command
5. 2016/12/25 5
Underactuated system：
• four independent inputs (total thrust and three-axis moment ),
• six outputs (position and attitude ).
f τ
p Θ
1. Framework of Low-level Flight Control
Figure 11.2 Closed-loop structure of a low-level fight control system for multicopters
Position
controller
Control
allocator
d
f
Attitude
controller
d d
,
d
τ
d,k
d
Motor
controller
Multicoptor
d,k
p,v
d
p
,
Θ ω
Low-level fight control
6. 2016/12/25 6
Based on the nonlinear model in Lesson 6, by ignoring the
term , the simplified model is
e e
p v
e
3 3
f
g
m
v e Re
b
Θ = W ω
b
J ω τ
b b
a
+
ω J ω G
where , the specific definitions are referred
to Lesson 6.
e e e
T
e 3
x y z
p p p
p
e
e
e
sin sin cos sin cos
cos sin cos sin sin
cos cos
x
y
z
f
p
m
f
p
m
f
p g
m
2. Model Simplification
7. 2016/12/25 7
Some assumptions are made that
. Then,
h h
h h
g
p v
v A Θ
, 0
f mg
τ
sin ,cos 1,sin ,cos 1
cos sin
sin cos
1
3
Re
where
Since is known,
the term can be
viewed as the input.
Thus, the horizontal
position channel model
is linear.
g
A
h
g
A Θ
Three linear models are obtained ：
(1) Horizontal position channel model
2. Model Simplification
e
e
sin sin cos sin cos
cos sin cos sin sin
x
y
f
p
m
f
p
m
h h
cos sin 0 1
, , ,
sin cos 1 0
x
y
p
p
p R A R Θ
e e
p v
e
3 3
f
g
m
v e Re
8. 2016/12/25 8
（3） Attitude model
z z
z
p v
f
v g
m
J
Θ = ω
ω τ
Linear model
（2）Altitude channel model
b
Θ = W ω
b
J ω τ
3
W I
Small-angle
assumption
2. Model Simplification
Linear model
cos cos
z
f
p g
m
e e
p v
e
3 3
f
g
m
v e Re
9. 2016/12/25 9
（3） Attitude model
z z
z
p v
f
v g
m
J
Θ = ω
ω τ
（2） Altitude channel model
Each channel is
a linear model！
（1） Horizontal position channel model
h h
h h
g
p v
v A Θ
Original model
2. Model Simplification
e e
p v
e
3 3
f
g
m
v e Re
b
Θ = W ω
b
J ω τ
10. 2016/12/25 10
Classification
According to the given position , the position control is divided
into three types: the set-point control, trajectory tracking and path
following.
d
p
3. Position Control
Figure 11.3 Three types of position control
11. 2016/12/25 11
（1）Set-Point Control. The desired position is a constant point .
The aim of this part is to design a controller to make or
as .
（2）Trajectory Tracking. The desired trajectory is a
time-dependent trajectory. The aim of this part is to design a controller to
make or as .
（3）Path Following[1]. The desired path is a path directly
determined by the parameter rather than the time . It aims to design a
controller to make or as .
3
d
p
t
d 0
t
p p
3
d : 0,
t
p
3
d
p
d 0
t t
p p
[1] Hespanha J P. Trajectory-tracking and path-following of underactuated autonomous vehicles with parametric
modeling uncertainty. Automatic Control, IEEE Transactions on, 2007, 52(8): 1362–1379.
3. Position Control
d 3 1,
t
p p 0
d 0
t t
p p
d 3 1,
t t
p p 0
t
Classification
t
d 3 1,
t t
p p 0
t
12. 2016/12/25 12
Desired attitude is the output of position controllers. According to
the attitude representation, the position control is divided into two
types:
(1) Euler angles as output. The controller, which is designed
according to the linear systems, generates the desired Euler angles
and thrust .
(2) Rotation matrix as output. The controller, which is designed
according to the nonlinear coupled systems, generates the desired
rotation matrix and thrust .
d d d
, , f
d d
, f
R
3. Position Control
Classification
13. 2016/12/25 13
Euler Angles as Output
h h
h hd d h hd p h hd
p p
p p K p p K p p
• Horizontal channel
h h
h h
g
p v
v A Θ
• Desired transient process
h h
hd hd d h hd p h hd
g
p p
A Θ p K p p K p p
h h
1 1
hd hd d h hd p h hd
g
p p
Θ A p K p p K p p
When a set-point control
problem is considered
hd hd 2 1
p p 0
（1）Traditional PID controller
where represent parameters.
K
3. Position Control
14. 2016/12/25 14
z z
z
p v
f
v g
m
• Altitude channel
• Desired transient process
d d d
d d p
z z
z p z z p z z
f mg m p k p p k p p
d d d
d p
z z
z z p z z p z z
p p k p p k p p
d d
0
z z
p p
d
d d p
z z
p z p z z
f mg m k p k p p
When a set-point control problem is considered . Then
3. Position Control
Euler Angles as Output
（1）Traditional PID controller
15. lim 0
t k
d
f d
τ
,d
k k
0
lim 0
t t
ω
e
0
lim 0
t t
Θ
e
h
0
lim 0
t t
v
e
h
0
lim 0
t t
p
e
lim 0
z
t v
e
0
lim 0
t z
e t
0 d
lim 0
t t t
2016/12/25 15
（2）PID controllers in open source autopilots
1）Horizontal channel
In order to make , according to
h
lim 0
t t
p
e
h h
p v
h
hd hd h
p
v K p p
hd 0
p
h
lim 0
t t
v
e
h
lim 0
t t
p
e
Under the assumption that , if
then
where .
h
v h hd
e v v
If velocity reaches the
desired value, then
position reaches the
desired value
Desired velocity
3. Position Control
Euler Angles as Output
Control objective decomposition for multicopters.
16. 2016/12/25 16
h
lim 0
t t
v
e
h h
g
v A Θ
h h h h h h
hd p i d
g
v v v v v v
A Θ K e K e K e
h h h h h h
1 1
hd p i d
g
v v v v v v
Θ A K e K e K e
If
h hd
lim 0
t t t
Θ Θ
then
h
lim 0
t t
v
e
If attitude reaches
the desired value,
then velocity reaches
the desired value
3. Position Control
Euler Angles as Output
（2）PID controllers in open source autopilots
1）Horizontal channel
In order to make , according to
Control objective decomposition for multicopters.
lim 0
t k
d
f d
τ
,d
k k
0
lim 0
t t
ω
e
0
lim 0
t t
Θ
e
h
0
lim 0
t t
v
e
h
0
lim 0
t t
p
e
lim 0
z
t v
e
0
lim 0
t z
e t
0 d
lim 0
t t t
17. 2016/12/25 17
If vertical velocity reaches the desired value,
then altitude reaches the desired value
Similar to the horizontal channel,
the controller is designed
d d
z
z p z z
v k p p
d p i d
z z z z z z
v v v v v v
f m g k e k e k e
3. Position Control
Euler Angles as Output
（2）PID controllers in open source autopilots
1）Altitude channel
Control objective decomposition for multicopters.
lim 0
t k
d
f d
τ
,d
k k
0
lim 0
t t
ω
e
0
lim 0
t t
Θ
e
h
0
lim 0
t t
v
e
h
0
lim 0
t t
p
e
lim 0
z
t v
e
0
lim 0
t z
e t
0 d
lim 0
t t t
18. 2016/12/25 18
Traditional PID
PID in open
source autopilots
Large position
error
Large attitude error
hd 2
Θ
Small-angle
assumption
is violated
Saturation is necessary
Controllers
make no
sense
3. Position Control
Euler Angles as Output
（2）PID controllers with saturation
h h
1 1
hd d h p h hd
g
p p
Θ A K p K p p
h h h h h h
1 1
hd p i d
g
v v v v v v
Θ A K e K e K e
19. 2016/12/25 19
3. Position Control
Euler Angles as Output
（2）PID controllers with saturation
By considering the saturation, the traditional PID controller
becomes
h h h h
1 1
hd gd p d 0
sat g ,a
p p v p
Θ A K e K e
Similarly, the PID controller used in open source autopilots becomes
h
h h h h h h
gd h hd 1
1 1
hd gd p i d 2
sat ,
sat g ,
a
a
v
v v v v v v
e v v
Θ A K e K e K e
where and the direction-guaranteed saturation function is
defined in Lesson 10.
1 2
,
a a
gd
sat ,a
x
20. 0 1 2 3 4 5
0
1
2
3
4
5
6
7
X Position (m)
Y
Position
(m)
satgd
sat
(m)
gd
sat
sat
y
p
Y
Position
(m)
x
y
T
h 0 0
p
T
hd 100 10
p
T
hd h
sat ,20 20 10
p p
T
gd hd h
sat ,20 20 2
p p
2016/12/25 20
Direction-guaranteed
saturation function
gd
sat , a
x
Traditional
saturation function
sat , a
x
1
sat ,
,
sat , ,sat ,
sign ,
sat ,
k k
k
k k
n
x a
x x a
a x a
a x x a
x a
x
Figure 11.4 Comparison of the effect of two saturation functions
Figure 11.5 The planar trajectories of a multicopter
based on PID control with two saturation
Traditional
saturation
function
The direction-guaranteed saturation function can guarantee that the multicopter
flies along a straight line, but the traditional saturation function cannot.
3. Position Control
Euler Angles as Output
（2）PID controllers with saturation
21. 2016/12/25 21
gd d 4
d gd p i d 5
sat ,
sat ,
z
z z z z z z
v z z
v v v v v v
e v v a
f m g k e k e k e a
3. Position Control
Euler Angles as Output
（2）PID controllers with saturation
In order to avoid a large throttle value, the saturation needs to be
considered as well. Thus, the traditional PID controller becomes
d
d gd d p 3
sat ,
z z
p z p z z
f m g k p k p p a
where . Similarly, the PID controller design used in the open source
autopilots becomes
where . For a scale, the direction-guaranteed saturation function
is the same as the traditional saturation function.
3
a
4 5
,
a a
22. d
f
d
τ
d
d
p
d 3
R e
d
R
,
R ω
d,k
p, v
d,k
The rotation matrix based attitude representation is
The desired attitude control command is . Thus, it is more
beneficial if the corresponding position control provides directly.
2016/12/25 22
Figure 11.6 The rotation matrix based closed-loop control diagram for multicopters
3. Position Control
Rotation Matrix as Output
b
R R
d
R
d
R
23. 2016/12/25 23
Position model
Desired acceleration，
saturation is needed
[2] Lee, T, Leoky M, and McClamroch N H. Geometric tracking control of a quadrotor UAV on SE(3). In:
Proceedings of the 49th IEEE Conference on Decision and Control. Atlanta, Georgia, USA: IEEE, 2010. 5420–5425.
3. Position Control
Rotation Matrix as Output [2]
According to the nonlinear model, we know that
where and
Then, it is obtained that
h h h h h h
gd p i d 1
d
gd p i d 2
sat ,
sat ,
z z z z z z
v v v v v v
a
k e k e k e a
v v v v v v
K e K e K e
a
d
3 3,d d
f
g
m
e r a
3,d d 3
r R e
e
e
3 3
e
f
g
m
v
v
e e
p
R
3 d
3,d
3 d
g
g
e a
r
e a
24. 2016/12/25 24
3. Position Control
Rotation Matrix as Output
What is more, it satisfies , thus
So far, the vector is determined.
In the following, two methods of obtaining are introduced. One
is based on the small-angle assumption, and the other is applicable
to large-angle flight.
(1) Small-angle case
In order to obtain , one of needs to be determined
additionally. According to the definition of the rotation matrix,
is
3
1
r
d
R
d
R
T
3,d 3,d 1
r r
3 d
3,d
3 d
g
g
e a
r
e a
3,d
r
1,d 2,d
,
r r
25. By using the small-angle assumption, it is approximated by
With , in hand, is determined as
2016/12/25 25
3. Position Control
Rotation Matrix as Output
1
cos cos
cos sin
sin
r
d
1,d d
cos
sin
0
r
1,d
r 3,d
r
3,d 1,d
2,d
3,d 1,d
r r
r
r r
2,d
r
26. According to the definition of cross-product, it has . Finally,
define . Then, .
So far, is obtained as
which satisfies .
(2) Large-angle case
Vector can also be expressed as
2016/12/25 26
3. Position Control
Rotation Matrix as Output
(*)
T
d d 3
R R I
d
R
2,d 3,d
r r
1,d 2,d 3,d
r r r 1,d 3,d 1,d 2,d
,
r r r r
d 2,d 3,d 2,d 3,d
R r r r r
3,d
r
d d d d d 11
3,d d d d d d 12
d d 13
cos sin cos sin sin
sin sin cos cos sin .
cos cos
a
a
a
r
27. 2016/12/25 27
3. Position Control
Rotation Matrix as Output
The solutions are
where
Although each Euler angle has two optional values in equation, the true
value is uniquely determined by equation (*) in most cases. Then,
is calculated by .
d
R
d d d
, ,
d,0 d 11 d 12 13
d,1 d 11 d 12 13
d,0 d 11 d 12
d,1 ,0 d,0
arctan 2(cos +sin , )
arctan 2(-cos -sin ,- )
arcsin(sin cos % )
sign .
d
a a a
a a a
a a
d d,0 d,1
d d,0 d,1
or
or
28. Next，desired Thrust is calculated.
Since and are coupled, both sides of are left
multiplied by . Then
If ，then
2016/12/25 28
3. Position Control
Rotation Matrix as Output
d
f
d min max
,
f f f
d
f
T
d 3,d 3 d
f m g
r e a
T min max max min min max
d gd 3,d 3 d
sat ,
2 2 2
f f f f f f
f m g
r e a
T
3,d
r
d
3 3,d d
f
g
m
e r a
3,d
r
29. 2016/12/25 29
4. Attitude Control
The successive loop closure is adopted in flight
controller design for multicopters. The outer
loop controller provides commands for the inner
loop controller. Here, or is viewed as
the reference. Subsequently, the attitude control
objective is to accomplish
or . It is required that the
convergence rate of the attitude loop is 4∼10
times higher than that of the horizontal position
channel dynamics. Then, it can be considered
that the attitude control objective or
has already been achieved from the control of
the horizontal position channel. As long as the
attitude control is achieved, the horizontal
position control problem can be solved.
hd
Θ d
R
h hd
lim 0
t t t
Θ Θ
T
d 3
lim 0
t
R R I
h hd
Θ Θ
d
t t
R R
lim 0
t k
d
f d
τ
,d
k k
0
lim 0
t t
ω
e
0
lim 0
t t
Θ
e
h
0
lim 0
t t
v
e
h
0
lim 0
t t
p
e
lim 0
z
t v
e
0
lim 0
t z
e t
0 d
lim 0
t t t
30. Attitude control is the basis of the position control. At present, the
commonly-used attitude representations for a rigid body include the
Euler angles and rotation matrix.
2016/12/25 30
Table11.1 Comparison of attitude representation
4. Attitude Control
Basic Conception
Attitude representations Advantages Disdvantages
Euler angles
No redundant parameter;
Clear physical meaning
Singularity problem when the
pitch angle is 90 degrees; Plenty
of transcendental function
operation; Gimbal Lock
Rotation matrix
No singularity problem; No
transcendental function
operation; Applicability for the
continuous rotation; Global and
unique; Easy to interpolate
Six redundant parameters
31. The attitude control objective is: given the desired attitude angle ，
design a controller such that ，where . Here is
generated by the position controller and is given by the mission planner. To
achieve that, according to ，the desired angle velocity is designed as
where .The above two equations build the angle control loop.
According to ，the remaining task is to design the desired moment
where , . The above two equations build the angular
velocity control loop.
2016/12/25 31
How to add saturation?
4. Attitude Control
Euler Angles Based Attitude Control
T
T
d hd d
Θ Θ
d
τ
lim 0
t t
Θ
e d
Θ
e Θ Θ
hd
Θ
d
Θ = ω
d Θ Θ
ω K e
3 3
K
Jω τ
d p i d
ω ω ω ω ω ω
τ = K e K e K e
d
ω
e ω ω
3 3
p i d
, ,
ω ω ω
K K K
32. 2016/12/25 32
T T
R d d
1
vex
2
e R R R R
Where
3 2
T 3
1 2 3 3 1
2 1
0
, 0 ,vex
0
x x
x x x x x
x x
x x x x
4. Attitude Control
Rotation Matrix Based Attitude Control
the attitude error matrix is defined as
From the definition of rotation matrix, if and only if , then .
Based on this, the control objective of the rotation matrix based attitude
control is .
First, the attitude tracking error is defined as
T
d
R R R
d
R R 3
R I
3
lim 0
t t
R I
33. Furthermore, the angular velocity tracking error is defined as
the following PD controller is designed
where . The PD controller above can only guarantee the system stability
within a small neighborhood of the hover position. In order to obtain a wider range
of stability, through bringing in the correct error term, a nonlinear controller is
designed as
This controller is guaranteed to be exponentially stable for almost all rotation action.
In practice, the last term in the controller is very small, so it can be ignored to
achieve satisfactory performance. However, for an aggressive maneuver flight, the
last term is dominant and must be considered.
2016/12/25 33
4. Attitude Control
Rotation Matrix Based Attitude Control
T
d d
ω
e = ω R R ω
d R R
ω ω
τ K e K e
3 3
R ω
K ,K
T T
d d d d d
R R ω ω
τ K e K e J ω R R ω R R ω
[3] Lee, T, Leoky M, and McClamroch N H. Geometric tracking control of a quadrotor UAV on SE(3). In:
Proceedings of the 49th IEEE Conference on Decision and Control. Atlanta, Georgia, USA: IEEE, 2010. 5420–5425.
34. 2016/12/25 34
• Separate upper-level control and
lower-level control to simplify the
controller design ;
• To improve the system robustness
through allocation ;
• ….
5. Control Allocation
Basic Conception
Briefly, the control allocation problem
can be described as: given ，seek
to satisfy
where is the mapping from
control inputs of actuators in controlled
systems to the pseudo control command.
In particular, the linear control allocation
problem is based on
where is the known control
efficiency matrix.
v t
u
t
δ
v t g t
u δ
: m n
g
v t t
u Bδ
m n
B
g
v t
u
t
δ
35. 2016/12/25 35
4
2
1
T T T T
2
2
T T
2
T T 3
2
M M M M 4
0 0
0 0
x
y
z
f c c c c
dc dc
dc dc
c c c c
M
r
r
r r
r
2
T T T 1
2
T 1 T 2 T 2
T 1 T 2 T
2
M 1 M 2 M
sin sin sin
cos cos cos
n
n
x
n
y
z n n
c c c
f
dc dc dc
dc dc dc
c c c
M
The control allocation maps for a plus-configuration quadcopter is
5. Control Allocation
Implementation of Control Allocation in Autopilots
For a quadcopter, since is nonsingular, the control allocation matrix can
be directly obtained by matrix inversion ，where . The allocation is
unique. Furthermore, if a multicopter has more than four propellers, the
allocation may be non-unique. In open source autopilots, the control allocation
matrix is often obtained by calculating the pseudo-inverse, that is
where ， .
4 4
4
M
1
4 4
P M 4 4
4
P
r r
†
n n
P M
r
r
4
n
n
P r
r
4 n
n
M
36. After the desired thrust and the desired moment have been obtained,
the desired propellers’ angular velocities are obtained as follows
Furthermore, some parameters in , namely are unknown.
Under this condition, a question arises: how is the control allocation
conducted？
r
r
2
d,1
2
d
d,2
d
2
d,
n
n
f
P
τ
2016/12/25 36
is related to parameters
？
5. Control Allocation
Implementation of Control Allocation in Autopilots
d
f d
τ
r
n
M T M r
, , , 1,
i
c c d i n
37. 2016/12/25 37
Unkown parameters
Known parameters
5. Control Allocation
Implementation of Control Allocation in Autopilots
In order to answer this question, the control effectiveness matrix for a
standard multicopter is defined as a function matrix that
which satisfies
where . Thus, the following relation exists
r
T T T
T 1 T 2 T
T M
T 1 T 2 T
M 1 M 2 M
sin sin sin
, ,
cos cos cos
r
r
r
n
n
n
n
c c c
dc dc dc
c c d
dc dc dc
c c c
M
r r
T M a
, , 1,1,1
n n
c c d
M P M
a T T T M
diag c dc dc c
P
r r
† † 1
T M a
, , 1,1,1
n n
c c d
M M P
38. 2016/12/25 38
†
6
1 0 2 1
1 3 1 1
1 3 1 1
1
1,1,1
1 0 2 1
6
1 3 1 1
1 3 1 1
M
Figure 11.7 A hexacopter with plus-configuration
T T T T T T
T T T T
6 T M
T T T T
T T
M M M M M M
3 3 3 3
0 0
2 2 2 2
, ,
2 2 2 2
c c c c c c
dc dc dc dc
c c d
dc dc dc dc
dc dc
c c c c c c
M
Taking the hexacopter for example
Then
2
d,1 d T
2
d T
d
d,2 † -1 ?
6 a 6
d T
d
2
d M
d,6
τ
1,1,1 1,1,1
τ
τ
x
y
z
f c
dc
f
dc
c
M P M
τ
In most autopilots,
controllers are all PID
controllers. So, the
unknown variables can
be compensated for by
adjusting PID
parameters.
5. Control Allocation
Implementation of Control Allocation in Autop
Propeller 1
60
Propeller 6
b
x
b
y
Propeller 5
Propeller 4
Propeller 3
Propeller 2
39. Suppose propeller angular speeds are . The controller design
for the motor based on the motor throttle command is proposed in the
following. The control objective is to design each motor throttle such
that . The propulsor model with throttle command as the
input is
where throttle is the input, propeller angular speeds is the output.
The motor control objective is to minimize the control error .
Thus, a controller is designed as
where .
2016/12/25 39
6. Motor Control
Closed-loop Control
r
, 1,2, ,
k k n
d,k
d,
lim 0
t k k
R b
1
1
k k
m
C
T s
k
k
d,
k k k
d, p i d
k k k k
k k k
p i d
, ,
k k k
40. Most autopilots usually adopt open-loop control. After being
obtained, the desired propeller angular speeds can be
obtained through control allocation, which is proportional to the motor
throttle command. Thus, a controller is designed as
where unknown parameters can be compensated for by PID
parameters in the position and attitude controllers.
2016/12/25 40
6. Motor Control
Open-loop Control
d d
,
f τ
, r
, 1,2, ,
d k k n
, ,
d k d k
a b
,
a b
41. 2016/12/25 41
To demonstrate the effectiveness of the introduced controllers,
simulations are carried out by using MATLAB. The selected quadcopter
is plus-configuration. In order to be consistent with practical scenario,
the input saturation, propulsor dead-zone and motor dynamics are all
considered in the simulation.
9.8100 0.0095
0.2223 0.0186
1.0230 0.0760
1.4865e-07 80.5840
2.9250e-09 976.2000
0.0095
g
2
m s yy
J
2
kg m
d
m zz
J
2
kg m
m
kg
T
c
2
N/RPM R
C
RPM
M
c
2
N m/RPM
b
RPM
xx
J
2
kg m
M
T
s
Table 11.2 System parameters for the quadcopter
7. Comprehensive Simulation
42. Time（s） （m ） （ m） （m ） （deg ）
0 0 0 -3 0
5 10 0 -3 0
10 0 10 -3 0
15 -10 0 -3 0
20 0 -10 -3 0
25 10 0 -3 0
30 0 0 -3 0
2016/12/25 42
Control task: a quadcopter is required to fly a square trajectory starting from the
central point in the horizontal xy plane, and then fly back to the central point to
hover. The initial states of the system are selected as
Table 11.3 Time series of the desired trajectory commands
T
0 = 4000 4000 4000 4000 ,
RPM
T
m
0 = 0 0 ,
3
p
T
0 = 0 0 0 m/s ,
v
T
0 = 0 0 0 deg ,
Θ
T
d
0 e
= 0 0 0 .
g/s
ω
d
x d
y d
z d
7. Comprehensive Simulation
43. 2016/12/25 43
Two kinds of control schemes:
(1)Euler angles based attitude control combined with Euler angles based position control.
(2)Rotation matrix based attitude control combined with rotation matrix based position control.
Figure 11.8 Structure chart of comprehensive simulation
（1）
Command
module
Position
module
Attitude
module
Control
allocation
module
Plant
model
module
7. Comprehensive Simulation
44. 2016/12/25 44
(1)Euler angles based attitude control combined with Euler angles based position control.
Figure 11.9 State response
Figure 11.10 Trajectory tracking effect
Figure11.11 Throttle commands and propeller angular speeds
6. Comprehensive Simulation
-10
0
10
-10
0
10
0
1
2
3
X (m)
Trajectory Tracking
Y (m)
h
(m)
-10 -5 0 5 10
-10
-8
-6
-4
-2
0
2
4
6
8
10
X (m)
Y
(m)
X-Y Plane Tracking
x
p
y
p
z
p
x
p
y
p
0 20 40
-0.2
-0.1
0
0.1
0.2
Time (s)
P
(rad/s)
0 20 40
-0.2
-0.1
0
0.1
0.2
Time (s)
Q
(rad/s)
0 20 40
-0.02
-0.01
0
0.01
0.02
Time (s)
R
(rad/s)
0 20 40
-0.4
-0.2
0
0.2
0.4
Time (s)
Phi
(rad)
0 20 40
-0.4
-0.2
0
0.2
0.4
Time (s)
Theta
(rad)
0 20 40
-0.02
-0.01
0
0.01
0.02
Time (s)
Psi
(rad)
0 20 40
-4
-2
0
2
4
Time (s)
Vx
(m/s)
0 20 40
-4
-2
0
2
4
Time (s)
Vy
(m/s)
0 20 40
-0.04
-0.02
0
0.02
0.04
Time (s)
Vz
(m/s)
0 20 40
-10
-5
0
5
10
Time (s)
X
(m)
0 20 40
-10
-5
0
5
10
Time (s)
Y
(m)
0 20 40
-3.02
-3
-2.98
-2.96
-2.94
Time (s)
Z
(m)
rad/s
x
rad/s
y
rad/s
z
rad
rad
rad
m/s
x
v
m/s
y
v
m/s
z
v
m
x
p
m
y
p
m
z
p
0 5 10 15 20 25 30 35 40 45
38
39
40
41
42
Time (s)
Motor
Throttle
Command
(%)
Motor 1
0 5 10 15 20 25 30 35 40 45
4000
4050
4100
4150
4200
Motor
Speed
(RPM)
0 5 10 15 20 25 30 35 40 45
38
40
42
Time (s)
Motor 2
0 5 10 15 20 25 30 35 40 45
4000
4100
4200
0 5 10 15 20 25 30 35 40 45
38
40
42
Time (s)
Motor 3
0 5 10 15 20 25 30 35 40 45
4000
4100
4200
0 5 10 15 20 25 30 35 40 45
38
40
42
Time (s)
Motor 4
0 5 10 15 20 25 30 35 40 45
4000
4100
4200
Propeller 1
Propeller 2
Propeller 3
Propeller 4
Time (s)
Throttle
command
%
Propeller
angular
speed
RPM
Throttle command
Propeller speed
45. 2016/12/25 45
(2)Rotation matrix based attitude control combined with rotation matrix based position control.
Figure 11.12 State response
Figure 11.13 Trajectory tracking effect
Figure11.14 Throttle commands and propeller angular speeds
6. Comprehensive Simulation
0 20 40
-0.2
-0.1
0
0.1
0.2
Time (s)
P
(rad/s)
0 20 40
-0.2
-0.1
0
0.1
0.2
Time (s)
Q
(rad/s)
0 20 40
-5
0
5
x 10
-3
Time (s)
R
(rad/s)
0 20 40
-0.2
-0.1
0
0.1
0.2
Time (s)
Phi
(rad)
0 20 40
-0.2
-0.1
0
0.1
0.2
Time (s)
Theta
(rad)
0 20 40
-0.01
0
0.01
0.02
Time (s)
Psi
(rad)
0 20 40
-4
-2
0
2
4
Time (s)
Vx
(m/s)
0 20 40
-4
-2
0
2
4
Time (s)
Vy
(m/s)
0 20 40
-0.01
0
0.01
0.02
Time (s)
Vz
(m/s)
0 20 40
-10
-5
0
5
10
Time (s)
X
(m)
0 20 40
-10
-5
0
5
10
Time (s)
Y
(m)
0 20 40
-3.005
-3
-2.995
-2.99
-2.985
Time (s)
Z
(m)
rad/s
x
rad/s
y
rad/s
z
rad
rad
rad
m/s
x
v
m/s
y
v
m/s
z
v
m
x
p
m
y
p
m
z
p
-10
0
10
-10
0
10
0
1
2
3
X (m)
Trajectory Tracking
Y (m)
h
(m)
-10 -5 0 5 10
-10
-8
-6
-4
-2
0
2
4
6
8
10
X (m)
Y
(m)
X-Y Plane Tracking
x
p
y
p
z
p
x
p
y
p
0 5 10 15 20 25 30 35 40 45
38
39
40
Time (s)
Motor
Throttle
Command
(%)
Motor 1
0 5 10 15 20 25 30 35 40 45
4000
4100
4200
Motor
Speed
(RPM)
0 5 10 15 20 25 30 35 40 45
38.5
39
39.5
40
Time (s)
Motor 2
0 5 10 15 20 25 30 35 40 45
4000
4050
4100
4150
0 5 10 15 20 25 30 35 40 45
38
39
40
41
Time (s)
Motor 3
0 5 10 15 20 25 30 35 40 45
4000
4050
4100
4150
0 5 10 15 20 25 30 35 40 45
38.5
39
39.5
40
Time (s)
Motor 4
0 5 10 15 20 25 30 35 40 45
4000
4050
4100
4150
Throttle
command
%
Propeller
angular
speed
RPM
46. 8. Conclusion
2016/12/25 46
The controller design for multicopters is based on specific control
requirements. For different control requirements, different
mathematical models and corresponding control methods are selected.
The design using successive loop closure is often adopted in flight
control. Multiple closed-loop feedbacks work together to realize the
flight control of multicopters.
Low-level flight control of multicopters mostly depends on the PID
control, which is sufficient for most requirements. However, it is not
enough for tough requirements. Therefore, a question is being asked:
how to design low-level flight controllers for multicopters to obtain a
stability region as large as possible, decrease vibration, save energy,
resist wind, achieve a high precision hover or an aggressive maneuver
flight?
48. 2016/12/25 48
Thank you!
All course PPTs and resources can be downloaded at
http://rfly.buaa.edu.cn/course
For more detailed content, please refer to the textbook:
Quan, Quan. Introduction to Multicopter Design and Control. Springer,
2017. ISBN: 978-981-10-3382-7.
It is available now, please visit http://
www.springer.com/us/book/9789811033810
