Design of STM32-based Quadrotor UAV Control System

Abstract

The four wing unmanned aerial vehicle owns the characteristics of small size, light weight, convenient operation and well stability. But it is easily disturbed by external environmental factors during flight with these disadvantages of short endurance and poor attitude solving ability. For solving these problems, a microprocessor based on STM32 chip is designed and the overall development is completed by the resources such as built-in timer and multi-function mode general-purpose input/output provided by the master micro controller unit, together with radio receiver, attitude meter, barometer, electronic speed control and other devices. The unmanned aerial vehicle can be remotely controlled and send radio waves to its corresponding receiver, control the analog level change of its corresponding channel pins. The master control chip can analyze and process the data to send multiple sets pulse signals of pulse width modulation to each electronic speed control. Then the electronic speed control will transform different pulse signals into different sizes of current value to drive the motor located in each direction of the frame to generate different rotational speed and generate lift force. To control the body of the unmanned aerial vehicle, so as to achieve the operator's requirements for attitude control, the PID controller based on Kalman filter is used to achieve quick response time and control accuracy. Test results show that the design is feasible.

1. Introduction

The control method of unmanned aerial vehicle (UAV) is usually divided into two types: one is semi-autonomous control by radio remote control; another is fully autonomous control by its own carrying program to control its flight [1]. They can let aerial missions and various load tasks to be performed unmanned. In recent years, as one of the research focuses in the field of aviation, UAVs have these advantages of low cost and convenience as to been widely used for development [2].

For various industry use demands, UAVs can be divided into rotary wing and fixed wing, the two are mainly different in the way of power generation and support. For the rotary wing UAV of our main research, it can take off and land vertically in the absence of a runway. It is also able to achieve precise hovering as well as accurate and careful observation of the target. The quadrotor is widely used because of small body and simple operation, but the poor stability and efficiency, low flight altitude, limited maneuvering distance, small load and poor range are its disadvantages. Besides, the rotor class UAV above the rotor blade speed movement may also appear high frequency reflection phenomenon causing its own signal transmission and communication delay and interference.

The applications of drone can be seen in all aspects of people's lives. Existent UAVs can be divided into the following areas according to their different applications [3]. In transportation, UAVs are the key role of monitoring road congestion and assisting in road planning and management. In the matter of emergency rescue, UAVs can perform their tasks by faster and better than ordinary methods. In aspect of construction, UAVs can take pictures of things at a better angle and show the building in a three-dimensional way through post-processing. In the military field [4], it has high requirements for its performance, such as in terms of combining with artificial intelligence, reaction sensitivity, the height and speed achieved during the flight of the UAV. In actual combat, in order to deal with unexpected and changed situations, the UAVs are usually used as the forerunner to detector the environment. In the ecological field, UAVs can track and photograph animal populations with high migration capacity in real time with relative safety for a long time. In the field of meteorology, UAVs can also obtain precise meteorological data in a very short period of time and own more accurate information on extreme weather such as thunderstorms to facilitate people's travel. Due to the convenient control of UAVs gaining wide application, it has high academic values [5].

The sensor and control method of the four-rotor UAV have become the key to realize the spatial attitude detection and accurate attitude adjustment [6][7]. The massive quadrotor MD4-1000 with excellent control performance and interference immunity is able to fly in the air for 60 minutes with a payload of 1000g and is capable of stable flight in a complex atmospheric environment, [8]. The dragan-flyer quadrotor UAV with carbon fiber propellers has well-stability as well as remote control for shooting, and the product has a modular design for convenient to disassemble and carry [9]. In terms of control, the attitude control strategy is realized by building the corresponding mathematical model and using multi-sensor technology and data fusion algorithm. Currently, it is dominated by Kalman filter algorithm and traditional self-tuning algorithm based on PID. The PID control method had been designed to control UAV [10]. Liu adopted linear quadratic optimal method was used to control the UVA [11]. The neural network algorithm had been adopted to realize self-learning of UAV attitude [12].

