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

СОЮЗ СОВЕТСКИХСОЦИАЛИСТИЧЕСКИХРЕСПУБЛИК ЯО 1229728 1146 05 В 19/ ОПИСАНИЕ ИЗОБРЕТЕНИЯ 21) 3788536/24-2422) 13.07.8446) 07.05.86. Бюл.рудового Крас- льный институт Л. Тимченк о СССР1975.СССР1976. ОСУДАРСТВЕННЫЙ КОМИТЕТ СССР ПО ДЕЛАМ ИЗОБРЕТЕНИЙ И ОТКРЫТ ВТОРСКОМУ СВИДЕТЕПЬСТ(71) Николаевский ордена Тного Знамени кораблестроитеим. адм. С. О. Макарова(57) Изобретение относится к робототехнике и может быть использовано при создании дистанционных манипуляторов. Изобретение позволяет повысить надежность и быстродействие устройства. В устройстве за счет введения сумматоров, релейных элементов, элементов НЕ, элементов И, блоков умножения, усилителей, инверторов учитывается влияние конструктивных ограничений звеньев исполнительного органа манипулятора, исключается переключение всех регулируемых координат манипулятора при наличии ограничения на управляющий сигнал только по одной координате, вырабатываются сигналы непосредственной обратной связи по положению исполнительного органа манипулятора. 1 з. п. ф-лы, 6 ил.Изобретение относится к рннготехцике и может быть использовано при создании устройств для управления дистанционными манипуляторами.Цель изобретенияповышение быстродействия и надежност устройства за счет учета конструктивных ограничений и ограничений по скорости, налагаемых ца звенья манипулятора. На фиг. 1 приведена функциональная схема устройства для управления манипулятором; ца фиг. 2 -- кинематическая схема манипулятора; на фиг. 3 -- фуцкциональцая схема блока вычисления текущих координат;.на фиг. 4 - статическая характеристика второго усилителя; на фиг. 5 - статическая характеристика третьего усилителя; на фиг. б - статическая характеристика релейных элементов.Схема содержиг блок 1 задания программы, задающий блок 2, .ок 3 вычисления текущих координат, блок 4 управления, вычислительные блоки 5, блоки 6 ограничений, приводы 7, исполнительные механизмы 8, датчики 9 положения, первый 1 О, второй 11 и третий 12 сумматоры, ицверторы 13, первый элемент НЕ 14, первый релейцыи элемент 5, четвертый 16, пятый 17, шееой 18, седьмой 19, восьмой 20 и девятый 21 сумматоры, второй элемент НЕ 22, второй релейный элемент 23, элемент И 24, первые усилители 25, интеграторы 26, вторыс и третьи усилители 27 и 28, первые 29 и вторые 30 блоки умножения.На фиг. 2 указаны: я - угловые перемещепия, 7 . - обобщенные координаты; 1 длины звеньев, Х, у текущие коордицаты схвата.На фиг. 3 указаны: синусцые преобразователи 31, третьи блоки 32 умножения, десятый сумматор 33. косицусцые преобразователи 34, четвертые блоки 35 умножения, одиннадцатый сумматор 36.Устройство для управления манипулятором работает с;едуоцих образом.Блок 1 задания программы движения хранит информацию о сигналах, соответствующих координатам /= (Р,;, й,) траектории движения схвата (/= 1,2, Л, гае Х - число точек траектории), которые последовательно появляются на первом (Р,) и втором (Яц,) выходах блока 1 задания программы при подаче тактового импульса на ео вход с выхода задаюцего блока 2.Сигналы Й,. и /, на первом и втором сумматорах 10 и 1 сравниваотея соответственно с сигналами Х,и Ът, гоступаюпими с первых выходов блока 3 вычис.ения текуцих коордицат, соответствуюцих координатам точки, в которой находится схнат (Хт, ). На выходе первого и второго сумматоров 10 и 11 формируются соответственно сигналы ЛХ и Лу, пропорциональные рассогласованию текущего (Х., У ) и требуемого (К, К) положений схвата. Многоканальный блок 4, ца входы которого поступают сигналы с выхдов первого и второго сумматоров 10 и 1 и с выходов блока 3 вычис, е гя теку щих коорди нат, последов ател ьно форхипует управлякцие сигналы (о; п ), 5соответствующие желаемым перемецениям звеньев г - звецного исполниельного ар.апа для перемецения схвата в требуемук точку с координатами (й, Яу).Приводы 7 преобразуют управляющие сиг палы с(, и (=1,2 г) в перемещение исполнительных механизмов 8, которые регистрируются с помощью датчиков 9 положения.По мере приближения схвата к требуемой точке траектории (КК, ) выходные сигналы ЛХ== К, - Х и Лу = К - ут первого и второго сумматоров 10 и 11 соответственно, представляющие собой оценки точности позиционирования схвата манипулятора по координатам Х и , уменьшаются по модулн. Сигналы ЛХ и Л поступают соответственно ца входы первого и второго блоков 29 и 30 умножения многоканального блока 4 и на входы релейных элементов 15 и 23 со статической характеристикой Х = - 1(Хв ), представляющих собой реле с регулируемой зоной нечувствительности 2 е, которые переключаются при выполнении усло- Ви Й /ЛХ/ = е, /Л 1 / (в.При этом величина зоны нечувствительности релейных элементов 15 и 23 пропорциональна требуемой точности позиционизо рова ни я манипулятора. Если обеспечиваетсятребуемая точность позиционирования схвата чаципутто 1 по обеим координатам Х и У, то срабатывают релейные элементы 15 и 23 и одают сигналы цулево:о уровня через .лехецы Н 1 14 и 22 на вход элемента 35 И 24, ца выходе которого формируется тактовй импульс, поступающий на вход блоказадания программы движения. При этом на выходах блока 1 формируются сигналы, соответствукщие координатам новой точки траектории движения схвата (К, К,).4 О Регулирование требуемой гности позиционирования манипулятора осуцествляется путем регулирования зоны нечувствительности релейных элементов 15 и 23.Блок 3 вычисления текуцих координат(фиг, 3) репа.т зада у предеения текуцих координат схвата (Х, ,) в процессе упразлеция манипулятором. На входы блока 3 поступакт сипалы.х и - - 1 ( = 1,2 г 1,50которые формируются на выхо,е датчика 9 положения первого звена исполнительного органа, так как а= д (фиг. 2), и на выходах сумматоров 21 (фиг. 1), ца первый вход каждого из которых поступает сигнал 55 7.п 1 с датчика 9 положения собственного канала, и на второй вход - сигнал с выхода сумматора 21 цреды;уще о канала . и 1, т. е., например, на сумматоре 21/ со, и / (со; к-го канала реализуется суммирование согласно выражениюа п - 1 = м- п - 13+ Ч и - 11(К= 2,3г).Каждый входной сигнал К и - 1 поступает одновременно на вход соответствующих синусных преобразователей 31 и косинусных преобразователей 34 (фиг, 3), на выходах которых формируются сигналы 5 и аа - 1 и Соя а и - 1.Выходные сигналы блоков 31 %па; и - 1 умножаются блоками 32 умножения с масштабным коэффициентом на соответствующий коэффициент 1;(1=1, 2, , г), пропорциональный длине 1-го звена г-звенного исполнительного механизма, а результирующие сигналы 1;%па; п - 1 поступают на входы многовходового десятого сумматора 33. На выходе сумматора 33 формируется сигнал Х, соответствующий координате схвата манипулятора по оси Х, т. е. сигналгХ = Х /5 ща/л - 1.Выходные сигналы блоков 34 Соя с п - 11, умножаясь в блоках 35 умножения с масштабным коэффициентом на соответствующие коэффициенты 1, суммируются многовходовым одиннадцатым сумматором 36, на выходе которого формируется сигнал У, соответствующий текущей координате схвата по оси у, т. е. сигналгу =1 Соя а п - 1.р 1Таким образом на первом и втором выходах блока 3 вычисления текущих координат формируются сигналы Х, Ут соответственно на третьих - сигналы Г,Я 1 пм и - 1, а на четвертых - сигналы 1;Соы; п - 1 (1=1, 2, , г).Поскольку многоканальный управляющий блок 4, предназначенный для формирования сигналов, соответствующих желаемому перемещению звеньев исполнительного органа, состоит из каналов, имеющих одинаковое функциональное построение, то его работу рассмотрим на примере к-го канала.На первый вход первого блока 29 умножения поступает сигнал 15 па п - 1 с соответствующего третьего выхода блока 3 вычисления текущих координат, а на второй вход - сигнал Я - Х.) с выхода первого сумматора 10. Выходной сигнал первого блока 29 умножения (Р - Х) Ьйпа х хп - 1 поступает на один из входов сумматора 12, на второй вход которого поступает сигнал (Ру - У.) 1 Сокаа в 1 с выхода второго блока 30 умножения, где он формируется путем умножения поступающего с выхода второго сумматора1 сигнала (Я. - Б) на сигнал 1 Соя а и - 1, поступающий на первый вход второго блока 30 умножения с соответствующего третьего выхода блока 3 вычисления текущих координат. На выходе сумматора 12 формируется сигнал (й, - Х) Г 5 па п -- (К - Ь) Р)(2,Сова. и - 1, поступающий на вход вычислительного блока 5, в частности на вход инвертора 13, который преобразуется усилителем 25 в сигнал У= -1 1(И -- Х) 5 паххп - 1 - (Р - Б) /Сояап - 1 ),Сигнал У поступает затем на вход дискретного интегратора 26, представляющегособой элемент задержки, охваченный единичО ной положительной обратной связью, и выходной сигнал ап 1 интегратора 26 подается на второй вход сумматора 16, на первыйвход которого поступает сигнал,а и =Ус выхода усилителя 25. При этом на выходесумматора 16 формируется сигнал ап,соответствующий желаемому угловому перемещению к-го звена.Таким образом на выходах всех вычислительных блоков 5 формируются сигналыа а и я, а - 1, (/= 1,2г), которые в20первом канале поступают соответственно навходы блока 6 ограничений, а в остальныхканалах соответственно на первый вход сумматора 19 и на вход сумматора 20 собственного канала, а также соответственно навходы седьмого и восьмого сумматоров 1925 и 20 последующего канала. Г 1 ри этом навыходе каждого сумматора 19 формируетсясигнал д п= ап - ап, а на выходекаждого сумматора 20 - сигнал д, п - 1 == я;и - 1 -и - 1, т. с. сигналы, соответствующие требуемому перемещениюЗО /-го звена относительно (/ 1) в и й и(и 1) -й моменты времени, которые приэтом подаются соответственно на первый ивторой входы блока 6 ограничений соответствующего)-го канала = 1,2г). Поскольку блоки 6 ограничений всех каналов имеют одинаковую структуру, то рассмотримработу блока 6 ограничений на примере/-го канала. Блок 6 ограничений предназначен для 4 О формирования управляющих сигналов д, ис учетом конструктнвных ограничений на угловые перемегцения /-го звена 45 а также на скорость углового перемещения -го звена При этом ы,п 1 в - ф где- интервал времени, за который должен обеспечиваться поворот 1-го звена н а угол 2 д, и = о; и - д,фг - 1 . Сигнал 55 д, и поступает на первый вход блока 6ограничений, в частности на вход усилителя 27, имеющего статическую характеристику, приведенную на фиг. 4, и преобразу 1229728ется с учетом конструктивных ограничений(1) -го звена следующим образом: д ; и) при д;д, п (д;.;кд, п = д; - при дДп) )д; ак.5при д,п) (д,мкЗатем в блоке ограничений осуществляется проверка условий (2). При этом выходной сигнал д,п) с выхода усилителя 7 поступает на первый вход пятого сумматора 17, на второй вход которого поступает сиг- О нал д п - 1), и на выходе сумматора 17 формируется сигнал кд;1 П =: дДП) - д; и - 1, поступающий на вход усилителя 28, имеющего статическую характеристику, приведенную на фиг. 5.Усилителем 28 сигнал с 1 (ппреобразуется с учетом ограничений по скорости (2) следующим образом:Лд; сп, при )ю;(и (соак; Ьд 1 п 20-согде Ь = соИ.Выходной сигнал 2 д, т третьего усилителя 28 суммируется в сумматоре 18 с сигналом д и -, поступающим с входа сум матора 1 б, и при этом на выходе блока б ограничений формируется сигнал, соответствующий желаемому перемещению (-го звена относительно (-)-го, т. е. сигнал д и , пРи /2 ы 22) / (щ макс 30 д 2 пи 1+ сс макс, /с 52 дп (д. (и) ПрнСО (П) ( )щ макс Обратная связь в устройстве для управления манипулятором осуществляется с по- З 5 мосцью датчиков 9 положения, установленных на каждом звене исполнительного органа.Устройство обеспечивает управление исполнительным органом манипулятора, содержащим только звенья качания, т. е. работающим в плоскости. Формула изобретения 1. Устройство для управления манипулятором, содержашее блок задания про граммы, блок вычисления текугцих координат и по каждой регулируемой координате привод, связанный через исполнительный механизм с датчиком положения, отличаю 2 иееся тем, что, с целью повышения быстродействия и надежности, оно содержит последовательно соединенные первый сумматор, первый релейный элемент, первый элемент НЕ и элемент И, последовательно подключенные второй сумматор, второй релейный элемент и второй элемент НЕ и по каждой регулируемой координате первый блок умножения и последовательно соединенные второй блок умножения, третий сумматор, инвертор, первый усилитель, интегратор и четвертый сумматор и последовательно подключенные второй усилитель, пятый сумматор, третий усилитель и шестой сумматор, а также по каждой регулируемой координате, кроме первой, седьмой, восьмой и девятый сумматоры, выход второго элемента НЕ соединен с вторым входом элемента И, выход которого подключен к входу блока задания прсграммы, соединенного первым и вторым выходом с первым входом первого и второго сумматора соответственно, второй вход которых подключен соответственно к первому и второму выходам блока вычисления текуших координат, а выходы к первому входу каждого первого и второго блока умсгожения, третьи выходы блока вычисления текуших координат соединены с вторым входом каждого первого блока умножения, четвертые выходы с вторым входом каждого второго блока умножения, а выходы - с выходом соответствующего датчика положения, причем первой регулируемой координаты непосредственно, а остальных координат через девятый сумматор, выход каж дого четвертого сумматора соединен с первым входом седьмого сумматора последующей регулируемой координаты и входом второго усилителя, причем первой регулируемой координаты непосредственно, а остальных регулируемых координат через седьмой сумматор его второму входу, выход каждого интегратора подключен к первому входу восьмого сумматора последующей регулируемой координаты и к второму входу пятого и шестого сумматоров, причем первой регулируемой координаты непосредственно, а остальных регулируемых координат через восьмой сумматор по его второму входу, выход .каждого первого блока умножения соединен с вторым входом соответствуюшего третьего сумматора, выход каждого шестого сумматора подключен к входу соответствукгшего привода, выход каждого датчика положения соединен с вторым входом девятого сумматора последующей регулируемой координаты, а выход каждого первого усилителя подключен к второму входу соответ. ствусощего четвертого сумматора.2. Устройство по п. 1, отличаюи 2 ееся тем, что блок вычисления текущих координат содержит первый и второй сумматоры и по количеству регулируемых координат последовате;с 2 апо соединенные синусный преобразователь и третий блок умножения, а также последовательно подключенные косинусный преобразователь и четвертый блок умножения, входы блока вычисления текущих координат соединены с входами соответствующих синусных и косинусных преобразователей, первый и второй выходы - с выходами десятого и одиннадцатого сумматора соответственно, третьи выходы - с выходами третьих блоков умножения и входами десятого сумматора, а четвертые выходы - с выходами четвертых блоков умножения и входами одиннадцатого сумматора.1130 илиа Сост ва ТехредТираж ИИПИ Государс по делам изоб 5, Москва, Ж - ППП Патент,итонКорректор Т. КолПодписноеитета СССРткрытийя наб., д. 4/5ул. Проектная, 4