Например, Бобцов

ТРАЕКТОРНОЕ УПРАВЛЕНИЕ ТВЕРДЫМ ТЕЛОМ ОТНОСИТЕЛЬНО ПОДВИЖНОГО ОБЪЕКТА

ТРАЕКТОРНОЕ УПРАВЛЕНИЕ ТВЕРДЫМ ТЕЛОМ ...

УДК 681.5
ТРАЕКТОРНОЕ УПРАВЛЕНИЕ ТВЕРДЫМ ТЕЛОМ ОТНОСИТЕЛЬНО ПОДВИЖНОГО ОБЪЕКТА1
Ю.А. Капитанюка, Д.А. Хвостова, С.А. Чепинскийа
а Санкт-Петербургский национальный исследовательский университет информационных технологий, механики и оптики (Университет ИТМО), Санкт-Петербург, Россия, Chepinsky_S@hotmail.com
Рассматривается задача динамического сопровождения внешнего подвижного объекта. Такие задачи возникают при построении систем слежения беспилотными летательными аппаратами за наземными подвижными объектами. В качестве модели объекта управления рассматривается динамическая модель твердого тела в трехмерном пространстве. Подвижный внешний объект задается кинематической моделью твердого тела на плоскости. Гладкая траектория, вдоль который должен осуществлять движение объект управления, представляется в виде неявно заданной кривой в связанной с внешним подвижным объектом системе координат. Дополнительно задается желаемая высота движения. Для построения алгоритма управления рассматривается относительная динамика объекта управления и внешнего подвижного объекта и применяются методы дифференциально-геометрического преобразования исходной модели к задачно-ориентированной системе координат, формулирующей исходную задачу в терминах продольного движения и двух ортогональных отклонений, для которой строятся пропорционально-дифференциальные алгоритмы управления с прямой компенсацией нелинейностей. Основные результаты представлены задачно-ориентированной моделью пространственного движения и соответствующими нелинейными алгоритмами управления. Для иллюстрации работоспособности предлагаемого метода приводится пример моделирования движения твердого тела вдоль круговой траектории, заданной относительно прямолинейно движущегося внешнего объекта. Ключевые слова: траекторное управление, управление движением, нелинейное управление.
TRAJECTORY CONTROL OF A SOLID BODY RELATIVE TO THE MOVABLE OBJECT1
Y.A. Kapitanyukа, D.A. Khvostovа, S.A. Chepinskiyа
а Saint Petersburg National Research University of Information Technologies, Mechanics and Optics (ITMO University), Saint Petersburg, Russia, Chepinsky_S@hotmail.com
The paper deals with the problem of dynamic tracking of an external movable object. These problems arise when designing tracking control systems for unmanned aircrafts following ground movable objects. A dynamic model of a solid body in three-dimensional space is selected as a control object model. An external object is described by the kinematic model of a solid body on the plane. Smooth trajectory is defined as an implicit curve associated with an external movable object. The desired height of movement is selected separately. Relative dynamics of the plant and an external movable object is considered for the synthesis of control algorithm, and methods of differential geometric transformation of the original model to the task-oriented coordinate system are applied. The original problem is formulated in terms of a longitudinal motion and two orthogonal deviations after transformation. The main results are represented by task-oriented model of spatial movement and the corresponding nonlinear control algorithms. An example of solid body motion along a circular trajectory with respect to a given rectilinear motion of an external object is given for the illustration of the proposed method performance. Keywords: trajectory control, motion control, nonlinear control.
Введение
В работе рассматривается задача управления движением твердого тела в пространстве вдоль траектории, заданной относительно внешнего подвижного объекта. Такая постановка задачи является естественным расширением классической постановки задачи со стационарным заданием желаемого пути следования. С практической точки зрения внешними объектами могут быть разнообразные препятствия, которые необходимо объехать, или другие роботы, относительно которых может быть задана цель управления, к примеру, необходимо сопровождать цель, совершая при этом круговые движения на заданном расстоянии от нее [1, 2].
Подход к управлению, используемый в работе, предусматривает нелинейное преобразование модели робота к системе задачно-ориентированных координат. Это дает возможность свести сложную многоканальную задачу управления к ряду простых задач компенсации линейных и угловых отклонений, а затем с помощью стандартных приемов нелинейной стабилизации [3, 4] найти адекватные законы управления. Основной результат является развитием известных решений задач управления пространственным движением, предложенных в [3–12], расширенным на случай движения относительно траекторий, связанных с локальной системой координат подвижных объектов.
Постановка задачи управления
В качестве модели робота рассмотрим уравнения движения твердого тела в пространстве [4] (рис. 1):

1 Работа выполнена при государственной финансовой поддержке ведущих университетов Российской Федерации (субсидия 074-U01). This work was financially supported by the Government of the Russian Federation, Grant 074-U01.