In this paper, Kalman filter is used to eliminate the noise and interference of the original data of the sensor, and data fusion and calculation are carried out. The cascade PID control algorithm is also utilized to realize the control function of UAV with short response time and control accuracy.

The main contributions of this paper can be summarized as follows: The total architecture of STM32-based quadrotor UAV control system is proposed, and the overall development and experiments are completed by the resources such as built-in timer and multi-function mode general-purpose input/output (GPIO) provided by the master microcontroller unit (MCU) together with radio receiver, attitude meter, barometer, electronic speed control (ESC) and other devices. The radio waves can be delivered to the corresponding receiver through remote control, control the analog level change of the corresponding channel pins. This quadrotor UAV completes data communication primarily through the inter-integrated circuit (IIC), while a nine-axis attitude sensor consisting of the MPU6050 and the three-axis magnetometer AK8975 are used to control the attitude of the UAV. At last, the reliability of the system is proved by flight test.

The remained structure of this paper is as follows. The section 2 carries out the establishment of the quadrotor UAV model, mainly mechanical structure design and dynamics analysis. The section 3 focuses on the control algorithm and the section 4 presents designs and program development. The section 5 conducts experiments and finally concludes is given out in section 6.

2. Modeling of UAV

2.1 Hardware design of UAV system

The whole structure of proposed UAV is shown in Fig. 1. The shape of it is the classical X-type quadrotor with a wider field of view. The principle of its division is based on whether the mutual line of the foremost and last two rotor axes is in the same vertical line with the forward direction of the airframe. There are two types of rotor platter planes: horizontal (forward flight with forward tilt) and tilted (forward flight with horizontal body). The relative position of the rotor platter planes and the arm are also divided into two cases: the rotor is located above the arm (to protect the propeller) or below (with the airflow under the propeller intact).

Fig. 1. The proposed UAV

2.2 Dynamics modeling

The quadrotor UAV with six degree of freedoms (6-DOFs) can rotate and translate along the coordinate XYZ axes when flying as shown in Fig. 2. The way to achieve hovering and flying in a predetermined direction is to make the four rotors at the same height with one pair of diagonal rotors rotating clockwise and the other pair of diagonal rotors rotating counterclockwise. Thus each other's torque can be offset with motor 1 and motor 3 rotating clockwise and motor 2 and motor 4 rotating counter clockwise.

Fig. 2. Quadrotor UAV motion analysis diagram

The dynamics models of the UAV is characterized by possessing multiple inputs and outputs with a strongly coupled front drive system [13]. Therefore, it can be expressed as follows from Lagrange's equation

\(\begin{aligned} \begin{cases} \ddot{x}=u_{1}(\cos \phi \sin \theta \cos \psi+\sin \phi \sin \psi)-k_{1} \dot{x} / m \\ \ddot{y}=u_{1}(\sin \phi \sin \theta \cos \psi-\cos \phi \sin \psi)-k_{2} \dot{y} / m \\ \ddot{z}=u_{1} \cos \phi \cos \psi-g-k_{3} \dot{z} / m \end{cases} \end{aligned}\)       (1)

\( \begin{aligned} \begin{cases} \ddot{\theta} =u_2-\frac{lk_4}{I_1}\dot{\theta}\\ \ddot{\psi} =u_3-\frac{lk_5}{I_1}\dot{\psi}\\ \ddot{\phi}=u_4-\frac{lk_6}{I_3}\dot{\phi} \end{cases} \end{aligned}\)      (2)

where cross-roll angle is 𝜙, pitch angle is θ, yaw angle is ψ, l is the distance between the center of rotation of the motor and the center of the frame, m is mass, I_i represents the rotational moment of inertia around each axis, k_i is the drag coefficient.

The final advective equations can be established when all equations are modeled in the geographic coordinate system [14][15]. Then the final equations of motion can be expressed as [16]

