RU1816684C

RU1816684C - Robot drive control device

Info

Publication number: RU1816684C
Authority: RU
Inventors: Владимир Федорович Филаретов
Original assignee: Дальневосточный политехнический институт им.В.В.Куйбышева
Priority date: 1990-06-19
Filing date: 1990-06-19
Publication date: 1993-05-23

Abstract

Использование: в робототехнике. Сущность изобретени : в устройство дополнительно введены четыре вычислительных блока, тринадцать сумматоров, привод, усилитель , датчик положени , три датчика скорости , три датчика ускорени , а также соответствующим образом выполнены вычислительные блоки. 2 з.п, ф-лы, 7 ил.Usage: in robotics. SUMMARY OF THE INVENTION: four computing units, thirteen adders, a drive, an amplifier, a position sensor, three speed sensors, three acceleration sensors, and also computing units are suitably made in the device. 2 z.p., f-ly, 7 ill.

Description

Изобретение относитс к робототехнике и может быть использовано при создании манипул торов, обеспечивающих перенос грузов различной массы с различными скорост ми .The invention relates to robotics and can be used to create manipulators for the transfer of goods of different masses with different speeds.

Целью изобретени вл етс повышение точности позиционировани манипул тора .The aim of the invention is to increase the accuracy of positioning of the manipulator.

Точность позиционировани можно повысить за счет отработки программного движени не только по положению, но и по скорости и ускорению. Такой поДход может быть реализован следующим образом . Вычисл ютс начальна qio и конечна qu обобщенные координаты первого звена манипул тора по следующим формулам:Positioning accuracy can be improved by practicing software movement not only in position but also in speed and acceleration. Such an approach can be implemented as follows. The initial qio and final q generalized coordinates of the first link of the manipulator are calculated using the following formulas:

ХоHo

qio arctg().qio arctg ().

ХкHk

qiK-arctg (),qiK-arctg (),

(1)(1)

(2)(2)

где Хо. Уо и Хк.Ук - соответственно начальные и конечные.декартовы координаты схвата.where is ho. Uo and Khk. Uk are the initial and final, respectively, Cartesian coordinates of the grip.

Далее вычисл ютс начальна дзо и конечна дзк обобщенные координаты третьего звена манипул тора по формулам:Next, the starting dzo and the final dzk are summarized by the general coordinates of the third link of the manipulator according to the formulas:

/ Яю qararccos),/ Yayu qararccos),

q3K arccoc(-|).q3K arccoc (- |).

(3)(3)

(4)(4)

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

Z0 и ZK - соответственно начальна и конечна декартовы координаты схвата;Z0 and ZK are the start and end Cartesian coordinates of the grip, respectively;

ti.fe и Ь - длины первого, второго и третьего звеньев манипул тора.ti.fe and b are the lengths of the first, second, and third links of the manipulator.

После этого осуществл етс вычисление начального qzo и конечного Ц2к значений обобщенных координат второго звена манипул тора:After that, the initial qzo and final C2k values of the generalized coordinates of the second link of the manipulator are calculated:

0000

о about

0000

CJCj

/ О20 / O20

q20 arccos { ,q20 arccos {,

(8)(8)

q3K arccos(),(9)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)where c & o (Zo-h) f3 - slnq30- (X30 / slnqio) xx (12 + 1з cosqao), (10) a2K (ZK-tl) f3 s nq3K- (XK / slnqiK) xx (la + bcosqsx) (11 )

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

В дальнейшем осуществл етс вычисление закона изменени обобщенных координат qs(t) (,2,3) каждого S-ro звена 1Ь манипул тораSubsequently, the law of variation of the generalized coordinates qs (t) (, 2,3) of each S-ro link 1b of the manipulator is calculated

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

+ 6|-Ј-1 +qso,+ 6 | -Ј-1 + qso,

где t - текущее врем ;where t is the current time;

tn - врем позиционировани ; qso и qsK - соответственно начальное и конечное значение обобщенной координаты S-ro звена манипул тора. Затем путем дифференцировани определ ютс законы изменени скоррсти qs(t) и ускорени cjs(t). Вычисленные значени qs(t), qXt) и qs(t) используютс дл сопоставлени с фактическими значени ми положени , скорости и ускорени и последующей выработки управл ющих воздействий. Фор- мулы (1), (2), (3), (4), (8) и (9) получены путем решени обратной кинематической задачи. Формула (14) получена путем минимизацииtn is the positioning time; qso and qsK are the initial and final values of the generalized coordinate S-ro of the manipulator link, respectively. Then, by differentiating the laws of change of velocity qs (t) and acceleration cjs (t) are determined. The calculated values of qs (t), qXt) and qs (t) are used to compare with the actual values of position, speed and acceleration and the subsequent generation of control actions. Formulas (1), (2), (3), (4), (8) and (9) were obtained by solving the inverse kinematic problem. Formula (14) is obtained by minimizing

1 «П i1 "P i

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

1 о s - 11 about s - 1

выбиралс из услови сокращени времени позиционирований и сокращени количества вычислительных операций.was selected from the condition of reducing positioning time and reducing the number of computational operations.

