CC BY
63
Algorithm for Control of Unmanned Aerial Vehicles in the Process of Visual Tracking of Objects with a Variable Movement's Trajectory
1 112 A.A. Adnastarontsau , D.A. Adnastarontsava , R.V. Fiodortsev , D.V. Katser ,
2 2 13 13
A.Y. Liavonau , D.V. Romanov , D.N. Tcherniakovski ' , A.O. Mikhailau '
'LLC "NTLab-IS",
Surganova str., 41, Minsk 220013, Belarus 2JSC "Peleng",
Makayonka str., 25, Minsk 220114, Belarus
3LLC "NTLab-Systems",
Surganova str., 41, Minsk 220013, Belarus
Received 20.01.2021
Accepted for publication 04.03.2021
Abstract
The purpose of the research was to create an algorithm for determining and correcting the output parameters of the navigation module and the flight-navigation complex of unmanned aerial vehicles which provides control of an aviation gyro-stabilized platform with a multispectral optoelectronic system during flight and tracking various objects of observation.
Principles of control of an aviation technical vision system located on an unmanned aerial vehicle on a two-degree gyro-stabilized platform with the possibility of full turn around two perpendicular axes along the course and pitch are considered. Stability of tracking of observation objects at a distance of up to 10000 m is ensured by the use of a multispectral optoelectronic system including a rangefinder, thermal imaging and two visual channels.
Analysis of the object of observation and the method of its support are carried out. An algorithm is proposed for integrating a Global Navigation Satellite System and a strapdown inertial navigation system based on the extended Kalman filter which includes two stages of calculations, extrapolation (prediction) and correction. Specialized software in the FreeRTOS v9.0 environment has been developed to obtain a navigation solution: latitude, longitude and altitude of the unmanned aerial vehicle in the WGS-84 coordinate system, as well as the pitch, heading and roll angles; north, east and vertical components of velocities in the navigation coordinate system; longitudinal, vertical and transverse components of free accelerations and angular velocities in the associated coordinate system based on data from the receiving and measuring module of the Global Navigation Satellite System and data from the 6-axis MEMS sensor STIM300.
Keywords: aviation technical vision system, optoelectronic system, algorithm, orientation angles, navigation module.
DOI: 10.21122/2220-9506-2021-12-1-46-57
Адрес для переписки: Фёдорцев Р.В. ООО «НТЛаб-ИС»,
ул. Сурганова, 41, г. Минск 220013, Беларусь e-mail: feod@gmail.com
Address for correspondence:
Fiodortsev R.V. LLC "NTLab-IS",
Surganova str., 41, Minsk 220013, Belarus e-mail: feod@gmail.com
Для цитирования:
A.A. Adnastarontsau, D.A. Adnastarontsava, R.V. Fiodortsev, D.V. Katser, A.Y. Liavonau, D.V. Romanov, D.N. Tcherniakovski, А.О. Mikhailau. Algorithm for Control of Unmanned Aerial Vehicles in the Process of Visual Tracking of Objects with a Variable Movement's Trajectory. Приборы и методы измерений. 2021. - Т. 12, № 1. - С. 46-57. DOI: 10.21122/2220-9506-2021-12-1-46-57
For citation:
A.A. Adnastarontsau, D.A. Adnastarontsava, R.V. Fiodortsev, D.V. Katser, A.Y. Liavonau, D.V. Romanov, D.N. Tcherniakovski, A.O. Mikhailau. Algorithm for Control of Unmanned Aerial Vehicles in the Process of Visual Tracking of Objects with a Variable Movement's Trajectory. Devices and Methods of Measurements. 2021, vol. 12, no. 1, pp. 46-57. DOI: 10.21122/2220-9506-2021-12-1-46-57
Алгоритм управления беспилотными летательными аппаратами в процессе визуального сопровождения
объектов с изменяемой траекторией движения
1 112 2 А.А. Односторонцев , Д.А. Односторонцева , Р.В. Фёдорцев , Д.В. Кацер , А.Ю. Леонов ,
2 13 13
Д.В. Романов , Д.Н. Черняковский ' , А.О. Михайлов '
1ООО «НТЛаб-ИС»,
ул. Сурганова, 41, г. Минск 220013, Беларусь 2ОАО «Пеленг»,
ул. Макаёнка, 25, г. Минск 220114, Беларусь
3ООО «НТЛаб-Системы»,
ул. Сурганова, 41, г. Минск 220013, Беларусь
Поступила 20.01.2021 Принята к печати 04.03.2021
Целью проводимых исследований являлось создание алгоритма определения и коррекции выходных параметров модуля навигации и пилотажно-навигационного комплекса беспилотных летательных аппаратов, обеспечивающего управление авиационной гиростабилизированной платформой с мультиспектраль-ной оптико-электронной системой в процессе полёта и сопровождения различных объектов наблюдения.
Рассмотрены принципы управления авиационной системой технического зрения, размещённой на беспилотном летательном аппарате на двухстепенной гиростабилизированной платформе с возможностью полного разворота вокруг двух перпендикулярных осей по курсу и тангажу. Устойчивость сопровождения объектов наблюдения на расстоянии до 10000 м обеспечивается применением мультиспектральной оптико-электронной системы, включающей дальномерный, тепловизионный и два визуальных канала.
Выполнен анализ объекта наблюдения и методика его сопровождения. Предложен алгоритм интеграции спутниковой радионавигационной системы и бесплатформенной инерциальной навигационной системы на основе интегрального фильтра Калмана, предусматривающей две стадии вычислений: экстраполяцию (предсказание) и коррекцию. В модуль навигации встроено специализированное программное обеспечение для многозадачной операционной системы реального времени ЕгееКТОБ, обеспечивающее получение навигационного решения: широты, долготы и высоты беспилотного летательного аппарата в системе координат ЖОБ-84, а также углов крена тангажа и курса; северной, восточной и вертикальной составляющих скоростей в навигационной системе координат; продольной, вертикальной и поперечной составляющих свободных ускорений и угловых скоростей в связанной системе координат на основе данных от приёмо-измерительного модуля спутниковой радионавигационной системы и данных от 6-осевого МЭМС-датчика STIM300.
Ключевые слова: система технического зрения авиационная, оптико-электронная система, алгоритм, углы ориентации, модуль навигации.
БОТ: 10.21122/2220-9506-2021-12-1-46-57
Адрес для переписки: Фёдорцев Р.В. ООО «НТЛаб-ИС»,
ул. Сурганова, 41, г. Минск 220013, Беларусь e-mail: feod@gmail.com
Address for correspondence:
Fiodortsev R.V. LLC "NTLab-IS",
Surganova str., 41, Minsk 220013, Belarus e-mail: feod@gmail.com
Для цитирования:
A.A. Adnastarontsau, D.A. Adnastarontsava, R.V. Fiodortsev, D.V. Katser, A.Y. Liavonau, D.V. Romanov, D.N. Tcherniakovski, А.О. Mikhailau. Algorithm for Control of Unmanned Aerial Vehicles in the Process of Visual Tracking of Objects with a Variable Movement's Trajectory. Приборы и методы измерений. 2021. - Т. 12, № 1. - С. 46-57. DOI: 10.21122/2220-9506-2021-12-1-46-57
For citation:
A.A. Adnastarontsau, D.A. Adnastarontsava, R.V. Fiodortsev, D.V. Katser, A.Y. Liavonau, D.V. Romanov, D.N. Tcherniakovski, A.O. Mikhailau. Algorithm for Control of Unmanned Aerial Vehicles in the Process of Visual Tracking of Objects with a Variable Movement's Trajectory. Devices and Methods of Measurements. 2021, vol. 12, no. 1, pp. 46-57. DOI: 10.21122/2220-9506-2021-12-1-46-57
Introduction
Airborne vision and navigation systems of an aircraft are a combination of various devices: a radar station (radar), a thermal imager, a television (TV) camera, a laser locator (lidar), etc., which ensure the performance of various tasks related to observing objects in difficult weather conditions as well as in poor visibility conditions. Each of the sensors represents specific characteristics of the environment (brightness, thermal, radar, or optical-location contrasts of objects), but separately does not provide enough information about the underlying surface. The use of information coming simultaneously from several sensors requires solving a number of auxiliary mathematical problems. One of them is associated with the fact that images from different sensors are generally formed in different coordinate systems [1]. The concept of a multispectral vision system also makes it possible to improve flight safety by automatically correcting the current coordinates of the location of the aircraft in space, taking into account the terrain.
Modern trends in the creation of systems for tracking objects in multipurpose aircraft provide for software and hardware integration of onboard equipment in the form of container reconnaissance complexes. For example, the Russian development of the "Sych" family includes a three-level container complex:
- multispectral optical-electronic complex (UKR-OE) (TV/IR/low-level ranges);
- X-band surface radar reconnaissance complex (UKR-RL/BKR-3), represented by a two-way small-element radar system (radar) with a passive phased antenna array (PFAR) "Pika-M" with a resolution of about 0.3 m in SAR/ISAR and instrumental range of more than 300 km;
- a multi-band passive radio-technical reconnaissance complex (UKR-RT), which allows detecting and analyzing the frequency parameters of enemy radio-emitting sources (modern enemy airborne radars of AN/APG-77/79/81 types in aisle tracking and even LPI modes, active radar homing heads (GOS) air combat missiles and anti-aircraft missiles (SAMs), as well as surveillance radars and illumination radars) [2].
One of the important directions for improving onboard avionics is the development of hardware and software systems for "enhanced vision" (Enhanced Flight Vision Systems, EFVS). According to the functional characteristics of the systems of improved vision are divided into three typical classes [3]:
- Enhanced Vision System (EVS);
- Synthetic Vision System (SVS);
- Automated Vision System (AVS).
In papers [4-5], basic algorithms for the functioning of strapdown inertial navigation systems (SINS) are considered.
The purpose of the research was to create an algorithm for determining and correcting the output parameters of the navigation module and the flight-navigation complex of unmanned aerial vehicles (UAVs), which ensures the control of an aviation gyro-stabilized platform with a multispectral optoelectronic system during flight and tracking various objects of observation.
Multispectral optical-electronic system of technical vision based on an aviation gyro-stabilized platform
When using UAVs, the task often arises of tracking an object of interest for a long period of time. There may also be additional requirements for observation parameters:
- providing a given viewpoint;
- keeping the object of interest in the center of the frame, and for adjusting the circular flight path and transferring the coordinates of the circle center to the flight and navigation complex (FNC);
- maintaining a given angle of inclination in terms of distance to the object, etc.
To fulfill these parameters, it is not enough to control only the position of the sighting axis of the aviation technical vision system (STZA), but it is also required to issue corrective signals to the autopilot.
Let's consider the options for solving this problem in more detail. Multichannel STZA as part of the UAV allows not only to consider objects of interest in the visible and infrared ranges of the spectrum, but also to determine their geographical coordinates in the selected frame of reference, as well as the speed and direction of movement, if any.
There are two options for solving this problem.
The first is the constant delivery of the coordinates of the object of interest to the flight-navigation complex [6-9]. In this case, all the functionality of these parameters is implemented in the flight and navigation complex.
The second option is the transfer of the required flight trajectory to the flight and navigation complex. In this embodiment, the functionality of the observation parameters is implemented in the STZA (Table).
Main technical characteristics of the «Sych» aviation technical vision system
Parameter name
STZA-1
STZA-2
A variant of the implementation of a gyro-stabilized optoelectronic platform (GOES)
Root mean square error of stabilization
Interfaces:
- management
- transmission of video information
Guidance speed (maximum) Working temperature Power consumption Overall dimensions (diameterxheight) The weight
two-stage, single-circuit
5G mrad
double-degree, double-circuit in azimuth
25 mrad
3GG0/s
RS422/485, Ethernet, CAN Ethernet, HD-SDI
от -4G0C до +5G0C 15G watt 25Gx34G mm
6G0/s
12.8 kg
16 kg
Visible camera - 1 (TV channel)
resolution field of view optical zoom digital zoom
192G X 1G8G pix. (RGB) 2.30-63.70
3Gx 12x
Visible camera - 2
resolution field of view optical zoom
Detection/recognition range (visible range cameras 1 and 2):
- person
- vehicle
192G X 1G8G pix. (RGB) 4.G0; 8.G0 2X
62GG/25GGm 1GGGG/5GGG m
192G X 1G8G pix. (RGB) 2.40; 4.8 2X
69GG/26GGm 1GGGG/58GGm
Laser rangefinder
working wavelength measurement range Measurement accuracy, not less
1.53...1.57 ^m 1GG...1GGGG m ±5 m
Thermal imager
detector type resolution spectral range field of view optical zoom digital zoom
Detection/recognition range:
- person
- vehicle
refrigerated 64GX512 pix. 3.5 ^m 20.270 12X 2X
42GG/17GGm 1GGGG/4GGGm
The composition of the "Sych" STZA includes a gyro-stabilized platform with a useful opto-electronic load: two cameras of the visible range of a wide and a narrow field of view; thermal imaging module for working with objects of observation in the night mode; laser rangefinder module with a receiving and transmitting channel; built-in navigation system (GPS, GLONASS) (Figure 1) [10]. In the extended version, the STZA can be connected to a ground-based hardware and software complex.
Figure 1 - Layout of the main modules of the aviation technical vision system on a gyro-stabilized platform
In the process of work, the monitoring object is automatically tracked; determination of the direction and speed of movement of the object; determination of the distance to the object and its characteristics (location coordinates and overall dimensions). For the convenience of transmission over the radio channel, hardware-software compression of video information is carried out. When accompanying an object during flight, a picture-in-picture display mode is possible for observation at different distances. If it is temporarily impossible to transmit a signal via a radio channel, recording of video information is provided in the built-in memory of the device in an equivalent volume of 20 hours of flight.
Analysis of the object of observation and the method of its support
An object of observation can be understood as any object resolved by an optoelectronic system under specific conditions. The tracking algorithm built into it allows updating the reference image, allowing you to continuously observe the object from different
angles and distances. Partial overlap of the object (up to 50 % of the area) is not an obstacle to continuous tracking. Short-term full coverage is also possible, for example when driving behind trees, poles or road infrastructure. The vision system is capable of tracking objects moving in a tangential direction at a speed of more than 100 km/h at a distance of at least 60 m. The maximum flight altitude is limited by the tactical and technical characteristics of the UAVs.
The coordinates of the object are calculated by the STZA on-board computer with access to the signals of the global navigation satellite system. If the signal from the satellites is noisy, distorted or absent altogether, then for a limited period of time the correct operation of the device is still possible if an inertial module with a magnetic sensor is used as part of it.
Determination of the position of the observed object is based on solving a direct geodetic problem. The reference point is the UAVs coordinates received from the navigation module (Figure 2).
Figure 2 - Scheme of information exchange when implementing the control algorithm for unmanned aerial vehicles in the tracking mode
To calculate the azimuth and spherical distance to the object, it is necessary to convert the line of sight vector from the body coordinate system to the global one. The starting vector is determined by the gimbal/camera rotation angles.
The orientation angles of the inertial module define the transition matrix from one basis to another. The resulting line of sight vector allows you to find the azimuth and angle of the observation point in the global coordinate system. These angles are referenced to the object relative to the north direction and the local horizon, respectively. Knowing the vertical angle and height of the UAVs above the ellipsoid, you can find the spherical
distance to the point of intersection of the line of sight with the ellipsoid.
However, the object is rarely located strictly at zero height. For decoupling from the ellipsoid, a special elevation map is used.
Observation of the object is carried out during the capture by the automatic tracking machine. After capturing an object for tracking, the reference image of the object is the frame area corresponding to the capture rectangle. In this case, it is not necessary that the entire object falls into the rectangle. The capture algorithm works on any contrasting part belonging to the object. The tracking rectangle may not always be in the center of the video frame. A special algorithm allows you to move from the line of sight of the camera to the object anywhere in the frame.
The calculation of the speed and direction of movement is based on the change in the obtained coordinates of the object in time. Smoothing values is based on a linear approximation of the trajectory in a sliding window. The window size adjusts to the speed of movement. It is assumed that at high speeds the maneuverability of ground objects is limited. Therefore, the averaging window increases, reducing the effect of the random component of the error. When driving at low speeds, significant changes in direction are possible, so the averaging window decreases.
Used coordinate systems and their relationship
Providing the specified functionality of the navigation unit of the combined anti-jamming multisystem inertial-satellite navigation receiver is implemented by establishing the initial relationship between the following coordinate systems (CS): geocentric inertial (i-frame), geocentric (geodesic) rectangular connected to the Earth (e-frame), navigation (n-frame) and asso-ciated (b-frame).
An inertial coordinate system is an ideal reference frame in which ideal inertial sensors (accelerometers and gyroscopes) have zero readings. The inertial CS X'Y'Z' has its origin in the center of the Earth and axes that do not rotate with respect to distant galaxies. Its Z' axis is parallel to the Earth's axis of rotation and is directed towards the North Pole. The X' axis is directed to the vernal equinox, and the Y ' axis forms a right orthogonal triple of vectors with the first two. The projections of vectors on the axis of this CS have a superscript i (inertial).
The geodetic coordinate system XeYeZe has its origin at the center of mass of the Earth and an axis whose position is fixed in relation to the Earth. The axis X e is directed to the midpoint of the reference meridian. The axis Z e is parallel to the Earth's axis of rotation and is directed to the North Pole, and the axis Ye forms with the first two a right orthogonal triple of vectors. The projections of vectors on the axis of this CS have the superscript e (earth). Geodetic coordinates are used to determine the position of an object on the Earth's surface. The position of a point is specified by latitude B, longitude L and altitude H. Latitude is determined by the angle formed by the normal to the Earth's reference ellipsoid and the equatorial plane. Reference ellipsoid is an approximation of the shape of the Earth's surface by an ellipsoid of revolution, used on a certain area of the earth's surface. Longitude is measured by the dihedral angle between the plane of the meridian passing through the point and the plane of the reference meridian.
The WGS-84 CS will be used as a geodetic. The radii of curvature of the reference ellipsoid CS WGS-84 RM and RN are calculated from the following relations:
RM = a /(1 - e2 sin2 B)1/2;
Rn = a(1 -e2)/(1 -e2 sin2 B)3/2,
where a = 6378137 m - equatorial radius of the Earth; e2 = 6,694379990141x10^ - the square of the eccentricity of the earth ellipsoid; B - latitude of the point at which the radii of curvature of the reference ellipsoid are calculated.
The navigation coordinate system X nY nZ n is a local CS with a vertex aligned with the center of mass of the object. The axis Xn is directed to the north along a tangent to the meridian, the axis Yn is directed to the parallel to the east. The axis Zn is orthogonal to the reference ellipsoid and directed downward. Thus, the navigation CS is formed by the axes directed northeast-down (north, east, down - NED). The projections of vectors on the axis of this CS have a superscript n (navigation). The navigation CS rotates in the geodesic space with an angular velocity, associated with the rates of change in latitude B and longitude L, caused by the movement of the object relative to the quasi-elliptical surface of the Earth.
A coordinate system is a Cartesian coordinate system XbYbZb, the axes of which are directed along the construction axes of the object. The axis Xb
coincides with the longitudinal axis of the object and is directed towards the front part, the axis Y bcoincides with the transverse axis of the object and is directed to the right side, the axis Zb is perpendicular to the plane XbYb and complements the CS to the right triplet (directed downward). The projections of vectors on the axis of this CS have the superscript b (body). The position of a body coordinate system relative to the navigation coordinate system is defined by three angles called roll, pitch, and heading angles. Heading angle Y - the angle measured in the horizontal plane X"Y" from the axis Xn clockwise, to the projection of the longitudinal axis of the object on this plane. The pitch angle 9 is measured in the vertical plane between the axis X b and its projection onto the horizontal plane X"Y". The roll angle y is measured in the transverse plane between the axis Y" and the line of intersection of the specified plane with the horizon plane. When the right side of the object is below the horizontal plane, the roll is considered positive.
Algorithm for the integration of Global Navigation Satellite System and Strapdown Inertial Navigation System
Integration of Global Navigation Satellite System (GNSS) and Strapdown Inertial Navigation System (SINS) is carried out according to a loosely coupled scheme, when they work independently of each other, but contains a block that processes and corrects data from both systems. This is done using an extended Kalman filter (KF) [12-15].
The input of the KF receives the difference between the values of the velocities and coordinates obtained using the SINS and GNSS algorithm. The Kalman filter estimates errors in determining angular orientation, velocities and coordinates. These estimates are used to correct the values of the corresponding parameters in the SINS algorithm. The corrected values are fed to the system output (Figure 3).
Figure 3 - Block diagram of the integration of a Strapdown Inertial Navigation System and Global Navigation Satellite System
The KF also makes it possible to evaluate the systematic components of the errors of inertial sensors, which makes it possible to correct the readings of the linear acceleration sensors (SLA) (accelerometer) and the angular velocity sensor (SAV) (gyroscope).
The refined values of the navigation parameters are used for subsequent calculations of the KF matrices.
The algorithm used for the loosely coupled inertial satellite navigation system is based on the error compensation scheme of the SINS algorithm.
To determine the errors of the SINS algorithm, difference measurements are used, obtained on the basis of data from both systems (SINS and GNSS). These measurements are formed by subtracting from the vectors of geodetic coordinates rS"NS and speed
vSnINS of the same vectors obtained from the GNSS -
rn vn
GNSS I11" "oNSS '
The obtained difference measurements are processed by a discrete Kalman filter in accordance with Figure 3.
The KF state vector (output) has the following form:
X = [(Srn )t • (Svn )T • (V)T • (Ssystemwbbb )t • (Ssystem/* f f,
where Sr" - vector of errors in determining coordinates using the SINS algorithm, size 3^1;
Svn - vector of errors in determining the speed using the SINS algorithm, size 3^1;
rn = [B, L, H]T - vector-column of WGS-84 coordinates, where B - latitude, L - longitude, H - height (unit of measurement - [rad, rad, m], respectively);
v" = [vn, ve, vd]T - column vector of velocity in the accompanying CS (unit of measurement - [m/s]);
y - vector of the final rotation of the azimuthal error and errors of constructing the vertical by the SINS algorithm, the size 3x1;
Ssystem wbb - vector of systematic components
of gyroscope errors, size 3x1;
Ssystem /* - vector of systematic components
of accelerometer errors, size 3x1;
wb, = [wb w* w* ]T - vector-column of indi-
ib L ibx? iby ibz J
cations of a 3-axis SAV (unit of measurement -[rad/s]);
/b = [ fb, fb, fb ]T - vector-column of readings of a 3-axis SLA (unit of measurement - [m/s2]).
The KF measurement vector (input) has the following structure:
7 = [(rn - r n )T • (vn - vn )T 1T
^ lv'sins 'gnss/ v sins ^gnss/ j •
Each iteration of the KF is divided into two phases: extrapolation (prediction) and correction.
At the first stage, the state of the system is estimated at each moment of time based on the data of the previous iteration and the given dynamic model of the system (taking into account its inaccuracy).
The KF prediction stage is as follows:
Xk\k-1 = Fk Xk-1|k-1; Fk = I15,15+Akdtk;
P = F P FT + Q •
k\k-1 1 k1 k-1\k-11 k ^^¿k'
A = Gkdtk; Qk = aqaT;
Q = diag (a f, o^),
where k - filtering step number;
Xk k - predicted value of the KF state vector
at the k-th moment of time, size 15x1;
Fk - the fundamental matrix of the state of the KF, size 15x15, describing the dynamic model of the
system;
Pk|k-i - the covariance matrix of the predicted vector of the state of the system at the current step k, size 15x15;
A - transient perturbation matrix of the KF, size 15x6;
Q - process covariance matrix (model noise), size 6x6.
At the second stage (correction), based on new measurements, the predicted value is corrected (also taking into account the inaccuracy and noisiness of this information). The initial conditions at the current iteration of the functioning of the KF algorithm are the assessment of the system state (observation matrix) and the error in the assessment of the previous iteration. The estimation error is the measurement noise covariance matrix Rk, in which the variances of the corresponding vector components are found on the diagonal.
After calculating the matrix of gains Kk, the system estimate is updated: the KF state vector Xk\k
and the covariance error matrix Pk\k are updated for the current and subsequent iterations.
The stage of KF correction is as follows:
Rk = diag(o
rkGNSS' vkGNSS
У k = Zk Hk Xk\k-1;
Kk = Pkkk-H (HkPkkk-HT + R)-1;
k\k
4\k-i+Kky k;
k\k ("^15x15 KkHk )Pk\k-1,
Pk
where g
- Root mean square (RMS) vector of
errors in determining geodetic coordinates using an GNSS receiver;
°vgnss - RMS vector of the errors in determining the speed using the GNSS receiver;
y - vector of deviation of the measurement result from the extrapolation result, size 6x1;
xk]k - estimate of the state vector of the KF at
the k-th moment of time, size 15x1;
Pk|k - covariance matrix of the system state vector estimate, size 15x15.
In the proposed algorithm, the system of linearized differential equations of errors of the SINS algorithm is used as the equations of state of the system in the KF. The error equations set the relationship between the errors of the elements and the inaccurate input of the initial conditions, on the one hand, and the errors of the SINS output data, on the other. This choice made it possible to estimate not only the coordinate and velocity errors of the SINS, but also the angular errors and systematic errors of the inertial sensors, based on the difference signals of coordinates and velocities. That is, despite the absence of direct information about these errors in the measurement vector, the information about the object's orientation in space is corrected.
The operation algorithm of the navigation unit is shown in Figure 4.
Figure 4 - Block diagram of the navigation block algorithm
Despite the fact that the frequency of the navigation solution issuance by the GNSS receiver is lower than the SINS operating frequency, the use of the KF algorithm made it possible not to carry out additional interpolation of navigation information from the GNSS receiver. This is achieved due to the fact that the KF prediction stage is performed with the frequency of receiving information from the SINS. And the stage of correction is performed upon receipt of the GNSS measurements. Thus, at the moments of arrival of information from the GNSS, both prediction based on the given dynamics of the system and the vector of measurements are used to estimate the state vector. At other times, the state vector is estimated only on the basis of the given dynamics of the system. Calculation results are issued on request via the RS485 interface.
Navigation solution software
The navigation module has built-in specialized software for the FreeRTOS real-time multitasking operating system, which provides the formation of a navigation solution (coordinates, speed, orientation) based on data from the receiving and measuring module of the Global Navigation Satellite System and data from the 6-axis MEMS sensor STIM300, as well as determining the orientation of the object and the possibility of information exchange (receiving data and control commands, issuance of navigation parameters and data on technical condition).
After turning on the power supply (about 0.5 s), a rough alignment of the initial parameters for SINS is carried out. At the stage of rough alignment, the roll and pitch angles are determined according to the readings of SLA and SAV. In this case, the initial coordinates and the heading angle take the values set by default, the velocity components are equal to zero. If an alignment by GNSS is used, the rough alignment timer must be set for more than 1 min (time sufficient to initialize the GNSS receiver) (Figure 5).
At the stage of exact alignment, the roll and pitch orientation angles are specified. It is recommended to set the exact alignment timer in the range of 10-20 s.
The input data for the program are:
- SLA and SAV data received from STIM300, which are received via the UART interface and have a binary representation. For a description of the data format, see the STIM300 documentation. Data is transmitted at 250 Hz without request. Data decoding is done in the STIMParser class;
- data of the GNSS-receiver. The data is received via the UART interface and has a binary representation. Data is transmitted at a frequency of 1 Hz, processed by NTLBinaryParser drivers.
Figure 5 - Graph of states of the navigation module: 1 - activation of the rough exhibition timer; 2 - triggering of the exact exhibition timer; 3 - command options: "Alignment on Global Navigation Satellite System", "Alignment on known coordinates", "Alignment on known coordinates and course", "Alignment on Global Navigation Satellite System and course"
The general structure of the program is shown in Figure 6.
Figure 6 - Software structure of the navigation module
SLA and SAV polling function (stimChanByteRx) - a function for controlling the STIM300 module. Provides initialization of STIM300, reception of data of angular velocities and linear accelerations and their return through variables of the STIMParser (stim300_parser.h).
GNSS polling function (nt1058_A_ByteRx, nt1058_B_ByteRx) - a function for controlling the GNSS receiver module. Implements receiving and returning data through the navsolt structure (ntlbinary_parser.h).
Function for solving the navigation problem (sinsUpdate) - provides the calculated data through the nav_data_t structure (commondata.h).
UART exchange functions (navChanByteRx, navChanTx) - a set of functions that implement the information exchange protocol via the UART interface (RS485).
The output is a navigation solution:
- latitude WGS-84;
- longitude WGS-84;
- height WGS-84;
- roll angle;
- pitch angle;
- heading angle;
- northern, eastern and vertical components of velocities in the navigation coordinate system;
- longitudinal, vertical and transverse components of free accelerations in a bound coordinate system;
- longitudinal, vertical and transverse components of angular velocities in the associated coordinate system.
- the quaternion of transformation from the associated coordinate system to the navigation one.
The output data is transmitted on request via the RS485 interface, in accordance with the communication protocol.
Conclusion
The proposed multispectral optical-electronic system of technical vision based on an aviation gyro-stabilized platform with two degrees of mobility and a two-loop stabilization system provides stable detection and tracking of objects at a distance of up to 10000 m in the visible and infrared ranges of the spectrum.
The tracking algorithm allows updating the reference image, allowing you to continuously observe the object from different angles and distances. Partial overlap of the object (up to 50 %
of the area) is possible, as well as its short-term full coverage with other objects.
An algorithm for the integration of a Global Navigation Satellite System and a Strapdown Inertial Navigation System based on the extended Kalman filter has been developed, through which the errors in determining the angular orientation, velocities and coordinates are estimated. These estimates are further used to correct the values ofthe corresponding parameters in the algorithm of the Strapdown Inertial Navigation System.
Specialized software has been developed in the FreeRTOS environment, which provides the calculation of a navigation solution (coordinates, speed, orientation) based on data from the receiving and measuring module of the Global Navigation Satellite System and data from the 6-axis MEMS sensor STIM300.
References
1. Obrabotka izobrazheniy v aviatsionnykh siste-makh tekhnicheskogo zreniya [Image processing in aeronautical vision systems]. Ed. Kostyashkina L.N., Nikiforova M.B. M.: FIZMATLIT Publ., 2016, 234 p.
2. Damantsev Ye. Neraskrytyye vozmozhnosti proyek-ta "Sych". Unikal'nyy udarno-razvedyvatel'nyy komp-leks na baze Su-34. "Voyennoye obozreniye". Analitika. [Undiscovered Possibilities of the "Sych" Project. A unique strike and reconnaissance complex based on the Su-34. "Military Review". Analytics]. 31.01.2020 (in Russian).
3. Advisory Circular AC №:20-167A. U.S. Department of Transportation Federal Aviation Administration. 12.06.2016, 104 p.
4. David H. Titterton, John L. Weston. Strapdown Inertial Navigation Technology, 2004, 2nd ed., Radar, sonar, navigations & avionics, 594 p.
5. Paul G. Savage. Strapdown Associates, 2000, vol. 1, 2nd edition, 1556 p.
6. Volkov V.G. Vertolotnyye optiko-elektronnyye sis-temy nablyudeniya i razvedki [Helicopter optoelectronic surveillance and reconnaissance systems]. Journal "Special Technique", 2001, no. 3 (in Russian). Available at: http:// www.bnti.ru/showart.asp?aid=510&lvl=09.01.&p=1 (accessed 22.02.2021).
7. Makarchenko O.F., Makarchenko F.I., Nilov A.V., Rumyantsev G.N., Sukhov S.V. Sposob uvelicheniya diapazona uglov povorota izdeliya otnositel'no giro-stabilizirovannoy platformy, ustanovlennoy na izdelii v kardannovom podvese [A method of increasing the range of angles of rotation of the product relative to the gyro-stabilized platform installed on the product in a gimbal]. Patent RF, no. 2552857, 2015.
8. Burets G.A., Gorokhov M.M., Denisov R.N., Nuzhin A.V., Pleshanov Y.V., Puysha A.E. Optiko-elektronnyy sledyashchiy koordinator [Optoelectronic tracking coordinator]. Patent RF, no. 2476826, 2013.
9. Amelin K.S. A method of orienting an ultralight UAVs with a rare update of its position data [Metod oriyentirovaniya sverkhlogkogo BPLA pri redkom obnovlenii dannykh o yego mestopolozhenii]. Saint Petersburg State University, 2014, pp. 3-14 (in Russian). Available at: https://www.math.spbu.ru/user/gran/ soi10_2/Am10_2.pdf (accessed: 22.02.2021).
10. Sozdaniye sistemy tekhnicheskogo zreniya aviatsionnoy, shifr "Sych" [Creation of an aviation technical vision system, code "Sych"]. JSC "Peleng". State rubricator of scientific and technical information SRSTI:59.14.29. from 21.08.2017. Register of scientific research, experimental design and experimental technological work registered in 2017 / ed. A.G. Shumilina. Minsk: State Institution "BellSA" Publ., 2018, 109 p.
11. Lukatsevich K. Tochno na tsel'. Pritsel'nyye kompleksy OAO «Peleng» sootvetstvuyut samym vysokim trebovaniyam. Oboronno-promyshlennyy kompleks Rossii [Right on target. The sighting systems of JSC "Peleng" meet the highest requirements. Defense industrial complex of Russia]. Special information and analytical project. № 2(23), July 2020, 70 p.
12. Bagmut I.A. Nastroyka fil'tra Kalmana v zadache korrektsii inertsial'nykh izmereniy v integrirovannoy
navigatsionnoy sisteme [Adjusting the Kalman filter in the problem of correcting inertial measurements in an integrated navigation system]. Bulletin of the National Technical University "KhPI": collection of scientific papers. Thematic issue: Dynamics and strength of machines. Kharkov: NTU "KhPI" Publ., 2011, no. 63, pp. 13-21 (in Russian).
13. Mkrtchyan V.I. Nastroyka fil'tra Kalmana dlya otsenivaniya oshibki BINS po kursu [Adjusting the Kalman filter for estimating the SINS error at the rate]. Modern science: actual problems of theory and practice. Series: Natural and Technical Sciences,2018, no. 6, pp. 24-28 (in Russian).
14. N. Al Bitar, Gavrilov A.I. Sravnitel'nyy analiz algoritmov kompleksirovaniya v slabosvyazannoy inertsial'no-sputnikovoy sisteme na osnove obrabotki real'nykh dannykh [Comparative analysis of integration algorithms in a loosely coupled inertial-satellite system based on real data processing]. Gyroscopy and navigation, 2019, vol. 27, no. 3(106), pp. 31-52 (in Russian).
DOI: 10.17285/0869-7035.0001
15. Benkafo A.S., Lobaty A.A. Osobennosti pri-meneniya fil'trov Kalmana-B'yusi v kompleksakh oriyentatsii i navigatsii [Features of Kalman-Bucy filters application in orientation and navigation complexes]. Belarusian State University of Informatics and Radioelectronics (BSUIR) reports, 2013, no. 5(75), pp. 67-71 (in Russian).