\(\begin{aligned} \begin{cases}m\left[\begin{array}{l}a_{x} \\ a_{y} \\ a_{z}\end{array}\right]=u_{1}\left[\begin{array}{c}\sin \phi \sin \psi+\cos \phi \sin \theta \cos \psi+\sin \phi \sin \psi \\ \sin \phi \sin \theta \cos \psi-\cos \phi \sin \psi \\ \cos \phi \cos \psi\end{array}\right]+\left[\begin{array}{l}d_{x} \\ d_{y} \\ d_{z}\end{array}\right] \\ J(\eta)\left[\begin{array}{c}\ddot{\phi} \\ \ddot{\theta} \\ \ddot{\psi}\end{array}\right]=-C(\eta \dot{\eta}) \dot{\eta}\left[\begin{array}{c}\dot{\phi} \\ \dot{\theta} \\ \dot{\psi}\end{array}\right]+\left[\begin{array}{l}u_{2}+d_{\alpha} \\ u_{3}+d_{\beta} \\ u_{4}+d_{\gamma}\end{array}\right] \end{cases} \end{aligned}\)       (3)

where a_x, a_y, and a_z are the accelerations of the body in the geographic coordinate system along the direction of the XYZ coordinate axes; u₁ is the total lift of the quadrotor; d_x, d_y, and d_z are the disturbances force decomposed to the direction of the XYZ axes; J(η) is the rotational moment of inertia of the body; \(\begin{aligned}C(\eta \dot{\eta}) \dot{\eta}\end{aligned}\) is the torque; u₂, u₃, and u₄ are the moments acting on the three axes of the body coordinate system; d_α, d_β, and d_γ are the interference torque; u_i is the amount of control input to the system from the flight control algorithm.

3. Control algorithm

3.1 Control algorithm

In the current design of complex system, numerous applications are combining standard control methods with software computational methods to make them more effective for the controller of such systems [17-21]. Some of the main control methods and evaluations for UAV are listed in Table 1, The cascade PID algorithm is used to establish proportional, integral and differential relationships between attitude information and propeller speed to achieve a fast dynamic response of the UAV by adjusting the parameter values of each link [22].

Table 1. Comparison of control methods

The cascade PID is used for error elimination in the UAV in this paper, which is actually two inner and outer-loop PID controls with strung together and the output of the outer-loop PID is the expected value of the inner-loop PID. The outer-loop PID is generally used to control the flight altitude and horizontal position of the UAV and other data. The whole control strategy of UAV system is given out as shown in Fig. 3. The design control algorithm is shown in Fig. 4.

Fig. 3. Schematic diagram of cascade PID controller

Fig. 4. PID control algorithm

Linearizing (3), the following equation can be obtained as

\(\begin{aligned} \begin{cases} a_{x}=u_{1} \theta / m \\ a_{y}=-u_{1} \phi / m, \\ a_{z}=u_{1} / m-g \end{cases} \begin{cases} \ddot{\psi}=u_{2} / I_{1} \\ \ddot{\theta}=u_{3} / I_{2} \\ \ddot{\phi}=u_{4} / I_{3} \end{cases} \end{aligned}\)       (4)

3.2 Attitude solving

Attitude solver is used to obtain real-time information of three angles of rotational motion of the multi-rotor UAV. The data of gyroscope, accelerometer, compass, etc. are accessed by the flight control program. These values are calculated and processed by the primary program to acquire the required amount of lift for each rotor and achieve dynamic balance and stability of the UAV or stable flight towards a direction of a certain target [23][24].

The UAV on-board sensors are mounted on the similar IIC bus and the block diagram is shown in Fig. 5. Kalman filter is preferentially applied by the flight control program for noise and interference reduction of the raw sensor data. The fused values are computationally processed to obtain the current coordinates of the UAV (i.e., the raw data is converted to attitude data). In the end the flight control program compares the variance of the attitude data with the desired attitude. The different PWM pulses are sent to the ESC of each rotor to attain a stable flight state of the UAV.