На фиг.1 и 2 представлена блок-схема устройства дл управлени манипул тором; на фиг.З, 4 и 5 - структурна схема блока вычислени обобщенных координат (дл ликвидации громоздкости св зи между распределителем импульсов и управл ющими входами соответствующих блоков показаны не полностью, а обозначены путем нумерации входов и выходов); на фиг.6 - структурна схема одного из блоков задани законов; на фиг.7 - циклограмма работы второго вычислительного блока (по оси ординат обозначены номера выходов распределител импульсов, а по оси абсцисс - число тактов причем длительности операций сложени и вычитани прин ты за одинFigures 1 and 2 show a block diagram of a device for controlling a manipulator; Figs. 3, 4 and 5 show a block diagram of a generalized coordinate calculation unit (for eliminating cumbersome communications between the pulse distributor and the control inputs of the corresponding blocks, they are not shown completely, but are indicated by numbering the inputs and outputs); Fig. 6 is a structural diagram of one of the blocks for defining laws; in Fig.7 - a sequence diagram of the second computing unit (the ordinates indicate the numbers of the outputs of the pulse distributor, and the abscissa indicates the number of ticks and the duration of the operations of addition and subtraction are taken in one

1010

1Ь 1b

2Q2Q

25 30 35 25 30 35

4040

45 50 55 45 50 55

такт, а длительности относительных вычислительных операций приведены в первом нижнем углу на фиг.7).tact, and the duration of the relative computational operations are shown in the first lower corner in Fig.7).

Устройство дл управлени манипул тором содержит первый дифференциатор 1, выход которого через выпр митель 2 подключен к первому входу первого вычислительного блока 3, первый привод 4, первый выход которого через датчик 5 момента соединен с первым входом первого сумматора 6 и вторым входом первого вычислительного блока 3. Второй выход первого привода 4 через первый датчик 7 положени подключен к первому входу второго сумматора 8 и к третьему входу первого вычислительного блока 3, а вход первого привода 4 - к выходу первого усилител 9. Первый вход блока 10 управлени соединен с выходом первого вычислительного блока 3, второй вход - с выходом второго сумматора 8, а выход - с вторым входом первого сумматора 6. Вход второго привода 11 подключен к выходу второго усилител 12, а выход кинематически св зан со вторым датчиком 13 положени , выход которого подключен к первому входу третьего сумматора 1.4.. .The device for controlling the manipulator contains a first differentiator 1, the output of which through the rectifier 2 is connected to the first input of the first computing unit 3, the first drive 4, the first output of which is connected via a torque sensor 5 to the first input of the first adder 6 and the second input of the first computing unit 3 The second output of the first drive 4 through the first position sensor 7 is connected to the first input of the second adder 8 and to the third input of the first computing unit 3, and the input of the first drive 4 is connected to the output of the first amplifier 9. First the input of the control unit 10 is connected to the output of the first computing unit 3, the second input to the output of the second adder 8, and the output to the second input of the first adder 6. The input of the second drive 11 is connected to the output of the second amplifier 12, and the output is kinematically connected to the second sensor 13 position, the output of which is connected to the first input of the third adder 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.The first, second and third inputs of the second computing unit 1B are the input of the job of the lengths of the links of the manipulator, the fourth to ninth inputs are the input of the job of the initial and final values, the Cartesian coordinates of the grip. The first and second outputs of block 15 are connected to the first and second inputs of the third computing block 16; the third and fourth outputs are to the first and second inputs of the fourth computing block 17, and the fifth and sixth outputs are connected to the first and second inputs of the fifth computing block 18. Third inputs the third 16, fourth 17 and fifth computing 18 units are the input of the positioning time setting, the first output of the third computing unit 16 is connected to the second input of the second adder 8 and the input of the first differentiator 1, and the second and third Exit - to first inputs respectively fourth 19 and fifth 20 adders. The second inputs of these adders are connected respectively to the outputs of the first speed sensor 21 and the first acceleration sensor 22, kinematically connected to the output of the first drive 4, and the outputs of the fourth 19 and fifth of 20 adders are connected to the first inputs of the sixth 23 and seventh 24 adders, second inputs which are connected to the outputs of the first 6 and sixth 23 of the adder, and the output of the seventh adder 24 is connected to the input of the first amplifier 9.

Первый выход четвертого вычислительного блока 17 подключен к второму входу третьего сумматора 14, а второй и третий выходы - к первым входам соответственно восьмого 25 и дев того 26 сумматоров, вторые входы которых соединены соответственно с выходами второго датчика 27 скорости и второго датчика 28 ускорени кинематически соединенными с выходом второго привода 11, а выходы сумматоров 25 и 26 с первыми входами соответственно дес того 29 и одиннадцатого 30 сумматоров . Вторые входы этих сумматоров подключены соответственно к выходу третьего сумматора 14 и выходу дес того сумматора 29, а выход одиннадцатого сумматора 30 соединен с выходом второго усилител 12.The first output of the fourth computing unit 17 is connected to the second input of the third adder 14, and the second and third outputs are connected to the first inputs of the eighth 25 and ninth 26 adders, respectively, the second inputs of which are connected respectively to the outputs of the second speed sensor 27 and the second acceleration sensor 28 kinematically connected with the output of the second drive 11, and the outputs of the adders 25 and 26 with the first inputs, respectively, of the tenth 29 and eleventh 30 adders. The second inputs of these adders are connected respectively to the output of the third adder 14 and the output of the tenth adder 29, and the output of the eleventh adder 30 is connected to the output of the second amplifier 12.

