RU2793982C1 - Device and method for optimizing aircraft trajectory - Google Patents

Abstract

FIELD: aircraft (AC).

SUBSTANCE: device and a method for optimizing the trajectory of an aircraft (AC). Device contains a mapping module, a distance estimation module, a global planning module, a local planning module, and a proportional-differentiating (PD) controller. To optimize the trajectory of the AC, a map of environmental objects is built based on a three-dimensional (3D) cloud of points obtained from the 3D point cloud generation module contained in the AC, the distance from the AC to surrounding objects is estimated based on the map of environmental objects and AC location data, received from navigation module contained in the AC, an updated trajectory of the AC movement around obstacles is formed based on the reference trajectory, a map of environmental objects and data on the distance from the AC to surrounding objects is formed, the optimal trajectory and optimal control signals for the AC are formed, PD controller is used to generate adjusted control signals for the AC, transmit control signals to the flight controller to control the actuators of the AC.

EFFECT: increased accuracy of following the initial trajectory of the aircraft in presence of obstacles, improved smoothness of movement and increased flight safety.

7 cl, 4 dwg

Description

Область техники, к которой относится изобретениеThe field of technology to which the invention belongs

Изобретение относится к летательным аппаратам, в частности к оптимизации траектории движения автономно движущегося летательного аппарата.The invention relates to aircraft, in particular to the optimization of the trajectory of an autonomously moving aircraft.

Уровень техникиState of the art

Беспилотные летательные аппараты (БПЛА, UAV) постепенно становятся частью повседневной жизни - они выполняют доставку грузов, различные исследования, анализ окружающего пространства, мониторинг территорий, контроль объектов, фото- и видеосъемку, используются в спасательных операциях и находят множество других применений.Unmanned aerial vehicles (UAVs, UAVs) are gradually becoming a part of everyday life - they carry out cargo delivery, various studies, analysis of the surrounding space, territory monitoring, object control, photo and video shooting, are used in rescue operations and find many other applications.

Усложнение задач, которые могут быть возложены на БПЛА, требует от них улучшений в качестве ориентирования и управления в пространстве, в том числе рядом с людьми. Также необходимо, чтобы летательные аппараты имели высокий уровень безопасности. При этом необходимо, чтобы летательные аппараты выполняли поставленные задачи оптимальным образом.The complication of the tasks that can be assigned to UAVs requires them to improve the quality of orientation and control in space, including near people. It is also necessary that aircraft have a high level of safety. At the same time, it is necessary that the aircraft perform their tasks in an optimal way.

Следование БПЛА по опорной траектории используется в различных областях, например, в кинематографии или при посадке на движущуюся платформу. В уровне техники предложены разные подходы для следования по предварительно заданной опорной траектории - например, в публикациях [1] Т. Baca, D. Hert, G. Loianno, М. Saska, and V. Kumar, "Model predictive trajectory tracking and collision avoidance for reliable outdoor deployment of unmanned aerial vehicles," 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp.6753-6760, [2] M.-E. Guerrero-Sanchez, O. Hernandez-Gonzalez, G. Valencia-Palomo, F.-R. Lopez-Estrada, A.-E. Rodnguez-Mata, and J. Garrido, "Filtered observer-based ida-pbc control for trajectory tracking of a quadrotor," IEEE Access, vol. 9, pp.114 821-114 835, 2021, [3] O. Mechali, L. Xu, Y. Huang, M. Shi, and X. Xie, "Observer-based fixedtime continuous nonsingular terminal sliding mode control of quadrotor aircraft under uncertainties and disturbances for robust trajectory tracking: Theory and experiment," Control Engineering Practice, vol. 111, p.104806, 2021.UAV following the reference trajectory is used in various fields, for example, in cinematography or when landing on a moving platform. Various approaches have been proposed in the art for following a predefined reference trajectory - for example, in [1] T. Baca, D. Hert, G. Loianno, M. Saska, and V. Kumar, "Model predictive trajectory tracking and collision avoidance for reliable outdoor deployment of unmanned aerial vehicles," 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp.6753-6760, [2] M.-E. Guerrero-Sanchez, O. Hernandez-Gonzalez, G. Valencia-Palomo, F.-R. Lopez-Estrada, A.-E. Rodnguez-Mata, and J. Garrido, "Filtered observer-based ida-pbc control for trajectory tracking of a quadrotor," IEEE Access, vol. 9, pp.114 821-114 835, 2021, [3] O. Mechali, L. Xu, Y. Huang, M. Shi, and X. Xie, "Observer-based fixedtime continuous nonsingular terminal sliding mode control of quadrotor aircraft under uncertainties and disturbances for robust trajectory tracking: Theory and experiment," Control Engineering Practice, vol. 111, p.104806, 2021.

Несмотря на прикладываемые усилия, обозначенная задача остается открытой для исследований по нескольким причинам: достижение производительности в реальном времени, избегание находящихся вблизи препятствий, возможность применения в различных погодных условиях и т.д. Вследствие этого генерация политики субоптимального управления для маневрирования в насыщенной препятствиями неизвестной и к тому же изменяющейся среде является довольно сложной задачей, особенно с точки зрения обеспечения ограничений по динамической осуществимости и безопасности полета в режиме реального времени.Despite the efforts made, the designated problem remains open for research for several reasons: achieving real-time performance, avoiding nearby obstacles, being able to be used in various weather conditions, etc. As a result, the generation of a suboptimal control policy for maneuvering in an obstruction-rich, unknown and, moreover, changing environment is quite a challenge, especially from the point of view of real-time dynamic feasibility and flight safety constraints.

Частично эти проблемы можно было бы решить методом управления с прогнозирующей моделью (Model Predictive Control, МРС) - например, методом на основе линейной модели (LMPC) или методом на основе нелинейной модели (NMPC), однако в чистом виде такой метод не позволяет выполнять управление в реальном времени в большинстве практических ситуаций из-за ограниченности доступных вычислительных ресурсов, из-за используемых в этом методе подходов к формулировке задачи и к обработке препятствий и входных данных. Более того, точность МРС сильно зависит от имеющихся в распоряжении возможностей обнаружения - например, поля обзора (FoV) камеры, дальности обнаружения и способа представления окружающего свободного пространства и препятствий. Кроме того, чем больше препятствий находится на заданной опорной траектории или в непосредственной близости от нее, тем выше вычислительная сложность метода МРС, из-за чего в условиях ограниченных вычислительных ресурсов МРС может выдавать лишь некий локальный минимум функции оптимизации, который не позволяет избежать столкновения с препятствием.Partially, these problems could be solved by the method of control with a predictive model (Model Predictive Control, MPC) - for example, a method based on a linear model (LMPC) or a method based on a non-linear model (NMPC), however, in its pure form, such a method does not allow control in real time in most practical situations due to the limited available computing resources, due to the approaches used in this method to the formulation of the problem and to the processing of obstacles and input data. Moreover, the accuracy of the MRS is highly dependent on the detection capabilities available - for example, the field of view (FoV) of the camera, the detection range, and the way the surrounding free space and obstacles are represented. In addition, the more obstacles are located on a given reference trajectory or in its immediate vicinity, the higher the computational complexity of the MPS method, due to which, under conditions of limited computing resources, MPS can only produce a certain local minimum of the optimization function, which does not allow avoiding a collision with an obstacle.

Сущность изобретенияThe essence of the invention

Настоящее изобретение направлено на создание устройств и способов, позволяющих устранить по меньшей мере некоторые из указанных выше недостатков предшествующего уровня техники.The present invention is directed to devices and methods to overcome at least some of the above disadvantages of the prior art.

В частности, предложено устройство для оптимизации траектории движения летательного аппарата (ЛА), содержащее:In particular, a device for optimizing the trajectory of an aircraft (LA) is proposed, containing:

модуль картографирования, выполненный с возможностью построения карты объектов окружающей среды на основе трехмерного (3D) облака точек, полученного от содержащегося в ЛА модуля генерации 3D облака точек;a mapping module configured to build a map of environmental objects based on a three-dimensional (3D) point cloud obtained from a 3D point cloud generation module contained in the aircraft;

модуль оценки расстояний, выполненный с возможностью оценки расстояния от ЛА до окружающих объектов на основе карты объектов окружающей среды и данных о местоположении ЛА, полученных от содержащегося в ЛА модуля навигации;a distance estimation module, configured to estimate the distance from the aircraft to the surrounding objects based on the map of the objects of the environment and the position data of the aircraft received from the navigation module contained in the aircraft;

модуль глобального планирования, выполненный с возможностью формирования уточненной траектории движения ЛА в обход препятствий на основе опорной траектории, карты объектов окружающей среды и данных о расстоянии от ЛА до окружающих объектов;a global planning module configured to generate an updated trajectory of the aircraft around obstacles based on the reference trajectory, a map of environmental objects and data on the distance from the aircraft to surrounding objects;

модуль локального планирования, выполненный с возможностью формирования оптимальной траектории и оптимальных управляющих сигналов для ЛА на основе уточненной траектории с использованием метода управления с нелинейной прогнозирующей моделью; иa local planning module configured to generate an optimal trajectory and optimal control signals for the aircraft based on the refined trajectory using a non-linear predictive model control method; And

пропорционально-дифференцирующий (ПД) регулятор, выполненный с возможностью формирования отрегулированных управляющих сигналов для ЛА и их передачи на контроллер полета для управления исполнительными механизмами ЛА, причем ПД регулятор использует данные о текущем состоянии ЛА от модуля навигации ЛА в качестве обратной связи и оптимальные управляющие сигналы от модуля локального планирования в качестве входного сигнала.proportional-differentiating (PD) controller, configured to generate adjusted control signals for the aircraft and transmit them to the flight controller to control the aircraft actuators, and the PD controller uses data on the current state of the aircraft from the aircraft navigation module as feedback and optimal control signals from the local scheduling module as input.

В одном из вариантов осуществления модуль глобального планирования выполнен с возможностью формирования уточненной траектории движения ЛА в обход препятствий, если определено, что опорная траектория проходит через объект окружающей среды или что ЛА или опорная траектория находится на таком расстоянии до объекта окружающей среды, которое меньше предварительно заданного расстояния уклонения.In one of the embodiments, the global planning module is configured to generate an updated trajectory of the aircraft movement around obstacles if it is determined that the reference trajectory passes through an environmental object or that the aircraft or the reference trajectory is at such a distance to the environmental object that is less than a predetermined avoidance distances.

В одном из вариантов осуществления местоположение ЛА соответствует центру ЛА, а расстояние уклонения больше радиуса окружности, описывающей ЛА.In one embodiment, the location of the aircraft corresponds to the center of the aircraft, and the deviation distance is greater than the radius of the circle describing the aircraft.

В одном из вариантов осуществления оптимальные управляющие сигналы для ЛА содержат векторы скорости в каждом из направлений х, у, z, а также угол рыскания.In one embodiment, the optimal control signals for the aircraft comprise velocity vectors in each of the x, y, z directions, as well as a yaw angle.

В одном из вариантов осуществления устройство дополнительно содержит модуль навигации, выполненный с возможностью определения местоположения, скорости и угла рыскания ЛА.In one embodiment, the device further comprises a navigation module configured to determine the location, speed, and yaw angle of the aircraft.

В одном из вариантов осуществления устройство дополнительно содержит модуль генерации 3D облака точек, выполненный с возможностью генерации 3D облака точек.In one embodiment, the device further comprises a 3D point cloud generation module configured to generate a 3D point cloud.

Также предложен способ оптимизации траектории движения ЛА, содержащий этапы, на которых:Also proposed is a method for optimizing the trajectory of the movement of the aircraft, containing the steps at which:

с использованием модуля картографирования выполняют построение карты объектов окружающей среды на основе трехмерного (3D) облака точек, полученного от содержащегося в ЛА модуля генерации 3D облака точек;using the mapping module, building a map of environmental objects based on a three-dimensional (3D) point cloud obtained from the 3D point cloud generation module contained in the aircraft;

с использованием модуля оценки расстояний выполняют оценку расстояния от ЛА до окружающих объектов на основе карты объектов окружающей среды и данных о местоположении ЛА, полученных от содержащегося в ЛА модуля навигации;using the distance estimation module, estimating the distance from the aircraft to the surrounding objects based on the environmental object map and the aircraft position data obtained from the navigation module contained in the aircraft;

с использованием модуля глобального планирования формируют уточненную траекторию движения ЛА в обход препятствий на основе опорной траектории, карты объектов окружающей среды и данных о расстоянии от ЛА до окружающих объектов;using the global planning module, an updated trajectory of the aircraft movement is formed to bypass obstacles based on the reference trajectory, a map of environmental objects and data on the distance from the aircraft to surrounding objects;

с использованием модуля локального планирования формируют оптимальную траекторию и оптимальные управляющие сигналы для ЛА на основе уточненной траектории с использованием метода управления с нелинейной прогнозирующей моделью; иusing a local planning module, generating an optimal trajectory and optimal control signals for the aircraft based on the refined trajectory using a control method with a non-linear predictive model; And

