Способ управления манипулятором промышленного робота Советский патент 1993 года по МПК B25J11/00 B25J9/16 

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

Цель изобретения состоит в повышении точности отработки закона движения схва- та и повышение надежности за счет возможности формирования расчетных силовых и скоростных характеристик отдельных приводов с учетом их кинематики, а также с учетом массово-геометрических характеристик всех подвижных звеньев при отработке требуемого закона движения схвата.

Поставленная цель достигается тем, что в манипуляторе предусматривается возможность получения для каждого из приводных двигателей чет.ырех величин в виде напряжения, два из которых изменяются прямопропорционально расчетной (номинальной) мощности Рр и расчетной (номинальной) частоте вращения двигателя uJp , a вторые два напряжения измеряются прямопропорционально действительной (измеренной) мощности приводного двигателя Рмех и действительной (измеренной) частоте вращения данного двигателя эти четыре сигнала попарно сравниваются и сигналом их рассогласования производится выравнивание действительных (измеренных) мощности и частоты вращения двигателя относительно расчетных (номинальных) их значений.

На фиг. 1 изображена схема манипулятора с тремя степенями подвижности; на фиг. 2 - схема сил и моментов, действующих на звено 3 манипулятора с деталью 6 (перемещаемым грузом), расположенной в схвате 4; на фиг. 3 и 4 - схемы сил и моментов, действующих на звено 2 и звено 1 соответственно; на фиг. 5 - схема, поясняющая выбранный закон изменения скорости схвата 4 манипулятора; на фиг. 6, 7 и 8 - законы изменения обобщенных скоростей, в соответствии с выбранным законом движения схвата 4 манипулятора вдоль заданной траел

С

00

СЛ К О Ю