Первый, второй и третий выходы п того вычислительного блока 18 подключены к первым входам соответственно двенадцатого 31, тринадцатого 32 и четырнадцатого 33 сумматоров, вторые входы которых соединены соответственно с выходами третьего датчика 34 положени , третьего датчика 35 скорости и третьего датчика 36 ускорени , кинематически св занными с выходом третьего привода 37, а выходы сумматоров 31, 32 и 33 подключены соответственно к первому и второму входам п тнадцатого сумматора 38 и к первому входу шестнадцатого сумматора 39, Второй вход блока 39 соединен с выходом п тнадцатого сумматора 38, а выход через третий усилитель 40 с входом третьего привода 37.The first, second and third outputs of the fifth computing unit 18 are connected to the first inputs of the twelfth 31, thirteenth 32 and fourteenth 33 adders, respectively, the second inputs of which are connected respectively to the outputs of the third position sensor 34, the third speed sensor 35 and the third acceleration sensor 36, kinematically connected to the output of the third drive 37, and the outputs of the adders 31, 32 and 33 are connected respectively to the first and second inputs of the fifteenth adder 38 and to the first input of the sixteenth adder 39, The second input of block 39 oedinen yield fifteen adder 38, and output through the third amplifier 40 to the input 37 of the third actuator.

Второй вычислительный блок 15 содержит распределитель 41 импульсов, тактовый вход которого соединен с выходом генератора 42 тактовых импульсов, а с первого по п тьдес т четвертый выходы распределител 41 импульсов соединены с управл ющими входами соответственно с первыми и вторыми входами первого 43, второго 44 и третьего 45 регистров, с первым , вторым и третьим входами четвертого регистров 46, с третьим входом третьего регистра 44, с первым входом первого коммутатора 47, с третьим входом первого регистра 43, с первым входом второго коммутатора 48, с первым и вторым входами п того регистра 49, с четвертым входом четвертого регистра 46, с третьим входом третьего регистра 45, с первым и вторым входами шестого регистра 50, с первым входом третьего коммутатора 51, с п тым и шестым входами четвертого регистра 46, с первыми входами четвертого 52 и п того 53 коммутаторов, с первыми входами седьмого 54 и восьмого 55 регистров, с вторым входом седьмого регистра 54, с первым входом шестого коммутатора 56, с вторым входомThe second computing unit 15 contains a pulse distributor 41, the clock input of which is connected to the output of the clock generator 42, and from the first to fifty fourth outputs of the pulse distributor 41 are connected to the control inputs, respectively, with the first and second inputs of the first 43, second 44, and third 45 registers, with the first, second and third inputs of the fourth registers 46, with the third input of the third register 44, with the first input of the first switch 47, with the third input of the first register 43, with the first input of the second switch 4 8, with the first and second inputs of the fifth register 49, with the fourth input of the fourth register 46, with the third input of the third register 45, with the first and second inputs of the sixth register 50, with the first input of the third switch 51, with the fifth and sixth inputs of the fourth register 46, with the first inputs of the fourth 52 and fifth of 53 switches, with the first inputs of the seventh 54 and eighth 55 registers, with the second input of the seventh register 54, with the first input of the sixth switch 56, with the second input

восьмого регистра 55, с первым входом седьмого коммутатора 57, с вторыми входами п того 53 и четвертого 52 коммутаторов, с первым входом первого сдвигового реги- 5 стра 58, с первым входом блока 59 вычислени арккосинуса, с первым и вторым входами дев того регистра 60, с первым входом восьмого коммутатора 61, с первым входом второго сдвигового регистра 62, сeighth register 55, with the first input of the seventh switch 57, with second inputs of the fifth 53 and fourth 52 switches, with the first input of the first shift register 5 of country 58, with the first input of the arccosine calculation unit 59, with the first and second inputs of the ninth register 60 , with the first input of the eighth switch 61, with the first input of the second shift register 62, s

0 первыми входами дев того 63 и дес того 64 коммутаторов, с первым входом дес того регистра 65, с четвертыми входами первого 43 и второго 44 регистров, с первыми входами одиннадцатого 66 и двенадцатого 67 ре5 гистров с первым входом одиннадцатого коммутатора 68, с первым входом тринадцатого регистра 69, с вторым входом одиннадцатого регистра 66, с первыми входами двенадцатого 70 и тринадцатого 71 комму0 таторов, с вторыми входами дес того 65 и тринадцатого 69 регистров, с четвертым входом третьего регистра 45.0 the first inputs of the ninth 63 and the tenth 64 switches, with the first input of the tenth register 65, with the fourth inputs of the first 43 and second 44 registers, with the first inputs of the eleventh 66 and twelfth 67 registers with the first input of the eleventh switch 68, with the first input thirteenth register 69, with the second input of the eleventh register 66, with the first inputs of the twelfth 70 and thirteenth 71 commutators, with the second inputs of the tenth 65 and thirteenth 69 registers, with the fourth input of the third register 45.

П тый и шестой входы первого регистра 43 вл ютс соответственно четвертым иThe fifth and sixth inputs of the first register 43 are respectively the fourth and

5 п тым входами второго вычислительного блока 15, а первый и второй выходы подключены соответственно к первому и второму входам первого элемента ИЛИ 72, выход которого соединен с вторым входом второго5 by the fifth inputs of the second computing unit 15, and the first and second outputs are connected respectively to the first and second inputs of the first element OR 72, the output of which is connected to the second input of the second

