Устройство для управления манипулятором
Изобретение относится к робототехнике и может быть использовано нри создании дистанционных манипуляторов. Изобретение позволяет повысить надежность и быстродействие устройства. В устройстве за счет введения сумматоров, релейных элементов, элементов НЕ, элементов И, блоков умножения , усилителей, инверторов учитывается влияние конструктивных ограничений звеньев исполнительного органа манипулятора, исключается переключение всех регулируемых координат манипулятора при наличии ограничения на управляющий сигнал только по одной координате, вырабатываются сигналы непосредственной обратной связи по положению исполнительного органа манипулятора . 1 3. п. ф-лы, 6 ил. ГчЭ ;о to 00
СОЮЗ СОВЕТСКИХ
СОЦИАЛИСТИЧЕСКИХ
РЕСПУБЛИК
„„SU„:,1229728
ГОСУДАРСТВЕННЫЙ КОМИТЕТ СССР ПО ДЕЛАМ ИЗОБРЕТЕНИЙ И ОТКРЫТИЙ Ф/1g ОПИСАНИЕ ИЗОБРЕТЕНИЯ 4Д К ABTOPCHOMV СВИДЕТЕЛЬСТВУ (21) 3788536/24-24 (22) 13.07.84 (46) 07.05.86. Бюл. № 17 (71) Николаевский ордена Трудового Красного Знамени кораблестроительный институт им. адм. С. О. Макарова (72) Ю. П. Кондратенко, В. Л. Тимченко и А. Н. Ткаченко (53) 62-50(088.8) (56) Авторское свидетельство СССР № 553101, кл. В 25 J 13/00, 1975. Авторское свидетельство СССР № 633725, кл. В 25 J 13/00, 1976. (54) УСТРОЙСТВО ДЛЯ УПРАВЛЕНИЯ МАНИПУЛЯТОРОМ (57) Изобретение относится к робототехнике и может быть использовано при создании дистанционных манипуляторов. Изобретение позволяет повысить надежность и быстродействие устройства. В устройстве за счет введения сумматоров, релейных элементов, элементов НЕ, элементов И, блоков умножения, усилителей, инверторов учитывается влияние конструктивных ограничений звеньев исполнительного органа манипулятора, исключается переключение всех регулируемых координат манипулятора при наличии ограничения на управляющий сигнал только по одной координате, вырабатываются сигналы непосредственной обратной связи по положению исполнительного органа манипулятора. 1 з. п. ф-лы, 6 ил. 1229728 Изобретение относится к робо.!.Отехнике и может быть использовано при создании устройств для управления дистанционными манипуляторами. Цель изобретения -- повышение быстродействия и надежное и устройства за счет учета конструктивных ограничений и ограничений ио скорости, налагаемых иа звенья манипулятора. На фиг. 1 приведена фуHKII,HoHH;IHHBH схема устройства для управления манипулятором; на фиг. 2 — — кинематическая схема манипулятора; на фиг. 3 — — функциональная схема блока вычисления текущих координат;.на фиг. 4 — статическая характеристика второго усилителя; на фиг. 5 — статическая характеристика третьего усилителя; на фиг. б — статическая характеристика релейных элементов. Схема содержи.г блок 1 задания программы, задающий блок 2, блок 3 вычисления текущих координат, блок 4 управления, вычислительные блоки 5, блоки 6 ограничении, приводы 7, исполнительные механизмы 8, датчики 9 положения, первый 10, второй 11 и третий 12 сумматоры, инверторы 13, первый элемент НЕ 14, первый релейиыи элемент 15, четвертый 16, пятый 17, шестой 18, седьмой 19, восьмой 20 и девятый 21 сумматоры, второй элемент НЕ 22, второй релейный элеме(гг 23, элемент И 24, первые усилители 25, интеграторы 26, вторыс и третьи усилители 27 и 28, первые 29 и вторые 30 блоки умножения. На фиг. 2 указаны: с(— угловые перемещепия, rl -- Обобщенные координаты; l длины звеньев, Х, Yi - текущие координаты схвата. На фиг. 3 указаны: син сные преобразователи 31, третьи блоки 32 умножения, д(сятый сумматор 33, косииусиые преобразователи 34, четвертые блоки 35 умножения, одиннадцатый сумматор 36. Устройство для управления манипулятором работает следуlo(IJ,HxI образом. Блок 1 задания программы:(вижсния (pHнит информацию о сигналах, соответствуюЩИХ КООРДИНатаМ R l,= (R Сигналы R,. и /, на первом и втором сумматорах 10 и 1 сравнива(отея соответственно с сигналами Х, и Ут, иос P>HailolilH»IH с первых выходов блока 3 вычис.(ения Тс куIIIHx координат, соответствую(цих координатам точки, в которой находится сжат (Х,, YT). Ha выходе первого и второго сумматоров 10 и 11 формируются соответственно сигналы ЛХ и АУ, пропорциональные рассогласованию текущего (Х., Y ) и требуемого (R., К„) положений схвата. Многоканальный блок 4, иа входы которого поступают сигналы с выходов первого и второго сумматоров 10 и 11 и с выходов блока 3 вычис. lc. lIl1H текущих коор.(инат, последовательно форх(ипует yiip JI!.JsrioiuHe сигналы (cl; (n) ), 5 соответствующие желаемым иереме(цениям з(цпьев г — звеиного исполни(ельногo орI.HIJ;.J,1clH перемещения схвата в требуемук1 T0чку с координатами (R< Rg). Приводы 7 преобразуют управляющие сигналы q, (JJ) (i=1,2,..., r) в перемещение исполнительных механизмов 8, которые регистрируются с помощью датчиков 9 положения. По мере приближения схвата к требуемой точке траектории (К„К, ) выходные сигна15 лы АХ==К,— Xi и ЛУ=R, -Ут первого и второго сумматоров 10 и 11 соответственно, представляющие собой оценки точности позиционирования схвата манипулятора по координатам Х и У, уменьшаются по модулю. Сигналы ЛХ и ЛУ поступают соответственно на входы первого и второго блоков 29 и 30 умножения многоканального блока 4 и на входы релейных элементов 15 и 23 со статической характеристикой Х,. = = — f (Хв ), представляющих собой реле с регулируемой зоной нечувствительности 2е, которые переключаются при выполнении услоВий /,хХ/= е, / 1(/(в. При этом величина зоны нечувствительности релейных элементов 15 и 23 пропорциональна требуемой точности позиционирования манипулятора. Если обеспечивается треб c. xIHH точность позиционирования схвата манипулятора ио обеим координатам Х и !, то срабатывают релейные элементы 15 и 23 и подают сигналы нулевого уровня через .>леме(пы Нl.: 14 и 22 на вход элемента з5 И 24, на выходе которого формируется тактовый импульс, поступающий на вхо,(блока 1 задания программы движения. При этом (а выходах блока 1 формируются сигналы, соответствук>щие координатам новой точки траектории движения схвата (Rx К.). 4О Регулирование требуемой го iHooTH Нои ционирования маниг(у. !ятора Осу(цествля(гся путем регулирования зоны нечувствите,1ьности релейных элемсцтов 15 и 23. Блок 3 вычисления текуnlvx координат (фиг. 3) рсliJH(ò за.(ач) ollpcде,,!(ния теку(цих координат схвата (Хр, У.) в процессе управления манипулятором. На входы блока 3 постхпаюг сигналы х. (ri — — 1) (j = 1,2....., rl, 50 которые формируются на выхо,l(I датчика 9 положения первого звена исполнительного органа. так как а(= cll (фиг. 2J, и на выходах сумматоров 21 (фиг. 1), На первый вход каждогo из которых поступает сигнал 55 q (TI--1) с датчика 9 положения собственного канала, II íà ВтОрОи вхоl - сигнал с вы.ходаа сумм атора 21 и реды.(у llicli о ка цал а i Jr. 1), т. е., например, на сумматоре 21 1229728 / со, (и) / (<о;»»»» (2) При этом ы,(и) †-- к-го канала реализуется суммирование согласно выражению а (и — 1) = а f n — 1) + q» (ï — 1) (К= 2,3,...,r) . Каждый входной сигнал < (n — 1) поступает одновременно на вход соответствующих синусных преобразователей 31 и косинусных преобразователей 34 (фиг. 3), на выходах которых формируются сигналы Sin а (и — 1) и Cos cc (и — 1). Выходные сигналы блоков 31 Since; (и — 1) умножаются блоками 32 умножения с масштабным коэффициентом на соответствующий коэффициент 1;(1=1, 2, ..., г), пропорциональный длине 1-го звена г-звенногo исполнительного механизма, а результирующие сигналы E;Sinn; (n — 1) поступают на входы многовходового десятого сумматора 33. На выходе сумматора 33 формируется сигнал Х», соответствующий координате схвата манипулятора по оси Х, т. е. сигнал г Х = Х l jSiin ccrc (n — 1 ) . Выходные сигналы блоков 34 Cos с (и — I), умножаясь в блоках 35 умножения с масштабным коэффициентом на соответствующие коэффициенты I, суммируются многовходовым одиннадцатым сумматором 36, на выходе которого формируется сигнал Y., соответствующий текущей координате схвата по оси У, т. е. сигнал Y» = l> Cos а (и — 1). 1» 1 Таким образом на первом и втором выходах блока 3 вычисления текугцих координат формируются сигналы Х», Y-, соответственно на третьих — сигналы Г,Sine (п — 1), а на четвертых — сигналы 1;Сова; (n — 1) ()=1, 2, ..., г). Поскольку многоканальный управляющий блок 4, предназначенный для формирования сигналов, соответствующих желаемому перемещению звеньев исполнительного органа, состоит из каналов, имеющих одинаковое функциональное построение, то его работу рассмотрим на примере к-го канала. На первый вход первого блока 29 умножения поступает сигнал 1»Since» (и — 1) с соответствующего третьего выхода блока 3 вычисления текущих координат, а на второй вход — сигнал (R.— Х.) с выхода первого сумматора 10. Выходной сигнал первого блока 29 умножения (R — Х ) l»Яиа х x(n — 1) поступает на один из входов сумматора 12, на второй вход которого поступает сигнал (R» — У.) l»Сока»(и†1) с выхода второго блока 30 умножения, где он формируется путем умножения поступающего с выхода второго сумматора ll сигнала (R. — Y.) на сигнал l» Cos а» (n — 1), поступающий на первый вход второго блока 30 умножения с соответствующего третьего выхода блока 3 вычисления текущих координат. На выходе сумматора 12 формируется сигнал (R.— Х») E.Sinn» (n — ) — (R,— Y ) E.)< 2»,Соза» (n — 1), поступающий на вход вычис,7ительного блока 5, в частности на вход инвертора 13, который преобразуется усилителем 25 в сигнал I»= -.1» 1(И вЂ” — Х») l» Sinn " x(n — 1) — (Р— Y и!»Coscc» (n — I ) }. Сигнал l» поступает затем на вход дискретного интегратора 26, представляющего собой элемент задержки, охваченный единичной положительной обратной связью, и выходной сигнал а»(n 1) интегратора 26 подается на второй вход сумматора 16, на первый вход которого поступает сигнал, а» (и) =I» с выхода усилителя 25. При этом на выходе сумматора 16 формируется сигнал а» (n), соответствующий желаемому угловому перемещению к-го звена. Таким образом на выходах всех вычислительных блоков 5 формируются сигналы а (и) и а, (и — 1) (j= 1,2,...,r), которые в 20 первом канале поступают соответственно на входы блока 6 ограничений, а в остальных каналах соответственно на первый вход сумматора 19 и на вход сумматора 20 собственного канала, а также соответственно на входы седьмого и восьмого сумматоров 19 25 и 20 последующего канала. При этом на выходе каждого сумматора 19 формируется сигнал q> (n)= а (и) — а i (и), а на выходе каждого сумматора 20 — сигнал q, (n — I) = = а;(и — 1) — j (и — 1), т. е. сигналы, соответствующие требуемому перемещению ЗО /-го звена относительно (j 1) в и-й и (и--1) -й моменты времени, которые при этом подаются соответственно на первый и второй входы блока 6 ограничений соответствующего j-го канала (j= 1,2,....r). Посколь ку блоки 6 ограничений всех каналов имеют одинаковую структуру, то рассмотрим работу блока 6 ограничений на примере j го канала. Блок 6 ограничений предназначен для 4О формирования управляющих сигналов q, (и) с учетом конструктивных ограничений на угловые перемегцения /-го звена 4S а также на скорость углового перемещения j-го звена где t — интервал времени, за который должен обеспечиваться поворот j го звена на угол q, (n) = q; f n) — q,)(и — 1) . Сигнал q, (и) поступает на первый вход блока 6 ограничений, в частности на вход усилителя 27, имеющего статическую характеристику, приведенную на фиг. 4, и преобразу1229728 ется с учетом конструктивных ограничений (! ) j-го звена следующим образом: а ; (и) при q;««« q, (n) (q;. q, (и) = q; - при q;(n) )q;..". q; при q,fn)(q;.«« Затем в блоке ограничений осуществляется проверка условий (2). При этом выходной сигнал q;(n) с выхода усилителя?7 поступает на первый вход пятого сумматора 17, на второй вход которого поступает сиг- !О нал дДп — 1), и на выходе сумматора 17 формируется сигнал +q;) n) =: q;(n) — q; (и — 1), поступающий на вход усилителя 28, имеюгцего статическую характеристику. приведенную на. фиг. 5. Усилителем 28 сигнал q (п)преобразуется с учетом ограничений по скорости (2) следующим образом: Qq;(n), при )ю;(и) /(ь; . к,цап) 20, ".. tScgn (q; (n) ), при /ю, (пф 0> «а«с (где,/ = соп$/. Выходной сигнал д, (т!) третьего усил ителя 28 суммируется в сумматоре 18 с сигналом ц(п -!), поступающим с входа сум- 25 матора 16, и при этом на выходе блока 6 ограничений формируется сигнал, соответствующий желаемому перемещению j-го звена относительно (j-! )-го, т. е. сигнал Ц (П), l1PH /0)t (П)/ ((О;«««« q; fnJ, (n — 1) + 0>r« -.. Q t Si g n (D q; (и ) ), при (сoj (и) ()ю,Обратная связь в устройстве для управления манипулятором осуществляется с помогцью датчиков 9 положения, установленных на каждом звене исполнительного органа. Устройство обеспечивает управление исполнительным органом манипулятора, содержащим только звенья качания, т. е. работающим в плоскости. Формула изобретения 1. Устройство для управления манипулятором, содержашее блок задания программы, блок вычисления текугцих координат и по каждой регулируемой координате привод, связанный через исполнительный механизм с датчиком положения, отличаюи ееся тем, что, с целью повышения быстродействия и надежности, оно содержит последовательно соединенные первый сумматор, первый релейный элемент, первый элемент HE и элемент И, последовательно подключенные второй сумматор, второй релейный элемент и второй элемент HE и по каждой регулируемой координате первый блок умножения и последовательно соединенные второй блок умножения, третий сумматор, инвертор, первый усилитель, интегратор и четвертый сумматор и последовательно подключенí6)е второй усилитель, пятый суммагор, третий усилитель и шестой сумматор, а также по каждой регулируемой координате, кроме первой, седьмой, восьмой и девятый сумматоры, выход второго элемента HE соединен с вторым входом элемента И. выход которого подключен к входу блока задания прсграммы, соединенного первым и вторым выходом с первым входом первого и второго сумматора соответственно, второй вход которых подключен соответственно к первому и второму выходам блока вычисления текуших координат, а выходы к первому входу каждого первого и второго блока умножения, третьи выходы блока вычисления текуших координат соединены с вторым входом каждого первого блока умножения, етвертые выходы с вторым входом каждого второго блока умножения, а выходы— с выходс м соответствующего датчика положения, причем первой регулируемой координаты непосредственно, а остальных координат через девятый сумматор, выход каж дого четвертого сумматора соединен с первым входом седьмого сумматора последуюгцей регулируемой координаты и входом второго усилителя, причем первой регулируемой координаты непосредственно, а остальных регулируемых координат через седьмой сумматор его второму входу, выход каждого интегратора подключен к первому входу восьмого сумматора последующей регулируеvой координаты и к второму входу пятого и шестого сумматоров, причем первой регулируемой координаты непосредственно, а остальных регулируемых координат через восьмой сумматор по его второму входу, выход:<аждого первого блока умножения соединен с вторым входом соответствующего третьего сумматора, выход каждого шестого сумматора подключен к входу соответствуюгцего привода, выход каждого датчика положения соединен с вторым входом девягого сумматора последующей регулируемой координаты, а выход каждого первого усилителя подключен к второму входу соответствуюгцего четвертого сумматора. 2. Устройство по п. 1, отличаюи ееся тем, что блок вычисления текущих координат содержит первый и второй сумматоры и по количеству регулируемых координат последовательно соединенные синусный преобразователь и третий блок умножения, а также последовательно подключенные косинусный преобразователь и четвертый блок умножения, входы блока вычисления текущих координат соединены с входами соответствующих синусных и косинусных преобразователей, первый и второй выходы — с выходами десятого и одиннадцатого сумматора соответственно, третьи выходы — с выходами третьих блоков умножения и входами десятого сумматора, а четвертые выходы — с выходами четвертых блоков умножения и входами одиннадцатого сумматора.! 229728 122 372" Редактор С. Патрушева Заказ 2228/47 COCT IBHTC7h Е. 11о.BHTOf3 Техред И. Верес Корректор Т. Колб Тираж 8;56 Подписное ВНИИПИ Государственного комитета СССР по делам изобретений и открытий 113035, Москва, Ж вЂ” 35, Раушская наб., д. 4/5 Филиал ППП «Патент», г. Ужгород, ул. Проектная, 4
