CC BY
42
УДК 621.865
ПЛАНИРОВАНИЕ ТРАЕКТОРИИ ПЕРЕМЕЩЕНИЯ МАНИПУЛЯТОРА С ПОДВЕСОМ СХВАТА НА ГИБКИХ ЗВЕНЬЯХ (ЧАСТЬ 1)
© 2011 г. Ю.А. Валюкевич, А.В. Алепко
Южно-Российский государственный университет South-Russian State University
экономики и сервиса, г. Шахты of the Economy and Service, Shahty
В первой части работы поставлены и решены прямая и обратная задачи кинематики по положению и скорости для манипулятора с подвесом схвата на гибких звеньях. Сформулирована в общем виде задача планирования траектории перемещения схвата манипулятора. Приведено решение кинематической задачи по положению для зоны обслуживания в форме трехмерного четырехгранника и параллелепипеда, для скорости - в форме параллелепипеда.
Ключевые слова: манипулятор; тросовая система; кинематика; интерполяция; гибкие связи.
In the first part of the article set and solved direct and inverse kinematics of the ticks for the position and velocity for the robot gripper with the suspension on the flexible links. Formulated in general terms the task of planning the trajectory of movement of the gripper arm. Received solution of the kinematics task on the position of the zone-servicing in the form of three-dimensional tetrahedron and the box, to speed - in the form of the box.
Keywords: crane; cable system; kinematics; interpolation; flexible communication.
В работах [1, 2] предложена конструкция манипулятора с подвесом схвата на гибких звеньях. Интерес к этой проблеме обозначен наличием достаточно большого количества патентов на кинематические схемы в патентных агентствах США, Китая, Японии и некоторых других стран. В США, начиная с 80-х годов прошлого века, по заказу министерства обороны ведутся разработки так называемого «Робокрана», представляющего собой разновидность платформы Стюарта, построенной с использованием гибких связей. Однако достаточно исследованный «Робокран» не предназначен для перемещения грузов по заданной траектории, а служит лишь для ориентации груза в пространстве. Известные конструкции предназначены для перемещения относительно малых грузов и/или обладают весьма ограниченной зоной обслуживания. Кроме того, пространство зоны обслуживания представленных манипуляторов ограничено простыми фигурами, такими как треугольная призма или параллелепипед [2, 3]. Настоящая работа посвящена созданию манипулятора с подвесом схвата на гибких звеньях, имеющего зону обслуживания сложной формы, большого объема, с возможностью перемещения объектов большой массы. Одной из основных задач при создании подобного манипулятора является планирование траектории перемещения схвата.
Планирование траектории перемещения задано набором условий:
- имеется область пространства, ограниченная: горизонтальной плоскостью XOY (рис. 1); плоскостями, проходящими через смежные перпендикуляры, восстановленные из точек А(х0, у0, z0), В(х1, у1, z1), С(х2, у2, z2), Б(х3, у3, z3) пространства к плоскости XOY; одной из плоскостей, заданной любыми 3 точками из возможного набора А, В, С, Б;
- координаты точек в декартовой системе координат А, В, С, Б известны и в общем случае связаны системой неравенств:
х0 Ф х1 Ф х2 Ф х3; • у0 Ф у1 Ф у2 Ф у3; (1)
z0 Ф z1 Ф z2 Ф z3 Ф 0;
- в точках А, В, С, Б подвижно закреплены нерастяжимые нити, причем их свободные концы связаны между собой в точке М, которая является точкой крепления груза массой т;
- изменение декартовых координат точки М в заданной области пространства достигается за счет изменения длин нитей L0^L3 между точками А, В, С, Б и точкой Мсоответственно.
Требуется определить алгоритм управления длинами нитей L0^L3 для перемещения точки М в пределах определенной области пространства из текущего положения в некоторое заданное координатами х3, у3, zз. Перемещение в заданную точку возможно по заданной траектории, либо в режиме позиционирования с независимым изменением длин нитей L0^L3.
Примем длины L0^L3 как обобщенные координаты механизма и определим выражения для прямой и обратной задач о положении.
Уравнения для обратной задачи о положении для указанной кинематической схемы механизма представлены как расстояния от точки М с текущими координатами х, у, z (1) до вершин четырехугольника А, В, С, Б:
L0 = ТсхО^ХоЧСуО^у^ + СгО^Т)2;
" I-
L2 = -у/(х2 - х)2 + (у2 - у)2 + (г2 - z)2;
L3 = г)2
Уравнение прямой задачи о положении механизма может быть получено как решение системы уравнений (2) относительно декартовых координат х, у, г. Для определения уравнений прямой задачи о положении в нашем случае достаточно любых трех уравнений сис-
темы (2). Переместим начало координат в точку А, сохранив ориентацию осей системы и запишем первые три уравнения системы 2 в виде
L02 — х2 + y2 + z2;
L12 = (х1 - х)2 + (y1 - y)2 + (z1 - z)2;
L22 = (х2 - х)2 + (y2 - y)2 + (z2 - z)2.
(3)
Преобразуя систему (3), получим выражения для прямой задачи о положении в виде
К
х1 О —
2av
к
4a 2
-L012;
y1,2 — Cy 0
х
2ах
Л
--1012
4а2
-Cyi(Cy2L02 + Cy3L12 + Cy4L22 -1;
(4)
— Czo(-± 2a
- L012) -
4a2
тическое описание прямой и обратной задач о положении существенно упрощаются. Имея в виду, что для этого случая в принятой системе координат х0 = х1=0, х2 = х3, у0 = уЗ = 0, у1 = у2, z0 = z1 = z2 = z3 = 0; систему уравнений (2) для обратной задачи о положении можно представить в виде
10 = у[х
11 = 4~х
¡х2 + y2 + z2; /х2 + (y1 - y)2 + z2; L2 — х2 - х )2 + (y2 - y) L3 — \j(х3 - х)2 + y2 +;
22 2 + z 2 ;
(5)
На основании системы уравнений (5) прямая задача о положении для зоны обслуживания, имеющей форму параллелепипеда, может быть представлена как
-сг1(сг2Ы02 + сгзЫ12 + сг4Ы22 -1, где 1012 = d0104 + d1 Ы14 + d2 124 + d3 Ы12Ы02 + d4 12102 + + d5Ы12Ы22- d6 Ы02, Вх = Сх0 Ы02 + сх1 Ы12 + Сх2 Ы22, ах, с„, су, сdi - константы, рассчитанные на основании декартовых координат вершин А, В, С (рис. 1), I = 0, \...т - индексная переменная.
Численное решение систем (3), (4) в режиме реального времени, при современном уровне развития средств вычислительной техники, особых затруднений не представляет. При данном выборе системы координат выбирается отрицательное значение корня по координате z и соответствующие ему значения по координатам х и у. Вторые значения из каждой пары корней однозначно выходят за зону обслуживания и могут быть исключены из рассмотрения.
Для большинства практических приложений зона обслуживания манипулятора может быть представлена в виде параллелепипеда (рис. 2). При этом матема-
А z
х — -
х22 + L12 - L22
y —
2х2
y22 + L02 - L12 ; 2 y1 ;
(6)
L02 -
ry12 + L02 - L12 ^ 2y1
Гх22 + L12 - L22 ^
2х2
При анализе свойств представленного манипулятора требуется также решение прямой и обратной задач о скорости. Прямая задача о скорости в рассматриваемом случае состоит в определении вектора скорости точки М в декартовой системе координат по заданным обобщенным координатам звеньев:
dx _ дх dЫ0 дх dЫ1 дх dЫ2 dt дЫ0 dt дЫ1 dt дЫ2 dt '
dy _ dy dL0 dy dL1 dy dL2
dt dL0 dt dL1 dt dL2 dt '
dz _ dz dL0 dz dL1 dz dL2
dt dL0 dt dL1 dt dL2 dt '
С(х2, y2, z2)
(7)
А(х0, y0, z0
Рис. 1. Зона обслуживания манипулятора
b
z
,2
z =
о
^ тdL
Или в матричной форме — = J —, где
dt dt
& (dx dy dz Л
— = 1 —, —,— I - вектор-столбец декартовых ско-dt ^ dt dt dt)
,, dL (dL0 dL1 dL2 Л
ростей точки М; — = 1-,-,-I - вектор-
dt ^ dt dt dt )
столбец обобщенных скоростей, J - матрица Якоби размерности 3*3, каждый элемент матрицы является соответствующей частной производной системы (7):
о Li _ L2
x2 x2
J -
L0 о -Li yl yi
к
"vx К
к к
где K -J 4L02 -
(yl2 + L02 - Li2
)2 (x22 + Li2 - L22 )2
yl2
x22
f
kvx - 4
8L0 -
4(yl2 + L02 - Li2)L0^
yi
( а/,л2 , 7-a2 ri2
Kvy - 4
2
4(yl2 + L02 -Li2)Li 4(xi2 + Li2 - L22)Li
yi
К -(
x22
-(x22 + Li2 - L22 )L2 .
Решая систему (6) относительно проекций скорости перемещения точки М в декартовых координатах, получим
L1 L2
vx - — vi--v2;
x2 x2
L0 LI vy - — v0--v2;
y yi yi
vz - KvXv0 + Kvyvi + "^v2.
(8)
Здесь ух, vy, vz - проекции вектора скорости точки М на оси декартовой системы координат, соответственно v0, V!, v2 - линейные скорости нитей.
Обратная задача по скорости может быть получена путем решения системы (8) относительно v0, v1, v2:
v0 -
2x2 ( LT + £ ) vx + f (^ + " >vy '
L0 L0 Kvx + Li "vy + L2
, Li (LO x2 yi ^ x2 vi -— —v0--v„--v„--v„;
L2^L2 L2 x L2 y) Li x'
L0 x2 yi
v2 - — v0--vx--vy.
L2 L2 x L2 y
Для определения скорости по четвертой обобщенной координате необходимо любой из столбцов мат-
д т dL3
рицы J заменить на- и матрицы L на -.
дL3 dt
v
v
v
к
v
Рис. 2. Зона обслуживания манипулятора в форме параллелепипеда
При этом система (6) должна быть определена на основе четвертого и любых двух из трех других уравнений системы (5).
При решении практических задач планирования прямая кинематическая задача о скорости используется относительно редко. Значительно чаще используется обратная задача, в связи с этим с целью получения более простых выражений обратной задачи можно воспользоваться соотношением
Зенитные углы:
Vi = Vxi + Vyi + V z
(9)
a 0 = arctg I ^^ I, a1 = arctg
Утр - у i.
a2 = arctg
У2 - Утр x2 X'yn
a3 = arctg
Утр
Pi = arccos
fz л
ZTP V Li /
Здесь vi - вектор линейной скорости по обобщённой координате i (i = 0...3); vxi, vyi, vzi - проекции векторов скорости точки М на вектор линейной скорости в принятой системе декартовых координат (рис. 2).
Проекции этих векторов достаточно просто получить, используя азимутальный и зенитный углы отрезков прямых L0^L3. На рис. 2 эти углы показаны для отрезка L2 как (а2, р2). Определение этих углов на этапе решения прямой и обратной задач кинематики целесообразно в связи с последующим решением задач статики и динамики управления манипулятором. С использованием азимутальных и зенитных углов обобщенных координат решение обратной задачи по скорости (9) для каждой точки перемещения по заданной траектории может быть представлено в виде
vi = (sgn (vx) vx cos (аг) + sgn (vy)vyi sin (ai ))x
x sin (рг) + sgn (vzi) vzi cos (рг), (10)
где vi - линейная скорость перемещения по обобщенной координате i; ai - азимутальный угол нити; p, -
зенитный угол нити; sgn (vxi) ,sgn (vyi) ,sgn (vzi) -
знак соответствующей проекции скорости относительно обобщенных координат; i - индексная переменная (i = 0, 1, 2, 3).
Ортогональные скорости определяются исходя из заданной технологической скорости перемещения схвата (10) способом, который будет описан во второй части статьи.
Азимутальные углы определены из соотношений:
Здесь хТР, уТР, zTP - декартовы координаты произвольной точки на траектории перемещения.
Для единообразия принято, что увеличению обобщенной координаты соответствует знак плюс ее скорости и, соответственно, вектора линейных скоростей обобщенных координат направлены по осям нитей к точке М. Определение знака проекции скорости перемещения точки М в декартовой системе координат может быть осуществлено из логических соотношений:
sgn ( ) = sgn ( vx )© sgn (vii );
I Sgn (v,i ) = Sgn (^ ) © Sgn (V2i ).
(JJ)
Здесь sgn(vx), sgn(vy) - знаки проекций скоростей точки М в декартовой системе координат; sgn(vlI), sgn(v2¿) - знаки проекций скоростей по обобщенным координатам на декартовы координаты х и у соответственно, определяемые для каждой из обобщенных координат как элемент бинарной матрицы (11)
sgn (Vji ) 0 0 11
sgn (V2i ) 0 110
Здесь принята стандартная кодировка «лог 0» соответствует знаку «плюс», «лог 1» - знаку «минус».
Значение sgn(vzi) - определяется одинаково для
всех обобщенных координат sgn (Vzi) = sgn (Vz).
Используя полученные выражения для прямой и обратной кинематических задач по скорости и положению можно синтезировать алгоритмы планирования, которые будут рассмотрены во второй части работы.
Литература
1. Пат. № 2372274 Российская Федерация, МПК B66C 21/00. Устройство перемещения грузов.
2. Jason J. Gorman, Kathryn W. Jablokow, David J. Cannon. The cable array robot: Theory and experiment // In Proceedings of the International Conference on Robotics and Automation. Seoul, Korea, May 2001. Р. 2804-2810.
3. Albus J., Bostelman R., Dagalakis N. The nist Robocrane // Journal of Robotic Systems. 1993. Vol. 10. P. 709 - 724.
x
Поступила в редакцию 29 сентября 2011 г.
Валюкевич Юрий Анатольевич - канд. техн. наук, профессор, кафедра «Радиоэлектронные системы», ЮжноРоссийский государственный университет экономики и сервиса. Тел. (9J8)5JJ-56-52. Email: [email protected]
Алепко Андрей Владимирович - ассистент, кафедра «Радиоэлектронные системы», Южно-Российский государственный университет экономики и сервиса. Тел. (951)500-94-63. Email: [email protected]
Valyukevich Yuriy Anatolievich - Candidate of Technical Sciences, assistant professor, department «Radio-Electronic Systems», South-Russian State University of the Economy and Service. Ph. (9J8)5J J-56-52. Email: [email protected]
Andrey Alepko Vladimirovich - assistant, department «Radio-Electronic Systems», South-Russian State University of the Economy and Service. Ph. (951)500-94-63. Email: [email protected]_
