Устройство для управления приводом робота Советский патент 1992 года по МПК B25J13/00 

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

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

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

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

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

W

Ё

XI

ел

t

со

00

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

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

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

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

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

5 выход - с вторым входом первого блока умножения. ,

Данное устройство по своей технической сущности является наиболее близким к предлагаемому.

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

0 манипуляторов типа SR 5407, 5408, ТУР-2- 5КМ, Гранат-2.5, кинематика которых изображена на фиг. 2 и фиг. 3. Для достижения высокого качества (точности) управления следует разрабатывать другие корректиру5 ющие устройства, которые обеспечивают точную компенсацию переменных параметров нагрузки, характерных для указанного типа манипуляторов.

Цель изобретения - повышение точно0 сти работы приводов манипуляторов типа SR, ТУР-2, 5КМ, Гранат-2,5.

Поставленная цель достигается тем, что в устройство, содержащее последовательно соединенные первый сумматор, первый

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

На фиг. 1 представлена блок-схема устройства управления приводом первой степени подвижности робота; на фиг. 2 - исполнительный орган манипулятора, вид сбоку; на фиг. 3 - исполнительный орган манипулятора, вид сверху.

Введены следующие обозначения: %х - сигнал с выхода программного устройстве; б- сигнал ошибки; Ui - сигнал с выхода второго блока умножения; U2 - сигнал с выхода корректирующего звена; 11з - скорректированный сигнал на входе первого усилителя,-д2.р2,р2 - сигналы с датчиков положения, скорости и ускорения второй степени подвижности робота; qi - сигнал с датчика положения первой степени подвижности; тг - сигнал с датчика массы.

Устройство для управления приводом робота содержит последовательно соединенные первый 1 сумматор, блок 2 коррекции, блок 3 умножения, второй 4 сумматор и блок 5 умножения, последовательно подключенные усилитель б и электродвигатель 7, кинематически связанный с датчиком 8 положения, соединенным выходом с первым входом первого сумматора 1, второй вход которого подключен к первому входу устройства, последовательно соединенные блок 9 деления и блок 10 умножения, последовательно подключенные третий сумматор

11 и блок 12 деления, выход которого соединен с вторым входом блока 3 умножения, а также вычислительный блок 13, подключенный семью входами к соответственно с вто- 5 рого по восьмой входам устройства, первым выходом к второму входу блока 5 умножения, вторым выходом к первому входу третьего сумматора 11 и второму входу блока 9 деления, третьим выходом к второму входу 10 первого 9 и второго 12 блоков деления, а четвертым выходом к второму входу третьего сумматора 11, второй выход блока 2 коррекции соединен с вторым входом второго сумматора 4, а третий выход - с вторым

15 входом блока 10 умножения, подключенного выходом к третьему входу второго сумма- тора 4. Устройство содержит также последовательно соединенные блок Т4 коррекции и четвертый сумматор 15, второй

20 вход которого подключен к выходу блока 5 умножения, а выход - к входу усилителя 6, восьмой вход вычислительного блока 13 соединен с девятым ЁХОДОМ устройства и пятый выход - с входом блока 14 коррекции.

5 Устройство работает следующим образом.

На его первый вход с программного устройства подается управляющее воздействие fifex , характеризующее требуемый

0 закон управления объектом. На выходе первого сумматора 1 формируется сигнал ошибки д , который поступает на вход блока 2 коррекции. Блок коррекции содержит первое инерционное дифференцирующее зве5 но 29, первое апериодическое звено 30, апериодическое звено 31 второго порядка и второе инерционное дифференцирующее звено 32. После коррекции в блоке 2 коррекции, а также в блоках 3,4,5,10,15, усилива0 ясь в первом усилителе б, сигнал поступает на электродвигатель с редуктором, приводя его вал, механически связанный с исполнительным органом робота, во вращательное движение с направлениемгскоррстью и ус5 корением, зависящим и от величины поступающего сигнала с%х и от внешнего моментного воздействия Мв на привод.

Электропривод при работе с различными грузами, а также за счет взаимовлияния

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

обеспечить стабильность заданного качества системы.

В данной заявке рассматривается устройство для управления приводом робота, обеспечивающим перемещение относительно вертикальной оси с координатой qi. Схема робота представлена на фиг. 2 (вид сбоку) и фиг. 3 (вид сверху). Данная конструкция исполнительного органа обеспечи- вает также вращение вокруг шарнира второй степени подвижности робота в горизонтальной плоскости (обобщенная координата qa).

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

Введены следующие обозначения: Н - момент инерции первого звена манипулятора при вращении его вокруг вертикальной оси, проходящей через первый шарнир (координата qi);