ектории; на фиг. 9,10 и 11 -законы изменения управляющей силы Rynp23 и управляющих моментов каналах приводных двигателей (звена 2 манипулятора Мупр12, звена 1 Mynpoi, обеспечивающих относительные движения соответствующих подвижных звеньев в соответствии с выбранной траекторией схва- та 4 и закона изменения его скорости; на фиг. 12,13 и 14 - расчетные (номинальные) законы изменения мощностей трех приводных двигателей с учетом постоянных потерь (кривые

2); на фиг. 15 - блок-схема системы управления по мощности.

Манипулятор содержит подвижные звенья 1, 2 и 3. схват 4, установленный неподвижно на звене 3, станину 5, а также перемещаемую деталь 6.

Как известно, перемещение объекта в пространстве может быть реализовано тремя обобщенными координатами. В качестве обобщенных координат выбирают угловые или линейные перемещения звеньев. В любом сочетании вращательных и поступательных кинематических пар может быть реализовано перемещение тела в пространстве. Оно может быть реализовано и при наличии только 3-х поступательных или 3-х вращательных. Таким образом, если рассмотреть манипулятор со степенями подвижности 3, например 5, то выбирают 3 основные (любые, как указано выше), остальные (в данном случае 2) замораживают или используют для ориентации тела в пространстве или других целей.

Неподвижная система координат XoYoZo с началом в точке О, совпадающей с центром вращения кинематической пары, образованной звеном 1 со станиной 5, подвижная система координат XiYiZi связана со звеном 1 таким образом, что начало ее (точка Oi) совпадает с точкой О, а ось Zi направлена вдоль звена 1, система координат X2Y2Z2 выбрана таким образом, что ее начало (точка 02) совпадает с центром вращения кинематической пары В, образованной звеном 2 со эвеном 1, а ось 7.1 направлена вдоль направляющих звена 2, в которых движется звено 3 поступательно, так что оси Z2 и 2з совпадают, а начало (точка

03) системы координат XsYaZa движется вдоль оси Z2, таким образом, манипулятор (фиг. 1) имеет возможность реализовать движение схвата 4 в пространстве с помощью трех обобщенных координат «ю - угла поворота звена 1 относительно станины 5, «31 «zi - угла поворота звена 2 относительно звена 1 и 5з2 перемещение эвена 3 в направляющих звена 2. На фиг, 1 изображена также траектория точки D детали 6, например, центры масс детали, установленной в схвате 4 неподвижно.

Движение точки D вдоль выбранной траектории задается координатами XQ. YD. ZD, а также радиусом-вектором 7Ь и его углами поворота прецессии ЧЬ и нутации Gb.

В основу функциональной взаимосвязи обобщенных координат с координатами точки D траектории схвата 4 положено пред- ставление вектора rD XDH-YDJ+ZDk в неподвижной системе координат с помощью углов (см. фиг. 1).

15

4b arctg(YD/XD)

Gb arctg(Zo/ 4 X2o+Y2o

(1)

Как видно из фиг. 1 обобщенные координаты аю 4 5з2 . «31 могут быть определены из условия гЬ М+Тво замкнутости векторного контура, которое выполняется в каждой точке D траектории схвата 4. После некоторых преобразований получают следующие соотношения:

«ю - W Ј arctg (YD/XD)

30

5з2 IBD Vro+ri -2rDlislnGb

«31 «21 лг -arccos

jl±JjDJl D

2HIBD

(2)

где X2D+Y2o+Z2D - длина звена 1 манипулятора.,

Каждому, закону .изменения скорости точки D VD XDT+YDJ+ZDk вдоль ее траектории соответствует свой закон изменения угг

лов ЧЪ и QD во времени, т.е.

,|b YDXD-XDYD X) 4- Y&

ёь - Zp(XЈ + Yfe) - Zp (Хр Хр + Yp YD) (Xo + YD + ZoVXo +YD)

(3)

Обобщенные скорости могут быть пол- учёны дифференцированием уравнений (2) по времени. После преобразований получают

-

X0Y0-YnXt

РО f1 - 2; 6D+ra SD cos 9n1

B° ,sinej,

2«BDK A0-rDr.I,ve№(e elttJ.r.

№J«UW(

M3MN3oMls3 Ј3+W3x ls3 W3)- инерционный момент звена 3 неподвижной системы координат

зз - тензор инерции звена 3 относительно его центра масс 5зПолученные значения проекций реактивных сил и моментов звена 3 по формулам (7) и (8) в неподвижной системе координат XoYoZp не дают представления о распределении реактивных сил и моментов в системе координат ХзУзТз, связанной со звеном 3. Поэтому производится преобразование векторов и Мг 23 вычисленных в неподвижной системе координат XoYoZo в систему координат ХзУз2з следующим образом:

R(3)(o)23:

M(3)(0)23,

где МозН№о т - матрица преобразования векторов, заданных координатами в системе координат XoYoZo в координаты, заданные в системе координат XaYqZs

Преобразование вектора . получаемого из уравнения (7), в проекциях на оси неподвижной системы координат XoYoZo в систему координат ХзУз2з уравнение (9) позволяет выделить его движущую R 23Z (3) и собственно

реактивную RR23 . ()2+()2 составляющие. Определяют законы изменения . и RR23 в функции от пути (длины траектории точки D) посредством решения уравнения (9) в фиксированных точках траектории. Аналогично, решая уравнение (10), в фиксированных точках траектории точки, выделяют реактивные составляющие М®2 зх, , и получают закон изменения собственно реактивной составляющей в функции от пути

(M(3)23X)4M(3)23Y)2+(M(3)23Z)2

Определение законов изменения движущих и реактивных сил и моментов звена 3 в функции от пути позволяет оценить динамическую нагруженность звена 3 при перемещении схвата 4 вдоль траектории заданной с заданными ускорениями и скоростями.

Определяют закон изменения управляющей силы Rynp23 -R23 в функции от пути. Для этого график РГВ23. построенный в функции от пути, отражают симметрично относительно оси S. Полученный в результате

отражения график представляет собой управляющую силу звена 3.

Аналогично определяют значения движущихся и реактивных сил и момент ов в

соединении звеньев 2 и 1, определяют законы их изменения функции от пути. Для звена 2 (см. фиг, 3) уравнения равновесия в векторной форме в неподвижной системе координат представляют в виде

R 12+R32+G2 0 1

М12+Мз2+Ми2 0 J(11)

Учитывая, что и , то (11) записывают в виде

R:i2 R23-G 1

.)

(12)

20 где Rt2 - сила реакции в соединении звена 2 со звеном 1:

R23 - находят из уравнения (7); G2 - сила тяжести звена 2; Mi2 - момент, действующий на звено 2 25 со стороны звена 1:

М23 - момент, действующий на звено 3 со стороны звена 2, ходят из уравнения (8); МИ2 - момент силы инерции звена 2.

30 ffls2 E3+W3 х )

Is2 - тензор инерции звена 2 относительно его центра масс S2.

Векторы Ri2H Mi2, вычисленные по фор- 35 мулам (12) в неподвижной системе координат XoYoZo, преобразовывают в систему координат X2Y2Z2 следующим образом:

ВЙР-flwe J}

Ц,, 40 М(2)(|

(13)

где - матрица преобразования векторов, заданных координатами в системе координат XoYoZo в координаты, задан- ные в системе координат X2Y2Za.

Выделяют движущую М M2X и собственно реактивную М 12

4

V((M 12Z)2 составляющие мо- мента звена 2. Определяют законы изменения М36. М 12. а также R 12 (2)() (

(R12X)2 + (R12Y)2 + (R12X)2 В ФУНКЦИИ ОТ

пути, ипредбляют закон изменения управ- ляющего момента Mynpi2 -lVu 12 в функции от пути. Для этого график М га, построенный в функции от пути, отражают симметрично относительно оси S. Полученный в результате отражения график представляет

собой управляющий момент на валу приводного двигателя звена 2.

Для звена 1 (см. фиг. 4) уравнение равновесия в векторной форме представляют в виде

R01+R21+GH)|

.Г №

Moi+M21+MiiT+MA(R2l)0 J

Так как и , (14) записывают jj виде R01+R21+G1

M01+M21+MM1+MA(R21)

(15)

где ROI сила реакции в соединении звена

1 со станиной 5;

Ri2 - сила реакции в соединении звена

2 со таниной 5, находят из уравнения (12);

GJ-сила тяжести звена 1;

Мо1 - момент, действующий на звено 1 со стороны станины 5;

Mi2 - момент, действующий на звено 2 со стороны звена 1, находят из уравнения (12).

Ми1 - момент силы инерции звена 1

Ми1НТМю (Isljfe + )

Isi -тензор инерции звена 1 относительно центра его масс Si.

MA(R2)li х - момент силы реакции

R2.1.

Все компоненты уравнений (14) и (15) заданы в неподвижной системе координат XoYoZo. .

Векторы ROI и MQI, вычисленные по формуле (1.5) в неподвижной системе координат XoYoZo, преобразовывают в систему координат XiYiZi следующим образом:

R(1)oHNoi R(o)oi M(1)oHNoi M(o)oi

где NoiJ -fNio - матрица преобразования векторов, заданных координатами в системе координат XoYoZo в координаты, заданные в системе координат XiYiZi.

Выделяют движущую М и собственно реактивную MRoi

V(Mbfix)2 + (M&W составляющие моменты звена 1.

Определяют законы изменения М98,

MRoi. а также ftfafaffltf + (RoyY)2 + (R в функции от пути. Определяют закон изменения управляющего момента Mynpoi -ML8Boi в функции от пути. Для этого график №TBOI, построенный в функции от пути, отражают симметрично относительно оси S. Полученный в результате отражения график представляет собой управляющий момент на валу приводного двигателя звена 1.

Еще одним достоинством предложенного способа управления является то, что при расчете силовых и скоростных характеристик отдельных приводов графики изменить расчетных (номинальных) управляющих моментов Мупрсн и Мупр12 и расчетной (номинальной) управляющей силы Рупр2з. а также расчетных (номинальных) угловых «ю и «21 и линейной 5з2 скоростей,

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

легко определить быстродействия по форт

муле t /dS/v(S) и значительно легче выйти

на закон изменения силы инерции перемещаемого объекта по формуле

.vV,

где v dv/dS, а дальше и увязать пройденный путь со временем (погрешностью позиционирования во времени) с учетом зависимости

At tj + 1 -ti

Si+i -Si vi -И - vi

35 и погрешностью по скорости с учетом зависимости

AV V|+I-V|

Si + 1 - SI

Закон же изменения силы инерции кладут в основу определения динамической на- груженности приводных двигателей. При этом значения выбранных обобщенных ко45 ординат, как и обобщенных скоростей определяют также в функции пути S, т.е. в функции траектории центра масс перемещаемого объекта.

Пример. Исходные данные: траекто50 РИЯ схвата - прямая линия, проходящая через начальную и конечную точку DH {0,289; 0,498; 0,805}, DK {0; 1; 0,65}; размеры звеньев манипулятора м, ,6 м (размерами звена 2 можно пренебречь); массы

55 звеньев кг, ,3 кг, кг; масса перемещаемой детали кг; начала подвижных систем координат, связанных жестко с подвижными звеньями, расположены на осях их относительного вращения, при этом оси Z этих систем координат направлены вдоль звеньев; координаты центров масс подвижных звеньев расположены в их середине; координаты центра масс детали 6 расположены на линии Он -Рк, быстродействие (время отработки траектории рн-0к) с; длина траектории ,6 м; движение схвата вдоль траектории DH-DK задано в виде трапецеидального закона (см. фиг. 5) с участком разгона (т. Т) равномерного движения ( Т) и участком торможения (тТорм 1/4 Т). м/с.

По полученным зависимостям (5), (9), (10), (13), (16), определяющим функциональную взаимосвязь обобщенных координат, а также их скоростей и ускорений с движущими и реактивными силами и моментами, воз- нищающими в подвижных звеньях манипулятора, составлена программа на ЭВМ. По результатам численного исследования рассматриваемого примера с помощью ЭВМ построены графики законов изменения обобщенных скоростей в соответствии с выбранным законом движения схвата вдоль заданной траектории (см. фиг. 6, 7 и 8)..,

На фиг. 9 представлен график закона изменения управляющей силы Rynp23 -R -23, обеспечивающей относительное движение звена 3 в соответствие с заданной траекторией схвата и закона изменения его скорости. На фиг. 10 и 11 представлены графики законов изменения управляющих моментов на валах приводных двигателей (звена 2 .манипулятора Мупр12 -МЗВ12, звена 1 - Mynpoi -MSBoi) соответственно, обеспечивающие относительные движения соответствующих подвижных звеньев в соответствии с заданной траекторией схвата и закона изменения его скорости. На фиг: 12,13 и 14-представлены законы изменения мощностей трех приводных двигателей (кривые 1), а также законы изменения мощностей с учетом постоянных потерь, всегда присутствуют как в приводе, так и в передаточных механизмах (редукторах и т.д.) (кривые 2). На фиг. 15 представлена блок-схема системы управления по мощности, структуру и способ функционирования которой мы рассмотрим более подробно.

Блок-схема системы управления по мощности одного из приводных двигателей, например, приводящего во вращение звено 1 (см. фиг. 2), представлена на фиг. 15. Данная схема содержит главный контур управления, состоящий из программируемого функционального генератора 1, электрически связанного с катушкой 2 электромагнита, имеющего сердечник 3 и подвижный общий якорь 4, который жестко соединен с

планкой 5, изготовленной из магнитомягко- го материала, которая перекрывает часть воздушного зазора магнитопровода в магнитной цепи, содержащей катушку 7, включенную последовательно в электрическую цепь обмотки управления 8, двухфазного асинхронного двигателя переменного тока 9, имеющего к тому же обмотку возбуждения 10, на валу приводного двигателя 9 раз0 мещен тахогенератор 11, электрически соединенный через реостат 12 с блоком перемножения аналоговых величин 13, а на выходном звене редуктора электродвигателя 9 размещен тензодатчик 14, электриче5 ски соединенный с усилителем 15, который также электрически соединен через реостат 16с блоком перемножения 13. Если относительное движение звена соответствующего приводного двигателя является возвратно0 поступательным, тензодатчик 14 может быть заменен соответственно датчиком, вырабатывающим электрический /ток, прямо- пропорциональный движущей силе, приложенной к этому звену, перемножитель

5 13 электрически соединен с катушкой 17 электромагнита, имеющего сердечник 18 и общий подвижный якорь 4, вспомогательный контур управления состоит из программируемого функционального генератора 19,

0 электрически связанного с катушкой 20 электромагнита, имеющего сердечник 21 и общий подвижный якорь 22, который жестко соединен с планкой 23, изготовленной из магнитомягкого материала, которая пере5 крываёт часть воздушного зазора магнитопровода 24 магнитной цепи, содержащей катушку 25, включенную последовательно в электрическую цепь обмотки управления 8 двухфазного асинхронного двигателя 9, та0 хогенератрр. 11 через реостат 12 электрически связан с катушкой 2.6 электромагнита, имеющего сердечник 27 и общий подвижный якорь 22, устройство управления двигателем 28 электрически связано с обмоткой

5 управления 8.

Принцип работы блок-схемы системы управления по мощности состоит в следующем.

В соответствии с необходимым законом

0 движения схвата вдоль заданной траектории программируемый функциональный генератор 1 формирует электрический сигнал, напряжение которого Ui прямопропорцио- нально расчетному (номинальному) закону

5 изменения мощности рассматриваемого приводного двигателя (рис. 9. кривая 1), а программируемый функциональный генератор 19 формирует электрический сигнал, напряжение Ua которого прямопропорцио- нально расчетному (номинальному) закону.

изменения угловой скорости рассматриваемого приводного двигателя (рис. 6), тахоге- нератор 11 преобразует частоту вращения двигателя (Умех в напряжение LM, которое при помощи реостата 12 выравнивается по величине с напряжением U2, тензодатчик преобразует крутящий момент двигателя в ток; который проходит через усилитель 15 и при помощи реостата 16 выравнивается до такой величины, которая при перемножении с напряжением Щ в блоке 13 давала бы на его выходе сигнал, напряжение которого Уз изменялось бы прямопропорционально действующей (измеренной) мощности Рмех и по величине соответствовало бы напряжению Uf. Устройство управления 28 изменяет напряжение Uy обмотки возбуждения в соответствии с требуемым жестким режимом работы приводного двигателя. Если по каким-либо внутренним причинам (уменьшением или увеличением переменной мощности потерь как в двигателе, так и в редукторе) произошло уменьшение (увеличение) действительной (измеренной) мощности двигателя Рмех по сравнению с расчетной (номинальной) Рр, тогда соответственно станет меньше (больше) по абсолютному значению напряжения УЗ, поступающее с блока 13 и эапитывающее катушку 17, по сравнению с напряжением Ut, поступающим с блока 1 и запитываю- щим катушку 2, произойдет нарушение равновесия, тяговое усилие F2 станет меньше (больше) тягового усилия FI, под действием силы F« ± FI ± Fa общий якорь 4 начнет двигаться влево (вправо), в результате чего произойдет увеличение (уменьшение) воздушного зазора магнитопровода 6 магнитной цепи, что вызовет увеличение (уменьшение) магнитного сопротивления данной магнитной цепи и соответственно уменьшение (увеличение) реактивного сопротивления катушки 7, что приведет у уменьшению (увеличению) мощности теряемой на данной катушке и соответственно вызовет дополнительное увеличение (уменьшение) мощности, поступающей на обмотку возбуждения, что приведет к увеличению (уменьшению) мощности, подводимой непосредственно к двигателю и увеличению (уменьшению) мощности Рмех, снимаемой с двигателя, равновесие мощностей и соответствующих им напряжений восстановится.

Однако равенство мощностей не означает, что двигатель 9 будет вращаться с необходимой угловой скоростью (Омвх ыр , для их выравнивается и необходим вспомогательный контур управления.

который работает следующим образом. Та- хогенёратор 11 преобразует частоту вращения двигателя flWx в напряжение U4, запитывающее катушку 26, если частота 5 вращения ротора электродвигателя (Оме. станет меньше расчетной (номинальной) частоты , тогда соответственно станет меньше (больше) по абсолютному значению напряжение U4 по сравнению с напряжени0 ем U2, поступающим с блока 19 и запитыва- ющем катушку 20, произойдет нарушение равновесия, тяговое усилие одного электромагнита F2, станет меньше тягового усилия другого электромагнита FL под действием

5 силы F ± FI + Fa общий якорь 22 начнет двигаться влево (вправо), в результате чего произойдет увеличение (уменьшение) воздушного зазора магнитопровода 24 магнитной цепи, что вызовет увеличение

0 (уменьшение) магнитного сопротивления данной магнитной цепи и соответственно уменьшение (увеличение) реактивного сопротивления катушки 25, что приведет к уменьшению (увеличению) напряжения, те5 ряемого на данной катушке, и соответственно вызовет дополнительное увеличеяие (уменьшение) напряжения, поступающего на обмотку возбуждения, что приведет к увеличению (уменьшению) частоты враще0 ния двигателя.

Таким образом, приведенный способ управления манипулятором позволяет решить поставленную цель, заключающуюся в

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

Формула изобре тения

Способ управления манипулятором

промышленного робота, заключающийся в

том, что посредством системы управления

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

.. -; .. .- ; z.

. - .;.- . . . 2-,

Фж.З

Использование: в манипуляторах промышленных роботов. Сущность изобретения: в способе управления манипулятором промышленного робота определяют расчетные (номинальные) законы изменения обоб- щенных скоростей всех приводных двигателей, а также мощности этих приводных двигателей, получают действительные (измеренные) обобщенные скорости всех приводных двигателей, а также действительные (измеренные) мощности этих приводных двигателей, сравнивают расчетные мощности с действительными (измеренными) и по сигналу рассогласования между ними формируют сигнал управления приводными двигателями. 15 ил.

Фиг: 5

6029Ш