60

Научно-технический вестник информационных технологий, механики и оптики Scientific and Technical Journal of Information Technologies, Mechanics and Optics

2014, №2 (90)

Ю.А. Капитанюк, Д.А. Хвостов, С.А. Чепинский

x  v, mv  F, R  S(ω)R, Jω  ω  Jω  M,

(1)

где x   x, y, zT  R3 – положение твердого тела в инерциальной системе координат XYZ ;

v  vx , vy , vz T  R3 – скорости твердого тела в инерциальной системе координат XYZ ; m – масса твердого тела; F  [Fx , Fy , Fz ]T  R3 – управляющая сила, приложенная к твердому телу;
ω  ωφ , ωθ , ωψ T  R3 – угловые скорости твердого тела в связанной системе координат X Y Z  ; J – момент инерции твердого тела; M  [Mφ , M , M ]T  R3 – управляющий момент, приложенный к твердому телу; R  SO(3) – матрица направляющих косинусов, характеризующая положение твердого тела в

инерциальной системе координат XYZ ;  – векторное произведение; S(ω)  SO(3) – кососимметриче-
ская матрица вида

0

S(ω)



 



 0

 φ

  

.

  φ 0 

Z Z

X C 1(rx, ry) Y S

O Y

2(z)=z–z* C0 X

Рис. 1. Твердое тело и траектория заданная относительно подвижного объекта

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

xoo

 

v*o , *o ,

(2)

где  xo  xo , yo T  R2 – положение твердого тела в инерциальной системе координат XY ;

vo  vox , voy T  R2 – скорости твердого тела в инерциальной системе координат XY ; o  R, o  R –
угол поворота и угловая скорость соответственно. Положение робота относительно объекта представим в подвижной системе координат, связанной с

точкой Co (рис. 1). Получаем следующую модель:

rx  cos o r  ry   sin o
rz  0

sin o cos o
0

0  x  xo 

0



 

y



yo

 



Ro

(o

)Δr

,

1  z  z 

(3)

где r  R3 – положение твердого тела в системе координат подвижного объекта; z  R – заданная высо-

та движения твердого тела.

Чтобы найти относительные скорости движения робота и внешнего объекта, продифференцируем

уравнение (3):

r  R o (o )Δr  Ro (o )Δr  So (o )Ro (o )Δr  Ro (o )(v  vo ),

(4)

0 So (o )  o

o 0

0 0 .

 0 0 0

Научно-технический вестник информационных технологий, механики и оптики Scientific and Technical Journal of Information Technologies, Mechanics and Optics 2014, №2 (90)

61

ТРАЕКТОРНОЕ УПРАВЛЕНИЕ ТВЕРДЫМ ТЕЛОМ ...

Теперь предположим, что траектория робота, задаваемая относительно внешнего подвижного объекта, является гладким отрезком кривой S (рис. 1), неявное описание которого имеет вид

1(rx , ry )  0. Желаемая высота задается с помощью соотношения

(5)

2 (z)  z  z  0.

(6)

Касательная скорость к заданной траектории движения в данной постановке задачи определяется

выражением

s  1  2 r. 1  2

(7)

Задача управления ставится следующим образом: необходимо осуществить стабилизацию движения мо-

бильного робота вдоль этой заданной траектории (рис. 1). Предполагается, что функции 1 выбрана таким образом, что при y  S матрица Якоби

 1

 

ry



1 rx

0 

Υ(r) 

1

1 rx

2



1 2 ry

  

1 rx

 

0

1 ry
0

0 r 
1

 

ортогональна. Тогда (8) удовлетворяет уравнению типа Френе [6]:

Υ (r)  sS(r) Υ(r),

(8) (9)

 0 (r) 0
S (r)  (r) 0 0 ,
 0 0 0

(r)





21 rx 2

 

1 ry

2 



 2 1 ry 2

  

1 rx

2  



2

 2 1 rx ry

  

1 rx

2



1 ry

2

3  

1 rx

1 ry

,

где (r) – кривизна кривой [4].

При переходе к математическому описанию поставленной выше задачи управления траекторным движением автономного робота она может быть сформулирована как задача поддержания голономных соотношений между выходами системы, заданных в форме (5) и (6). Она дополняется описанием желае-

мого режима продольного движения основной точки s(t) , обычно устанавливаемого с помощью эталон-

