Устройство для управления приводом робота
Использование: изобретение относится к робототехнике и может быть использовано в электроприводах промышленных манипуляторов . Сущность изобретения: устройство содержит дополнительно второй корректирующий блок, четвертый сумматор и их связи , что обеспечивает повышение точности привода для различных манипуляторов. 3 ил.
„„, « Ц „„1754438 А1
СОЮЗ СОВЕТСКИХ
СОЦИАЛИСТИЧЕСКИХ
РЕСПУБЛИК (я)5 В 25 J 13/00
ГОСУДАРСТВЕННЫЙ КОМИТЕТ
ПО ИЗОБРЕТЕНИЯМ И ОТКРЫТИЯМ
ПРИ ГКНТ СССР
ОПИСАНИЕ ИЗОБРЕТЕНИЯ
К АВТОРСКОМУ СВИДЕТЕЛЬСТВУ
Изобретение относится к области ав-: дифференцирующего звеньев подключены томатического управления и может быть: к выходу второго сумматора, а их выходыиспользовано в электроприводах промыш-::: соответственно к первому и через блок ум-, ленных манипуляторов. ." ножения к второму входам третьего сумма Известна система автоматического уп-": тора, выход которого пЬдключен к входу д равления электроприводом промышленно- усилителя . го робота, содержащая последовательно .. Недостатком этого устройства является у соединенные усилитель, электродвигатель,: малая точность при- управлении" мтногохстедатчик тока, первый сумматор, интегратор, пенным исполнительным органом робота, блок деления иблокумножения, причем вал " когда меняются не только момент инерции электродвигателя связан с исполнительным привода, но появляются и момейтнтые возмеханизмом, датчиком положения и датчи--. - действия, обусловленные взаимовлиянием 00 ком скорости. выход которого подключен к " степеней подвижности роботадругна друга втарому вхОду блока деления, выход датчи-: (моменты скоростных сил). )нл ка положейия соединен с первым входом Известно также устройство для упвторого сумматора, вторым входом подклю- равления приводом робота, содержащее ченного к выходу задающего устройства, а последовательно соединенные инерцивторой вход первого сумматора подключен онно-дифференцирующее звено, первый к выходу измерителя внешнего момента. блок умножения и сумматор, второй вход
Кроме того, устройство содержит апериоди- которого соединен с входом апериодическоческое звено, инерционное дифференциру- го звена, вход которого соединен с входом ющее звено и третий сумматор, причем инерционно-дифференцирующего звена, входы апериодического и инерционногсо второй вход первого блока умножения сое1 (21) 4744231/08 (22) 27.09.89 (46) 15.08.92. Бюл. М 30 (71) Дальневосточный политехнический институт им. В.В. Куйбышева и Николаевский кораблестроительный институт им. адм.
С.О. Макарова . (72) В.Ф, Филаретов. Ю.П. Кондратенко и
А,Е. Герасименко (56) Авторское свидетельство СССР
М 1579770, кл. В 25 J 13/00, 1987.
2 (54) УСТРОЙСТВО ДЛЯ УПРАВЛЕНИЯ
ПРИВОДОМ РОБОТА (57) Использование: йзобретение относится к робототехнике и может быть использовано в электроприводах йромышлейных манипуляторов. Сущность изобретения:.устройство содержит дополнителльно второй корректирующий блок, четвертый сумматор и их связи, что обеспечивает повышение "точности привода для различних" манипуляторов. 3
: ил.
1754438 динен с выходом блока деления, кроме того, . электродвигатель, а также вычислительный устройство содержйт последовательно сое- блок, семь входов которого соединены с диненные второй блок умножения, первый семью входами устройства, а выходы подусилитель и электродвигатель, причем пер- ключены соответственно первый — к второвый вход второго блока умножения соеди- 5 му входу второго блока умножения, второй нен с выходом первого сумматора, а выход и третий — к первому и второму входам пер— cвходом первого усилителя,,а также вы- вого блока деления, соединенного выходом числительный блок, выполненный в виде по- с входом третьего блока умножения, второй следовательно соединеннйх второго входкоторого подключен квторомувыходу сумматора, первого квадратора, третьего 10 блока коррекции, а выход — к второму входу блока умножения, третьего сумматора, вто- . первого сумматора, подключенного третьим рой вход которого соединен с первым вхо- входом к третьему выходу блока коррекции; дом вычислительного блока, а третий — с вход которого соединен с выходом второго выходом второго ква фатора, вход кбторого сумматора, первый вход которого подклюсоефинен с первым входом четвертого сум- 15 чен к восьмому входу устройства, а второй матора и выходом пятого сумматора, пер- вход — к выходудатчика положения, кинемавый вход которого соединен с первым тическисвязанногосваломэлектродвигатевходом второго сумматора и вторым входом ля, пятый выход вычислительного блокавычислительного блока, а второй — с вторым подключен к последовательно соединенвходом второго сумматора и третьим вхо- 20 ным третьему сумматору, второй вход котодомвычислительногоблока,четвертыйвход рого соединен с шестым выходом-.. которого соединен с третьим входом второ- вычислительного блока, и второму блокуде-. го сумматора, пятый — с первым входом ше- ления, второй вход которого соединен с чет-. стого сумматора, а шестой. — с вторым вертым выходом вычислительного блока, а ..=.. входом третьего блока умножения и первым 25 выход — с вторым входом первого блока умвходом четвертого блока умножения, вто- ножения.: .. рой.вход которого соединен с выходом вто- Данное устройство по своей техничерого сумматора, а выход — с вторым входом ской сущности является наиболее близким четвертого сумматора, вйход которбго сое- к предлагаемому. динен с первым входом пятого блока умно- 30 Однако известное устройство сохраняет жения, второй вход которого соединен с достаточно высокую точность работы с пере- . седьмым входом вычислительного блока, а меннйми параметрами нагрузки. привода
- вйхьд - с вторйм входом шестого суммато- только для определенной степени подвижнора, выход которого соединен с входом вто- стиопределенного исполнительного оргарогоусилителя, причем второй входвторого 35 на робота (для определенного закона .блока умножения соединен с первым выхо- . изменения параметров нагрузки). Для дом вычислительного блока, второй и тре- других манипуляторов с помощью указантий выходы которого подкл ючен ы ного устройства высокая точность обеспечесоответственно к первомуи второму входам на не будет. В частности, для приводов блока деления. 40 манипуляторов типа SR 5407, 5408, ТУР-2Такоеустройствосохраняетдостаточно 5КМ, Гранат-2.5, кинематика которых изо-. вйсокую точность работы с переменными бражена на фиг.2 и фиг.3.Для достижения нагрузочными параметрами привода только высокого качества (точности) управления в том случае, когда электрическая постоян- следует разрабатывать другие корректируная времени двигателя близка к нулю. Од- 45 ющие устройства, которые обеспечивают нако это условие справедливо только в точнуюкомпенсациюпеременныхпараметотдельных случаях. В общем случае индук- ров нагрузки, характерных для указанного тивное сопротивление якорной обмотки типа манипуляторов. двигателя, а следовательно, и электромаг- Цель изобретения — повышение точнонитная постоянная времени якорной цепи 50 сти работы приводов манипуляторов типа не равны нулю, s результате чего в данном SR, ТУР-2, 5КМ, Гранат-2,5. устройстве коррекция уже не может обеспе- Поставленная цель достигается тем, что чить заданную точность привода, так как не в устройство, содержащее последовательно учитывает существенную особенность дви- соединенные первый сумматор, первый гателя. 55 блок коррекции, первый блок умножения, Известно также устройство для управ- . второй сумматор и второй блок умножения, ления приводом робота, содержащее после- последовательно подключенные усилитель довательно соединенные блок коррекции, и электродвигатель, кинематически связанпервый блок умножения, первый сумматор, ный с датчиком положения, соединенным второй блок умножения, первый усилитель, выходом с первым входом первого суммато175443S
5 6 ра; второй вход которого-подключен к пер- 11 и блок 12 деления, выход которого соедивому входуустройства, последовательно со- : нен с вторым входом блока 3 умножения, а единенные первый блок деления и третий .. также вычислительный блок 13, подключенблок умножения, последовательно подклю-": ный семью входами к cîîòâåòñòâåíHî с вто. ченные третий сумматор и второй. блок де- 5 рого по восьмой входам устройства, первым ления, выход которого соединен с вторым, выходом к второму входу. блока 5 умножевходом первого блока"умножения, а также, . ния, вторымвыходомкйервомувходу третьвычислительный блок, подключенн"ый " его сумматора 11 и второму входу блока 9 семью входами к соответственно с второго .:: деления, третьим выходом- к второму входу по восьмой входам устройства, первым вы- 10 первого 9 и второго 12 блоков деления. а ходом к второму входу второго блока умно- четвертым выходом к второму входутретьежения, вторым выходом к первому- входу: го сумматора 11, второй выход блока 2 кор. третьего сумматора и второму входу перво- рекции соединен с вторым входом второго гоблокаделения,третьим выходом к второ- . сумматора 4, а третий выход — с вторым му входу первого и второго блоков деления,: 15 входом блока 10 умножения, подключенное четвертйм выходом к второму входу треть-- ... го выходом к третьему входу второго суммаегосумматора,второйвыходпервогоблока тора 4, Устройство содержит также коррекции соединен с вторым входом вто- .: последовательно соединенные блок t4 кор: рого сумматора, а третий выход — с вторым ::,. рекции и четвертый сумматор 15, второй входом третьего блока умножения, подклю- 20 вход которого подключен к выходу блока 5 ченного выходом к третьему входу второro":."-óìíoæåéèÿ, а выход — к входу усйлйтеля 6, сумматора, дополнительно введены после- .:: восьмой вход вычислительного блока 13 со довательно соединенные второй блок кор- единен с девятым входом устройства и пярекции и четвертый сумматор, второй вход:: тый выход — с входом блока 14 коррекции. которого подключен к выходу второго блока 25 .. Устройство работает следующим обраумножения, а выход -"к входу усилйтеля,: - .зом, восьмой вход вычислительного блока соеди-: На его первый вход с программного уснен с девятым. входом устройства, и пятый тройства подается управляющее воздействыход — с входбм второго блока коррекции. : .вие а к, характеризующее требуемый
На фиг. 1 представлена блок-схема уст- 30 закон управления обьектом. На выходе перройства управления приводом первой сте- „::.вогосумматора1 формируется сигнал ошиб- . пени подвижности робота; на фиг, 2 — .. ки д, который поступает на вход. блока 2 исполнительный орган манипулятора, вид .::: .коррекции. Блок коррекцийсодержит пер-". сбоку; на фиг. 3 — исполнительный орган, вое инерционное дифференцирующее звеманипулятора, вид сверху. 35 но 29, первое апериодическое звено 30, Введены следующие обозначения: а х . апериодическое звено 31 второго порядка и — сигнал с выхода программного устройст- .. второе инерционное дифференцирующее ва; д — сигнал ошибки; U< — сигнал с выхода звено 32. После коррекции в блоке 2 корреквторого блока умножения; Uz — сигнал с . ции,атакжевблоках3,4,5,10,15,усилива" .. выхода корректирующего звена; Uz — скор- 40 ясь в первом усилителе 6, сигнал поступает
:ректированный сигнал на входе первого наэлектродвигательсредуктором, приводя усилителя; ср,(р,Q2 — сигналы с датчиков по- его вал, механически связанный с исполниложения, скорости и ускорения второй сте- тельным органом робота, во вращательное: пени подвижности робота; Qi — сигнал сдвижение с направлением,,скоростью и усдатчика положения первой степени подвиж- 45 корением, зависящим и от величины постуности; ег — сигнал с датчика массы. -- -, пающего сигнала ®x и От внвшнего
Устройство для .управления приводом моментного воздействия Мв на привод. робота содержит последовательно соеди- "-.-.: Электропривод при работе с различныненные первый 1 сумматор, блок 2 коррек-: ми грузами, а также За счет взаимовлияйия ции, блок 3 умножения, второй 4 сумматор 50 степеней подвижности манипулятора облаи блок 5 умножения, последовательно под-:::. дает переменными моментными характериключемные усилитель 6 и электродвигатель . стиками, которые могут мейяться в широких.
: - 1, кинематически связанный с датчиком 8::: пределах. Это снижает качество электроположения, соединенным выходом .с пер- привода и в некоторых случаях может привым входом первого сумматора 1, второй 55 вести к потере устойчивости его работы. В вход которого подключен к первому входу:. : результате возникает задача, связанная с устройства, йоследовательно соединенные обеспечением инвариантности динамичеблок 9 деления и блок 10 умножения, после- ских свойств электропривода к изменениям довательноподключенныетретийсумматор ег0 хорактеристик нагрузки, что позволяет
1754438 обеспечить стабильность заданного качества системы.
В данной заявке рассматривается уст, ройство для управления приводом робота, обеспечивающим перемещение относительно вертикальной оси с координатой q1.
Схема робота представлена на фиг. 2 (вид сбоку) и фиг.. 3 (вид сверху), Данная конструкция исполнительного органа обеспечивает также вращение вокруг шарнира второй степени подвижности робота в горизонтальной плоскости (обобщенная координата ср).
Для разработки у тройства управления приводом первой степени подвижности, обеспечивающего инвариантность динамических характеристик этого нриъода к изменяющимся параметрам нагрузки, определим характеристики момейтного действия на этот привод со стороны перемещающихся масс исполнительного органа, схематически изображенного на фиг. 2 и фиг. 3.
Введены следующие обозначения:
I1 — момент инерции первого звена манипулятора при вращении его вокруг вертикальной оси, проходящей через первый шарнир (координата q 1);
lz — момент инерции второго звена манипулятора при вращении его вокруг вертикальной оси, проходящей через второй шарнир (координата о2);
1 - момент. инерции двигателя и вращающихся частей силовой механической передачи (редуктор) привода, .приведенный к выходу вала редуктора; .
R1 — длина первого звена манипулятора; . R2 — длина второго звена майипулятора; г1 — расстояние от оси вращения первого звена до его центра масс;
r2- расстояние от оси вращения второго звена манипулятора до его центра масс.
Из уравнения Лагранжа 2 рода определяется моментное воздействие йа привод поворота первой степени подвижности. Это воздействие имеет вид
Мв=Н(Ц2)Ц1+п(Ц2 Ц2)41+Мв (1) где H(qz)=l1+lz+(mz+mr)R1 +mrR22+
+2R1(mZr2+mrR2)COSqZ, h(q2вqz) 2R (mzr2+mrRz)sIn(q2)q2 (2)
Мв =(!2+гпгй z+R1(mzrz+mrRzgosqgjz-R1(mzr2+mrRz)q zstnqz °
Для решения поставленной задачи с учетом изменяющихся параметров нагрузки привода первой степени подвижности робота, выражения (1) и (2), необходимо сформировать такое устройство управления, чтобы оно обеспечивало независимость параметров привода от изменяющихся параметров моментного действия, 10
15 родвигателя;
К вЂ” моментный коэффициент электродвигателя;
При разработке этого устройства полагается, что на первый, второй, третий и шестой входы вычислительного блока 13 соответственно от датчиков 33 положения, скорости 34 и ускорения 35 второй степени подвижности и датчика 38 массы груза поступают сигналы q2, с12, ср и mr. Полагается также, что источники постоянного напряжения 36, 37,39,40 соответственно на четвертый, пятый, седьмой и восьмой входы вычислительного блока 13 подают сигналы Iz, 2R1mzr2 I +l1+lz+mzR 1 и Kö) К>1 где К „, =Kelp, К и=Ко!р, К ю — коэффициент противоЭДС электip — передаточное отношение редуктора.
Кроме того, полагается, что функциональные преобразователи 21 и 26 соответственно реализуют следуюЩие функциональные зависимости: sin u cos. Первый (co стороны источника постоянного напряжения 37) и второй (со стороны датчика 38 массы груза) положительные входы сумматора 16 имеют соответственно коэффициенты усиления 1 и
2 R1Rz. В результате на выходе функционального преобразователя 21 формируется сигнал зЬц2, на выходе сумматора 15- сигнал 2 R1(m2r2+mrR2). а на выходе блока умножения 22 — сигнал
2Й1(п12г2+п1гй2)эмап(Ц2) qz=h (см. выражение (2)).
На выходе функционального преобразователя 26 формируется сигнал cosqz, а на выходе блока 17 умножения — сигнал
2R1(m2r2+mrR2)cosq2.
Поскольку второй и третий положительные входы сумматора 27 (соответственно с выхода блока 17 умножения и источника 39 постоянного напряжения) имеют единичный коэффициент усиления, à его первый положительный вход(со стороны датчика 38 масы груза) коэффициент усиления (R 1+Й 2). то на выходе сумматора 27 формируется сигнал, равный Г+Н (см. выражение (2)), Сигнал, равный 2R1(mzr2+mrR2)sin(qz)q 2, формируется на выходе блока 23 умножения.
Поскольку первый положительный вход сумматора 18 (со стороны блока 17 умножения) имеет коэффициент усиления, равный 1/2, второй положительный вход (со стороны датчика 38 массы груза) имеет коэффициент усиления, равный R z, а третий положительный вход (со стороны источника 37 постоянного напряжения) единичный, то на выходе этого сумматора будет формироваться сигнал, P3RHbll1 Iz+fTirR 2 "Й1(п12Г2 Гп Я2)с0$ч2, 2
1754438
Очевидно, что в этом случае на выходе блока Помножив этот сигнал с помощью блока
19 умножения будет сформировай сигйал .5 умножения на сигнал с выхода усилителя
f12+m).R 2+RE(m2r2+m
К„К Я.h+K„ Ê. °,h КЬК. входы которрго соответственно имеют ко- . " h +h> т„+к> эффициенты усиленйя 1/Км и (1/(2К)и)),фор-. мируется сигнал М»/Кмм (см. выражение:(2)).
Первый отРицактелкьнкыуй вход сумматора 1О С Учетом соотношений (1) и (2) электро24 (с выхода блока 22 умножения) имеет-: привод первой степени подвижности на баКОЭффИцИЕНт уСИЛЕйИя R>R (RR — аКтИВНОЕ" ЗЕ ЭЛЕКтрсдВИГатЕЛя" ПОСТОяйНгОГО тОКа сопротивление як>орйой обмотки электро-.. независимого возбуждения или с постоян: двигателя), а второй положительный (c вы- ными магнитами достаточно точно можно хода . источника 40 постояннoro 15 описатьпередаточнойфункциейвида напряжения) — едийичныйг В результате на:,,". выходе сумматора 24 формируется сигнал; ., „, кк „„,,, „
Рлеимй Rhh+KhhK)P . УСИЛИВаЯ Етат СИГНВЯ»)::."."" Г > и, х>ГГН >h,, ГН>К. +К К. „, усилителе 25 в 1/К,К У раз, на его выходе формируется сигнал (RRh+KMK )/Куб К:, .,::::- „20
Сучетомуказаннoro íà выходе бло-:. :- - Если бы Мбит=0, то Uz=Ul,hи тогда, с
ka 9 деления формируется сигнакл. - учетом выражений (3) и (4), передаточйая (Г+H)t(R1)h+KMK ), на выходе сумматора-: функция разомкнутого привода с разрабо11, первыйотрицательныйвходкоторото(со".-: :.: таиным самонастрайвающимся устройстстороны блока 22 умножения) имеет коэф- 25 вом управления приняла бы вид: фициент усйления LR, (L< — индуктивнОсть::,. ::®(qi K /К(о якОрной обмотки двигателя), а второй поло-: ; д (Т р + 1 ) (Т2р + 1 )
Ч/(Р)= (5) жительный (со стороны сумматора 27) — коэффициент усиления RR, формируется - " :, " -Из выражения (5) йидно, что параметры . сигнал (Г+Н)йя+ЬЬ, В результате на выходе ЗО передаточной функции W(P) являются поблока 12 деления будет сформирован сиг- .:: стоянными, не зависящими от изменения нал ((Г+Н)йя+Ы я)/(М+Км иКм)) . - "параметров нагрузочныххарактеристик. ТаПринимается, что передаточная фуйк- - .=: ким образом, с помощью сформированного ция звена 29 Равна 1 -(- ) . а „ e, ... кОрректирующего УстРойства повышаетсЯ
Т )р +1 . 35 To HocTb. редаточная функция звекна 32 — Юlзз Однакосогласно выражению(2), М» 0, Р >.я .„„ .„. „ ::-::. И вто воздействие (М,х) бУдет УхУдшать точ72p + 1 .: ..., .:: ., :.: постные характеристики привода. Лоскопьжения формируется сигнал .;: " .. :::: ку.М» не зависит от фазовых координат
Г+ Н Lg 2/+ 1+ 1)) -- 40 рассматриваемого привода, можно сформиR„h + К„ K 1Р .2Р, :,.; ровать добавочный сигнал U2. компенсируУчитывая, что передаточная функций ющий отрицательное влияние моментного апериодического звена 30 равна Ч/зл(р)= воздействия М» на качественные показате1 ли:привода,, на выходе блока 3 умножения
Т2р + 1 .. °..., 5 Полагается, что передаточная функция будет получен сигнал, равный: корректирующего звена 14 имеет вид:
I+H R. +hL, (й h+й д р>(7>ак>)(7>а+1)) о °, " " 1„ +R„
Поскольку передаточная функция апе- т., ь> риодическогд звена второго порядка 31 равна 50 где Тз- некоторая, достаточно малая, постоз1 г Т + 1 ) (Т2 + 1 ), то на выходе янная времени. этОгО звена формируется .сигнал То да сигнал 132 принимае вид: д /(Т1р+1ф2р+1), а на выходе сумматора 4— сигнал . ) ) я + RR М . (6)
55 ккк>ь;, шх>ьккь.., ">ú.+ > а,л+кк + айл+к. к (т1, +1)(т2, + ) ) Поскольку для электродвигателей постоянного тока справедливо соотношение
1754438
0з =(Г+H)q<+hq>+M >, (7)
Lap + Rs то с учетом того, что Ua=U>+U2, а также выражений (6) и P), будем иметь
К (01+ 2) ™ =(! +H)q>+hq<+M =
LAP + РЯ
U1 Км + Мвн ., (8) саяр +Йя Тзр+1
Из выражения (8) видно, что, если величина Тз достаточно мала, то с помощью дополнительного сигнала U2 с достаточно высокой степенью точности удается компенсировать и отрицательное влияние М н на качественные показатели рабаты привода рассматриваемой степени подвижности робота.
Таким образом, применение нового вычислительного блока 13 вместе с корректирующим звеном 14.и четвертым сумматором I5 позволило достичь высокой точности управления указанным приводом манипулятора типа SR, ТУР-2,5 КМ и Гранат-2,5.
Формула изобретения
Устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, первый блок коррекции, первый блок умножения, второй сумматор и второй блок умножения, последовательно подключенные усилитель и электродвигатель, кинематически связанный с датчиком положения, соединенным, выходом с первым входом первого сумматора, второй вход которого подключен к первому входу устройства, последовательно соединенный первый блок деления и третий
5 блок умножения, последовательно подклю- . ченные третий сумматор и второй блок де-.— ления, выход которого соединен с вторым . входом первого блока умножения, а также" вычислительный блок, "подключенный
10 семью входами к соответственно с второго по восьмой входам устройства, первым выходом к второму входу второго блока умножения, вторым выходом - к первому входу третьего сумматора и второму входу перво- "
15 ro блока деления, третьим выходом — к вторым входам- первого и второго блоков деления, а четвертым выходом — к второму входу третьего сумматора, второй выход первого блока коррекции соединен с вто20 рым входом третьего блока умножения, подключенного выходом к третьему входу второгосумматора, отл ича ющееся тем, что, с целью повышения точности устройства, оно содержит последовательно соеди25 ненные второй блок коррекции и четвертый сумматор, второй вход которого подключен к выходу второго блока умножения, а выход — к входу усилителя, восьмой вход вычислительного блока соединен с девятым входом
30 устройства и пятый выход — с входом второго блока коррекции.
1754438
Составитель 8. Филаретов
Техред M.Моргентал Корректор О. Юрковецкая
Редактор M. Бандура
Производственно-издательский комбинат "Патент". г. Ужгород. ул,Гагарина, 101
Заказ 2852 Тираж . . . Подписное
ВНИИПИ Государственного комитета по изобретениям и откритиям при ГКНТ СССР
113035. Москва, Ж-35, Раушская наб„4/5