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

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

Целью изобретения является повышение точности позиционирования манипулятора.

Точность позиционирования можно повысить за счет отработки программного движения не только по положению, но и по скорости и ускорению. Такой поДход может быть реализован следующим образом. Вычисляются начальная qio и конечная qu обобщенные координаты первого звена манипулятора по следующим формулам:

Хо

qio arctg().

Хк

qiK-arctg (),

(1)

(2)

где Хо. Уо и Хк.Ук - соответственно начальные и конечные.декартовы координаты схвата.

Далее вычисляются начальная дзо и конечная дзк обобщенные координаты третьего звена манипулятора по формулам:

/ Яю qararccos),

q3K arccoc(-|).

(3)

(4)

rfleaio(Yo/cosqio)2+(Z0-h)2-fc2-fc3 ; (5) «1к (YK/cosqiK)2+(ZK-fi)2-fc2-f23 ; (6) #o /8iK 2bb; ()

Z0 и ZK - соответственно начальная и конечная декартовы координаты схвата;

ti.fe и Ь - длины первого, второго и третьего звеньев манипулятора.

После этого осуществляется вычисление начального qzo и конечного Ц2к значений обобщенных координат второго звена манипулятора:

00

о

00

CJ

/ О20

q20 arccos { ,

(8)

q3K arccos(),(9)

где c&o (Zo-h)f3 - slnq30-(X30/slnqio) x x (12+1з cosqao),(Ю) a2K(ZK-tl)f3 s nq3K-(XK/slnqiK) x x (la+bcosqsx) (11)

fao f22+b2 -2b2b2cosq3o, #ZK - r22+t32- -2l22fe2cosq3K.

В дальнейшем осуществляется вычисление закона изменения обобщенных координат qs(t) (,2,3) каждого S-ro звена 1Ь манипулятора

qs(t)lqSK-qSol 10 -)3-15(-l-)V 2Q

+ 6|-Ј-1 +qso,

где t - текущее время;

tn - время позиционирования; qso и qsK - соответственно начальное и конечное значение обобщенной координаты S-ro звена манипулятора. Затем путем дифференцирования определяются законы изменения скоррсти qs(t) и ускорения cjs(t). Вычисленные значения qs(t), qXt) и qs(t) используются для сопоставления с фактическими значениями положения, скорости и ускорения и последующей выработки управляющих воздействий. Фор- мулы (1), (2), (3), (4), (8) и (9) получены путем решения обратной кинематической задачи. Формула (14) получена путем минимизации

1 «П i

функционала Ф -я-/ 2, QS dt, который

1 о s - 1

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

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

10

1Ь

2Q

25 3035

40

455055

такт, а длительности относительных вычислительных операций приведены в первом нижнем углу на фиг.7).

Устройство для управления манипулятором содержит первый дифференциатор 1, выход которого через выпрямитель 2 подключен к первому входу первого вычислительного блока 3, первый привод 4, первый выход которого через датчик 5 момента соединен с первым входом первого сумматора 6 и вторым входом первого вычислительного блока 3. Второй выход первого привода 4 через первый датчик 7 положения подключен к первому входу второго сумматора 8 и к третьему входу первого вычислительного блока 3, а вход первого привода 4 - к выходу первого усилителя 9. Первый вход блока 10 управления соединен с выходом первого вычислительного блока 3, второй вход - с выходом второго сумматора 8, а выход - с вторым входом первого сумматора 6. Вход второго привода 11 подключен к выходу второго усилителя 12, а выход кинематически связан со вторым датчиком 13 положения, выход которого подключен к первому входу третьего сумматора 1.4.. .