ной переменной s (t) или эталонной скорости продольного движения Vs  s (t) . Введем в рассмотрение ошибки траекторного движения [4–6]. Нарушение условий (6) характери-
зуется ортогональными отклонениями e1  1(rx , ry )  e2  2 (z),
принимающими на множестве S нулевые значения. Таким образом, задача траекторного управления автономным роботом, модель движения которого
описывается уравнениями (1)–(2), заключается в определении (в замкнутой форме) управляющего воздействия F , которое обеспечивает: 1. стабилизацию движения робота относительно кривой S, заданной на основе информации о подвиж-
ном объекте, что подразумевает обнуление вектора пространственных отклонений e1 и e2 ;
2. поддержание требуемого режима продольного движения мобильного робота s  s (t) , часто задавае-
мого с помощью простейшей эталонной модели
s  Vs  const, или обнуления скоростной ошибки
Vs  Vs  s . В данной работе мы считаем, что все переменные вектора состояния доступны измерению, а параметры объекта известны.

62

Научно-технический вестник информационных технологий, механики и оптики Scientific and Technical Journal of Information Technologies, Mechanics and Optics

2014, №2 (90)

Ю.А. Капитанюк, Д.А. Хвостов, С.А. Чепинский

Синтез алгоритмов управления движением

Предложим алгоритм синтеза управления траекторным движением на основе подхода, представленного в работах [1, 3, 4, 6]. Этот алгоритм состоит из следующей последовательности действий. 1. Введение в рассмотрение первой и второй производных задачно-ориентированных переменных (про-
странственные отклонения e1 и e2 , и длина пройденного пути s ) с учетом (3)–(9):
s

e1  Υ(r)r  Υ(r)So (o )Ro (o )Δr  Υ(r)Ro (o )v  Υ(r)Ro (o )vo , e2

s s

e1 e2

 sS((r)) e1 e2



Υ(r)So2

(o

)Ro

(o

)Δr



Υ(r)So

(o

)Ro

(o

)(v



vo

)



1 m

Υ(r)Ro

(o

)F.

2. Введение виртуальных (локальных) управлений us , ue и u :

Υ(r)So2 (o )Ro (o )Δr

 Υ(r)So (o )Ro (o )(v



vo ) 

1 m

Υ(r)Ro (o )F



us ue1 ue2

.

3. Синтез локальных регуляторов:

us  k1(Vs  s), ue1  s2(r)  ke1d e1  ke1pe1, ue2  ke2d e2  ke2 pe2 ,

где k1, ke1d , ke1p , ke2d , ke2 p – положительные константы, выбираемые исходя из желаемой динамики. 4. Синтез регулятора, решающего указанную траекторную задачу:



F



mR

T o

(o

)ΥT

(r)

 

us ue1





Υ(r)So2

(o

)R

o

(

o

)Δr



Υ(r)So

(o

)Ro

(

o

)(

v



vo

)

 

.

 ue2



В результате получен алгоритм управления движением вдоль траектории, которая задана относи-

тельно подвижного объекта, дополняющий результаты, представленные в [1–8] и позволяющий естест-

венным образом решать такие задачи, как слежение за заданной целью или объезд подвижных и непод-

вижных препятствий.

Y 4 S* 3

2

1 0

V*=0,1

–1

–2

–3 –4 V=1

–5

–2 0 2 4 6 8 10 12 14 X

Рис. 2. Движение вокруг движущегося объекта

Результаты моделирования

Для иллюстрации рассмотрим задачу слежения роботом за внешним объектом. При этом робот должен перемещаться вдоль заданной в системе координат этого объекта траектории S * . В качестве желаемой траектории выбрана окружность с радиусом R = 6 м, описываемая выражением
1(rx , ry )  rx2  ry2  R2  rx2  ry2  36  0 .
Результат моделирования для случая движения робота с заданной скоростью V  1 м/с вокруг
движущего со скоростью V *  0,1 м/с объекта представлен на рис. 2. Как видно на рис. 2, управляемый
автономный робот выходит на заданную круговую траекторию и точно следует вдоль окружности, заданной в системе координат подвижного объекта. Такого рода задачи встают перед разработчиками беспилотных летательных аппаратов самолетного типа, так как последние не могут зависать на одном месте.

Научно-технический вестник информационных технологий, механики и оптики Scientific and Technical Journal of Information Technologies, Mechanics and Optics 2014, №2 (90)

63

ТРАЕКТОРНОЕ УПРАВЛЕНИЕ ТВЕРДЫМ ТЕЛОМ ...

Заключение

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

References

