Navigation and Control for an Unmanned Aerial Vehicle

Abstract

Recent advancement in micro-electronic technology makes the development of quadrotor fully possible. This paper elaborates on the complete designing process of a quadrotor control platform. Firstly a hardware structure with a microcontroller and embedded sensors is reported. Then through the implementation of a linear complementary filter, the real-time attitude estimation of the quadrotor is realized. Furthermore, aiming at the outdoor environment navigation, a barometer and a Global Positioning System (GPS) are also integrated within this platform. A multisensory data fusion algorithm is then proposed to combine these sensors for the outdoor navigation. Finally, a group of PID controllers are designed to stabilize the attitude and position. The experiment results demonstrate the capability of this platform to navigate autonomously through user-defined waypoints.

1 Introduction

In recent decades, the application of unmanned aerial vehicle (UAV) has been widely reported in both military and civil fields. Application of UAV is usually considered in the dangerous environments. For instance, with additional sensors, a UAV is reported to support wildfire observations [1]. After the earthquake in Fukushima, a T-Hawk Micro Aerial Vehicle equipped with radiation sensors accomplishes the missions to help operators locate the nuclear fuel debris [2]. In this way, the use of UAV could promote the success rate of the missions and ensure the safety of the human lives.

Among all these UAVs, quadrotor stands out due to its flexibility and potential capabilities. The designing of the control algorithm as well as the real-time state estimation algorithm for the quadrotor is challenging due to the complex dynamic model and the unreliable measurement from the sensors on the UAV. During the past decade, many researchers involve themselves into the designing of the quadrotor controller. In their works, some mathematic models are built to understand the dynamics of the quadrotor [3, 4]. Several control algorithms, both linear and nonlinear, are also proposed to stabilize the quadrotor. Examples include a non-linear controller derived using back-stepping approaches [5], a PD controller presented by V. Kumar and performs well in their papers [6]. Apart from the controller designing, work has also been done to do the flight state estimation [7, 8]. However, Most of these studies are verified in a simulation environment or carried out in some commercial platforms which provide little experience in the practical development of the flight controller. In practical experiments, the accuracy of attitude and position estimation would be influenced by the noise of the embedded sensor. And the performance of these control algorithms depend on the accuracy of the real-time state estimation.

Meanwhile, success has been reported from many research groups or commercial companies in constructing their own quadrotors. From the commercial companies, the DJI Wookong, Parrot ARDrone, Asctec Humming bird have been introduced. Nevertheless, these controllers are based on the closed-source projects. There is little information we could get. As for the open-source project, such as the STARMAC [9], the pixhawk [10], X4-flyer [11] and so on, the published articles just report parts of their work.

For these reasons, a complete development process of the flight controller is presented in this paper. Firstly, this paper proposes a hardware platform which functions as the flight controller. Inertial sensor, barometer and GPS are integrated into the platform to estimate the real-time state of the quadrotor. Taking the noise of the sensors into consideration, a pose estimation algorithm based on a linear complementary filter and a group of PID controllers are implemented on this platform. To verify their validity and stability, flight tests are carried out.

The remainder of this paper is organized as follows. Section 2 develops the dynamic model of the quadrotor. Section 3 describes the hardware structure of this platform. Section 4 elaborates on the multisensory data fusion algorithm for attitude and position estimation, along with the corresponding attitude and position control algorithm. Outdoor flight test results are given in Sect. 5 and a conclusion is reached in Sect. 6.

2 Modeling

In order to develop the dynamic model of the quadrotor, the rotation matrix between two coordinate systems, the brushless motor dynamics and the rigid body dynamics are studied in this section.

2.1 Rotation Matrix

Firstly, coordinate systems including an inertial frame and a body fixed frame are defined. All of the following equations are derived in these two coordinate systems.

Fig. 1.

Inertial frame and body fixed frame

The body fixed frame {B} is defined in Fig. 1. The X axis points to the front of the quadrotor. The Y axis points to the left. Then the Z axis is defined by the right hand rule.

For the inertial frame {G}, the origin point O would be set at the takeoff point of the quadrotor. Then an east-north-up orthogonal frame is established by the right hand rule.