Первый, второй и третий входы второго вычислительного блока 1В являются входом задания длин звеньев манипулятора, с четвертого по девятый входы являются входом задания начальных и конечных значений, декартовых координат схвата. Первый и второй выходы блока 15 подключены к первому и второму входам третьего вычислительного блока 16 третий и четвертый выходы к первому и второму входам четвертого вычислительного блока 17, а пятый и шестой выходы - к первому и второму входам пятого вычислительного блока 18. Третьи входы третьего 16, четвертого 17 и пятого вычислительных 18 блоков являются входом задания времени позиционирования, первый выход третьего вычислительного блока 16 подключен к второму входу второго сумматора 8 и входу первого дифференциатора 1, а второй и третий выходы - к первым входам соответственно четвертого 19 и пятого 20 сумматоров. Вторые входы этих сумматоров соединены соответственно с выходами первого датчика 21 скорости и первого датчика 22 ускорения, кинематически связанными с выходом первого привода 4, а выходы четвертого 19 и пятого 20 сумматоров подключены к первым входам соответственно шестого 23 и седьмого 24 сумматоров, вторые входы которых подключены к выходам соответственно первого 6 и шестого 23 сумматора, а выход седьмого сумматора 24 соединен с входом первого усилителя 9.

Первый выход четвертого вычислительного блока 17 подключен к второму входу третьего сумматора 14, а второй и третий выходы - к первым входам соответственно восьмого 25 и девятого 26 сумматоров, вторые входы которых соединены соответственно с выходами второго датчика 27 скорости и второго датчика 28 ускорения кинематически соединенными с выходом второго привода 11, а выходы сумматоров 25 и 26 с первыми входами соответственно десятого 29 и одиннадцатого 30 сумматоров. Вторые входы этих сумматоров подключены соответственно к выходу третьего сумматора 14 и выходу десятого сумматора 29, а выход одиннадцатого сумматора 30 соединен с выходом второго усилителя 12.

Первый, второй и третий выходы пятого вычислительного блока 18 подключены к первым входам соответственно двенадцатого 31, тринадцатого 32 и четырнадцатого 33 сумматоров, вторые входы которых соединены соответственно с выходами третьего датчика 34 положения, третьего датчика 35 скорости и третьего датчика 36 ускорения, кинематически связанными с выходом третьего привода 37, а выходы сумматоров 31, 32 и 33 подключены соответственно к первому и второму входам пятнадцатого сумматора 38 и к первому входу шестнадцатого сумматора 39, Второй вход блока 39 соединен с выходом пятнадцатого сумматора 38, а выход через третий усилитель 40 с входом третьего привода 37.

Второй вычислительный блок 15 содержит распределитель 41 импульсов, тактовый вход которого соединен с выходом генератора 42 тактовых импульсов, а с первого по пятьдесят четвертый выходы распределителя 41 импульсов соединены с управляющими входами соответственно с первыми и вторыми входами первого 43, второго 44 и третьего 45 регистров, с первым, вторым и третьим входами четвертого регистров 46, с третьим входом третьего регистра 44, с первым входом первого коммутатора 47, с третьим входом первого регистра 43, с первым входом второго коммутатора 48, с первым и вторым входами пятого регистра 49, с четвертым входом четвертого регистра 46, с третьим входом третьего регистра 45, с первым и вторым входами шестого регистра 50, с первым входом третьего коммутатора 51, с пятым и шестым входами четвертого регистра 46, с первыми входами четвертого 52 и пятого 53 коммутаторов, с первыми входами седьмого 54 и восьмого 55 регистров, с вторым входом седьмого регистра 54, с первым входом шестого коммутатора 56, с вторым входом

восьмого регистра 55, с первым входом седьмого коммутатора 57, с вторыми входами пятого 53 и четвертого 52 коммутаторов, с первым входом первого сдвигового реги- 5 стра 58, с первым входом блока 59 вычисления арккосинуса, с первым и вторым входами девятого регистра 60, с первым входом восьмого коммутатора 61, с первым входом второго сдвигового регистра 62, с

0 первыми входами девятого 63 и десятого 64 коммутаторов, с первым входом десятого регистра 65, с четвертыми входами первого 43 и второго 44 регистров, с первыми входами одиннадцатого 66 и двенадцатого 67 ре5 гистров с первым входом одиннадцатого коммутатора 68, с первым входом тринадцатого регистра 69, с вторым входом одиннадцатого регистра 66, с первыми входами двенадцатого 70 и тринадцатого 71 комму0 таторов, с вторыми входами десятого 65 и тринадцатого 69 регистров, с четвертым входом третьего регистра 45.