с использованием ПД регулятора формируют отрегулированные управляющие сигналы для ЛА и передают их на контроллер полета для управления исполнительными механизмами ЛА, причем используют данные о текущем состоянии ЛА от модуля навигации ЛА в качестве обратной связи и оптимальные управляющие сигналы от модуля локального планирования в качестве входного сигнала.using the PD controller, the adjusted control signals for the aircraft are generated and transmitted to the flight controller to control the aircraft actuators, and the data on the current state of the aircraft from the aircraft navigation module as feedback and the optimal control signals from the local planning module as an input signal are used.

Технический результатTechnical result

Заявленное устройство содержит модули планирования, которые позволяют в автономном режиме планировать в неизвестных заранее, изменяющихся условиях наиболее рациональную с точки зрения динамической осуществимости и безопасности траекторию в режиме реального времени, обеспечивая также плавность хода.The claimed device contains planning modules that allow you to plan in offline mode in unknown in advance, changing conditions, the most rational trajectory in terms of dynamic feasibility and safety in real time, also ensuring smooth running.

Техническим результатом, достигаемым изобретением, является повышение точности следования исходной траектории движения летательного аппарата при наличии препятствий, улучшение плавности движения и повышение безопасности полетов.The technical result achieved by the invention is to increase the accuracy of following the initial trajectory of the aircraft in the presence of obstacles, improve the smoothness of movement and increase flight safety.

Эти и другие преимущества настоящего изобретения станут понятны при прочтении нижеследующего подробного описания со ссылкой на сопроводительные чертежи.These and other advantages of the present invention will become apparent upon reading the following detailed description with reference to the accompanying drawings.

Краткое описание чертежейBrief description of the drawings

На Фиг. 1 показана блок-схема устройства для оптимизации траектории движения БПЛА.On FIG. 1 shows a block diagram of a device for optimizing the trajectory of the UAV.

На Фиг. 2 показана визуализация принципов формирования оптимальной траектории.On FIG. Figure 2 shows the visualization of the principles of optimal trajectory formation.

На Фиг. 3 показано соотношение между формируемыми траекториями.On FIG. 3 shows the relationship between the formed trajectories.

На Фиг. 4 показан пример обхода препятствия.On FIG. 4 shows an example of an obstacle avoidance.

Следует понимать, что фигуры могут быть представлены схематично и не в масштабе и предназначены, главным образом, для улучшения понимания настоящего изобретения.It should be understood that the figures may be represented schematically and not to scale and are intended primarily to improve understanding of the present invention.

Подробное описаниеDetailed description

Беспилотный летательный аппарат (БПЛА) может содержать корпус, движители, двигатели, посадочное шасси, модуль управления двигателями, полетный контроллер, приемопередатчик, модуль навигации, источник энергии, камеру, набор датчиков.An unmanned aerial vehicle (UAV) may contain a body, propellers, engines, a landing gear, an engine control module, a flight controller, a transceiver, a navigation module, an energy source, a camera, a set of sensors.

БПЛА также может содержать инерциальный измерительный блок.The UAV may also contain an inertial measurement unit.

Набор датчиков может включать в себя, но без ограничения, гироскоп, акселерометр, барометр, магнитометр и т.д. Какие именно датчики установлены на БПЛА, зависит от назначения БПЛА и выполняемых им функций.The set of sensors may include, but is not limited to, a gyroscope, an accelerometer, a barometer, a magnetometer, and so on. Which sensors are installed on the UAV depends on the purpose of the UAV and the functions it performs.

Источником энергии может быть аккумулятор для подачи электрического питания на бортовые средства БПЛА. В других вариантах осуществления источником энергии может служить керосин, водород, топливный элемент, бензин, дизельное топливо или иные источники топлива, предназначенные для обеспечения энергией силовых установок и электроники БПЛА.The energy source can be a battery for supplying electrical power to the UAV onboard facilities. In other embodiments, the energy source may be kerosene, hydrogen, a fuel cell, gasoline, diesel fuel, or other fuel sources designed to power UAV propulsion systems and electronics.

Двигатели предназначены для преобразования энергии от источника энергии в механическую энергию и могут быть выполнены, например, в виде двигателей внутреннего сгорания (ДВС) или электродвигателей. Электродвигатели могут быть выполнены, например, в виде бесколлекторных двигателей.Engines are designed to convert energy from an energy source into mechanical energy and can be made, for example, in the form of internal combustion engines (ICE) or electric motors. Electric motors can be made, for example, in the form of brushless motors.

Движители могут быть выполнены в виде пропеллеров, в частности пропеллеров, вращающихся по часовой стрелке (толкающих пропеллеров), или вращающихся против часовой стрелки (тянущих пропеллеров). При этом, опционально, в БПЛА могут быть использованы толкающие и/или тянущие пропеллеры в любых сочетаниях. Пропеллеры могут быть выполнены из двух и более лопастей. Пропеллеры могут быть выполнены из металла, пластика или из композитных материалов, например углеволокна. БПЛА может быть выполнен с одним движителем (например, с толкающим пропеллером) или с несколькими движителями.The propellers can be in the form of propellers, in particular clockwise rotating propellers (pushing propellers) or counterclockwise rotating propellers (pulling propellers). At the same time, optionally, pushing and/or pulling propellers in any combination can be used in the UAV. Propellers can be made of two or more blades. Propellers can be made of metal, plastic or composite materials such as carbon fiber. The UAV can be made with a single propeller (for example, with a pusher propeller) or with multiple propellers.

Приемопередатчик - это устройство, отвечающее за прием и передачу радиосигналов, посылаемых на летательный аппарат и с него. Приемопередатчик может содержать систему передачи видео, включающую в себя передатчик видео и приемник видео.A transceiver is a device responsible for receiving and transmitting radio signals sent to and from an aircraft. The transceiver may comprise a video transmission system including a video transmitter and a video receiver.

Модуль управления двигателями управляет работой двигателя через исполнительные механизмы с целью повышения эффективности и может быть выполнен в виде электронного регулятора хода, регулятора холостого хода, регулятора фазы газораспределения и т.д.The engine control module controls the operation of the engine through actuators in order to increase efficiency and can be made in the form of an electronic speed controller, idle speed controller, valve timing controller, etc.

Модуль навигации может содержать модуль спутниковой навигации, который на основе данных, принимаемых от глобальной навигационной спутниковой системы (GNSS), такой как GPS, ГЛОНАСС, BeiDou, Galileo, QZSS, определяет местоположение (географические координаты), направление, скорость и угол рыскания БПЛА и синхронизирует время.The navigation module may include a satellite navigation module that, based on data received from a global navigation satellite system (GNSS) such as GPS, GLONASS, BeiDou, Galileo, QZSS, determines the position (geographical coordinates), direction, speed and yaw angle of the UAV and synchronizes time.

Модуль генерации опорной траектории генерирует опорную траекторию R. Могут использоваться разные подходы - например, в одном из вариантов предлагается следующий подход. Согласно полетному заданию задаются контрольные точки в трехмерном пространстве, через которые должен пролететь БПЛА, а также моменты времени, в которые БПЛА предпочтительно должен быть в каждой контрольной точке. Далее траектория между этими контрольными точками с учетом времени интерполируется с использованием однородного В-сплайна 3-го порядка на основе формул Кокса - де Бура. Сгенерированная таким образом опорная траектория R загружается в память БПЛА, и далее, выполняя свое полетное задание, он стремится следовать этой заранее заданной опорной траектории. При необходимости опорная траектория может генерироваться и не заранее, а когда БПЛА уже находится в полете. В таком случае за начальную контрольную точку может приниматься текущее местоположение БПЛА, полученное от модуля навигации, а остальные контрольные точки и моменты их прохождения извлекаются из полетного задания.The reference trajectory generation module generates the reference trajectory R. Various approaches may be used - for example, one embodiment proposes the following approach. According to the flight task, control points are set in three-dimensional space through which the UAV must fly, as well as time points at which the UAV should preferably be at each control point. Further, the trajectory between these control points, taking into account time, is interpolated using a homogeneous B-spline of the 3rd order based on the Cox-de Boer formulas. The reference trajectory R generated in this way is loaded into the UAV memory, and then, while performing its flight task, it seeks to follow this predetermined reference trajectory. If necessary, the reference trajectory can be generated not in advance, but when the UAV is already in flight. In this case, the current location of the UAV received from the navigation module can be taken as the initial control point, and the remaining control points and the moments of their passage are extracted from the flight task.

Тем не менее, на практике, особенно в условиях изменяющегося окружения, может случаться, что некоторые участки опорной траектории на момент подлета к ним БПЛА могут оказаться в области препятствия (например, на пути может оказаться переместившийся человек или транспортное средство, либо изначально траектория может быть проложена через неизвестную окружающую среду, в которой могут быть расположены стационарные объекты, такие как здания, деревья, и т.д.). Соответственно, требуется непосредственно в полете корректировать опорную траекторию, при этом стремясь по возможности максимально придерживаться ее, с тем чтобы полетное задание было выполнено, а с другой стороны, чтобы полет выполнялся безопасно как для БПЛА, так и для окружающей среды.However, in practice, especially in a changing environment, it may happen that some sections of the reference trajectory at the time the UAV approaches them may be in the area of an obstacle (for example, a person or vehicle may have moved on the path, or the trajectory may initially be routed through an unknown environment that may contain stationary objects such as buildings, trees, etc.). Accordingly, it is required to correct the reference trajectory directly in flight, while striving to adhere to it as much as possible so that the flight task is completed, and on the other hand, so that the flight is carried out safely both for the UAV and for the environment.

Далее со ссылкой на Фиг. 1 описывается работа БПЛА по оптимизации траектории полета. Для этого БПЛА может содержать дополнительные блоки, которые также упоминаются в нижеследующем описании.Next, with reference to FIG. 1 describes the operation of the UAV to optimize the flight path. For this, the UAV may contain additional blocks, which are also mentioned in the following description.

Для определения положения БПЛА в пространстве вместо модуля спутниковой навигации или в дополнение к нему модуль навигации может содержать модуль визуальной одометрии, алгоритм которой включает следующие шаги:To determine the position of the UAV in space, instead of the satellite navigation module or in addition to it, the navigation module may contain a visual odometry module, the algorithm of which includes the following steps:

- детектирование ключевых точек изображения,- detection of key points of the image,

- выявление соответствия или трекинг ключевых точек между двумя последовательными изображениями,- matching or tracking of key points between two successive images,

- оценку параметров движения на основе смещения ключевых точек.- estimation of motion parameters based on the displacement of key points.

Соответственно, БПЛА может содержать модуль визуальной одометрии. Визуальные одометры могут быть основаны на монокулярной, стерео или всенаправленной системе технического зрения. Одометр выбирает объект в кадре и отслеживает его на последовательных кадрах, вычисляя ориентацию и расстояние до объекта - например, путем триангуляции. Эти данные позволяют не только определить, в какой точке в данный момент находится БПЛА, а также определить скорость движения БПЛА и угол рыскания.Accordingly, the UAV may include a visual odometry module. Visual odometers can be based on a monocular, stereo, or omnidirectional vision system. The odometer selects an object in a frame and tracks it on successive frames, calculating the orientation and distance to the object - for example, by triangulation. These data allow not only to determine at what point the UAV is currently located, but also to determine the speed of the UAV and the angle of yaw.

Кроме того, БПЛА может содержать модуль генерации трехмерного (3D) облака точек, реализованный на базе сканера (например, лазерного, такого как лидар, на основе радиоволн, такого как радар, и т.д.) или камеры (например, монокулярной или стерео).In addition, the UAV may contain a three-dimensional (3D) point cloud generation module implemented based on a scanner (for example, laser, such as lidar, based on radio waves, such as radar, etc.) or a camera (for example, monocular or stereo ).

Данные от модуля навигации и данные от модуля генерации 3D облака точек передаются на модуль отображения, в свою очередь содержащий модуль картографирования и модуль оценки расстояний.The data from the navigation module and the data from the 3D point cloud generation module are transmitted to the display module, which in turn contains a mapping module and a distance estimation module.

Модуль картографирования на основе полученных данных строит карту объектов окружающей среды, которая может рассматриваться, по существу, как карта препятствий, поскольку БПЛА в полете должен избегать столкновения с объектами. В одном из вариантов осуществления карта объектов окружающей среды может формироваться в виде октодерева на основе облака точек - например, с помощью библиотеки octomap.Based on the received data, the mapping module builds a map of environmental objects, which can be considered, in essence, as an obstacle map, since the UAV must avoid collision with objects in flight. In one embodiment, the map of environmental objects can be formed as an octree based on a point cloud - for example, using the octomap library.

