Motion Control System for Quadcopter Based on Cascade Kalman Filters

Abstract

To solve the problem of estimating the orientation and position of an aircraft, we propose a system based on a series of Kalman filters: a linear Kalman filter in the attitude stabilization loop and an extended Kalman filter in the subsystem of maintaining a given location. The efficiency of the developed control system is confirmed by experimental results on the indoors flight control of a quadcopter.

1 INTRODUCTION

When creating motion control systems for aircraft, manipulators, autonomous mobile robots, or human positioning systems, it becomes necessary to solve the problem of estimating the state vector of an object containing spatial coordinates and movement velocities. The systems for determining attitude, position and velocity, described in works [1–7], include inertial measurement modules (IMMs) and algorithms for data processing and integration.

Currently, IMMs that consist of an assembly of micromechanical sensors—accelerometers, gyrosensors and magnetometers, are ubiquitous. Navigation systems with IMMs are complemented by other types of sensors—satellite signal receivers, optical and ultrasonic sensors.

In a number of cases algorithms for estimating attitude are based on simple state observers—complementary filters. An algorithm based on a nonlinear complementary filter using data from inertial sensors and a magnetometer was proposed in [1]. The work [2] considers an adaptive complementary filter based on quaternions. In [3], an algorithm based on the gradient descent method is described; and in [4] an alternative extended complementary filter is developed that does not require minimization procedures.

Estimation of aircraft location requires integration of data on the attitude and position of the object. Methods of optimal linear filtering are used to solve this problem. The linear Kalman filter is suggested for estimating the coordinates of a nonlinear double pendulum system [5]. The gradient descent method for estimating the measured attitude quaternion and the linear Kalman filter are used to estimate the attitude of the quadcopter in [6]. In [7], an algorithm was proposed for integrating data from an IMM and an optical flow sensor using an extended Kalman filter in the control system of a multirotor aircraft.

A kinematic or simplified dynamic (with constant accelerations) model of the vehicle motion is usually used for constructing a coordinate estimation system based on the Kalman filter. Often, model inputs corresponding to real control actions applied to the object are not considered.

The purpose of this work is to create a system for estimating the attitude and position of an aircraft based on series-connected or cascade Kalman filters [8]. It is proposed to use a linear Kalman filter for a fast attitude stabilization loop and an extended Kalman filter for the subsystem of maintaining a given position. The efficiency of the developed control system is confirmed by experimental results on the indoor flight control of a quadcopter.

2 PROBLEM SETUP

The spatial position of the quadcopter is described by the coordinates of the center of mass of the device $x$, $y$, $z$ in the inertial frame of reference and by the angles of rotation around the axes $x_{b}$, $y_{b}$, $z_{b}$ of the frame of reference rigidly bound to the device with the origin at the center of gravity of the quadcopter. We accept the following designations: $\phi$ is the angle of rotation around $x_{b}$ axis (roll), $\theta$ is the angle of rotation around axis $y_{b}$ (pitch), and $\psi$ is the angle of rotation around axes $z_{b}$ (yaw). A simplified mathematical model that characterizes the movement of a quadcopter in these coordinates is given by the equations [9]

$$\begin{cases}m\ddot{x}=(\sin{\psi}\cdot\sin{\phi}+\cos{\psi}\cdot\cos{\phi}\cdot\sin{\theta})u_{1};\\ m\ddot{y}=(-\cos{\psi}\cdot\sin{\phi}+\sin{\psi}\cdot\cos{\phi}\cdot\sin{\theta})u_{1};\\ m\ddot{z}=\cos{\phi}\cdot\cos{\theta}\dot{u}_{1}-mg,\end{cases}$$

(1)

$$\begin{cases}I_{xx}\ddot{\phi}=u_{2}-(I_{zz}-I_{yy})\dot{\theta}\dot{\psi};\\ I_{yy}\ddot{\theta}=u_{3}-(I_{xx}-I_{zz})\dot{\phi}\dot{\psi};\\ I_{zz}\ddot{\psi}=u_{4},\end{cases}$$

(2)

where $m$ is the quadcopter mass; $g$ is the acceleration of gravity; $I_{xx}$, $I_{yy}$, $I_{zz}$ are moments of inertia about the principal axes of the quadcopter; $u_{1}$ is the thrust force along the $z_{b}$ axis; and $u_{2},u_{3},u_{4}$ are torque moments. Dots over variables denote time derivatives.

Equations (1) and (2) can be represented in a compact form:

$$\dot{q}=f(q(t),u(t)),$$

(3)

where