Пятый и шестой входы первого регистра 43 являются соответственно четвертым и

5 пятым входами второго вычислительного блока 15, а первый и второй выходы подключены соответственно к первому и второму входам первого элемента ИЛИ 72, выход которого соединен с вторым входом второго

0 коммутатора 48. Первый выход коммутатора 48 подключен к первому входу блока 73 вычисления арктангенса, второй выход- к первому входу первого блока 74 деления.

Пятый и шестой входы второго регистра

5 44 являются соответственно шестым и седьмым входами второго вычислительного блока 15, а первый и второй выходы подключены соответственно к первому и второму входам второго элемента ИЛИ 75,

0 выход которого соединен со вторым входом первого коммутатора 47. Первый выход этого коммутатора подключен к первому входу второго блока 76 деления, а второй выход через инвертор 77 - к второму входу блока

5 73 вычисления арктангенса, выход которого соединен с третьими входами пятого 49 и одиннадцатого 66 регистров. Выходы этих регистров подключены к вторым входам соответствен но .двенадцатого 70 и тринадца0 того 71 коммутаторов, первые выходы которых являются соответственно первым и вторым выходами второго вычислительного блока 15, а вторые выходы соединены соответственно с первым и вторым входами

5 третьего элемента ИЛИ 78. Выход этого элемента подключен к входу первого блока 79 вычисления синуса и косинуса, первый выход которого через первый элемент 80 задержки соединен с вторым входом первого блока 74 деления, а второй выход - с вторым

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

Пятый и шестой входы третьего регистра 45 являются соответственно восьмым и девятым входами второго вычислительного блока 15, а первый и второй выходы подключены соответственно к первому и второму входам четвертого элемента ИЛИ 83, выход которого соединен с первым входам первого блока 84 вычитания. Выход этого блока подключен к третьему входу шестого регистра 50, выход которого соединен с вторым входом третьего коммутатора 51. Первый выход этого коммутатора подключен к первому входу пятого элемента ИЛИ 85, а второй выход через второй квадратор 86 - к второму входу семнадцатого сумматора 82, выход которого соединен с первым входом второго блока 87 вычитания.

Седьмой, восьмой и девятый входы четвертого регистра 46 являются соответственно первым, вторым и третьим входами второго вычислительного блока 15. Первый выход четвертого регистра 46 соединен с вторым входом первого блока 84 вычитания, а второй и третий выходы - с третьими входами соответственно четвертого 52 и пятого 53 коммутаторов, первые выходы которых подключены соответственно к первым входам первого 88 и второго 89 блоков умножения и первому входу шестого элемента ИЛИ 90, вторые выходы - соответственно к первому и второму входам третьего блока 91 умножения, а третьи выходы соответственно через третий 92 и четвертый 93 квадраторы - к третьим входам восьмого 55 и седьмого 54 регистров. Выходы этих регистров соединены с вторыми входами соответ- ственно седьмого 57 и шестого 56 коммутаторов, первые выходы которых подключены соответственно к первому входу третьего 94 и второму входу второго 87 блоков вычитания, а вторые выходы - соответственно к первому и второму входам восемнадцатого сумматора 95, выход которого соединен с вторым входом шестого элемента ИЛИ 90.

Выход третьего блока 91 умножения соединен через пятый квадратор 96 с вторым входом второго сдвигового регистра 62. а также непосредственно с вторым входом первого сдвигового регистра 58, выход которого подключен к первому входу седьмого элемента ИЛИ 97. Выход этого элемента соединен с вторым входом блоком 59 вычисления арккосинуса, третий вход которого подключен к выходу восьмого элемента ИЛИ 98, а выход - к третьим входам девято го 60, десятого 65, двенадцатого 67 и тринадцатого 69 регистров.