0 коммутатора 48. Первый выход коммутатора 48 подключен к первому входу блока 73 вычислени арктангенса, второй выход- к первому входу первого блока 74 делени .0 of the switch 48. The first output of the switch 48 is connected to the first input of the arc tangent computing unit 73, and the second output is connected to the first input of the first division unit 74.

П тый и шестой входы второго регистраFifth and sixth inputs of the second register

5 44 вл ютс соответственно шестым и седьмым входами второго вычислительного блока 15, а первый и второй выходы подключены соответственно к первому и второму входам второго элемента ИЛИ 75,5 44 are respectively the sixth and seventh inputs of the second computing unit 15, and the first and second outputs are connected respectively to the first and second inputs of the second OR element 75,

0 выход которого соединен со вторым входом первого коммутатора 47. Первый выход этого коммутатора подключен к первому входу второго блока 76 делени , а второй выход через инвертор 77 - к второму входу блока0 the output of which is connected to the second input of the first switch 47. The first output of this switch is connected to the first input of the second division unit 76, and the second output through the inverter 77 to the second input of the unit

5 73 вычислени арктангенса, выход которого соединен с третьими входами п того 49 и одиннадцатого 66 регистров. Выходы этих регистров подключены к вторым входам соответствен но .двенадцатого 70 и тринадца0 того 71 коммутаторов, первые выходы которых вл ютс соответственно первым и вторым выходами второго вычислительного блока 15, а вторые выходы соединены соответственно с первым и вторым входами5 73 computing the arc tangent, the output of which is connected to the third inputs of the fifth 49th and eleventh 66 registers. The outputs of these registers are connected to the second inputs of the twelfth 70 and thirteen of the 71 switches, the first outputs of which are respectively the first and second outputs of the second computing unit 15, and the second outputs are connected respectively to the first and second inputs

5 третьего элемента ИЛИ 78. Выход этого элемента подключен к входу первого блока 79 вычислени синуса и косинуса, первый выход которого через первый элемент 80 задержки соединен с вторым входом первого блока 74 делени , а второй выход - с вторым5 of the third OR element 78. The output of this element is connected to the input of the first sine and cosine calculation unit 79, the first output of which is connected through the first delay element 80 to the second input of the first division unit 74, and the second output to the second

входом второго блока 76 делени , выход которого через первый квадратор 81 подключен к первому входу семнадцатого сумматора 82.the input of the second division unit 76, the output of which through the first quadrator 81 is connected to the first input of the seventeenth adder 82.

П тый и шестой входы третьего регистра 45 вл ютс соответственно восьмым и дев тым входами второго вычислительного блока 15, а первый и второй выходы подключены соответственно к первому и второму входам четвертого элемента ИЛИ 83, выход которого соединен с первым входам первого блока 84 вычитани . Выход этого блока подключен к третьему входу шестого регистра 50, выход которого соединен с вторым входом третьего коммутатора 51. Первый выход этого коммутатора подключен к первому входу п того элемента ИЛИ 85, а второй выход через второй квадратор 86 - к второму входу семнадцатого сумматора 82, выход которого соединен с первым входом второго блока 87 вычитани .The fifth and sixth inputs of the third register 45 are respectively the eighth and ninth inputs of the second computing unit 15, and the first and second outputs are connected respectively to the first and second inputs of the fourth OR element 83, the output of which is connected to the first inputs of the first subtracting unit 84. The output of this unit is connected to the third input of the sixth register 50, the output of which is connected to the second input of the third switch 51. The first output of this switch is connected to the first input of the fifth OR element 85, and the second output through the second quadrator 86 to the second input of the seventeenth adder 82, the output of which is connected to the first input of the second subtraction unit 87.

Седьмой, восьмой и дев тый входы четвертого регистра 46 вл ютс соответственно первым, вторым и третьим входами второго вычислительного блока 15. Первый выход четвертого регистра 46 соединен с вторым входом первого блока 84 вычитани , а второй и третий выходы - с третьими входами соответственно четвертого 52 и п того 53 коммутаторов, первые выходы которых подключены соответственно к первым входам первого 88 и второго 89 блоков умножени и первому входу шестого элемента ИЛИ 90, вторые выходы - соответственно к первому и второму входам третьего блока 91 умножени , а третьи выходы соответственно через третий 92 и четвертый 93 квадраторы - к третьим входам восьмого 55 и седьмого 54 регистров. Выходы этих регистров соединены с вторыми входами соответ- ственно седьмого 57 и шестого 56 коммутаторов, первые выходы которых подключены соответственно к первому входу третьего 94 и второму входу второго 87 блоков вычитани , а вторые выходы - соответственно к первому и второму входам восемнадцатого сумматора 95, выход которого соединен с вторым входом шестого элемента ИЛИ 90.The seventh, eighth and ninth inputs of the fourth register 46 are respectively the first, second and third inputs of the second computing unit 15. The first output of the fourth register 46 is connected to the second input of the first block 84 of the subtraction, and the second and third outputs are with the third inputs, respectively, of the fourth 52 and five 53 switches, the first outputs of which are connected respectively to the first inputs of the first 88 and second 89 blocks of multiplication and the first input of the sixth element OR 90, the second outputs, respectively, to the first and second inputs of the third Lok multiplier 91, and third outputs, respectively, through a third 92 and fourth 93 Quad - a third input 55 of the eighth and seventh registers 54. The outputs of these registers are connected to the second inputs, respectively, of the seventh 57 and sixth 56 switches, the first outputs of which are connected respectively to the first input of the third 94 and second input of the second 87 blocks of subtraction, and the second outputs, respectively, to the first and second inputs of the eighteenth adder 95, output which is connected to the second input of the sixth element OR 90.

