Устройство для управления приводом робота

 

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

СОЮЗ СОВЕТСКИХ

СОЦИАЛИСТИЧЕСКИХ

РЕСПУБЛИК (я)5 В 25 J 13/00

ГОСУДАРСТВЕННОЕ ПАТЕНТНОЕ

ВЕДОМСТВО СССР. (ГОСПАТЕНТ СССР) ОПИСАНИЕ ИЗОБРЕТЕНИЯ

К АВТОРСКОМУ СВИДЕТЕЛЬСТВУ (21) 4848056/08 (22) 19;06.90 (46) 23.05.93. Бюл, f+ 19 (71) Дальневосточный политехнический институт им. В.В.Куйбышева (72) В.Ф.Филаретов (56) Авторское свидетельство СССР

ЬВ 1484702, кл. В 25 J 13/00, 1987.

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

Целью изобретения является обеспечение высокой точности и устойчивости привода другой степени подвижности другого робота, На фиг.1 дана блок-схема предлагаемого самонастраивающегося электропривода робота; на фиг.2 — кинематическая схема исполнительного органа робота, Самонастраивающийся злектропривод содержит последовател ьно соединенные первый сумматор 1, первый блок 2 умножения, второй сумматор 3, усилитель 4 и двигатель 5, связанный с первым датчиком 6 скорости непосредственно и через редуктор 7 с первым датчиком 8 положения, выход которого соединен с первым отрицательным входом третьего сумматора 9, подключенного вторым положительным входом ко входу устройства, последовательно соединенные релейный блок 10 и четвертый сумматор 11, второй вход которого

„„. Ж„„1816684 А1 (54) УСТРОЙСТВО ДЛЯ УПРАВЛЕНИЯ

ПРИВОДОМ РОБОТА (57) Использование: при создании высокоточных приводов робота. Сущность изобретения: устройство дополнительно содержит датчик ускорения, два функциональных преобразователя и два блока умножения, что обеспечивает высокую точность и устойчивость привода при выбранной кинематической схеме манипулятора. 2 ил. соединен с выходом первого датчика 6 скорости и входом релейного блока 10, последовательно соединенные первый эадатчик

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