Модуль оценки расстояний оценивает расстояние от БПЛА до окружающих объектов, используя данные о местоположении БПЛА в пространстве и карту объектов окружающей среды. В одном из вариантов осуществления может в качестве расстояния может рассматриваться евклидово расстояние, и для оценки может применяться метод EDTM (построение карты расстояний на основе преобразования матрицы Евклидовых расстояний).The distance estimation module estimates the distance from the UAV to the surrounding objects using the data on the UAV's location in space and a map of environmental objects. In one embodiment, the distance may be considered a Euclidean distance, and an EDTM (Euclidean Distance Matrix Transform Distance Mapping) method may be used for estimation.

Полученные в модуле картографирования данные передаются на модуль планирования траектории. Кроме того, на модуль планирования траектории передается опорная траектория от модуля генерации опорной траектории. Соответственно, имея данные об объектах окружающей среды, данные о местоположении БПЛА в пространстве, данные о расстоянии от БПЛА до объектов окружающей среды и данные об опорной траектории, модуль планирования траектории в режиме реального времени корректирует текущую траекторию движения БПЛА с целью избегания препятствий.The data obtained in the mapping module is transmitted to the trajectory planning module. In addition, the reference trajectory from the reference trajectory generation module is transmitted to the path planning module. Accordingly, having data on environmental objects, data on the location of the UAV in space, data on the distance from the UAV to environmental objects and data on the reference trajectory, the trajectory planning module in real time corrects the current trajectory of the UAV in order to avoid obstacles.

Модуль планирования траектории содержит модуль глобального планирования и модуль локального планирования. Предлагаемый подход основан на концепции двухуровневого уточнения траектории, в которой модуль глобального планирования пытается уточнить исходную опорную траекторию, отодвигая опорную траекторию от известных препятствий, в то время как модуль локального планирования генерирует субоптимальную политику управления движением БПЛА по уточненной траектории.The trajectory planning module contains a global planning module and a local planning module. The proposed approach is based on the concept of two-level trajectory refinement, in which the global planning module attempts to refine the original reference trajectory by moving the reference trajectory away from known obstacles, while the local planning module generates a suboptimal UAV motion control policy along the refined trajectory.

В частности, модуль глобального планирования сначала выполняет попытку формирования уточненной траектории в обход препятствий на основе опорной траектории, когда она проходит либо через препятствие, либо в непосредственной близости от препятствия, а если попытка завершилась неудачей, вычисляет резервную уточненную траекторию. Модуль глобального планирования в основном фокусируется на производительности в реальном времени и обходе препятствий.In particular, the global planning module first attempts to generate a refined trajectory around obstacles based on the reference trajectory when it passes either through an obstacle or in close proximity to an obstacle, and if the attempt fails, calculates a backup refined trajectory. The global scheduling module mainly focuses on real-time performance and obstacle avoidance.

Модуль локального планирования путем прогнозирующего управления движением на основе ограниченной нелинейной модели обеспечивает безопасность, сглаживание и динамическую осуществимость уточненной траектории, формируя тем самым оптимальную траекторию и оптимальные управляющие воздействия на БПЛА.The local planning module, through predictive motion control based on a limited nonlinear model, ensures the safety, smoothing and dynamic feasibility of the refined trajectory, thereby forming the optimal trajectory and optimal control actions on the UAV.

На выходе модуля планирования траектории обеспечиваются управляющие сигналы, которые позволяют БПЛА двигаться по определенной оптимальной траектории - в частности, задаются векторы скорости в каждом из направлений х, у, z, а также угол рыскания вокруг оси z.At the output of the trajectory planning module, control signals are provided that allow the UAV to move along a certain optimal trajectory - in particular, the velocity vectors are set in each of the x, y, z directions, as well as the yaw angle around the z axis.

Данные от модуля навигации и данные от модуля планирования траектории передаются на пропорционально-дифференцирующий регулятор, который выдает отрегулированные управляющие сигналы путем сравнения текущего состояния БПЛА (координат, скорости и угла рыскания) в качестве обратной связи и оптимальных управляющих сигналов в качестве входного сигнала. В частности, регулятор формирует управляющий сигнал, являющийся суммой двух слагаемых, первое из которых пропорционально разности входного сигнала и сигнала обратной связи (сигнал рассогласования), а второе -производной сигнала рассогласования. Таким образом, учитываются рассогласования с оптимальным управляющим сигналом, вызванные внешними условиями (ветер, дождь и т.д.), неидеальностью физических исполнительных механизмов (двигатели, пропеллеры) и задержками обработки сигналов.The data from the navigation module and the data from the trajectory planning module are transmitted to a proportional-differential controller, which outputs adjusted control signals by comparing the current state of the UAV (coordinates, speed and yaw angle) as feedback and optimal control signals as an input signal. In particular, the controller generates a control signal that is the sum of two terms, the first of which is proportional to the difference between the input signal and the feedback signal (error signal), and the second is the derivative of the error signal. Thus, discrepancies with the optimal control signal caused by external conditions (wind, rain, etc.), non-ideal physical actuators (motors, propellers) and signal processing delays are taken into account.

Отрегулированные управляющие сигналы (векторы скорости в каждом из направлений х, у, z, а также угол рыскания вокруг оси z) от пропорционально-дифференцирующего (ПД) регулятора передаются на контроллер полета, который на их основе управляет двигателями, и тем самым БПЛА совершает требуемые перемещения вдоль оптимальной траектории.The adjusted control signals (velocity vectors in each of the x, y, z directions, as well as the yaw angle around the z axis) from the proportional-differentiating (PD) controller are transmitted to the flight controller, which controls the engines on their basis, and thereby the UAV performs the required moving along the optimal trajectory.

Таким образом, обеспечивается повышенная безопасность и точность следования опорной траектории, в том числе в неизвестных заранее, изменяющихся условиях.Thus, increased safety and accuracy of following the reference trajectory is provided, including in previously unknown, changing conditions.

Модуль планирования траекторииTrajectory planning module

Поскольку основными параметрами функционирования модуля планирования траектории являются сглаживание траектории, избежание препятствий и динамическая осуществимость, то целевую функцию J, выполняемую модулем планирования траектории, можно представить в следующем виде:Since the main parameters of the trajectory planning module operation are trajectory smoothing, obstacle avoidance and dynamic feasibility, the objective function J performed by the trajectory planning module can be represented as follows:

где λ - это весовые коэффициенты, определяющие вклад каждой из функции J сглаживания траектории, избегания препятствий и динамической осуществимости.where λ are weight coefficients defining the contribution of each of the function J of trajectory smoothing, obstacle avoidance, and dynamic feasibility.

Как указывалось выше, модуль глобального планирования и модуль локального планирования имеют несколько отличающиеся задачи, поэтому весовые коэффициенты в работе этих модулей по отдельности могут соответствующим образом отличаться.As mentioned above, the global scheduling module and the local scheduling module have slightly different tasks, so the weighting factors in the operation of these modules individually may differ accordingly.

Визуализация работы модуля планирования траектории показана на Фиг. 2. В момент времени t_n сам БПЛА находится в точке Q_p. Построенная модулем картографирования карта объектов окружающей среды, поступающая на вход модуля планирования траектории, обозначена символом

, при этом ввиду того, что поле зрения датчиков БПЛА является ограниченным, карта объектов окружающей среды также ограничена соответствующей областью вокруг БПЛА (показана в виде круга с центром в местоположении Q_p БПЛА). Остальное окружающее пространство неизвестно (или недостаточно известно) БПЛА, оно обозначено символом

.The visualization of the work of the trajectory planning module is shown in Fig. 2. At time t _n the UAV itself is at point Q _p. The map of environmental objects built by the mapping module, which is input to the trajectory planning module, is marked with the symbol

, while due to the fact that the field of view of the UAV sensors is limited, the map of environmental objects is also limited to the corresponding area around the UAV (shown as a circle centered on the location Q _p of the UAV). The rest of the surrounding area is unknown (or insufficiently known) to the UAV, it is indicated by the symbol

.

Затемненная область O_m обозначает известное препятствие, то есть препятствие, о наличии которого БПЛА знает, так как оно расположено внутри карты

. Затемненная область O_u обозначает неизвестное препятствие, то есть препятствие, которое существует на самом деле, но о наличии которого БПЛА не знает, так как оно расположено за пределами карты

, в неизвестной области

. Области препятствий окружены участками D_z, которые обусловлены расстоянием уклонения, то есть минимальным расстоянием, допустимым между БПЛА и препятствием для предотвращения столкновения.The shaded area O _m denotes a known obstacle, i.e. an obstacle that the UAV knows about because it is located inside the map

. The shaded area O _u denotes an unknown obstacle, i.e. an obstacle that actually exists, but the presence of which the UAV does not know, since it is located outside the map

, in an unknown area

. The obstacle areas are surrounded by sections D _z , which are determined by the avoidance distance, that is, the minimum distance allowed between the UAV and the obstacle to prevent collision.

Область F_m обозначает известное свободное пространство, то есть пространство внутри карты

, свободное от объектов (препятствий). Область F_u обозначает неизвестное свободное пространство, то есть пространство за пределами карты

, в неизвестной области

, свободное от объектов (препятствий). Соответственно, можно считать, что пространство в пределах карты

состоит из областей O_m (известных препятствий) и F_m (известного свободного пространства), а пространство в неизвестной области

состоит из областей O_u (неизвестных препятствий) и F_u (неизвестного свободного пространства). В свою очередь, карта

и неизвестная область

вместе составляют общее трехмерное пространство R³.Area F _m denotes the known free space, i.e. the space inside the map

, free from objects (obstacles). Area F _u denotes unknown free space, i.e. space outside the map

, in an unknown area

, free from objects (obstacles). Accordingly, we can assume that the space within the map

consists of regions O _m (known obstacles) and F _m (known free space), and the space in the unknown region

consists of areas O _u (unknown obstacles) and F _u (unknown free space). In turn, the card

and unknown area

together make up the common three-dimensional space R ³ .

Исходная опорная траектория R (помечена частыми точками) содержит набор контрольных точек c_i, где i=0,…, N_c, при этом число N_c задает общее количество контрольных точек в опорной траектории. Начальная контрольная точка (с индексом 0) расположена в начале траектории. Момент инициализации планирования траектории на Фиг. 2 обозначен временем t₀. Контрольная точка, соответствующая текущему моменту времени (обозначен временем t_n), показана на Фиг. 2 как точка c_s. С одной стороны, это точка, в которой должен находиться БПЛА согласно опорной траектории в момент t_n. С другой стороны, это точка, которая должна быть стартовой (начальной) для дальнейшего движения БПЛА по опорной траектории, если рассматривать движение именно начиная с момента t_n. Следует понимать, что в зависимости от времени ближайшая стартовая контрольная точка c_s может иметь индекс s от 0 до N_c-1. Уточненная траектория для момента времени t_n, которую формирует модуль глобального планирования, обозначена как

. Количество контрольных точек в пределах

обозначено как N_r. Конечная контрольная точка, показанная на Фиг. 2 как точка с_е, определяется моментом окончания уточненной траектории и может иметь индекс е от s+1 до N_c. Оптимальная траектория для момента времени t_n, которую формирует модуль локального планирования, обозначена как

.The initial reference trajectory R (marked with frequent dots) contains a set of control points c _i , where i=0,…, N _c , while the number N _c specifies the total number of control points in the reference trajectory. The initial control point (with index 0) is located at the beginning of the trajectory. The path planning initialization point in FIG. 2 is denoted by time t ₀ . The checkpoint corresponding to the current point in time (denoted by time t _n ) is shown in FIG. 2 as point c _s . On the one hand, this is the point where the UAV should be located according to the reference trajectory at the moment t _n . On the other hand, this is the point that should be the starting (initial) point for the further movement of the UAV along the reference trajectory, if we consider the movement starting from the moment t _n . It should be understood that, depending on time, the nearest starting checkpoint c _s may have an index s from 0 to N _c -1. The refined trajectory for the moment of time t _n , which is generated by the global planning module, is designated as

. Number of control points within

denoted as N _r . The end checkpoint shown in Fig. 2 as a point with _e , is determined by the end of the refined trajectory and may have an index e from s+1 to N _c . The optimal trajectory for the moment of time t _n , which is formed by the local planning module, is denoted as

.

Соотношение между опорной траекторией R, уточненной траекторией

и оптимальной траекторией

, демонстрируемое предложенным способом в реальных условиях, показано также на Фиг. 3. Как можно видеть, уточнение траектории выполняется в пределах части предстоящей опорной траектории, тогда как оптимальная траектория ищется в пределах части уточненной траектории. Это связано как с ограниченностью поля обзора датчиков и вычислительных ресурсов БПЛА, так и с тем, что уточнение и оптимизация траектории на большом отдалении от БПЛА может не иметь практического смысла в условиях изменяющейся и/или насыщенной препятствиями окружающей среды, что может приводить к растрате ресурсов обработки, при том что необходимо обеспечивать обработку в режиме реального времени.Relationship between reference trajectory R, refined trajectory

and optimal trajectory