Выход третьего блока 91 умножени соединен через п тый квадратор 96 с вторым входом второго сдвигового регистра 62. а также непосредственно с вторым входом первого сдвигового регистра 58, выход которого подключен к первому входу седьмого элемента ИЛИ 97. Выход этого элемента соединен с вторым входом блоком 59 вычислени арккосинуса, третий вход которого подключен к выходу восьмого элемента ИЛИ 98, а выход - к третьим входам дев то го 60, дес того 65, двенадцатого 67 и тринадцатого 69 регистров.The output of the third multiplying unit 91 is connected through the fifth quadrator 96 to the second input of the second shift register 62. and also directly to the second input of the first shift register 58, the output of which is connected to the first input of the seventh OR element 97. The output of this element is connected to the second input of the block 59 computing an arccosine whose third input is connected to the output of the eighth OR element 98, and the output to the third inputs of the ninth 60, tenth 65, twelfth 67 and thirteenth 69 registers.

Выходы дес того 65 и тринадцатого 69 регистров вл ютс соответственно третьим и четвертым выходами второго вычислительного блока 15, а выходы дев того 60 и двенадцатого 67 регистров соединены с вторыми входами соответственно восьмого 61 и одиннадцатого 68 коммутаторов, первыеThe outputs of the tenth 65 and thirteenth 69 registers are respectively the third and fourth outputs of the second computing unit 15, and the outputs of the ninth 60 and twelfth 67 registers are connected to the second inputs of the eighth 61 and eleventh 68 switches, respectively, the first

0 выходы которых вл ютс соответственно п тым и шестым выходами второго вычислительного блока 15, а вторые выходы подключены соответственно к первому и второму входам дев того элемента ИЛИ 99. Выход0 outputs of which are the fifth and sixth outputs of the second computing unit 15, respectively, and the second outputs are connected respectively to the first and second inputs of the ninth OR element 99. Output

5 этого элемента соединен с входом второго блока 100 вычислени синуса и косинуса, первый выход которого подключен к второму входу первого блока 88 умножени , а второй выход - к второму входу второго бло0 ка 89 умножени и к первому входу четвертого блока 101 умножени . Второй вход блока 101 через второй элемент 102 задержки соединен с выходом второго сдвигового регистра 62, а выход через третий элемент5 of this element is connected to the input of the second sine and cosine calculation unit 100, the first output of which is connected to the second input of the first multiplication unit 88, and the second output is connected to the second input of the second multiplication unit 89 and to the first input of the fourth multiplication unit 101. The second input of block 101 through the second delay element 102 is connected to the output of the second shift register 62, and the output through the third element

5 103 задержки - с первым входом дес того элемента ИЛИ 104, второй вход которого подключен к выходу второго блока 89 умножени , а выход - к первому входу дев тнадцатого сумматора 105. Второй вход этого5 103 delays - with the first input of the tenth element OR 104, the second input of which is connected to the output of the second block 89 of multiplication, and the output to the first input of the nineteenth adder 105. The second input of this

0 сумматора соединен с выходом шестого элемента ИЛИ 90, а выход - с вторым входом дев того коммутатора 63, первый выход которого через четвертый элемент 106 задержки подключен к второму входу седьмо5 го элемента ИЛИ 97, а второй выход через п тый элемент 107 задержки - к второму входу п того элемента ИЛИ 85. Выход этого элемента соединен с первым входом п того блока 108 умножени , второй вход которого0 adder is connected to the output of the sixth OR element 90, and the output is connected to the second input of the ninth switch 63, the first output of which is connected through the fourth delay element 106 to the second input of the seventh OR OR 97 element, and the second output through the fifth delay element 107 to the second input of the fifth element OR 85. The output of this element is connected to the first input of the fifth block 108 of multiplication, the second input of which

0 подключен к выходу одиннадцатого элемента ИЛИ 109, первый вход которого соединен с выходом первого блока 88 умножени , а второй вход - с выходом первого блока 74 делени .. .0 is connected to the output of the eleventh element OR 109, the first input of which is connected to the output of the first block 88 multiplication, and the second input to the output of the first block 74 division ...

5 Второй вход дес того коммутатора 64 подключен к выходу п того блока 108 умножени первый выход через шестой элемент 110 задержки - к первому входу четвертого блока 111 вычитани , втЪрой вход которого5 The second input of the tenth switch 64 is connected to the output of the fifth block 108 of the multiplication, the first output through the sixth delay element 110 - to the first input of the fourth block subtraction 111, the second input of which

0 соединен с вторым выходом дес того коммутатора 64, а выход - с первым входом восьмого элемента ИЛИ 98. Второй вход этого элемента подключен к выходу третьего блока 94 вычитани , второй вход которого0 is connected to the second output of the tenth switch 64, and the output is connected to the first input of the eighth OR element 98. The second input of this element is connected to the output of the third subtraction unit 94, the second input of which

