МІНІСТЕРСТВО ОСВІТИ І НАУКИ УКРАЇНИ

Національний університет кораблебудування

імені адмірала Макарова

О. О. ЧЕРНО, А. П. ГУРОВ, М. В. ПОКРОВСЬКИЙ, О. А. АВДЄЄВА

МЕТОДИЧНІ ВКАЗІВКИ ДО ЛАБОРАТОРНИХ РОБІТ

З дисципліни «Системи управління маніпуляційних роботів»

Рекомендовано Методичною радою НУК

Електронне видання
комбінованого використання на DVD-ROM

Миколаїв  НУК  2018

УДК 621.865.8: 007.52
Ч49

Автори О. О. Черно, канд. техн. наук, доцент,

А. П. Гуров, канд. техн. наук, професор,
М. В. Покровський, канд. техн. наук,
О. А. Авдєєва, канд. техн. наук
Рецензент С. М. Новогрецький, канд. техн. наук, доцент

Черно О. О.
Ч Системи управління маніпуляційних роботів: Методичні вказів-
ки до лабораторних робіт / О. О. Черно, А. П. Гуров, М. В. Покровсь-
кий, О. А. Авдєєва. – Миколаїв : НУК, 2018. – 28 с.

Вказівки містять опис лабораторних робіт, що виконуються студен-

тами при вивченні дисципліни "Системи управління маніпуляційних робо-
тів".
Призначені для студентів спеціальностей "Автоматизація та
комп’ютерно-інтегровані технології" і "Електроенергетика, електротехніка
та електромеханіка". Також можуть бути використані в процесі вивчення
інших дисциплін технічного спрямування, студентами інших спеціальнос-
тей вищих навчальних закладів.

в А.П.

© Черно О. О., Гуров А. П.,

Покровський М. В., Авдєєва О. А., 2018
© Національний університет кораблебудування
імені адмірала Макарова, 2018

2
 ВСТУП

Промислові роботи і збудовані на їх основі роботизовані комплекси є

перспективним засобом комплексної автоматизації виробничих процесів
[1 – 3]. Застосування роботів дозволяє істотно підвищити продуктивність
праці, ефективність діючого і заново розробляємого обладнання, а також
позбавляє людину від монотонних, фізично важких, ризикованих та шкід-
ливих для здоров'я робіт [4, 5].
Дані вказівки присвячені комплексу лабораторних робіт з кінематики
маніпуляторів та систем управління промисловими роботами. Проведення
лабораторних робіт дозволить поглибити і закріпити знання студентів з
дисципліни "Системи управління маніпуляційних роботів".

3
 ЛАБОРАТОРНА РОБОТА № 1
МАТРИЦІ ПОВОРОТУ

Мета роботи: визначення координат точки, зв’язаної з системою ві-

дліку, що обертається, за допомогою матриці повороту.

