Электрогидравлический следящий привод робота Советский патент 1992 года по МПК F15B9/03 

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

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

Изобретение относится к области гидроавтоматики и может быть использовано в системах управления промышленными роботами и манипуляторами. Целью изобретения является повышение точности и устойчивости. На вход привода подается управляющее воздействие и на выходе измерителя 1 вырабатывается сигнал ошибки, который после коррекции в блоках 2-5,7,16 поступает на привод 9 регулирующего органа насоса 10. Последний, создавая поток рабочей.жидкости в гидролиниях 11,12, воздействует на гидромотор 13, вал 14 которого изменяет положение рабочего органа.

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

Цель изобретения - повышение точности и устойчивости.

На фиг. 1 изображена блок-схема электрогидравлического следящего привода робота; на фиг.2 -.кинематическая схема привода поворота относительно вертикальной оси исполнительного механизма робота.

Электрогидравлический следящий привод робота содержит последовательно соединенные измеритель 1 рассогласования, первое дифференцирующее звено 2 с замедлением, блок 3 деления, второе дифференцирующее звено 4 с замедлением, первый блок 5 умножения, первый сумматор 6, второй блок 7 умножения, первый усилитель 8, привод 9 регулирующего органа не показан) насоса 10, связанного гид- ролинйями 11 и 12 с гидромотором 13, выходной вал 14 которого кинематически связан редуктором (не показан) и исполнительным механизмом с рабочим органом (не обозначены) робота и первым датчиком 15 положения, подключенным своим выходом к второму входу измерителя 1 рассогласования, выход которого через апериодическое звено 16 второго порядка подключен к второму входу первого сумматора 6, последовательно соединенные второй сумматор 17, третий блок 18 умножения, к второму входу которого подключен выход датчика 19 усилия рабочего органа (например, схвата), и третий сумматор 20, к второму входу которого подключен выход первого источника 21 опорного напряжения, последовательно соединенные четвертый сумматор 22 и четвертый блок 23 умножения, к второму входу которого подключен выход первого датчика 24 скорости второй степени подвижности исполнительного механизма, последовательно соединенные пятый блок 25 умножения и апериодическое звено 26 первого порядка, выход которого подключен к третьему входу первого сумматора 6, а также последовательно соединенные второй источник 27 опорного напряжения, пятый сумматор 28, второй усилитель 29, выход которого подключен к второму входу второго блока 7 умножения, последовательно соединенные шестой блок 30 умножения и

второе апериодическое звено 31 первого порядка, выход которого соединен с четвертым входом первого сумматора 6, причем выход пятого сумматора 28 также соединен

с вторым входом первого блока 3 деления, выход которого подключен к первому входу пятого блока 25 умножения и первому входу шестого блока 30 умножения, второй вход которого соединен с выходом второго сумматора 20 и вторым входом первого блока 5 умножения. Кроме того, привод содержит последовательно соединенные второй датчик 32 положения второй степени подвижности исполнительного органа, шестой

сумматор 33, первый блок 34 вычисления синуса, седьмой сумматор 35, седьмой блок 36 умножения, последовательно соединенные третий датчик 37 положения третьей степени подвижности, восьмой сумматор

38, второй блок 39 вычисления синуса, девятый сумматор 40, восьмой блок 41 умножения, десятый сумматор 42, девятый блок 43 умножения Одиннадцатый сумматор 44, последовательно соединённые первый блок

45 вычисления косинуса, десятый блок 46

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

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

сумматора 17, а также к третьему входу третьего сумматора 20, последовательно соединенные третий блок 49 вычисления синуса и первый квадратор 50, выход которого подключен к четвертому входу третьего сумматора 20, последовательно соединенные четвертый блок 51 вычисления синуса и второй квадратор 52, выход которого подключен к пятому входу третьего сумматора 20, а также последовательно соединенные пятый

блок 53 вычисления синуса и двенадцатый сумматор 54, причем второй вход шестого сумматора 33 соединен с выходом третьего датчика 37 положения, второй вход восьмого сумматора 38 подключен к выходу второго датчика 32 положения, а также к выходам первого блока 45 вычисления косинуса, четвертого и пятого блоков 51 и 53 вычисления синуса, второй вход седьмого сумматора 35 подключен к выходу пятого блока 53 вычисления синуса, а третий - к выходу, второго блока 39 вычисления синуса и второму входу двенадцатого сумматора 54, третий вход которого соединен с выходом первого блока 34 вычисления синуса, а выход - с первым

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