, demonstrated by the proposed method in real conditions, is also shown in Fig. 3. As can be seen, the trajectory refinement is performed within a portion of the upcoming reference trajectory, while the optimal trajectory is sought within a portion of the refined trajectory. This is due both to the limited field of view of the sensors and computing resources of the UAV, and to the fact that the refinement and optimization of the trajectory at a great distance from the UAV may not make practical sense in a changing and/or saturated environment with obstacles, which can lead to a waste of resources. processing, while it is necessary to provide processing in real time.

Возвращаясь к Фиг. 2, расчетная разница во времени между двумя последовательными контрольными точками, т.е. между c_i и c_i+1, обозначена как δt_d. Фактическое время, затраченное на прохождение пути от момента t₀ инициализации планирования траектории до стартовой контрольной точки c_s, обозначено как δt_c. Соответственно, на практике величины δt_c и δt_d могут быть одинаковыми или могут отличаться друг от друга.Returning to Fig. 2, the estimated time difference between two successive test points, i.e. between c_i and c_i+1, denoted as δt_d. The actual time taken to complete the path from the moment t₀initialization of trajectory planning up to the starting control point c_s, denoted as δt_c. Accordingly, in practice, the values δt_c and δt_d may be the same or may differ from each other.

Далее работа модуля глобального планирования и модуля локального планирования будет раскрыта более подробно.Next, the operation of the global scheduling module and the local scheduling module will be described in more detail.

Модуль глобального планированияGlobal planning module

Главной задачей модуля глобального планирования является избежание препятствий, поэтому в упоминавшемся выше представлении (1) конкретно для этого модуля среди весовых коэффициентов λ_{сглаживание}, λ_{препятствие} и λ_{осуществимость} наибольшее значение должен иметь коэффициент λ_{препятствие}, определяя тем самым важность вклада именно функции избегания препятствий в общий результат работы модуля глобального планирования.The main task of the global planning module is to avoid obstacles, therefore, in the representation (1) mentioned above, specifically for this module, among the weight coefficients λ _smoothing , λ _obstacle and λ _feasibility , the coefficient λ _obstacle should have the highest value, thereby determining the importance of the contribution of the obstacle avoidance function to the overall result of the global planning module.

Определение направления смещения траектории.Determination of the direction of the path offset.

Некоторые контрольные точки в опорной траектории R могут находиться в области препятствий O_m, и они должны быть перемещены в свободную от препятствий область F_m. Смещение одной контрольной точки приводит к тому, что соседние с этой точкой участки траектории тоже смещаются. Пример смещения траектории показан на Фиг. 4.Some control points in the reference trajectory R may be in the obstacle area O _m and they must be moved to the obstacle-free area F _m . The displacement of one control point leads to the fact that the sections of the trajectory adjacent to this point are also displaced. An example of a path offset is shown in FIG. 4.

Здесь ряд контрольных точек c_j на опорной траектории R (показана пунктирной линией) находятся в области препятствия. Соответственно, их необходимо сместить в свободную область. На протяжении всей траектории движения БПЛА может встречаться несколько таких сегментов, которые необходимо сместить, особенно в условиях насыщенной препятствиями окружающей среды и в условиях, когда объекты окружающей среды могут перемещаться и перекрывать траекторию, изначально построенную по свободным от препятствий областям, i-й сегмент траектории, подлежащий изменению, обозначен на Фиг. 4 как

Этот сегмент состоит из набора контрольных точек c_j, их число составляет

Here, a number of control points c _j on the reference trajectory R (shown by the dotted line) are in the area of the obstacle. Accordingly, they must be shifted to a free area. Throughout the entire trajectory of the UAV, there may be several such segments that need to be shifted, especially in conditions of an environment saturated with obstacles and in conditions when environmental objects can move and overlap the trajectory originally built along obstacle-free areas, i-th segment of the trajectory to be changed is indicated in FIG. 4 as

This segment consists of a set of control points c _j , their number is

Выделение на траектории сегментов, подлежащих изменению, предлагается производить следующим образом: если на опорной траектории имеется препятствие, то проверяется, имеется ли на траектории до этого препятствия и после него предварительно заданное количество последовательных контрольных точек, находящихся в свободном пространстве. В примере на Фиг. 4 такое число равно 2, то есть на траектории встречается препятствие, до препятствия на траектории имеется 2 соседних контрольных точки в свободном пространстве, внутри препятствия на траектории расположено 4 контрольных точки, и далее после препятствия на траектории имеется еще 2 соседних контрольных точки в свободном пространстве. Соответственно, сегмент траектории, включающий в себя эти 8 контрольных точек, рассматривается в качестве подлежащего изменению сегмент

. Если видоизменить пример на Фиг. 4 так, что в нем одна точка c_j-1 находилась бы в свободном пространстве, а предыдущие и последующие контрольные точки по-прежнему оставались бы внутри препятствия, то это означало бы, что условие по предварительно заданному количеству последовательных контрольных точек за пределами препятствия не выполнено, и все равно изменению подлежал бы сегмент, включающий в себя все эти 8 контрольных точек.The selection of segments on the trajectory to be changed is proposed to be carried out as follows: if there is an obstacle on the reference trajectory, then it is checked whether the trajectory before this obstacle and after it has a predetermined number of consecutive control points located in free space. In the example in FIG. 4 this number is 2, i.e. there is an obstacle on the trajectory, there are 2 adjacent control points in free space before the obstacle on the trajectory, 4 control points are located inside the obstacle on the trajectory, and further after the obstacle on the trajectory there are 2 more adjacent control points in free space . Accordingly, the path segment including these 8 control points is considered as the segment to be modified.

. If we modify the example in Fig. 4 in such a way that one point c _j-1 would be in free space, and the previous and subsequent control points would still remain inside the obstacle, this would mean that the condition for a predetermined number of successive control points outside the obstacle does not completed, and the segment that includes all of these 8 control points would still be subject to change.

Смещение контрольных точек c_j, входящих в сегмент

в свободное пространство может повлечь за собой также смещение некоторых соседних контрольных точек - например, точки c_j-1. Тем самым, формируется уточненный сегмент траектории (показан сплошной линией, обозначен

). В примере на Фиг. 4 уточненный сегмент также содержит 8 контрольных точек.Offset of control points c _j included in the segment

into free space may also entail the displacement of some neighboring control points - for example, point c _j-1 . Thus, a refined segment of the trajectory is formed (shown by a solid line, denoted by

). In the example in FIG. 4 refined segment also contains 8 control points.

Направление смещения каждой контрольной точки определяется путем решения следующей задачи выпуклой оптимизации для каждого сегмента:The direction of displacement of each control point is determined by solving the following convex optimization problem for each segment:

Данная задача может решаться, например, с помощью решателя MOSEK.This problem can be solved, for example, using the MOSEK solver.

гдеWhere

А и b представляют область свободного пространства F_m в виде выпуклого многогранника от со до c_n в

(здесь начало подлежащего изменению сегмента для удобства обозначено с₀), как показано на Фиг. 4 в виде набора полупрозрачных прямоугольников;A and b represent the region of free space F _m in the form of a convex polyhedron from co to c _n in

(here, the beginning of the segment to be changed is denoted with ₀ for convenience), as shown in FIG. 4 as a set of translucent rectangles;

р₀, …, p_n представляют собой контрольные точки уточненного сегмента

соответствующего подлежащему изменению сегменту

используемые для определения направления градиента каждой контрольной точки;p ₀ , …, p _n are control points of the refined segment

corresponding to the segment to be changed

used to determine the direction of the gradient of each control point;

t₁ представляет собой предельное смещение начальной контрольной точки (от с₀ к р₀);t ₁ is the limit offset of the initial control point (c ₀ to p ₀ );

t₂ представляет собой предельное смещение конечной контрольной точки (от c_n к p_n);t ₂ is the limit offset of the end control point (from c _n to p _n );

t₃ представляет собой предельное суммарное расстояние между соседними промежуточными контрольными точками (то есть сумма расстояний между точками p_k и p_k+1, где k принимает значения от 1 до n-1), что в некотором приближении можно рассматривать как предельную длину уточненного сегмента траектории на участке между промежуточными контрольными точками;t ₃ is the limit total distance between adjacent intermediate control points (that is, the sum of the distances between points p _k and p _k+1 , where k takes values from 1 to n-1), which, to some extent, can be considered as the limit length of the refined segment trajectories on the section between intermediate control points;

λ₁, λ₂ и λ₃ представляют собой параметры регуляризации, определяющие вклад каждого из пределов t₁, t₂ и t₃ в общий результат. Параметры регуляризации могут быть установлены таким образом, чтобы обеспечить большее смещение в начальной и конечной контрольных точках по сравнению с промежуточными контрольными точками - например, λ₁=0,8, λ₂=0,8 и λ₃=0,6.λ ₁ , λ ₂ and λ ₃ are regularization parameters that determine the contribution of each of the limits t ₁ , t ₂ and t ₃ to the overall result. The regularization parameters can be set to provide more bias at the start and end breakpoints than intermediate breakpoints - for example, λ ₁ =0.8, λ ₂ =0.8, and λ ₃ =0.6.

Параллельная выпуклая декомпозицияParallel convex decomposition

Для уменьшения времени вычисления может использоваться параллельная версия выпуклой декомпозиции. Выпуклая декомпозиция представляет собой разделение объекта (в данном случае свободного пространства F_m, прилегающего к подлежащему изменению сегменту

) на выпуклые части. Как только определены контрольные точки с₀ … c_n, находящиеся в

необходимо проверить, имеются ли среди них промежуточные контрольные точки, которые находятся в свободном пространстве F_m. Выпуклая декомпозиция применяется к таким последовательным контрольным точкам параллельно, что в результате дает свободное пространство в виде множества решений конечной системы линейных алгебраических уравнений и неравенств, соответствующих крайним опорным гиперплоскостям (Н-rep), Ах≤b для каждого

To reduce the computation time, a parallel version of the convex decomposition can be used. A convex decomposition is a division of an object (in this case, the free space F _m adjacent to the segment to be modified

) into convex parts. As soon as the control points with ₀ … c _n are determined, which are in

it is necessary to check whether there are intermediate control points among them, which are in the free space F _m . The convex decomposition is applied to such successive control points in parallel, resulting in a free space in the form of a set of solutions to a finite system of linear algebraic equations and inequalities corresponding to the extreme support hyperplanes (H-rep), Ax≤b for each

В данном случае начальная контрольная точка со сегмента

находится в свободном пространстве и является началом первого прямоугольника (выпуклого многоугольника). Все следующие контрольные точки, которые находятся в свободном пространстве между с₀ и препятствием, также попадают в первый прямоугольник. Конечная контрольная точка c_n сегмента

находится в свободном пространстве и является концом второго прямоугольника. Первый прямоугольник и второй прямоугольник частично перекрываются между собой. Участок сегмента

между препятствием и контрольная точка c_n, в котором находится еще одна контрольная точка, не попадает во второй прямоугольник, поэтому строится третий прямоугольник, который охватывает этот участок, а также контрольную точку c_n, то есть частично перекрывается со вторым прямоугольником. Таким образом, внутри первого, второго и третьего прямоугольников, которые огибают препятствие и находятся в свободном пространстве, можно построить уточненную траекторию движения.In this case, the starting control point from the segment

is in free space and is the beginning of the first rectangle (convex polygon). All of the following control points that are in the free space between c ₀ and the obstacle also fall within the first box. End control point c _n segment

is in free space and is the end of the second rectangle. The first rectangle and the second rectangle partially overlap each other. segment plot

between the obstacle and the control point c _n , which contains another control point, does not fall into the second rectangle, so a third rectangle is built that covers this area, as well as the control point c _n , that is, it partially overlaps with the second rectangle. Thus, inside the first, second and third rectangles that go around the obstacle and are in free space, it is possible to build a refined trajectory.

Вычисление градиентовComputing gradients

Уточненная траектория

(см. Фиг. 2) в момент времени t_n может быть разбита на множество N^seg сегментов

(где i=0, N^seg), которые находятся в области препятствий и которые необходимо передвинуть в область, свободную от препятствий. Как упоминалось выше, в результате формируется множество уточненных сегментов

.Refined trajectory

(see Fig. 2) at time t _n can be divided into many N ^seg segments

(where i=0, N ^seg ) that are in the area of obstacles and which need to be moved to the area free of obstacles. As mentioned above, the result is a set of refined segments

.

Чтобы найти вектор направления градиента для контрольной точки c_j сегмента

находящейся в области препятствия, зададим вектор v₁ (см. Фиг. 4), который представляет собой аппроксимированный вектор направления движения по траектории вдоль сегмента

около точки c_j. Вектор v₁ можно представить путем движения от точки c_j-1 до точки c_j+1, которые являются соседними с точкой c_j, то есть v₁=c_j+1 - c_j-1. Также зададим контрольную точку p_k на уточненном сегменте

