1 INTRODUCTION

In contrast to satellite systems, unmanned aerial vehicles (UAVs) make it possible to obtain images in cloudy conditions and provide higher spatial resolution. Today, it can be said that UAVs are successfully used in aerial photography and digital map generation. Multirotor vehicles are widely used in solving these problems due to controllability, reliability, and simplicity of construction. Promising applications of multirotor aircraft are cargo delivery, aero-geophysical works, and precision agriculture [1–4].

The application of UAVs is accompanied by formulating the problems of constructing the specified routes and the formation of control actions for traction motors taking into account the shape and size of the surveyed areas, terrain, external disturbances, and the dynamics of the used UAV.

When the robot moves in an unknown or complex environment, the motion trajectory is continuously planned and constructed [5, 6]. In this case, it is convenient to specify the trajectory by control points and further represent the trajectory in a parametric form [7]. If it is necessary to meet the requirements for the boundedness of the time derivatives of the target position coordinates or for the smoothness of the generated control actions, different methods of interpolation of the desired trajectory are used, e.g., the mathematical apparatus of the spline theory [8].

In this article, an algorithm for controlling the flight path of a multirotor UAV based on the developed method of structural synthesis is proposed. The target position is determined from the procedure of planning smooth motion trajectories defined by the sequence of turning points of the route using Cornu curves.

A prototype of the autonomous flight control system is created. The conducted experiments on the indoor quadcopter flight control showed that the proposed control system is operational.

2 FORMULATION OF THE PROBLEM

The position of a multirotor aircraft in space is characterized by the coordinates \(x\), \(y\), \(z\) of the center of mass of the vehicle in a fixed Cartesian system and Euler’s angles \(\psi\), \(\varphi\), \(\theta\), which determine the orientation of the coordinate system connected with the aircraft’s axes in relation to the fixed system \(xyz\) [9].

Simplified dynamics equations describing motion in the specified coordinates are as follows:

$$\begin{cases}m\ddot{x}=(\sin{\psi}\cdot\sin{\varphi}+\cos{\psi}\cdot\cos{\varphi}\cdot\sin{\theta})u_{1};\\ m\ddot{y}=(-\cos{\psi}\cdot\sin{\varphi}+\sin{\psi}\cdot\cos{\varphi}\cdot\sin{\theta})u_{1};\\ m\ddot{z}=\cos{\varphi}\cdot\cos{\theta}\cdot u_{1}-mg.\end{cases}$$
(1)

Here, \(m\) is the aircraft mass, \(g\) is the gravitational acceleration, and \(u_{1}\) is the total traction force. Here and further, the dots above the variable signs indicate time derivatives.

Due to the fact that the built-in flight controller control system is used to adjust the required attitude of the aircraft, let us assume that the processes of changing the attitude angles are represented as independent second order differential equations [10]:

$$\begin{cases}T_{\psi}^{2}\ddot{\psi}+2d_{\psi}T_{\psi}\dot{\psi}+\psi=\psi_{\textrm{ref}};\\ T_{\varphi}^{2}\ddot{\varphi}+2d_{\varphi}T_{\varphi}\dot{\varphi}+\varphi=\varphi_{\textrm{ref}};\\ T_{\theta}^{2}\ddot{\theta}+2d_{\theta}T_{\theta}\dot{\theta}+\theta=\theta_{\textrm{ref}}.\end{cases}$$
(2)

The values of \(T\) and d coefficients were estimated experimentally.

The kinematic diagram determines the recalculation of the thrust force of the motors in the control torques on the corresponding axes of rotation. We assume that this recalculation is by default in the flight controller, and the control actions are the required thrust \(u_{1}\) and the orientation given by the angles \(\psi_{\textrm{ref}}\), \(\varphi_{\textrm{ref}}\), and \(\theta_{\textrm{ref}}\).

We consider the control problem to be the trajectory control of the vehicle motion, namely, the placement of the center of mass of the vehicle into the target position with coordinates \(x_{\textrm{ref}}\), \(y_{\textrm{ref}}\), and \(z_{\textrm{ref}}\) and further following it. It is necessary to find such control actions \(u_{1}\), \(\psi_{\textrm{ref}}\), \(\varphi_{\textrm{ref}}\), and \(\theta_{\textrm{ref}}\) which will ensure that the mismatch between the current and the target coordinates is reduced to zero. It is assumed that the coordinates of the center of mass and its orientation in space at every moment of time as well as the first derivatives of these values are known. It is also assumed that traction motors have sufficient power.

3 CONTROL ALGORITHM SYNTHESIS