Fig. 5. Attitude solution

In the PID control algorithm, the attitude angle is solved by the quaternion method to complete the whole control process. Using the Eulerian angle approach to describe the control process, the attitude Eulerian angle is converted by the previously obtained one after the solution [25-27].

The core of Euler's angle method is to represent a coordinate system by using the reference coordinate system being rotated by three spatial axes, and the rotation matrix obtained by three rotations between the ground coordinate system (OX_eY_eZ_e) to the airframe coordinate system (OX_bY_bZ_b) b to derive the conversion matrix RBE between the airframe coordinate system to the ground coordinate system.

\(\begin{aligned} R_{B E} & =R_{E B}^{-1} \\ & =\left[\begin{array}{ccc}\cos \theta \cos \psi & \sin \theta \sin \phi \cos \psi-\cos \phi \sin \psi & \sin \theta \cos \phi \cos \psi+\sin \phi \sin \psi \\ \cos \theta \sin \psi & \sin \theta \sin \phi \sin \psi+\cos \phi \cos \psi & \sin \theta \cos \phi \sin \psi-\sin \phi \cos \psi \\ -\sin \theta & \sin \phi \cos \theta & \cos \phi \cos \theta\end{array}\right]\end{aligned}\)       (5)

where 𝜙 is the angle of rotation around the X-axis; θ is the angle of rotation around the Y-axis; ψ is the angle of rotation about Z-axis.

The four elements are defined as

Q = q₀ + q₁i + q₂j + q₃k      (6)

where q₀ represents the real number and it is also called the Rodriguez-Hamilton parameter; i, j, and k represent the imaginary part of the unit vector.

The vector in four dimensions can be mapped to three dimensions. The conversion matrix of the geographic coordinate system can be obtained by converting the UAV body coordinate system as following equation

\(\begin{aligned}R_{B E}=\left[\begin{array}{ccc}q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{1} q_{3}+q_{0} q_{2}\right) \\ 2\left(q_{1} q_{2}+q_{0} q_{3}\right) & q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2} & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) \\ 2\left(q_{1} q_{3}-q_{0} q_{2}\right) & 2\left(q_{2} q_{3}+q_{0} q_{1}\right) & q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2}\end{array}\right]\end{aligned}\)       (7)

The Eulerian angle to quaternion formula is obtained by combining equations (6) with (7) as

\(\begin{aligned}q=\left[\begin{array}{l}q_{0} \\ q_{1} \\ q_{2} \\ q_{3}\end{array}\right]=\left[\begin{array}{c}\cos \frac{\phi}{2} \cos \frac{\theta}{2} \cos \frac{\psi}{2}+\sin \frac{\phi}{2} \sin \frac{\theta}{2} \sin \frac{\psi}{2} \\ \sin \frac{\phi}{2} \cos \frac{\theta}{2} \cos \frac{\psi}{2}-\cos \frac{\phi}{2} \sin \frac{\theta}{2} \sin \frac{\psi}{2} \\ \sin \frac{\phi}{2} \cos \frac{\theta}{2} \cos \frac{\psi}{2}+\cos \frac{\phi}{2} \sin \frac{\theta}{2} \sin \frac{\psi}{2} \\ \cos \frac{\phi}{2} \cos \frac{\theta}{2} \sin \frac{\psi}{2}-\sin \frac{\phi}{2} \sin \frac{\theta}{2} \cos \frac{\psi}{2}\end{array}\right]\end{aligned}\)       (8)

3.3 Kalman filter algorithm

In the actual process, the simple PID control has these disadvantages of poor accuracy, long response time, and poor stability due to noise interference and parameter interference. Whereas the PID control system with Kalman filter using the optimal estimation algorithm for the system state has shorter response time and higher control accuracy [28]. The input and output of the system is used to monitor the linear system state equation of the data. The Kalman filter control system structure is illustrated as shown in Fig. 6.

