Устройство для управления манипулятором Советский патент 1986 года по МПК G05B19/00 

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

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

На фиг. 1 приведена функциональная схе.ма устройства для ун)авления манипулятором; на фиг. 2 - кинематическая схема манипулятора; па фиг. 3 - функциональная схема блока вычисления текущих координат;.на фиг. 4 - статическая характеристика второго усилителя; на фиг. 5 - статическая характеристика третьего усилителя; па фиг. 6 - статическая характеристика релейных эле.ментов.

Схема содержит блок 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 указаны: к - угловые пере- меп1ения, q --- обобщеппые коордипаты; / длины звеньев, Хт, YT - текундие координаты схвата.

На фиг. 3 указаны: синусные преобрази- ватели 31, третьи блоки 32 умножения, десятый сумматор 33, косинусные преобразователи 34, четвертые блоки 35 умножения, одиннадцатый сумматор 36.

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

Блок 1 задания программы движения хранит информацию о сигналах, соответствующих координатам /.pj (R,j, Ry,) траектории движения схвата (/ l,2,..yV , где Л -- число точек траектории), которые носледова- тельно появляются на перво.м (Rxi) и вто- ро.м (Ryj) выходах блока 1 задания програм- .мы при подаче тактового импульса па его вход с выхода задающего блока 2.

Сигналы и Ry на первом и втором сумматорах 10 и 11 сравниваются соотвот- ственно с сигналами XT и YT, поступаюпшми с первых выходов блока 3 вычисления те- куншх координат, соответствующих координатам точки, в которой пахо.:1,ится схват (Хт, YT). На выходе первого и второго торов 10 и 11 формируются соответственно сигналы ЛХ и ДУ, пропорциональные рассогласованию текущего (Хт, У ) и требуемого (RN, R,/) положений схвата. Многоканаль

5

0

.

5 0

5

5

0