14 массы, а выход — с вторым входом первого блока 2 умножения, последовательно соединенные второй датчик 15 скорости, второй блок 16 умножения и третий блок 17 умножения, второй вход которого соединен с выходом первого датчика 6 скорости, а О выход- с третьим входом четвертого сумма- (Ь тора 11, а также второй датчик 18 положе- QQ ния, причем выход третьего сумматора 9 ф„ подключен к первому входу первого сумматора 1, второй вход которого соединен с выходом первого датчика скорости, выход четвертого сумматора 11 подключен ко второму входу второго сумматора 3. Кроме того он содержит последовательно соединенные второй задатчик 19 сигнала, шестой сумматор 20, четвертый блок 21 умножения, второй вход которого через первый функциональный преобразователь

1816684

22 соединен с выходом второго датчика 18 положения, седьмой сумматор 23, второй вход которого соединен с выходом третьего задатчика 24 сигнала, и пятый блок 25 умножения, второй вход которого соединен с выходом датчика 26 ускорения, а выход подключен к четвертому входу четвертого сумматора 11, последовательно соединенные второй функциональный преобразователь 27, вход которого соединен со входом первого функционального преобразователя

23, и шестой блок 28 умножения, в второй вход которого подключен к выходу шестого сумматора 20, а выход — ко второму входу второго блока 16 умножения, пятый вход четвертого сумматора 11 подключен к выходу седьмого блока 29 умножения, первый вход которого соединен с выходом второго датчика 15 скорости, а второй вход — с выходом второго блока 16 умножения, третий вход пятого сумматора 13 соединен с выходом четвертого блока 21 умножения, третий вход седьмого сумматора 23 подключен к выходу датчика 14 массы и второму входу шестого сумматора 20. Объект управления

30 механически соединен с выходным валом редуктора7.

На фиг.1 и фиг.2 введены следующие обозначения: авх — сигнал желаемого положения;

q>,цг,qз — соответствующие обобщенные координаты исполнительного органа робота; цг.цз; скорости изменения соответствующих обобщенных координат; е- ошибка привода (величина рассогласования);

mq,вг,ma,mr — соответственно массы первого, второго, третьего звеньев исполнительного органа и захваченного груза; (+г, +з — расстояния от осей вращения горизонтальных звеньев до их центров масс;

4,I5 — длинЬ соответствующих горизонтальных звеньев; аг — скорость вращения ротора двигателя;

0*, 0 — соответственно усиливаемый сигнал и сигнал. управления двигателем 5.

Устройство работает следующим образом. Сигнал ошибки r. сумматора 9 после коррекции в блоках 1,2,3, усиливаясь, поступает на электродвигатель 5. приводя его вал во вращательное движение с направлением и скоростью (ускорением), зависящими от величины поступающего сигнала, U. моментов трения и внешнего моментного воздействия Мв. Электропривод при работе с

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

Рассматриваемый привод управляет обобщенной координатой цг. Конструкция робота (см.фиг,2) является наиболее типовой для отечественных и зарубежных промышленных роботов (см. например, роботы типа skilam (SR-2, SR-3, SR-4), ТУР-h, гранат2,5 и т.д,). Эта конструкция позволяет осуществлять вертикальное прямолинейное перемещение груза (координата q>) и два вращательных в горизонтальной плоскости (координаты цг и q3), Моментные характеристики привода, управляющего координатой цг существенно зависят от изменения координат цз,цз,цз и

mr. В связи с этим для качественного управления координатой цг необходимо точно компенсировать отрицательное влияние изменения координат цз,цз и цз, а также переменной массы груза mr на динамические свойства рассматриваемого привода поворота (координата q2), Для определения моментных воздействий на рассматриваемый привод(обобщенных моментов неконсервативных сил) используют уравнение Лагранжа II poА .

Кинетическая энергия Т всех движущихся масс исполнительного органа (см,фиг,2) представляется в виде (г + 2 2 )Цг +

+Is + mg 1 ззг + mr i), . г .(цг+цз) +

+ 2 + (гпз Ь*+ m б) 1г со$ (цэ)х

° . (m i + %г + пауз + mr I ° г

2 где Iz,lç — моменты инерции соответствую1816684 выходной вал привода. управления координатой цг при движении робота (см.фиг.2) с грузом имеет вид щих горизонтальных звеньев относительно их центров масс.

Потенциальная энергия является функцией только координаты ql.

5 Мв=Н(цз)цг+г1(цз,цз)цг+Мвн, (1) Учитывая, что

ГДЕ H(q3)=iz+I3+mZIZ* +В2ГЗ* +(Вэ+Вг)!2 +

*2 »2 2

+ mr гэ+2гг(вэ "3+вгЫсоэ(цэ), h(q3,qç)=-24(m3t»з+вгЫэ!п (Цз)Цэ, (2)

MBHQI3+m3f3* +mrp3+(m3t,"З+.— «lz+mzh*+la+mob*+(ms+mr)b + дТ г „г 2 дцг

+ 2(взВ*з+вгЦЬсоэ(цз))цг+ (13+

+ m3b* +Вг гэ(ВЗФЗ» + mrb)ICOS(q3))q3.

15 + вггз)ггсоэцз)цз-Ь(взС»3+вЛз) х о дТ вЂ” — =(l2+l3+m2t2* +m3I3* +

*2 *2 дцг

+ Вг(23+(ВЗ+Вг)Ь2 +2(ВЗФ»З+ВгР,)ЬСОЗ(цэ)) Ср— х э1п(цэ}ц э. — 2(m3E»3+вгб)(гэ! п(цЗ)цгцэ+Рг+вэ 5* +

+ mrF3+(m3t"Э+ВгЬ)ЬСОЗ(Ц3}ЦЗ— (m3f"3+Вгб)Ы П(ЦЭ)Цгэ. дТ вЂ” =О. дц2 описа с ду w А фф р

На основе уравнений Лагранжа II рода можно записать, что моментное воздействие на управлением.

KyKMU*=R(lgl2+l3+m2t» г+вз(» 3+(вэ+в )(22+в Рз+2Ь(вэР"3+в Ысоз(цэ))/12 } аг+ВМ п,+

+(KM Kw+R(KB 212(m3l*3+mrl 3)з>п(цз)цэ/i р)) аг+ (Им Kw+R(KB-2Ь(взй 3+вгЬ) sin(цз) цэ/1р )КО+

+ R(l3+mg гз+вг гз+Ь(вз("3+mr(3)cosq3)q3/ip-RQ(m35*+mrb)sin(q3)qгз/ip=R(i+H*) rà+

+ (KMK w+R(KB+ *)) аг+ (Истр+ *Вн}

rye Н*=Н/i р, h*=h/I p. M*BH=MBH/lp.

R — активное сопротивление якорной цепи двигателя;

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

Км — коэффициент крутящегося момента;

К г — коэффициент противо-ЭДС;

KB — коэффициент вязкого трения;

Ið — передаточное отношение редуктора:

Мстр — момент сухого трения;

Ку — Коэффициент усиления усилителя 4;

l — ток якоря;

"аг — ускорение вращения вала двигателя второй степени подвижности.

Иэ уравнения (3) видно, что параметры этого уравнения, а следовательно и параметры привода, управляющего координатой

q2, являются существенно переменными, зависящими от величин цэ.цз,цз,mr В результате в процессе работы привода

С учетом соотношений (1) и (2), а также

20 уравнений электрической U=IR+Kw аг и механической

iKM=(*+I) -аг +(й*+КВ) az+McrpM*BH цепей электродвигателя постоянного тока

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

Полагается, что первый положительный вход сумматора 1 (со стороны сумматора 9) единичный, а его второй отрицательный вход имеет коэффициент усиления Kw/Ky

Следовательно на выходе сумматора 1 форK мируется сигнал е — — аг, Ку

Первый вход сумматора 20 единичный, а задатчик 19 сигнала подает на него сигнал

ЬРзвз, Второй вход этого сумматора имеет коэффициент усиления (25. В результате на выходе этого сумматора формируется сигНаЛ Ь(В33"3+ВгЬ).

1816684

10 Выходной сигнал релейного элемента 10 с нулевой нейтральной точкой имеет вид

20

Второй датчик 18 положения измеряет обобщенную координату q3 робота, а функциональный преобразователь 22 реализует соз q3. В результате на выходе блока 21 умножения формируется сигнал

b(m3f 3+ГПг(З)СО$РЗ.

Первый вход сумматора 13 имеет единичный коэффициент усиления, а задатчик

12 сигнала по2дает на этот вход2 сигнал (!+(!2+!3+Ю2г2* +m 13* +m3f2 )/!2 )/!н °

На второй его вход с коэффициентом

УСИЛЕНИЯ (l2 +IÇ )/(I р!н) ДатЧИК 14 МаССЫ ПОдает сигнал тг. Третий вход сумматора 13 имеет коэффициент усиления 2/(i р!н). В

2 результате на его выходе формируется сигнал (I+(I2+I3+m2I2* +гпзгз* +п з!2 +

*2 2

+ mr(I2 +!3 )+2(2(mg" зп гЫ х

Х COS(q3))/I р)/!н=Д, а на выходе блока 2 умножения — сигнал

А(е — — а2). Функциональный преобразоК, Ку ватель 27 реализует функциональную зависимость $!поз. В результате на выходе блока 28 умножения формируется сигнал

h(m3t"з+п1гb) slnq3, Датчик 15 скорости измеряет скорость изменения обобщенной координаты с!3 и, как и датчик 18 положения, установлен в третьей степени подвижности робота. В результате на третий отрицательный вход сумматора 11 (со стороны блока 17 умножения) с коэффициентом усиления 2/! р поступает

2 сигнал

2(m3t 3+mr(3)SIn(q3)q3 а2. а на пятый отрицательный вход этого сумматора (со стороны блока 29 умножения) с коэффициентом усиления 1/!р — сигнал

6(АЗ(3+гпгЬ)$!п(рЗ)с!23.

Первый и второй входы сумматора

23 (соответственно со стороны блока

21 умножения и задатчика 24 сигнала) имеют единичные коэффициенты усиления, а его третий вход — коэффициент усиления 23. Задатчик сигнала 24 формирует сигнал I3+m34*2, а датчик 26 ускорения измеряет ускорение изменения обобщенной координаты q3 и установлен в третьей степени подвижности робота. В результате на выходе блока 25 умножения формируется сигнал

55 (!3+ГПЗЬ* +mrP3+f2(m3(. 3+mr6)COSq3}q3 который поступает на четвертый положительный вход сумматора 11, имеющий коэффициент усиления 1/!р, Первый и второй положительные входы сумматора 11 (соответственно со стороны релейного элемента l0 и датчика 6 скорости) соответственно имеют единичный коэффициент усиления и коэффициент усиления, равный KpKw/В+Кц.

Мт а2 > 0

08ых1о= — Мт»ри а2 < 0

0 а2 = О, где I Mò! — величина момента сухого трения при движении. В результате на выходе сумматора 11 формируется сигнал (-+Кв-2$(m3f3+mrb)sin(q3)q3/i р) а2 +

+Мт$!ЯП (Х2 +(! 3+ГПЗ(" 3+mr 3+(2(m3Р 3+

+mг(3)со$ч3)с!3/ið Q(m3P3+mr6)$in(q3)/!р=B°, Первый положительный вход сумматора

3 (со стороны блока 2 умножения) имеет единичный коэффициент усиления, а его второй положительный вход — коэффициент усиления 8/(КмКу). В результате на выходе сумматора 3 формируется сигнал U*, равный

0*=А(г — — (Х2)+

K. R

=В.

* К„КмК

Поскольку при движении привода

Mrsign а2 достаточно точно соответствует

М гр, то сиг нал U* {4), как несложно убедиться, обеспечивает превращение уравнения (3) с существенно переменными параметрами в уравнение с постоянными номинальными желаемыми параметрами, обеспечивающими приводу заданные динамические свойства и качественные показатели и !н (х2 +Км К а2 =-Куку Я.

Таким образом за счет дополнительного введения второго 19 и третьего 24 задатчиков сигнала, шестого 20 и седьмого 23 сумматоров, четвертого 21, пятого 25, шестого

28 и седьмого 29 блоков умножения, первого 22 и второго 27 функциональных преобразователей и датчика 26 ускорения удалось обеспечить полную инвариэнтность привода к эффектам взаимовлияния между степенями подвижности и моментам трения. Это позволяет получить стабильно высокое ка1816684

10 чество управления в любых режимах работы привода.

Практическая реализация предлагаемого устройства не вызывает затруднений, так как в нем использованы только типовые электронные элементы, Формула изобретения

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

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

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

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

Составитель С. Грибов

Техред M,Mîpãåíòàë Корректор А. Обручар

Редактор О, Стенина

Производственно-издательский комбинат "Патент", г. Ужгород, ул.Гагарина, 101

Заказ 1703 Тираж Подписное

ВНИИПИ Государственного комитета по изобретениям и открытиям при ГКНТ СССР

113035, Москва, Ж-35, Раушская наб., 4/5