1. Oliveira T., Encarnacao P., Aguiar A.P. Moving path following for autonomous robotic vehicles. Proc. of 12th European Control Conference, ECC 2013. Zurich, Switzerland, 2013, art. no. 6669459, pp. 3320–3325.
2. Oliveira T., Encarnacao P. Ground target tracking control system for unmanned aerial vehicles. Journal of Intelligent and Robotic Systems: Theory and Applications, 2013, vol. 69, pp. 373–387. doi: 10.1007/s10846-012-9719-0
3. Burdakov S.F., Miroshnik I.V., Stel’makov R.E. Sistemy upravleniya dvizheniem kolesnykh robotov [Motion control systems of wheeled robots]. St. Petersburg, Nauka Publ., 2001, 232 p.
4. Miroshnik I.V., Nikiforov V.O., Fradkov A.L. Nelineinoe i adaptivnoe upravlenie slozhnymi dinamicheskimi sistemami [Nonlinear and adaptive control of complex dynamic systems]. St. Petersburg, Nauka Publ., 2000, 549 p.
5. Bushuev A.B., Isaeva E.G., Morozov S.N., Chepinskiy S.A. Upravlenie traektornym dvizheniem mnogokanal’nykh dinamicheskikh system [Control over trajectory motion of multichannel dynamic system]. Izv. vuzov. Priborostroenie, 2009, vol. 52, no. 11, pp. 50–56.
6. Kapitanyuk Yu.A., Chepinskiy S.A. Upravlenie mobil’nym robotom po zadannoi kusochno-gladkoi traektorii [Control of the mobile robot motion along a predetermined piecewise smooth path]. Gyroscopy and Navigation, 2013, no. 2, pp. 42–52.
7. Kapitanyuk Yu.A., Chepinskiy S.A. Zadacha upravleniya mnogokanal’noi dinamicheskoi sistemoi po kusochno-gladkoi traektorii [Task of control for multichannel dynamic system over piecewise smooth trajectory]. Izv. vuzov. Priborostroenie, 2013, vol. 56, no. 4, pp. 65–70.
8. Miroshnik I.V. Soglasovannoe upravlenie mnogokanal’nymi sistemami [Matched control of multi-channel systems]. Leningrad, Energoatomizdat Publ., 1990, 128 p.
9. Miroshnik I.V., Chepinskiy S.A. Upravlenie mnogozvennymi kinematicheskimi mekhanizmami [Control of multilink kinematic mechanisms]. Scientific and Technical Journal of Information Technologies, Mechanics and Optics, 2001, no. 3 (3), pp. 144–149.
10. Chepinskiy S.A., Miroshnik I.V. Traektornoe upravlenie kinematicheskimi mekhanizmami netrivial’noi konstruktsii [Kinematic trajectory control mechanisms with nontrivial design]. Scientific and Technical Journal of Information Technologies, Mechanics and Optics, 2004, no. 3 (14), pp. 4–10.
11. Kapitanyuk Yu.A., Chepinskiy S.A. Traektornoe upravlenie mobil'nym robotom v izmenyayushcheisya srede [Trajectory control of mobile robot in a changing environment]. Materialy ХIV Konferentsii molodykh uchenykh “Navigatsiya i upravlenie dvizheniem” [Proc. of ХIV Conference of young scientists “Navigation and motion control”]. St. Petersburg, 2012, pp. 506–512.
12. Bobtsov A.A., Kapitanyuk Yu.A., Kapitonov A.A., Kolyubin S.A., Pyrkin A.A., Chepinskiy S.A., Shavetov S.V. Tekhnologiya Lego Mindstorms NXT v obuchenii studentov osnovam adaptivnogo upravleniya [Lego Mindstorms NXT for teaching the principles of adaptive control theory to students]. Scientific and Technical Journal of Information Technologies, Mechanics and Optics, 2011, no. 1 (71), pp. 103–108.

Капитанюк Юрий Андреевич Хвостов Денис Алексеевич Чепинский Сергей Алексеевич

– аспирант, Санкт-Петербургский национальный исследовательский

университет информационных технологий, механики и оптики (Уни-

верситет

ИТМО),

Санкт-Петербург,

Россия,

yura.kapitanyuk@gmail.com

– студент, Санкт-Петербургский национальный исследовательский

университет информационных технологий, механики и оптики (Уни-

верситет ИТМО), Санкт-Петербург, Россия, talionar@yandex.ru

– кандидат технических наук, доцент, Санкт-Петербургский нацио-

нальный исследовательский университет информационных техноло-

гий, механики и оптики (Университет ИТМО), Санкт-Петербург, Рос-

сия, Chepinsky_S@hotmail.com

Yuri A. Kapitanyuk Denis A. Khvostov Sergei A. Chepinskiy

– postgraduate, Saint Petersburg National Research University of Information Technologies, Mechanics and Optics (University ITMO), Saint Petersburg, Russia, yura.kapitanyuk@gmail.com
– student, Saint Petersburg National Research University of Information Technologies, Mechanics and Optics (University ITMO), Saint Petersburg, Russia, talionar@yandex.ru
– PhD, Associate professor, Saint Petersburg National Research University of Information Technologies, Mechanics and Optics (University ITMO), Saint Petersburg, Russia, Chepinsky_S@hotmail.com Принято к печати 22.01.14 Accepted 22.01.14

64

Научно-технический вестник информационных технологий, механики и оптики Scientific and Technical Journal of Information Technologies, Mechanics and Optics

2014, №2 (90)