Выходы десятого 65 и тринадцатого 69 регистров являются соответственно третьим и четвертым выходами второго вычислительного блока 15, а выходы девятого 60 и двенадцатого 67 регистров соединены с вторыми входами соответственно восьмого 61 и одиннадцатого 68 коммутаторов, первые

0 выходы которых являются соответственно пятым и шестым выходами второго вычислительного блока 15, а вторые выходы подключены соответственно к первому и второму входам девятого элемента ИЛИ 99. Выход

5 этого элемента соединен с входом второго блока 100 вычисления синуса и косинуса, первый выход которого подключен к второму входу первого блока 88 умножения, а второй выход - к второму входу второго бло0 ка 89 умножения и к первому входу четвертого блока 101 умножения. Второй вход блока 101 через второй элемент 102 задержки соединен с выходом второго сдвигового регистра 62, а выход через третий элемент

5 103 задержки - с первым входом десятого элемента ИЛИ 104, второй вход которого подключен к выходу второго блока 89 умножения, а выход - к первому входу девятнадцатого сумматора 105. Второй вход этого

0 сумматора соединен с выходом шестого элемента ИЛИ 90, а выход - с вторым входом девятого коммутатора 63, первый выход которого через четвертый элемент 106 задержки подключен к второму входу седьмо5 го элемента ИЛИ 97, а второй выход через пятый элемент 107 задержки - к второму входу пятого элемента ИЛИ 85. Выход этого элемента соединен с первым входом пятого блока 108 умножения, второй вход которого

0 подключен к выходу одиннадцатого элемента ИЛИ 109, первый вход которого соединен с выходом первого блока 88 умножения, а второй вход - с выходом первого блока 74 деления.. .

5 Второй вход десятого коммутатора 64 подключен к выходу пятого блока 108 умножения первый выход через шестой элемент 110 задержки - к первому входу четвертого блока 111 вычитания, втЪрой вход которого

0 соединен с вторым выходом десятого коммутатора 64, а выход - с первым входом восьмого элемента ИЛИ 98. Второй вход этого элемента подключен к выходу третьего блока 94 вычитания, второй вход которого

5 соединен с выходом второго блока 87 вычитания.

Каждый из третьего 16, четвертого 17 или пятого 18 вычислительных блоков содержит четырнадцатый регистр 112, первый вход которого является третьим входом каждого из блоков 16, 17 и 18, а второй и третий входы подключены соответственно к первому и второму выходам блока 113 синхронизации, третий выход которого соединен с первым входом четырнадцатого коммутатора 114, а четвертый и пятый выходы - с соответственно с первым и вторым входами счетчика 115 времени. Выход блока 115 подключен к первому входу третьего блока 116 деления, второй вход которого соединен с выходом четырнадцатого регистра 112. а выход через седьмой элемент 117 задержки - с первым входом шестого блока

118 умножения и через восьмой элемент

119 задержки - с первым входом седьмого блока 120 умножения, а также непосредственно - с входом блока 121 возведения в степень. Выход блока 121 подключен через последовательно соединенные первый элемент 122 масштабирования и девятый элемент 123 задержки к первому входу двенадцатого сумматора 124, а также непосредственно - к второму входу шестого блока 118 умножения.

Выход блока 118 подключен через последовательно соединенные второй элемент 125 масштабирования и десятый элемент 126 задержки к второму входу двадцатого сумматора 124, а также непосредственно к второму входу седьмого блока 120 умножения, выход которого соединен через третий элемент 127 масштабирования с третьим входом двадцатого сумматора 124. Выход блока 124 подключен к первому входу восьмого блока 128 умножения, второй вход которого соединен через одиннадцатый элемент 129 задержки с первым выходом четырнадцатого коммутатора 114, а выход непосредственного -- с первым входом двадцать первого сумматора 130. Второй вход блока 130 подключен через двенадцатый элемент 131 задержки к второму выходу четырнадцатого коммутатора 114, а выход является первым выходом каждого из третьего 16, четвертого 17 или пятого 18 вычислительных блоков, а также соединен с входом второго дифференциатора 132, Выход блока 132 подключен непосредственно к входу тринадцатого 133, а также через третий дифференциатор 134 к входу четырнадцатого 135 элементов задержки, выходы которых являются соответственно вторым и третьим выходами каждого из третьего 16, четвертого 17 и пятого 18 вычислительных блоков.

