Abstract
A control algorithm based on the required differential equations of deviations is proposed to solve the problem of tracing the desired trajectory by target position. The flight path is planned on the basis of the author’s method of calculation of a flat trajectory consisting of oriented segments of straight lines connected by Euler spirals. The software and hardware complex of the automatic flight control system is presented. The operability of the control system in the presence of measurement noises and external disturbances is confirmed by the results of experiments to control the flight of a quadcopter indoors.
Similar content being viewed by others
Avoid common mistakes on your manuscript.
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:
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]:
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}}\):
Let us define the constrained motion to the desired trajectory (\(S_{x}=0\), \(S_{y}=0\), \(S_{z}=0\)) by the equations
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
Taking into account (1) and (2), let us exclude the highest derivatives of the controlled values in (5)
where
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
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):
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
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.
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).
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.
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).
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.
REFERENCES
M. Kuckelhaus, D. Niezgoda, and S. Endriß, Unmanned Aerial Vehicles in Logistics (DHL, Troisdorf, 2014).
V. Duggal, M. Sukhwani, K. Bipin, G. S. Reddy, and K. M. Krishna, ‘‘Plantation monitoring and yield estimation using autonomous quadcopter for precision agriculture,’’ in Proc. IEEE Int. Conf. on Robotics and Automation (ICRA), Stockholm, Sweden, 2016, pp. 5121–5127. https://doi.org/10.1109/ICRA.2016.7487716
P. Ćwiakała, R. Kocierz, E. Puniach, M. Nedzka, K. Mamczarz, W. Niewiem, and P. Wiacek, ‘‘Assessment of the possibility of using unmanned aerial vehicles (uavs) for the documentation of hiking trails in alpine areas,’’ Sensors 18, 81 (2017). https://doi.org/10.3390/s18010081
Ch. Zhang and J. M. Kovacs, ‘‘The application of small unmanned aerial systems for precision agriculture: a review,’’ Precis. Agric. 13, 693–712 (2012). https://doi.org/10.1007/s11119-012-9274-5
O. Brock and O. Khatib, ‘‘Real-time replanning in high-dimensional configuration spaces using sets of homotopic paths,’’ in Proc Int. Conf. on Robotics and Automation (ICRA), San Francisco, 2000, pp. 550–555. https://doi.org/10.1109/ROBOT.2000.844111
J. S. Pershina, S. Ya. Kazdorf, and A. V. Lopota, ‘‘Methods of mobile robot visual navigation and environment mapping,’’ Optoelectron., Instrum. Data Process. 55, 181–188 (2019). https://doi.org/10.3103/S8756699019020109
S. Thrun, M. Montemerlo, H. Dahlkamp, et al., ‘‘Stanley: The robot that won the DARPA Grand Challenge,’’ J. Field Rob. 23, 661–692 (2006). https://doi.org/10.1002/rob.20147
J. Connors and G. Elkaim, ‘‘Analysis of a spline based, obstacle avoiding path planning algorithm,’’ in Proc. IEEE 65th Vehicular Technology Conf. (VTC2007-Spring), Dublin, 2007), pp. 2565–2569. https://doi.org/10.1109/VETECS.2007.528
J. Kim, M.-S. Kang, and S. Park, ‘‘Accurate modeling and robust hovering control for a quad–rotor vtol aircraft,’’ J. Intell. Rob. Syst. 57, 9–26 (2010). https://doi.org/10.1007/s10846-009-9369-z
S. A. Belokon’, Yu. N. Zolotukhin, K. Yu. Kotov, A. S. Mal’tsev, A. A. Nesterov, V. Ya. Pivkin, M. A. Sobolev, M. N. Filippov, and A. P. Yan, ‘‘Using the Kalman filter in the quadrotor vehicle trajectory tracking system,’’ Optoelectron., Instrum. Data Process. 49, 536–545 (2013). https://doi.org/10.3103/S8756699013060022
S. A. Belokon’, Yu. N. Zolotukhin, and A. A. Nesterov, ‘‘Aircraft path planning with the use of smooth trajectories,’’ Optoelectron., Instrum. Data Process. 53, 1–8 (2017). https://doi.org/10.3103/S8756699017010010
S. A. Belokon’, Yu. N. Zolotukhin, A. S. Mal’tsev, A. A. Nesterov, M. N. Filippov, and A. P. Yan, ‘‘Control of flight parameters of a quadrotor vehicle moving over a given trajectory,’’ Optoelectron., Instrum. Data Process. 48, 454–461 (2012). https://doi.org/10.3103/S8756699012050044
A. S. Dimova, K. Yu. Kotov, A. S. Mal’tsev, A. A. Nesterov, and M. N. Filippov, ‘‘Quadrotor control in payload transportation on suspension,’’ Optoelectron., Instrum. Data Process. 54, 520–524 (2018). https://doi.org/10.3103/S8756699018050151
L. Meier, D. Honegger, and M. Pollefeys, ‘‘PX4: A node-based multithreaded open source robotics framework for deeply embedded platforms,’’ in Proc. IEEE Int. Conf. on Robotics and Automation (ICRA), Seattle, 2015, pp. 6235–6240. https://doi.org/10.1109/ICRA.2015.7140074
M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, ‘‘ROS: an open-source robot operating system,’’ in ICRA Workshop on Open Source Software 3, 2009, p. 5.
Funding
This work was supported by the Ministry of Science and Higher Education of the Russian Federation, project no. AAAA-A17-117060610006-6.
Author information
Authors and Affiliations
Corresponding authors
Additional information
Translated by O. Pismenov
About this article
Cite this article
Kotov, K.Y., Mal’tsev, A.S., Nesterov, A.A. et al. Algorithms and Architecture of the Multirotor Aircraft Trajectory Motion Control System. Optoelectron.Instrument.Proc. 56, 228–235 (2020). https://doi.org/10.3103/S8756699020030085
Received:
Revised:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.3103/S8756699020030085