The methods of interpolation of the required trajectory, such as splines [8] or the Cornu spiral theory [11], make it possible to determine the time derivatives of the target position coordinates on the constructed route. With this in mind, let us write down the required differential equations for the deviation of the center of mass of the vehicle from the target position \(x_{\textrm{ref}}\), \(y_{\textrm{ref}}\), \(z_{\textrm{ref}}\):

$$\begin{cases}S_{x}=\dot{x}-\dot{x}_{\textrm{ref}}+k_{x}(x-x_{\textrm{ref}});\\ S_{y}=\dot{y}-\dot{y}_{\textrm{ref}}+k_{y}(y-y_{\textrm{ref}});\\ S_{z}=\dot{z}-\dot{z}_{\textrm{ref}}+k_{z}(z-z_{\textrm{ref}}).\end{cases}$$
(3)

Let us define the constrained motion to the desired trajectory (\(S_{x}=0\), \(S_{y}=0\), \(S_{z}=0\)) by the equations

$$\dot{S}_{x}=-\alpha_{x}S_{x},\quad\dot{S}_{y}=-\alpha_{y}S_{y},\quad\dot{S}_{z}=-\alpha_{z}S_{z}.$$
(4)

Here, \(k_{x}\), \(k_{y}\), \(k_{z}\) and \(\alpha_{x}\), \(\alpha_{y}\), \(\alpha_{z}\) are the positive constant coefficients that determine the time to reach the target position. The system synthesized based on Eqs. (3) and (4) is invariant with respect to the formulation of \(x_{\textrm{ref}}\), \(y_{\textrm{ref}}\), \(z_{\textrm{ref}}\) and has astaticism.

By substituting (3) in (4), we obtain

$$\begin{cases}\ddot{x}-\ddot{x}_{\textrm{ref}}+k_{x}(\dot{x}-\dot{x}_{\textrm{ref}})=-\alpha_{x}(\dot{x}-\dot{x}_{\textrm{ref}})-\alpha_{x}k_{x}(x-x_{\textrm{ref}});\\ \ddot{y}-\ddot{y}_{\textrm{ref}}+k_{y}(\dot{y}-\dot{y}_{\textrm{ref}})=-\alpha_{y}(\dot{y}-\dot{y}_{\textrm{ref}})-\alpha_{y}k_{y}(y-y_{\textrm{ref}});\\ \ddot{z}-\ddot{z}_{\textrm{ref}}+k_{z}(\dot{z}-\dot{z}_{\textrm{ref}})=-\alpha_{z}(\dot{z}-\dot{z}_{\textrm{ref}})-\alpha_{z}k_{z}(z-z_{\textrm{ref}}).\end{cases}$$
(5)

Taking into account (1) and (2), let us exclude the highest derivatives of the controlled values in (5)

$$\begin{cases}(\sin{\psi}\cdot\sin{\varphi}+\cos{\psi}\cdot\cos{\varphi}\cdot\sin{\theta})u_{1}=-mA_{x};\\ (-\cos{\psi}\cdot\sin{\varphi}+\sin{\psi}\cdot\cos{\varphi}\cdot\sin{\theta})u_{1}=-mA_{y};\\ \cos{\varphi}\cdot\cos{\theta}\;u_{1}=-mA_{z},\end{cases}$$
(6)

where

$$\begin{cases}A_{x}=(\alpha_{x}+k_{x})(\dot{x}-\dot{x}_{\textrm{ref}})+\alpha_{x}k_{x}(x-x_{\textrm{ref}})-\ddot{x}_{\textrm{ref}};\\ A_{y}=(\alpha_{y}+k_{y})(\dot{y}-\dot{y}_{\textrm{ref}})+\alpha_{y}k_{y}(y-y_{\textrm{ref}})-\ddot{y}_{\textrm{ref}};\\ A_{z}=(\alpha_{z}+k_{z})(\dot{z}-\dot{z}_{\textrm{ref}})+\alpha_{z}k_{z}(z-z_{\textrm{ref}})-\ddot{z}_{\textrm{ref}}-g.\end{cases}$$
(7)

Let us square the left and right parts of Eq. (6) and, having summed them up term by term, obtain the necessary total thrust of the engines

$$u_{1}=m\sqrt{A_{x}^{2}+A_{y}^{2}+A_{z}^{2}}=mg_{u}.$$
(8)

System of equations (6) is not independent. This objectively reflects the fact that it is sufficient to control any two angles of \(\psi\), \(\varphi\), \(\theta\) to control the course of the multirotor aircraft. When controlling, e.g., pitch and roll angles, the yaw angle \(\psi_{\textrm{ref}}\) can be selected from other considerations not related to changing the flight direction. According to the method described in [12], we find the necessary pitch and roll values of \(\theta_{\textrm{ref}}\) and \(\varphi_{\textrm{ref}}\) from Eqs. (6):