Three attitude angels are defined in Fig. 2. $\phi$ is the attitude angel of roll which represents the quadrotor rotates around the X axis. $\theta$ is the attitude angel of pitch which represents the quadrotor rotates around the Y axis. $\psi$ is the attitude angel of yaw which represents the quadrotor rotates around the Z axis.

Fig. 2.

Attitude definition

With these attitude angels and a rotation order of X-Y-Z, a rotation matrix {R} from the inertial frame to the body fixed frame is defined as

$$\begin{aligned} R \left( \phi , \theta ,\psi \right) =\left[ \begin{matrix} c\psi c\theta &{} c\psi s\theta s\phi -s\psi c\phi &{}c\psi s\theta c\phi +s\psi s\phi \\ s\psi c\theta &{}s\psi s\theta s\phi +c\psi c\phi &{}s\psi s\theta c\phi -s\phi c\psi \\ -s\theta &{}c\theta s\phi &{} c\theta c\phi \end{matrix}\right] \end{aligned}$$

(1)

where s stands for sin, c stands for cos.

2.2 Brushless Motor Dynamics

The sequence of the motors is defined in Fig. 1 with a clock-wise direction. The thrusts F and the momentums M of the motors are all defined in the Z direction of the body fixed frame. In the steady state, the thrust and the momentum from a single motor would be proportional to the square of this motor’s angular speed [12].

