Построение и исследование подсистемы планирования траектории перемещения для системы управления автономным подводным аппаратом Текст научной статьи по специальности «Математика»

Построение и исследование подсистемы планирования траектории перемещения для системы управления автономным подводным аппаратом

Б.В. Гуренко, И.О. Шаповалов, В.В. Соловьев, М.А. Береснев Южный федеральный университет, Таганрог

Аннотация: В данной работе предлагается решение задачи плоского перемещения автономного необитаемого подводного аппарата к цели с помощью иерархической системы управления. Для планирования траектории используется поисковый А*-алгоритм. Перемещение аппарата по траектории обеспечивается позиционно-траекторным регулятором на основе квадратичных форм. Рассматриваемый тип аппарата имеет торпедообразную форму с двумя маршевыми движителями и подруливающим устройством, расположенным в носовой части.

Ключевые слова: математическая модель, необитаемый подводный аппарат, планирование, позиционно-траекторное управление, А*-алгоритм.

Введение

Современные подводные аппараты способны выполнять широкий круг задач, в который входят экологический и климатический мониторинг, океанологические исследования, обслуживание глубоководных систем и устройств подводных мест, поиск подводных месторождений, охрана акваторий и др. Все эти задачи могут быть наиболее эффективно решены в классе автономных необитаемых подводных аппаратов (АНПА). Причина заключается в снижении стоимости эксплуатации и вероятности ошибки за счет уменьшения степени участия человека-оператора, повышения времени непрерывной работы мобильных систем, уменьшение массогабаритных параметров техники и другие. Однако для проектирования и создания системы управления такого сложного объекта, необходимо построить математическую модель, которая смогла бы в полной мере учесть всю нестационарность параметров, нелинейность динамики и многосвязность подводного аппарата (ПА), как объекта управления, с учетом всех сил взаимодействия корпуса с вязкой средой.

Специфика использования подводного аппарата для точной отработки траекторий на высоких скоростях требует обязательного учета, а также оценки или измерения, нестационарности его параметров (присоединенные моменты и массы и т.п.) [3-6], кроме того, при неопределенности среды функционирования необходима разработка структуры и алгоритмов подсистемы планирования перемещений [7-10].

Математическая модель АНПА

Для описания математической модели АНПА будем использовать две прямоугольные системы координат, показанные на рис. 1. Математическая модель на базе известных уравнений твердого тела может быть представлена в следующем векторно-матричном виде:

У = £(в, X) = Е

% (в, X) ^

(1)

мх =

2в(в, X)

Ё (Р,У,о) + Ёи (5) + (О, Ап, Яг) ] (2)

ту +3=х*«> (6, и), (3)

где тиу - диагональная матрица постоянных времени исполнительных

устройств (ИУ); ^иу(5,и)- вектор нелинейных функций правых частей

уравнений ИУ; 5 - вектор управляющих воздействий на элементы АНПА, формируемый ИУ; и - вектор управлений, формируемый системой управления АНПА, где х - т-вектор внутренних координат (координат состояния);М - (т*т)-матрица массо-инерционных параметров, элементами которой являются масса, моменты инерции, присоединенные массы АНПА; Ёи(х,У,5,1) - т-вектор управляющих сил и моментов, здесь I - вектор конструктивных параметров; (х,У, I) - т-вектор нелинейных элементов динамики АНПА; - т-вектор измеряемых и неизмеряемых внешних возмущений; У = (Р, ©)т - и-вектор положения Р и ориентации 0 (выходных

координат) связанной системы координат относительно базовой, п < 6; Е(0, х) - п-вектор кинематических связей; Ер (0, х) - вектор линейных

скоростей связанной системы координат относительно базовой; Е0 (0, х) -

вектор угловых скоростей связанной системы координат относительно базовой.

Для проведения исследования иерархической системы управления АНПА был разработан регулятор перемещения на основе ПД-закона. Регулятор реализован в дискретной форме с вычислением Д-составляющей по формуле трапеций.