$$q=[x,y,z,\dot{x},\dot{y},\dot{z},\phi,\theta,\psi,\dot{\phi},\dot{\theta},\dot{\psi}]^{\top}$$

(4)

is the system state vector; $f(q,u)$ is a vector function defined by the object model; and $u$ is the vector of control actions.

Figure 1 shows forces and moments acting on the quadcopter. The aircraft frame has a diagonal length of $2l_{0}$ m and is rotated at an angle $\gamma=\pi/4$ rad about the $z_{b}$ axis of the body-fixed reference system. Each rotor generates aerodynamic thrust $F_{i}$ directed upwards and moment $M_{i}$ directed opposite to the rotation of the propeller $i$. The total thrust is given by the ratio

Fig. 1

Coordinate systems, forces and moments acting on the quadcopter.

$$u_{1}=F_{1}+F_{2}+F_{3}+F_{4}.$$

(5)

In the quadcopter coordinate system, the roll and pitch torques are calculated using the right hand rule. Moments of force over $x_{b}$ and $y_{b}$ axes are, respectively,

$$u_{2}=l(F_{1}+F_{2}-F_{3}-F_{4}),$$

(6)

$$u_{3}=l(-F_{1}+F_{2}+F_{3}-F_{4}),$$

(7)

where $l=l_{0}\sin{\gamma}$.

For the $z_{b}$ axis, the thrust of the motors does not create a moment of force. However, the rotation of the propellers generates an aerodynamic moment directed against the rotation of the blades of each of the four propellers:

$$u_{4}=M_{1}-M_{2}+M_{3}-M_{4}.$$

The transition from the forces created by each rotor to the thrust and moments of the entire system is written in matrix form:

$$\left(\begin{matrix}u_{1}\\ u_{2}\\ u_{3}\\ u_{4}\end{matrix}\right)=\left(\begin{matrix}1&1&1&1\\ l&l&-l&-l\\ l&-l&-l&l\\ \lambda&-\lambda&\lambda&-\lambda\end{matrix}\right)\left(\begin{matrix}F_{1}\\ F_{2}\\ F_{3}\\ F_{4}\end{matrix}\right)=M\left(\begin{matrix}F_{1}\\ F_{2}\\ F_{3}\\ F_{4}\end{matrix}\right),$$

where $\lambda$ is the coefficient of proportionality between aerodynamic moments along the $z_{b}$ axis and propeller thrusts. Thus, the thrust forces that the control action creates on each engine are calculated as

$$\left(\begin{matrix}F_{1}\\ F_{2}\\ F_{3}\\ F_{4}\end{matrix}\right)=M^{-1}\left(\begin{matrix}u_{1}\\ u_{2}\\ u_{3}\\ u_{4}\end{matrix}\right).$$

We consider the task of control to be the trajectory control of the movement of the aircraft, namely, bringing the center of mass of the craft to the target position with coordinates $x_{\textrm{ref}}$, $y_{\textrm{ref}}$, $z_{\textrm{ref}}$ and following it. Papers [10, 11] present a method for synthesizing control actions $u_{1}-u_{4}$, which ensure that the mismatch between the current and target coordinates is reduced to zero.

The implementation of control algorithms requires estimates of the coordinates of the center of mass of the aircraft and its attitude angles at each moment of time, as well as the first derivatives of these variables.

This work aims to create a system for estimating the above state variables of the aircraft using a mathematical motion model and motion control algorithms.

3 ALGORITHMS AND ARCHITECTURE OF THE ATTITUDEAND POSITION ESTIMATION SYSTEM

The motion model (1), (2) prescribes a natural division of the problem of estimating the coordinates coordinates into two stages.

First of all, it is necessary to estimate the attitude angles of the aircraft from the noisy IMM data.

Values of torques that implement monotonous and stable processes of establishing the required values of angles $\phi_{\textrm{ref}}$, $\theta_{\textrm{ref}}$, $\psi_{\textrm{ref}}$, are presented in [11]:

$$\begin{cases}u_{2}=I_{xx}[-(\alpha_{\phi}+k_{\phi})\dot{\phi}-\alpha_{\phi}k_{\phi}(\phi-\phi_{\textrm{ref}})];\\ u_{3}=I_{yy}[-(\alpha_{\theta}+k_{\theta})\dot{\theta}-\alpha_{\phi}k_{\phi}(\phi-\phi_{\textrm{ref}})];\\ u_{4}=I_{zz}[-(\alpha_{\psi}+k_{\psi})\dot{\psi}-\alpha_{\psi}k_{\psi}(\psi-\psi_{\textrm{ref}})],\end{cases}$$