Вектор v₂ указывает направление смещения точки c_j к точке p_k, проходя через них и пересекая как уточненный сегмент

так и подлежащий изменению сегмент

(а именно, для упрощения вычисления - вектор v₁). Соответственно, вектор v₂ можно представить путем движения от точки c_j до точки p_k, то есть v₂=p_k - c_j.To find the gradient direction vector for the control point c _j of the segment

located in the area of the obstacle, we set the vector v ₁ (see Fig. 4), which is an approximate vector of the direction of movement along the trajectory along the segment

near the point c _j . Vector v ₁ can be represented by moving from point c _j-1 to point c _j+1 , which are adjacent to point c _j , that is, v ₁ =c _j+1 - c _j-1 . We also set a control point p _k on the refined segment

The vector v ₂ indicates the direction of displacement of the point c _j to the point p _k , passing through them and intersecting as a refined segment

and the segment to be changed

(namely, to simplify the calculation - the vector v ₁ ). Accordingly, the vector v ₂ can be represented by moving from the point c _j to the point p _k , that is, v ₂ =p _k - c _j .

Оптимальное значение k, указывающее индекс контрольной точки p_k уточненного сегмента, в которую должна быть смещена текущая точка c_j. определяется путем вычисления угла между v₁ и v₂:An optimal value of k indicating the index of the reference point p _k of the refined segment into which the current point c _j should be shifted. is determined by calculating the angle between v ₁ and v ₂ :

Таким образом, вектор градиента, соответствующий c_j, может быть полностью определен как:Thus, the gradient vector corresponding to c _j can be fully defined as:

При этом в процессе оптимизации значение k сначала может быть установлено как соответствующее середине уточненного сегмента

то есть при инициализации значение k сначала может быть равно половине числа

точек в уточненном сегменте (при необходимости округленно). Затем выполняется итеративное приращение или уменьшение значения к в соответствии с тем, дала ли текущая итерация по сравнению с предыдущей оптимизацию угла между v₁ и v₂.In this case, in the optimization process, the value of k can first be set as corresponding to the middle of the refined segment

that is, during initialization, the value of k may initially be equal to half the number

points in the refined segment (rounded if necessary). Then, k is iteratively incremented or decremented according to whether the current iteration, compared to the previous one, has optimized the angle between v ₁ and v ₂ .

После того, как выполнено определение вектора градиента для каждого i-го сегмента, определяется функция J_{препятствие}, упоминавшаяся выше в выражении (1), в качестве непрерывно дифференцируемой штрафной функции:After the determination of the gradient vector for each i-th segment, the function J _obstacle , mentioned above in expression (1), is determined as a continuously differentiable penalty function:

Как упоминалось выше, D_z представляет собой предварительно заданное расстояние уклонения, то есть минимальное расстояние, допустимое между БПЛА и препятствием для предотвращения столкновения. В настоящем изобретении считается, что БПЛА находится в контрольной точке, когда его центр находится в контрольной точке. Соответственно, во избежание столкновения с препятствием контрольная точка должна отстоять от препятствия больше, чем на радиус БПЛА (с учетом вращения лопастей). В качестве неограничивающего примера, расстояние D_z уклонения может быть задано равным 0,8 м.As mentioned above, D _z is a predetermined avoidance distance, that is, the minimum distance allowed between the UAV and an obstacle to avoid collision. In the present invention, the UAV is considered to be at the reference point when its center is at the reference point. Accordingly, in order to avoid a collision with an obstacle, the control point must be more than the radius of the UAV (taking into account the rotation of the blades) from the obstacle. As a non-limiting example, the evasion distance D _z may be set to 0.8 m.

Мертвая зонаDead zone

Когда датчик глубины имеет малый угол обзора, построение карты может быть не вполне точным, так как сразу за пределами угла обзора может находиться препятствие, и область D_z вокруг препятствия может ошибочно рассматриваться как область, свободная от препятствий. Кроме того, метод EDTM занимает значительное количество времени, когда окружающая среда загромождена (насыщена препятствиями), а из-за того, что существует ограничение по времени вычислений (обработка должна вестись в реальном времени), метод может не успеть отработать. В таких ситуациях сегментация свободного пространства может не обеспечить корректный набор ограничений А, b, и уточненная траектория может привести БПЛА Q_p в область D_z. Таким образом, предлагавшийся выше подход со сдвигом сегмента и поиском контрольных точек уточненного сегмента может не определить должным образом контрольные точки p_k, т.е. оцененные контрольные точки могут оказаться на концах (с₀, c_Ni^seg) сегмента

и такая траектория

может оказаться динамически неосуществимой. Следовательно, требуется защитный механизм, который позволил бы построить уточненную траекторию в обход препятствия, даже если вышеуказанный подход выдал неудовлетворительный результат. Для таких случаев предлагается следующий механизм восстановления, при котором вся траектория рассматривается целиком как один-единственный сегмент, который необходимо сместить в свободное пространство:When the depth sensor has a small field of view, the mapping may not be completely accurate, since there may be an obstacle immediately outside the field of view, and the area D _z around the obstacle may be mistakenly considered an area free of obstacles. In addition, the EDTM method takes a significant amount of time when the environment is cluttered (saturated with obstacles), and due to the fact that there is a calculation time limit (processing must be carried out in real time), the method may not have time to work out. In such situations, the segmentation of free space may not provide the correct set of restrictions A, b, and the refined trajectory may lead the UAV Q _p to the area D _z . Thus, the approach proposed above with segment shift and search for control points of the refined segment may not properly determine the control points p _k , i.e. estimated breakpoints may end up at the ends (with ₀ , c _N i ^seg ) of the segment

and such a trajectory

may not be dynamically feasible. Therefore, a protective mechanism is required that would allow building an improved trajectory around the obstacle, even if the above approach gave an unsatisfactory result. For such cases, the following recovery mechanism is proposed, in which the entire trajectory is considered as a whole as a single segment that must be shifted into free space:

Данная задача может решаться, например, с помощью решателя MOSEK.This problem can be solved, for example, using the MOSEK solver.

где

- это контрольные точки, подлежащие перемещению, при этом

a N_r - это количество контрольных точек в момент времени t_n на траектории

.Where

are the control points to be moved, while

a N _r is the number of control points at time t _n on the trajectory

.

В остановленные контрольные точки определяются с помощью

The stopped breakpoints are defined using

A_r и b_r определяются методом параллельной выпуклой декомпозиции.A _r and b _r are determined by the method of parallel convex decomposition.

СглаживаниеSmoothing

Чтобы определить функцию J_{сглаживание}, упоминавшуюся выше в выражении (1), во избежание эффектов вибрации в общем случае помимо скорости необходимо учитывать другие производные от положения высшего порядка, такие как ускорение, рывок и т.д. Настоящее изобретение направлено на оптимизацию траектории при движении БПЛА на относительно низкой скорости (до 1,5-2 м/с), поэтому такая производная, как рывок, и производные высшего порядка являются пренебрежимо малыми и могут не учитываться.In order to determine the _smoothing function J mentioned above in expression (1), in order to avoid vibration effects, in general, in addition to speed, other derivatives of the higher order position, such as acceleration, jerk, etc., must be taken into account. The present invention is aimed at optimizing the trajectory when the UAV moves at a relatively low speed (up to 1.5-2 m/s), so such a derivative as jerk and higher order derivatives are negligible and can be ignored.

Таким образом, для обеспечения плавности функция сглаживания (J_{сглаживание}) может быть сформулирована только как минимизация составляющих ускорения:Thus, to ensure smoothness, the smoothing function (J _smoothing ) can only be formulated as a minimization of the acceleration components:

где

Where

a_i - это ускорение (a_i=c_i+2 - 2c_i+1+c_i) при i-м индексе

,a _i is the acceleration (a _i =c _i+2 - 2c _i+1 +c _i ) at the i-th index

,

v_i - это скорость (v_i=c_i+1 - c_i) при i-м индексе

,v _i is the speed (v _i =c _i+1 - c _i ) at the i-th index

,

c_i - это контрольная точка при i-м индексе

.c _i is the breakpoint at the i-th index

.

ОсуществимостьFeasibility

Чтобы уточненная траектория была динамически осуществима (то есть чтобы БПЛА мог на практике осуществить маневр, исходя из своих технических возможностей), соответствующая функция осуществимости (см. J_{осуществимость} в выражении (1) выше) накладывает штраф за компоненты скорости и ускорения только тогда, когда они превышают минимальные и максимальные пределы, следующим образом:In order for the refined trajectory to be dynamically feasible (i.e., so that the UAV can actually perform a maneuver based on its technical capabilities), the corresponding feasibility function (see J _feasibility in expression (1) above) imposes a penalty on the velocity and acceleration components only when they exceed the minimum and maximum limits, as follows:

где оператор ⊕ определяется следующим образом:where the operator ⊕ is defined as follows:

при этом допустимые максимальные пределы скорости и ускорения задаются через v_max и a_max, соответственно, а минимальные пределы - через - v_max и -a_max. Когда скорость и ускорение находятся в пределах допустимого диапазона, штраф не накладывается.in this case, the permissible maximum speed and acceleration limits are specified through v _max and a _max , respectively, and the minimum limits - through - v _max and -a _max . When the speed and acceleration are within the allowable range, no penalty is applied.

Вычисление целевой функцииCalculation of the objective function

Таким образом, выполнив приведенные выше этапы, можно определить целевую функцию J (см. выражение (1)), включающую в себя функции сглаживания траектории (J_{сглаживание}), избегания препятствий (J_{препятствие}) и динамической осуществимости (J_{осуществимость}). для решения целевой функции J может использоваться, например, метод L-BFGS-B (алгоритм Бройдена -Флетчера - Гольдфарба - Шанно с ограниченным использованием памяти в многомерном кубе).Thus, by completing the above steps, it is possible to determine the objective function J (see expression (1)), which includes the functions of trajectory smoothing (J _smoothing ), obstacle avoidance (J _obstacle ) and dynamic feasibility (J _feasibility ). to solve the objective function J, for example, the L-BFGS-B method (Broyden-Fletcher-Goldfarb-Shanno algorithm with limited memory usage in a multidimensional cube) can be used.

Модуль локального планированияLocal planning module

В настоящем изобретении локальное планирование основано на решении задачи нелинейной оптимизации, в частности NMPC. Время вычисления NMPC увеличивается, когда увеличивается количество ограничений, то есть когда траектория пролегает вблизи препятствий или через них. Такая ситуация может привести к попаданию в локальный минимум и к неудаче при поиске оптимальной политики управления БПЛА, что в конечном счете может вылиться в столкновение с препятствием. Однако настоящее изобретение лишено данного недостатка, так как модуль глобального планирования уточняет исходную опорную траекторию, отодвигая ее от области препятствий.In the present invention, local scheduling is based on solving a non-linear optimization problem, in particular NMPC. NMPC calculation time increases when the number of constraints increases, that is, when the trajectory passes near or through obstacles. Such a situation can lead to falling into a local minimum and failure in the search for the optimal UAV control policy, which ultimately can result in a collision with an obstacle. However, the present invention does not have this disadvantage, since the global planning module refines the original reference trajectory, moving it away from the obstacle area.

Кроме того, тот факт, что траектория разбита на множество отрезков от контрольной точки до контрольной точки, между которыми БПЛА проходит относительно небольшое расстояние за относительно небольшой промежуток времени, ограничения на движение, накладываемые такими отрезками, по отдельности являются не очень сложными, вследствие чего метод NMPC может успешно применяться к решению задач оптимизации на таких отрезках в режиме реального времени.In addition, the fact that the trajectory is divided into many segments from the checkpoint to the checkpoint, between which the UAV passes a relatively small distance in a relatively short period of time, the restrictions on movement imposed by such segments are individually not very complex, as a result of which the method NMPC can be successfully applied to solving optimization problems on such segments in real time.

Соответственно, модуль локального планирования в настоящем изобретении может сосредоточиться на задачах обеспечения сглаживания, динамической осуществимости и точности следования уточненной траектории. Вследствие этого в упоминавшемся выше представлении (1) конкретно для этого модуля среди весовых коэффициентов J_{сглаживание}, λ_{препятствие} и λ_{осуществимость} наибольшее значение должны иметь коэффициенты λ_{сглаживание} и λ_{осуществимость}, определяя тем самым важность вклада именно функций сглаживания траектории и динамической осуществимости в целевую функцию J.Accordingly, the local scheduling module in the present invention can focus on the tasks of providing smoothing, dynamic feasibility, and accuracy of the refined trajectory. As a result, in the representation (1) mentioned above, specifically for this module, among the weight coefficients J _smoothing , λ _obstacle and λ _feasibility , the coefficients λ _smoothing and λ _feasibility should have the highest value, thereby determining the importance of the contribution of the trajectory smoothing functions and dynamic feasibility to the objective function J.

В момент времени t_n модуль локального планирования формирует оптимальную траекторию