4iЈ) W(P) + Wy(P)Wp(P)WHrM(P)WK(P),

где Р - символ дифференцирования;

W(P) - передаточная функция привода;

Wy(P) и Wp(P) - соответственно передаточные функции усилителя 8 и регулирующего органа 9 насоса 10;

WHPM(P)- передаточная функция гидропередачи, состоящей из насоса 10 и гидро- мотора 13.

Передаточная функция гидропередачи имеет вид

KWgip

q.tti«)OL,htw;;v .

Г 1 ГГ ( (нофмТа ГЯ / UEU,ti«noiw|ipf KE(L,h40iofw|;pptJ(3)

ггдеу- угол поворота органа насоса 10;

регулирующего

К

WHCfH

--.-

flnn

Ут - const - максимальный угол поворота регулирующего органа насоса 10;

КЕ const - приведенное значение модуля объемной упругости жидкости;

1р-передаточное отношение редуктора;

Wg-характерный объем гидромотора 13;

WH-характерный объем насоса 10;

Он - скорость вращения насоса 10;

V - объем рабочей жидкости в гидроЯи- нии 11 нагнетания и полости нагнетания насоса 10;

Ц.- утечки рабочей жидкости.

Из выражения (3) видно, что параметры передаточной функции WHTM(P) являются существенно переменными и зависят от Н (Q2, qa, mr). n (Q2. qa. qa, qa, mr). В результате, значительно изменяются и динамические свойства электрогидравлического следящего привода. Для сохранения динамических свойств электрогидравлического следящего привода неизменными необходимо заста- билизировать все параметры передаточной функции W(P). Так как переменные параметры содержатся только в (Р), то для стабилизации свойств всего привода в целом вид последовательного корректирующего устройства WK(P) необходимо выбрать с уче-, том вида передаточной функции Мнгм(Р).

Коррекция Л/к(Р) осуществляется с помощью апериодического звена 16 второго порядка с передаточной функцией

WK(

(Т,ОИ).(ТгР«11

I

двух дифференцирующих звеньев 2 и 4 с замедлением, причем Wa(P) и Л/4(Р) имеют

W2(P)

К«РКор

,м.

ТеРИ

гдеК1 1/КЕ;

Ka V,

10 а также апериодических звеньев 26 и 31 с передаточными функциями:

15

Т2РН

Параметры Ti и Та выбирают достаточно малыми с целью придания приводу необходимых динамических свойств. Апериодическое звено 16 второго порядка,

0 дифференцирующие звенья 24 с замедлением, а также апериодические звенья 26 и 31 включены так, как показано на фиг.1.

Для непрерывной подстройки параметров коррекции WK(P) по текущим значениям

5 Н (qa, qa, mr), h (qa, qa, qa. qa, mr) используются блоки 5, 30 и 25 умножения, на вторые входы которых подаются сигналы, пропорциональные Н + и h соответственно.

Сигналы, пропорциональные Н + I и h,

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

С помощью третьего датчика 37 третьей степени подвижности манипулятора измеряют угол qa и подают этот сигнал на второй

5 вход шестого 33 и первый вход восьмого 38 сумматоро-в. На выходе шестого сумматора 33 присутствует сигнал 2qa + qs и его подают на перый блок 34 вычисления синуса, на выходе - сигнал sin (2q2 + qa). Тогда на вы0 ходе восьмого сумматора присутствует сигнал qa + qa. Подав его на вход второго блока 39 вычисления синуса с коэффициентом 2, на выходе получают сигнал sin (qa + рз) Подав также сигнал qa + qa на выходы вто5 рого блока 47 вычисления косинуса и третьего блока 49 вычисления синуса, на их выходах получают сигналы cos (qa + qa) и sin (qa + qa) соответственно..Подав на первый вход двенадцатого сумматора 54 сигнал sin2qa с коэффициентом Isa - JNZ

9172336010

-тз(12 + )2. на третий вход - сигнал sin (2q24- i, .-. о(е. .. Ыр .,ш9 .

+ qs) с коэффициентом-2тз1з(12 + 12), а на; Чс,)(

второй вход - сигнал sin2fq2+ qs) с коэффи-+ jlco5 25((li))циенгом Iss - 1мз - тз1 3v на его выходеПодав этот сигнал на первый вход десяполучают сигнал t5того сумматора 42, на второй вход которого

(JS2 - IN2 - m2l 2 - тз02 + ) sin2qa -подают с коэффициентом -2m3l (12 + 2 )

2тз1 з(12 + I 2) sin (2q2+ qs) + sin2(q2.+ q3)(IS3 сигнал sin (q2 + q3) cos q2 с выхода десятого

-1ыз-тз1 з).блока 46 умножения, а на третий вход Подав на второй вход седьмого сумма-сигнал sin 2(qz+.qj) c коэффициентом g3 тооа 35 сигнал sin 2q2 с коэффициентом -(I24 10IMS c выхода второго блока 39 вы + I 2)2. на первый вход- сигнал sin (2q2 + рз)числения синуса, на выходе получают сигс коэффициентом -2(12 + 1г.)0з + I з). а нанал

третий вход-сигнал sin2.(q2 + q3) с коэффи-5 n((l){ 63- Ha n4 2)(22f

циентом -{1з + Гз)2, на его выходе получают+)co5Cjis :n()-2mr((.

лиТ | Гв1п2Ч2-(Ь + .пфд,), 15 (vn(eJ«q,eJ)cos1,svn(

-2(l2 + I 2)0з + з) sin (2q2 + qs).Подав этот сигнал на первый вход девяПодав этот сигнал на первый вход седь-того блока 43 умножения, на второй вход

мого блока 36 умножения, на второй входкоторого подаются сигнал qac выхода даткоторого надеется сигнал тг датчика 19, на20 чика 57 третьей степени подвижности манивыходе получают сигналпулятора, на его выходе получают сигнал .

-тг((12 )sin2q2 + Оз + ) sin2(q2+i s- n«wnsr3w.«iЈ Wsfe; e;i7-

+q3)(.2 + r2X.3 + )sin(2q2 + q3)),

Просуммировав его с сигналом с выхода /1 4 , //о г -1

двенадцатого сумматора 54 в четвертом , гтг1С г.-сг 5in2qa +

сумматоре 22, получают сигнал 5 5 Ца 1+ (С2+)(в} + К)х

-i«Vv3«.nif)WSM;K- ffit

rtmi(u+tw rinei 1u3«-V« -Prt-;:(,n(v)lv

-4U B 4;ne2 Hei+tt S n44taj4 s)i 30(,%4) .

)(ej+et|sinfeqe+o,и подают его на первый вход одиннадцатого

з v -ji T(5vсумматора 44, в результате на его выходе

Подав этот сигнал на первый вход чет-получают сигнал

. вертого блока 23 умножения, на второй вход . . ,)-

которого подается сигнал q2 с выхода датчи- 35 о /я т

ка 24 второй степени подвижности манипу- 2« e$iei « JK)a(«J.

лятора, на выходе получают сигнал . зН бз-Зну. r rU-Bj+tjlem.

q2jsin2q2i(ls2 - I N2 - - тз (fe + Vifh+((M()

-2m3l 3(l2 + )sin(2q2 + q +sin2(q2 + q3).((36 -3ie(S3-.lN3 - m3l 3) -: тг ((12 + I 2) Sin q2 + (з + 40-m fHm l/ +BJlcosq.Sinfo.+qO-

+l3)2sina(q24-q3)(l2.-fl2Xl3-M3}s n(2q2.

рз) и подают его на второй вход одиннадца-V +eS)cosai5:«fa a n.hfo / А

того сумматора 44. На первый вход десятого...Б 1СОЗ ««-«1 Я И:И{а,,.г,яв,..

блока 46 умножения подают сигнал cos q2 сСигнал cos q2 с выхода первого блока 45

выхода первого блока 45 вычисления коси- 45вычисления косинуса подают на вход четнуса, на второй вход - сигнал sin (q2 + рз) свертого квадратора 56, на выходе которого

третьего блока 49 вычисления синуса, наполучают сигнал cos2q2, а также на второй

выходе получают сигнал sin (q2 + qs) cos q2.вход одиннадцатого блока 48 умножения, на

Подают этот сигнал на второй вход девятогопервый вход которого пода ют сигнал cos (q2-f

сумматрра 40 с коэффициентом -2(12 + 1 2Х з4- 50+ qs) с выхода второго блока 47 вычисления

+ а на его первый вход подают сигналкосинуса. Сигнал cos (q2 + q) подают и на

sin2(q2 + qs) с выхода второго блока 39вход третьего квадратора 55, на выходе ков ычисления синуса с коэффициентом -2(1з+торого nofly4arot сигнал + Q)- На

)2. на выходе получают сигнал А выходе одиннадцатого блока 48 умножения

-2((l3 + 3)2sin2(q2 + q3) + l 2X 3+ l зК55получают сигнал cos Q2 cos (q2 +Јз). Подав

xcos q2 sin (q2 + qs))этот сигнал с коэффициентом 2(1 2 +12Х з+ и подают его на первый вход восьмого блока ) на первый вход второго сумматора 17, на

41 умножения, на второй вход которого под-второй вход которого подают с коэффициенают сигнал mf с выхода датчика 19, навыхо-том (I3 + l)z сигнал cos2 (qa.+ Рз). а на

де получают сигнал- ,

третий вход с коэффициентом г + 1 }2 - сигнал cos2a«, на выходе получают сигнал

(12 +,)2 COS Q2 2 02 + )(Ф® COSQ2 COS X

х(д1+дз) + (1з+ )со82(д2 + дз).

Подав его на первый вход третьего бло- 5 ка 18 умножения, на второй вход которого . подают сигнал mrc выхода датчика 19 усилия, на выходе получают сигнал.

тг( + $г cos 2 qe + 2(г + )0 j + l

cosq2cos(qz + q3) + (U+lt)eos2(q2+q3) 10 и подают его на первый вход второго сумма- тооа 20, на второй вход которого подают сигнал tsi + I2ip с первого/ источника 21

опорного напряжения, Сигнал cos q cos (q2+ + QJ) с выхода одиннадцатого блока 48 ум- 15 .ножения подают на третий вход второго сумматора 20 с коэффициентом 2(1 + IV)

1 тэ.

Сигнал sin (q + q) с выхода третьего блока 49 вычисления синуса подают на вход 20 .первого квадратора 56, на выходе которого получают сигнал sin2(q2 + q3), и подают его с коэффициентом IS3 на четвертый вход второго сумматора 20. Сигнал sin q2 с выхода четвертого блока 51 вычисления синуса под- 25 ают на вход второго квадратора 52, на выходе которого получают сигнал sinzq2. и далее подают его с коэффициентом Ј1 на пятый вход второго сумматора 20, на шестой вход которого подают с коэффициентом 30 mjjl + 1цз сигнал cos (qЈ+ q3) с выхода третьего квадратора 50, а. на седьмой вход- сигнал ) с коэффициентом 1. + . + тз( 2 + г )2 с выхода четвертого квадратора 56. В результате на выходе вто- 35 рого сумматора 20 получают сигнал

3iptV3sis;nV+3Sssina{( +-3M2co5 ci4t3H,co5e(s+o| ,

+m3((e24et)2e|coS(}2x

fCo6(qu,e5acos(qt+)+tnr fej4e|j . (et+4)(B4+e)co5 jacoe((}et()4Jf

+(ej+e«ieco5(i«H,3Jj.

Первый вход пятого сумматора 28 имеет 45 коэффициент усиления, равный единице, а второй - коэффициент, пропорциональный L3. Второй источник 27 опорного напряжения имеет на выходе сигнал, пропорциональный 0,01 W2gl2p. На выходе пятого 50 сумма-тора 28 присутствует + О,ОТ W2gl2p. который подают на блок 3 деления, Второй усилитель 29 имеет коэффициент усиления, пропорциональный величине 0,011 W2gi2p.. Второе апериодическое звено 31 55

имеет передаточную функцию , - .

г

. /: w«w.

Таким образом, с учетом вида переда- j04Hoft функции W2(P), W4(P). Wie(P),

40

W26(P), Wai(P) на выходе второго блока 7 умножения получают сигнал

(H,3%W 2 () KЈ(, р + KE-(.)pt .

(Т,РИ)(тгР+1)|OQ(.OIW|)

&.

Ю

В результате передаточная функция корректирующего устройства имеет вид

0

5

0 5

0 5

5 0 5

0

Wk(p)

(HJ%W 5 () ,otw|)p + KE(i.9h4o,)ptt

(Т,Р««У1Т«Р 1)(U9btO.OIw|ij

1,2 :1

42: fr

w|,-p

а передаточная функция прямой цепи привода имеет вид

,K/Waip

wiP)(PiwP(Pi(T|p4(;T;pHrl

t

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

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

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

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

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

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

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

«г

Фиг2