OST ´05

OST ´05
DEVELOPMENT OF KINEMATICS ALGORITHMS OF ROBOTIC
MANIPULATORS
Gennady Aryassov, Dmitri Meder and Jevgeni Zinovjev
Abstract
The purpose of this paper is to develop algorithms of forward and inverse kinematics of
articulate manipulators for various designs. The forward kinematics analysis with the help of
transformation matrices is determined. For the inverse kinematics analysis two variants of a
geometric method are elaborated. In the first case a set of kinematics inverse equations in case
of three degree of freedom robot are derived. The kinematics parameters of the links by
approximate method are obtained. For the inverse kinematics solution of robots in the second
case a next procedure are used. The initial construction of the robots is imaged with two-link
model for which extreme permitted positions of the links are determinated. Then the concrete
configuration proceeding from external conditions and internal structure of the robots are
selected. In order to drive the robot to the desired position path of an end effector is divided
on equal parts of length. Is estimated that a move time don’t depend on the number of
approximation parts of the path. On the basis of the received algorithms programs in
MATLAB are composed for calculating on a computer. These programs can be used for
design of new robotic manipulators.
Keywords: Robotic Manipulator, Kinematics Analysis, Algorithm
1
Introduction
Кинематический анализ структуры манипулятора касается описания движения
манипулятора относительно неподвижной декартовой системы координат, игнорируя
силы и моменты, которые вызывают движение структуры. Важно проводить различия
между кинематикой и дифференциальной кинематикой. Кинематика описывает
аналитические отношения между положениями соединений и положением
исполнительного элемента конца, и его ориентацией. Дифференциальной кинематикой
описывают аналитические отношения между движением соединений и движением
исполнительного элемента конца на основе скоростей и ускорений. [4], [10], [11]
Формулировка отношений кинематики позволяет изучать две ключевых задачи
робототехники: прямую задачу и обратную задачу кинематики. Раньше разработчики
беспокоились об определение систематического, общего метода, чтобы описать
движение исполнительного элемента конца как функция объединенного движения
посредством линейных инструментов алгебры. Сейчас разработчики заинтересованы в
обратной задаче, ее решение имеет фундаментальное значение, чтобы преобразовать
желательное движение конца исполнительный элемент в рабочем пространстве в
соответствующее объединенное движение. [4], [5], [11].
The inverse problem of kinematics has a lot of solutions. It is important to mention, that
the number of solutions increases, if the number of manipulator links increase. This
1
means that if a robot has seven degrees of freedom, there are an infinite number of ways
it can position a part and orientate it at the desired location. For the controller to know
what to do there must be some additional decision making routine that allows it to pick
only one of the infinite ways. As an example, one may use an optimization routine to
pick the fastest or shortest path to the desired destination. Then computer has to check
all solutions to find the shortest or fastest response and perform it. Due to this
additional requirement, which can take much computing power and time, no sevendegree-of-freedom robot used in industry?
Наличие кинематической модели манипулятора полезно также для обнаружения связей
между силами и моментами вращения приложенных к узлам соединений, и нагрузок
приложенных к исполнительному элементу конца в статическом равновесии.
Кинематика
манипулятора
представляет
основополагающий
принцип
систематического, общего начала его динамики, то есть, уравнение движения
манипулятора как функция сил и моментов, действующих на него во времени. Наличие
динамической модели очень полезно для механического проектирования структуры,
выбора силовых приводов, определение стратегий контроля, и компьютерного
моделирования движения манипулятора.
On the basis of the obtained algorithms the programs in MATLAB are composed. These
programs can be used for the design of new robotic manipulators and for the direct and
inverse kinematics analysis of present-day robots.
Целью работы было изучение структуры манипуляторов и их кинематический анализ.
При кинематическом анализе манипуляторов авторами была выдвинута идея
упрощения вычислительных задач, путем использования алгоритмов написанных в
программе MATLAB. В качестве примеров были взяты манипуляторы с тремя и
шестью степенями подвижности.
The kinematics parameters of the links (positions, velocities and accelerations) are
obtained by approximate method. The two-link model has been used to describe the
initial construction of the robot, whereas the extreme permitted positions of the links
have been determined. Then the concrete chosen configuration depends on the external
conditions and the internal structure of the robot. In order to drive the robot to the
desired position the trajectory of an end effector is divided on equal parts of length. It is
estimated, that a time of movement don’t depend on the number of approximation parts
of the path. These programs can be used for the design of new robotic manipulators and
for the direct and inverse kinematics analysis of present-day robots.
Очень часто при проектировании манипуляторов, когда известны только требуемые
показатели технологического процесса и производительности, требуется выбрать
оптимальные длины, количество звеньев, и скоростные характеристики приводов, а
также при модификации манипулятора часто могут возникать следующие проблемы:
- Требуется анализ для выявления альтернативных решений, возможно более дешевых;
- Требуется анализ для определения изменений скоростных характеристик и
маневренности;
- Появляется необходимость модифицировать программу управления манипулятором;
После анализа рынка существующих программ было выделено несколько из них, но к
сожалению они оказались дорогими или очень трудными в освоении, требующие
предварительной подготовки.
Поэтому возникла идея создать программу, позволяющую легко и гибко анализировать
скоростные характеристики манипуляторов с различными конфигурациями. Программа
должна имеет возможность, изменяя угловую скорость, длину и количество звеньев,
анализировать время и эффективность при различных траекториях движения. При этом
2
конкретная конфигурация системы выбирается исходя из внешних условий и
внутренней структуры манипулятора. Программа была применена к конкретной модели
манипулятора S-500iB™ (фирмы FANUC Robotics) [16].
Для решения обратной задачи использовался способ, при котором определяются крайне
допустимые положения звеньев, полученных путем замены исходной системы
двухзвенной моделью.
Данный алгоритм позволяет обеспечивать достаточную точность движения схвата по
заданной траектории, в то время как затраченное время не зависит от количества точек
аппроксимирующих данную траекторию.
При написании алгоритма программы ставились следующие задачи:
- Алгоритм не должен быть зависим от количества звеньев
- Алгоритм не должен быть зависим от траектории конечной точки
- Движение манипулятора должно происходить плавно и непрерывно
- Время движения должно быть минимальным
- Визуализация расчетных данных (изображение схематической трехмерной модели
манипулятора)
- Иметь возможность усовершенствования, доработки алгоритма и внедрения новых
функций в будущем.
Данная работа помогает понять кинематическую задачу манипулятора и может быть
использована для общего проектирования модели манипулятора.
2
Forward kinematics analysis
Сформулируем прямую задачу кинематики в робототехнике:
По заданному n-мерному вектору изменяемых параметров сочленений, таких как углы
или поступательные перемещения получить соответствующую матрицу положения,
связанную с положением и ориентацией схвата робота.
2.1 Пример преобразования координат
Задана схема механизма манипулятора с 3-мя степенями подвижности (Рисунок 2.12).
В качестве трех обобщенных координат заданы углы  10 ,  21 ,  32 . Эти углы задаются
как функции от времени. Заданы длины звеньев AB  l1 , BC  l2 и координаты
некоторой точки E3 на звене 3: x E 3 , y E 3 . Требуется найти траекторию точки
E3 относительно стойки.
3
Рисунок 1 - Схема механизма манипулятора с 3-мя степенями подвижности
Введем в рассмотрение точки Е2, Е1, Е0 которые в данный момент времени совпадают
с точкой Е3, но принадлежат соответственно звеньям 2, 1, 0 (стойка).
Найдем положение точки Е3, относительно системы координат 2-го звена (Рис. 2).
Рисунок 2 – Положение точки Е3 в системе координат 2-го сочленения
На основании уравнений преобразования плоских декартовых координат получаем:
X E 2  l1  X E 3  cos  32  YE 3  sin  32
(1)
YE 2  X E 3  sin  32  YE 3  cos  32
Найдем положение точки Е2, относительно системы координат 1-го звена (Рис. 3).
Рисунок 3 Положение точки Е2 в системе координат 1-го сочленения
На основании уравнений преобразования плоских декартовых координат получаем:
X E1  l1  X E 2  cos  21  YE 2  sin  21
(2)
YE1  X E 2  sin  21  YE 2  cos  21
Найдем положение точки Е1, относительно системы координат 0-го звена (Рис. 4).
Рисунок 4 Положение точки Е1 в системе координат 0-го сочленения
На основании уравнений преобразования плоских декартовых координат получаем:
X E 0  X E1  cos 10  YE1  sin 10
YE 0  X E1  sin 10  YE1  cos 10
(3)
Решение системы шести линейных уравнений с шестью неизвестными дает
возможность найти по точкам искомую траекторию точки Е3, т.е. положение точки Е0.
С целью установления определенных правил вычисления и сокращения записи
используют матричную форму записи уравнения преобразования, как уже
рассматривалось в главах ранее.
Коэффициенты уравнений (1-3), соответствующие повороту осей и переносу начала
координат, дают матрицу порядка (2х3). Чтобы иметь дело только с квадратными
матрицами, которые можно умножать, добавим к каждым двум уравнениям
преобразования координат третье уравнение в виде тождества 1  1 .
Тогда коэффициенты правых частей уравнений (1) с добавлением тождества 1  1
образуют квадратную матрицу третьего порядка преобразования координат звена 3 в
координаты X 2 O2Y2 . [10]
cos  32
 sin  32
