Input휓
Input ZInput휃Input휙휙휙휙휙
휃휃휃휃휃desired
휓desired
휙desired
Z
휓휓휓휓PID controllerG (^1) G 1
G 2 G 2
G 3 G^3
G 4 G^4
X
Y X x
Y y
Z
Z z
- ++
++
Sine wave
Sine wave 1
Sine wave 2
Integrator Integrator 1
Integrator 2Integrator 3
Integrator 4Integrator 5
1
s
1
s
1
s
1
s
1
s
1
s To workspace 2
To workspace 6
To workspace 7
To workspace 8
To workspace 1
To workspace 3
To workspace 4
To workspace 5
To workspace
Quadrotor system
U 1
ax
ay
az
Figure 7: Implementation of the control system in MATLAB Simulink (layer 3 is excluded).
Then, this equation can be expressed as follows:
휓(cos^2 휙cos휃+sin^2 휙cos휃) = 푟cos휙+푞sin휙,
휓=
푟cos휙+푞sin휙
cos휃.
(11)
Finally, the following expression is obtained:[
[
휙̇
휃̇
휓̇
]
]
=
[
[
[
[
[[
[
[
푝cos휃+푞sin휙sin휃+푟cos휙sin휃
cos휃
푞cos휙+푟sin휙푟cos휙+푞sin휙
cos휃]
]
]
]
]]
]
]
. (12)
The quadrotor was previously assumed to be symmetrical
in quality and structure, so the inertia matrix. So it can be
defined as a diagonal matrix:
퐼[
[
퐼푥
퐼푦
퐼푧
]
]
, (13)
where퐼푥푥,퐼푦푦,and퐼푧푧are the rotary inertia around the푋,
푌,and푍axes, respectively.
By calculating the angular momentum, we can obtain the
three axial components’ angular motion equations of푀in
the quadrotor coordinate system:푀푥,푀푦,and푀푧. Consider푀푥=푝퐼̇푥+푞푟(퐼푧−퐼푦),푝=̇
푀푥+푞푟(퐼푦−퐼푧)
퐼푥
,
(14)
푀푦=푞퐼̇푦+푝푟(퐼푥−퐼푧)+(푝^2 −푟^2 )퐼푥푧,
푀푦=푞퐼̇푦+푝푟(퐼푥−퐼푧),
푞=̇
푀푦+푝푟(퐼푧−퐼푥)
퐼푦
,
(15)
푀푧=푟퐼̇푧−푝퐼̇푥푧+푝푞(퐼푦−퐼푥)+푞푟퐼푥푧,
푀푧=푟퐼̇푧+푝푞(퐼푦−퐼푥),
푟=̇
푀푧+푝푞(퐼푥−퐼푦)
퐼푧
.
(16)
After simplification, the formula becomes