для заданного горизонта планирования, то есть для того количества N_p контрольных точек, которое подлежит обработке модулем локального планирования.At time t _n, the local planning module generates the optimal trajectory

for a given planning horizon, that is, for the number N _p of checkpoints to be processed by the local planning module.

где Q_p - это текущее местоположение БПЛА, а с - это последующие контрольные точки на траектории, которые должен пройти БПЛА.where Q _p is the current location of the UAV, and c are the next control points on the trajectory that the UAV must pass.

Модуль локального планирования генерирует оптимальное управление для маневрирования БПЛА с учетом накладываемых ограничений, где w=[u_tn, …, u_tn+Np-1, x_tn,…, x_tn+Np] обозначает набор подлежащих минимизации переменных решения. Здесь x_tn - это вектор состояния, определяемый с учетом направлений х, у, z и угла рыскания для каждой контрольной точки на уточненной траектории, полученной от модуля глобального планирования, а u_tn - это входные сигналы управления (управляющие воздействия). Соответственно, задачей модуля локального планирования является одновременная оптимизация как входных сигналов управления, так и векторов состояний. Такая задача (целевая функция J_P(x,u)_tn) может быть сформирована с использованием метода многократной стрельбы следующим образом:The local planning module generates the optimal control for UAV maneuvering, taking into account the imposed restrictions, where w=[u _tn , …, u _tn+Np-1 , x _tn ,…, x _tn+Np ] denotes the set of decision variables to be minimized. Here, x _tn is a state vector determined considering the x, y, z directions and yaw angle for each control point on the refined trajectory received from the global planning module, and u _tn are control inputs (control actions). Accordingly, the task of the local scheduling module is to simultaneously optimize both input control signals and state vectors. Such a task (objective function J _P (x,u) _tn ) can be formed using the multiple shooting method as follows:

Функция g₁(w) указывает ограничения, накладываемые на движение динамикой системы, а функция g₂(w) указывает ограничения, накладываемые на движение приближающимися препятствиями. x_min и x_max обозначают минимальную и максимальную границу для вектора состояния

, где

изменяется в пределах от 0 до N_p. v_min и v_max обозначают минимальную и максимальную линейную и угловую скорости, допустимые для движения данного БПЛА.The function g ₁ (w) indicates the restrictions imposed on the movement by the dynamics of the system, and the function g ₂ (w) indicates the restrictions imposed on the movement by approaching obstacles. x _min and x _max denote the minimum and maximum bound for the state vector

, Where

varies from 0 to N _p . v _min and v _max denote the minimum and maximum linear and angular speeds allowed for the movement of this UAV.

Модуль локального планирования, как и модуль глобального планирования, работает итеративно. Когда завершается одна итерация с пересчетом траектории, немедленно или через некоторый промежуток времени начинается следующая. На каждой итерации планирования модуль локального планирования получает Q_p, x_tn и u_tn в качестве входных данных и дает на выходе оптимальную политику управления, т.е.

где v обозначает скорость в каждом направлении х, у, z, а ω обозначает угол рыскания вокруг оси z.The local planning module, like the global planning module, works iteratively. When one iteration with trajectory recalculation is completed, the next one starts immediately or after a certain period of time. At each scheduling iteration, the local scheduling module takes Q _p , x _tn and u _tn as input and produces the optimal control policy as output, i.e.

where v denotes the speed in each direction x, y, z, and ω denotes the yaw angle around the z-axis.

Таким образом, в результате обработки, выполненной в модуле глобального планирования и в модуле локального планирования, формируется оптимальная траектория, по которой должен следовать БПЛА, и управляющие сигналы, которые позволяют БПЛА двигаться в точности по заданной траектории.Thus, as a result of processing performed in the global planning module and in the local planning module, an optimal trajectory is formed that the UAV should follow, and control signals that allow the UAV to move exactly along the given trajectory.

Данные от модуля визуальной одометрии и данные от модуля планирования траектории передаются на пропорционально-дифференцирующий регулятор. Конечным результатом обработки, связанной с оптимизацией траектории, является подача отрегулированных управляющих сигналов (векторов скорости в каждом из направлений х, у, z, а также угла рыскания вокруг оси z) от пропорционально-дифференцирующего регулятора на контроллер полета, который на их основе управляет двигателями, и тем самым БПЛА совершает требуемые перемещения вдоль оптимальной траектории.Data from the visual odometry module and data from the trajectory planning module are transmitted to the proportional-derivative controller. The end result of the processing associated with trajectory optimization is the supply of adjusted control signals (velocity vectors in each of the x, y, z directions, as well as the yaw angle around the z axis) from the proportional-differential controller to the flight controller, which, on their basis, controls the engines , and thus the UAV performs the required movements along the optimal trajectory.

СинхронизацияSynchronization

Обработка данных в каждом модуле выполняется итеративно. Когда завершается одна итерация, немедленно или через некоторый промежуток времени начинается следующая.Data processing in each module is performed iteratively. When one iteration is completed, the next one begins immediately or after a certain period of time.

В настоящем изобретении для получения конечного результата применяется множество модулей. В условиях неизвестного и/или меняющегося окружения и вследствие этого меняющихся входных данных отдельно взятый модуль выполняет обработку в течение разных итераций за разное время. Соответственно, невозможно заранее определить, сколько в точности времени потребуется тому или иному модулю для выполнения обработки.In the present invention, a plurality of modules are used to obtain the final result. Under conditions of an unknown and/or changing environment and consequently changing input data, a single module performs processing during different iterations at different times. Accordingly, it is impossible to determine in advance exactly how long it will take a particular module to perform processing.

В одном из вариантов осуществления итерации в каждом модуле выполняются непрерывно. Результат обработки последней итерации сохраняется в памяти. Когда модуль завершает обработку, он немедленно приступает к следующей итерации, при этом входные данные он получает не непосредственно от того модуля, данные которого ему необходимы для обработки, а из памяти, извлекая из нее результат обработки последней итерации необходимого модуля. Тем самым, обеспечивается простой механизм обновления данных, в котором актуализация данных выполняется непрерывно. Недостаток такого подхода заключается в том, что время обработки одного модуля может сильно отличаться от времени обработки другого модуля, из-за чего за один и тот же промежуток времени первый модуль может отработать один раз, а второй модуль, который поставляет данные для первого - например, 8 раз, из которых 7 окажутся бесполезными, что вызывает растрату вычислительных ресурсов и энергии. Кроме того, на практике сохраненные в памяти данные иногда успевают утратить актуальность, что может отрицательно сказаться на точности определения траектории и безопасности полета.In one embodiment, the iterations in each module are performed continuously. The result of processing the last iteration is stored in memory. When the module completes processing, it immediately proceeds to the next iteration, while receiving input data not directly from the module whose data it needs to process, but from memory, extracting from it the result of processing the last iteration of the required module. This provides a simple data update mechanism in which the data is updated continuously. The disadvantage of this approach is that the processing time of one module can be very different from the processing time of another module, due to which, in the same period of time, the first module can work once, and the second module, which supplies data for the first - for example , 8 times, of which 7 will be useless, which causes a waste of computing resources and energy. In addition, in practice, the data stored in memory sometimes lose their relevance, which can adversely affect the accuracy of determining the trajectory and flight safety.

В другом варианте осуществления один или более модулей выполняют итерации с предварительно заданной периодичностью. Например, модуль локального планирования начинает новую итерацию с периодом, равным 80 мс. Если обработка завершилась за 50 мс, то следующая итерация начнется через 30 мс. Если выбранный метод обработки позволяет устанавливать ограничение на время обработки, то в качестве ограничения может задаваться время обработки, которое равно или меньше периода обработки. Если выбранный метод обработки не позволяет устанавливать ограничение на время обработки, то в качестве периода обработки может задаваться значение, равное максимальному или среднему максимальному времени обработки, продемонстрированному данным модулем во время испытаний для схожих условий применения. В случае превышения времени обработки и отсутствия текущих выходных данных следующий модуль использует в качестве входных данных результат обработки последней итерации данного модуля.In another embodiment, one or more modules iterate at predetermined intervals. For example, the local scheduling module starts a new iteration with a period of 80 ms. If processing completed in 50ms, then the next iteration will start in 30ms. If the selected processing method allows you to set a limit on the processing time, then the limit can be set to a processing time that is equal to or less than the processing period. If the selected processing method does not allow a limit on the processing time, then the processing period can be set to a value equal to the maximum or average maximum processing time demonstrated by this module during tests for similar conditions of use. If the processing time is exceeded and there is no current output, the next module uses as input the result of processing the last iteration of this module.

Соответственно, зная максимальный интервал обработки среди всех модулей, можно установить такой же интервал обработки для всех модулей. Например, если максимальный интервал обработки среди всех модулей имеет модуль планирования траектории, и его значение равно 100 мс, то значение 100 мс может быть назначено в качестве интервала обработки для всех модулей, и все модули могут начинать обработку в один и тот же момент времени. Тем самым, обеспечивается предотвращение растраты ресурсов из-за расхождения во времени обработки.Accordingly, knowing the maximum processing interval among all modules, it is possible to set the same processing interval for all modules. For example, if the maximum processing interval among all modules has a path planning module and its value is 100 ms, then a value of 100 ms can be assigned as a processing interval for all modules, and all modules can start processing at the same time. Thereby, it is ensured that resources are not wasted due to differences in processing time.

В другом варианте осуществления, зная, какой модуль имеет максимальный интервал обработки среди всех модулей, можно установить такой же интервал обработки для тех модулей, которые поставляют данные только этому модулю. Например, модуль генерации опорной траектории поставляет данные только в модуль планирования траектории, поэтому его интервал обработки может быть установлен равным интервалу обработки модуля планирования траектории, тогда как модуль навигации поставляет данные сразу в несколько модулей, поэтому его интервал обработки может устанавливаться равным наименьшему из интервалов обработки тех модулей, которым он поставляет данные - например, интервалу обработки ПД-регулятора. В отличие от предыдущего варианта, в данном варианте обеспечивается повышение актуальности данных, что снижает задержку обработки и вызванные этим ошибки отклонения от оптимальной траектории, а также повышает безопасность полета.In another embodiment, knowing which module has the maximum processing interval among all modules, one can set the same processing interval for those modules that only supply data to that module. For example, the reference path generation module only supplies data to the path planning module, so its processing interval can be set equal to the processing interval of the path planning module, while the navigation module supplies data to several modules at once, so its processing interval can be set to the smallest of the processing intervals. those modules to which it supplies data - for example, the processing interval of a PD controller. Unlike the previous option, this option provides more up-to-date data, which reduces the processing delay and the errors of deviation from the optimal trajectory caused by this, and also improves flight safety.

В другом варианте осуществления может быть сформировано расписание работы каждого модуля, так чтобы к тому моменту, когда начнет работу тот модуль, который потребляет информацию от данного модуля, данный модуль успевал выполнить обработку и выдать относительно актуальный результат. Например, модуль локального планирования имеет среднеквадратическое максимальное время обработки 70 мс, в соответствии с которым устанавливается его период обработки; модуль глобального планирования имеет среднеквадратическое максимальное время обработки 50 мс. Зная момент, когда модуль локального планирования начнет новую итерацию, можно установить время начала работы модуля глобального планирования, так чтобы он завершал свою обработку непосредственно перед началом обработки в модуле локального планирования (то есть с упреждением в 50-55 мс), чтобы предоставить ему более актуальные данные, чем если бы итерация запускалась одновременно, и после завершения обработки модулем глобального планирования данные ожидали бы обработки модулем локального планирования на следующей итерации около 20 мс. Таким образом, обеспечивается дополнительное повышение актуальности данных, что снижает задержку обработки и вызванные этим ошибки отклонения от оптимальной траектории, а также повышает безопасность полета.In another embodiment, the operation schedule of each module can be generated so that by the time the module that consumes information from this module starts working, this module has time to process and produce a relatively up-to-date result. For example, the local scheduling module has an RMS maximum processing time of 70 ms, according to which its processing period is set; the global scheduling module has an RMS maximum processing time of 50 ms. Knowing the moment when the local scheduling module starts a new iteration, you can set the start time of the global scheduling module so that it finishes its processing just before the local scheduling module starts processing (that is, with a lead of 50-55 ms) to give it more up-to-date data than if the iteration started at the same time, and after the completion of processing by the global scheduling module, the data would wait for processing by the local scheduling module at the next iteration for about 20 ms. In this way, an additional increase in the relevance of the data is provided, which reduces the processing delay and the deviation errors caused by this deviation from the optimal trajectory, and also improves flight safety.

РасстояниеDistance