$$\begin{cases}\theta_{\textrm{ref}}=\arctan{(A_{x}\cos{\psi_{\textrm{ref}}}+A_{y}\sin{\psi_{\textrm{ref}}})/A_{z}};\\ \varphi_{\textrm{ref}}=\arctan{(A_{x}\sin{\psi_{\textrm{ref}}}-A_{y}\cos{\psi_{\textrm{ref}}})/\sqrt{g_{u}^{2}-(A_{x}\sin{\psi_{\textrm{ref}}}-A_{y}\cos{\psi_{\textrm{ref}})^{2}}}}.\end{cases}$$
(9)

If the coefficients \(\alpha\) and \(k\) are positive, the obtained control actions provide stable transients, and if \(\alpha=k\), the processes will be monotonous. The solutions of these equations have the same form for different laws of changing control actions.

Input data for the control algorithm are noisy realizations of the coordinates of the center of mass of the aircraft. In addition, estimates of the first derivatives of these variables are required. Relations (1) and (2) can be used to estimate object state variables in the extended Kalman filter. Let us supplement the process model (1) and (2) with the observation model and write down the obtained nonlinear dynamic system in compact form

$$\begin{cases}\dot{X}=f(X,U)+W;\\ \dot{Z}=h(X)+V,\end{cases}$$
(10)

where \(X=[x,y,z,\dot{x},\dot{y},\dot{z},\psi,\varphi,\theta,\dot{\psi},\dot{\varphi},\dot{\theta}]^{\top}\), \(U=[u_{1},\psi_{\textrm{ref}},\varphi_{\textrm{ref}},\theta_{\textrm{ref}}]^{\top}\), \(f\) is the nonlinear vector function corresponding to the right side of Eqs. (1) and (2), \(W\) is the input noise, and \(V\) is the measurement noise.

In the extended Kalman filter, nonlinear system (10) is linearized by decomposing the functions \(f\) and \(h\) into the Taylor series. Further estimation of the state vector \(X\) consists of successive extrapolation and correction stages [10].

Augmented reality (AR) coordinates of the marker fixed in the center of the vehicle mass, yaw, roll, and pitch angles, and angular velocities of the vehicle calculated by the onboard inertial system were used as measured components of the vector \(X\). The resulting measurement vector is \(Z=[x_{\textrm{cam}},y_{\textrm{cam}},z_{\textrm{cam}},\psi_{\textrm{imu}},\varphi_{\textrm{imu}},\theta_{\textrm{imu}},\dot{\psi}_{\textrm{imu}},\dot{\theta}_{\textrm{imu}}]^{\top}\). Unlike in [10, 13], here, the vector \(Z\) has no linear velocity components \(\dot{x}_{\textrm{cam}}\), \(\dot{y}_{\textrm{cam}}\), \(\dot{z}_{\textrm{cam}}\). The use of these values, calculated by filtering the signals \(x_{\textrm{cam}}\), \(y_{\textrm{cam}}\), \(z_{\textrm{cam}}\), causes a low-frequency oscillation in control system (9) caused by a delay that occurs during filtering.

4 CONTROL SYSTEM HARDWARE AND SOFTWARE DESCRIPTION

The developed control system has a two-level structure. The exchange of data between the controllers of the upper and lower levels is implemented via the mavlink protocol through a serial interface UART (Fig. 1). An autopilot Pixhawk with 32-bit STM32F427 Cortex M4 core processor and NuttX RTOS open-source operating system is used as a control controller of the lower level. The angular position of the vehicle is estimated in the module of the software complementary filter PX4 [14] using data from the integrated inertial sensor MPU 6000. The given orientation is set by the PX4 control module built in the software based on linear controllers. The autopilot operation modes are switched from the radio control (RC) console. In the automatic mode, the control command set_attitude containing the required values of Euler angles and total thrust of engines is submitted to the autopilot input. Takeoff and landing of the vehicle are performed in the manual mode by the control commands from the RC console. Absolute coordinates of the center of mass of the vehicle during indoor flights are determined using external video cameras and positioning system based on visual AR markers. The calculation of control actions taking into account the given flight path and current coordinates of the vehicle, integration, and filtering of these sensors is performed at the upper level; single board Raspberry Pi 3B microcomputer with 1.2 GHz 4-core Cortex-A53 processor (ARM v8) and 1 GB RAM. Ubuntu 16.04 LTS is used as the operating system.