Выходом регулятора является вектор скоростей вращения движителей. Управляемые переменные - угол рыскания и продольная скорость перемещения. Результирующий вектор желаемых скоростей вращения винтов вычисляется по формуле

Рис. 1. - Системы координат К(ОХ°У 020) и К(ОХ У 2)

Позиционно-траекторный регулятор АНПА

(4)

Составляющие выражения (4) вычисляются следующим образом:

^хе(г +1) = кР1(Ух3(I) - Ух(I)) + к, 1((УХ3(I) - Ух(I)) - (Ух з (I) - Ух (I - 1)))/ Т

Wyc(i + 1) = + kp 2 ( у з (I) - у (I )) + (б)

+К2 ((Уз (I) - У(I)) - (Уз (I) - У(I -1))) / T,

wzc(i + 1) = kp3 ( ^ (!) - (I)) + к,3 ((з (I) - (I)) -

-(V,з (I) - V, (I -1)))/ т , (7)

где I - номер шага вычисления управляющего воздействия; кр/, кф/, /= 1, 2, 3 -коэффициенты пропорциональной и дифференцирующей составляющих; V з,

Vz з - вычисленные регулятором проекции желаемой скорости перемещения; Vx, Vz - проекции скорости перемещения; уз - заданная глубина плавания, Т -шаг дискретизации.

В процессе перемещения по траектории регулятор вычисляет проекции желаемой скорости перемещения по формуле:

К ^^х™^) 0 ^х^П^ ], (8)

где р- угол между осью 0Х и прямой, соединяющей начало координат со следующей точкой траектории; Р^- максимальная скорость перемещения АНПА.

При торможении в окрестности целевой точки проекция V3 х желаемой скорости определяется в соответствии с выражением (выражение для Vз, аналогично):

уг = Vmax • COs(P) • sën(хз - х) (9)

зх л -а|х-Ь| ' ^ '

1 + е 1 1

где а, Ь - параметры, определяющие форму экспоненциальной функции.

Планирование траектории перемещения

Для проверки эффективности работы алгоритма планирования с учетом ограничений, накладываемых на перемещение АНПА, использовался распространенный подход в виде комбинации А * -алгоритма и модифицированного метода динамического окна. Построение оптимальной

траектории осуществляется с помощью А* - алгоритма по разведанному участку сцены, а с помощью метода динамического окна траектория модифицируется таким образом, чтобы при движении на максимально возможной скорости АНПА не приближался к препятствиям на недопустимые расстояния.

Смысл планирования траектории на основе А*-алгоритма заключается в следующем. Создается карта местности. Для этого плоскость функционирования разбивается на квадратные ячейки. Каждой ячейке присваивается значение, характеризующее ее либо как свободную, либо как занятую препятствием, либо как стартовую (целевую). Первоначально все ячейки считаются свободными, затем по мере получения данных от лазерного дальномера, карта обновляется. A * -алгоритм запускается каждый раз, как только обновляется значение какой-либо ячейки, находящейся на построенной траектории, либо при изменении задаваемых параметров движения.

Безопасность перемещения по траектории обеспечивается путем обозначения прилегающих к препятствию ячеек как непроходимых. Количество ячеек с виртуальными препятствиями определяется размером ячеек и заданным минимально допустимым расстоянием между АНПА и препятствием. Чтобы избежать попадания АНПА в локальные минимумы, ячейки, которые робот при перемещении по траектории посещает несколько раз, обозначаются как занятые препятствием.

После создания или обновления карты создаются два списка ячеек. В первый список, называемый открытым, помещаются ячейки, для которых необходимо рассчитать длину траектории движения АНПА. Во второй список, называемый закрытым, помещаются ячейки, для которых эта длина уже рассчитана.

Затем на сформированной карте рассматривается ячейка, в которой в настоящий момент находится подвижный объект и окружающие ее восемь

ячеек, как показано на рис. 2.

Рис. 2. -Блок ячеек кандидатов для добавления в открытый список