Fig. 6. Structure diagram of Kalman filter control system

Assuming that the system model is linear in the standard condition, the observation model will be linear, but it is not the case in practice. The carrier attitude has been represented by quaternion method in the previous section and the state variables are chosen as attitude quaternion. The Kalman equation of state with the attitude quaternion as the state variable is nonlinear due to the presence of trigonometric functions in the state variable. Supposing the nonlinear system can be represented by equation.

The equations of state and measurement are given out as the following

X(k) = f[X(k - 1)] + W(k)       (9)

Z(k) = g[X(k)] + V(k)      (10)

To obtain the linearized equation of state, the nonlinear system is linearized as following equation

\(\begin{aligned} X(k) & =A^{\prime} X(k-1)+\{f[\hat{X}(k-1)]\}+W(k) \\ & \approx A^{\prime} X(k-1)+W(k)\end{aligned}\)       (11)

The linearized observation equation can be obtained as

\(\begin{aligned} Z(k) & =C^{\prime} X(k-1)+\left\{g\left[\hat{X}(k)-C^{\prime} \hat{X}(k-1)\right]\right\}+V(k) \\ & \approx C^{\prime} X(k-1)+V(k)\end{aligned}\)       (12)

4. System design and program development

4.1 System design

The system diagram is presented as shown in Fig. 7. The comprehensive design of the UAV revolves around the primary control MCU. Resources such as built-in timer and GPIO in multi-function mode are provided by the main control MCU. The overall development and experimentation of the UAV is completed by matching the radio receiver, attitude meter, barometer, ESC and other equipment.

Fig. 7. System diagram

The fixed-wing aircraft generates aerodynamic force through trapezoidal wings with large span-to-chord ratio. In the same way, through its slender rotor blades moving relative to the air at a certain speed and angle of attack, the quadrotor UAV generates aerodynamic force to allow the airframe to hover and generate upward lift.

The radio wave is adjusted and sent to the corresponding matching receiver. It is then received and conditioned by the receiver, and the analog level of its corresponding channel pins is controlled to change. The analog signal value of each channel of the receiver is captured and processed by the timer provided by the master control. The result is fed back to the master control. The received remote control data is analyzed and processed by the flight control program, and then controlled by the resource timer set in the master control chip to generate multiple sets of PWM pulse signals to each ESC. Different PWM pulses are converted into contrasting current values by the ESC, which are used to drive the motors located in each direction of the frame to generate various rotational speeds and the lift force to control the pose of UAV and fulfill the operator's expectations.

In addition, to assist the UAV in attaining balance, stability and relative positioning, measurement data is transmitted to the flight control program by the on-board sensors. External flash memory is connected via SPI bus to extend the program storage space. The status of the current program operation is indicated by signal lights. The analog digital converter (ADC), universal synchronous/asynchronous receiver/transmitter (USART) and transmission interface module (TIM) peripheral interfaces are used to communicate with external devices. Due to the hardware area and wiring, the quadrotor UAV uses the serial interface serial wire debug (SWD) provided by the core of the main control chip as the program download and debug debugging method.

4.2 Program design

The block diagram of program design is presented as shown in Fig. 8. The power-up is first started and then the initialization of each functional module is completed. The remote sensing data transmitted by the control input module is received by the UAV flight control program (i.e., the voltage value of each channel of the receiver is detected in the timer input capture mode) and compared with the detected attitude data (roll angle, pitch angle, yaw angle and relative air pressure altitude of the UAV). Then the data is processed by the sensor module to derive the difference (the compensation value needed to reach the desired value). Subsequently, it is controlled by the control output module to complete the correction and maintenance of the UAV attitude after the cascade PID processing.

Fig. 8. The block diagram of program design

To achieve specific target requirements, auxiliary devices are externally connected to the UAV's communication module. For example, an external ultrasonic detection module allows for accurate height measurement and height fixing operations of the UAV. An extraneous vision processing module, programmed by the upper logic code, allows the UAV to perform functions that include vision tracking. By external optical positioning or GPS module, it can realize indoor/outdoor positioning flight of UAV.