(8)

where $\alpha$ and $k$ and constant parameters that determine the type and duration of the transition process according to the controlled values.

At the next stage, it is necessary to estimate the coordinates of the aircraft location and the linear speeds of movement according to the data of the external coordinate system and the attitude angles of the aircraft. The values of angles $\phi_{\textrm{ref}}$, $\theta_{\textrm{ref}}$, required for the quadcopter to steadily reach the target position $x_{\textrm{ref}}$, $y_{\textrm{ref}}$, $z_{\textrm{ref}}$, have the following form:

$$\begin{cases}\phi_{\textrm{ref}}=\arctan{(H_{xx}/H_{zz})};\\ \theta_{\textrm{ref}}=\arctan{(-H_{yy}/\sqrt{H^{2}_{xx}+H^{2}_{zz}})},\end{cases}$$

(9)

where

$$\begin{cases}H_{xx}=-(\alpha_{x}+k_{x})\dot{x}-\alpha_{x}k_{x}(x-x_{\textrm{ref}});\\ H_{yy}=-(\alpha_{y}+k_{y})\dot{y}-\alpha_{y}k_{y}(y-y_{\textrm{ref}});\\ H_{zz}=-(\alpha_{z}+k_{z})\dot{z}-\alpha_{z}k_{z}(z-z_{\textrm{ref}})+g.\end{cases}$$

(10)

To control a quadcopter, it is enough to control any two angles, so we assume $\psi_{\textrm{ref}}=0$. The thrust force is given as

$$u_{1}=m\sqrt{H^{2}_{xx}+H^{2}_{yy}+H^{2}_{zz}}.$$

(11)

3.1 Attitude Estimation

The main source of noise in the received readings of linear accelerations and angular velocities from the IMMs are vibrations caused by the operation of propelling motors. To eliminate the noise component, low-pass filters are used, for example, Butterworth filters or the simplest filters with a first-order aperiodic link transfer function:

$$\frac{y^{\textrm{f}}}{y}=\frac{1}{\tau p+1},$$

(12)

where $\tau$ is a parameter that defines the filter cutoff frequency, $y$ and $y^{\textrm{f}}$ are the original signal and the filter output signal, and $p$ is the differentiation operator.

Fig. 2

Quadcopter control system.

To take into account the phase shift and amplitude distortions introduced by the filter, we supplement the model (2) with signals of angular velocities along the $x,y,z$ axes and attitude angles (calculated using linear accelerations) at the input and output of the filter (12).

For example, for the $x_{b}$ axis we have

$$\begin{cases}\dot{w}_{x}=u_{2}/I_{xx};\\ \dot{w}^{\textrm{f}}_{x}+k_{\textrm{f}}w^{\textrm{f}}_{x}=k_{\textrm{f}}w_{x};\\ \dot{\phi}=w_{x};\\ \dot{\phi}^{\textrm{f}}+k_{\textrm{f}}\phi^{\textrm{f}}=k_{\textrm{f}}\phi,\end{cases}$$

(13)

where $k_{\textrm{f}}=1/\tau$ is the filter coefficient.

Equations (13) do not take into account the cross-link component between the axes $y$ and $z$, which occurs when the aircraft rotates along the yaw angle. To simplify the obtained relations, we assume that the control is carried out by an independent change in the roll and pitch angles.

For the angular velocities $w_{y}$, $w_{z}$ and angles $\theta$, $\psi$, Eqs. (13) are the same. Control actions $u_{2}-u_{4}$ are defined by relations (8).

In addition to noise, transport lags inevitably arise in the system due to the finite speed of the data transmission channels, different operating frequencies of the modules, and sampling of the measured continuous quantities. The total lag time in a discrete system is related to the sampling period $\Delta t$ by the relation

$$T=N\,\Delta t.$$

(14)

One of the ways to compensate for the transport lag $T$ is to use an analytical extrapolator, which makes it possible to estimate the state vector $q$ for the current time $t$, using the mathematical model of the control object [12]. The estimation is written as

$$\hat{q}^{k}=q^{k}_{d}+\sum_{i=k}^{k+N}f(q^{i}_{m},u^{i-N})\,\Delta{t},$$

(15)

where $\hat{q}^{k}$ is the estimation of the state vector of the system at a discrete moment of time $k\Delta t$; $q^{k}_{d}$ is the measurement of the state vector at the current time; and $q^{i}_{m}$ is the state of model (4), initialized by the value $q^{k}_{m}=q^{k}_{d}$. Using the extrapolator requires storing $N$ of the latest control values $u^{i}$ received by the object.

