ZX휃휃Figure 5: Directional diagram of the control system.PID control u Plant yErrorDisturbanceFeedback+ +
Reference − +Figure 6: Block diagram of the PID feedback.to the표푥푧plane; the positive direction is the direction
conforming to the right hand rule.
Thesetwocoordinatescanbeconvertedtoeachother
through transition matrix푅.
Euler angles are defined as follows.
Pitch angle휃:anglebetweenthe푍-axis and the projection
of푂푧in the푂푋푌plane.
Yaw angle휑:anglebetweenthe푋-axis and the projection
of푂푥in the푂푋푌plane.
Roll angle휓:anglebetweenthe푌-axis and the projection
of푂푦in the푂푋푌plane.
Consequently, we can obtain the transition matrix푅,
which is from the quadrotor coordinate system to the inertial
frame. Consider
푅푥=[
[
10 0
0 cos휙−sin휙
0 sin휙 cos휙]
]
,
푅푦=[
[
cos휃0sin휃
010
−sin휃0cos휃]
]
,
푅푧=[
[
cos휓−sin휓0
sin휓 cos휓0
001]
]
.
(4)
With attitude angles defined as inFigure 2,thetransfor-
mation matrix from the inertial coordinates to the fixed body
coordinates is
푅(휙,휃,휓)
=푅푥⋅푅푦⋅푅푧=[cos휓cos휙 cos휓sin휃sin휙 cos휓sin휃cos휙+sin휓sin휙
sin휓cos휃 sin휓sin휃sin휙 sin휓sin휃cos휙−sin휙cos휓
−sin휃 cos휃sin휙 cos휃cos휙].(5)Then, we can obtain퐹푥=푘푡
4
∑
푖=1휔^2 푡(cos휓sin휃cos휙+sin휓sin휙),퐹푦=푘푡
4
∑
푖=1휔^2 푡(sin휓sin휙cos휙+cos휓sin휙),퐹푧=푘푡
4
∑
푖=1휔푡^2 (cos휙cos휙).(6)
By Newton’s second law of motion퐹=푚푎=푚⃗
㨀㨀→
푑푉
푑푡
. (7)
By Newton’s second law, the dynamic equation of the
quadrotor, the line motion equation can be obtained. It is
defined as follows:푥=̈
(퐹푥−퐾 1 푥)̇
푚
=
푘푡∑^4 푖=1휔푡^2 (cos휓sin휃cos휙+sin휓sin휙) − 퐾 1 푥̇
푚,
푦=̈
(퐹푦−퐾 2 푦)̇
푚
=
푘푡∑^4 푖=1휔푡^2 (sin휓sin휃cos휙+cos휓sin휙) − 퐾 2 푦̇
푚,
푧=̈
(퐹푧−퐾 3 푧−푚푔)̇
푚
=
푘푡∑^4 푖=1휔푡^2 (cos휙cos휙) − 퐾 3 푦̇
푚−푔,
(8)
where,퐾 1 푥̇,퐾 2 푦̇,and퐾 3 푧̇is the air resistance.
According to the relationship between Euler angle and
angular velocity of the quadrotor, the following result can be
obtained:[
[
푝
푞
푟
]
]
=[
[
휙−̇휓̇sin휃
휃̇cos휙+휓̇sin휙cos휃
−휃̇sin휙+휓cos휙cos휃]
]
. (9)
Expressed with regard to휓,weobtain−휃=̇
푟−휓cos휙cos휃
sin휙,
휃=̇푞−휓sin휙cos휃
cos휙,
휓cos휙cos휃−푟
sin휙=
푞−휓sin휙cos휃
cos휙