2 - момент инерции второго звена манипулятора при вращении его вокруг вертикальной оси, проходящей через второй щарнир (координата qa);

Г - момент инерции двигателя и вращающихся частей силовой механической передачи (редуктор) привода, приведенный к выходу вала редуктора;

RI- длина первого звена манипулятора; Ra - длина второго звена майипулятора; П - расстояние от оси вращения первого звена до его центра масс;

гг - расстояние от оси вращения второго звена манипулятора до его центра масс. Из уравнения Лагранжа 2 рода определяется моментное воздействие на привод поворота первой степени подвижности. Это воздействие имеет вид

(q2)qi+h(q2,q2)qi+MBH,(1)

где H(q2)h+l2+(m2+mr)Ri +mrR 2+ +2Ri(m2r2+mrR2)cosq2, h(q2,(m2r2+mrR2)sin(q2)q2, (2) MBH(l2+mrR 2+Ri(m2r2+mrR2)cosq2 qV -Ri(m2r2+mrR2)q astnqa. Для решения поставленной задачи с учетом изменяющихся параметров нагрузки привода первой степени подвижности робота, выражения (1) и (2), необходимо сформировать такое устройство управления, чтобы оно обеспечивало независимость параметров привода от изменяющихся параметров моментного действия.

При разработке этого устройства полагается, что на первый, второй, третий и шестой входы вычислительного блока 13 соответственно от датчиков 33 положения,

скорости 34 и ускорения 35 второй степени подвижности и датчика 38 массы груза поступают сигналы q2, q2, q 2 и mr. Полагается также, что источники постоянного напряжения 36,37, 39,40 соответственно на четвертый,

0 пятый, седьмой и восьмой входы вычислительного блока 13 подают сигналы г,

2Rim2r2, r+ i+l2+rri2R2l и K w Км, где К ц) К (л) р , .

К w - коэффициент противоЭДС элект5 родвигателя;

Км - моментный коэффициент электродвигателя;

IP - передаточное отношение редуктора.

0 Кроме того, полагается, что функциональные преобразователи 21 и 26 соответственно реализуют следующие функциональные зависимости: sin и cos. Первый (со стороны источника постоянного напряжения 37) и

5 второй (со стороны датчика 38 массы груза) положительные входы сумматора 16 имеют соответственно коэффициенты усиления 1 и 2 RiR2. В результате на выходе функционального преобразователя 21 формируется

0 сигнал sinq2, на выходе сумматора 15 - сигнал 2 Ri(m2r2+mrR2). а на выходе блока умножения 22 - сигнал

2Ri(m2r2+mrR2)sln(q2) (см. выражение (2)).

5 На выходе функционального преобразователя 26 формируется сигнал cosq2, а на выходе блока 17 умножения - сигнал 2Ri(m2n2+mrR2)cosq2. Поскольку второй и третий положитель0 ные входы сумматора 27 (соответственно с выхода блока 17 умножения и источника 39 постоянного напряжения) имеют единичный коэффициент усиления, а его первый положительный вход (со стороны датчика 38

5 масы груза) коэффициент усиления (R i+R22). то на выходе сумматора 27 формируется сигнал, равный Г+Н (см. выражение

(2))Сигнал, равный 2Ri(m2r2+mrR2)sln(q2)q 2,

0 формируется на выходе блока 23 умножения. Поскольку первый положительный вход сумматора 18 (со стороны блока 17 умножения) имеет коэффициент усиления, равный 1/2, второй положительный вход (со стороны

5 датчика 38 массы груза) имеет коэффициент усиления, равный R 2, а третий положительный вход (со стороны источника 37 постоянною напряжения) единичный, то на выходе этого сумматора будет формироваться сигнал, равный l2+tnrR224Ri()cosq2.

Очевидно, что в этом случае на выходе блока 19 умножения будет сформирован сигнал l2+mrR 2+Rl{m2r2+mrR2)cosq2 q2, а на выхо- де сумматора 20 первый положительный (с выхода блока 19 умножения) и второй (с выхода блока 23 умножения) отрицательный входы которого соответственно имеют коэффициенты усиления 1/Км и (1/(21С)),фор- мируется сигнал МВн/Км (см. выражение (2)).

Первый отрицательный вход сумматора 24 (с выхода блока 22 умножения) имеет коэффициент усиления ka (R - активное сопротивление якорной обмотки электродвигателя), а второй положительный (с выхода источника 40 постоянного напряжения) - единичный. В результате на выходе сумматора 24 формируется сигнал, равный Rgh+KiKfc). Усиливая этот сигнал в усилителе 25 в 1/КмКц, раз, на его выходе формируется сигнал (РяЬ+КмКсо )/КмКа.