Первый и второй входы блока 136 вычитания по модулю являются соответственно первым и вторым входами каждого из блоков 16,17 и 18, а выход блока 136 вычитания по модулю, соединен с первым входом двенадцатого элемента ИЛИ 137. второй вход

которого является первым входом каждого из блоков 16, 17 и 18, а выход двенадцатого элемента ИЛИ 137 подключен через циф- роаналоговый преобразователь 138 к вто- рому входу четырнадцатого коммутатора 114.

Блок 59 вычисления арккосинуса позволяет оценивать функцию вида c arccos(A/B), (его структура известна).

0 Блок 73 вычисления арктангенса позволяет оценивать функцию вида c arctg (А/В) (его структура известна).

Структура первого 79 и второго 100 блоков вычисления синуса и косинуса также

5 известна.

Структура первого вычислительного блока 3 и блока 10 управления представлены в описании прототипа.

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

На входы второго вычислительного блока 15 поступает исходная информация в виде цифровых электрических сигналов: ri.b.fo -. длины звеньев манипулятора;

5 Xo.Yo.Zo - начальные декартовы координаты схвата; Хк, YK.ZK - конечные декартовы координаты схвата. Сигналы, соответствующие значениям Х0 и Хк. поступают на пятый и шестой входы первого регистра 43 второго

0 вычислительного блока 15. Управляющие сигналы на запись подаются на первый и второй входы блока 43 с первого и второго выходов распределителя 41 импульсов (темп работы блока 41 задается генерато5 ром 42 тактовых импульсов). Сигналы, соответствующие значениям Y0 и YK, поступают на пятый и шестой входы второго регистра 44. Управляющие сигналы на запись подаются на первый и второй входы блока 44 с

0 третьего и четвертого выходов распределителя 41 импульсов. Сигналы, соответствующие значениям Z0 и ZK, поступают на пятый и шестой входы третьего регистра 45. Управляющие сигналы на запись подаются на

5 первый и второй входы блока 45 с пятого и шестого выходов распределителя 41 импульсов. Сигналы, соответствующие значениям i,2 и 1з, поступают на седьмой, восьмой и девятый входы четвертого регист0 ра 46. Управляющие сигналы на запись подаются на первый, второй и третий входы блока 46 с седьмого, восьмого и девятого выходов распределителя 41 импульсов. В дальнейшем осуществляется вычис5 ление начальных обобщенных координат для первого, второго и третьего звеньев манипулятора по формулам (1), (8) и (3). При этом промежуточные вычисления производятся по формулам (10), (12), (5) и (7). Порядок вычислений представлен на циклограмме

работы второго вычислительного блока. Сигналы, соответствующие величинам qio, Q20 и дзо, поступают на третьи входы соответственно пятого 49, десятого 65 и девятого 60 регистров. При этом управляющие сигналы на первые входы этих регистров подаются соответственно с четырнадцатого, сорок первого и тридцать пятого выходов распределителя 41 импульсов. Аналогичным образом осуществляется вычисление конечных обобщенных координат для первого, второго и третьего звеньев манипулятора по формулам (2). (9) и (4). При этом промежуточные вычисления производятся по формулам (11). (13), (6) и (7). Отличия заключаются лишь в том, что в качестве исходной информации используются не величины Х0, Yo Z0, а значения Хк.У и ZK. Сигналы, соответствующие величинам qiK, Р2к и дзк. поступают на третьи входы соответственно одиннадцатого 66, тринадцатого 69 и двенадцатого 67 регистров. При этом управляющие сигналы на первые входы этих регистров подаются соответственно с сорок четвертого, сорок восьмого и сорок пятого выходов распределителя 41 импульсов.

