CC BY
59
УДК 681.5
DOI: 10.17586/0021-3454-2017-60-8-704-711
ТРАЕКТОРНОЕ УПРАВЛЕНИЕ ДВИЖЕНИЕМ ТВЕРДОГО ТЕЛА
В ПРОСТРАНСТВЕ
Ван Цзянь1, А. Ю. Краснов2, Ю. А. Капитанюк3, С. А. Чепинский12, С. А. Холунин2, Чэнь Ифань2, Лю Хуэйминь2, Д. А. Хвостов2
1 Ханчжоу Дяньцзи Университет. Институт Автоматизации, 310018, г. Ханчжоу, КНР
2Университет ИТМО, 197101, Санкт-Петербург, Россия E-mail: aleksander@gmail. com 3Университет Гронингена, 9700 AB, Гронинген, Нидерланды
Рассматривается процедура синтеза алгоритма траекторного управления движением твердого тела вдоль заданной в неявном виде пространственной траектории с заданной скоростью. Математическое описание объекта управления представлено динамической моделью с однонаправленной тягой. Предложена процедура синтеза законов траекторного управления, решающих задачу следования неявно заданной пространственной траектории. Работоспособность синтезированных законов управления подтверждена результатами моделирования. Предложенные законы могут быть полезны при разработке систем траекторно-го управления движением мобильных роботов (подводных или воздушных) в пространстве.
Ключевые слова: алгоритм, движение, ориентация, траекторное управление, преобразование координат
Первоначально задача траекторного управления движением твердого тела вдоль заданной в неявном виде пространственной траектории с заданной скоростью формулировалась как задача слежения. Точка, характеризующая желаемое положение объекта управления, перемещалась вдоль заданной траектории, а регулятор генерировал закон управления, сводящий невязку между текущим и желаемым положением к нулю. В работе [1] желаемая траектория параметризована временем, однако в [2] продемонстрировано, что такая постановка задачи приводит к ухудшению качества регулирования при наличии у объекта управления неустойчивой нуль-динамики. Этих осложнений можно избежать, параметризовав желаемую траекторию вместо времени, например, ее длиной. Сейчас этот подход остается доминирующим в области управления движением и имеет многочисленные варианты, например, LOS (Line-Of-Sight) [3, 4], основанный на формализации действий рулевого при управлении судном.
Подход к синтезу систем траекторного управления в виде следящей системы, при ряде преимуществ (хорошее качество регулирования, низкая алгоритмическая сложность), не обеспечивает инвариантности траектории. Достижение этого свойства обеспечивает альтернативный подход, основанный на стабилизации инвариантных многообразий в пространстве выходов объекта управления. Здесь задача траекторного управления ставится как задача стабилизации аттрактора, описываемого уравнением желаемой траектории, в пространстве выходов. Поэтому в рамках этого подхода желаемая траектория представляется неявно заданной кривой, характеризующей уравнение связи выходов объекта управления.
Первоначально подобная постановка задачи была предложена в статье [5], подход нашел развитие в работах отечественных авторов А. А. Колесникова [6], представившего концепцию синергетического анализа объектов управления, и И. В. Мирошника [7—11], предложившего подход на основе методов дифференциальной геометрии и преобразования
к задачно-ориентированным координатам. Дальнейшее развитие подход получил в работах [12—17], и именно на его основе в настоящей статье получены результаты.
Постановка задачи траекторного управления. Рассмотрим малоприводную динамическую модель движения твердого тела с однонаправленной тягой:
Р (t ) = gn (t),
m
(1)
(2) (3)
Я (а) = Я (а) 5 (ш( г)), Лш (г) + ш (г) х Лш (г) = Мс (г),
где р = [х у z]Т е Я3 — декартов радиус-вектор положения центра масс С в опорной
3
системе координат ХУ2 (рис. 1); g е Я — ускорение свободного падения; т е Я — масса объекта управления; / е Я — величина управляющей силы, п — вектор текущей ориента-
Т 3
ции; а = [ф 9 у] е Я — вектор углов Эйлера, характеризующий ориентацию связанной с телом системы координат X' У' 2' (см. рис. 1) относительно опорной системы координат, составленный из углов рыскания, тангажа и крена соответственно; Я (а) е БО(3) — поворотная
матрица от связанной с телом опорной системы координат; Л — момент инерции объекта
3 3
управления; ше Я , Мс е Я — векторы угловых скоростей и управляющих моментов в связанной с телом системе координат; Б(ш) е 5О(3) — кососимметричная матрица вида
S (ш) =
Z
0 ш3 -ш
-ш3 0 ш1
ш2 -ш1 0
Z' \
с' ) -X'
(4)
Рис. 1
Поворотная матрица может быть выражена через углы Эйлера как
Я (а) = Я3 (у) Я2 (9) Я (ф),
0 0
' Я2 (9) =
(5)
где R1 (ф) =
1
0 cos ф sin ф 0 - sin ф cos ф
, R3 (у) =
cos у sin у 0 - sin у cos у 0 0 0 1
cos 9 0 - sin 9 0 1 0 sin 9 0 cos 9
Желаемая траектория представлена гладким сегментом кривой S (см. рис. 1), описываемой как пересечение двух неявно заданных поверхностей:
ф1 (p) = 0оф2 (p) = 0, (6)
где ф1 (p) и ф2 (p) — гладкие функции.
Тангенциальная скорость вдоль желаемой кривой определяется выражением
Уф! х Уф2 .
^ = ■
(7)
||Уф! хУф2||
где х — векторное произведение, Ц-Ц — норма вектора, У е Я3 — градиент функции / .
Стоит отметить, что описание кривой как гладкого геометрического объекта не единственное, и выбор функций (6) неоднозначен. Выбор функций ф (р) и ф2 (р) главным образом ограничен условием регулярности [8], подразумевающим, что матрица Якоби
Уф1 хУф2
¥(р )= Уф1 (8)
_ Уф2 _
невырождена для любого вектора р , принадлежащего кривой £, т.е. ёй У(р) Ф 0 .
Введем в рассмотрение пространственные отклонения текущего положения объекта управления от желаемого. Нарушение условий (6) характеризуется отклонениями
ех ( р ) = ф! ( р ), (9)
^2 ( р ) = ф2 ( р), (10)
обнуляемыми на множестве £.
Сформулируем три подзадачи траекторного управления движением твердого тела в пространстве:
1) стабилизация движения объекта управления относительно кривой £, что предполагает асимптотическое обнуление векторов пространственных отклонений е (р) и ^ (р) ;
2) асимптотическое обнуление ошибки скорости
АУ = V -5;
(11)
3) стабилизация требуемой ориентации объекта управления относительно кривой £ . Управление поступательным движением. Для синтеза закона траекторного управления преобразуем модель системы (1)—(3) к задачно-ориентированной форме [14—17] с выходами 5, е и ^2. Преобразование координат осуществим через якобиан (8):
= У(р)р.
(12)
Соответственно обратное преобразование координат (от задачно-ориентированных к исходным координатам объекта управления) задается в виде
5
р = У-1( р)
1
(13)
Продифференцируем (12) с учетом (1):
5 ел
= ¥ (р) р + У(р)
йИ п )Л
т
(14)
Теперь рассмотрим виртуальные (задачно-ориентированные) законы управления
2
¥ (р )р + ¥(р ) Подставим (15) в (14) и получим
(
\
* - ^ " С)
V т У
м
*е 2
(15)
5 " и5 "
е1 = ие1
_е2 _ ие2 _
(16)
Теперь выберем задачно-ориентированные законы управления в виде:
и? = к? А5,
(17)
(18) (19)
ие1 = -к1е1е1 - к2е1е1, ие2 = -к1е2е2 - к2е2е2 ,
где к5, к^, к2е1, к^, к2е2 — положительные константы, обеспечивающие желаемую динамику асимптотического обнуления отклонений А?, е1 и е2 и решение первой и второй подзадач управления.
Теперь определим реальное управляющее воздействие / (г), осуществив обратное преобразование координат:
/ ( ) =
( ( -1
* - т¥ (р)
и
е1
и
е 2
-¥(р )р
п
-1
(20)
/у
Управление угловой ориентацией. Теперь сосредоточимся на обеспечении требуемой ориентации робота. Введем вектор угловых ошибок б = ^бф бд б^еЯ3 и матрицу угловых отклонений
Я(б) = Я(а)ЯТ (а*)Ят (А), (21)
где Я (а )е £О(3) — матрица угловой ориентации связанной с телом системы координат
вдоль кривой £, Ят (А) е БО(3) — матрица желаемой угловой ориентации относительно кривой Б.
Для стабилизации желаемой ориентации и обнуления вектора б (или обеспечения равенства Я (б) = I) определим функцию угловой ошибки как
е^ = 2 (Я (б)- Ят (б))4
(22)
о
где V — преобразование £О(3) ^ Я .
Определим ошибку угловой скорости еи . Продифференцируем (21) с учетом (2):
йЯ (б) = Б (б)Я (б) = еюЯ (б),
—Я (б) = Б (ш)Я (б)- Я (а) Ят (а*) £ (и*) Ят (А).
йг
Воспользуемся свойством кососимметричных матриц Я5 (ю )ЯТ = 5 (Яю) и получим финальные выражения
^-Я(5) = (5(ю)-5(Я(а)ЯТ (а*)ю*))Я(5),
ею = ю -Я
(а) ЯТ (а* )ю*. (23)
Продифференцируем (23) с учетом (2):
е<а = у ((-юхУю) + , (24)
где аа = -5(ю)Я(а)ЯТ (а*)ю* +Я(а)ЯТ (а*)ю*.
Результирующий закон управления ориентацией будет иметь вид
Мс =юх3ю-Мл-кгег -кюею, (25)
где кг и кю — положительные константы.
Численный пример. Представим желаемую траекторию 5 (рис. 2) в виде пересечения цилиндра и параболы:
ф (р) = 0,2х2 + у2 -100 = 0 оф2 (р) = 2 + 0,05у2 - 5 = 0. Предположим, что параметры объекта управления равны т = 1 и 5 — единичная матрица. Начальное положение центра масс 5 объекта управления р0 =[-10 5 10] и начальная ориентация а0 = [3 2 1] .
Параметры регулятора представлены ниже:
к1е1 = 1, к2е1 = 10, к1е2 = 1, к2е2 = 10, К = 20, кю = 50.
*
Желаемая скорость вдоль заданной траектории V = 1.
Для управления ориентацией использована упрощенная версия закона управления (25) без компонента прямой связи а^ :
Мс = ю х Ую - кгег - кюею. Результаты моделирования представлены на рис. 2—5.
10 5
0
10
г
20
-10
Рис. 2
На рис. 2 приведена траектория пространственного движения объекта управления. Рис. 3—5 демонстрируют временные диаграммы пространственных и угловых отклонений объекта управления. Как видно из полученных графиков, все отклонения сходятся к нулю за короткие промежутки времени, колебания угловых отклонений в окрестности нуля обусловлены кривизной траектории и невозможностью мгновенно изменять соответствующим образом ориентацию объекта управления. В целом это говорит об эффективности синтезированных законов управления.
У
е\
Рис. 3
Рис. 5
Заключение. Разработан алгоритм синтеза законов траекторного управления, решающих поставленную задачу следования неявно заданной пространственной траектории. Предложенные законы управления могут быть полезны при разработке систем траекторного управления движением мобильных роботов (подводных или воздушных роботов) в пространстве. Приоритет дальнейших исследований состоит в детальном изучении свойств представленных регуляторов в условиях наличия параметрических неопределенностей и внешних возмущений.
Работа выполнена при поддержке Российского фонда фундаментальных исследований (грант 17-58-53129). Работа выполнена при поддержке гранта Государственного фонда естественных наук Китая (грант 61611530709 и 61503108).
СПИСОК ЛИТЕРАТУРЫ
1. Lee T., Leoky M., andMcClamroch N. Geometric tracking control of a quadrotor uav on se(3) // IEEE Conf. on 49th Decision and Control (CDC). 2010. Р. 5420—5425.
2. Aguiar A., Hespanha J. P., Kokotovic P. V. Path-following for nonminimum phase systems removes performance limitations // IEEE Transact. on Automatic Control. 2005. Vol. 50, N 2. P. 234—239.
3. Breivik M. and Fossen T. Principles of guidance-based path following in 2d and 3d // IEEE Conf. on Decision and Control. 2005. CDC-ECC '05. Р. 627—634.
4. Fradkov A., Miroshnik I., and Nikiforov V. Nonlinear and Adaptive Control of Complex Systems. Mathematics and Its Applications. Springer, 1999.
5. Banaszuk A., Hauser J. Feedback linearization of transverse dynamics for periodic orbits // Systems & control letters. 1995. Vol. 26, N 2. P. 95—105.
6. Колесников А. А., Веселов Г. Е. и др. Синергетические методы управления сложными системами: механические и электромеханические системы. M.: Едиториал УРСС/КомКнига, 2006. 304 с.
7. Бурдаков С. Ф., Мирошник И. В., Стельмаков Р. Э. Системы управления движением колесных роботов. СПб: Наука, 2001. 236 с.
8. Мирошник И. В. Согласованное управление многоканальными системами. Л.: Энергоатомиздат, 1990.
9. Miroshnik I., Nikiforov V. O. Trajectory motion control and coordination of multilink robots // Prepr. 13th IFAC World Congress. 1996. Vol. A. P. 361—366.
10. Miroshnik I. V., Chepinsky S. A. Trajectory control of underactuated mechanisms // 2nd IFAC Conf. on Mechatronic Systems. Berkeley, 2002. P. 959—1004.
11. Miroshnik I. V., Chepinsky S. A. Trajectory motion control of underactuated manipulators // Preprints 7th IFAC Symposium on Robot Control. Wroclaw, Poland, 2003. P. 105—110.
12. Pyrkin A. A., Bobtsov A. A., Chepinskiy S. A., and Kapitanyuk Y. A. Compensation of Unknown Multiharmonic Disturbance for Nonlinear Plant with Delay in Control // Preprints 8th IFAC Symposium on Nonlinear Control Systems. Bologna, Italy (NOLCOS 2010). 2010. P. 481—486.
13. Bobtsov A., Kolyubin S., Pyrkin A., Shavetov V., Chepinskiy S., Kapitanyuk Y., Kapitonov A., Bardov V., Titov A., Surov M. Using of LEGO Mindstorms NXT Technology for Teaching of Basics of Adaptive Control Theory // 18th IFAC World Congress. Milan, Italy. 2011.
14. Капитанюк Ю. А., Чепинский С. А. Задача управления многоканальной динамической системой по кусочно-гладкой траектории // Изв. вузов. Приборостроение. 2013. Т. 56, № 4. С. 65—70.
15. Kapitanyuk Y. and Chepinsky S. Control of mobile robot following a piecewise-smooth path. Gyroscopy and Navigation. 2013. Vol. 4, N 4. Р. 198—203.
16. Kapitanyuk Y., Chepinskiy S., and Kapitonov A. Geometric path following control of a rigid body based on the stabilization of sets // 19th IFAC World Congress. 2014.
17. Wang J., Kapitanyuk Y. A., Chepinskiy S. A., Dongliang Liu, Krasnov A. J. Geometric path following control in a moving frame // IFAC-PapersOnLine 48-11. 2015. P. 150—155.
Сведения об авторах
Ван Цзянь — канд. техн. наук; Ханчжоу Дяньцзи Университет. Институт Автома-
тизации; Университет ИТМО, кафедра систем управления и информатики; E-mail: [email protected]
Александр Юрьевич Краснов — аспирант; Университет ИТМО, кафедра систем управления и ин-
форматики; E-mail: [email protected]
Юрий Андреевич Капитанюк — аспирант; Университет Гронингена, кафедра дискретных технологий
и автоматизации производства; E-mail: [email protected]
Сергей Алексеевич Чепинский — канд. техн. наук; Ханчжоу Дяньцзи Университет. Институт Автома-
тизации; Университет ИТМО, кафедра систем управления и информатики; E-mail: [email protected]
Сергей Анатольевич Холунин — канд. техн. наук; Университет ИТМО, кафедра систем управления и
информатики; E-mail: [email protected]
Чэнь Ифань — аспирант; Университет ИТМО, кафедра систем управления и ин-
форматики; E-mail: [email protected]
Лю Хуэйминь — аспирант; Университет ИТМО, кафедра систем управления и ин-
форматики; E-mail: [email protected]
Денис Алексеевич Хвостов — аспирант; Университет ИТМО, кафедра систем управления и ин-
форматики; E-mail: [email protected]
Рекомендована кафедрой Поступила в редакцию
систем управления и информатики 21.03.17 г.
Ссылка для цитирования: Ван Цзянь, Краснов А. Ю., Капитанюк Ю. А., Чепинский С. А., Холунин С. А., Чэнь Ифань, Лю Хуэйминь, Хвостов Д. А. Траекторное управление движением твердого тела в пространстве // Изв. вузов. Приборостроение. 2017. Т. 60, № 8. С. 704—711.
TRAJECTORY CONTROL OF SOLID BODY SPATIAL MOTION
Wang Jian1, A. Yu. Krasnov2, Yu. A. Kapitanyuk3, S. A. Chepinsky1,2, S. A. Kholunin2, Chen Yifan2, Liu Huimin2, D. A. Khvostov2
1Hangzhou Dianzi University, School of Automation, 310018, Hangzhou, China
2ITMO University, 197101, St. Petersburg, Russia E-mail: [email protected]
3University of Groningen, 9700 AB, Groningen, Netherlands
The problem of synthesis of the trajectory control algorithm for a solid body moving with a preas-signed speed along a spatial trajectory given in an implicit form, is considered. The control law is synthesized using differential geometry methods through the nonlinear transformation of the initial dynamic model. Effectiveness of the proposed mathematical model of spatial motion and corresponding nonlinear control algorithm is confirmed by presented results of computer simulation. The formulated control laws is supposed to be useful in development of systems of trajectory control over airborne and underwater mobile robots.
Keywords: algorithm, motion, orientation, trajectory control, coordinates transformation Wang Jian
Aleksander Yu. Krasnov — Yury A. Kapitanyuk —
Sergey A. Chepinsky
Sergey A. Kholunin Chen Yifan Liu Huimin Denis A. Khvostov
For citation: Wang Jian, Krasnov А. Yu., Kapitanyuk Yu. А., Chepinsky S. А., Kholunin S. А., Chen Yifan, Liu Huimin, Khvostov D. А. Trajectory control of solid body spatial motion. Journal of Instrument Engineering. 2017. Vol. 60, N 8. P. 704—711 (in Russian).
DOI: 10.17586/0021-3454-2017-60-8-704-711
Data on authors
PhD; Hangzhou Dianzi University, School of Automation; ITMO University, Department of Control Systems and Informatics; E-mail: [email protected]
Post-Graduate Student, ITMO University, Department of Control Systems and Informatics; E-mail: [email protected] Post-Graduate Student; University of Groningen, Department of Discrete Technology and Production Automation; E-mail: [email protected]
PhD; Hangzhou Dianzi University, School of Automation; ITMO University, Department of Control Systems and Informatics; E-mail: [email protected]
PhD; ITMO University, Department of Control Systems and Informatics; E-mail: [email protected]
Post-Graduate Student, ITMO University, Department of Control Systems and Informatics; E-mail: [email protected] Post-Graduate Student, ITMO University, Department of Control Systems and Informatics; E-mail: [email protected] Post-Graduate Student, ITMO University, Department of Control Systems and Informatics; E-mail: [email protected]