Стартовая ячейка, от которой начинается построение участка траектории, добавляется в открытый список. Затем проверяются все ячейки вокруг нее. Ячейки добавляются в открытый список, если в них не находятся препятствия и они не выходят за границы сцены. Для каждой из добавляемых в открытый список ячеек: запоминаются координаты предыдущей (родительской) ячейки, вычисляется значение функции /, характеризующей длину траектории через данную ячейку. Стартовая ячейка добавляется в закрытый список. Затем из открытого списка берется ячейка с наименьшим значением^ и так далее.

Значение функции эффективности вычисляется по формуле

f=g+h, (10) где g- мера стоимости перемещения АНПА из стартовой ячейки в данную ячейку, h- мера стоимости перемещения АНПА из текущей ячейки в целевую.

Значение g определяется как сумма стоимости перемещения ПО из стартовой ячейки в родительскую и стоимости перемещения из родительской ячейки в текущую. Например, для случая формы ячеек, приведенных на рис. 3Ри, принимаем, что стоимость перемещения из центральной ячейки в ячейку, смещенную только по вертикали или только по горизонтали, равна 10. А стоимость перемещения в ячейку, смещенную по диагонали относительно центральной, равна 14.

Значение h определяется как стоимость перемещения по вертикали и по

горизонтали между текущей ячейкой и целевой, равная произведению количества проходимых ячеек на 10.

А*-алгоритм планирования траектории можно представить в виде следующей последовательности шагов:

1) Стартовая ячейка добавляется в открытый список.

2) Повторяются следующие шаги:

а) Поиск в открытом списке ячейки с наименьшей величиной / Обозначение найденной ячейки как текущей.

б) Помещение текущей ячейки в закрытый список и удаление из открытого списка.

в) Для каждой из окружающих восьми ячеек выполняется:

- если в ячейке находится препятствие или она находится в закрытом списке, то она игнорируется.

- если ячейка еще не в открытом списке, то она в него вносится. Текущая ячейка становится родительской для рассматриваемой ячейки. Рассчитываются значения /, g и к для рассматриваемой ячейки.

- если ячейка уже в открытом списке, то проверяется длина пути траектории через эту ячейку. Для проверки используется величина g. Уменьшение значения g указывает на то, что траектория будет короче. В этом случае текущая ячейка становится родительской для рассматриваемой ячейки, и пересчитываются g и/

г) Прекращение работы, если целевая ячейка добавлена в открытый список (в этом случае траектория спланирована) или открытый список пуст, а целевая ячейка не достигнута, тогда построение траектории невозможно.

3) Сохранение траектории.

Моделирование

Результаты моделирования перемещения АНПА по пяти сценам с

учетом динамических ограничений приведены на рисункеЗРис.

1 1 1 1

----н о/ эу

О/ 'о

0 5 10 15 20 25

X, м

а

в

0 5 10 15 20 25

X, м

0 5 10 15 20 25

X, м

б

0 5 10 15 20 25

X, м

соосс

О о -4 У Ь-

<

0 5 10 15 20 25

X, м

д

Рис. 4.- Траектория АНПА при проходе сцены

Моделирование проводилось в программе Simulink. На карте расставлялись препятствия (обозначены кругами) и задавалась точка выхода

25

25

20

20

15

15

10

5

0

25

25

20

20

15

15

10

10

5

5

г

25

20

15

10

5

АНПА (0,0). Синим цветом обозначена рассчитанная планировщиком траектория движения.

Выводы

Результаты моделирования показали эффективность подхода на основе ^*-алгоритмадля планирования перемещения и позиционно-траекторного ПД-регулятора. Приведенный гибридный подход к управлению горизонтальным перемещением позволяет АНПА достигать цели даже на сложных сценах, за исключением сцен с П-образными препятствиями.

Благодарности

Работа поддержана Министерством образования и науки РФ, НИР №114041540005 по государственному заданию ВУЗам и научным организациям в сфере научной деятельности и грантом Президента Российской Федерации №НШ-3437.2014.10.

Литература

