Санкт-Петербургский государственный университет
Кафедра механики управляемого движения
Тимченко Богдан Павлович
Выпускная квалификационная работа
бакалавра
Управление движением
космического манипуляционного
робота
010400
Прикладная математика и информатика
Научный руководитель,
кандидат физ.-мат. наук,
доцент
Шиманчук Д.В.
Санкт-Петербург
2016
Содержание
Введение . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Глава 1. Постановка задачи управляемого движения космического
манипуляционного робота . . . . . . . . . . . . . . . . . . . . . . . . .
1.1. Модель космического манипуляционного робота . . . . . . .
1.2. Постановка задачи . . . . . . . . . . . . . . . . . . . . . . . .
Глава 2. Уравнения движения космического манипуляционного робота
2.1. Динамическая модель космического манипуляционного робота
2.2. Построение функции кинетической энергии . . . . . . . . . .
2.3. Неуправляемое движение космического манипуляционного
робота . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.4. Управляемое движение космического манипуляционного робота . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Примеры . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Заключение . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Список литературы . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Приложение . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
3
7
7
9
10
10
11
13
18
20
27
28
29
Введение
Прогресс не стоит на месте. Он, как и человечество развивается быстрыми темпами. Инновации находят свое место практически во всех сферах
деятельности человека. В ХХ веке, все чаще деятельность человека оказывалась в среде, где условия окружающего пространства представляли
опасность для жизни и здоровья. Постепенно, возникла потребность замены человека роботами. Сначала роботы стали появляться на заводах,
где выполняли трудоемкие операции. Потом, с развитием прогресса, они
начали появляться в самых разных областях деятельности человека.
Так, в середине ХХ века, вследствие удачного начала освоения космического пространства, одним из перспективных направлений робототехники стала разработка управляемых космических манипуляционных систем
и роботов для выполнения монтажных и ремонтных работ, а так же для
взаимодействия с другими космическими объектами. Работа космонавтов
на станции и в открытом космосе связана с высокой опасностью и ограниченностью ресурсов, а дополнительные «руки» позволили существенно
облегчить работу.
Использование робототехники для исследования и освоения космоса
является одним из самых знаковых направлений науки и техники. За полувековую историю космического роботостроения было разработано множество устройств, многие из которых используются и сейчас.
Основное преимущество космических роботов в том, что в отличие от
человека, они могут работать в крайне неблагоприятных условиях и практически без каких-либо ресурсов, ведь в большинстве случаев они работают
на солнечных батареях.
С задачами космических роботов способны справиться и обычные
роботы, используемые на Земле, но в связи с тем, что работа происходит в
космосе, появляются несколько дополнительных требований к конструкции
робота:
- способность перенести запуск корабля;
- функционирование в неблагоприятных условиях;
- минимально-возможный вес;
3
- долгий срок службы при минимальных энергозатратах;
- возможность работы в автономном режиме;
- возможность работы при удаленном управлении;
- повышенная надежность.
Для соответствия этим и другим предъявляемым требованиям, ученые разрабатывают новые и более совершенные комплектующие для роботов.
Одним из многих положительных качеств является заменимость роботов, ведь пережить потерю робота гораздо легче, чем гибель космонавта.
В список основных задач космических роботов входят такие работы
как сборка на орбите космических объектов, техническое обслуживание
спутников, кораблей и орбитальной станции, а так же очистка космического пространства от мусора.
Космическая робототехника позволяет существенно расширить возможности беспилотных космических аппаратов, доводя их практически до
уровня пилотируемых кораблей.
Роботы-манипуляторы, по сути, являются имитацией человеческой
руки. Они представлены в виде многозвенных устройств с захватом на
конце и используются практически во всех сферах деятельности человека,
в том числе и в космосе.
Приведем краткий обзор манипуляторов эксплуатируемых на Международной Космической Станции Рассмотрим некоторые манипуляторы,
используемые на МКС.
Манипулятор «ERA» (Европа) [1], одной из наиболее интересных
функций которого является способность перемещаться между заранее определёнными базовыми креплениями, установленными на обшивке станции,
используя собственную систему управления. Это даёт возможность расширения зоны полезной работы, и достигается при помощи универсальной
конструкции двух концевых эффекторов, которые могут действовать как
рабочие органы или присоединяться к креплениям, размещённым на обшивке станции. Так как главный манипулятор станции «Канадарм-2» не
может работать на российском сегменте МКС из-за ряда конструктивных
4
ограничений, «ERA» служит основным манипулятором российского сегмента МКС и выполняет следующие задачи:
- обследование внешней поверхности станции;
- работа с оборудованием, размещённым на поверхности МКС;
- перемещение грузов массой до 8000 кг с точностью наводки до 5 мм.
Манипулятор «ERA» является симметричным механизмом, имеющим семь степеней подвижности. Он состоит из двух одинаковых штанг
(длиной по 5 м каждая), соединенных шарниром. На концах этих штанг
находятся одинаковые крепежные системы, поэтому робот может прикрепляться к станции любым концом и перемещаться по ее поверхности, подобно пауку. Также оснащен подсистемами телевидения, освещения, телеметрии и другими, обеспечивающими контроль над его работой.
Бортовой манипулятор «Канадарм-2» [2], представляет собой многозвенный механизм с электромеханическим приводом, длиной 17.6 метра,
семью степенями свободы и предназначен для выполнения операций на
орбите с многотонными грузами:
- выгрузка доставленного груза;
- размещение объектов на околоземной орбите или помещение их в состав орбитальной станции;
- захват свободно летающего в космосе объекта и погрузка его с последующим возвращением на Землю;
- перемещение астронавтов во время работы в открытом космосе.
Японский экспериментальный блок «KIBO» [3] состоит следующих
модулей:
- Научная лаборатория:
Это центральная часть блока, которая позволяет проводить все виды
экспериментов в условиях невесомости. Внутри модуля установлено 10
экспериментальных блоков.
5
- Багажный модуль:
Выполняет роль хранилища оборудования, в котором находятся перемещаемые контейнеры.
- Рука-манипулятор JEM RMS:
Обслуживает внешний грузовой блок. Основная часть руки (9.9 метра)
переносит тяжелые объекты, а для деликатной работы используется
малая съемная рука (1.9 метра). Оснащены видеокамерами, которые
позволяют точно управлять движениями руки.
В отличие от всех этих робототехнических систем космического назначения, в работе исследуется модель космического манипуляционного
робота, который функционирует в окрестности космической станции [4],
в качестве помощника астронавта.
6
Глава 1. Постановка задачи управляемого
движения космического манипуляционного
робота
1.1. Модель космического манипуляционного робота
Космический манипуляционный робот является сложной механической системой, которая состоит из несущего тела и подсистемы носимых
тел, связанной с ним шарнирно. Одним из носимых тел является трехзвенный манипулятор с концевым схватом.
Идеализированная модель манипулятора в виде системы шарнирно
связанных между собой жестких звеньев длиной ri представлена на Рис.1.
Распределенные массы звеньев отнесены к шарнирным сочленениям и совместно с ними рассматриваются в виде материальных точек mi , а сами
звенья при этом предполагаются невесомыми. Дополнительным элементом этой системы может оказаться захваченный манипулятором полезный
груз.
Рис. 1: Кинематическая схема космического манипуляционного робота
7
Условные обозначения
Системы координат:
OA XY - инерциальная (базовая) СК, формируемая радиотехническими
или иными средствами на борту пилотируемой станции;
OX 0 Y 0 - подвижная СК с началом в полюсе O, совмещенном с центром
масс корпуса космического манипуляционного робота, и перемещающаяся вместе с ним поступательно относительно осей базовой СК;
Ox0 y0 - связанная СК, начало которой совпадает с центром масс несущего
тела, а оси совмещены с его главными осями инерции.
axa ya - СК манипулятора с началом в точке a = (xa , ya ), координаты
которой определяют место крепления манипулятора к корпусу робота.
Регулируемые координаты:
- положение полюса O и центра масс C на траектории движения;
- угловое отклонение несущего тела ϑ в осях инерциальной СК;
- координаты α1 , α2 , α3 конфигурирующие манипулятор в связанной СК.
Независимые обобщенные координаты qj , однозначно определяющие
состояние и конфигурацию механической системы в любой момент времени:
q1 = X0 , q2 = Y0 - положение полюса о в инерциальной СК;
q3 = ϑ - угловое отклонение связанной СК относительно подвижной;
q4 = α1 , q5 = α2 , q6 = α3 - межзвенные углы манипулятора.
Управляющие воздействия:
- реактивные силы Fx ,Fy , приложенные вдоль осей x0 , y0 к полюсу O;
- моменты Mϑ и Mαj .
8
1.2. Постановка задачи
Построить уравнения управляемого движения космического манипуляционного робота в обобщенных координатах qi (t), i = 1 . . . 6 . Определить
управляющие силы и моменты, необходимые для реализации заданного
движения характеристической точки манипулятора.
Предварительно необходимо рассмотреть следующие подзадачи [5]:
1. Первая прямая задача кинематики - определить ориентацию звеньев и
координат точек робота.
2. Вторая прямая задача кинематики - определить кинематические параметры робота, скорости и ускорения точек робота в определенный
момент времени.
9
Глава 2. Уравнения движения космического
манипуляционного робота
2.1. Динамическая модель космического
манипуляционного робота
Механическая структура космического манипуляционного робота является свободной механической системой, которая подчиняется внутренним голономным не освобождающим стационарным связям. Для описания
динамики данных систем удобно будет использовать уравнения Лагранжа
2го рода, записанные в виде [4-6]:
∂T
d ∂T
−
= Q,
dt ∂ q̇
∂q
где q - шестимерный вектор обобщенных координат,
T - кинетическая энергия движения системы,
Q - вектор управляющих обобщенных сил.
10
(1)
2.2. Построение функции кинетической энергии
Предположим, что в момент времени t положение системы на плоскости OA XY для несущего тела задается вектором q 0 = (X0 , Y0 , ϑ), а точки
положения манипулятора m1 , m2 и m3 имеют проекции x0i и yi0 на оси
системы координат Ox0 y0 .
Так как начало этой системы координат совпадает с центром масс
несущего тела, то по теореме о движении центра инерции [6] положение
центра масс космического манипуляционного робота в проекциях на эти
же оси имеет вид:
x0c
3
1 X
mi x0i ,
=
M i=1
3
1 X
0
yc =
mi yi0 ,
M i=1
(2)
где M - это суммарная масса космического манипуляционного робота.
Проекции материальных точек на оси подвижной системы координат
задаются формулами:
xi =x0i cos ϑ − yi0 sin ϑ,
yi =x0i sin ϑ + yi0 cos ϑ.
(3)
Проекции относительных скоростей этих точек можно получить дифференцированием выражения:
ẋi = ẋ0i cos ϑ−ẏi0 sin ϑ − ϑ̇(x0i sin ϑ + yi0 cos ϑ),
ẏi = ẋ0i sin ϑ+ẏi0 cos ϑ + ϑ̇(x0i cos ϑ − yi0 sin ϑ).
(4)
Аналогично можем получить проекции относительной скорости центра масс космического манипуляционного робота на оси подвижной системы координат:
xc = x0c cos ϑ − yc0 sin ϑ,
yc = x0c sin ϑ + yc0 cos ϑ.
11
(5)
И, следовательно, проекции относительных скоростей:
ẋc = ẋ0c cos ϑ − ẏc0 sin ϑ − ϑ̇(x0c sin ϑ + yc0 cos ϑ),
ẏc = ẋ0c sin ϑ + ẏc0 cos ϑ + ϑ̇(x0c cos ϑ − yc0 sin ϑ).
(6)
Соответственно из (2) теперь получим:
ẋ0c
3
1 X
=
mi ẋ0i ,
M i=1
3
1 X
0
ẏc =
mi ẏi0 .
M i=1
(7)
Применяя теорему Кёнига [6] к системе, так как скорость ее центра маг относительно подвижной системы отсчета не равна нулю (полюс
движется относительно центра), можем записать функцию кинетической
энергии абсолютного движения космического манипуляционного робота:
3
1X
M 2
1
2
2
mi [(ẋ0i − yi0 ϑ̇)2 + (ẏi0 + x0i ϑ̇)2 ]+
T = (Ẋ0 + Ẏ0 ) + J0 ϑ̇ +
2
2
2 i=1
+(Ẋ0 cos ϑ + Ẏ0 sin ϑ)
+(Ẏ0 cos ϑ − Ẋ0 sin ϑ)
3
X
i=1
3
X
mi (ẋ0i − yi0 ϑ̇)+
(8)
mi (ẏi0 + x0i ϑ̇).
i=1
Полученное соотношение задает функцию кинетической энергии абсолютного движения космического манипуляционного робота, выраженную через декартовы и угловые координаты, определяющие положения
всех материальных тел рассматриваемой системы в инерциальной системе координат.
12
2.3. Неуправляемое движение космического
манипуляционного робота
Для построения уравнения неуправляемого движения космического
манипуляционного робота в форме (1) (когда Q = 0), в определении кинетической энергии необходимо перейти от декартовых координат к обобщенным.
Из схемы космического манипуляционного робота, представленной на
Рис. 1, можно выписать соотношения, необходимые для перехода:
x01 =xa − r1 sin(α1 ),
y10 = ya − r1 cos(α1 ),
x02 =x01 + r2 sin(α1−2 ), y20 = y10 + r2 cos(α1−2 ),
x03 =x02 + r3 sin(α1−3 ), y30 = y20 + r3 cos(α1−3 ),
(9)
αk−s = αk + αk+1 + · · · +αs−1 + αs .
Заметим, что правые части (9) не содержат времени, следовательно, кинетическая энергия (8) для рассматриваемой системы, будет квадратичной формой относительно обобщенных скоростей q̇i с коэффициентами,
зависящими от параметров и координат системы [7].
Далее, при помощи дифференцирования получим выражения скоростей:
ẋ01 = −r1 α̇1 cos(α1 ), ẏ10 = r1 α̇1 sin(α1 ),
ẋ02 = ẋ01 + r2 α̇1−2 cos(α1−2 ), ẏ20 = ẏ10 − r2 α̇1−2 sin(α1−2 ),
(10)
ẋ03 = ẋ02 + r3 α̇1−3 cos(α1−3 ), ẏ30 = ẏ20 − r3 α̇1−3 sin(α1−3 ),
и ускорений точек системы:
ẍ01 = −r1 (α̈1 cos(α1 ) − α̇12 sin(α1 )),
ÿ10 = r1 (α̈1 sin(α1 ) + α̇12 cos(α1 )),
2
ẍ02 = ẍ01 + r2 (α̈1−2 cos(α1−2 ) − α̇1−2
sin(α1−2 )),
2
ÿ20 = ÿ10 − r2 (α̈1−2 sin(α1−2 ) + α̇1−2
cos(α1−2 )),
2
ẍ03 = ẍ02 + r3 (α̈1−3 cos(α1−3 ) − α̇1−3
sin(α1−3 )),
2
ÿ30 = ÿ20 − r3 (α̈1−3 sin(α1−3 ) + α̇1−3
cos(α1−3 )).
13
(11)
Таким образом, все необходимое для операций с функцией кинетической энергии получено.
Подставляя обобщенные координаты в (1) получим:
d ∂T
∂T
−
= 0,
dt ∂ Ẋ0 ∂X0
∂T
d ∂T
−
= 0.
dt ∂ Y˙0 ∂Y0
(12)
Далее, подставляя в (12) уравнение кинетической энергии (8) получим:
M Ẍ0 +
3
X
mi [(ẍ0i cos ϑ − ÿi0 sin ϑ) − ϑ̈(x0i sin ϑ + yi0 cos ϑ)−
i=1
−2ϑ̇(ẋ0i sin ϑ + ẏi0 cos ϑ) + ϑ̇2 (yi0 sin ϑ − x0i cos ϑ)] = 0,
M Ÿ0 +
3
X
(13)
mi [(ẍ0i sin ϑ + ÿi0 cos ϑ) + ϑ̈(x0i cos ϑ + yi0 sin ϑ)+
i=1
+2ϑ̇(ẋ0i cos ϑ − ẏi0 sin ϑ) − ϑ̇2 (x0i sin ϑ + yi0 cos ϑ)] = 0.
Можно заметить, что на движение полюса O, кроме активных сил,
оказывают большое влияние внутренние силы, порождаемые реакциями
относительного движения носимых тел и отображенные в уравнениях как
группы выражений находящихся под знаком суммы. Из выражений (13)
видно, что подобные группы слагаемых представляют собой проекции относительных ускорений ẍc , ÿc центра масс системы на оси подвижной системы координат.
Значит (13) можно записать как уравнения движения центра масс
космического манипуляционного робота:
M (Ẍ0 + ẍc ) =M Ẍc = 0,
M (Ÿ0 + ÿc ) =M Ÿc = 0.
(14)
Теперь выпишем уравнения Лагранжа 2го рода для третьей обобщенной координаты - ϑ:
∂T
d ∂T
−
= 0.
dt ∂ ϑ̇
∂ϑ
14
(15)
Действуя аналогично действиям с (12), выпишем последнее уравнение несущего тела:
ϑ̈[J0 +
3
X
2
mi (x0i
+
2
yi0 )]
− Ẍ0
i=1
+Y¨0
3
X
mi (x0i sin ϑ + yi0 cos ϑ)+
i=1
3
X
mi (x0i cos ϑ − yi0 sin ϑ)+
(16)
i=1
+
3
X
h
i
0 0
0 0
0 0
0 0
mi (xi ÿi − yi ẍi ) + 2ϑ̇(xi ẋi − yi ẏi ) = 0.
i=1
Исключая ускорение полюса, получаем уравнение углового движения
несущего тела относительно центра масс космического манипуляционного
робота:
c
J ϑ̈ +
3
X
h
mi ÿi0 (x0i
−
x0c )
−
ẍ0i (yi0
−
yc0 )
+
2ϑ̇(ẋ0i (x0i
+
x0c )
−
ẏi0 (yi0
+
i
yc0 ))
= 0,
i=1
(17)
P
2
2
2
2
где J c = J0 + 3i=1 mi (x0i + yi0 ) − M (x0c + yc0 ) - осевой момент инерции космического манипуляционного робота.
Для упрощения описания динамики носимых тел, в уравнениях (13)
и (16), нелинейные члены переносятся в правые части уравнений, добавляясь к обобщенным силам, так как они представляют собой обобщенные
центростремительные, центробежные и кориолисовы силы, приложенные
к несущему телу и появляющиеся в результате относительного движения
носимых тел [6]. В итоге получим:
M Ẍ0 − ϑ̈
3
X
mi (x0i sin ϑ
+
yi0 cos ϑ)
i=1
+
3
X
C
mi (ẍ0i cos ϑ − ÿi0 sin ϑ) = QK
X + QX ,
i=1
(18)
P3
+ ẏi0 cos ϑ) - кориолисова сила
P3
− yi0 sin ϑ) - центростремительная сила.
где QK
X = 2ϑ̇
QCX = ϑ̇2
0
i=1 mi (ẋi sin ϑ
0
i=1 mi (xi cos ϑ
15
M Y¨0 − ϑ̈
3
X
mi (x0i cos ϑ
−
yi0 sin ϑ)
+
i=1
3
X
C
mi (ẍ0i sin ϑ + ÿi0 cos ϑ) = QK
Y + QY ,
i=1
(19)
P3
− ẋ0i cos ϑ) - кориолисова сила
P3
+ yi0 cos ϑ) - центростремительная сила.
где QK
Y = 2ϑ̇
QCY = ϑ̇2
−Ẍ0
3
X
0
i=1 mi (ẏi sin ϑ
0
i=1 mi (xi sin ϑ
mi (x0i sin ϑ
+
yi0 cos ϑ)
+ Ÿ0
i=1
"
+ϑ̈ J0 +
3
X
#
2
2
P3
mi (x0i cos ϑ − yi0 sin ϑ)+
i=1
3
X
mi (x0i + yi0 ) +
i=1
где QK
ϑ = −2ϑ̇
3
X
0 0
i=1 mi (xi ẋi
(20)
mi (x0i ÿi0 − yi0 ẍ0i ) = QK
ϑ
i=1
− yi0 ẏi0 ) - момент кориолисовых сил инерции.
Для того чтобы полностью описать динамику космического манипуляционного робота, нужно дополнительно составить уравнения носимых
тел.
Подставив в уравнение Лагранжа 2го рода (1) только обобщенные
координаты q4 , q5 и q6 получим:
3
X
d ∂T
d ∂ ẋ0i 0
=
mi [
(ẋi − yi0 ϑ̇ + Ẋ0 cos ϑ + Ẏ0 sin ϑ)+
dt ∂ q̇j
dt ∂ q̇j
i=1
d ∂ ẏi0 0
+
(ẋi − yi0 ϑ̇ + Ẏ0 cos ϑ − Ẋ0 sin ϑ)+
dt ∂ q̇j
∂ ẋ0
+ i [ẍ0i − yi0 ϑ̈ − ϑ̇(ẏi0 + Ẋ0 sin ϑ − Ẏ0 cos ϑ) + Ẍ0 cos ϑ + Ÿ0 sin ϑ]+
∂ q̇j
∂ ẏi0 0
+
[ÿi + x0i ϑ̈ + ϑ̇(ẋ0i − Ẏ0 sin ϑ − Ẋ0 cos ϑ) + Ÿ0 cos ϑ − Ẍ0 sin ϑ]]
∂ q̇j
16
(21)
3
X
∂T
∂ ẋ0i
∂yi0 0
=
mi [(
− ϑ̇
)(ẋi − yi0 ϑ̇ + Ẋ0 cos ϑ + Ẏ0 sin ϑ)+
∂ q̇j
∂ q̇j
∂qj
i=1
+(
∂ ẏi0
∂ q̇j
− ϑ̇
∂x0i
∂qj
(22)
)(ẏi0 + x0i ϑ̇ + Ẏ0 cos ϑ − Ẋ0 sin ϑ)],
для j = 4, 6.
Так как тип геометрических связей (9) представленных в виде тригонометрических функций вектора обобщенных координат, в рассматриваемой системе обуславливает справедливость следующих соотношений:
d ∂ ẋ0i
∂ ẋ0i
=
,
∂ q̇j
dt ∂ q̇j
∂ ẋ0i
∂x0i
=
,
∂qj
∂ q̇j
∂ ẏi0
d ∂ ẏi0
=
,
∂ q̇j
dt ∂ q̇j
∂yi0
∂ ẏi0
=
,
∂qj
∂ q̇j
(23)
для j = 4, 6, и i = 1, 4.
Учитывая (21), (22), (23), уравнения Лагранжа 2го рода (1) для обобщенных координат qj , могут быть переписаны в форме с перенесенными
вправо членами, определяющими обобщенные кориолисовы и центробежные силы:
Ẍ0
3
X
i=1
+ϑ̈
3
X
∂x0i
∂x0i
∂yi0
∂yi0
mi (
cos ϑ −
sin ϑ) + Ÿ0
mi (
sin ϑ +
cos ϑ)+
∂qj
∂qj
∂q
∂q
j
j
i=1
3
X
i=1
3
0
0
0
0
X
0 ∂yi
0 ∂xi
0 ∂xi
0 ∂yi
C
mi (xi
− yi
)+
mi (ẍi
+ ÿi
) = QK
jr + Qjr
∂qj
∂qj
∂qj
∂qj
i=1
где QK
jr = −2ϑ̇
0
0
0 ∂yi
0 ∂xi
i=1 mi (ẋi ∂qj − ẏi ∂qj ) - обобщенные центробежные силы,
P3
QCjr = −ϑ̇2
инерции.
(24)
0
0
0 ∂xi
0 ∂yi
i=1 mi (xi ∂qj + yi ∂qj ) - обобщенные кориолисовы силы
P3
Таким образом, уравнения (18), (19), (20) и (24) образуют неуправляемую систему уравнений движения космического манипуляционного робота.
17
2.4. Управляемое движение космического
манипуляционного робота
Для управляемого движения космического манипуляционного робота
в уравнениях (1) необходимо определить шестимерный вектор обобщенных
сил Q, соответствующий управляющим силам и моментами [4-6]:
d ∂T
∂T
= Q.
−
dt ∂ q̇
∂q
(25)
Из курса аналитической динамики известно, что возможная работа
δAi , соответствующая возможному изменению обобщенной координаты δqi ,
может быть выражена произведением [6]:
(26)
δAi = Qi δqi .
Тогда для компонент вектора Q можем записать:
Qi =
δAi
,
δqi
i = 1, 6.
(27)
Рассматривая изменения соответствующих обобщенных координат при
предположении, что изменения остальных равны нулю, для вектора Q получаем:
Fx cos ϑ − Fy sin ϑ
Fx sin ϑ + Fy cos ϑ
Mϑ
Q=
Mα1
Mα2
Mα3
.
(28)
Вектор управления U представляется в виде шестимерного вектора
18
управляющих воздействий рассматриваемой системы:
U =
Fx
Fy
Mϑ
Mα1
Mα2
Mα3
.
(29)
Тогда вектор Q можно представить в виде произведения:
(30)
Q = MU,
где M - матрица преобразования размерности 6x6 и имеет вид:
cos ϑ − sin ϑ
sin ϑ cos ϑ
0
0
M =
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
.
(31)
Таким образом, система уравнений управляемого движения космического манипуляционного робота представима в форме:
Fx cos ϑ − Fy sin ϑ
Fx sin ϑ + Fy cos ϑ
Mϑ
d ∂T
∂T
−
=
dt ∂ q̇
∂q
Mα1
Mα2
Mα3
19
.
(32)
Примеры
Пример 1.
Пусть для космического манипуляционного робота будут заданы следующие параметры:
xa = 1, ya = 1, r1 = 2, r2 = 2, r3 = 0.5.
Обобщенные координаты изменяются согласно формуле [9]:
qi (t) = qi0 +
(t − t0 ) 1
(qi − qi0 ), i = 1, 6.
(t1 − t0 )
Начальные значения обобщенных координат:
q10 = 0, q20 = 0, q30 = 0, q40 = 0, q50 =
π
3π 0
, q6 = − .
4
4
Конечные значения обобщенных координат:
π
π
π
q11 = 4, q21 = 4, q31 = − , q41 = 0, q51 = , q61 = − .
2
3
2
Характеристики масс: m0 = 10, m1 = 2, m2 = 2, m3 = 0.5.
При данных параметрах космического манипуляционного робота получим следующие результаты:
20
Рис. 2: Начальное и конечное положение космического манипуляционного робота.
Рис. 3: График кинетической энергии космического манипуляционного робота.
21
Рис. 4: Графики компонент силы тяги Fx , Fy и управляющего момента ϑ.
Рис. 5: Графики управляющих моментов M α1, M α2 и M α3.
22
Из примера видно, что при изменении обобщенных координат космического манипуляционного робота согласно формуле:
qi (t) = qi0 +
(t − t0 ) 1
(qi − qi0 ), i = 1, 6,
(t1 − t0 )
к системе требуется прикладывать очень большие усилия для того, чтобы
начать и завершить движение по заданной траектории (скорости и ускорения в граничных точках траектории претерпевают разрывы 1го и 2го рода
соответственно).
Для обеспечения плавного движения космического манипуляционного робота вдоль всей траектории (режим Разгон-Торможение [9]), воспользуемся полиномиальной интерполяцией.
23
Пример 2.
Пусть, для i-той координаты манипулятора заданы значения в граничных точках:
qi (t0 ) = qi 0 , qi (t1 ) = qi 1 .
Также, потребуем, чтобы на концах отрезка [t0 ,
условия:
q̇i (t0 ) = q̇i (t1 ) = 0, q̈i (t0 ) = q̈i (t1 ) = 0.
t1 ] выполнялись
Таким образом, полином пятой степени:
qi (t) = a0i + a1i t + a2i t2 + a3i t3 + a4i t4 + a5i t5 ,
представляет собой многочлен с минимальной степенью, удовлетворяющий
наложенным на систему условиям.
Не умаляя общности, определим t0 = 0, t1 = T , тогда коэффициенты полинома можно найти при помощи следующей замкнутой системы
алгебраических уравнений:
a0i = qi 0 ,
a1i = 0,
a2i = 0,
a1i + 2a2i T + 3a3i T 2 + 4a4i T 3 + 5a5i T 4 = 0,
2a2i + 6a3i T + 12a4i T 2 + 20a5i T 3 = 0,
a0i + a1i T + a2i T 2 + a3i T 3 + a4i T 4 + a5i T 5 = qi 1 .
Разрешив ее - получим формулу изменения i-й обобщенной координаты космического манипуляционного робота:
(qi 1 − qi 0 ) 4
(qi 1 − qi 0 ) 5
(qi 1 − qi 0 ) 3
qi (t) = qi + 10
t − 15
t +6
t.
T3
T4
T5
0
Используя параметры космического манипуляционного робота и начальные данные из примера 1, получим следующие результаты:
24
Рис. 6: Начальное и конечное положение космического манипуляционного робота.
Рис. 7: График кинетической энергии космического манипуляционного робота.
25
Рис. 8: Графики компонент силы тяги Fx , Fy и управляющего момента ϑ.
Рис. 9: Графики управляющих моментов M α1, M α2 и M α3.
26
Заключение
По итогам проделанной работы:
1. Построены уравнения управляемого движения космического манипуляционного робота, представленного в виде основания и прикрепленного к нему трехзвенного манипулятора.
2. Решена обратная задача динамики для робототехнической системы, а
также первая и вторая прямые задачи кинематики роботов.
3. Представлен листинг программы моделирования управляемого движения космического манипуляционного робота, движение которого построено в том числе и с учетом условия обеспечения режима РазгонТорможение. Результатом выполнения программы является построение траекторий полюсов и характеристической точки космического манипуляционного робота, а также графиков кинетической энергии, управляющих сил и моментов.
Дальнейшие направления исследования:
1. Полученная модель может быть использована при решении прямой задачи динамики.
2. Построение пространственной модели космического манипуляционного
робота с упругими звеньями манипулятора.
27
Список литературы
1. Манипулятор
ERA.
http://www.esa.int/Our
Activities/Human
Spaceflight/International Space Station/European Robotic Arm
2. Манипулятор
Канадарм-2.
http://www.nasa.gov/mission
pages/station/structure/elements/mss.html
3. Манипулятор
JEM
RMS.
http://www.nasa.gov/mission
pages/station/structure/elements/jem.html
4. Рутковский В. Ю., Суханов В. М., Динамическая модель свободнолетающего космического робототехнического модуля // Автоматика и телемеханика. М.: Институт проблем управления, 2000. C. 39 – 57 .
5. Попов Е. В., Верещагин А. Ф., Зенкевич С. Л., Манипуляционные роботы. Динамика и алгоритмы. М.: Наука, 1978. 400 c.
6. Бухгольц Н. Н., Основной курс теоритической механики. Часть 2. Динамика системы материальных точек. М.: Лань, 2016. 336 c.
7. Пятницкий Е. С., Принцип декомпозиции в управлении механическими
и электромеханическими системами // Синтез систем управления манипуляционного роботами на принципе декомпозиции. М.: Институт
проблем управления, 1987. С. 4 – 15.
8. Кулаков Ф. М., Супервизорное управление манипуляционными роботами. М.: Наука, 1980. 448 c.
9. Зенкевич С. Л., Ющенко А. С., Основы управления манипуляционными
роботами: Учебник для вузов. 2-е изд., исправ. и доп. М.: Изд-во МГТУ
им. Н. Э.Баумана, 2004. 480 с.
28
Приложение
Приложение 1.
Remove["Global‘*"];
xa = 1; ya = 1;
dl1 = 2; dl2 = 2; dl3 = 0.5;
ra = Sqrt[xa^2 + ya^2];
R = 2*ra;
mo = 10; m1 = 2; m2 = 2; m3 = 0.5;
Io = mo*R^2/2;
M = mo + m1 + m2 + m3;
q1[t] = q1n + (q1k - q1n)*lambdat;
q2[t] = q2n + (q2k - q2n)*lambdat;
q3[t] = q3n + (q3k - q3n)*lambdat;
q4[t] = q4n + (q4k - q4n)*lambdat;
q5[t] = q5n + (q5k - q5n)*lambdat;
q6[t] = q6n + (q6k - q6n)*lambdat;
lambdat = (t - tn)/deltat;
deltat = tk - tn;
q1n = 0; q1k = 4;
q2n = 0; q2k = 4;
q3n = 0; q3k = -Pi/2;
q4n = 0; q4k = 0;
q5n = 3 Pi/4; q5k = Pi/3;
q6n = -Pi/4; q6k = -Pi/2;
tn = 0; tk = 10;
C3 = Cos[q3[t]];
S3 = Sin[q3[t]];
C4 = Cos[q4[t]];
S4 = Sin[q4[t]];
C5 = Cos[q5[t]];
S5 = Sin[q5[t]];
C6 = Cos[q6[t]];
29
S6 = Sin[q6[t]];
C45 = Cos[q4[t] + q5[t]];
S45 = Sin[q4[t] + q5[t]];
C56 = Cos[q5[t] + q6[t]];
S56 = Sin[q5[t] + q6[t]];
C456 = Cos[q4[t] + q5[t] + q6[t]];
S456 = Sin[q4[t] + q5[t] + q6[t]];
x10 = xa - dl1*S4;
y10 = ya - dl1*C4;
x20 = x10 + dl2*S45;
y20 = y10 + dl2*C45;
x30 = x20 + dl3*S456;
y30 = y20 + dl3*C456;
pdx104 = -dl1*C4;
pdy104 = dl1*S4;
pdx105 = 0;
pdy105 = 0;
pdx106 = 0;
pdy106 = 0;
pdx204 = -dl1*C4 + dl2*C45;
pdy204 = dl1*S4 - dl2*S45;
pdx205 = dl2*C45;
pdy205 = -dl2*S45;
pdx206 = 0;
pdy206 = 0;
pdx304 = -dl1*C4 + dl2*C45 + dl3*C456;
pdy304 = dl1*S4 - dl2*S45 - dl3*S456;
pdx305 = dl2*C45 + dl3*C456;
pdy305 = -dl2*S45 - dl3*S456;
pdx306 = dl3*C456;
pdy306 = -dl3*S456;
vx10 = -dl1*D[q4[t], t]*C4;
vy10 = dl1*D[q4[t], t]*S4;
vx20 = vx10 + dl2*D[q4[t] + q5[t], t]*C45;
30
vy20 = vy10 - dl2*D[q4[t] + q5[t], t]*S45;
vx30 = vx20 + dl3*D[q4[t] + q5[t] + q6[t], t]*C456;
vy30 = vy20 - dl3*D[q4[t] + q5[t] + q6[t], t]*S456;
pdvx104 = dl1*D[q4[t], t]*S4;
pdvy104 = dl1*D[q4[t], t]*C4;
pdvx105 = 0;
pdvy105 = 0;
pdvx106 = 0;
pdvy106 = 0;
pdvx204 = dl1*D[q4[t], t]*S4 - dl2*D[q4[t] + q5[t], t]*S45;
pdvy204 = dl1*D[q4[t], t]*C4 - dl2*D[q4[t] + q5[t], t]*C45;
pdvx205 = -dl2*D[q4[t] + q5[t], t]*S45;
pdvy205 = -dl2*D[q4[t] + q5[t], t]*C45;
pdvx206 = 0;
pdvy206 = 0;
pdvx304 =
dl1*D[q4[t], t]*S4 - dl2*D[q4[t] + q5[t], t]*S45 dl3*D[q4[t] + q5[t] + q6[t], t]*S456;
pdvy304 =
dl1*D[q4[t], t]*C4 - dl2*D[q4[t] + q5[t], t]*C45 dl3*D[q4[t] + q5[t] + q6[t], t]*C456;
pdvx305 = -dl2*D[q4[t] + q5[t], t]*S45 dl3*D[q4[t] + q5[t] + q6[t], t]*S456;
pdvy305 = -dl2*D[q4[t] + q5[t], t]*C45 dl3*D[q4[t] + q5[t] + q6[t], t]*C456;
pdvx306 = -dl3*D[q4[t] + q5[t] + q6[t], t]*S456;
pdvy306 = -dl3*D[q4[t] + q5[t] + q6[t], t]*C456;
wx10 = -dl1*(D[D[q4[t], t], t]*C4 - D[q4[t], t]^2*S4);
wy10 = dl1*(D[D[q4[t], t], t]*S4 + D[q4[t], t]^2*C4);
wx20 = wx10 +
dl2*(D[D[q4[t] + q5[t], t], t]*C45 - D[q4[t] + q5[t], t]^2*S45);
wy20 = wy10 dl2*(D[D[q4[t] + q5[t], t], t]*S45 + D[q4[t] + q5[t], t]^2*C45);
wx30 = wx20 +
31
dl3*(D[D[q4[t] + q5[t] + q6[t], t], t]*C456 D[q4[t] + q5[t] + q6[t], t]^2*S456);
wy30 = wy20 dl3*(D[D[q4[t] + q5[t] + q6[t], t], t]*S456 +
D[q4[t] + q5[t] + q6[t], t]^2*C456);
Xo = q1[t];
Yo = q2[t];
Xa = q1[t] + xa*C3 - ya*S3;
Ya = q2[t] + xa*S3 + ya*C3;
X1 = q1[t] + (xa - dl1*S4)*C3 - (ya - dl1*C4)*S3;
Y1 = q2[t] + (xa - dl1*S4)*S3 + (ya - dl1*C4)*C3;
X2 = q1[t] + (xa - dl1*S4 + dl2*S45)*C3 - (ya - dl1*C4 + dl2*C45)*S3;
Y2 = q2[t] + (xa - dl1*S4 + dl2*S45)*S3 + (ya - dl1*C4 + dl2*C45)*C3;
X3 = q1[t] + (xa - dl1*S4 + dl2*S45 + dl3*S456)*
C3 - (ya - dl1*C4 + dl2*C45 + dl3*C456)*S3;
Y3 = q2[t] + (xa - dl1*S4 + dl2*S45 + dl3*S456)*
S3 + (ya - dl1*C4 + dl2*C45 + dl3*C456)*C3;
{Plot1 = ParametricPlot[{Xo, Yo}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Dashed],
AspectRatio -> 1, AxesLabel -> {"Xo", "Yo"}],
Plot2 = ParametricPlot[{Xa, Ya}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Dashed],
AspectRatio -> 1, AxesLabel -> {"Xa", "Ya"}],
Plot3 = ParametricPlot[{X1, Y1}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Dashed],
AspectRatio -> 1, AxesLabel -> {"X1", "Y1"}],
Plot4 = ParametricPlot[{X2, Y2}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Dashed],
AspectRatio -> 1, AxesLabel -> {"X2", "Y2"}],
Plot5 = ParametricPlot[{X3, Y3}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Thick],
AspectRatio -> 1, AxesLabel -> {"X3", "Y3"}]}
{Plot6 = Graphics[{EdgeForm[Thick], Yellow,
Disk[{Xo /. t -> 0, Yo /. t -> 0}, R], Green, Thick,
32
Line[{{Xa /. t -> 0, Ya /. t -> 0}, {X1 /. t -> 0,
Y1 /. t -> 0}, {X2 /. t -> 0, Y2 /. t -> 0}, {X3 /. t -> 0,
Y3 /. t -> 0}}],
Red, Point[{{Xa /. t -> 0, Ya /. t -> 0}, {X1 /. t -> 0,
Y1 /. t -> 0}, {X2 /. t -> 0, Y2 /. t -> 0}}],
Disk[{X3 /. t -> 0, Y3 /. t -> 0}, 0.05]},
PlotRange -> {{-ra - dl1 - dl2 - dl3,
q1k + ra + dl1 + dl2 + dl3}, {-ra - dl1 - dl2 - dl3,
q2k + ra + dl1 + dl2 + dl3}}, Axes -> True,
AxesLabel -> {"X", "Y"}],
Plot7 = Graphics[{EdgeForm[Thick], Yellow,
Disk[{Xo /. t -> deltat, Yo /. t -> deltat}, R], Green, Thick,
Line[{{Xa /. t -> deltat, Ya /. t -> deltat}, {X1 /. t -> deltat,
Y1 /. t -> deltat}, {X2 /. t -> deltat,
Y2 /. t -> deltat}, {X3 /. t -> deltat, Y3 /. t -> deltat}}],
Red, Point[{{Xa /. t -> deltat,
Ya /. t -> deltat}, {X1 /. t -> deltat,
Y1 /. t -> deltat}, {X2 /. t -> deltat, Y2 /. t -> deltat}}],
Disk[{X3 /. t -> deltat, Y3 /. t -> deltat}, 0.05]},
PlotRange -> {{-ra - dl1 - dl2 - dl3,
q1k + ra + dl1 + dl2 + dl3}, {-ra - dl1 - dl2 - dl3,
q2k + ra + dl1 + dl2 + dl3}}, Axes -> True,
AxesLabel -> {"X", "Y"}]}
Show[Plot7, Plot6, Plot5, Plot4, Plot3, Plot2, Plot1]
T = 1/2*M*(D[q1[t], t]^2 + D[q2[t], t]^2) + 1/2*Io*D[q3[t], t]^2 +
1/2 (m1*((vx10 - y10*D[q3[t], t])^2 + (vy10 + x10*D[q3[t], t])^2) +
m2*((vx20 - y20*D[q3[t], t])^2 + (vy20 + x20*D[q3[t], t])^2) +
m3*((vx30 - y30*D[q3[t], t])^2 + (vy30 +
x30*D[q3[t], t])^2)) + (D[q1[t], t]*C3 +
D[q2[t], t]*S3)*(m1*(vx10 - y10*D[q3[t], t]) +
m2*(vx20 - y20*D[q3[t], t]) +
m3*(vx30 - y30*D[q3[t], t])) + (D[q2[t], t]*C3 D[q1[t], t]*S3)*(m1*(vy10 + x10*D[q3[t], t]) +
m2*(vy20 + x20*D[q3[t], t]) + m3*(vy30 + x30*D[q3[t], t]));
33
Plot[T, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Blue, Thick], AxesLabel -> {"t", "T"}]
QXCor = 2*
D[q3[t],
t]*(m1*(vx10*S3 + vy10*C3) + m2*(vx20*S3 + vy20*C3) +
m3*(vx30*S3 + vy30*C3));
QXn = D[q3[t], t]^2*(m1*(x10*C3 - y10*S3) + m2*(x20*C3 - y20*S3) +
m3*(x30*C3 - y30*S3));
FX = M*D[D[q1[t], t], t] D[D[q3[t], t],
t]*(m1*(x10*S3 + y10*C3) + m2*(x20*S3 + y20*C3) +
m3*(x30*S3 + y30*C3)) + m1*(wx10*C3 - wy10*S3) +
m2*(wx20*C3 - wy20*S3) + m3*(wx30*C3 - wy30*S3) - QXCor - QXn;
QYCor = 2*
D[q3[t],
t]*(m1*(vy10*S3 - vx10*C3) + m2*(vy20*S3 - vx20*C3) +
m3*(vy30*S3 - vx30*C3));
QYn = D[q3[t], t]^2*(m1*(x10*S3 + y10*C3) + m2*(x20*S3 + y20*C3) +
m3*(x30*S3 + y30*C3));
FY = M*D[D[q2[t], t], t] D[D[q3[t], t],
t]*(m1*(x10*C3 - y10*S3) + m2*(x20*C3 - y20*S3) +
m3*(x30*C3 - y30*S3)) + m1*(wx10*S3 + wy10*C3) +
m2*(wx20*S3 + wy20*C3) + m3*(wx30*S3 + wy30*C3) - QYCor - QYn;
sol = Solve[{Fx*C3 - Fy*S3 == FX, Fy*C3 + Fx*S3 == Fy}, {Fx, Fy}];
Fx = Fx /. sol[[1, 1]];
Fy = Fy /. sol[[1, 2]];
QVCor = -2*
D[q3[t],
t]*(m1*(x10*vx10 - y10*vy10) + m2*(x20*vx20 - y20*vy20) +
m3*(x30*vx30 - y30*vy30));
MV = -D[D[q1[t], t], t]*(m1*(x10*S3 + y10*C3) + m2*(x20*S3 + y20*C3) +
m3*(x30*S3 + y30*C3)) +
D[D[q2[t], t],
34
t]*(m1*(x10*C3 - y10*S3) + m2*(x20*C3 - y20*S3) +
m3*(x30*C3 - y30*S3)) +
D[D[q3[t], t],
t]*(Io + m1*(x10^2 + y10^2) + m2*(x20^2 + y20^2) +
m3*(x30^2 + y30^2)) + m1*(x10*wy10 - y10*wx10) +
m2 (x20*wy20 - y20*wx20) + m3 (x30*wy30 - y30*wx30) - QVCor;
{Plot[QXCor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QXCor"}],
Plot[QXn, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QXn"}],
Plot[QYCor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QyCor"}],
Plot[QYn, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QYn"}],
Plot[Sqrt[QXCor^2 + QYCor^2], {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QCor"}],
Plot[Sqrt[QXn^2 + QYn^2], {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Qn"}]}
{Plot[Fx, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Red, Thick], AxesLabel -> {"t", "Fx"}],
Plot[Fy, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Red, Thick], AxesLabel -> {"t", "Fy"}],
Plot[Sqrt[Fx^2 + Fy^2], {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Red, Thick], AxesLabel -> {"t", "F"}],
Plot[MV, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Red, Thick], AxesLabel -> {"t", "MV"}]}
Q4Cor = -2*
D[q3[t],
t]*(m1*(vx10*pdy104 - vy10*pdx104) +
m2*(vx20*pdy204 - vy20*pdx204) + m3*(vx30*pdy304 - vy30*pdx304));
Q4n = D[q3[t], t]^2*(m1*(x10*pdx104 + y10*pdy104) +
m2*(x20*pdx204 + y20*pdy204) + m3*(x30*pdx304 + y30*pdy304));
Q4 =
-m1*(pdvx104*(vx10 - y10*D[q3[t], t] + D[q1[t], t]*C3 +
D[q2[t], t]*S3) +
35
pdvy104*(vx10 - y10*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx104*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy104*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m2*(pdvx204*(vx20 - y20*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy204*(vx20 - y20*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx204*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy204*(D[q3[t], t]*(-D[q2[t], t]*S3 - D[q1[t], t]*C3))) +
-m3*(pdvx304*(vx30 - y30*D[q3[t], t] + D[q1[t], t]*C3 +
D[q2[t], t]*S3) +
pdvy304*(vx30 - y30*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx304*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy304*(D[q3[t], t]*(-D[q2[t], t]*S3 - D[q1[t], t]*C3))) +
m1*((pdvx104 - D[q3[t], t]*pdy104)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy104 D[q3[t], t]*pdx104)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m2*((pdvx204 - D[q3[t], t]*pdy204)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy204 D[q3[t], t]*pdx204)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m3*((pdvx304 - D[q3[t], t]*pdy304)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy304 D[q3[t], t]*pdx304)*(D[q2[t], t]*C3 - D[q1[t], t]*S3));
Ma1 = D[D[q1[t], t],
t]*(m1*(pdx104*C3 - pdy104*S3) + m2*(pdx204*C3 - pdy204*S3) +
m3*(pdx304*C3 - pdy304*S3)) +
D[D[q2[t], t],
t]*(m1*(pdx104*S3 + pdy104*C3) + m2*(pdx204*S3 + pdy204*C3) +
m3*(pdx304*S3 + pdy304*C3)) +
D[D[q3[t], t],
t]*(m1*(x10*pdy104 - y10*pdx104) + m2*(x20*pdy204 - y20*pdx204) +
36
m3*(x30*pdy304 - y30*pdx304)) +
m1*(wx10*pdx104 + wy10*pdy104) + m2*(wx20*pdx204 + wy20*pdy204) +
m3*(wx30*pdx304 + wy30*pdy304) - Q4Cor - Q4n - Q4;
{Plot[Q4Cor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q4Cor"}],
Plot[Q4n, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q4n"}],
Plot[Q4, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q4"}]}
Plot[Ma1, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Blue, Thick], AxesLabel -> {"t", "Ma1"}]
Q5Cor = -2*
D[q3[t],
t]*(m1*(vx10*pdy105 - vy10*pdx105) +
m2*(vx20*pdy205 - vy20*pdx205) + m3*(vx30*pdy305 - vy30*pdx305));
Q5n = -D[q3[t], t]^2*(m1*(x10*pdx105 + y10*pdy105) +
m2*(x20*pdx205 + y20*pdy205) + m3*(x30*pdx305 + y30*pdy305));
Q5 = -m1*(pdvx105*(vx10 - y10*D[q3[t], t] + D[q1[t], t]*C3 +
D[q2[t], t]*S3) +
pdvy105*(vx10 - y10*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx105*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy105*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m2*(pdvx205*(vx20 - y20*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy205*(vx20 - y20*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx205*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy205*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m3*(pdvx305*(vx30 - y30*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy305*(vx30 - y30*D[q3[t], t] + D[q2[t], t]*C3 37
D[q1[t], t]*S3) +
pdx305*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy305*(D[q3[t], t]*(-D[q2[t], t]*S3 - D[q1[t], t]*C3))) +
m1*((pdvx105 - D[q3[t], t]*pdy105)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy105 D[q3[t], t]*pdx105)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m2*((pdvx205 - D[q3[t], t]*pdy205)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy205 D[q3[t], t]*pdx205)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m3*((pdvx305 - D[q3[t], t]*pdy305)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy305 D[q3[t], t]*pdx305)*(D[q2[t], t]*C3 - D[q1[t], t]*S3));
Ma2 = D[D[q1[t], t],
t]*(m1*(pdx105*C3 - pdy105*S3) + m2*(pdx205*C3 - pdy205*S3) +
m3*(pdx305*C3 - pdy305*S3)) +
D[D[q2[t], t],
t]*(m1*(pdx105*S3 + pdy105*C3) + m2*(pdx205*S3 + pdy205*C3) +
m3*(pdx305*S3 + pdy305*C3)) +
D[D[q3[t], t],
t]*(m1*(x10*pdy105 - y10*pdx105) + m2*(x20*pdy205 - y20*pdx205) +
m3*(x30*pdy305 - y30*pdx305)) +
m1*(wx10*pdx105 + wy10*pdy105) + m2*(wx20*pdx205 + wy20*pdy205) +
m3*(wx30*pdx305 + wy30*pdy305) - Q5Cor - Q5n - Q5;
{Plot[Q5Cor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q5Cor"}],
Plot[Q5n, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q5n"}],
Plot[Q5, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q5"}]}
Plot[Ma2, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Blue, Thick], AxesLabel -> {"t", "Ma2"}]
Q6Cor = -2*
D[q3[t],
t]*(m1*(vx10*pdy106 - vy10*pdx106) +
38
m2*(vx20*pdy206 - vy20*pdx206) + m3*(vx30*pdy306 - vy30*pdx306));
Q6n = -D[q3[t], t]^2*(m1*(x10*pdx106 + y10*pdy106) +
m2*(x20*pdx206 + y20*pdy206) + m3*(x30*pdx306 + y30*pdy306));
Q6 = -m1*(pdvx106*(vx10 - y10*D[q3[t], t] + D[q1[t], t]*C3 +
D[q2[t], t]*S3) +
pdvy106*(vx10 - y10*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx106*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy105*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m2*(pdvx206*(vx20 - y20*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy206*(vx20 - y20*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx206*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy205*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m3*(pdvx306*(vx30 - y30*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy306*(vx30 - y30*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx306*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy305*(D[q3[t], t]*(-D[q2[t], t]*S3 - D[q1[t], t]*C3))) +
m1*((pdvx106 - D[q3[t], t]*pdy106)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy106 D[q3[t], t]*pdx106)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m2*((pdvx206 - D[q3[t], t]*pdy206)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy206 D[q3[t], t]*pdx206)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m3*((pdvx306 - D[q3[t], t]*pdy306)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy306 D[q3[t], t]*pdx306)*(D[q2[t], t]*C3 - D[q1[t], t]*S3));
Ma3 = D[D[q1[t], t],
t]*(m1*(pdx106*C3 - pdy106*S3) + m2*(pdx206*C3 - pdy206*S3) +
39
m3*(pdx306*C3 - pdy306*S3)) +
D[D[q2[t], t],
t]*(m1*(pdx106*S3 + pdy106*C3) + m2*(pdx206*S3 + pdy206*C3) +
m3*(pdx306*S3 + pdy306*C3)) +
D[D[q3[t], t],
t]*(m1*(x10*pdy106 - y10*pdx106) + m2*(x20*pdy206 - y20*pdx206) +
m3*(x30*pdy306 - y30*pdx306)) +
m1*(wx10*pdx106 + wy10*pdy106) + m2*(wx20*pdx206 + wy20*pdy206) +
m3*(wx30*pdx306 + wy30*pdy306) - Q6Cor - Q6n - Q6;
{Plot[Q6Cor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q6Cor"}],
Plot[Q6n, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q6n"}],
Plot[Q6, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q6"}]}
Plot[Ma3, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Blue, Thick], AxesLabel -> {"t", "Ma3"}]
40
Приложение 2.
Remove["Global‘*"];
xa = 1; ya = 1;
dl1 = 2; dl2 = 2; dl3 = 0.5;
ra = Sqrt[xa^2 + ya^2];
R = 2*ra;
mo = 10; m1 = 2; m2 = 2; m3 = 0.5;
Io = mo*R^2/2;
M = mo + m1 + m2 + m3;
q1[t] = q1n + 10 *((q1k - q1n)/tk^3)*t^3
15 *((q1k - q1n)/tk^4)*t^4 + 6 *((q1k
q2[t] = q2n + 10 *((q2k - q2n)/tk^3)*t^3
15 *((q2k - q2n)/tk^4)*t^4 + 6 *((q2k
q3[t] = q3n + 10 *((q3k - q3n)/tk^3)*t^3
15 *((q3k - q3n)/tk^4)*t^4 + 6 *((q3k
q4[t] = q4n + 10 *((q4k - q4n)/tk^3)*t^3
15 *((q4k - q4n)/tk^4)*t^4 + 6 *((q4k
q5[t] = q5n + 10 *((q5k - q5n)/tk^3)*t^3
15 *((q5k - q5n)/tk^4)*t^4 + 6 *((q5k
q6[t] = q6n + 10 *((q6k - q6n)/tk^3)*t^3
15 *((q6k - q6n)/tk^4)*t^4 + 6 *((q6k
deltat = tk - tn;
q1n = 0; q1k = 4;
q2n = 0; q2k = 4;
q3n = 0; q3k = -Pi/2;
q4n = 0; q4k = 0;
q5n = 3 Pi/4; q5k = Pi/3;
q6n = -Pi/4; q6k = -Pi/2;
tn = 0; tk = 10;
C3 = Cos[q3[t]];
S3 = Sin[q3[t]];
C4 = Cos[q4[t]];
S4 = Sin[q4[t]];
41
-
q1n)/tk^5)*t^5;
q2n)/tk^5)*t^5;
q3n)/tk^5)*t^5;
q4n)/tk^5)*t^5;
q5n)/tk^5)*t^5;
q6n)/tk^5)*t^5;
C5 = Cos[q5[t]];
S5 = Sin[q5[t]];
C6 = Cos[q6[t]];
S6 = Sin[q6[t]];
C45 = Cos[q4[t] + q5[t]];
S45 = Sin[q4[t] + q5[t]];
C56 = Cos[q5[t] + q6[t]];
S56 = Sin[q5[t] + q6[t]];
C456 = Cos[q4[t] + q5[t] + q6[t]];
S456 = Sin[q4[t] + q5[t] + q6[t]];
x10 = xa - dl1*S4;
y10 = ya - dl1*C4;
x20 = x10 + dl2*S45;
y20 = y10 + dl2*C45;
x30 = x20 + dl3*S456;
y30 = y20 + dl3*C456;
pdx104 = -dl1*C4;
pdy104 = dl1*S4;
pdx105 = 0;
pdy105 = 0;
pdx106 = 0;
pdy106 = 0;
pdx204 = -dl1*C4 + dl2*C45;
pdy204 = dl1*S4 - dl2*S45;
pdx205 = dl2*C45;
pdy205 = -dl2*S45;
pdx206 = 0;
pdy206 = 0;
pdx304 = -dl1*C4 + dl2*C45 + dl3*C456;
pdy304 = dl1*S4 - dl2*S45 - dl3*S456;
pdx305 = dl2*C45 + dl3*C456;
pdy305 = -dl2*S45 - dl3*S456;
pdx306 = dl3*C456;
pdy306 = -dl3*S456;
42
vx10 = -dl1*D[q4[t], t]*C4;
vy10 = dl1*D[q4[t], t]*S4;
vx20 = vx10 + dl2*D[q4[t] + q5[t], t]*C45;
vy20 = vy10 - dl2*D[q4[t] + q5[t], t]*S45;
vx30 = vx20 + dl3*D[q4[t] + q5[t] + q6[t], t]*C456;
vy30 = vy20 - dl3*D[q4[t] + q5[t] + q6[t], t]*S456;
pdvx104 = dl1*D[q4[t], t]*S4;
pdvy104 = dl1*D[q4[t], t]*C4;
pdvx105 = 0;
pdvy105 = 0;
pdvx106 = 0;
pdvy106 = 0;
pdvx204 = dl1*D[q4[t], t]*S4 - dl2*D[q4[t] + q5[t], t]*S45;
pdvy204 = dl1*D[q4[t], t]*C4 - dl2*D[q4[t] + q5[t], t]*C45;
pdvx205 = -dl2*D[q4[t] + q5[t], t]*S45;
pdvy205 = -dl2*D[q4[t] + q5[t], t]*C45;
pdvx206 = 0;
pdvy206 = 0;
pdvx304 =
dl1*D[q4[t], t]*S4 - dl2*D[q4[t] + q5[t], t]*S45 dl3*D[q4[t] + q5[t] + q6[t], t]*S456;
pdvy304 =
dl1*D[q4[t], t]*C4 - dl2*D[q4[t] + q5[t], t]*C45 dl3*D[q4[t] + q5[t] + q6[t], t]*C456;
pdvx305 = -dl2*D[q4[t] + q5[t], t]*S45 dl3*D[q4[t] + q5[t] + q6[t], t]*S456;
pdvy305 = -dl2*D[q4[t] + q5[t], t]*C45 dl3*D[q4[t] + q5[t] + q6[t], t]*C456;
pdvx306 = -dl3*D[q4[t] + q5[t] + q6[t], t]*S456;
pdvy306 = -dl3*D[q4[t] + q5[t] + q6[t], t]*C456;
wx10 = -dl1*(D[D[q4[t], t], t]*C4 - D[q4[t], t]^2*S4);
wy10 = dl1*(D[D[q4[t], t], t]*S4 + D[q4[t], t]^2*C4);
wx20 = wx10 +
dl2*(D[D[q4[t] + q5[t], t], t]*C45 - D[q4[t] + q5[t], t]^2*S45);
43
wy20 = wy10 dl2*(D[D[q4[t] + q5[t], t], t]*S45 + D[q4[t] + q5[t], t]^2*C45);
wx30 = wx20 +
dl3*(D[D[q4[t] + q5[t] + q6[t], t], t]*C456 D[q4[t] + q5[t] + q6[t], t]^2*S456);
wy30 = wy20 dl3*(D[D[q4[t] + q5[t] + q6[t], t], t]*S456 +
D[q4[t] + q5[t] + q6[t], t]^2*C456);
Xo = q1[t];
Yo = q2[t];
Xa = q1[t] + xa*C3 - ya*S3;
Ya = q2[t] + xa*S3 + ya*C3;
X1 = q1[t] + (xa - dl1*S4)*C3 - (ya - dl1*C4)*S3;
Y1 = q2[t] + (xa - dl1*S4)*S3 + (ya - dl1*C4)*C3;
X2 = q1[t] + (xa - dl1*S4 + dl2*S45)*C3 - (ya - dl1*C4 + dl2*C45)*S3;
Y2 = q2[t] + (xa - dl1*S4 + dl2*S45)*S3 + (ya - dl1*C4 + dl2*C45)*C3;
X3 = q1[t] + (xa - dl1*S4 + dl2*S45 + dl3*S456)*
C3 - (ya - dl1*C4 + dl2*C45 + dl3*C456)*S3;
Y3 = q2[t] + (xa - dl1*S4 + dl2*S45 + dl3*S456)*
S3 + (ya - dl1*C4 + dl2*C45 + dl3*C456)*C3;
{Plot1 = ParametricPlot[{Xo, Yo}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Dashed],
AspectRatio -> 1, AxesLabel -> {"Xo", "Yo"}],
Plot2 = ParametricPlot[{Xa, Ya}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Dashed],
AspectRatio -> 1, AxesLabel -> {"Xa", "Ya"}],
Plot3 = ParametricPlot[{X1, Y1}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Dashed],
AspectRatio -> 1, AxesLabel -> {"X1", "Y1"}],
Plot4 = ParametricPlot[{X2, Y2}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Dashed],
AspectRatio -> 1, AxesLabel -> {"X2", "Y2"}],
Plot5 = ParametricPlot[{X3, Y3}, {t, 0, deltat},
AxesOrigin -> {0, 0}, PlotStyle -> Directive[Blue, Thick],
44
AspectRatio -> 1, AxesLabel -> {"X3", "Y3"}]}
{Plot6 = Graphics[{EdgeForm[Thick], Yellow,
Disk[{Xo /. t -> 0, Yo /. t -> 0}, R], Green, Thick,
Line[{{Xa /. t -> 0, Ya /. t -> 0}, {X1 /. t -> 0,
Y1 /. t -> 0}, {X2 /. t -> 0, Y2 /. t -> 0}, {X3 /. t -> 0,
Y3 /. t -> 0}}],
Red, Point[{{Xa /. t -> 0, Ya /. t -> 0}, {X1 /. t -> 0,
Y1 /. t -> 0}, {X2 /. t -> 0, Y2 /. t -> 0}}],
Disk[{X3 /. t -> 0, Y3 /. t -> 0}, 0.05]},
PlotRange -> {{-ra - dl1 - dl2 - dl3,
q1k + ra + dl1 + dl2 + dl3}, {-ra - dl1 - dl2 - dl3,
q2k + ra + dl1 + dl2 + dl3}}, Axes -> True,
AxesLabel -> {"X", "Y"}],
Plot7 = Graphics[{EdgeForm[Thick], Yellow,
Disk[{Xo /. t -> deltat, Yo /. t -> deltat}, R], Green, Thick,
Line[{{Xa /. t -> deltat, Ya /. t -> deltat}, {X1 /. t -> deltat,
Y1 /. t -> deltat}, {X2 /. t -> deltat,
Y2 /. t -> deltat}, {X3 /. t -> deltat, Y3 /. t -> deltat}}],
Red, Point[{{Xa /. t -> deltat,
Ya /. t -> deltat}, {X1 /. t -> deltat,
Y1 /. t -> deltat}, {X2 /. t -> deltat, Y2 /. t -> deltat}}],
Disk[{X3 /. t -> deltat, Y3 /. t -> deltat}, 0.05]},
PlotRange -> {{-ra - dl1 - dl2 - dl3,
q1k + ra + dl1 + dl2 + dl3}, {-ra - dl1 - dl2 - dl3,
q2k + ra + dl1 + dl2 + dl3}}, Axes -> True,
AxesLabel -> {"X", "Y"}]}
Show[Plot7, Plot6, Plot5, Plot4, Plot3, Plot2, Plot1]
T = 1/2*M*(D[q1[t], t]^2 + D[q2[t], t]^2) + 1/2*Io*D[q3[t], t]^2 +
1/2 (m1*((vx10 - y10*D[q3[t], t])^2 + (vy10 + x10*D[q3[t], t])^2) +
m2*((vx20 - y20*D[q3[t], t])^2 + (vy20 + x20*D[q3[t], t])^2) +
m3*((vx30 - y30*D[q3[t], t])^2 + (vy30 +
x30*D[q3[t], t])^2)) + (D[q1[t], t]*C3 +
D[q2[t], t]*S3)*(m1*(vx10 - y10*D[q3[t], t]) +
m2*(vx20 - y20*D[q3[t], t]) +
45
m3*(vx30 - y30*D[q3[t], t])) + (D[q2[t], t]*C3 D[q1[t], t]*S3)*(m1*(vy10 + x10*D[q3[t], t]) +
m2*(vy20 + x20*D[q3[t], t]) + m3*(vy30 + x30*D[q3[t], t]));
Plot[T, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Blue, Thick], AxesLabel -> {"t", "T"}]
QXCor = 2*
D[q3[t],
t]*(m1*(vx10*S3 + vy10*C3) + m2*(vx20*S3 + vy20*C3) +
m3*(vx30*S3 + vy30*C3));
QXn = D[q3[t], t]^2*(m1*(x10*C3 - y10*S3) + m2*(x20*C3 - y20*S3) +
m3*(x30*C3 - y30*S3));
FX = M*D[D[q1[t], t], t] D[D[q3[t], t],
t]*(m1*(x10*S3 + y10*C3) + m2*(x20*S3 + y20*C3) +
m3*(x30*S3 + y30*C3)) + m1*(wx10*C3 - wy10*S3) +
m2*(wx20*C3 - wy20*S3) + m3*(wx30*C3 - wy30*S3) - QXCor - QXn;
QYCor = 2*
D[q3[t],
t]*(m1*(vy10*S3 - vx10*C3) + m2*(vy20*S3 - vx20*C3) +
m3*(vy30*S3 - vx30*C3));
QYn = D[q3[t], t]^2*(m1*(x10*S3 + y10*C3) + m2*(x20*S3 + y20*C3) +
m3*(x30*S3 + y30*C3));
FY = M*D[D[q2[t], t], t] D[D[q3[t], t],
t]*(m1*(x10*C3 - y10*S3) + m2*(x20*C3 - y20*S3) +
m3*(x30*C3 - y30*S3)) + m1*(wx10*S3 + wy10*C3) +
m2*(wx20*S3 + wy20*C3) + m3*(wx30*S3 + wy30*C3) - QYCor - QYn;
sol = Solve[{Fx*C3 - Fy*S3 == FX, Fy*C3 + Fx*S3 == Fy}, {Fx, Fy}];
Fx = Fx /. sol[[1, 1]];
Fy = Fy /. sol[[1, 2]];
QVCor = -2*
D[q3[t],
t]*(m1*(x10*vx10 - y10*vy10) + m2*(x20*vx20 - y20*vy20) +
m3*(x30*vx30 - y30*vy30));
46
MV = -D[D[q1[t], t], t]*(m1*(x10*S3 + y10*C3) + m2*(x20*S3 + y20*C3) +
m3*(x30*S3 + y30*C3)) +
D[D[q2[t], t],
t]*(m1*(x10*C3 - y10*S3) + m2*(x20*C3 - y20*S3) +
m3*(x30*C3 - y30*S3)) +
D[D[q3[t], t],
t]*(Io + m1*(x10^2 + y10^2) + m2*(x20^2 + y20^2) +
m3*(x30^2 + y30^2)) + m1*(x10*wy10 - y10*wx10) +
m2 (x20*wy20 - y20*wx20) + m3 (x30*wy30 - y30*wx30) - QVCor;
{Plot[QXCor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QXCor"}],
Plot[QXn, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QXn"}],
Plot[QYCor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QyCor"}],
Plot[QYn, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QYn"}],
Plot[Sqrt[QXCor^2 + QYCor^2], {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "QCor"}],
Plot[Sqrt[QXn^2 + QYn^2], {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Qn"}]}
{Plot[Fx, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Red, Thick], AxesLabel -> {"t", "Fx"}],
Plot[Fy, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Red, Thick], AxesLabel -> {"t", "Fy"}],
Plot[Sqrt[Fx^2 + Fy^2], {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Red, Thick], AxesLabel -> {"t", "F"}],
Plot[MV, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Red, Thick], AxesLabel -> {"t", "MV"}]}
Q4Cor = -2*
D[q3[t],
t]*(m1*(vx10*pdy104 - vy10*pdx104) +
m2*(vx20*pdy204 - vy20*pdx204) + m3*(vx30*pdy304 - vy30*pdx304));
Q4n = D[q3[t], t]^2*(m1*(x10*pdx104 + y10*pdy104) +
47
m2*(x20*pdx204 + y20*pdy204) + m3*(x30*pdx304 + y30*pdy304));
Q4 =
-m1*(pdvx104*(vx10 - y10*D[q3[t], t] + D[q1[t], t]*C3 +
D[q2[t], t]*S3) +
pdvy104*(vx10 - y10*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx104*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy104*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m2*(pdvx204*(vx20 - y20*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy204*(vx20 - y20*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx204*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy204*(D[q3[t], t]*(-D[q2[t], t]*S3 - D[q1[t], t]*C3))) +
-m3*(pdvx304*(vx30 - y30*D[q3[t], t] + D[q1[t], t]*C3 +
D[q2[t], t]*S3) +
pdvy304*(vx30 - y30*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx304*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy304*(D[q3[t], t]*(-D[q2[t], t]*S3 - D[q1[t], t]*C3))) +
m1*((pdvx104 - D[q3[t], t]*pdy104)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy104 D[q3[t], t]*pdx104)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m2*((pdvx204 - D[q3[t], t]*pdy204)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy204 D[q3[t], t]*pdx204)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m3*((pdvx304 - D[q3[t], t]*pdy304)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy304 D[q3[t], t]*pdx304)*(D[q2[t], t]*C3 - D[q1[t], t]*S3));
Ma1 = D[D[q1[t], t],
t]*(m1*(pdx104*C3 - pdy104*S3) + m2*(pdx204*C3 - pdy204*S3) +
m3*(pdx304*C3 - pdy304*S3)) +
D[D[q2[t], t],
t]*(m1*(pdx104*S3 + pdy104*C3) + m2*(pdx204*S3 + pdy204*C3) +
48
m3*(pdx304*S3 + pdy304*C3)) +
D[D[q3[t], t],
t]*(m1*(x10*pdy104 - y10*pdx104) + m2*(x20*pdy204 - y20*pdx204) +
m3*(x30*pdy304 - y30*pdx304)) +
m1*(wx10*pdx104 + wy10*pdy104) + m2*(wx20*pdx204 + wy20*pdy204) +
m3*(wx30*pdx304 + wy30*pdy304) - Q4Cor - Q4n - Q4;
{Plot[Q4Cor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q4Cor"}],
Plot[Q4n, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q4n"}],
Plot[Q4, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q4"}]}
Plot[Ma1, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Blue, Thick], AxesLabel -> {"t", "Ma1"}]
Q5Cor = -2*
D[q3[t],
t]*(m1*(vx10*pdy105 - vy10*pdx105) +
m2*(vx20*pdy205 - vy20*pdx205) + m3*(vx30*pdy305 - vy30*pdx305));
Q5n = -D[q3[t], t]^2*(m1*(x10*pdx105 + y10*pdy105) +
m2*(x20*pdx205 + y20*pdy205) + m3*(x30*pdx305 + y30*pdy305));
Q5 = -m1*(pdvx105*(vx10 - y10*D[q3[t], t] + D[q1[t], t]*C3 +
D[q2[t], t]*S3) +
pdvy105*(vx10 - y10*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx105*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy105*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m2*(pdvx205*(vx20 - y20*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy205*(vx20 - y20*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx205*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy205*(D[q3[t],
t]*(-D[q2[t], t]*S3 49
D[q1[t], t]*C3))) + -m3*(pdvx305*(vx30 - y30*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy305*(vx30 - y30*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx305*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy305*(D[q3[t], t]*(-D[q2[t], t]*S3 - D[q1[t], t]*C3))) +
m1*((pdvx105 - D[q3[t], t]*pdy105)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy105 D[q3[t], t]*pdx105)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m2*((pdvx205 - D[q3[t], t]*pdy205)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy205 D[q3[t], t]*pdx205)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m3*((pdvx305 - D[q3[t], t]*pdy305)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy305 D[q3[t], t]*pdx305)*(D[q2[t], t]*C3 - D[q1[t], t]*S3));
Ma2 = D[D[q1[t], t],
t]*(m1*(pdx105*C3 - pdy105*S3) + m2*(pdx205*C3 - pdy205*S3) +
m3*(pdx305*C3 - pdy305*S3)) +
D[D[q2[t], t],
t]*(m1*(pdx105*S3 + pdy105*C3) + m2*(pdx205*S3 + pdy205*C3) +
m3*(pdx305*S3 + pdy305*C3)) +
D[D[q3[t], t],
t]*(m1*(x10*pdy105 - y10*pdx105) + m2*(x20*pdy205 - y20*pdx205) +
m3*(x30*pdy305 - y30*pdx305)) +
m1*(wx10*pdx105 + wy10*pdy105) + m2*(wx20*pdx205 + wy20*pdy205) +
m3*(wx30*pdx305 + wy30*pdy305) - Q5Cor - Q5n - Q5;
{Plot[Q5Cor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q5Cor"}],
Plot[Q5n, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q5n"}],
Plot[Q5, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q5"}]}
Plot[Ma2, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Blue, Thick], AxesLabel -> {"t", "Ma2"}]
50
Q6Cor = -2*
D[q3[t],
t]*(m1*(vx10*pdy106 - vy10*pdx106) +
m2*(vx20*pdy206 - vy20*pdx206) + m3*(vx30*pdy306 - vy30*pdx306));
Q6n = -D[q3[t], t]^2*(m1*(x10*pdx106 + y10*pdy106) +
m2*(x20*pdx206 + y20*pdy206) + m3*(x30*pdx306 + y30*pdy306));
Q6 = -m1*(pdvx106*(vx10 - y10*D[q3[t], t] + D[q1[t], t]*C3 +
D[q2[t], t]*S3) +
pdvy106*(vx10 - y10*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx106*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy105*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m2*(pdvx206*(vx20 - y20*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy206*(vx20 - y20*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx206*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy205*(D[q3[t],
t]*(-D[q2[t], t]*S3 D[q1[t], t]*C3))) + -m3*(pdvx306*(vx30 - y30*D[q3[t], t] +
D[q1[t], t]*C3 + D[q2[t], t]*S3) +
pdvy306*(vx30 - y30*D[q3[t], t] + D[q2[t], t]*C3 D[q1[t], t]*S3) +
pdx306*(-D[q3[t], t]*(D[q1[t], t]*S3 - D[q2[t], t]*C3)) +
pdy305*(D[q3[t], t]*(-D[q2[t], t]*S3 - D[q1[t], t]*C3))) +
m1*((pdvx106 - D[q3[t], t]*pdy106)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy106 D[q3[t], t]*pdx106)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m2*((pdvx206 - D[q3[t], t]*pdy206)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy206 D[q3[t], t]*pdx206)*(D[q2[t], t]*C3 - D[q1[t], t]*S3)) +
m3*((pdvx306 - D[q3[t], t]*pdy306)*(D[q1[t], t]*C3 +
D[q2[t], t]*S3) + (pdvy306 51
D[q3[t], t]*pdx306)*(D[q2[t], t]*C3 - D[q1[t], t]*S3));
Ma3 = D[D[q1[t], t],
t]*(m1*(pdx106*C3 - pdy106*S3) + m2*(pdx206*C3 - pdy206*S3) +
m3*(pdx306*C3 - pdy306*S3)) +
D[D[q2[t], t],
t]*(m1*(pdx106*S3 + pdy106*C3) + m2*(pdx206*S3 + pdy206*C3) +
m3*(pdx306*S3 + pdy306*C3)) +
D[D[q3[t], t],
t]*(m1*(x10*pdy106 - y10*pdx106) + m2*(x20*pdy206 - y20*pdx206) +
m3*(x30*pdy306 - y30*pdx306)) +
m1*(wx10*pdx106 + wy10*pdy106) + m2*(wx20*pdx206 + wy20*pdy206) +
m3*(wx30*pdx306 + wy30*pdy306) - Q6Cor - Q6n - Q6;
{Plot[Q6Cor, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q6Cor"}],
Plot[Q6n, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q6n"}],
Plot[Q6, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Green, Thick], AxesLabel -> {"t", "Q6"}]}
Plot[Ma3, {t, 0, deltat}, AxesOrigin -> {0, 0},
PlotStyle -> Directive[Blue, Thick], AxesLabel -> {"t", "Ma3"}]
52
Отзывы:
Авторизуйтесь, чтобы оставить отзыв