С учетом указанного на выходе блока 9 деления формируется сигнал (Г+Н)/ ял+КмКю), на выходе сумматора 11, первый отрицательный вход которого (со стороны блока 22 умножения) имеет коэф- фициент усиления . (1Я - индуктивность якорной обмотки двигателя), а второй положительный (со стороны сумматора 27) - коэффициент усиления Ra, формируется - сигнал (r+H)Rs+hLB. В результате на выходе блока 12 деления будет сформирован сигнал ((I +HjRs+hLMRah+K,; иК)).

Принимается, что передаточная функция звена 29 равна Л/2э(р) + Р. - , а пеTtp+1звена 32

Wasредаточная функция р I

f . . Тогда на выходе блока 10 умножения формируется сигнал

( n.ri№ .

Учитывая, что передаточная функция апериодического звена 30 равна Wso(p)

-j-Т77 « на блока 3 умножения будет получен сигнал, равный

ХТта а.

Поскольку передаточная функция апериодического звена второго порядка 31 равна

W31(P) (Tlp + 1)(T2p + 1) Т° НЯ ВЫХОЯв этого звена формируется сигнал 5 /(Г1р-ИХТ2р-И), а на выходе сумматора 4 - сигнал

5

5

(l4H)U p2 . (1ЧН)НЯ +Ы„

I y t w . i tf t tf

R, h + КД K M

jRfl h Ь Км Киз

(Tip-H)(T2p + 1)

Помножив этот сигнал с помощью блока 5 умножения на сигнал с выхода усилителя 25, на выходе этого блока умножения будет сформирован сигнал

{Ml + KijC ( (I +H)L, рг

+ Кц Кц

JCCT

( Ч-Н)И. +ht.

R. h+Кц

(Тц,+1)(Тзр+ 1)

P 4-1 )

d.

(3)

С учетом соотношений (1) и (2) электропривод первой степени подвижности на базе электродвигателя постоянного тока независимого возбуждения или с постоянными магнитами достаточно точно можно описать передаточной функцией вида

J -ч,.

КУ/(К.Ь + К:КШ)

(1ЧН)1. (1ЧН)Н. +HL. , . Э- h J-VJ. v о к jri TV г -г 1

т

R, h -I- Ki Ki,

R. h + Ki K,

Если бы , то , n тогда, с учетом выражений (3) и (4), передаточная функция разомкнутого привода с разработанным самонастраивающимся устройством управления приняла бы вид:

W(P)-

Ку/Кц)

(Т1р + 1)(Т2р + 1)

(5)

25 0

5

0

5

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

Однако согласно выражению (2), МВн 0, И это воздействие (М8н) будет ухудшать точностные характеристики привода. Поскольку МВн не зависит от фазовых координат рассматриваемого привода, можно сформировать добавочный сигнал U2. компенсирующий отрицательное влияние моментного воздействия МВн на качественные показатели привода.

Полагается, что передаточная функция корректирующего звена 14 имеет вид:

WH

50

Lap RH Тэр + 1

где Тз некоторая, достаточно малая, постоянная времени.

Тогда сигнал IJ2 принимает вид:

U2

Lap b R;

Км(Тзр + 1)

М

вн(6)

Поскольку для электродвигателей постоянного тока справедливо соотношение

U3

ti

(l +H)qi+hqi+MBH, (7)

Lap + R«

то с учетом того, что UsHJi+Ua, а также выражений (6) и (7), будем иметь

(LH+Ua) (l +H)qi+hqi+MBH

Lqp т Кя

Ui K , Мвн

+

Lflp-f-R ТзР-М(8)

Из выражения (8) видно, что, если величина Тз достаточно мала, то с помощью дополнительного сигнала U2 с достаточно высокой степенью точности удается компенсировать и отрицательное влияние МВн на качественные показатели работы привода рассматриваемой степени подвижности робота.

Таким образом, применение нового вычислительного блока 13 вместе с корректирующим звеном 14 и четвертым сумматором 15 позволило достичь высокой точности управления указанным приводом манипулятора типа SR, ТУР-2,5 КМ и Гранат-2,5. Формула изобретения Устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, первый блок коррекции, первый блок умножения, второй сумматор и второй блок умножения, последовательно подключенные усилитель и электродвигатель, кинематически связанный с датчиком положения, соединенным

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

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

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

устройства и пятый выход - с входом второго блока коррекции.

;9&7

Фиг. 2

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

Ъ

фие.З