1. Пятницкий Е.С. Управляемость классов лагранжевых систем с ограниченными управлениями // АиТ. 1996. №12. С. 29-37.

2. Пшихопов В.Х., Медведев М.Ю. Структурный синтез автопилота для подвижных объектов с оцениванием возмущений // Информационно-измерительные и управляющие системы. 2006. №1. С.103-109.

3. Пшихопов В.Х. Позиционно-траекторное управление подвижными объектами. Таганрог: Изд-во ТТИ ЮФУ, 2009. 183 с.

4. Пшихопов В.Х., Сиротенко М.Ю., Гуренко Б.В. Структурная организация систем автоматического управления подводными аппаратами для априори неформализованных сред // Информационно-измерительные и управляющие системы. Интеллектуальные и адаптивные роботы. 2006. № 13. т.4. С. 73-79.

5. Управление подвижными объектами. Библиографический указатель. В 3-х выпусках. Вып. 3. Морские объекты. М.: 2011. 150 с.

6. Лукомский Ю.А., Чугунов В.С. Системы управления морскими подвижными объектами: Учебник. Л: Судостроение, 1988. 272 с.

7. Medvedev M. Y., PshikhopovV.Kh., Robust control of nonlinear dynamic systems // Proceedings of 2010 IEEE Latin-American Conference on Communications (ANDERSON). 2010. Pp.1-7.

8. Pshikhopov V., Medvedev M., Kostjukov V., Fedorenko R., Gurenko B., Krukhmalev V. Airship autopilot design// SAE Technical Paper №2011-01-2736. 2011. 5 p.

9. Федоренко Р.В. Алгоритмы автопилота посадки роботизированного дирижабля // Инженерный вестник Дона, 2011, №1. URL: ivdon.ru/magazine/archive/n1y2011/371.

10. Кульченко А.Е. Структурно-алгоритмическая организация автопилота робота-вертолета // Инженерный вестник Дона, 2011, №1. URL: ivdon.ru/magazine/archive/n1y2011/330.

References

1. Pyatnitskiy E.S. AiT. 1996. №12. pp. 29-37.

2. Pshikhopov V.Kh., MedvedevM.Yu. Informatsionno-izmeritel'nye i upravlyayushchie sistemy. 2006. №1. pp. 103-109.

3. Pshikhopov V.Kh. Pozitsionno-traektornoe upravlenie podvizhnymi ob"ektami [Position-path management of mobile objects]. Taganrog: Izd-vo TTI YuFU, 2009. 183 p.

4. PshikhopovV.Kh., SirotenkoM.Yu., Gurenko B.V. Informatsionno-izmeritel'nye i upravlyayushchie sistemy. Intellektual'nye i adaptivnye roboty. 2006. № 1-3. Vol.4. pp. 73-79.

5. Upravlenie podvizhnymi ob"ektami. Bibliograficheskiy ukazatel'. V 3-kh vypuskakh. Vyp. 3. Morskie ob"ekty [Mobile object management: In 3 volumes. Vol. 3. Underwater objects]. M.: 2011. 150 p.

6. LukomskiyYu.A., Chugunov V.S. Sistemy upravleniya morskimi podvizhnymi ob"ektami: Uchebnik [Control systems for marine mobile objects]. L: Sudostroenie, 1988. 272 p.

7. Medvedev M. Y., PshikhopovV.Kh., Robust control of nonlinear dynamic systems. Proceedings of 2010 IEEE Latin-American Conference on Communications (ANDERSON). 2010. Pp.1-7.

8. Pshikhopov V., Medvedev M., Kostjukov V., Fedorenko R., Gurenko B., Krukhmalev V. Airship autopilot design. SAE Technical Paper №2011-012736. 2011. 5 p.

9. Fedorenko R.V. Inzenernyj vestnik Dona (Rus), 2011, №1, URL: ivdon.ru/magazine/archive/n1y2011/371.

10. Kulchenko A.E. Inzenernyj vestnik Dona (Rus), 2011, №1, URL: ivdon.ru/magazine/archive/n1y2011/330.