Fig. 1
figure 1

Control system structural diagram.

Full size image

The software of the created control system has a modular architecture and is built on the mechanism of interprocess communication called the Robot Operation System (ROS) [15]. In this article, we use ROS software packages designed to solve the problems of communication between controllers, detection of AR markers, and data storage. The modules of control and state estimation in the extended Kalman filter developed by the authors are written in the Python language using asynchronous processing based on ROS Timer and are integrated into the ROS.

The developed flight control system includes a set of independent programs that interact with each other by exchanging messages in the ROS. Its structure is shown in Fig. 2. The system includes software modules that perform the following functions:

– Receiving images from the camera (camera);

– Detection of AR markers (ar_track_alvar);

– Determination of the spatial position of the markers (vision_pose);

– Trajectory motion control and state vector estimation in the extended Kalman filter (controller);

– Communication with the PX4 flight controller (mavros).

Fig. 2
figure 2

Structural diagram of the software part of the control system.

Full size image

Absolute coordinates of the center of mass in space, attitude angles, and angular velocities relative to a fixed coordinate system are fed to the extended Kalman filter. The obtained coordinate estimations are used by the controller program, which calculates control commands for the required attitude angles and total thrust of the engines according to the developed trajectory motion algorithm. These commands are then sent via the mavros module to the PX4 flight controller for execution.

5 EXPERIMENTAL RESULTS

The method of trajectory planning using the Euler spiral for transition between segments of a straight line of the route defined by a sequence of turning points was implemented in the MATLAB/Simulink environment [10]. The generated program trajectory is fed to the input of control algorithm (9). When constructing the trajectory, the following parameter values were used: the linear velocity of the target \(V=0{.}2\); 0.4 m/s and the maximum normal g-force \(n_{\textrm{enable}}=0{.}08\); 0.3.

Aircraft motion Eqs. (1) and (2) have the following parameters: \(m=0{.}4\) kg, \(T_{\psi}=T_{\varphi}=T_{\theta}=0{.}17\) s, and \(d_{\psi}=d_{\varphi}=d_{\theta}=0{.}35\), and for the control system we have \(k_{x}=\alpha_{x}=4{.}0\), \(k_{y}=\alpha_{y}=4{.}0\), and \(k_{z}=\alpha_{z}=4{.}0\).

Control commands setpoint_attitude were fed with a sampling frequency of 0.01 s. Data on the attitude of the vehicle calculated by its navigation system as well as coordinates of the center of mass of the vehicle from the external video system came at intervals of 0.02 and 0.05 s, respectively.

Before the motion started, the vehicle executed a command to hold a stationary target position until the time \(t=120\) s. The time \(t=0\) s corresponds to the startup of the controller control program. The calculated value of the standard deviation from the average variables \(x\), \(y\), \(z\) was 0.03 m. The calculation was performed in a time interval from 30 to 115 s; the values exceeding the originally calculated value by 3.5 in absolute value were discarded.

Figure 3 shows the results for the case of motion along the trajectory with the parameters \(V=0{.}2\) m/s, \(n_{\textrm{enable}}=0{.}08\). The deviation from the trajectory during the motion was 0.05 m.

Fig. 3
figure 3

Motion along the trajectory defined by the segments of the straight lines and Euler spirals: (a) the quadcopter trajectory in the plane \((x,y)\), (b) the height \(z\) above the surface, (c) the estimation of the linear velocity \(\dot{x}\), and (d) the estimation of the linear velocity \(\dot{y}\).

Full size image

The estimation of the translational velocity components \(\dot{x}\), \(\dot{y}\), obtained in the Kalman’s filter was compared with the estimation of linear velocities obtained by filtering signals \(x\), \(y\). The difference in estimation was 0.1 m/s (Figs. 3c and 3d).

Increase of the velocity of motion along the programmed trajectory up to \(V=0{.}4\) m/s and g-force up to \(n_{\textrm{enable}}=0{.}3\) leads to the decrease in accuracy of maintaining the given position during maneuvers (Fig. 4).

Fig. 4
figure 4

Motion along the trajectory defined by the segments of the straight lines and Euler spirals (designations correspond to Fig. 3).

Full size image

6 CONCLUSIONS

In this article, the algorithm for trajectory motion control of a multirotor vehicle in the problem of target position tracking was proposed. The hardware and software complex of the automatic flight control system was developed. The efficiency of the previously proposed route planning method using Cornu curves, which provides a smooth change of g-force on turns, was experimentally confirmed. The achieved indoor positioning accuracy of the quadcopter relative to the desired trajectory was 0.05–0.1 m.