Выше в отношении Фиг. 2 указывалось, что расчетная разница во времени между двумя последовательными контрольными точками на траектории обозначена как δt_d. Данное значение предварительно задается в зависимости от требований конкретного применения. Например, значение δt_d может быть установлено равным 0,05 с. Соответственно, расстояние, которое должен преодолеть БПЛА за это время, может отличаться между разными соседними точками на траектории. Чем меньше время пролета между двумя последовательными контрольными точками, тем больше точек содержит траектория и тем ближе оптимальная траектория будет к опорной, но тем выше будет сложность вычислений.Above with respect to FIG. 2 it was indicated that the estimated time difference between two successive control points on the trajectory is denoted as δt _d . This value is preset depending on the requirements of the particular application. For example, the value of δt _d may be set to 0.05 s. Accordingly, the distance that the UAV must overcome during this time may differ between different neighboring points on the trajectory. The shorter the flight time between two successive control points, the more points the trajectory contains and the closer the optimal trajectory will be to the reference one, but the higher the computational complexity will be.

В настоящем изобретении предлагается устанавливать время пролета между соседними контрольными точками приблизительно равным или несколько большим, чем среднее максимальное время обработки модуля глобального планирования. Таким образом, за одну итерацию модуль глобального планирования успевает обработать текущие условия окружения при приближении к следующей контрольной точке, чтобы при необходимости скорректировать траекторию. Это позволяет учитывать каждую следующую контрольную точку без потери данных и сильного отклонения от траектории, и с другой стороны, без выполнения излишней обработки. Таким образом, обеспечивается повышение точности обработки и безопасности полета.The present invention proposes to set the time-of-flight between adjacent checkpoints to be approximately equal to or somewhat greater than the average maximum processing time of the global scheduling module. Thus, in one iteration, the global planning module manages to process the current environmental conditions when approaching the next control point in order to correct the trajectory if necessary. This allows each successive control point to be taken into account without data loss and severe deviation from the trajectory, and on the other hand, without excessive processing. Thus, an increase in processing accuracy and flight safety is ensured.

На практике при облете препятствий расстояние между контрольными точками может увеличиваться, и предварительно заданный постоянный период обработки может привести к тому, что начало итерации может оказываться на разном расстоянии от ближайшей точки, из-за чего может быть утрачена синхронизация. Во избежание этого в одном из вариантов осуществления обработка инициируется событием. Например, при приближении к контрольной точке на предварительно заданное расстояние (или на предварительно заданное время подлета) по меньшей мере один модуль (например, модуль глобального планирования) может начинать новую итерацию, если обработка предыдущей итерации уже была ранее завершена. Это позволяет учитывать каждую следующую контрольную точку без потери данных и сильного отклонения от траектории, а также повысить адаптируемость к изменяющимся условиям окружающей среды. Таким образом, обеспечивается повышение точности обработки и безопасности полета.In practice, when flying over obstacles, the distance between checkpoints can increase, and the predetermined constant processing period can cause the iteration start to be at a different distance from the nearest point, which may cause loss of synchronization. To avoid this, in one embodiment, processing is triggered by an event. For example, when approaching a checkpoint at a predetermined distance (or predetermined approach time), at least one module (eg, a global planning module) may start a new iteration if the processing of the previous iteration has already been previously completed. This makes it possible to take into account each successive control point without data loss and severe deviation from the trajectory, as well as improve adaptability to changing environmental conditions. Thus, an increase in processing accuracy and flight safety is ensured.

При описании работы модуля локального планирования выше указывались выражения, которые вычисляются в процессе обработки в этом модуле. Исходя из этих выражений и известных особенностей метода NMPC было бы логично предположить, что чем больше горизонт планирования, то есть то количество контрольных точек, которое подлежит обработке модулем локального планирования, тем сложнее обработка и тем больше времени занимает одна итерация. Тем не менее, испытания предложенного изобретения неожиданно показали, что при горизонтах планирования, равных около 4,5-6 (предпочтительно 5) диаметров БПЛА (считая от текущего местоположения Q_p, то есть от центра БПЛА), по сравнению с меньшими и большими горизонтами планирования обеспечивается повышение частоты успешного (без сбоев) выполнения полетного задания и даже уменьшение среднего времени обработки при поддержании схожего общего расстояния, пройденного БПЛА от начальной до конечной точки.When describing the operation of the local planning module, the expressions that are calculated during processing in this module were indicated above. Based on these expressions and the well-known features of the NMPC method, it would be logical to assume that the larger the planning horizon, that is, the number of control points that must be processed by the local planning module, the more difficult the processing and the longer one iteration takes. However, tests of the proposed invention unexpectedly showed that with planning horizons equal to about 4.5-6 (preferably 5) UAV diameters (counting from the current location Q _p , that is, from the center of the UAV), compared with smaller and larger horizons planning provides an increase in the frequency of successful (without failures) execution of a flight task and even a decrease in the average processing time while maintaining a similar total distance traveled by the UAV from the start to the end point.

ПримерExample

Для испытаний настоящего изобретения использовался БПЛА ЭЛ Matrice 100, который содержал следующие модули: модуль генерации 3D облака точек на базе прикрепленного к БПЛА лидара Velodyne Lite 16 для анализа окружающей среды, модули отображения, генерации опорной траектории, планирования траектории и ПД-регулятора на базе прикрепленного к БПЛА процессора Nvidia Jetson NX, встроенный в БПЛА модуль GPS-навигации и контроллер полета DJI A3. Максимальная скорость БПЛА была установлена равной 1,2 м/с, горизонт планирования 4 м, время между контрольными точками 0,05 с, расстояние уклонения 0,8 м, исходная опорная траектория 54,3 м. Итоговая оптимальная траектория полета из-за наличия препятствий на пути составила 79,8 м, общее время полета около 150 с, среднее отклонение от траектории менее 1 м в течение всего полета.For testing the present invention, an EL Matrice 100 UAV was used, which contained the following modules: a module for generating a 3D point cloud based on a Velodyne Lite 16 lidar attached to the UAV for environmental analysis, modules for mapping, generating a reference trajectory, trajectory planning and a PD controller based on an attached Velodyne Lite 16 lidar to the Nvidia Jetson NX processor UAV, the UAV’s built-in GPS navigation module and the DJI A3 flight controller. The maximum UAV speed was set to 1.2 m/s, planning horizon 4 m, time between fixes 0.05 s, avoidance distance 0.8 m, initial reference trajectory 54.3 m. obstacles on the way was 79.8 m, the total flight time was about 150 s, the average deviation from the trajectory was less than 1 m during the entire flight.

ПрименениеApplication

Устройства, системы и способы согласно настоящему изобретению можно использовать для автономного перемещения по опорной траектории БПЛА, выполняющих доставку грузов, различные исследования, анализ окружающего пространства, контроль объектов, фото- и видеосъемку, спасательные операции, автономный мониторинг для задач охраны территорий, сельхозугодий, картографии, дистанционного химико-физического анализа, контроля всхожести и спелости урожая, химической обработки и т.д.Devices, systems and methods according to the present invention can be used for autonomous movement along the reference trajectory of UAVs performing cargo delivery, various studies, analysis of the surrounding space, object control, photo and video filming, rescue operations, autonomous monitoring for the tasks of protecting territories, farmland, cartography , remote chemical-physical analysis, control of germination and ripeness of the crop, chemical treatment, etc.

Также возможно применение настоящего изобретения в пилотируемых летательных аппаратах в режиме автономного движения (автопилота).It is also possible to use the present invention in manned aircraft in the mode of autonomous movement (autopilot).

Дополнительные особенности реализацииAdditional Implementation Features

Различные иллюстративные блоки и модули, описанные в связи с раскрытием сущности в данном документе, могут реализовываться или выполняться с помощью процессора общего назначения, процессора цифровых сигналов (DSP), специализированной интегральной схемы (ASIC), программируемой пользователем вентильной матрицы (FPGA) или другого программируемого логического устройства (PLD), дискретного логического элемента или транзисторной логики, дискретных аппаратных компонентов либо любой комбинации вышеозначенного, предназначенной для того, чтобы выполнять описанные в данном документе функции. Процессор общего назначения может представлять собой микропроцессор, но в альтернативном варианте, процессор может представлять собой любой традиционный процессор, контроллер, микроконтроллер или конечный автомат. Процессор также может реализовываться как комбинация вычислительных устройств (к примеру, комбинация DSP и микропроцессора, несколько микропроцессоров, один или более микропроцессоров вместе с DSP-ядром либо любая другая подобная конфигурация).Various illustrative blocks and modules described in connection with the disclosure herein may be implemented or executed by a general purpose processor, digital signal processor (DSP), application specific integrated circuit (ASIC), field programmable gate array (FPGA), or other programmable logic device (PLD), discrete logic element or transistor logic, discrete hardware components, or any combination of the foregoing, designed to perform the functions described in this document. A general purpose processor may be a microprocessor, but in the alternative, the processor may be any conventional processor, controller, microcontroller, or state machine. A processor may also be implemented as a combination of computing devices (eg, a combination of a DSP and a microprocessor, multiple microprocessors, one or more microprocessors together with a DSP core, or any other similar configuration).

Некоторые блоки или модули по отдельности или вместе могут представлять собой, например, компьютер, и включать в себя процессор, который сконфигурирован для вызова и выполнения компьютерных программ из памяти для выполнения этапов способа или функций блоков или модулей в соответствии с вариантами осуществления настоящего изобретения. Согласно вариантам осуществления, устройство может дополнительно включать в себя память. Процессор может вызывать и выполнять компьютерные программы из памяти для выполнения способа. Память может быть отдельным устройством, независимым от процессора, или может быть интегрирована в процессор. Память может хранить код, инструкции, команды и/или данные для исполнения на наборе из одного или более процессоров описанного устройства. Коды, инструкции, команды могут предписывать процессору выполнять этапы способа или функции устройства.Some blocks or modules, individually or collectively, may be, for example, a computer, and include a processor that is configured to call and execute computer programs from memory to perform method steps or functions of blocks or modules in accordance with embodiments of the present invention. According to embodiments, the device may further include a memory. The processor may call and execute computer programs from memory to execute the method. The memory may be a separate device independent of the processor or may be integrated with the processor. The memory may store code, instructions, commands, and/or data for execution on a set of one or more processors of the device described. Codes, instructions, commands may direct the processor to perform the steps of a method or device function.

Функции, описанные в данном документе, могут реализовываться в аппаратном обеспечении, программном обеспечении, выполняемом посредством одного или более процессоров, микропрограммном обеспечении или в любой комбинации вышеозначенного, если это применимо. Аппаратные и программные средства, реализующие функции, также могут физически находиться в различных позициях, в том числе согласно такому распределению, что части функций реализуются в различных физических местоположениях, то есть может выполняться распределенная обработка или распределенные вычисления.The functions described in this document may be implemented in hardware, software running on one or more processors, firmware, or any combination of the foregoing, as applicable. The hardware and software implementing the functions may also be physically located in different locations, including such a distribution that parts of the functions are implemented in different physical locations, that is, distributed processing or distributed computing may be performed.

В случае если объем данных велик, может производиться многопоточная обработка данных, которая в простом представлении может выражаться в том, что все множество подлежащих обработке данных разделяется на набор подмножеств, и каждое ядро процессора выполняет обработку в отношении назначенного для него подмножества данных.If the amount of data is large, multi-threaded data processing can be performed, which in a simple representation can be expressed in the fact that the entire set of data to be processed is divided into a set of subsets, and each processor core performs processing in relation to its assigned data subset.

Вышеупомянутая память может быть энергозависимой или энергонезависимой памятью или может включать в себя как энергозависимую, так и энергонезависимую память. Специалисту в области техники должно быть также понятно, что, когда речь идет о памяти и о хранении данных, программ, кодов, инструкций, команд и т.п., подразумевается наличие машиночитаемого (или компьютерно-читаемого, процессорно-читаемого) запоминающего носителя. Машиночитаемый запоминающий носитель может представлять собой любой доступный носитель, который может использоваться для того, чтобы переносить или сохранять требуемое средство программного кода в форме инструкций или структур данных, и к которому можно осуществлять доступ посредством компьютера, процессора или иного устройства обработки общего назначения или специального назначения.The above memory may be volatile or non-volatile memory, or may include both volatile and non-volatile memory. One of ordinary skill in the art will also understand that when referring to memory and storage of data, programs, codes, instructions, instructions, and the like, it is understood that there is a machine-readable (or computer-readable, processor-readable) storage medium. A computer-readable storage medium can be any available medium that can be used to carry or store a desired piece of program code in the form of instructions or data structures, and that can be accessed by a computer, processor, or other general purpose or special purpose processing device. .