ный блок 4, на входы которого поступают сигналы с выходов первого и второго еумма- торов 10 и 11 и с выходов блока 3 вычис- .лення текущих координат, последовательно формирует управляющие сигналы ((), соответствуюн1ие желаемым пере. е1цениям звеньев г - звенного исполнительного органа для перемещения схвата в требуемую точку с координатами ( Rj.

Приводы 7 преобразуют управляющие сиг- палы q,n (,2,..., г) в перемещение исполнительных механизмов 8, которые регистрируются с помощью датчиков 9 положения. По мере приближения схвата к требуемой точке траектории (R, R,,) выходные сигналы - Xi и R,, - Ут первого и второго сумматоров 10 и 11 соответственно, представляющие собой оценки точности позиционирования схвата манипулятора по координатам X и У, уменьшаются по модулю. Сигналы .Х и ДУ поступают соответственно па входы первого и второго блоков 29 и 30 умножения многоканального блока 4 и на входы релейных элементов 15 и 23 со статической характеристикой Хвых f(Xfcc), представляющих собой реле с регулируемой зоной нечувствительности 2е, которые переключаются при вьп1о;1нении УСЛОВИЙ , .

При этом величина зоны нечувствительности релейных элементов 15 и 23 пропорциональна требуемой точности позиционирования манипулятора. Если обеспечивается требуемая точность позициони)овапия схвата манипулятора по обеим координатам X и У, то срабатывают релейные :злементы 15 и 23 и подают сигналы нулевого уровня через -элементы НЕ 14 и 22 на вход элемента И 24, на вььходе которого формируется тактовый импульс, поступающий на вход блока 1 задания нрограммы движения. При этом на выходах блока 1 формируются сигналы, соответствующие координатам повой точки траектории движепия схвата (R, R,,).

Регулирование требуемой точности позиционирования манипулятора осу|цествля- ется путем регулирования зопы нечувствительности релейнЕях элементов 15 и 23.

Б.ток 3 вычисления текущих координат (фиг. 3) решает :1адачу определения текущих координат схвата (Xtj., У:) в процессе управления манипулятором. На входы блока 3 посп Упают сигпалы

«Н

(j 1,2,..., п.

которые формируются на выходе датчика 9 положепия первого звена испол1 ительного органа, так как а. qi (фиг. 2), и на выходах сумматоров 21 (фиг. 1), на первый вход каждого из которых поступает сигнал (п-- - с датчика 9 положения собственного канала, а па второй вход - сигнал с выхода сумматора 21 предыдунито кана.ча як I п - 1|, т. е., например, на сумматоре 21

к-го канала реализуется суммирование согласно выражению

1 ос55ч1«-1} + (/(2,3,...,л).

Каждый входной сигнал oCj л-1 посту- пает одновременно на вход соответствующих синусных преобразователей 31 и косинусных преобразователей 34 (фиг. 3), на выходах которых формируются сигналы Sin и Cos a. 1.

Выходные сигналы блоков 31 Since, п-1 умножаются блоками 32 умножения с масштабным коэффициентом на соответствующий коэффициент f/(, 2, ..., г), пропорциональный длине j-ro звена г-звенного исполнительного механизма, а результирую- щие сигналы E/Sincc/ п- 1 поступают на входы многовходового десятого сумматора 33. На выходе сумматора 33 формируется сигнал XT, соответствующий координате схвата манипулятора по оси X, т. е. сигнал

г

Хт Е liSinai In- 11.

l

Выходные сигналы блоков 34 Cos оу л -1 умножаясь в блоках 35 умножения с масштабным коэффициентом на соответствующие коэффициенты I , суммируются много- входовым одиннадцатым сум.матором 36, на выходе которого формируется сигнал YT, соответствующий текущей координате схвата по оси Y, т. е. сигнал

YT Z / Cos а,- п- 1 .

Таким образом на первом и втором выходах блока 3 вычисления текущих координат формируются сигналы Хт, Ут соответственно на третьих - сигналы F/Sina п-1, а на четвертых - сигналы f,Cosa/ n-1 (,2, ..., г).

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

На первый вход первого блока 29 умножения поступает сигнал 1к51пак п-1 с соответствующего третьего выхода блока 3 вычисления текущих координат, а на второй вход - сигнал () с выхода первого сумматора 10. Выходной сигнал первого блока 29 умножения ( Хт) поступает на один из входов сумма- тора 12, на второй вход которого поступает сигнал () /KCosaK /i-1 с выхода второго блока 30 у.множения, где он формируется путем умножения поступающего с выхода второго сумматора 11 сигнала () на сигнал /к Cos ак «-1, поступающий на первый вход второго блока 30 у.множения с соответствующего третьего выхода блока 3 вычисления текущих координат.

г

0

5

0

5

О

o 5

На выходе сумматора 12 формируется сигнал (R;f-Хт) - к51пак п-1 -(R/-У,)ЕкХ ХСозак п-1 . поступающий на вход вычислительного блока 5, в частности на вход инвертора 13, который преобразуется усилителем 25 в сигнал {() /к 5шоскХ -1 - ()-/кСо5ак п 1 }.

Сигнал /к поступает зате.м на вход дискретного интегратора 26, представляющего собой элемент задержки, охваченный единичной положительной обратной связью, и выходной сигнал сек л-1 интегратора 26 подается на второй вход сумматора 16, на первый вход которого поступает сигнал Дак с выхода усилителя 25. При этом на выходе сум.матора 16 формируется сигнал , соответствующий желаемому угловому перемещению к-го звена.

Таким образом на выходах всех вычислительных блоков 5 формируются сигналы и а,п-1 (/ 1,2,...,г), которые в первом канале поступают соответственно на входы блока 6 ограничений, а в остальных каналах соответственно на первый вход сумматора 19 и на вход сумматора 20 собственного канала, а также соответственно на входы седьмого и восьмого су.мматоров 19 и 20 последующего канала. При этом на выходе каждого сумматора 19 формируется сигнал ОС, , а на выходе каждого сумматора 20 - сигнал , л - 1 - aj-i /г-I, т. е. сигналы, соответствующие требуе.мому пере.мещению /-ГО звена относительно (/-1) в л-й и (/7 - 1)-и моменты времени, которые при этом подаются соответственно на первый и второй входы блока 6 ограничений соответ- ствуюпаего j-ro канала (/ 1,2,...,г). Посколь ку блоки 6 ограничений всех каналов имеют одинаковую структуру, то рассмотрим работу блока 6 ограничений на примере /-ГО канала.

Блок 6 ограничений предназначен для формирования управ, яющих сигналов q,n с учетом конструктивных ограничений на угловые перемещения /-го звена

(//«ин (7/мзкс,(Г)

а также на скорость углового перемещения /-ГО звена

Ш, л / Ыр акс

(2)

При этом ш,п g. ,

где Л - интервал времени, за который должен обеспечиваться поворот j-ro звена на угол (,л - (,л-1. Сигнал q, п поступает на первый вход блока 6 ограничений, в частности на вход усилителя 27, имеющего статическую характеристику, приведенную на фиг. 4, и преобразуется с учетом конструктивных ограничений (1) /-ГО звена следующим образом:

9,-м„,,л ,..акс.

умакспри )

(/ иипри

Затем в блоке ограничений осуществляется проверка условий (2). При этом выходной сигнал ( с выхода усилителя 27 поступает на первый вход пятого сумматора 17, на второй вход которого поступает сиг- нал , и на выходе сумматора 17 формируется сигнал Л(/Г« : (, поступающий на вход усилителя 28, имеющего статическую характеристику, приведенную на. фиг. 5.

Усилителем 28 сигнал q п| преобразуется с учетом ограничений по скорости (2) следующим образом:

, при / OJpi iKC ;«пкс.д/5гя«(), при /(i)

03, ч а КС

где Л const.

Выходной сигнал ,т11 третьего усилителя 28 суммируется в сумматоре 18 с сигналом , поступающим с входа сум- матора 16, и при этом на выходе блока 6 ограничений формируется сигнал, соответствующий желаемому перемещению /-го звена относительно (/-1)-го, т. е. сигнал

, при OJ/макс

qi п- + о},макс. д tSign (л / л 1),

при |СО/ л I (jJ/vaKc.

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

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

Формула изобретения

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

0

5

0

5

5

0

четвертый сумматор и последовательно подключенные второй усилитель, пятый сумматор, третий усилитель и шестой сумматор, а также но каждой регулируемой координате, кроме первой, седьмой, восьмой и девятый сумматоры, выход второго элемента НЕ соединен с вторым входом элемента И, выход которого подключен к входу блока задания программы, соединенного первым и вторым выходом с первым входом первого и второго сумматора соответственно, второй вход которых подключен соответственно к первому и второму выходам блока вычисления текупдих координат, а выходы - к первому 13ХОДУ каждого первого и второго блока умножения, третьи выходы блока вычисления текупдих координат соединены с вторым входо1и каждого первого блока умножения, четвертые выходы - с вторь М входо.м каждого второго блока умножения, а выходы - с выходом соответствующего датчика положения, причем первой регулируемой координаты непосредственно, а остальных координат через девятый сумматор, выход каж дого четвертого сум.матора соединен с первым входом седь.мого сумматора последую- П1ей регулируемой координаты и входом второго усилителя, причем первой регулируемой координать непосредственно, а остальных регулируемых-- координат через седьмой сумматор его второму входу, выход каждого интегратора подключен к первому входу восьмого сумматора последую1цей регу;гируе- мой координаты и к второму входу пятого и щестого сумматоров, причем первой регулируемой координаты непосредственно, а остальных регулируемых координат через восьмой сумматор по его второму входу, выход каждого первого блока умножения соединен с вторым входом соответствующего третьего сумматора, выход каждого шестого сумматора подключен к входу соответствующего привода, выход каждого датчика положения соединен с вторым входом девятого сумматора последующей регулируемой координаты, а выход каждого первого усилителя подключен к второму входу соответ- ствуюп1.его четвертого сумматора.

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

J

Jo

J5

Cpuz.Z

5ЫХ

Si

b/Jf

/ftaf/t

Sx

ex

a.

Изобретение относится к робототехнике и может быть использовано нри создании дистанционных манипуляторов. Изобретение позволяет повысить надежность и быстродействие устройства. В устройстве за счет введения сумматоров, релейных элементов, элементов НЕ, элементов И, блоков умножения, усилителей, инверторов учитывается влияние конструктивных ограничений звеньев исполнительного органа манипулятора, исключается переключение всех регулируемых координат манипулятора при наличии ограничения на управляющий сигнал только по одной координате, вырабатываются сигналы непосредственной обратной связи по положению исполнительного органа манипулятора. 1 3. п. ф-лы, 6 ил. ГчЭ ;о to 00

Mufi

Фиг.

(pvz.G

Составитель E. Иолитов

Редактор С. ПатрушеваТехред И. ВересКорректор Т. Колб

Заказ 2228/47Тираж 836Подписное

ВНИИПИ Государственного комитета СССР

по делам изобретений и открытий

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

Филиал ППП «Патент, г. Ужгород, ул. Проектная, 4

/

- /гйгг

фиг.5