Теоретичні відомості
Для опису обертального руху зв'язаної системи відліку відносно аб-
солютної використовується матриця повороту розмірністю 3 × 3. Вона
визначається як матриця перетворення тривимірного вектора положення в
евклідовому просторі, переводить його координати з поверненої (зв'язаної)
системи відліку UVW в абсолютну систему координат XYZ [4].

PUVW  ( pU , pV , pW )T

P XYZ  ( p X , pY , pZ )T

Необхідно визначити матрицю R, яка перетворює координати PUVW в

координати P XYZ :
 
P XYZ  RPUVW
    (1)
PUVW  pU iU  pV jV  pW k W

Визначаємо проекції вектора P на осі X,Y і Z :
       
p X  i X  P  i X  iU  pU  i X  jV  pV  i X  k W  pW
       
pY  j Y  P  j Y  iU  pU  jY  jV  pV  j Y  k W  pW
       
pZ  k Z  P  k Z  iU  pU  k Z  jV  pV  k Z  k W  pW

Або в матричній формі :

     
p
 X  i X  k W   pU 
i X  iU i X  jV
 p    j i  
    
j Y  jV
j Y  k W   pV
 Y    Y U     
 
 pZ   k Z  iUk Z  k W   pW 
k Z  jV

 
Аналогічно координаті PUVW можна отримати з координат P XYZ :
 
PUVW  Q  P XYZ . (2)
Або

4
      
p
 U  i X  iU j Y  iU k Z  iU   p X 
 p   i  j
   
j Y  jV
  
k Z  jV    pY 
 V   X  V     
 pW  i X  k W k Z  jV k Z  k W   pZ 


З останнього виразу випливає, що

Q  R 1  R T

Перетворення, обумовлене формулою (1) або (2), називається орто-

гональним. Особливий інтерес представляють матриці поворотів навколо
координатних осей. Матриця повороту на кут  навколо осі Х:
1 0 0 
Rx , 
 0 cos  sin   .
 
 0 sin  cos  

Матриця повороту на кут  навколо осі Y:

cos  0 sin   cos sin 0
Ry , 
 0 1 0  ., Rz ,   sin  cos  0 
 
 sin  0 cos    0 0 1 

Матриця повороту на кут  навколо осі Z:

cos  0 sin    cos  sin  0 
Ry ,   0 1 0  , Rz ,    sin  cos  0  .

 sin  0 cos    0 0 1 

Матриці Rx , , Ry , , Rz , називаються матрицями елементарних пово-

ротів.
Будь-який поворот у просторі можна представити послідовністю з 3-
х поворотів навколо координатних осей, а матрицю повороту отримати
шляхом перемноження матриць елементарних поворотів. Оскільки опе-
рація множення матриць не є комутативною, тут істотне значення має
послідовність виконання поворотів. Результуюча матриця повороту може
бути отримана з використанням наступних простих правил:
− якщо рухома система координат UVW робить поворот навколо од-
нієї з осей системи XYZ, відповідну матрицю елементарного повороту по-
трібно помножити зліва на матрицю попереднього результуючого поворо-
ту;

5
 − якщо рухома система координат UVW робить поворот навколо од-
нієї зі своїх власних осей, відповідну матрицю елементарного повороту по-
трібно помножити справа на матрицю попереднього результуючого пово-
роту.
Завдання
Задані координати точки Р відносно рухомої системи UVW. Система
UVW здійснює поворот у просторі, що описується наступною послідовніс-
тю поворотів: на кут 55 навколо осі Z, на 40 навколо осі V, на 15 навколо
осі Х. Необхідно знайти координати точки Р відносно нерухомої системи
XYZ.
Хід роботи
В середовищі Mathcad складаємо обчислювальну програму (рис. 1.1).
Початкові дані

Координати вектора відносно рухомої системи координат:

T
Puvw  ( 3 10 5 )

Кути поворотів:
  
  15   40   55
180 180 180

Послідовність поворотів:  навколо Z,  навколо V,  навколо X,

Розв'язання задачі

Матриці елементарних поворотів:

1 0 0   cos   0 sin    cos    sin   0 

Rx    0 cos   sin    
 Rv     0 1 0  Rz    sin    cos    0 
     
 0 sin    cos     sin   0 cos     0 0 1

Матриця результуючого повороту:

R  Rx   Rz   Rv  

 0.439 0.819 0.369

R   0.772 0.554 0.31 
 
 0.458 0.148 0.876
Координати вектора Р відносно абсолютної системи координат:

T
Pxyz  R Puvw Pxyz  ( 5.03 9.409 4.49)

Рис. 1.1. Програма розрахунку координат вектора

6
 Таким чином, за допомогою розробленої програми, були знайдені
координати точки Р у нерухомій системі відліку.

Зміст звіту
1. Тема та мета роботи.
2. Завдання.
3. Текст програми.
4. Висновки.
Контрольні питання
1. Що таке матриця повороту?
2. Для чого вона використовується?
3. Який поворот називають елементарним?
4. Як представити складний поворот у просторі за допомогою мат-
риць елементарних поворотів?

ЛАБОРАТОРНА РОБОТА №2
ПРЯМА ЗАДАЧА КІНЕМАТИКИ

Мета роботи: розв’язати пряму задачу кінематики для трьохланко-

вого маніпулятора

Теоретичні відомості

Ланки роботів зазвичай з'єднані зчленуваннями з одним ступенем

свободи: обертальними або поступальними. Для опису обертальних і по-
ступальних зв'язків між сусідніми ланками Денавіт і Хантенберг запропо-
нували матричний метод послідовної побудови систем координат, пов'яза-
них з кожною ланкою кінематичного кола [4]. Сенс представлення Де-
навіта-Хартенберга полягає у формуванні однорідної матриці перетворен-

7
ня розмірністю 4 × 4 і описує положення системи кожної ланки щодо си-
стеми координат попередньої ланки. Це дає можливість послідовно пере-
творити координати робочого органу маніпулятора з системи відліку, по-
в'язаної з останньою ланкою, в базову систему відліку, яка є інерціальній
системою координат.
Крім базової системи (x0, y0, z0) для кожної ланки на осі її зчленуван-
ня визначається система координат (xi, yi, zi), де i=1, 2, ... n; n - число сту-
пенів свободи маніпулятора. Кожна система координат (xi, yi, zi) відповідає
(i +1)-y сочленению і пов'язана з i-м ланкою.Кожна система координат фо-
рмується на підставі наступних правил:
1. Вісь Zi-1 спрямована вздовж осі i-го зчленування.
2. Вісь Xi перпендикулярна осі Xi-1 і спрямована від неї.
3. Вісь Yi доповнює осі Xi і Zi до правої системи координат.
Нульова і n-а системи координат можуть бути обрані довільним чи-
ном, за умови, що вісь Z0 спрямована уздовж осі першого зчленування, а
вісь Xn перпендикулярна осі Zn-1.Положення пов'язаних систем координат
описується за допомогою чотирьох параметрів: ai - найкоротша відстань
між zi і zi-1; αi - кут між zi і zi-1 (навколо xi); di - найкоротша відстань між xi і
xi-1; Θi - кут між xi xi-1 (навколо zi-1).
Якщо ланки з'єднані обертальним зчленуванням, то кут Θi є змінною
величиною. Якщо поступальним - то змінною є di.
Координати довільної точки в i-тій системі координат можна пере-
творити в координати цієї ж точки в (i-1)-й системі, виконавши наступну
послідовність операцій:
1. Поворот навколо осі zi-1 на кут Θi, щоб вісь xi-1 стала співспрямо-
вана з віссю xi.
2. Зрушення уздовж осі zi-1 на відстань di, щоб поєднати xi-1 і xi.
3. Зрушення уздовж осі xi на відстань ai, щоб поєднати початку коор-
динат.
4. Поворот навколо осі xi на кут αi, в результаті якого досягається

8
збіг систем координат.
Кожну з цих операцій можна представити однорідною матрицею
елементарного повороту чи зсуву, а добуток таких матриць дасть од-
норідну матрицю складного перетворення для суміжних систем координат:
1 0 0 0
0 1 0 0 
i 1
Ai  Tz , Tz ,d Tx , aTx ,  
0 0 1 di 
 
0 0 0 1
 cos i  sin i 0 0  1 0 0 ai  1 0 0 0
 sin  cos i 0    
0   0 1 0 0   0 cos  i  sin i 0 
 i
  
 0 0 1 0   0 0 1 0   0 sin  i cos  i 0
     
 0 0 0 1 0 0 0 1  0 0 0 1
 cos i  cos  i sin i sin  i sin i ai cos i 
 sin  cos  i cos i  sin  i cos i ai sin i 
 i

 0 sin  i cos  i di 
 
 0 0 0 1 
i 1
Матрица Ai называется ДХ – матрицею перетворення.
i 1
Матрица, зворотна до Ai має вигляд:

 cos i sin i 0  ai 
  cos  sin  cos  i cos i sin  i  d i sin i 
(i 1 Ai )1 i 1 Ai   i i

 sin  i sin i  sin  i cos i cos  i d i cos  i 

 
 0 0 0 1 

Однорідна матриця 0 Ti , що визначає положення i-ї системи коорди-

нат відносно базової являє собою добуток послідовності однорідних мат-
риць перетворення:

0
i
0R 0
Pi 
Ti  0 A 1 A  ...  i 1 Ai   j 1
Aj   i 
j 1  0 1 

де 0 Ri - матриця, що визначає орієнтацію i-й системи координат по відно-

шенню до базової; 0 Pi - вектор, що з'єднує початку базової і i-ї систем ко-
ординат.
Пряма задача кінематики полягає у визначенні положення і орієнта-
ції робочого органа (схвата) маніпулятора щодо абсолютної системи коор-
динат. Її розв’язання зводиться до визначення так званої «матриці маніпу-

9
лятора»:
 nx s x a x p x 
   

n
 0 R 0 Pi   n s a p   n y s y a y p y 
T  0 Tn   i 1 Ai   i   ,
i 1  0 1   0 0 0 1   nz s z az pz 
 
0 0 0 0 
  
де n - вектор нормалі до схвату; s - дотичний вектор схвату; a - вектор

підходу схвату; p - вектор положення схвату.
Завдання
За заданою конфігурацією маніпулятора і поточними значеннями ві-
дносних переміщень ланок визначити координати та орієнтацію схвату у
просторі.
Хід роботи
В середовищі Mathcad складаємо обчислювальну програму (рис. 2.1).

Довжини ланок:
T
L  ( 0 1 3 3 )
Параметри, що характеризують конфігурацію маніпулятора:

a  0 0 L
2
L
3 T
T

   0 0 0 
 2 

d  0 L
1
0 0 T
N  4 (Номер варианта)
Поточні значення кутів повороту ланок:
T
N 2 N   3 N  
   0 

 180 180 180 
Визначаємо матриці перетворення для систем координат сміжних ланок:
n  3 (Число ланок)
i  1  n (Номер ланки)

cos  i  cos  i  sin  i  sin i sin  i ai cos i
 
 sin  i cos  i  cos  i  sin  i  cos i ai sin  i 
A   
i
 0 sin    cos    d 
i i i
 
 0 0 0 1 
Перемножуємо матриці перетворення для систем координат сміжних ланок і
отримуємо матрицю маніпулятора:
n
T   Ai
i 1

10
  0.995 0.07 0.07 5.949
 3

T  0.07 4.866 10 0.998 0.416
 0.07 0.998 0 1.208
 
 0 0 0 1 

Перевірка:
 1  2  2  L3 cos 2  3
x  cos   L  cos 

y  sin    L  cos    L  cos      

1 2 2 3 2 3
z  L  L  sin    L  sin    
1 2 2 3 2 3
x  5.949
y  0.416
z  1.208
Рис. 2.1. Програма розв’язання прямої задачі кінематики

Результати перевірки підтвердили правильність розв’язання задачі.

Зміст звіту
1. Тема та мета роботи.
2. Завдання.
3. Текст програми.
4. Висновки.
Контрольні питання
1. Що являє собою матриця перетворення?
2. Що описує матриця елементарного перетворення?
3. Як за допомогою матриць елементарних перетворень описується
взаємне розташування двох систем координат?
4. Що таке зв’язані системи координат?
5. Якими параметрами задається взаємне розташування ланок мані-
пулятора?
6. Як визначається матриця перетворення систем координат суміж-
них ланок?
7. У чому полягає пряма задача кінематики?
8. Що задається у початкових даних?
9. Яким чином отримують матрицю маніпулятора?
10. Які основні блоки містить розроблена програма?
11
 ЛАБОРАТОРНА РОБОТА №3
ЗВОРОТНА ЗАДАЧА КІНЕМАТИКИ

Мета роботи: розв’язати зворотну задачу кінематики для трьохлан-

кового маніпулятора.

Теоретичні відомості

Зворотна задача кінематики полягає у визначенні приєднаних коор-

динат ланок маніпулятора, що забезпечують певне положення та орієнта-
цію схвату у просторі [4]. Розв’язання цієї задачі здійснюється для плану-
вання траєкторії маніпулятора.
Контрольними точками траєкторії є: початкова точка, точка уходу,
точка підходу і кінцева точка. Координати початкової точки x t 0 , y t 0 i z t 0 і
точки уходу x t1 , y t1 i z t1 задані у початкових даних. Координати кінцевої
точки дорівнюють координатам вантажу в момент часу t  t 3 :
xt 3  xв t3 ; yt 3  yв t 3 ; zt 3  z в t3  .
Точка підходу повинна знаходитись на відстані z t 23 над вантажем в
момент часу t  t 2 :
xt 2  xв t 2 ; yt 2  yв t 2 ; zt 2  zв t 2   z t 23 .
Для кожної контрольної точки необхідно розв'язати зворотну задачу
кінематики, тобто визначити кути повороту ланок маніпулятора 1 ,  2 i  3
(рис. 3.1), при яких координати схвату x; y; z з певною точністю співпада-
ють з координатами заданої точки x t ; y t ; z t :
x  xt  x; y  yt  y; z  zt  z .
Похибки x, y i z повинні бути на порядок меншими за припустимі
похибки позиціювання схвату:
x  0,1x max ; y  0,1y max ; z  0,1z max .

12
 z0
3
L2

L3
2

1
L1

y0
x0
Рис. 3.1. Кінематична схема маніпулятора
T
Таким чином, вектор приєднаних координат θ  1 2  3  по-
винен задовольняти наступній умові:
 xθ  xt  x    y θ   y t  y    z θ   z t  z  , (1)
де координати схвату xθ , y θ  i z θ  визначаються в процесі розв'я-
зання прямої задачі кінематики для кінематичної схеми (рис. 3):
xθ   L2 cos  2  L3 cos 2   3  cos 1 ; (2)
y θ  L2 cos  2  L3 cos 2   3  sin 1 ; (3)
z θ   L1  L2 sin  2  L3 sin  2   3  . (4)
Для визначення необхідних кутів 1 ,  2 i  3 найбільш зручно вико-
ристовувати метод градієнтного спуску. Суть даного методу зводиться до
пошуку мінімуму функціоналу
1
J θ  
2
 
xθ   xt 2   yθ   yt 2  z θ   zt 2  min (5)

шляхом зміни координат вектора θ (кутів повороту ланок) у циклі за на-

ступним алгоритмом:
J
 i :  i   i , (6)
 i
де і – номер ланки;  i – ітераційний коефіцієнт. Враховуючи (2) – (6), ал-
горитм зміни кутів 1 ,  2 i  3 для маніпулятора (рис. 9.1), визначається на-
ступною послідовністю операцій присвоєння:

13
  x y 
1 : 1   1    x  x t    y  yt   ; (7)
  1   1 

 x y z 
 2 :  2   2    x  x t    y  yt   z  z t   ; (8)
  2  2  2 
 x y z 
 3 :  3   3    x  x t    y  yt   z  z t   , (9)
  3  3  3 

де
x
 L2 cos  2  L3 cos 2   3  sin 1 ;
1
x x
 L2 sin  2  L3 sin  2   3  cos 1 ;   L3 sin  2   3  cos 1 ;
 2  3
y
 L2 cos  2  L3 cos 2   3  cos 1 ;
1
y y
 L2 sin  2  L3 sin  2   3  sin 1 ;   L3 sin  2   3  sin 1 ;
 2  3
z z
 L2 cos  2  L3 cos 2   3  ;  L3 cos 2   3  .
 2  3
Таким чином, розв'язання зворотної задачі кінематики зводиться до
наступного: спочатку кутам 1 ,  2 i  3 присвоюються довільні значення, а
потім вони змінюються в циклі за алгоритмом (7) – (9), доки не задоволь-
нять умові (1). Ітераційні коефіцієнти  i впливають на швидкість та стій-
кість розрахунку. Якщо значення цих коефіцієнтів обрати занадто малими,
процес розрахунку буде дуже довгим, а якщо великими – обчислювання
буде розбіжним, і задачу взагалі не буде розв'язано. Тому, для кожного
окремого випадку ітераційні коефіцієнти треба підбирати індивідуально.

Завдання
За заданою конфігурацією маніпулятора, координатами схвату і при-
пустимими помилками визначити відносні кути повороту ланок маніпуля-
тора.

14
 Хід роботи
В середовищі Mathcad складаємо обчислювальну програму, де спо-
чатку задаємо початкові дані для зворотної задачі кінематики (рис. 3.2).

Номер студента за списком: N  25

Потрібні координати схвату:

xr  0.4  0.03 N xr  1.15

y r  0.3  0.04 N y r  1.3

zr  0.8  0.05 N zr  2.05

Довжини ланок маніпулятора:

L1  1 L2  1.3 L3  1.2

Конфігурація маніпулятора:

    
x   cos  1  L2 cos  2  L3 cos  2   3 
    
y    sin  1  L2 cos  2  L3 cos  2   3 
  
z   L1  L2 sin  2  L3 sin  2   3 
Припустимі помилки позиціювання схвату:
x  0.001 y  0.001 z  0.001
Рис. 3.2. Завдання початкових даних для зворотної задачі кінематики

Задаємо первинні значення ітераційних коефіцієнтів (рис. 3.3).

Задаємо ітераційні коефіцієнти:

 1  1  2  1  3  1
Рис. 3.3. Завдання ітераційних коефіцієнтів

На підставі методу градієнтного спуску складаємо програму

розв’язання зворотної задачі кінематики (рис. 3.4). Результат виводимо у
матрицю  , в 0-й елемент якої записуємо кількість виконаних ітерацій і, а
в 1-й, 2-й і 3-й – знайдені значення кутів повороту ланок. Якщо після вихо-
ду з циклу і = 104, умова (1) не виконалася, і потрібно змінити ітераційні
коефіцієнти і повторити розрахунок. Таким чином, підбираємо ітераційні
коефіцієнти та отримуємо розв’язок задачі (рис. 3.4).

15
Складаємо обчислювальну програму за методом градієнтного спуску:

  1 
6

2 
6

3  
4
4
for i  1  10
x  cos  1 L2 cos  2  L3 cos  2  3
 
y  sin  1 L2 cos  2  L3 cos  2  3 

z  L1  L2 sin  2  L3 sin  2  3

break if  xr  x  x   yr  y  y   zr  z  z 

dx1  sin 1   L2 cos  2  L3 cos 2  3

dx2  cos  1   L2 sin  2  L3 sin  2  3 

dx3  L3 cos  1  sin 2  3


dy1  cos  1 L2 cos  2  L3 cos  2  3 
dy2  sin  1   L2 sin  2  L3 sin  2  3 

dy3  L3 sin  1  sin  2  3

dz1  0
dz2  L2 cos  2  L3 cos 2  3

dz3  L3 cos  2  3

      
1  1   1  xr  x  dx1  y r  y  dy1  zr  z  dz1

2  2   2  xr  x  dx2   y r  y   dy2  zr  z  dz2
 
3  3   3  xr  x  dx3   y r  y   dy3  zr  z  dz3
 
0  i

 1  1
 2  2
 3  3


 308 
0.847 

 1.139 
 1.248
 

Рис. 3.4. Програма розв’язання зворотної задачі кінематики

16
 Шляхом розв’язання прямої задачі кінематики робимо перевірку пра-
вильності визначення кутів повороту ланок (рис. 3.5).
Розв'язання прямої задачі кінематики:

    
x  cos  1  L2 cos  2  L3 cos  2   3  x  1.151 xr  1.15

y  sin   1   L2 cos  2  L3 cos   2   3  y  1.301 y r  1.3

  
z  L1  L2 sin  2  L3 sin  2   3  z  2.051 zr  2.05

Помилки:
4 4 4
xr  x  8.791 10 y r  y  9.938 10 zr  z  5.519 10

Рис. 3.5. Перевірка результатів розв’язання зворотної задачі кінематики

Результати перевірки підтверджують правильність розв’язання задачі,

а отримані помилки не перевищують заданих величин.

Зміст звіту
1. Тема та мета роботи.
2. Завдання.
3. Текст програми.
4. Висновки.
Контрольні питання
1. У чому полягає зворотна задача кінематики?
2. Що задається у початкових даних?
3. Що таке приєднані координати маніпулятора?
4. Який метод розв’язання зворотної задачі кінематики використову-
ється в даній лабораторній роботі?
5. Що означають змінні, використані в програмі?
6. Що означає вираз у квадратних дужках?
7. На що впливають ітераційні коефіцієнти?
8. Яку змінну виводять у результуючу матрицю θ для перевірки пра-
вильності розв’язання задачі?

17
 ЛАБОРАТОРНА РОБОТА №4
ПЛАНУВАННЯ ТРАЄКТОРІЇ МАНІПУЛЯТОРА

Мета роботи: визначити закони зміни у часі кутів повороту ланок

маніпулятора, що забезпечують задану траєкторію руху схвату.

Теоретичні відомості
Задача планування траєкторії маніпулятора полягає у визначенні за-
конів зміни у часі приєднаних змінних, що забезпечують задану траєкто-
рію схвату.
Траєкторія схвату задається у вигляді координат контрольних точок,
через які повинен пройти схват і моментів часу їх проходження. Звичайно
задаються 4 точки: початкова точка, точка уходу, точка підходу і кінцева
точка [4].
Найбільш розповсюджений підхід до планування траєкторії маніпу-
лятора передбачає 2 етапи:
− розв’язання зворотної задачі кінематики для контрольних точок
траєкторії;
− інтерполяція законів зміни приєднаних змінних неперервними фу-
нкціями часу.
Для інтерполяції залежностей, які задані великим числом точок, або
для яких сформульовано багато додаткових умов (задані значення першої
та другої похідних у точках), звичайно використовуються сплайн-функції
[4]. Сплайн-функція представляє собою послідовність поліномів низького
ступеня (поліноміальних сплайнів), кожен з яких описує її поведінку на
певному інтервалі значень аргументу. В залежності від кількості інтерва-
лів розбиття області значень аргументу та порядків інтерполюючих полі-
номів існує багато різновидів сплайн-функцій. Для інтерполяції законів
зміни кутів повороту ланок маніпулятора будемо використовувати функ-
цію "4 – 3 – 4". Тоді закон зміни кута повороту і-ї ланки прийме наступ-

18
ний вигляд:
 a14 i  t  t 0 4  a13 i  t  t 0 3  a12 i  t  t 0  2 

  a11i  t  t 0   a10 i при t 0  t  t1 ;

 a 23  t  t 3  a 22  t  t  2 
 i t    i 1 i 1

  a 21i  t  t1   a 20 i при t1  t  t 2 ;

a34  t  t  4  a33  t  t 3  a32  t  t  2 
i 2 i 2 i 2

 a31i  t  t 2   a30 i при t 2  t  t 3 ,

де коефіцієнти поліномів визначаються шляхом розв'язання рівнянь, скла-

дених на основі умов проходження через контрольні точки, забезпечення
необхідних значень першої та другої похідних функції у крайніх точках, а
також умов неперервності першої та другої похідних в точках сполучень
сплайнів:
a10 i  i t 0 ; a 20 i  i t1 ; a30 i  i t 2 ; a11i  0; a12 i  0;
T
a14 i a13i a 23i a 22 i a 21i a34 i a33i a32 i a31i   C 1  Fi ;

 t14 t13 0 0 0 0 0 0 0 
 3 
 4t1 3t12 0 0 1 0 0 0 0 
12t12 6t1 0 2 0 0 0 0 0 
 
 0 0 t 23 t 22 t 2 0 0 0 0 
C 0 0 3t 22 2t 2 1 0 0 0  1 ;
 
 0 0 6 t 2 2 0 0 0 2 0 
 0 0 0 0 0 t 34 t 33 t 32 t 3 
 
 0 0 0 0 0 4t 33 3t 32 2 t 3 1 
 
 0 0 0 0 0 12t 32 6t 3 2 0 
T
Fi  i t1   i t 0  0 0 i t2   i t1  0 0 i t3   i t 2  0 0 ;
t1  t1  t 0 ; t 2  t 2  t1 ; t 3  t3  t 2 .

Завдання
Розв’язати задачу планування траєкторії трьохланкового маніпуля-
тора.

19
 Хід роботи
В середовищі Mathcad складаємо обчислювальну програму, де зада-
ємо початкові дані: координати контрольних точок траєкторії схвату та
моменти часу їх проходження, довжини ланок і конфігурацію маніпулято-
ра, припустимі помилки позиціювання схвату (рис. 4.1).

Номер студента за списком: N  25

Координати контрольних точок траєкторії схвату:

T T
xr  ( 0.01 N 0.01 N 1.2 1.2 ) xr  ( 0.25 0.25 1.2 1.2 )

T
y r  ( 1 1 1.1 1.1 )

T
zr  ( 0 0.3 1.3 1 )

Моменти часу проходження контрольних точок:

T
T  ( 0 2 12 15 )

Довжини ланок маніпулятора:

L1  1 L2  1.3 L3  1.2

Конфігурація маніпулятора:

    
x   cos  1  L2 cos  2  L3 cos  2   3 
y    sin   1 L2 cos   2  L3 cos  2   3

  
z   L1  L2 sin  2  L3 sin  2   3 
Припустимі помилки позиціювання схвату:
x  0.001 y  0.001 z  0.001

Рис. 4.1. Початкові дані до задачі планування траєкторії маніпулятора

Задаємо ітераційні коефіцієнти і складаємо програму розв’язання

зворотної задачі кінематики для 4-х точок траєкторії маніпулятора
(рис. 4.2). Розраховані кути повороту ланок записуємо в матрицю q , де
номери строк відповідають номерам ланок маніпулятора (окрім 0-ї строки,
де зберігається кількість виконаних ітерацій і), а номери стовбців відпові-
дають номерам точок траєкторії.

20
Задаємо ітераційні коефіцієнти:
 1  0.05  2  0.05  3  0.05

Складаємо обчислювальну програму за методом градієнтного спуску:

q  for j  0 3

1 
6

2 
6

3  
4
for i  1  10000
x  cos 1  L2 cos  2  L3 cos 2  3
 
y  sin 1  L2 cos  2  L3 cos  2  3 

z  L1  L2 sin  2  L3 sin  2  3

break if  xr  x  x   y r  y  y    zr  z  z

 j   j   j 

dx1  sin 1  L2 cos  2  L3 cos 2  3 
dx2  cos  1  L2 sin  2  L3 sin  2  3 

dx3  L3 cos  1 sin 2  3

dy1  cos 1 L2 cos  2  L3 cos  2  3

 
dy2  sin  1   L2 sin  2  L3 sin  2  3 

dy3  L3 sin  1  sin 2  3

dz1  0
dz2  L2 cos  2  L3 cos 2  3

dz3  L3 cos  2  3

1  1   1   xr  x  dx1   y rj  y   dy1   zrj  z  dz1

 j      
2  2   2   xr  x  dx2   y rj  y   dy2   zrj  z  dz2
 j      
3  3   3   xr  x  dx3   y rj  y   dy3   zrj  z  dz3
 j      
 0 j  i

 1  j  1
 2  j  2
 3  j  3


 170 168 212 202


1.326 1.326 0.742 0.742 
q
 0.132 0.383 0.984 0.815 
 1.919 
 2.1 1.695 1.724

Рис. 4.2. Програма розв’язання зворотної задачі кінематики

21
 Складаємо програму інтерполяції законів зміни кутів повороту ланок
сплайн-функціями (рис. 4.3).
Проміжки часу:
j  0  rows ( T)  2 t j 1  T T
j 1 j
n  rows ( q )  1 i  1  n

a12  0 a11  0 a10  q a20  q a30  q

i i i i 0 i i 1 i i 2
Матриця коефіцієнтів системи рівнянь:

  t  4  t  3 0 0 0 0 0 0 0 
 1 1 
 3 2

 4  t1 3  t1 0 0 1 0 0 0 0 
 
 12  t 1 2 6 t 1 0 2 0 0 0 0 0 
 
 0 0  t23  t22 t 2 0 0 0 0 
 
C   2
0 0 3 t 2 2 t2 1 0 0 0 1 
 
 0 0 6 t2 2 0 0 0 2 0 
 
 0 0 0 0 0 t34 t33 t32 t 3 
 
 0 0 0 0 0  3
4 t3  2
3 t3 2 t 3 1 
 
 0
 0 0 0 0  2
12 t 3 6 t 3 2 0 

Матриця правих частин системи рівнянь:

T
   q i  2  q i  1 qi 3  qi 2
B   q q 0 0 0 0 0 0
i i 1 i 0 
Розв'язання системи рівнянь матричним методом:
1
A  C B
i i
Коефіцієнти сплайн-функцій:
a14  A
i  i0 a13  A
i  i1 a23  A
i  i2 a22  A
i  i3 a21  A
i  i4
a34  A
i  i5 a33  A
i  i6 a32  A
i  i7 a31  A
i  i8
Сплайн-функції:
 ( t)  for i  1  n

i  04  a13it  T03  a12i t  T02  a11i t  T0  a10i

X  a14  t  T
i
if T  t  T
0 1
3 2
X  a23   t  T   a22   t  T   a21  t  T   a20 if T  t  T
i i 1 i 1 i 1 i 1 2
4 3 2
X  a34   t  T   a33   t  T   a32  t  T   a31   t  T   a30 if T  t  T
i i 2 i 2 i 2 i 2 i 2 3
X

22
 t  T  T  0.1 T
0 0 3

100

50

180
 ( t ) 1

0
180
 ( t ) 2


180 50
 ( t ) 3


100

150
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
t

Рис. 4.3. Програма інтерполяції законів зміни кутів повороту ланок

Робимо перевірку отриманих результатів і будуємо траєкторію схвату

в трьох проекціях (рис. 4.4).
Розв'язання прямої задачі кінематики:

   
x( t)  cos  ( t ) 1  L2 cos  ( t) 2  L3 cos  ( t) 2   ( t ) 3  
   
y ( t)  sin  ( t ) 1  L2 cos  ( t) 2  L3 cos  ( t) 2   ( t ) 3  
 
z( t)  L1  L2 sin  ( t) 2  L3 sin  ( t ) 2   ( t) 3  
1.5

1
x( t )

y ( t) 0.5
z ( t)
0

0.5
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
t

T T T
xr  ( 0.25 0.25 1.2 1.2 ) y r  ( 1 1 1.1 1.1 ) zr  ( 0 0.3 1.3 1 )

Траєкторія руху схвату в трьох проекціях:

23
 1.5 1.5
1.429 1.429

1 1

z( t) 0.5 z( t) 0.5

0 0

4 4
9.69510 0.5 9.69510 0.5
0.95 1 1.05 1.1 0 0.5 1 1.5
0.985 y( t) 1.105 0.249 x( t ) 1.216
0
 0.249

0.5

 x( t )

 1.216 1.5
0.95 1 1.05 1.1
0.985 y ( t) 1.105

Рис. 4.4. Перевірка результатів розв’язання задачі та побудова траєкторії

схвату в 3-х проекціях
Результати перевірки підтверджують правильність розв’язання задачі.

Зміст звіту
1. Тема та мета роботи.
2. Завдання.
3. Текст програми.
4. Висновки.
Контрольні питання
1. У чому полягає задача планування траєкторії?
2. Що задається у початкових даних?
3. У чому полягають 2 підходи до планування траєкторії? Які етапи
передбічає кожен з них?
4. Яка функція використовується для інтерполяції законів зміни кутів
повороту ланок у часі? Скільки вона має коефіцієнтів?
5. З яких умов визначаються коефіцієнти інтерполюючої функції?

24
 6. Як використовують результати планування траєкторії для керу-
вання маніпулятором?

ЛАБОРАТОРНА РОБОТА №5
ДИНАМІКА МАНІПУЛЯТОРА

Мета роботи: визначення керуючих моментів, що забезпечують за-

дані кути повороту, кутові швидкості та кутові прискорення ланок маніпу-
лятора.

Завдання
Задана конфігурація маніпулятора, поточні значення кутів повороту
ланок, кутові швидкості та кутові прискорення. Знайти величини керуючих
моментів, прикладених до ланок з боку електроприводів.

Хід роботи
На підставі викладеної в [4] методики в середовищі Mathcad склада-
ємо обчислювальну програму, де задаємо початкові дані: кути повороту
ланок, кутові швидкості, кутові прискорення, маси ланок, довжини і радіу-
си ланок (рис. 5.1).
N  25 номер варіанту
Початкові дані:

 0   0   0   0   0   0   0 
1.2  0.008 N  0.2  0.0  100  1  0.1 
q   q1   q1   q2   m   L   R  
 0.8   0.012 N   0.3   0.06   110   1.3   0.04 
 0.9   0.016 N   0.4   0.0   120   1.2   0.04 
             
Рис. 5.1. Початкові дані для задачі динаміки маніпулятора

Задаємо функцію "слід матриці", прискорення вільного падіння, ви-

значаємо матриці інерції ланок і матриці перетворення систем координат
ланок (рис. 5.2).

25
 Функція "слід матриці":
cols( A)  1
Tr( A )   A
i i
g  ( 0 0 9.8 0 )
i 0

n  rows ( q )  1 i  2  n

Матриці інерції ланок:

 1 m  L 2 0 0 m L   1 m  R 2 
 3 i  i i i  2 1  1 0 0 0
  0 0 0 0

    0 0 0 0
J   0 0 0 0  J   0 0 0 0  O  
i 1 0 0 0 0
 0 0 0 0   0 0 0 0 
0 
 m L 0 0 m   0 0 0 m   0 0 0
 i i i   1
T
  
i  1  n   q    0
 2
0 0


a  0 0 L
2
L
3 T 
d  0 L
1
0 0 T
Матрицi перетворення для сміжних ланок маніпулятора:

 cos  i cos  i  sin   i sin   i  sin  i ai cos   i 

   1 0 0 0

 sin  i cos   i  cos  i sin  i cos   i ai sin  i  0 1 0 0
A1    A1  
i 0 0 0 1 0
 0 sin  i cos   i d 

i
 0 
 0 0 1
 0 0 0 1 
Матриці перетворення для систем координат ланок з номерами k i p:
p
A ( k  p )   A1
j
j  k 1

Рис. 5.2. Визначення матриць інерції та перетворення

Визначаємо першу та другу похідні від матриць перетворення
(рис. 5.3).

26
 Матриця-похідна:

 0 1 0 0 

1 0 0 0
Q  
0 0 0 0
0 
 0 0 0

Координати центрів мас ланок:

 0   L2   L3 
 L     
 1   2   2 
r   2  r2   0  r3   0 
1
 0   0   0 
     
 0   0   0 
Похідна від матриці перетворення за приєднаною координатою:

U  for i  1  n
for j  1  n
v  A ( 0  j  1)  Q A ( j  1  i) if j  i
i j
v  O otherwise
i j
v

Друга похідна:

U1( s )  for j  1  n
for k  1  n
v  A ( 0  k  1)  Q A ( k  1  s  1)  Q A ( s  1  j) if j  s  s  k
j k
v  A ( 0  s  1)  Q A ( s  1  k  1)  Q A ( k  1  j) if j  k  k  s
j k
v

Рис. 5.3. Визначення похідних матриць перетворення

Визначаємо керуючі моменти як суму трьох складових: обумовленої

прискоренням лянки, обумовленої відцентровими та коріолісовими силами
і обумовленою силою ваги (рис. 5.4).
В результаті виконання програми отримуємо значення трьох керую-
чих моментів, що діють відповідно на першу, другу і третю ланки маніпу-
лятора.

27
Складова керуючого моменту, обумовлена прискоренням ланки:
D  for i  1  n
for k  1  n
D 0
i k
for j  max( i  k)  n
T
D D  Tr U J U
i k i k  j k j j i 
D

Складова керуючого моменту, обумовлена відцентровими та коріолісовими силами:

n n n 
 T
h     Tr U1( s )  J  U  q1  q1
i   j k j j i   k s
k  1 s  1  j  max( i  k  s) 
Складова керуючого моменту, обумовлена вагою ланки:
n
C 
i  mj gUi  j rj
ji

 0 
  14
  0   0 
C  2.796 10  173.443 65.64 
 488.182  h D q2  
 20.891   111.836
 
 702.075   6.16   48.524 
   
Результуючий момент:
M  D q2  h  C

 0 
239.083
M
 620.909
 756.759
 
Рис. 5.4. Визначення керуючих моментів

Зміст звіту
1. Тема та мета роботи.
2. Завдання.
3. Текст програми.
4. Висновки.
Контрольні питання
1. У чому полягає задача динаміки маніпулятора?
2. Що задається у початкових даних?
3. Яким чином розраховуються керуючі моменти?
4. Які складові моментів сили враховуються?

28
 ЛІТЕРАТУРА

1. Бурдаков, С. Ф. Проектирование манипуляторов промышленных

роботов и роботизированных комплексов: Учеб. пособие [Текст] /
С. Ф. Бурдаков, В. А. Дьяченко, А. Н. Тимофеев. – М.: Высш. шк., 1986. –
264 с.
2. Зенкевич, С. Л. Основы управления манипуляционными робота-
ми: Учеб. для вузов [Текст] / А. С. Ющенко, С. Л. Зенкевич. – М.: Изд-во
МГТУ им. Баумана, 2004. – 480 с.
3. Крутько П. Д. Управление исполнительными системами роботов
[Текст] / П. Д. Крутько. – М.: Наука, 1991. – 336 с.
4. Фу, К. Робототехника: Пер. с англ. [Текст] / К. Фу, Р. Гонсалес,
К. Ли. – М.: Мир, 1989. – 624 с.
5. Шахинпур М. Курс робототехники: Пер. с англ. [Текст] / М. Ша-
хинпур. – М.: Мир., 1990. – 527 с.

Зміст
Вступ ............................................................................................................... 3
Лабораторна робота №1. Матриці повороту ................................................ 4
Лабораторна робота №2. Пряма задача кінематики ..................................... 7
Лабораторна робота №3. Зворотна задача кінематики ................................ 12
Лабораторна робота №4. Планування траєкторії маніпулятора .................. 18
Лабораторна робота №5. Динаміка маніпулятора ........................................ 24
Література ....................................................................................................... 28

29