$$\begin{aligned} {\left\{ \begin{array}{ll} F_{i} = k\omega _{i}^{2}\\ M_{i} = b\omega _{i}^{2} \end{array}\right. } \end{aligned}$$

(2)

where i is from 1 to 4, the coefficient k and b would be decided by the shape of the blade, and $\omega$ is the motor’s angular speed.

2.3 Rigid Body Dynamics

Rigid body dynamics governs the motion of the quadrotor. In this subsection, the Newton-Euler equations for the quadrotor would be developed. Firstly, four control variables are defined as

$$\begin{aligned} {\left\{ \begin{array}{ll} U_{1} = F_{1}+F_{2}+F_{3}+F_{4}\\ U_{2} =\left( F_{4}+F_{3}-F_{2}-F_{1}\right) \frac{L}{\sqrt{2}} \\ U_{3} = \left( F_{1}-F_{2}-F_{3}+F_{4}\right) \frac{L}{\sqrt{2}}\\ U_{4} = M_{1}-M_{2}+M_{3}-M_{4}\\ \end{array}\right. } \end{aligned}$$

(3)

where $F_{1}$, $F_{2}$, $F_{3}$, $F_{4}$ are the thrusts, $M_{1}$, $M_{2}$, $M_{3}$, $M_{4}$ are the momentums, and L is the distance between the rotor and the center of the quadrotor.

In these control variables, $U_{1}$ represents the total thrusts. $U_{2}$ represents the torque on the Y axis. $U_{3}$ represents the torque on the X axis. $U_{4}$ represents the torque on the Z axis.

According to the Newton-Euler formalism, the rigid body dynamic equation is developed as

$$\begin{aligned} {\left\{ \begin{array}{ll} m \ddot{r}=R\left[ \begin{matrix}0\\ 0 \\ U_{1} \end{matrix}\right] -\left[ \begin{matrix}0\\ 0 \\ mg \end{matrix}\right] \\ I \ddot{q}=\left[ \begin{matrix}U_{2}\\ U_{3} \\ U_{4} \end{matrix}\right] -\dot{q}\times I \dot{q} \end{array}\right. } \end{aligned}$$

(4)

where m stands for the mass of the quadrotor, r stands for the position in the inertial frame, g stands for the gravity constant, q stands for the attitude in the body fixed frame, and I stands for the moment of inertia.

3 Hardware Design

This section describes the hardware structure of this platform including the Microcontroller Unit (MCU), the embedded sensors and the other related peripherals. The computation power comes from a 48 MHz 32 bit ARM RISC MCU with 64 KB SRAM and 256 KB Flash. This MCU is a low cost MCU without the Float Computation Unit (FPU) and does not support Digital Signal Processing Instructions. The poor computation performance of this MCU gives a high requirement for the simplicity of the algorithm.

The Inertial Measurement Unit (IMU) MPU-9150 consists of an integrated 3-axis Accelerometer, a 3-axis Gyroscope and a 3-axis Magnetometer. With these inertial sensors, the attitude estimation is achieved. Regarding of the position estimation, a barometer MS5611 for measuring the altitude and a GPS receiver U-Blox M8N for measuring the horizontal position are also integrated in the platform.

Fig. 3.

Hardware structure

Fig. 4.

Input and output data streams of the system

As for the other peripherals, the XBEE has been chosen as the wireless communication system between this platform and the ground station. The signal from the remote control (RC) is transformed into a PPM signal by the RC receiver then processed by the MCU. Besides, this platform has 4 PWM outputs to driver the corresponding DC motors. The status LEDs and an external buzzer are also used to indicate the real-time working status of this platform.

With all these sensors and peripherals, the input and the output data streams within this platform is summarized in Fig. 4.

4 State Estimating and Control

To control a quadrotor, the current states including the local position, linear velocity, attitude and angular velocity are required [13]. Among these states, the attitude and angular velocity are the primary variables used in the attitude control of the quadrotor. The local position and the linear velocity are used in the position control. To estimate these states, many different sensors are required including the accelerometer, gyroscope, magnetometer, barometer and the GPS. In some specific application, lidar, VICON system and the Kinect are also integrated into the estimating process.

4.1 Estimating Attitude

In this paper, a linear complementary filter is implemented to combine the sensors data from MPU9150 which includes the accelerometer, gyroscope and magnetometer. Generally, the gyroscope measures the angular velocity in the body fixed frame {B}

$$\begin{aligned} \dot{\theta }_{u}=\dot{\theta }+b+\eta \in \{B \} \end{aligned}$$

(5)

where $\dot{\theta }_{u}$ is the measured angular velocity of the gyroscope. $\dot{\theta }$ is the true value of the angular velocity. b is the slow time-varying gyroscope bias. $\eta$ is the additive measurement noise.

The major defect of the gyroscope is the slow time-varying bias which makes the method of integrating these measurements to estimate the attitude impossible. For these reasons, we need to combine the measurements of accelerometer which is far more stable in the long term but susceptible in a short time to compensate the bias of the gyroscope.

Fig. 5.

Complementary filter for attitude estimation

Let $\dot{\theta }_{u}$ be the measurement angular velocity of the gyroscope and $\dot{\hat{\theta }}$ the compensated angular velocity which is used for the integration. $\hat{\theta }$ denotes the attitude estimation and $\theta _{a}$ is the attitude measured by the accelerometer. $\hat{b}$ is the time-varying bias estimation of the gyroscope (Fig. 5).

$$\begin{aligned} {\left\{ \begin{array}{ll} \dot{\hat{\theta }}=\dot{\theta }_{u}-\hat{b}+K_{p}(\theta _{a}-\hat{\theta }) \\ \dot{\hat{b}}=K_{i}(\theta _{a}-\hat{\theta }) \end{array}\right. } \end{aligned}$$

(6)

4.2 Estimating Position

To estimate the local position of the quadrotor, the primary sensor would be the accelerometer. The mathematic model of the accelerometer is defined as

$$\begin{aligned} a_{u}=R^T(a+ g\overrightarrow{\mathbf {z}})+b_{a}+\eta _{a} \in \{B \} \end{aligned}$$

(7)

where $a_{u}$ is the acceleration measurement. R is the rotation matrix defined in Sect. 2. a is the true motion acceleration in the inertial frame. $g\overrightarrow{{\varvec{z}}}$ is the gravity. $b_{a}$ is the bias of the accelerometer. $\eta _{a}$ is the additive measurement noise (Fig. 6).

Fig. 6.

Position estimation

However, due to the steady error and the stochastic noise listed in the formula, there is no effective way to use the accelerometer solely. With careful calibration, these defects could be relieved but not completely eliminated [14]. For these reasons, in this paper the platform would include a barometer to get the altitude information and a GPS to provide the position information in the horizontal plane. And a PI controller is designed to fuse the data from the accelerometer, barometer and the GPS. The key point of the PI controller is to correct the steady error of the accelerometer with the measurement from barometer and the GPS.

The position estimation algorithm is defined as

$$\begin{aligned} {\left\{ \begin{array}{ll} \hat{a}=a_{u}-\hat{b}_{a}\\ \dot{\hat{b}}_{a}=K_{i}(r-\hat{r})\\ \hat{r}=\hat{r}_{u}-K_{p}(r-\hat{r}) \end{array}\right. } \end{aligned}$$

(8)

where $a_{u}$ is the acceleration measurement. $\hat{a}$ is the modified acceleration. $\hat{r}$ is the position estimation. r is the position measurement.

4.3 Control

In this section, the controllers will be designed based on the dynamic model in Sect. 2 and the states estimation algorithm in Subsects. 4.1 and 4.2. The block diagram of this controller is shown in Fig. 7, which includes a position loop(P), a velocity loop(V), an attitude loop(A)and a rate loop(R).

Fig. 7.

Structure of control system

The PID controller has been proved to be the simplest but most efficient methods for the control of the quadrotor [15, 16]. In this paper, PID controllers are also implemented in these four loops defined as

$$\begin{aligned} {\left\{ \begin{array}{ll} P=K_{pp}\\ V=K_{vp}(1+\frac{1}{T_{vi}s}+T_{vd}s)\\ A=K_{ap}\\ R=K_{rp}(1+\frac{1}{T_{ri}s}+T_{rd}s) \end{array}\right. } \end{aligned}$$

(9)

where P controller is adopted in the position loop and the attitude loop. PID controller is implemented in the velocity loop and the rate loop.

5 Flight Test

In order to verify the effectiveness of this platform including the hardware and the algorithm, flight test has been carried out in the outdoor environment. Position controller and Velocity controller are running at 250 Hz. Meanwhile, Attitude controller and Rate controller are running at 500 Hz. Thanks to the wireless communication system XBEE which is shown in the Fig. 3, all of the states in the quadrotor during the experiments are collected and sent back to the ground station. The altitude control experiment and horizontal position control experiment are carried out separately.

5.1 Altitude Control

For the altitude control, the quadrotor is ordered to hover at a specific height. The test result is shown in Fig. 8. $Z_{p}$ is the current altitude estimation and $Z_{cmd}$ is the corresponding target altitude.

Fig. 8.

Results altitude control

The experiment result shows that the quadrotor well responds to the command and the steady error is within 0.3 m. Comparing with the reported result [16], the performance is good enough.

Fig. 9.

Results position control

5.2 Position Control

For the horizontal position control, the experiment is carried out at a specific height of 1.7 m and the response of the quadrotor has been tested with the hovering and ramp command in the horizontal plane. The test result is shown in Fig. 9.

In the X axis of the inertial frame, the quadrotor firstly hovers for 150 s, then it follows several ramp commands. In the Y axis of the inertial axis, the quadrotor firstly hovers for 100 s, then it also follows several ramp commands. The experiment result shows that the quadrotor well responds to the command. The steady error is within 1 m and the delays are within 2 s. Comparing with the reported result [17], the performance is good enough.

6 Conclusions

This paper elaborates on the development of a quadrotor platform which is able to navigate through the user-defined waypoints in the outdoor environment. The hardware structure is reported in Sect. 3 with the necessary sensors. In Sect. 4, a simple linear complementary filter and a PI-based multisensory data fusion algorithm are proposed to estimate the attitude and the local position of the quadrotor. In the outdoor flight experiment, this platform has demonstrated good performance in the attitude stability and position control ability through the PID controllers which are discussed in Sect. 4. The test result shows the steady error is within 0.3 m in the altitude direction. As for the horizontal direction, the steady error is within 1 m and the delay is within 2 s. Future work would include some additional functions such as the path-planning and collision avoidance. To realize these functions, some external sensors such as the laser rangefinder and sonar would be added to this platform. These sensors would improve the navigation ability of this platform in the exploration of the unknown environment.