In this article we present an algorithm of controlling the quadrocopters swarm and a theory of applying the Kalman lter for the equations of motion of a quadrocopter in mountainous conditions. In our case, in order to coordinate the group, it is necessary to form the spatial programmatic trajectory of the UAV using the appropriate control law. The concept of coordinated reversal is introduced, which allows to obtain analytical equations of spatial motions expressed through the definition of the velocity vector and the yaw angle. The algorithm was tested in the Gazebo simulator. The results are used for spatial motion of quadrocopter groups.
Currently, there is an increasing interest in the use of UAVs in a number of practical applications. Nowadays, many inexpensive and easy-to-assemble UAV models with powerful on-board computers and auxiliary portable computers on the board are available on the market.
One of the most popular types used in the commercial eld and for academic research is the quadrocopter. Quadrocopters equipped with onboard sensors are popular because of their size, the ability to hover over objects, to navigate in 3D space. They can be used in various elds ranging from monitoring [1], [2], [4] to searching and implementing rescue operations [3]. The use of these UAVs in mountain areas has the following advantage: it can be located where human presence is impossible. At the same time, in mountain conditions, it may be di cult to receive signals from the GPS / GLONASS navigation systems, as well as distortion of signals from other sensors. In the presented work, the Kalman lter is used to reduce the errors of signals in the UAV control system in mountain conditions. The combined quadrocopter groups or swarms that can cover large space, collect more information and are less susceptible to the loss of a group member. Groups control can be carried out using di erent strategies, each of them has advantages and disadvantages. There are several examples of successful use of swarm strategies for controlling quadrocopter groups. Kushleyev developed a swarm of 20 miniature quadrocopters capable of forming several formations using centralized strategies [5]. Burkle implemented a swarm of quadrocopters capable of ying in external spaces, also using the centralized method and a ground station as a control device [6]. Vsrhelyi used swarm rules for the y of 10 quadrocopters in external environments [7]. In [8], a group of simulation UAVs was used for monitoring. The most used algorithms found in the literature can be classi ed into three types: leader-follower [9], [10], virtual structure [11], [12] and behavioral approach [13], [14]. In the rst type, one of the swarm is selected as the leader of the group, the other participants change their position in the group according to the position of the leader. This approach has an advantage, as it simpli es the task for the operator who controls only the leader, however, in the cases of a leader failure, the control of the entire group will be lost. A virtual structure is a consideration of the whole group as one xed structure which is represented as the one control point. The behavioral approach represents a programmed motion of the group in a certain way, for example, avoiding collisions, or moving as close as possible to each other. In most cases, this type of strategy is based on real natural phenomena [15]. In order to control the swarm and maintain the formation, we suggest using an approach based on a virtual leader. 2
The xed coordinate system XYZ is given. The dynamics of the quadrocopter motion will be considered as a set of the translational motion of the center of the quadrocopter mass and the rotational motion of the quadrocopter relatively to the center of mass. This motion is described by the following equations: 0 0 mg 1 0 p 1
0 p 1 m { the mass of quadrocopter
sin ' sin sin + cos sin ' sin cos ' sin
cos ' sin cos ' cos sin ' cos sin sin ' sin + cos sin sin ' 1
cos cos sin ' A cos ' cos R - the matrix of rotation, '; ; - Euler angles, Ti - thrust, Ql { torgue 0 p 1 cos ' sin
sin ' cos ' cos 0 1 0 A Izz (2) (3) (4) The matrix of inertia moments.
The solution of system (1) completely determines the quadrocopter motion trajectory.
In the conditions of mountain areas there is a high probability of noise in the operation of sensors (strong wind, weak electromagnetic signal, etc) in this case, it is desirable to correct the transfer of coordinates to the control system with the help of the lter. This can be an alpha-beta lter or a Kalman lter.
Consider the work of the Kalman lter in detail.
In general, the Kalman lter system for a discrete system is: xk =
k 1xk 1 + wk; zk = kxk + vk: Here xk { the dynamic system state vector, zk { the measurements at the time, tk; wk { the noise of the system, vk { the noise of the measurements.
The task of the Kalman lter is to determine the estimate of the state vector xk, this estimate we denote x^k, which is a function of the measurement z1; :::; zk and minimizes mean-root-square error.
The Kalman lter algorithm works according to the predictor corrector scheme (prediction correction). The algorithm of the Kalman lter can be presented in more detail. In the moment of time tk 1 there is an the estimate of the state vector x^k 1 and we need to receive a vector xk in the moment of time tk.
A predictive estimate of x^k - is built, based on the estimate of x^k+ 1 of the previous step. We obtain the measurement zk and the correction of the estimate occurs at the moment of time tk. As a result, we have an estimate of the state vector at some step x^k+ ( g.2). So "-" means a priori estimate. The sign "+" posterior estimate. Our system consists of 2 UAVs and a ground station communicating with the group via a Wi-Fi protocol. The ground station is responsible for collecting information from each UAV and sends information about the desired position of the quadrocopter in the space. The goal is to send a group of quadrocopters to the desired point avoiding collisions and keeping the group structure. The quadrocopter has a simple design with 4 engines, two of them rotate clockwise, two anticlockwise.
It is capable of taking o and landing (VTOL) and having 6 degrees of freedom. The motion of the duadrocopter in space is carried out by adjusting the thrust.
By adjusting the speed of engines rotation, we can move the quadrocopter relatively to the yaw angle. The qudrocopter is moved depending on the pitch and roll angles by adjusting the engine moments. The assembled quadrocopters designed for the testing of group control algorithms have a 650 mm frame with 490 Kv engines, each of them is connected to its own electronic speed controller (ESC) which is connected to the ight controller via the I2C bus. Each UAV has an external GPS which is very important for applying our algorithm. The used ight controller is Pixhawk. 3.1
The Pixhawk is an open source architecture [16] equipped with a variety of sensors such as a gyroscope, an accelerometer, a magnetometer and a barometer, with the ability to connect an external GPS module. The software which is running on the ight controller is called the PX4. This software allows you to interpret information received from sensors and signals from a manual control station, generates ight data and monitors each engine. The main advantage of the Pixhawk is to provide low-level stabilization and control of UAVs. A useful feature of the Pixhawk is the ability to interact with other devices using the MAVLink protocol developed speci cally for UAV applications. We can also use this protocol to get autonomous control of a UAV using a companion computer, such as the Raspberry Pi for communication with the ight controller. In addition, you can use it to read ight information and send various commands to control the ight controller.
Typically, the Pixhawk is controlled by a manual control station through a radio channel. However, in our case, the main control is carried out via a companion computer (Raspberry Pi with the installed OS Raspbian) which is mounted on the board and connected to the ight controller via the UART interface. Communication with the ground station is carried out through an external Wi-Fi module which is connected to the companion computer. Thus, we can use the onboard computer as a tool for autonomous ights, with the ability to switch to a manual control station in case of emergency. 3.2
We de ne the global system as a Cartesian coordinate system xed on the ground. Each UAV uses its sensors to determine its position within the global coordinate system. Considering the geometry of the earth and the originality of the global frame, the latitude and longitude obtained from the GPS sensor can be converted into a value in meters into a position along the X and Y axes. This will allow us to determine the initial position of the UAV. Next, the ight controller uses the Extended Kalman lter to estimate the position which is obtained from the ight controllers sensors. The Z coordinate is calculated from the total measurements received from the GPS and barometer. Each UAV gets a unique ID. The UAV group is viewed as a set of positions relative to the global frame. The distribution of positions in the group is carried out according to ID. 4
The software is represented as a console utility running on OS Arch Linux. The software was developed in C++17. The software allows controlling the UAV using a ground-to-air system: UAVs are connected in a star topology, where the ground station is represented as a central point with the running software. UAVs send telemetry data to the ground station, where, the position of UAV is calculated in the air by sending data back to the UAV. The main control is carried out by setting the velocity vector of the UAV, as well as the angle of yaw rotation. The ground station monitors the change and adjusts the velocity vector to achieve optimal performance of the task. At this stage, the task is supposed to set the target ight coordinates of the UAV group in the projected coordinate system: the group position is changed by specifying a vector of three coordinates x, y, z, where the positive increments are directed towards the east, north and upwards respectively. In addition to the linear displacement vector, it is possible to set the angle of rotation of the swarm in degrees as a true azimuth. In order to abstract and simplify the development of the communication module with UAVs, the DroneCode SDK library has been applied. With the help of this library, it is possible to implement a fast connection to UAVs using the MAVLink v2 protocol, using both TCP-UDP protocols and serial ports. Among other things, it is possible to track errors that occur in the process of conducting experiments. The software allows to create various methods of swarm manipulation. These methods include various ways of forming and moving the group. In order to debug a forming group UAVs the simple methods are used. The group is located in the same horizontal plane and kept at equal distances from the virtual center of the swarm. At the top level of abstraction, the control of this center is given, after which the necessary calculations are made for each of the agents. In order to move the group the velocity vector setting of the UAV is performed.
The process of group monitoring is controlled by the ground station. The accuracy of motion of each agent is checked and its deviation in a given radius is corrected. At the end of the motion, the UAVs correct their positions in the group according to the heterogeneity of the used devices: the di erence in mass, dimensions and force of the thrust. 5
The algorithm was tested in a software simulator. All simulations were performed in the Gazebo simulator, which allows to simulate the 3D spaces, robots and sensors. The main advantage of Gazebo is the ability to integrate various platforms. Using the Pixhawk and Gazebo software, we have the opportunity to create a simulation close to reality and test our algorithm as on a real device. This article presents the software that allows to control a group of quadrocopters based on Pixhawk ight controllers. The software was developed in the C++ environment and runs on the Arch Linux OS. In a simulation environment based on a Gazebo simulator, the algorithm was launched and tested under conditions as close to reality as possible. Five simulation ights were performed. Each of the examples had di erent starting GPS positions. We de ned a scenario where two quadrocopters with di erent identi cation numbers ew in a line at a height of 2.5 meters. The application of this algorithm is quite simple, and some improvements can be made in the future. In the software developed by us, such functions are implemented that allow the group to y in a straight line, without the possibility of a system reversal, and further, with the realization of this possibility, the group's maneuverability will increase signi cantly. Another interesting direction will be the implementation of the function to identify xed obstacles and avoiding collisions with them.