Lag compensation using an extrapolator leads to an increase in noise due to the fact that the integral over $N$ samples is added to the original vector, which leads to error accumulation. To eliminate deviations and outliers relative to the state vector, it is suggested to apply a linear Kalman filter.

For the $x$ axis the estimated state vector has the form

$$\hat{q}^{k}_{x}=[w^{k}_{x},w^{\textrm{f}k}_{x},\phi^{k},\phi^{\textrm{f}k}]^{\top}.$$

(16)

Kalman filtering includes prediction steps

$$\hat{q}^{k+1}_{x}=F^{k}\hat{q}^{k}_{x}+B^{k}u^{k}$$

(17)

and correction steps:

$$P^{k+1}=F^{k}P^{k}(F^{k})^{T}+Q^{k},$$

(18)

$$K^{k}=P^{k}/(P^{k}+R^{k}),$$

(19)

$$\hat{q}^{k+1}_{x}=\hat{q}^{k}_{x}+K^{k}[z^{k}_{x}-H\hat{q}^{k}_{x}],$$

(20)

where $F^{k}$ is the model motion matrix (13); $B^{k}$ is the control function matrix; $H^{k}$ is the measurement model matrix; $P^{k}$, $R^{k}$, and $Q^{k}$ are covariance state matrices, measurement noise, and model error, respectively.

Fig. 3

Block diagram of the software of the control system in the ROS environment.

Fig. 4

Changing the pitch angle of the aircraft in stabilization mode: (a) estimate $\theta$ and required value of the pitch angle $\theta_{\textrm{ref}}$; (b) control moment $u_{3}$; (c) estimate of the angular velocity $w_{y}$; (d) current height $z$ (dash-dotted lines mark $\theta_{\textrm{ref}}$, $x_{\textrm{ref}}$).

Fig. 5

Changing the aircraft coordinates in location maintenance mode: (a) coordinates $x$ and $x_{\textrm{ref}}$; (b) coordinates $y$ and $y_{\textrm{ref}}$; (c) coordinates $z$ and $z_{\textrm{ref}}$; (d) thrust force $u_{1}$ (dash-dotted lines mark $x_{\textrm{ref}}$, $y_{\textrm{ref}}$, $z_{\textrm{ref}}$).

3.2 Coordinate Estimation

In addition to attitude angles and angular velocities, the input data for the control algorithm (8)–(11) are noisy realizations of the coordinates of the center of mass of the aircraft, obtained from an external positioning system. Additionally, the estimates of the first derivatives of these variables are required. Relations (1) and (2) can be used to evaluate object state variables in the extended Kalman filter.

Similar to the lower layer, the transport time lags associated with the transmission of signals are eliminated by the analytical extrapolator (15).

Since the model (3) is nonlinear, in order to smooth the extrapolated state vector, it is proposed to apply the extended Kalman filter, in which the complete mathematical model of the aircraft is linearized near the operating point $(\hat{q}^{k}_{\textrm{ekf}},u^{k})$ using the Taylor series expansion:

$$\hat{q}^{k+1}_{\textrm{ekf}}\approx f(\hat{q}^{k}_{\textrm{ekf}},u^{k})+F^{k}[\hat{q}^{k}-\hat{q}^{k}_{\textrm{ekf}}]+\omega(k),$$

(21)

$$F^{k}=\frac{\partial f(q^{k},u^{k})}{\partial q^{k}}\Big{|}_{q^{k}=\hat{q}^{k}_{\textrm{ekf}}},$$

(22)

where $q$ is the total state vector (4) and $\omega(k)$ is a random process describing modeling errors. The prediction step of the extended Kalman filter differs from the linear case (17) and is written as

$$\hat{q}^{k+1}_{\textrm{ekf}}=f(\hat{q}^{k}_{\textrm{ekf}}u^{k}).$$

(23)

The correction stage is similar to the linear Kalman filter and is calculated using formulas (18)–(20).

The input of the extended Kalman filter receives estimates of angular velocities and attitude angles obtained in the linear Kalman filter at the attitude estimation stage. The resulting measurement vector has the form

$$z^{k}=[x^{k},y^{k},z^{k},\phi^{k},\theta^{k},\psi^{k},w^{k}_{x},w^{k}_{y},w^{k}_{z}]^{\top}.$$

(24)

The control calculated at the first stage is fed to the extended Kalman filter and to the software module of the motors, where, taking into account the transition matrix and the power characteristics of the motors, a pulse-width modulated signal is generated that determines the speed of rotation of the rotors connected to the electronic speed controllers (Fig. 2).