В качестве примера, а не ограничения, машиночитаемые носители могут содержать постоянное запоминающее устройство (ROM), программируемое постоянное запоминающее устройство (PROM), стираемое программируемое постоянное запоминающее устройство (EPROM), электронно-стираемое программируемое постоянное запоминающее устройство (EEPROM), флэш-память, оперативную память (RAM), статическую память с произвольным доступом (SRAM), динамическую память с произвольным доступом (DRAM), синхронную динамическую память с произвольным доступом (SDRAM), синхронную динамическую память с произвольной выборкой с двойной скоростью передачи данных (DDR SDRAM), синхронную динамическую память с произвольной выборкой с повышенной скоростью (ESDRAM), DRAM с синхронной линией связи (SLDRAM) и оперативную память с шиной прямого доступа (DR RAM) и т.п.By way of example, and not limitation, computer-readable media may include read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electronically erasable programmable read-only memory (EEPROM), flash memory. , Random Access Memory (RAM), Static Random Access Memory (SRAM), Dynamic Random Access Memory (DRAM), Synchronous Dynamic Random Access Memory (SDRAM), Double Data Rate Synchronous Dynamic Random Access Memory (DDR SDRAM) , increased speed synchronous dynamic random access memory (ESDRAM), synchronous link DRAM (SLDRAM), and direct access bus random access memory (DR RAM), and the like.

Информация и сигналы, описанные в данном документе, могут представляться с помощью любой из множества различных технологий. Например, данные, инструкции, команды, информация, сигналы, биты, символы и элементарные сигналы, которые могут приводиться в качестве примера в вышеприведенном описании, могут представляться посредством напряжений, токов, электромагнитных волн, магнитных полей или частиц, оптических полей или частиц либо любой комбинации вышеозначенного.The information and signals described herein may be represented using any of a variety of different technologies. For example, the data, instructions, commands, information, signals, bits, symbols, and elementary signals that may be exemplified in the above description may be represented by voltages, currents, electromagnetic waves, magnetic fields or particles, optical fields or particles, or any combinations of the above.

Процессор может включать в себя один или более процессоров. В то же время, один или более процессоров могут быть процессором общего назначения, например центральным процессором (CPU), прикладным процессором (АР) и т.п., блоком обработки графики, таким как графический процессор (GPU), визуальный процессор (VPU) и т.д.The processor may include one or more processors. At the same time, one or more processors may be a general purpose processor such as a central processing unit (CPU), an application processor (AP) and the like, a graphics processing unit such as a graphics processing unit (GPU), a visual processing unit (VPU) etc.

Следует понимать, что хотя в настоящем документе для описания различных элементов, компонентов, областей, слоев и/или секций могут использоваться такие термины, как "первый", "второй", "третий" и т.п., эти элементы, компоненты, области, слои и/или секции не должны ограничиваться этими терминами. Эти термины используются только для того, чтобы отличить один элемент, компонент, область, слой или секцию от другого элемента, компонента, области, слоя или секции. Так, первый элемент, компонент, область, слой или секция может быть назван вторым элементом, компонентом, областью, слоем или секцией без выхода за рамки объема настоящего изобретения. В настоящем описании термин "и/или" включает любые и все комбинации из одной или более из соответствующих перечисленных позиций. Элементы, упомянутые в единственном числе, не исключают множественности элементов, если отдельно не указано иное.It should be understood that although terms such as "first", "second", "third" and the like may be used herein to describe various elements, components, regions, layers and/or sections, these elements, components, areas, layers and/or sections should not be limited by these terms. These terms are only used to distinguish one element, component, region, layer, or section from another element, component, region, layer, or section. Thus, a first element, component, region, layer, or section may be referred to as a second element, component, region, layer, or section without departing from the scope of the present invention. As used herein, the term "and/or" includes any and all combinations of one or more of the respective listed positions. Elements mentioned in the singular do not exclude the plurality of elements, unless otherwise specified.

Функциональность элемента, указанного в описании или формуле изобретения как единый элемент, может быть реализована на практике посредством нескольких компонентов устройства, и наоборот, функциональность элементов, указанных в описании или формуле изобретения как несколько отдельных элементов, может быть реализована на практике посредством единого компонента.The functionality of an element specified in the description or claims as a single element may be practiced by means of several components of the device, and conversely, the functionality of elements indicated in the description or claims as several separate elements may be practiced by means of a single component.

В одном варианте осуществления элементы/блоки/модули предложенного устройства находятся в общем корпусе, могут быть размещены на одной раме/конструкции/печатной плате/кристалле и связаны друг с другом конструктивно посредством монтажных (сборочных) операций и функционально посредством линий связи. Упомянутые линии или каналы связи, если не указано иное, являются типовыми, известными специалистам линиями связи, материальная реализация которых не требует творческих усилий. Линией связи может быть провод, набор проводов, шина, дорожка, беспроводная линия связи (индуктивная, радиочастотная, инфракрасная, ультразвуковая и т.д.). Протоколы связи по линиям связи известны специалистам и не раскрываются отдельно.In one embodiment, the elements/blocks/modules of the proposed device are in a common housing, can be placed on the same frame/structure/printed circuit board/crystal and are structurally connected to each other through assembly (assembly) operations and functionally through communication lines. The mentioned communication lines or channels, unless otherwise indicated, are typical communication lines known to specialists, the material implementation of which does not require creative efforts. The communication link may be a wire, a set of wires, a bus, a track, a wireless link (inductive, RF, infrared, ultrasonic, etc.). Communication protocols over communication lines are known to those skilled in the art and are not disclosed separately.

Под функциональной связью элементов следует понимать связь, обеспечивающую корректное взаимодействие этих элементов друг с другом и реализацию той или иной функциональности элементов. Частными примерами функциональной связи может быть связь с возможностью обмена информацией, связь с возможностью передачи электрического тока, связь с возможностью передачи механического движения, связь с возможностью передачи света, звука, электромагнитных или механических колебаний и т.д. Конкретный вид функциональной связи определяется характером взаимодействия упомянутых элементов, и, если не указано иное, обеспечивается широко известными средствами, используя широко известные в технике принципы.The functional connection of elements should be understood as a connection that ensures the correct interaction of these elements with each other and the implementation of one or another functionality of the elements. Particular examples of functional communication may be communication with the ability to exchange information, communication with the ability to transmit electric current, communication with the ability to transmit mechanical motion, communication with the ability to transmit light, sound, electromagnetic or mechanical vibrations, etc. The specific type of functional connection is determined by the nature of the interaction of the mentioned elements, and, unless otherwise indicated, is provided by well-known means, using principles well-known in the art.

Несмотря на то, что примерные варианты осуществления были подробно описаны и показаны на сопроводительных чертежах, следует понимать, что такие варианты осуществления являются лишь иллюстративными и не предназначены ограничивать настоящее изобретение, и что данное изобретение не должно ограничиваться конкретными показанными и описанными компоновками и конструкциями, поскольку специалисту в данной области техники на основе информации, изложенной в описании, и знаний уровня техники могут быть очевидны различные другие модификации и варианты осуществления изобретения, не выходящие за пределы сущности и объема данного изобретения.Although the exemplary embodiments have been described in detail and shown in the accompanying drawings, it should be understood that such embodiments are illustrative only and are not intended to limit the present invention, and that the present invention should not be limited to the particular arrangements and structures shown and described, since a person skilled in the art on the basis of the information set forth in the description and the knowledge of the prior art may be obvious various other modifications and embodiments of the invention, without going beyond the essence and scope of this invention.

Claims (17)

1. Устройство для оптимизации траектории движения летательного аппарата (ЛА), содержащее:1. A device for optimizing the trajectory of an aircraft (LA), containing: модуль картографирования, выполненный с возможностью построения карты объектов окружающей среды на основе трехмерного (3D) облака точек, полученного от содержащегося в ЛА модуля генерации 3D-облака точек;a mapping module configured to build a map of environmental objects based on a three-dimensional (3D) point cloud obtained from a 3D point cloud generation module contained in the aircraft; модуль оценки расстояний, выполненный с возможностью оценки расстояния от ЛА до окружающих объектов на основе карты объектов окружающей среды и данных о местоположении ЛА, полученных от содержащегося в ЛА модуля навигации;a distance estimation module, configured to estimate the distance from the aircraft to the surrounding objects based on the map of the objects of the environment and the position data of the aircraft received from the navigation module contained in the aircraft; модуль глобального планирования, выполненный с возможностью формирования уточненной траектории движения ЛА в обход препятствий на основе опорной траектории, карты объектов окружающей среды и данных о расстоянии от ЛА до окружающих объектов;a global planning module configured to generate an updated trajectory of the aircraft around obstacles based on the reference trajectory, a map of environmental objects and data on the distance from the aircraft to surrounding objects; модуль локального планирования, выполненный с возможностью формирования оптимальной траектории и оптимальных управляющих сигналов для ЛА на основе уточненной траектории с использованием метода управления с нелинейной прогнозирующей моделью; иa local planning module configured to generate an optimal trajectory and optimal control signals for the aircraft based on the refined trajectory using a non-linear predictive model control method; And пропорционально-дифференцирующий (ПД) регулятор, выполненный с возможностью формирования отрегулированных управляющих сигналов для ЛА и их передачи на контроллер полета для управления исполнительными механизмами ЛА, причем ПД-регулятор использует данные о текущем состоянии ЛА от модуля навигации ЛА в качестве обратной связи и оптимальные управляющие сигналы от модуля локального планирования в качестве входного сигнала.proportional-differentiating (PD) controller, configured to generate adjusted control signals for the aircraft and transmit them to the flight controller to control the aircraft actuators, moreover, the PD controller uses data on the current state of the aircraft from the aircraft navigation module as feedback and optimal control signals signals from the local scheduling module as input. 2. Устройство по п. 1, в котором модуль глобального планирования выполнен с возможностью формирования уточненной траектории движения ЛА в обход препятствий, если определено, что опорная траектория проходит через объект окружающей среды или что ЛА или опорная траектория находится на таком расстоянии до объекта окружающей среды, которое меньше предварительно заданного расстояния уклонения.2. The device according to claim 1, in which the global planning module is configured to generate an updated trajectory of the aircraft around obstacles if it is determined that the reference trajectory passes through the environmental object or that the aircraft or the reference trajectory is at such a distance from the environmental object , which is less than the preset avoidance distance. 3. Устройство по п. 2, в котором местоположение ЛА соответствует центру ЛА, а расстояние уклонения больше радиуса окружности, описывающей ЛА.3. The device according to claim 2, in which the location of the aircraft corresponds to the center of the aircraft, and the avoidance distance is greater than the radius of the circle describing the aircraft. 4. Устройство по п. 1, в котором оптимальные управляющие сигналы для ЛА содержат векторы скорости в каждом из направлений x, y, z, а также угол рыскания.4. The device according to claim 1, in which the optimal control signals for the aircraft contain velocity vectors in each of the x, y, z directions, as well as the yaw angle. 5. Устройство по п. 1, дополнительно содержащее модуль навигации, выполненный с возможностью определения местоположения, скорости и угла рыскания ЛА.5. The device according to claim 1, further comprising a navigation module configured to determine the location, speed and yaw angle of the aircraft. 6. Устройство по п. 1, дополнительно содержащее модуль генерации 3D-облака точек, выполненный с возможностью генерации 3D-облака точек.6. The apparatus of claim 1, further comprising a 3D point cloud generation module configured to generate a 3D point cloud. 7. Способ оптимизации траектории движения ЛА, содержащий этапы, на которых:7. A method for optimizing the trajectory of the movement of the aircraft, containing the steps in which: с использованием модуля картографирования выполняют построение карты объектов окружающей среды на основе трехмерного (3D) облака точек, полученного от содержащегося в ЛА модуля генерации 3D-облака точек;using the mapping module, building a map of environmental objects based on a three-dimensional (3D) point cloud obtained from the 3D point cloud generation module contained in the aircraft; с использованием модуля оценки расстояний выполняют оценку расстояния от ЛА до окружающих объектов на основе карты объектов окружающей среды и данных о местоположении ЛА, полученных от содержащегося в ЛА модуля навигации;using the distance estimation module, estimating the distance from the aircraft to the surrounding objects based on the environmental object map and the aircraft position data obtained from the navigation module contained in the aircraft; с использованием модуля глобального планирования формируют уточненную траекторию движения ЛА в обход препятствий на основе опорной траектории, карты объектов окружающей среды и данных о расстоянии от ЛА до окружающих объектов;using the global planning module, an updated trajectory of the aircraft movement is formed to bypass obstacles based on the reference trajectory, a map of environmental objects and data on the distance from the aircraft to surrounding objects; с использованием модуля локального планирования формируют оптимальную траекторию и оптимальные управляющие сигналы для ЛА на основе уточненной траектории с использованием метода управления с нелинейной прогнозирующей моделью; иusing a local planning module, generating an optimal trajectory and optimal control signals for the aircraft based on the refined trajectory using a control method with a non-linear predictive model; And с использованием ПД-регулятора формируют отрегулированные управляющие сигналы для ЛА и передают их на контроллер полета для управления исполнительными механизмами ЛА, причем используют данные о текущем состоянии ЛА от модуля навигации ЛА в качестве обратной связи и оптимальные управляющие сигналы от модуля локального планирования в качестве входного сигнала.using a PD controller, adjusted control signals for the aircraft are generated and transmitted to the flight controller to control the aircraft actuators, using data on the current state of the aircraft from the aircraft navigation module as feedback and optimal control signals from the local planning module as an input signal .

Images