l2
(4)
M 32  sin  32 cos  32 0 ,
0
0
1
Аналогично коэффициенты правых частей уравнений (2-3), с добавлением тождества
1  1 , дают матрицы:
cos  21  sin  21 l1
cos 10  sin 10 0
(5)
M 21  sin  21 cos  21 0 , M 10  sin 10 cos 10 0 .
0
0
1
0
0
1
4
Левые части уравнений (2-3) с добавлением тождества 1  1 дают столбцовые матрицы
третьего порядка.
X E2 
 X E0 
 X E1 







(6)
rE 2   YE 2  , rE1   YE1  , rE 0   YE 0  .
 1 
 1 
 1 

Аналогично выражениям (6) получаем rE 3 :
 X E3 

(7)
rE 3   YE 3  .
 1 
Теперь уравнения (1-3) с добавлением тождества 1  1 можно записать в виде:


(8)
rE 2  M 32  rE 3 ,


(9)
rE1  M 21  rE 2 ,


rE 0  M 10  rE1 .
(10)
Подстановка матричных уравнений (8-9) в уравнение (10) позволяет определить
искомые координаты x E 0 , y E 0 :


rE 0  M 10  M 21  M 32  rE 3 .
(11)
Для дальнейшего вычисления положения движения звеньев можно воспользоваться
программой MATLAB (Приложение 1).
Рисунок 5 Графики движения схвата манипулятора
В данном алгоритме (код программы – Приложение 1), при решении прямой задачи
кинематики, используется метод преобразования координат. Далее представлена блоксхема алгоритма (Рисунок 4.2), в которой реализован этот метод.
Рис. 6
Блок-схема алгоритма для решения прямой задачи кинематики
Этот алгоритм рассчитан на любое количество звеньев, т.е. если будет задано п.
звеньев, то программа автоматически сделает расчет позиций для n количества. На
рисунке 7 показан результат работы программы (в среде MATLAB). Здесь показано 30
положений 3–х звенного манипулятора, с последовательно изменяющимися углами
поворота.
Рис. 7
Результат расчета прямой задачи кинематики
В дальнейшем этот алгоритм будет использоваться для вывода результатов расчета
программы, решающей обратную задачу.
3
Inverse kinematics analysis
Нужно по заданному положению или траектории движения схвата манипулятора,
определить присоединенные параметры манипулятора, обеспечивающие заданное
положение схвата [4], [8], [11].
2.1 Первый способ решения обратной задачи кинематики
5
Возьмем для примера 3-х звенный механизм манипулятора (Рисунок 8). Договоримся
сразу, о направлениях отсчета углов 1 ,  2 ,  3 (Рисунок 8).
Захват (точка С) движется согласно некоторому уравнению:
X C (t )  a0  b0 t 
.
YC (t )  d 0  c0 t 
(12)
Рисунок 8 3-х звенный механизм манипулятора
В начальный момент времени (t=0):
t2
,
2
1   0   0 t   0
(13)
1  0   0 t ,(2.81)
1   0 .(2.82)
Вычислим значения углов 1 ,  2 ,  3 , в разные промежутки времени. Определим
угловые скорости 1 ,  2 ,  3 и угловые ускорения 1 , 2 , 3 .[2], [7], [9], [10]
Для любого положения механизма справедливы векторные соотношения (Рисунок
9).
Рисунок 9 – Векторное положение звеньев манипулятора
___
___
___
OB  OA AB ,
___
___
___
(14)
___
___
___
OC  OB  BC  OA AB BC .
(15)
Эти равенства являются уравнениями связей, наложенных на систему. Спроецируем их
на оси координат:
 X B  OA cos 1  AB cos  2

 YB  OA sin 1  AB sin  2