The download/debug module can complete the system development and debug operation of UAV. It is an essential module for developing and debugging UAV flight control program. The status indicator module makes it convenient for the operator to know the operation status of the UAV in real time. Thus, for ensuring the safety and maintenance of the UAV, it is one of the most essential modules in the development process.

5. Experiment

5.1 Test conditions and debugging

The hardware of the UAV mainly includes a brushless motor of Lang Yu X2212-9 specification, a rotor 40A of Haoying Lotte XRotor and a battery type of Dapu 2600mAh lithium battery. Since KV1400 brushless motor is used in this paper, 1045 specification propeller is selected according to debugging experience, and i6 remote control of Fuce is used as the remote control. The mainly hardware equipment are shown in Fig. 9. Through the above control system hardware and circuit connection, assemble and build the flight test platform. The initial setting is carried out firstly, including the calibration of the accelerometer, the connection and calibration of the remote control, and the calibration of the electrical adjustment. At the same time, in order to ensure that the four-rotor UAV control system can be more safe and reliable, it is also necessary to fine-tune the PID parameters.

Fig. 9. Electrical ESC and other accessories

The serial assistant is used to adjust the inner-loop parameter P. Selecting a suitable parameter P can feel the resistance when artificially disturbed, and the drone will oscillate a few times to tend to the equilibrium position. Then the parameter D is adjusted, which can suppress the UAV overshoot phenomenon when the UAV is artificially disturbed, it can quickly return to the equilibrium position. Eventually, parameter I is added on top of parameters P and D. The effect of parameter I can make it possible to correct the influence of gravity within a certain angle range. When hitting the rudder back to the center of the appropriate parameter I, the UAV will not sequentially rotate because of gravity or inertia. When the inner ring parameters are adjusted, the serial assistant is used to adjust the outer ring parameters P. After adding the outer ring parameter P, the drone will turn to the balance state. Finally, the inner and outer loop parameters of roll, pitch and yaw directions can be adjusted in the same way.

Through debugging, the 4-axis flight control can quickly and accurately respond to the commands of the remote control unit. The experiment debugging is given out as shown in Fig. 10. In the fixed height mode, it can position the corresponding height smoothly. It has good control effect and strong anti-interference ability. The automatic/manual altitude control mode can be manually switched, and the aircraft can perfectly follow the remote control commands to make corresponding actions and complete the expected design goals.

Fig. 10. Experiment of drone debugging

5.2 Flying test

The flight test was conducted to verify whether the quadrotor UAV control system met the requirements. The indoor flight test was mainly to verify the UAV could take off, fly and land stably and remain hovering at a certain height. The altitude of UAV is presented as shown in Fig. 11. From the Figure, it can be seen that the UAV took off to a height of 0.6m, then hovered for 3 seconds, then flew forward for 5m and hovered for 1 second, and finally returned to the landing point. In the Fig. 11, the quadrotor UAV can hover with the maximum height of 0.63m and the minimum height of 0.52m. The most significant value of variation is 0.11m, it can be seen that the quadrotor UAV can achieve a more consistent hover at a certain height.

Fig. 11. Flight test IMU data

6. Conclusion

The control system of UAV based on STM32 is designed in this paper. The STM32 is used as the main control circuit to design the quad-axis flight control circuit. The software function part of the UAV is designed in KEIL5 environment. The Kalman filter algorithm is used to complete the attitude control of the UAV. The altitude measurement data is captured according to the timer to complete the UAV altitude. Finally, the remote control device is designed to complete the UAV control system design. The experimental debugging results show that the quad rotor UAV can complete the action according to the remote control command with good control effect and anti-interference ability. It has potential application value. In future work, to improve the autonomous performance of UAV, VSLAM will be adopted to realize the mapping and path planning of UAV and complete autonomous navigation.