Research On Key Technologies of Four-Rotor UAV Flight Control System Based On STM32 Microcontroller
Research On Key Technologies of Four-Rotor UAV Flight Control System Based On STM32 Microcontroller
microcontroller
Zhiwei Ren*,# Zucai Tang# Ranran Wang#
School of Geophysics and Information School of Automation Leicester International Institute
Technology Shenyang Aerospace University Dalian University of Technology
China University of Geosciences Shenyang, China Panjin, China
(Beijing) [email protected] [email protected]
Beijing, China #
These authors contributed equally.
[email protected]
Abstract—In order to realize the self-stabilization control of control system of the four-rotor UAV, which achieved good
the four-rotor UAV, the dynamic modeling and control of the control accuracy and response speed. Aiming at the error
four-rotor UAV are carried out. In the modeling, the dynamic problem of direct attitude determination of accelerometer and
simulation model of the four-rotor aircraft is established by magnetometer sensors, a four-element complementary
using the combination of mechanism modeling and filtering fusion algorithm is proposed in literature [3], which
experimental measurement. The inverse kinematics algorithm is reduces the drift error, optimizes the dynamic performance,
used to decompose the attitude model into three independent and achieves the effect of stable flight. Reference [4]
subsystems, which control the roll, pitch and yaw motion of the proposed a position-attitude control algorithm based on back
body respectively. The sliding mode controller based on the
stepping integral-iterative learning control and active
stability theorem is designed for the decomposed subsystems
through the distributed control strategy, and the whole system
disturbance rejection control, which improved the tracking
is simulated on the Matlab platform. The simulation results performance of the system in the effective time.
show that the dynamic model and PID controller can effectively In this paper, a four-rotor UAV flight control system is
realize the self-stabilization control of the aircraft, which lays a developed based on STM32 single-chip microcomputer. By
foundation for the subsequent control research of the four-rotor using STM32 single-chip microcomputer as the core
aircraft. controller, a highly flexible flight control algorithm and real-
time data processing ability are realized[5]. A set of main
Keywords—four-rotor UAV, dynamic modeling, flight
principle, inverse kinematics algorithm, feedforward PID control
control program is designed and improved to realize the space
algorithm attitude control of the four-rotor aircraft.
.
I Ω+ Ω × IΩ= M f − M d (1)
1107
Authorized licensed use limited to: COLLEGE OF ENGINEERING - Pune. Downloaded on February 07,2024 at 03:30:48 UTC from IEEE Xplore. Restrictions apply.
l represents the distance between the center of the rotor Where Ci denotes cos i and Si denotes sin i.
and the center of mass of the fuselage, d represents the drag
coefficient of the rotor, w represents the rotor speed, Fi Considering the flight characteristics of the UAV, in order
represents the lift generated by the four rotors. to simplify the controller process, the gyroscopic effect and
the influence of air resistance generated during the flight are
When the aircraft is rolling and pitching, the gyroscopic neglected. The UAV dynamic model is simplified as follows:
effect Md can be expressed as:
I rθW I y − Iz
l
=ϕ θψ + U2
M d = I rϕW (4) Ix Ix
0 I −I l
θ
= ϕψ
z x + U3
I I
Among them, is the moment of inertia,
Ir y y
2 2 2 2
W = w + w − w − w and φ, θ, ψ represent the angles
1 3 2 4
=ψ Ix − I y + 1 U
θϕ
around the x-axis, y-axis, and z-axis, which are the rolling 4
angle, pitch angle, and yaw angle, respectively. Iz Iz (8)
1
When the four-rotor aircraft rotates in a small range, the
attitude angular velocity of the aircraft can be equal to the
=
x (C ψ Sθ Cϕ + Sψ Sϕ )
m
U1
angular velocity of the coordinate axis around the body
coordinate system, that is: =
y ( Sψ Sθ Cϕ − Cψ Sϕ ) m1 U1
ϕ = p
= z
( Cθ Cϕ ) m1 U1 − g
θ = q (5)
ν = r III. FOUR-ROTOR UAV CONTROL ALGORITHM DESIGN
In order to solve the robustness problem of the traditional
Ui is defined as the control input of four control channels, PID algorithm in the control of the four-rotor UAV, this paper
which are: introduces the design of the attitude loop controller of the
feedforward PID control algorithm. When designing the
attitude control loop, if it is assumed that there is a simple
U1= b ( w22 + w42 + w32 + w12 ) differential relationship between the attitude angle and the
angular velocity, it is generally aimed at the situation where
U 2=
b ( w32 + w42 − w12 − w22 ) the attitude angle is small. In order to expand the scope of
application, this paper uses the inverse kinematics algorithm
(6)
b ( w22 + w32 − w12 − w42 )
to decompose the attitude model into three independent
U 3= subsystems, which control the roll, pitch and yaw motion of
the body respectively. The distributed control strategy is used
U=
4 d ( w12 + w32 − w2 − w42 ) to design the sliding mode controller based on the stability
theorem for the decomposed subsystems[9]. This method can
In summary, the nonlinear mathematical model of the effectively improve the robustness and stability of the four-
four-rotor aircraft can be obtained: rotor UAV control system and make it more reliable in
complex environments.
A. Attitude loop controller design based on feedforward PID
I y − I z − I r θW + l U control algorithm
ϕ θψ
=
Under the assumption that φ = p , θ = q , ψ = r are
2
Ix Ix Ix
I −I I flying in the case of small attitude angle, the attitude angle
θ ϕψ l model of quadrotor UAV can be simplified as follows :
= z x − r ϕW + U 3
I I Iy
y y
Ix − I y + 1 U Uφ
=ψ θϕ 4 (7) =φ + fφ
Iz Iz Jx
1 −k x
x= ( Cψ Sθ Cϕ + Sψ Sϕ ) U1 + dx
m m = θ
Uθ
+ fθ (9)
Jy
1 − k
y= ( Sψ Sθ Cϕ − Cψ Sϕ ) U1 + dy y
m m
=
Uψ
ψ + fψ
1 −kdz Jz
= z ( Cθ Cϕ ) U1 +
z − g
m m
1108
Authorized licensed use limited to: COLLEGE OF ENGINEERING - Pune. Downloaded on February 07,2024 at 03:30:48 UTC from IEEE Xplore. Restrictions apply.
Where, 1
0 0
Jx
J y − Jz Jr
=fφ ( ) qr + ( − w1 + w2 − w3 + w4 ) q (10) 1
Jx Jx G = 0 0 (16)
Jy
Jx − J y 1
fψ = ( ) pq (11) 0 0
Jz Jz
Based on the traditional PID algorithm, this paper adopts u = G −1 (− f ( x) + xdes ) (17)
the decentralized control strategy, and controls the related
trajectory planning through feedforward and nonlinear Among them, the expected condition signal is xdes , and
compensation. For the related paths in the attitude stabilization
controller, the corresponding control algorithm is designed as the dynamic characteristics of x and xdes have the same
follows: properties after the inverse kinematics control of the system.
Assuming that the constant coefficient matrices
u=φ J x (k pφ eφ + kiφ ∫ eφ dt + kdφ eφ + s (φc ) − fφ ) K [k p kq kr ]T and
= = v [v p vq vr ] , the new system model
is expected to be established after the inverse kinematics is
u=θ J y (k pθ eθ + kiθ ∫ eθ dt + kdθ eθ + s (θc ) − fθ ) (13) introduced as follows:
u= J z (k pψ eψ + kiψ ∫ eψ dt + kdψ eψ + s (ψc ) − fψ )
ψ
x = Kv (18)
The relative deviations of the corresponding tilt angles are
At this time, the attitude angular velocity model is
eφ= φc − φ , e= ψ ψ c −ψ and eθ= θ c − θ , respectively. completely decomposed, and the decomposition process is
The proportional, integral, and differential coefficients of the illustrated by the angular velocity control loop structure
diagram, as shown in Fig.4.
rotation angle control law are k pφ , kiφ and kdφ ,
respectively. The proportional, integral, and differential
coefficients of the pitch angle control law are k pθ , kiθ and
kdθ , respectively. The proportional, integral, and differential
coefficients of the yaw angle control law are k pψ , kiψ and
kdψ , respectively.
B. Controller design based on hybrid algorithm of inverse
kinematics and sliding mode control
1) Attitude angular velocity model decoupling and
Fig.4. Structure diagram of angular velocity control loop
control
The angular velocity model of the four-rotor UAV is In view of the poor robustness of the control system
defined as follows: designed by the simple inverse kinematics method, after
decoupling by inverse kinematics, the stability theorem
=x f ( x) + Gu (14) sliding mode method can be used to deal with each
decomposed attitude channel to realize the design of the
Where, controller. In the front, the complete decomposition of each
attitude angular velocity channel is realized by the inverse
kinematics method. If the attitude angular velocity channel has
J y − Jx J
[( ) qr + r (− w1 + w2 − w3 + w4 )q the same characteristics, k= p k=
q kr is selected. The
Jx Jx following is an example of p-channel stability theorem sliding
J −J J
=f ( x) [( z x
) pr + r (− w1 + w2 − w3 + w4 ) p (15) mode controller design. From formula (18), we can get:
Jy Jx
J − Jy
( x ) pq p = k p v p (19)
Jz
1109
Authorized licensed use limited to: COLLEGE OF ENGINEERING - Pune. Downloaded on February 07,2024 at 03:30:48 UTC from IEEE Xplore. Restrictions apply.
s p c p ∫ e p dt + e p [φ θ ψ]
T
= (20) Rewrite the defined x = into the following
form.
The value of the design constant c p is proportional to the
speed when the sliding surface approaches the origin. In order x = g1 ( x)u (25)
to ensure the existence of sliding mode control and the
stability of the system, this paper adopts the method of where,
stability theorem to design. The stability theorem is as follows:
−ε p sgn( s p ) − k sp s p
s p = (21)
1 sin φ tan θ cos φ tan θ
where ε p and k sp are positive numbers, and the sign = g1 ( x) 0 cos φ − sin φ (26)
sin φ cos φ
function is denoted by sgn() . Since s p s p < 0 , the stability 0
of the system can be guaranteed. The exponential law of the cos θ cos θ
approaching process represented by (21) is superimposed on
u = [ p q r]
T
a constant velocity law at the same time. The characteristics (27)
of the motion are only related to the initial state and the
parameters ε p and k sp , that is, the change of the system state
1 π
is independent of the existence of the disturbance. The Since g1 ( x) = obviously when θ ≠ ± , the
approaching mode has certain robustness, and its quality is cos θ 2
realized by the selection of ε p and k sp . Generally speaking, inverse kinematics of the system exists. Therefore, the
following control algorithm can be designed[12]:
in order to weaken the influence of ' jitter ' and speed up the
normal flight, a smaller ε p should be selected and a larger u = g1 ( x) −1 xdes (28)
ksp should be selected. Derivation of formula (20) can be
obtained[10]. At this time, x and xdes have the same dynamic
characteristics, define the constant coefficient matrix
s p = c p e p + p c − k p v p (22) T
K = kφ kθ kψ , V = vφ vθ vψ .After the inverse
kinematics decomposition, a new system model is obtained as
Comprehensive formula (21) and formula (22) are shown in formula (22) :
available, and the rolling angular velocity control algorithm is
designed as follows: x = Kv (29)
1
vq
= (cq eq + p c + ε q sgn( sq ) + ksq sq )
kq
(24)
= 1
v (cr er + p c + ε r sgn( sr ) + ksr sr )
r kr Fig.5. Angle inverse kinematics loop system structure diagram
Among them, the reference input of the pitch angular After decomposition, each channel becomes a simple
integral structure[13].
velocity is qc , e=
q qc − q , the reference input of the yaw
After using the inverse kinematics method, the new
angular velocity is rc , and the constant parameters of the attitude angle model and the angular velocity model have the
corresponding controller are cq , ε q , ksq , cr , ε r , ksr [11], same structure, so it is no longer repeated here. The control
law of each channel of the attitude angle is given directly
where er= rc − r ,=sq cq eq dt + eq∫ , below:
=sr cr ∫ er dt + er .
2) Attitude angle model decoupling and control
1110
Authorized licensed use limited to: COLLEGE OF ENGINEERING - Pune. Downloaded on February 07,2024 at 03:30:48 UTC from IEEE Xplore. Restrictions apply.
also has good scalability. The main purpose of this
1 experimental simulation is to verify the linear flight
= vφ (cφ eφ + φc + ε φ sgn( sφ ) + ksφ sφ )
kφ performance and attitude controller of the quadcopter
unmanned aerial vehicle.
1
= vθ (cθ eθ + θc + εθ sgn( sθ ) + ksθ sθ ) (30) The specific simulation parameters for this simulation
kθ
experiment are as follows: constant drift of gyroscope:
1
=vψ (cψ eψ +ψ c + εψ sgn( sψ ) + ksψ sψ ) 0.0349rad/s, zero drift: 0.003878rad/s, zero drift of
kψ accelerometer: 0.007m/s ², Constant drift: 0.016m/s ²,
Accuracy of magnetometer: 1 °, accuracy of Acronyms and
The reference inputs corresponding to the attitude angle abbreviations in avionics: 10cm.
are φc , φc and ψ c , respectively. The output errors of the roll B. Simulation Experiment of Aircraft Control System
angle, pitch angle and yaw angle are eφ , eθ and eψ , 1) Attitude controller simulation experiment
The corresponding parameters of the controller were
respectively[14]. The constant parameters of the
debugged in this simulation experiment, and simulation
corresponding controller are cφ , ε φ , k sφ , cθ , ε θ , k sθ , cψ , analysis was conducted using the Matlab toolbox. Finally, the
designed controller was loaded onto the experimental
=
and k sψ , where sφ cφ ∫ eφ dt +=
e , sψ cψ ∫ eψ dt + e , platform for actual testing, verifying the effectiveness of the
φ ψ
attitude controller designed in this article. According to Fig.6
=sθ cθ ∫ eθ dt + e . and Fig.7, there is significant vibration in the response curve,
θ
which is caused by the rotation of the propeller during the
IV. FLIGHT CONTROL SYSTEM SIMULATION EXPERIMENT actual measurement process. The control result error is
around 4 °, and it takes about 6 seconds for the cascade PID
A. Experimental simulation environment attitude controller to complete angle tracking. In contrast, the
In the simulation experiment of this flight system, a self- multi-mode PID controller designed in this article reduces the
developed Matlab quadcopter unmanned aerial vehicle required response time to around 4 seconds while ensuring
simulation toolbox with functions such as flight attitude angle control accuracy of around 4 °, resulting in better dynamic
control and autonomous fixed-point flight was used, which performance of the system.
1111
Authorized licensed use limited to: COLLEGE OF ENGINEERING - Pune. Downloaded on February 07,2024 at 03:30:48 UTC from IEEE Xplore. Restrictions apply.
2) Linear Flight Simulation Experiment dynamic model (and hardware) of the quadcopter drone, the
This linear flight simulation experiment was conducted control algorithm of the quadcopter drone, and flight system
using the Matlab toolbox to verify the dynamic flight simulation experiments. The hardware design and control
performance of the controller designed in the article. Set the algorithm design of the quadcopter drone are studied, and the
flight state of the aircraft to forward flight (north direction), following results are obtained:
and the power supply voltage of its motor is at a stable level. (1) In this paper, by studying the composition of the four
The starting coordinates are (0, 0, -10), and the flight rotor UAV and analyzing its flight principle, the navigation
trajectory is shown in Fig.8, with a flight time of 120 seconds. coordinate system and the airframe coordinate system are
established, and the transformation matrix of the two
coordinate systems is given to describe the flight of the four
rotor UAV. Through a series of reasonable assumptions and
Classical mechanics principles, the successful establishment
of nonlinear dynamic equations provides a research basis for
the design of the controller.
(2) Successfully designed an attitude loop controller using
feedforward PID control algorithm, solving the robustness
problem of traditional PID algorithm in the control of
quadcopter unmanned aerial vehicles. More accurate real-
time information has been successfully provided for the flight
control system of quadcopter unmanned aerial vehicles.
(3) After successfully designing various controllers, they
were verified through Matlab simulation and then
summarized in the flight system, ensuring the effectiveness
Fig.8. 3D trajectory diagram of forward flight simulation and stability of the designed control algorithm.
1112
Authorized licensed use limited to: COLLEGE OF ENGINEERING - Pune. Downloaded on February 07,2024 at 03:30:48 UTC from IEEE Xplore. Restrictions apply.