(16)
 X C  OA cos 1  AB cos  2  BC cos  3

 YC  OA sin 1  AB sin  2  BC sin  3
(17)
___
OC  const ;
___
где
AB  const ;
___
BC  const .
6
Продифференцируем эти системы дважды по времени. В результате первого
дифференцирования получим алгебраические уравнения относительно неизвестных
угловых скоростей 1 ,  2 ,  3 :
 X B  OA sin 11  AB sin  2 2
 
 YB  OA cos 11  AB cos  2 2
(18)
 X C  OA sin 11  AB sin  2 2  BC cos  3 3
 
 YC  OA cos 11  AB cos  2 2  BC cos  3 3
(19)
После второго дифференцирования получим две системы алгебраических уравнений,
связывающих угловые ускорения 1 , 2 , 3 :
 X B (0)  OA cos 1 (0)(1 (0)) 2  AB cos  2 (0)( 2 (0)) 2  OA sin 1 (0)1 (0)  AB sin  2 (0)2 (0)
 
2
2
 YB (0)  OA sin 1 (0)(1 (0))  AB sin  2 (0)( 2 (0))  OA cos 1 (0)1 (0)  AB cos  2 (0)2 (0)
(2.89)
 X C  OA cos 1 (1 ) 2  AB cos  2 ( 2 ) 2  BC cos  3 ( 3 ) 2 
 