5 соединен с выходом второго блока 87 вычитани .5 is connected to the output of the second subtraction unit 87.

Каждый из третьего 16, четвертого 17 или п того 18 вычислительных блоков содержит четырнадцатый регистр 112, первый вход которого вл етс третьим входом каждого из блоков 16, 17 и 18, а второй и третий входы подключены соответственно к первому и второму выходам блока 113 синхронизации , третий выход которого соединен с первым входом четырнадцатого коммутатора 114, а четвертый и п тый выходы - с соответственно с первым и вторым входами счетчика 115 времени. Выход блока 115 подключен к первому входу третьего блока 116 делени , второй вход которого соединен с выходом четырнадцатого регистра 112. а выход через седьмой элемент 117 задержки - с первым входом шестого блокаEach of the third 16, fourth 17 or fifth 18 computing units contains a fourteenth register 112, the first input of which is the third input of each of the units 16, 17 and 18, and the second and third inputs are connected respectively to the first and second outputs of the synchronization unit 113, the third output of which is connected to the first input of the fourteenth switch 114, and the fourth and fifth outputs are connected to the first and second inputs of the time counter 115, respectively. The output of block 115 is connected to the first input of the third division block 116, the second input of which is connected to the output of the fourteenth register 112. and the output through the seventh delay element 117 is connected to the first input of the sixth block

118 умножени и через восьмой элемент118 times and through the eighth element

119 задержки - с первым входом седьмого блока 120 умножени , а также непосредственно - с входом блока 121 возведени в степень. Выход блока 121 подключен через последовательно соединенные первый элемент 122 масштабировани и дев тый элемент 123 задержки к первому входу двенадцатого сумматора 124, а также непосредственно - к второму входу шестого блока 118 умножени .119 delays - with the first input of the seventh multiplication unit 120, as well as directly with the input of the power unit 121. The output of block 121 is connected via a series-connected first scaling element 122 and a ninth delay element 123 to the first input of the twelfth adder 124, and also directly to the second input of the sixth multiplying block 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 вычислительных блоков.The output of block 118 is connected via a second scaling element 125 and a tenth delay element 126 connected in series to the second input of the twentieth adder 124, as well as directly to the second input of the seventh multiplier 120, the output of which is connected via the third scaling element 127 to the third input of the twentieth adder 124. The output of block 124 is connected to the first input of the eighth block 128 of multiplication, the second input of which is connected through the eleventh delay element 129 to the first output of the fourteenth switch 114, and the output with the first input of the twenty-first adder 130. The second input of block 130 is connected through the twelfth delay element 131 to the second output of the fourteenth switch 114, and the output is the first output of each of the third 16, fourth 17, or fifth 18 computing units, as well connected to the input of the second differentiator 132, the output of block 132 is connected directly to the input of the thirteenth 133, and also through the third differentiator 134 to the input of the fourteenth 135 delay elements, the outputs of which are the second and third respectively odes of each of the third 16, fourth 17 and fifth 18 computing units.

Первый и второй входы блока 136 вычитани по модулю вл ютс соответственно первым и вторым входами каждого из блоков 16,17 и 18, а выход блока 136 вычитани по модулю, соединен с первым входом двенадцатого элемента ИЛИ 137. второй входThe first and second inputs of modulo subtraction block 136 are respectively the first and second inputs of each of blocks 16.17 and 18, and the modulo subtraction block 136 output is connected to the first input of the twelfth OR element 137. second input

которого вл етс первым входом каждого из блоков 16, 17 и 18, а выход двенадцатого элемента ИЛИ 137 подключен через циф- роаналоговый преобразователь 138 к вто- рому входу четырнадцатого коммутатора 114.which is the first input of each of the blocks 16, 17 and 18, and the output of the twelfth OR element 137 is connected via a digital-to-analog converter 138 to the second input of the fourteenth switch 114.

Блок 59 вычислени арккосинуса позвол ет оценивать функцию вида c arccos(A/B), (его структура известна).The arccosine calculation unit 59 makes it possible to evaluate a function of the form c arccos (A / B), (its structure is known).

0 Блок 73 вычислени арктангенса позвол ет оценивать функцию вида c arctg (А/В) (его структура известна).0 The arctangent calculation unit 73 allows the evaluation of a function of the form c arctg (A / B) (its structure is known).

Структура первого 79 и второго 100 блоков вычислени синуса и косинуса такжеThe structure of the first 79 and second 100 blocks for calculating the sine and cosine is also

5 известна.5 known.

Структура первого вычислительного блока 3 и блока 10 управлени представлены в описании прототипа.The structure of the first computing unit 3 and the control unit 10 is presented in the description of the prototype.

Устройство дл управлени манипул 0 тором работает следующим образом.The device for controlling the manipulator 0 operates as follows.

На входы второго вычислительного блока 15 поступает исходна информаци в виде цифровых электрических сигналов: ri.b.fo -. длины звеньев манипул тора;The inputs of the second computing unit 15 receive the initial information in the form of digital electrical signals: ri.b.fo -. lengths of links of the manipulator;