4 DESCRIPTION OF SOFTWARE AND HARDWARE OF THE CONTROL SYSTEM

Calculation of control actions and current coordinates of the aircraft, integration and filtering of sensor data, and generation of control signals to motors are performed on a Raspberry Pi 3B single-board microcomputer with a 4-core Cortex-A53 (ARM v8) 1.2 GHz processor and 1 GB RAM. The operating system is Ubuntu 20.04 LTS.

The software of the created control system has a modular architecture and is built on the ROS (Robot Operating System) interprocess communication mechanism [13].

In this work, ROS software packages are used, designed to solve the problems of obtaining IMM data, recognizing AR markers, and saving data (Fig. 3). The developed modules for controlling and estimating the state in the linear and extended Kalman filters are written in C++/Python using asynchronous processing based on ROS Timer and integrated into the ROS system. Takeoff and landing of the aircraft are carried out in manual mode according to the operator’s commands, which are processed in the console module. The absolute coordinates of the center of mass of the aircraft during indoor flights are determined using an external video camera and a localization system based on visual AR markers camera, ar_track_alvar and vision_pose modules.

The developed control system has a two-level structure.

The estimation of the angular position and the establishment of the given attitude of the aircraft are performed in the attitude_controller module using the data of the integrated inertial module InvenSense MPU-9250.

Estimation of the position of the aircraft and the establishment of the specified coordinates are performed in the position_controller module using data from the lower-level module and data from an external video positioning system.

In the motor software module, a linear dependence of the width of the pulse-width modulation signal on the thrust force of force $F_{i}$ $i$th motor is implemented. This dependence has been estimated experimentally using a tensometric balance and, in the working area of the thrust force, it is represented with sufficient accuracy by a linear dependence.

5 EXPERIMENTAL RESULTS

To evaluate the effectiveness of the proposed control system a quadcopter prototype was assembled based on an X-shaped frame with a diagonal of 250 mm in the laboratory environment. The aircraft has the following mass-inertial characteristics: $m=0.4$ kg, $J_{x}=7.5\times 10^{-3}$ kg m${}^{2}$, $J_{y}=7.5\times 10^{-3}$ kg m${}^{2}$, $J_{z}=14.2\times 10^{-3}$ kg m${}^{2}$.

The control signals were supplied to the motors by the software module motor_driver at a frequency of 400 Hz. The frequency of the attitude_controller and position_controller modules is set to 200 and 100 Hz, respectively. Data on the attitude of the aircraft, calculated by its navigation system, as well as the coordinates of its center of mass from an external video system, were received at a frequency of 270 and 20 Hz, respectively.

Test bench checks included a series of flights in the stabilization mode or maintaining the given angles and flights in the mode of maintaining the given coordinates $x_{\textrm{ref}}$, $y_{\textrm{ref}}$. Maintenance of the altitude $z_{\textrm{ref}}$ was performed in all modes.

The quality of transition processes was evaluated with a stepwise change in the controlled variable.

The following parameters of the control algorithm were set: $k_{\theta}=\alpha_{\theta}=16$; $k_{\psi}=\alpha_{\psi}=16$; $k_{\phi}=\alpha_{\phi}=16$ and $k_{x}=\alpha_{x}=4$; $k_{y}=\alpha_{y}=4$; $k_{z}=\alpha_{z}=4$.

In the stabilization mode over angles $\theta$ and $\psi$ (the specified heading angle $\phi_{\textrm{ref}}=0$ in all cases) the transition characteristics obtained for these parameters are monotonous without overshoot (Fig. 4). Settling time for specified pitch and roll angles $\theta_{\textrm{ref}}$, $\psi_{\textrm{ref}}$ after switching at times $t=60$ s and $t=120$ s does not exceed 0.2 s.

Figure 5 presents the results of maintaining the specified coordinates $x_{\textrm{ref}}$, $y_{\textrm{ref}}$. At time $t=60$ s the coordinate $x_{\textrm{ref}}$ is switched. The time of the transition process along the coordinate $x$ is determined by the distance to the target location and amounts to a few seconds. The standard deviation of the coordinate from the given value was 0.5 m.

6 CONCLUSIONS

This paper suggests the architecture of the system for estimating the attitude and location of a multirotor vehicle in the problem of controlling trajectory motion. A software and hardware complex for an automatic flight control system has been created. The efficiency of the suggested control system has been experimentally confirmed. The achieved positioning accuracy of the quadcopter relative to the desired trajectory when flying indoors was 0.05 m.