2
2
2
 YC  OA sin 1 (1 )  AB sin  2 ( 2 )  BC sin  3 ( 3 ) 
 OA sin 11  AB sin  22  BC sin  33
 OA cos 11  AB cos  22  BC cos  33
Величины X C , YC , X C , YC ,
движения схвата:
определяются
путем
(20)
дифференцирования
X C  b0 , X C  0
.
YC  c0 , YC  0 
уравнений
(21)
Для начального момента времени t  t 0  0 , получаем:
 b0  OA sin 1 (0)1 (0)  AB sin  2 (0) 2 (0)  BC sin  3 (0) 3 (0)
.

  c0  OA cos 1 (0)1 (0)  AB cos  2 (0) 2 (0)  BC cos  3 (0) 3 (0)
(22)
 X B (0)  OA sin 1 (0)1 (0)  AB sin  2 (0) 2 (0)
 
 YB (0)  OA cos 1 (0)1 (0)  AB cos  2 (0) 2 (0)
(23)
Для определения  2 (0) и  3 (0) воспользуемся уравнением (2.86)
a0  OA cos 1 (0)  AB cos  2 (0)  BC cos  3 (0)
,

 d 0  OA sin 1 (0)  AB sin  2 (0)  BC sin  3 (0)
Обозначим:
(24)
a0  OA cos 1 (0)  A0
d 0  OA sin 1 (0)  B0
 AB cos  2 (0)  BC cos  3 (0)  A0
.

 AB sin  2 (0)  BC sin  3 (0)  B0
(25)
Решаем систему уравнений (25), возводим в квадрат и складываем обе части системы
уравнений:
7




AB 2 cos 2  2 (0)  sin 2  2 (0)  BC 2 cos 2  3 (0)  sin 2  3 (0) 
 2 AB  BC cos  2 (0) cos  3 (0)  sin  2 (0) sin  3 (0)  A02  B02
cos 2 (0)   3 (0) 
A02  B02  ( AB) 2  ( BC ) 2
2 AB  BC
A02  B02  ( AB) 2  ( BC ) 2
Пусть Z 0 
2 AB  BC
 2 (0)   3 (0)  arccos( Z 0 )
Подставляя в выражение (25)
ABcos  2 (0) sin  3 (0)  sin  2 (0) cos  3 (0)  A0 sin  3 (0)  B0 cos  3 (0)
AB sin  2 (0)   3 (0) 
  arctg