5 Xo.Yo.Zo - начальные декартовы координаты схвата; Хк, YK.ZK - конечные декартовы координаты схвата. Сигналы, соответствующие значени м Х0 и Хк. поступают на п тый и шестой входы первого регистра 43 второго5 Xo.Yo.Zo - the initial Cartesian coordinates of the grip; Xk, YK.ZK - the final Cartesian coordinates of the grip. Signals corresponding to X0 and Xk. arrive at the fifth and sixth inputs of the first register 43 of the second

0 вычислительного блока 15. Управл ющие сигналы на запись подаютс на первый и второй входы блока 43 с первого и второго выходов распределител 41 импульсов (темп работы блока 41 задаетс генерато5 ром 42 тактовых импульсов). Сигналы, соответствующие значени м Y0 и YK, поступают на п тый и шестой входы второго регистра 44. Управл ющие сигналы на запись подаютс на первый и второй входы блока 44 с0 of the computing unit 15. Recording control signals are supplied to the first and second inputs of the unit 43 from the first and second outputs of the pulse distributor 41 (the operation rate of the unit 41 is set by the clock generator 5 42). The signals corresponding to the values of Y0 and YK are fed to the fifth and sixth inputs of the second register 44. Control signals for recording are fed to the first and second inputs of block 44 s

0 третьего и четвертого выходов распределител 41 импульсов. Сигналы, соответствующие значени м Z0 и ZK, поступают на п тый и шестой входы третьего регистра 45. Управл ющие сигналы на запись подаютс на0 of the third and fourth outputs of the distributor 41 pulses. The signals corresponding to the values of Z0 and ZK are fed to the fifth and sixth inputs of the third register 45. The control signals for recording are fed to

5 первый и второй входы блока 45 с п того и шестого выходов распределител 41 импульсов . Сигналы, соответствующие значени м i,2 и 1з, поступают на седьмой, восьмой и дев тый входы четвертого регист0 ра 46. Управл ющие сигналы на запись подаютс на первый, второй и третий входы блока 46 с седьмого, восьмого и дев того выходов распределител 41 импульсов. В дальнейшем осуществл етс вычис5 ление начальных обобщенных координат дл первого, второго и третьего звеньев манипул тора по формулам (1), (8) и (3). При этом промежуточные вычислени производ тс по формулам (10), (12), (5) и (7). Пор док вычислений представлен на циклограмме5, the first and second inputs of block 45 of the fifth and sixth outputs of the pulse distributor 41. The signals corresponding to the values of i, 2, and 1z are fed to the seventh, eighth, and ninth inputs of the fourth register 46. The control signals for recording are fed to the first, second, and third inputs of block 46 from the seventh, eighth, and ninth outputs of the distributor 41 pulses. Subsequently, the initial generalized coordinates are calculated for the first, second and third parts of the manipulator according to formulas (1), (8) and (3). In this case, intermediate calculations are performed according to formulas (10), (12), (5) and (7). The order of calculations is presented in the cyclogram

работы второго вычислительного блока. Сигналы, соответствующие величинам qio, Q20 и дзо, поступают на третьи входы соответственно п того 49, дес того 65 и дев того 60 регистров. При этом управл ющие сигналы на первые входы этих регистров подаютс соответственно с четырнадцатого , сорок первого и тридцать п того выходов распределител 41 импульсов. Аналогичным образом осуществл етс вычисление конечных обобщенных координат дл первого , второго и третьего звеньев манипул тора по формулам (2). (9) и (4). При этом промежуточные вычислени производ тс по формулам (11). (13), (6) и (7). Отличи заключаютс лишь в том, что в качестве исходной информации используютс не величины Х0, Yo Z0, а значени Хк.У и ZK. Сигналы, соответствующие величинам qiK, Р2к и дзк. поступают на третьи входы соответственно одиннадцатого 66, тринадцатого 69 и двенадцатого 67 регистров. При этом управл ющие сигналы на первые входы этих регистров подаютс соответственно с сорок четвертого, сорок восьмого и сорок п того выходов распределител 41 импульсов .the work of the second computing unit. The signals corresponding to qio, Q20, and zo values are supplied to the third inputs of the fifth, ninth, tenth, 65th, and ninth, 60 registers, respectively. In this case, control signals are supplied to the first inputs of these registers from the fourteenth, forty-first and thirty-fifth outputs of the pulse distributor 41, respectively. Similarly, the final generalized coordinates are calculated for the first, second and third parts of the manipulator according to formulas (2). (9) and (4). In this case, intermediate calculations are performed according to formulas (11). (13), (6) and (7). The only difference is that the source information is not the values X0, Yo Z0, but the values Xk. U and ZK. Signals corresponding to qiK, P2k and dzk. arrive at the third inputs, respectively, of the eleventh 66, thirteenth 69 and twelfth 67 registers. In this case, control signals are supplied to the first inputs of these registers from the forty-fourth, forty-eighth and forty-fifth outputs of the pulse distributor 41, respectively.