По сигналам с пятнадцатого, сорок девятого, пятьдесят второго, пятьдесят третьего, тридцать шестого и сорок шестого выходов распределителя 41 импульсов сигналы, соответствующие величинам qio,qiK,q2o,q2K,q3o,q3R, поступают с выходов соответственно пятого 49, одиннадцатого 66, десятого 65, тринадцатого 69, девятого 60 и двенадцатого 67 регистров через двенадцатый 70 и тринадцатый 71 коммутаторы, qao и q2K непосредственно, восьмой 61 и одиннадцатый 68 коммутаторы на первый, второй, третий, четвертый, пятый и шестой выходы второго вычислительного блока 15. С первого и второго выходов блока 15 сигналы, соответствующие величинам qio и qu. подаются соответственно на первый и второй входы третьего вычислительного блока 16. С третьего и четвертого выходов блока 15 сигналы, соответствующие величинам q2o и q2K, подаются соответственно на первый и второй входы четвертого вычислительного блока 17. С пятого и шестого выходов блока 15 сигналы, соответствующие величинам qao и q3K. подаются соответственно на первый и второй входы пятого вычислительного блока 18. На третьи входы третьего 16, четвертого 17 и пятого 18 вычислительных блоков с входа устройства задания времени позиционирования поступает сигнал в аналоговой форме, соответствующий величине tn.

Блоки 16, 17 и 18 работают идентично и поэтому обладают одинаковой структурой, определяя законы изменения положения, скорости и ускорения для трех различных

звеньев манипулятора. Работу одного из этих блоков, например третьего вычислительного блока 16 осуществляется следующим образом. С первого и второго входов блока 16 сигналы, соответствующие величи0 нам qio и qu поступают соответственно на первый и второй входы блока 136 вычитания по модулю. Кроме того сигнал, соответствующий величине qio, через двенадцатый элемент ИЛИ 137 поступает на вход цифро5 аналогового преобразователя 138. С выхода блока 138 сигнал, соответствующий величине qi0, уже в аналоговой форме поступает через четырнадцатый коммутатор 114 (при этом подается управляющий сигнал на его

0 вход с третьего выхода блока 113 синхронизации) на вход двенадцатого элемента 131 задержки. С выхода блока 136 вычитания по модулю сигнал, соответствующий величине IqiK-qiol, поступает через двенадцатый

5 элемент ИЛИ 137 на вход цифроаналогово- го преобразователя 138. С выхода блока 138 сигнал, соответствующий величине lqiK-qio, в аналоговой форме поступает через четырнадцатый коммутатор 114 (при

0 этом управляющий сигнал на его вход не подается) на вход одиннадцатого элемента 129 задержки.

С третьего входа третьего вычислительного блока 16 сигнал соответствующий ве5 личине времени позиционирования. tn подается на третий вход четырнадцатого регистра 112. Управляющий сигнал подается на второй вход блока 112с первого выхода блока 113 синхронизации. С пятого выхода

0 блока 113 подается сигнал на счетчик 115 времени для его обнуления, а затем с четвертого выхода блока 113 синхронизации поступает сигнал на счетный вход блока 115 для его запуска. С выхода счетчика 115 вре5 мени текущие значения времени t подаются на первый вход третьего блока 116 деления, на второй вход которого засылается по сигналу с второго выхода блока 113 синхронизации на второй вход регистра 112 величина

0 tn. С выхода блока 116 сигнал, соответствующий величине t/tn. поступает на входы блока 121 возведения в куб, седьмого 117 и восьмого 119 элементов задержки. С выхода блока 121 сигнал, соответствующий величи5 не (t/tn)3, подается на второй вход шестого блока 118 умножения и на вход первого элемента 122 масштабирования. С выхода блока 122 сигнал, соответствующий величине 10 (t/tn)3, поступает на вход девятого элемента 123 задержки.

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