A02  B02 sin (   3 (0)
B0
A0
AB sin arccos( Z 0 )  A02  B02 sin (  3 (0)
sin (   3 (0) 
 3 (0) 
AB sin arccos( Z 0 )
A02  B02
arcsin AB sin arccos( Z 0 )
A02  B02
 ,
(26)
 2 (0)  arccos( Z 0 )   3 (0) .
(27)
Теперь можно найти X B (0) и YB (0) зная,  2 (0) в системе уравнений (16).
Подставляя найденные значения  2 (0) и  3 (0) в уравнения (22-23) можно определить
производные  2 (0),  3 (0), X B (0), YB (0) :
 AB sin  2 (0) 2 (0)  BC sin  3 (0) 3 (0)  b0  OA sin 1 (0)1 (0)
,

 AB cos  2 (0) 2 (0)  BC cos  3 (0) 3 (0)  c0  OA cos 1 (0)1 (0)
(28)
Зная 2 (0), 3 (0) , найдем X B (0), YB (0) :
 X B (0)  OA sin 1 (0)1 (0)  AB sin  2 (0) 2 (0)
.
 
 YB (0)  OA cos 1 (0)1 (0)  AB cos  2 (0) 2 (0)
В дальнейшем решаем системы уравнений с помощью программы MATLAB.
Для начального t=0, уравнения (24-25) будут:
0  OA cos 1 (0)(1 (0)) 2  AB cos  2 (0)( 2 (0)) 2  BC cos  3 (0)( 3 (0)) 2 

2
2
2
 0  OA sin 1 (0)(1 (0))  AB sin  2 (0)( 2 (0))  BC sin  3 (0)( 3 (0)) 
 OA sin 1 (0)1 (0)  AB sin  2 (0)2 (0)  BC sin  3 (0)3 (0)
 OA cos 1 (0)1 (0)  AB cos  2 (0)2 (0)  BC cos  3 (0)3 (0)
8
(29)
В дальнейшем используем для расчета программу MATLAB (Приложение 4), откуда
находим 2 (0), 3 (0) .
 X B (0)  OA cos 1 (0)(1 (0)) 2  AB cos  2 (0)( 2 (0)) 2  OA sin 1 (0)1 (0)  AB sin  2 (0)2 (0)
 
2
2
 YB (0)  OA sin 1 (0)(1 (0))  AB sin  2 (0)( 2 (0))  OA cos 1 (0)1 (0)  AB cos  2 (0)2 (0)
Раскладывая в ряд Маклорена, получаем:
 2 (1)   2 (0)   2 (0)t  2 (0)
t 2
,
2
 3 (1)   3 (0)   3 (0)t  3 (0)
t 2
.
2
(30)
(31)
Учитывая что 1 (t ) заданное, мы можем получить 1 (1)
1 (1)   0   0 t 
t 2
2
, подставим в (17) и получим:
 AB sin  2 (1) 2 (1)  BC sin  3 (1) 3 (1)  b0  OA sin 1 (1)1 (1)

 AB cos  2 (1) 2 (1)  BC cos  3 (1) 3 (1)  c0  OA cos 1 (1)1 (1)
В дальнейшем используем для расчета программу MATLAB (Приложение 4), откуда
находим  2 (1),  3 (1) .
Аналогично при нахождении следующего приближения
2 (1), 3 (1) подставляем в выражения (18), затем (26) и т.д.
для
ускорений
Строим схему положения звеньев с помощью программы MATLAB (Приложение5).
Повторения циклов зависит от количества нужных измерений. Ниже приводятся
некоторые графики результатов вычислений.
Рисунок 10 График изменения углов во времени
Рисунок 11 График скоростей точки В
Рисунок 13 График ускорений точки В
Рисунок 14 Схема движения звеньев манипулятора
2.1 Второй способ решения обратной задачи кинематики
Для решения обратной задачи, был создан набор функций (приложения 3, 4, 5, 6) и
главная программа (приложение 2). Для лучшего понимания общего алгоритма
рассмотрим с начала функции, а потом их взаимодействие с главной программой.
9
Функция “of6”
Эта функция является фундаментальной частью всего алгоритма решения обратной
задачи. В этой функции производится расчет углов поворотов звеньев, которые потом
обрабатываются в главной программе, где и выбирается оптимальный вариант углов
поворотов звеньев.
Далее приводится блок-схема (рисунок 15), где описывается общий алгоритм этой
функции.
Рис. 15
Блок-схема функции “of6”
Из этой блок-схемы видно, что функция рассчитывает крайние возможные положения
звеньев, а также обнаруживает ошибки, при которых конечная расчетная точка
находится слишком далеко или слишком близко.
Рассмотрим подробнее эту функцию на пример расчета углов поворота для 3–х
звенного манипулятора OABC, где точка C меняет координаты. (Рисунок 16)
Рис. 16
Обратная задача: входные данные
При вызове функции “of6”, ей передаются следующие данные: массив данных о
звеньях манипулятора, координаты начальной и конечной точек, а также номер
рассчитываемого звена. После вызова функции в первый раз, она возвращает углы
поворота b1, b2 и b3. (Рисунок 17)
Рис. 17
Обратная задача: расчет первого звена
Далее уже главная программа на основе полученных данных от “of6” рассчитывает
угол c1.
c1  b1  (b 2  2 / 3)
(32)
Если в будущем при развитии алгоритма заменить эту формулу, более
интеллектуальным решением, таким как изменение параметров, в зависимости от
значения угла в предыдущей позиции, сделать зависимость от скорости и ускорения,
тогда мы можем добиться более эффективного движения всей системы в целом.
Если это не последнее звено, то программа повторяется и снова происходит вызов
функции “of6”, но только уже с измененными начальными координатами – z’ и x’
(Рисунок 18). Здесь происходит расчет для звеньев ABC.
Если звено оказывается последним (Рисунок 16), тогда это означает, что мы не можем
изменять углы, полученные от “of6”. Поэтому формула 32 пропускается, и для расчета
положений звеньев используются углы b3 и c2  b1  b2
Рис. 18
Обратная задача: расчет последних двух звеньев
Таким образом на этом этапе, мы получили углы поворота для всех звеньев во всех
расчетных точках. (Рисунок 19)
Рис. 19
Обратная задача: результат расчета
Функция “rotate_time”
Эта функция занимается расчетом минимального времени, необходимого для поворота
каждого звена, между расчетными точками. (Приложение 4).
Траектория движения рабочего органа манипулятора состоит из участков разгона и
торможения, а также участка, где перемещение происходит с постоянной скоростью.
Очевидно, что минимальное время перемещения будет достигнуто при максимально
возможных значениях скорости и ускорения, определяемых из совместной
механической характеристики манипулятора
10
В будущем, при развитии этого алгоритма, по данным этой функции мы можем увидеть
самые слабые места в нашем манипуляторе - самые медленные звенья, а также это
позволяет нам анализировать выбранную нами траекторию движения звеньев – может
возникнуть необходимость изменения траектории движения звеньев, для увеличения
общей скорости манипулятора.
Приведенная блок-схема (Рисунок 20) показывает, как производится определение типа
движения. В алгоритме предусмотрено три типа движения: равномерное, ускоренное и
ускоренное переходящее в равномерное (торможение рассматривается как мгновенное,
поэтому время торможения равняется нулю)
Рис. 20 Блок-схема функции “rotate_time”
Функция “rotate_d”
На основе полученных данных от функции “rotate_time”, программа выбирает
максимальное время, для каждой расчетной позиции, и с помощью функции “rotate_d”
(Приложение 5) рассчитывает скорости, и ускорения для всех звеньев для заданного
времени движения. Это сделано для непрерывного движения всех звеньев
манипулятора. Ниже приведена блок схема этой функции (Рисунок 21)
Рис. 21 Блок-схема функции “rotate_d”
Функция “rotate_d” похожа на функцию “rotate_time”, за одним исключением “rotate_d” рассчитывает скорость и ускорение от времени, а функция “rotate_time”
время от скорости и ускорения.
Функция “rotate_time”
Это заключительная функция (Приложение 6), которая возвращает расчетное
положение звеньев от времени движения. Эта функция используется для визуального
представления движения манипулятора, т.е. используя эту функцию, мы можем узнать
положение всех звеньев манипулятора в любое расчетное время. Блок-схема этой
функции показана на рисунке 422
Рис. 22 Блок-схема функции “ rotate_time ”
Программа моделирования движения робота-манипулятора
Программа моделирования движения робота-манипулятора представляет собой
расчетную схему для определения скоростей и ускорений звеньев манипулятора от
времени, для движения конечной точки (схвата) по заданной траектории.
На рисунке 23 приведена блок-схема программы для моделирования движения роботаманипулятора.
Рис. 23 Блок-схема главной программы
Все расчетные данные, требуемые для моделирования манипулятора, находятся в
одном массиве: data{k}(i,1:7), где
k – номер расчетной позиции
i – номер звена (1 – база, 2, 3, 4 ..i – звенья манипулятора)
1 – угол поворота (разница между двумя расчетными позициями)
2 – время, требуемое для полного движения манипулятора до расчетной позиции k
3 – время, за которое звено i совершает равноускоренное движение
4 – время, за которое звено i совершает равномерное движение
5 – начальная скорость движения для секции i в расчетной позиции k
6 – начальный угол для секции i в расчетной позиции k
7 – Флаг изменения направления движения
11







Заключение
Целью данной работы являлось изучение структуры манипуляторов, прямой и
обратной задачи, разработка алгоритма движения и проверка с характеристиками
существующего и модифицированного антропоморфного манипулятора.
В ходе выполнения заключительной работы были исследованы конструкции
манипуляторов, выявлены их преимущества и недостатки. А также разработан
алгоритм и написана программа для моделирования движения манипулятора, где
наглядно можно увидеть результаты решения прямой и обратной задач кинематики, и
произведен запуск программы с характеристиками существующего манипулятора.
Программа определяет значения угловых скоростей и ускорений звеньев в любой
момент времени. Результаты, полученные в ходе проведения кинематических расчетов
по разработанной модели, показали правильность ее составления.
Возможности алгоритма:
Алгоритм предоставляет возможность моделирования манипулятора с любым
количеством звеньев
Алгоритм позволяет пользователю вводить любую траекторию конечной точки (для
примера в программе заложено 6 готовых траекторий)
Движение манипулятора происходит плавно и непрерывно
Расчетное время движения минимальное
Визуализация расчетных данных (изображение трехмерной модели манипулятора)
Дальнейшее усовершенствование алгоритма и внедрение новых функций в будущем.
В приложениях приводятся вышеописанная программа и функции, написанные в
MATLAB.
Предполагающиеся доработки алгоритма, позволяющие реализовать следующие
возможности:
Расчет торможения, в данный момент время торможения равно 0 (мгновенное)
Задание траектории по времени
Обход препятствий, как в конечной точке, так и при возникновении препятствия
у какого-либо из звеньев
Возможность полной остановки некоторых звеньев, анализ структуры
(возможность блокировки некоторых звеньев) для большего быстродействия
манипулятора в целом
Возможность расчета всех позиций после каждой расчетной точки во время
движения для изменения траектории при запущенном роботе
За счет изменения траектории движения отдельных звеньев (медленных звеньев,
тормозящих всю систему) изменять быстродействие всей системы в целом
Возможность создания расчетной модели для звеньев с любой длиной (без
каких-либо ограничений). На данный момент длина звена не должна быть меньше
суммы длин последующих звеньев.
Поскольку в последнее время автоматизация производства носит весомый характер, эта
работа позволяет сократить вычислительные операции, уменьшить денежные затраты и
время на проектирование. В дальнейшем предполагается использование алгоритма для
проектирования динамических моделей манипуляторов.
12
References
[1] van Wezel W. and Jorna W.J., “Paradoxes in planning”, Engineering Applications of
Artificial Intelligence, Vol. 14, 2001, pp.269-286.
[2]
Angell I.O. and Straub B., “Rain-Dancing with Pseudo-Science”, Cognition,
Technology & Work, Vol. 1, 1999, pp.179-196.
[3]
Crisfield M.A., “Non-linear Finite Element Analysis of Solids and Structures. Volume
2: Advanced Topics”, John Wiley & Sons, Chichester, 1997.
[4]
Eppinger S.D. and Salminen V.K., “Patterns of product development interactions”,
Proceedings of ICED ’01, Vol. 1, Glasgow, 2001, pp.283-290.
Corresponding author:
Assoc. Professor Kjell Andersson
Department of Machine Design
School of Industrial Technology and Management
Royal Institute of Technology, KTH
SE – 100 44 Stockholm, Sweden
Tel: Int +46 8 790 6374
Fax: Int +46 8 20 22 87
E-mail: kan@md.kth.se
13
Z0
Y5 = Y6
6
Z6
Y3 = Y 4
α54
Z5
Z4
5
3
Z3
4
α32
2
α21
Y2
1
Y0
X0
Z0
Y5 = Y6
6
Z6
α54
Z5
Y3 = Y 4
Z4
5
3
Z3
α32
2
α21
4
Y2
1
Y0
X0
14
1. При повороте 6-го звена (схвата) относительно 5-го звена (запястья) вокруг оси Y
 X E 5   cos 65 0 sin  65  0 
Y    0
1
0  l6 
 E5  
 Z E 5   sin  65 0 cos 65  0 
(4.19)
2. При повороте 5-го звена (запястье) относительно 4-го звена (локоть) вокруг оси X
 X E 4  l5
Y    0
 E4  
 Z E 4   0
0
cos 54
sin  54
0  X E5 
 sin  54  YE 5 
cos 54   Z E 5 
(4.20)
3. При повороте 4-го звена (локоть) относительно 3-го звена (рука) вокруг оси Y
 X E 3   cos 43 0 sin  43   X E 4 
Y 3    0
1
0  YE 4 

 
 Z E 3   sin  43 0 cos 43   Z E 4 
(4.21)
4. При повороте 3-го звена (локоть) относительно 2-го звена (рука) вокруг оси Y
 X E 2  l3
Y    0
 E2  
 Z 2   0
0
cos 32
sin  32
0  X E3 
 sin  32  YE 3 
cos 32   Z E 3 
(4.22)
5. При повороте 2-го звена (локоть) относительно 1-го звена (рука) вокруг оси Y
 X E1  l 2
Y    0
 E1  
 Z E1   0
6.
0
0  X E2 
cos 21  sin  21  YE 2 
sin  21 cos 21   Z E 2 
(4.23)
При повороте 1-го звена относительно абсолютной системы координат
15
 X E 0  cos 10
Y    sin 
10
 E0  
 Z E 0   0
Обозначим
 sin 10
cos 10
0
0   X E1 
0 YE1  .
1  Z E1 
матрицы преобразования (поворота) и радиус-векторы соответственно
M 65
 cos 65 0 sin  65 
  0
1
0 
,
 sin  65 0 cos 65 
M 54
l5
  0
 0
0
cos 54
sin  54
(4.25.1)
0 
 sin  54  ,
cos 54 
(4.25.2)
 cos 43 0 sin  43 
M 43   0
1
0  ,
 sin  43 0 cos 43 
M 32
l3
  0
 0
l 2
M 21   0
 0
(4.24)
(4.25.3)
0 
 sin  32  ,
cos 32 
(4.25.4)
0
0 
cos 21  sin  21  ,
sin  21 cos 21 
(4.15.5)
0
cos 32
sin  32
16
M 10
cos 10
  sin 10
 0
 sin 10 0
cos 10 0 ,
0
1
(4.25.6)
0
 X E5 
 X E4 
 X E3 
 X E2 
 X E1 






rE 6  l6  , rE 5   YE 5  , rE 4   YE 4  , rE 3   YE 3  , r2   YE 2  , rE1   YE1  . (4.26)
 Z E 5 
 Z E 3 
 0 
 Z E 4 
 Z E 2 
 Z E1 
Таким образом, оканчательно, радиус-вектор в абсолютной системе координат в
матричной форме имеет вид,


rE 0  M  rE 6 .
(4.27)
где
M = M10 * M21 * M32 * M43 * M54 * M65
(4.28)
17
ЛИТЕРАТУРА
1. Фу К., Гонсалес Р., Ли К. Робототехника, Москва: Издательство «Мир», 1989, 622 с.
2. Шахинпур М., Курс робототехники, Москва: Издательство «Мир»,1990, 528 с.
18