По сигналам с п тнадцатого, сорок дев того , п тьдес т второго, п тьдес т третьего , тридцать шестого и сорок шестого выходов распределител 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.According to the signals from the fifteenth, forty-nine, fifty-second, fifty-third, thirty-sixth and forty-sixth outputs of the pulse distributor 41, the signals corresponding to the values qio, qiK, q2o, q2K, q3o, q3R come from the outputs, respectively, p 49, eleventh 66, tenth 65, thirteenth 69, ninth 60 and twelfth 67 registers through the twelfth 70 and thirteenth 71 switches, qao and q2K directly, eighth 61 and eleventh 68 switches to the first, second, third, fourth, fifth and the sixth outputs of the second computing unit 15. From the first and second outputs of block 15, signals corresponding to qio and qu. respectively, are fed to the first and second inputs of the third computing unit 16. From the third and fourth outputs of the block 15, signals corresponding to q2o and q2K are supplied, respectively, to the first and second inputs of the fourth computing unit 17. From the fifth and sixth outputs of block 15 qao and q3K. respectively, are fed to the first and second inputs of the fifth computing unit 18. The third inputs of the third 16, fourth 17 and fifth 18 computing blocks from the input of the positioning timing device receive a signal in analog form corresponding to tn.

Блоки 16, 17 и 18 работают идентично и поэтому обладают одинаковой структурой, определ законы изменени положени , скорости и ускорени дл трех различныхBlocks 16, 17 and 18 work identically and therefore have the same structure, determining the laws of change of position, speed and acceleration for three different

звеньев манипул тора. Работу одного из этих блоков, например третьего вычислительного блока 16 осуществл етс следующим образом. С первого и второго входов блока 16 сигналы, соответствующие величи0 нам qio и qu поступают соответственно на первый и второй входы блока 136 вычитани по модулю. Кроме того сигнал, соответствующий величине qio, через двенадцатый элемент ИЛИ 137 поступает на вход цифро5 аналогового преобразовател 138. С выхода блока 138 сигнал, соответствующий величине qi0, уже в аналоговой форме поступает через четырнадцатый коммутатор 114 (при этом подаетс управл ющий сигнал на егоlinks of the manipulator. The operation of one of these units, e.g., the third computing unit 16, is carried out as follows. From the first and second inputs of block 16, the signals corresponding to the quantities qio and qu are respectively supplied to the first and second inputs of block 136 of subtraction modulo. In addition, the signal corresponding to qio through the twelfth OR element 137 is input to digital5 of analog converter 138. From the output of block 138, the signal corresponding to qi0 already arrives in analog form through the fourteenth switch 114 (in this case, a control signal is supplied to it

0 вход с третьего выхода блока 113 синхронизации ) на вход двенадцатого элемента 131 задержки. С выхода блока 136 вычитани по модулю сигнал, соответствующий величине IqiK-qiol, поступает через двенадцатый0 input from the third output of synchronization block 113) to the input of the twelfth delay element 131. From the output of the unit 136 of subtraction modulo the signal corresponding to the value IqiK-qiol, comes through the twelfth

5 элемент ИЛИ 137 на вход цифроаналогово- го преобразовател 138. С выхода блока 138 сигнал, соответствующий величине lqiK-qio, в аналоговой форме поступает через четырнадцатый коммутатор 114 (при5 OR element 137 to the input of the digital-analog converter 138. From the output of block 138, the signal corresponding to the value lqiK-qio, in analog form, enters through the fourteenth switch 114 (at

0 этом управл ющий сигнал на его вход не подаетс ) на вход одиннадцатого элемента 129 задержки.0, a control signal is not supplied to its input) to the input of the eleventh delay element 129.

С третьего входа третьего вычислительного блока 16 сигнал соответствующий ве5 личине времени позиционировани . tn подаетс на третий вход четырнадцатого регистра 112. Управл ющий сигнал подаетс на второй вход блока 112с первого выхода блока 113 синхронизации. С п того выходаFrom the third input of the third computing unit 16, a signal corresponding to a value of positioning time. tn is supplied to the third input of the fourteenth register 112. A control signal is supplied to the second input of the block 112c of the first output of the synchronization block 113. Since that exit

0 блока 113 подаетс сигнал на счетчик 115 времени дл его обнулени , а затем с четвертого выхода блока 113 синхронизации поступает сигнал на счетный вход блока 115 дл его запуска. С выхода счетчика 115 вре5 мени текущие значени времени t подаютс на первый вход третьего блока 116 делени , на второй вход которого засылаетс по сигналу с второго выхода блока 113 синхронизации на второй вход регистра 112 величина0, block 113 sends a signal to a time counter 115 to reset it, and then a signal is received from the fourth output of synchronization block 113 to the counting input of block 115 to start it. From the output of the time counter 115, the current time t is supplied to the first input of the third division unit 116, the second input of which is sent by the signal from the second output of the synchronization unit 113 to the second input of the register 112

0 tn. С выхода блока 116 сигнал, соответствующий величине t/tn. поступает на входы блока 121 возведени в куб, седьмого 117 и восьмого 119 элементов задержки. С выхода блока 121 сигнал, соответствующий величи5 не (t/tn)3, подаетс на второй вход шестого блока 118 умножени и на вход первого элемента 122 масштабировани . С выхода блока 122 сигнал, соответствующий величине 10 (t/tn)3, поступает на вход дев того элемента 123 задержки.0 tn. From the output of block 116, the signal corresponding to the value of t / tn. arrives at the inputs of cube block 121, seventh 117 and eighth 119 delay elements. From the output of block 121, a signal corresponding to 5 not (t / tn) 3 is supplied to the second input of the sixth multiplication block 118 and to the input of the first scaling element 122. From the output of block 122, a signal corresponding to a value of 10 (t / tn) 3 is input to the ninth delay element 123.