RU2129953C1 - Apparatus for program control of manipulator - Google Patents

Abstract

FIELD: robotics, namely systems for program control of manipulators with standard kinematic circuit. SUBSTANCE: apparatus forms control signals fed to each drive unit of respective freedom degree of manipulator according to predetermined motion law of characteristic point of grip and its orientation in three dimensional space. Control signals are formed in real time by means of standard units. EFFECT: formation of control signals fed to drives of all freedom degrees of manipulator according to time functions characterizing linear movement of grip point in space. 3 dwg

Description

 The invention relates to robotics and can be used to create contour control systems for multi-stage manipulators.

 A device for numerical program control is known that contains a logic element, the first input of which is connected to the learning console, its second input is to the operator’s console, the third to the magnetic tape drive, the fourth is connected to the output of the microcomputer via the bus coordinator, the fifth and sixth are connected respectively to shapers of technological teams installed on the equipment, and with feedback sensors, moreover, from the first output of this logic element, control signals from servo drives are sent, and from its second output - technological teams for equipment (see G. A. Spin Industrial Works. Design and Application - Kiev: Vyshcha Shkola, 1991, pp. 165-166).

 The disadvantage of this device is that with its help it is possible to generate a control program for the drives only in the training mode by personnel, when the necessary movements of the robot arm are reproduced by the operator, and the signals that are generated in each degree of the manipulator's mobility are recorded in the memory of the control device. Then, in automatic mode, the movements recorded in each degree of mobility are reproduced. This device does not allow generating control signals for the drive of each degree of mobility on the basis of information about the current position of the characteristic gripping point and its orientation. That is, this device is not able to solve the inverse kinematics problem for a given manipulator design.

 A device for programmatically controlling a manipulator is also known, comprising a program setting unit, a drive for each adjustable coordinate, a first adder connected in series, a first input of which is connected to a first output of a program set unit, a first division unit, a first functional converter, a second adder, a first relay element, the first multiplication unit, the second functional converter, the second division unit and the third functional converter, connected in series with the fourth function an al converter, a second multiplication unit, a third multiplication unit, the second input of which is connected to the seventh output of the program setting unit, a third adder, a fifth functional converter and a fourth multiplication unit, connected in series with the third division unit, and a sixth functional converter, the output of which is connected to the second input the second adder, the fifth multiplication unit, the fourth adder and the second relay element, the output of which is connected to the second input of the fourth unit are multiplied in series connected in series to the sixth multiplication unit, a fifth adder, the output of which is connected to the first input of the fifth multiplication unit, and its second input to the output of the seventh multiplication unit, in series with the eighth multiplication unit, the first input of which is connected to the ninth output of the program task unit, the sixth an adder, the second input of which through the ninth multiplication unit is connected to the eighth output of the program task unit, the ninth multiplication unit, the second input of which is connected to the second output of the program task unit, second to the first adder and the first input of the third division unit, and its output - with the second input of the fourth adder, the seventh functional converter connected in series, the input of which is connected to the input of the fourth functional converter and the eleventh multiplication unit, the second input of which is connected to the first input of the seventh multiplication unit , its output is with the second input of the third adder, and its first input is with the second input of the sixth multiplication unit, the eighth functional transform is connected in series spruce, twelfth multiplication block, seventh adder, the second input of which through the thirteenth multiplication block is connected to the second input of the second multiplication block and the ninth functional converter, the output of which is connected to the second input of the first multiplication block, the fourteenth multiplication block is connected in series, the first input of which is connected to the output the eighth functional converter, and the second with the output of the fourth functional converter, and the fifteenth multiplication block, the output of which is connected to the third input ode of the third adder, the eighth adder, the first input of which is connected to the third output of the program unit and the second inputs of the third division unit and the fifth multiplication unit, its second input - with the fourth output of the program unit, and the output - with the second input of the first division unit and the first the input of the fourth division block, the ninth adder, the first input of which is connected to the fifth, and the second to the sixth output of the program task block (see RF patent N 2054349 MKI B 25 J 9/16, BI N 5, 1996).

 This device is the closest to the proposed solution. It allows you to generate control signals to the drives of all degrees of mobility of the manipulator, which provide the required movement of the grip in three-dimensional space with a given orientation. That is, this device provides a solution to the inverse kinematics problem in real time.

 The disadvantage of this device is that it is intended for a different type of manipulator working in a cylindrical coordinate system, and cannot be used for manipulators of other types.

 The task to which the proposed technical solution is directed is to create such a control device that, using known current linear coordinates of the characteristic gripping point and angular coordinates characterizing its orientation in three-dimensional space in real time, could generate such control signals to the drives all degrees of mobility of a particular manipulator, which would provide the required movement of the grip in space with a given orientation. That is, this device should provide a solution to the inverse kinematics problem in real time, taking into account the design and kinematic capabilities of the manipulator in question.

 The technical result that is achieved in solving this problem is that without tedious and lengthy training operations, as well as without expensive and complex control devices, it is possible to immediately generate the required control signals to the drives of all degrees of manipulator mobility, being set only by time functions characterizing linear displacements of a certain point of the grip in three-dimensional space, as well as time functions characterizing the orientation of the grip in this space.

 The problem is solved in that the device for programmatically controlling the manipulator, comprising a program task unit, for each adjustable coordinate, a drive, a first adder connected in series, the first input of which is connected to the first output of the program task unit, a first division unit, a first functional converter, a second adder, the first relay element, the first multiplication unit, the second functional converter, the second division unit and the third functional converter, connected in series the fourth functional converter, the second multiplication unit, the third multiplication unit, the second input of which is connected to the eighth output of the program unit, the third adder, the fifth functional converter and the fourth multiplication unit, the third division unit connected in series, and the sixth functional converter, the output of which is connected to the second input of the second adder, the fifth multiplication unit, the fourth adder and the second relay element, the output of which is connected to the second input h of the fourth multiplication block, the sixth multiplication block is connected in series, the fifth adder, the output of which is connected to the first input of the fifth multiplication block, and its second input is the output of the seventh multiplication block, the eighth multiplication block is connected in series, the first input of which is connected to the ninth output of the program task block the sixth adder, the second input of which through the ninth multiplication block is connected to the eighth output of the program task block, the tenth multiplication block, the second input of which is connected to the second output of the rear block program, the second input of the first adder and with the first input of the third division block, and its output with the second input of the fourth adder, connected in series to the seventh functional converter, the input of which is connected to the input of the fourth functional converter and the eleventh multiplication unit, the second input of which is connected to the first the input of the seventh multiplication block, its output is with the second input of the third adder, and its first input is with the second input of the sixth multiplication block, connected in series with the eighth function the onal converter, the twelfth multiplication unit, the seventh adder, the second input of which through the thirteenth multiplication unit is connected to the second input of the second multiplication unit and the ninth functional converter, the output of which is connected to the second input of the first multiplication unit, connected in series with the fourteenth multiplication unit, the first input of which is connected to the output of the eighth functional converter, and the second with the output of the fourth functional converter, and the fifteenth multiplication block, the output of which connected to the third input of the third adder, the eighth adder, the first input of which is connected to the third output of the program unit and the second inputs of the third division unit and the fifth multiplication unit, its second input to the fourth output of the program unit, and the output to the second input of the first unit division and the first input of the fourth division unit, the ninth adder, the first input of which is connected to the fifth, and the second to the sixth output of the program task unit, differs in that it is additionally introduced in series connected the first constant signal source, the tenth adder, the second input of which is connected to the output of the ninth adder, the sixteenth multiplication unit, the first quadrator, the eleventh adder, the tenth functional converter, the fifth division unit, the eleventh functional converter, the twelfth adder, the twelfth functional converter, the seventeenth multiplication unit, thirteenth adder, thirteenth functional converter, fourteenth and fifteenth adders, the second inputs of which are connected to the output of the door the twelfth adder, the fourteenth functional converter, the eighteenth multiplication unit, the sixteenth adder, the second input of which is connected to the fifth output of the program setting unit and the second input of the thirteenth multiplication unit, the first unit of which is connected to the output of the fourteenth functional converter, the third relay element, the nineteenth multiplication unit, the twentieth a multiplication unit, the second input of which is connected to the output of the third functional converter, and a tracking and storage element, the second input of which through the fourth relay element connected to the output of the second functional converter, and its output to the input of the fourth functional converter, a second constant signal source connected in series, a second quadrator, seventeenth adder, the second input of which is connected to the third constant signal source, the third input through the third quadrator with the output of the tenth adder and the second inputs of the fourth division block and the thirteenth adder, the eighteenth adder, the output of which is connected to the second input the fifth division block, and its second input through the fourth quadrator - with the second input of the first division block and connected in series with the twenty-first multiplication block, the second input of which is connected to the output of the second constant signal source and the second inputs of the sixteenth and seventeenth multiplication blocks, and the fifth quadrator, the output which is connected to the second input of the eleventh adder, the fifteenth functional converter, the input of which is connected to the inputs of the second functional converter and the fifth relay element, and the output is with the second input of the eighteenth multiplication block, the sixteenth functional converter, the input of which is connected to the output of the fourth division block, and the output is with the second input of the twelfth adder, and the second inputs of the sixth and fifteenth multiplication blocks are connected to the ninth output of the program task block, the second input of the second division block is connected to the second output of the program task block, the second inputs of the seventh and ninth multiplication blocks are to the output of the fourteenth multiplication block, the third output of the task block programs to the second input of the twelfth multiplication block, the output of the fifth relay element to the second input of the nineteenth multiplication block, the output of the fifteenth adder to the input of the eighth functional converter, the second input of the third multiplication block to the eighth output of the program task block, and the second input of the eighth multiplication block - to the output of the second multiplication block, and the outputs of the first, twelfth and fourteenth adders, as well as the tracking element - storage, the first and fourth multiplication blocks are connected to the inputs drives located respectively in the first, second, third, fourth, fifth and sixth degrees of mobility of the manipulator.

 A comparative analysis of the features of the proposed solutions with the signs of analogues and prototype indicates its compliance with the criterion of "novelty."

 The claimed combination of features, given in the characterizing part of the claims, allows real-time control actions to be generated for all degrees of mobility of the manipulator under consideration when defining time functions for changing the position of the characteristic gripping point in three-dimensional space and temporal functions that determine the gripping orientation in this space.

 In FIG. 1 shows a block diagram of the proposed control device, and in FIG. 2 - kinematic diagram of a six-step manipulator; in FIG. 3 - a characteristic of the functional Converter 75.

r - вектор, задающий положение характерной точки схвата P в трехмерном пространстве, определяемом неподвижной системой координат xyz; r_x, r_y, r_z - координаты вектора r (характерной точки схвата манипулятора) в трехмерном пространстве: R - вектор, определяющий положение шарнира (точка W) пятой степени подвижности (координата q₅) в трехмерном пространстве xyz; R_x, R_y, R_z - координаты вектора R в трехмерном пространстве xyz; L₁, L₂, L₃, L₄ - длины соответствующих звеньев манипулятора; a, b - единичные векторы, расположенные в плоскости схвата и определяющие его ориентацию в трехмерном пространстве xyz; a_x, a_y, a_z, b_x, b_y, b_z - соответствующие координаты векторов a и b в трехмерном пространстве xyz; e_i - единичные векторы, совпадающие с осями шарниров степеней подвижности манипулятора (i =

Устройство для программного управления манипулятором (см. фиг. 1) содержит блок 1 задания программы, по каждой регулируемой координате привод, последовательно соединенные первый сумматор 2, первый вход которого подключен к первому выходу блока 1 задания программы, первый блок 3 деления, первый функциональный преобразователь 4, второй сумматор 5, первый релейный элемент 6, первый блок 7 умножения, второй функциональный преобразователь 8, второй блок 9 деления и третий функциональный преобразователь 10, последовательно соединенные четвертый функциональный преобразователь 11, второй блок 12 умножения, третий блок 13 умножения, второй вход которого подключен к восьмому выходу блока 1 задания программы, третий сумматор 14, пятый функциональный преобразователь 15 и четвертый блок 16 умножения, последовательно соединенные третий блок 17 деления и шестой функциональный преобразователь 18, выход которого соединен со вторым входом второго сумматора 5, последовательно соединенные пятый блок 19 умножения, четвертый сумматор 20 и второй релейный элемент 21, выход которого соединен со вторым входом четвертого блока 16 умножения, последовательно соединенные шестой блок 22 умножения, пятый сумматор 23, выход которого соединен с первым входом пятого блока 19 умножения, а его второй вход - с выходом седьмого блока 24 умножения, последовательно соединенные восьмой блок 25 умножения, первый вход которого подключен к девятому выходу блока 1 задания программы, шестой сумматор 26, второй вход которого через девятый блок 27 умножения соединен с восьмым выходом блока 1 задания программы, десятый блок 28 умножения, второй вход которого соединен со вторым выходом блока 1 задания программы, вторым входом первого сумматора 2 и с первым входом третьего блока 17 деления, а его выход - со вторым входом четвертого сумматора 20, последовательно соединенные седьмой функциональный преобразователь 29, вход которого подключен ко входу четвертого функционального преобразователя 11 и одиннадцатый блок 30 умножения, второй вход которого соединен с первым входом седьмого блока 24 умножения, его выход - со вторым входом третьего сумматора 14, а его первый вход - со вторым входом шестого блока 22 умножения, последовательно соединенные восьмой функциональный преобразователь 31, двенадцатый блок 32 умножения, седьмой сумматор 33, второй вход которого через тринадцатый блок 34 умножения подключен ко второму входу второго блока 12 умножения и девятый функциональный преобразователь 35, выход которого соединен со вторым входом первого блока 7 умножения, последовательно соединенные четырнадцатый блок 35 умножения, первый вход которого соединен с выходом восьмого функционального преобразователя 31, а второй - с выходом четвертого функционального преобразователя 11, и пятнадцатый блок 37 умножения, выход которого подключен к третьему входу третьего сумматора 14, восьмой сумматор 38, первый вход которого соединен с третьим выходом блока 1 задания программы и вторыми входами третьего блока 17 деления и пятого блока 19 умножения, его второй вход - с четвертым выходом блока 1 задания программы, а выход - со вторым входом первого блока 3 деления и первым входом четвертого блока 39 деления, девятый сумматор 40, первый вход которого соединен с пятым, а второй - с шестым выходом блока 1 задания программы, последовательно соединенные первый источник 41 постоянного сигнала, десятый сумматор 42, второй вход которого соединен с выходом девятого сумматора 40, шестнадцатый блок 43 умножения, первый квадратор 44, одиннадцатый сумматор 45, десятый функциональный преобразователь 46, пятый блок 47 деления, одиннадцатый функциональный преобразователь 48, двенадцатый сумматор 49, двенадцатый функциональный преобразователь 50, семнадцатый блок 51 умножения, тринадцатый сумматор 52, тринадцатый функциональный преобразователь 53, четырнадцатый 54 и пятнадцатый 55 сумматоры, вторые входы которых подключены к выходу двенадцатого сумматора 49, четырнадцатый функциональный преобразователь 56, восемнадцатый блок 57 умножения, шестнадцатый сумматор 58, второй вход которого соединен с пятым выходом блока 1 задания программы и вторым входом тринадцатого блока 34 умножения, первый вход которого подключен к выходу четырнадцатого функционального преобразователя 56, третий релейный элемент 59, девятнадцатый блок 60 умножения, двадцатый блок 61 умножения, второй вход которого соединен с выходом третьего функционального преобразователя 10, и элемент 62 слежения - хранения, второй вход которого через четвертый релейный элемент 63 подключен к выходу второго функционального преобразователя 8, а его выход - ко входу четвертого функционального преобразователя 11, последовательно соединенные второй источник 64 постоянного сигнала, второй квадратор 65, семнадцатый сумматор 66, второй вход которого соединен с третьим источником 67 постоянного сигнала, третий вход через третий квадратор 68 - с выходом десятого сумматора 42 и вторыми входами четвертого блока 39 деления и тринадцатого сумматора 52, восемнадцатый сумматор 69, выход которого соединен со вторым входом пятого блока 47 деления, а его второй вход через четвертый квадратор 70 - со вторым входом первого блока 3 деления и последовательно соединенными двадцать первым блоком 71 умножения, второй вход которого подключен к выходу второго источника 64 постоянного сигнала и вторым входам шестнадцатого 43 и семнадцатого 51 блоков умножения, и пятый квадратор 72, выход которого соединен со вторым входом одиннадцатого сумматора 45, пятнадцатый функциональный преобразователь 73, вход которого соединен со входами второго функционального преобразователя 8 и пятого релейного элемента 74, а выход - со вторым входом восемнадцатого блока 57 умножения, шестнадцатый функциональный преобразователь 75, вход которого соединен с выходом четвертого блока 39 деления, а выход - со вторым входом двенадцатого сумматора 49, причем вторые входы шестого 22 и пятнадцатого 37 блоков умножения подключены к девятому выходу блока 1 задания программы, второй вход второго блока 9 деления подключен ко второму выходу блока 1 задания программы, вторые входы седьмого 24 и девятого 27 блоков умножения - к выходу четырнадцатого блока 36 умножения, третий выход блока 1 задания программы - ко второму входу двенадцатого блока 32 умножения, выход пятого релейного элемента 74 - ко второму входу девятнадцатого блока 60 умножения, выход пятнадцатого сумматора 55 - ко входу восьмого функционального преобразователя 31, второй вход третьего блока 13 умножения - к восьмому выходу блока 1 задания программы, а второй вход восьмого блока 25 умножения - к выходу второго блока 12 умножения, причем выходы первого 2, двенадцатого 49 и четырнадцатого 54 сумматора, а также элемента 62 слежения - хранения, первого 7 и четвертого 16 блоков умножения соединены со входами приводов, расположенных соответственно в первой, второй, третьей, четвертой, пятой и шестой степенях подвижности манипулятора.The following notation is introduced in these drawings: q _i is the generalized coordinate of the degree of mobility i of the manipulator, these are the control coordinates of the corresponding drives of the manipulator (i =

r is a vector defining the position of the characteristic gripping point P in three-dimensional space defined by the fixed coordinate system xyz; r _x , r _y , r _z are the coordinates of the vector r (the characteristic grip point of the manipulator) in three-dimensional space: R is the vector defining the position of the hinge (point W) of the fifth degree of mobility (coordinate q ₅ ) in three-dimensional space xyz; R _x , R _y , R _z - coordinates of the vector R in the three-dimensional space xyz; L ₁ , L ₂ , L ₃ , L ₄ - the lengths of the corresponding links of the manipulator; a, b are unit vectors located in the grasp plane and determining its orientation in the three-dimensional space xyz; a _x , a _y , a _z , b _x , b _y , b _z - the corresponding coordinates of the vectors a and b in the three-dimensional space xyz; e _i are unit vectors coinciding with the axes of the hinges of the degrees of mobility of the manipulator (i =

A device for programmatically controlling the manipulator (see Fig. 1) contains a program task unit 1, a drive for each adjustable coordinate, a first adder 2 connected in series, the first input of which is connected to the first output of the program task unit 1, the first division unit 3, the first functional converter 4, the second adder 5, the first relay element 6, the first multiplication unit 7, the second functional converter 8, the second division unit 9 and the third functional converter 10, connected in series to the fourth the national converter 11, the second multiplication block 12, the third multiplication block 13, the second input of which is connected to the eighth output of the program setting block 1, the third adder 14, the fifth functional converter 15 and the fourth multiplication block 16, connected in series with the third division block 17 and the sixth functional converter 18, the output of which is connected to the second input of the second adder 5, the fifth multiplication unit 19, the fourth adder 20 and the second relay element 21, the output of which is connected to the second input, are connected in series of the fourth multiplication unit 16, connected in series with the sixth multiplication unit 22, the fifth adder 23, the output of which is connected to the first input of the fifth multiplication unit 19, and its second input - with the output of the seventh multiplication unit 24, connected in series with the eighth multiplication unit 25, the first input of which is connected to the ninth output of the program task block 1, the sixth adder 26, the second input of which is connected through the ninth multiplication block 27 to the eighth output of the program task block 1, the tenth multiplication block 28, the second input of which is connected the output of the program setting block 1, the second input of the first adder 2 and with the first input of the third division unit 17, and its output - with the second input of the fourth adder 20, sequentially connected to the seventh functional converter 29, the input of which is connected to the input of the fourth functional converter 11 and the eleventh block 30 multiplication, the second input of which is connected to the first input of the seventh multiplication unit 24, its output - with the second input of the third adder 14, and its first input - with the second input of the sixth multiplication unit 22, the next the eighth functional converter 31, the twelfth multiplication unit 32, the seventh adder 33, the second input of which through the thirteenth multiplication unit 34 is connected to the second input of the second multiplication unit 12 and the ninth functional converter 35, the output of which is connected to the second input of the first multiplication unit 7, are connected in series connected the fourteenth multiplication block 35, the first input of which is connected to the output of the eighth functional converter 31, and the second to the output of the fourth functional converter 11, and the fifteenth multiplication unit 37, the output of which is connected to the third input of the third adder 14, the eighth adder 38, the first input of which is connected to the third output of the program unit 1 and the second inputs of the third division unit 17 and the fifth multiplication unit 19, its second input is with the fourth output of block 1 of the program task, and the output with the second input of the first block 3 division and the first input of the fourth block 39 division, the ninth adder 40, the first input of which is connected to the fifth, and the second with the sixth output of the block 1 of the program task, the follower about connected the first constant signal source 41, the tenth adder 42, the second input of which is connected to the output of the ninth adder 40, the sixteenth multiplication unit 43, the first quadrator 44, the eleventh adder 45, the tenth functional converter 46, the fifth division unit 47, the eleventh functional converter 48, twelfth adder 49, twelfth functional converter 50, seventeenth multiplier 51, thirteenth adder 52, thirteenth functional converter 53, fourteenth 54 and fifteenth 55 adders, sec whose inputs are connected to the output of the twelfth adder 49, the fourteenth functional converter 56, the eighteenth multiplication unit 57, the sixteenth adder 58, the second input of which is connected to the fifth output of the program setting unit 1 and the second input of the thirteenth multiplication unit 34, the first input of which is connected to the fourteenth output functional converter 56, third relay element 59, nineteenth multiplication unit 60, twentieth multiplying unit 61, the second input of which is connected to the output of the third functional conversion atelier 10, and a tracking and storage element 62, the second input of which through the fourth relay element 63 is connected to the output of the second functional converter 8, and its output is connected to the input of the fourth functional converter 11, the second constant signal source 64 connected in series, the second quadrator 65, seventeenth an adder 66, the second input of which is connected to the third constant signal source 67, the third input through the third quadrator 68 - with the output of the tenth adder 42 and the second inputs of the fourth division unit 39 and the thirteenth the adder 52, the eighteenth adder 69, the output of which is connected to the second input of the fifth division unit 47, and its second input through the fourth quadrator 70 - to the second input of the first division unit 3 and connected in series to the twenty-first multiplication unit 71, the second input of which is connected to the output of the second a constant signal source 64 and second inputs of the sixteenth 43 and seventeenth 51 multiplication blocks, and a fifth quadrator 72, the output of which is connected to the second input of the eleventh adder 45, the fifteenth functional converter 73, One of which is connected to the inputs of the second functional converter 8 and the fifth relay element 74, and the output is connected to the second input of the eighteenth multiplication unit 57, the sixteenth functional converter 75, whose input is connected to the output of the fourth division unit 39, and the output - to the second input of the twelfth adder 49 and the second inputs of the sixth 22 and fifteenth 37 multiplication blocks are connected to the ninth output of the program task block 1, the second input of the second division block 9 is connected to the second output of the program task block 1, the second the moves of the seventh 24 and ninth 27 blocks of multiplication - to the output of the fourteenth block 36 of multiplication, the third output of block 1 of the program job - to the second input of the twelfth block 32 of multiplication, the output of the fifth relay element 74 - to the second input of the nineteenth block 60 of multiplication, the output of the fifteenth adder 55 - to the input of the eighth functional converter 31, the second input of the third multiplication block 13 to the eighth output of the program unit 1, and the second input of the eighth multiplication block 25 to the output of the second multiplication block 12, the outputs of the first 2, twelve the fourth 49th and fourteenth 54 adders, as well as the tracking element 62 for storage, the first 7 and fourth 16 multiplication units are connected to the inputs of the drives located respectively in the first, second, third, fourth, fifth and sixth degrees of mobility of the manipulator.

 A device for controlling the manipulator operates as follows.

), которые необходимо подать одновременно на все шесть приводов манипулятора, установленных в соответствующие степени подвижности для обеспечения задаваемого блоком 1 закона перемещения схвата в пространстве xyz, т.е. для обеспечения закона изменения векторов r = r(t), a = a(t), b = b(t), где t - текущий момент времени.Block 1 of the program task generates the coordinates of the vector r characterizing the current position of the midpoint P of the grip of the manipulator in the three-dimensional space xyz, as well as the current coordinates of the vectors a and b, characterizing the orientation of this grip in the specified space at a particular moment in time. The coordinates of the vector r: r _x , r _y , r _z are formed respectively at the first, fourth and sixth outputs of block 1 of the program task, the coordinates of the vector a: a _x , a _y , a _z are respectively at the second, third and fifth outputs, and the coordinates vectors b: b _x , b _y , b _z - respectively at the seventh, eighth and ninth outputs of this block. All specified coordinates are functions of time. The proposed device should provide the formation of the required control signals q _i (i =

), which must be submitted simultaneously to all six manipulator drives installed in the appropriate degrees of mobility to ensure the grip displacement law specified in block 1 in the space xyz, i.e. to ensure the law of variation of the vectors r = r (t), a = a (t), b = b (t), where t is the current time.

где φ = q₂ + q₃.The following restrictions are imposed on the generalized coordinates of the manipulator:

where φ = q ₂ + q ₃ .

The counting of the rotational degrees of mobility begins from the position of the manipulator shown in FIG. 2. In this case, counterclockwise movement is considered positive, and clockwise - negative. The direction of rotation is determined relative to the corresponding vectors e _i if the gaze is directed from the arrow to the base of the vector e _i .

From FIG. 2 shows that always holds equality
R = r - L ₄ e ₆ = r - L ₄ a.

The first positive input of adder 2 has a unity gain, its second negative input has a gain of L ₄ . As a result, at the output of this adder, a signal q ₁ = R _x = r _x -L ₄ • a _x is generated.

The first negative inputs of the adders 38 and 40 have gains L ₄ , and their second positive inputs have gains equal to unity. As a result, at the outputs of these adders, signals equal to
R _y = r _y - L ₄ a _y and R _z = r _z - L ₄ a _z .

На выходе блока умножения 71 имеем сигнал R_y • f₂. Квадратор 72 имеет коэффициент усиления 1/L² ₃. В результате на его выходе формируется сигнал

Первый и второй положительные входы сумматора 45 имеют коэффициенты усиления 4, поэтому на его выходе имеем сигнал -g₁ ² + g₂ ² = 4 (f₁ ² • f₂ ² + f₂ ² • f₃ ²). Функциональный преобразователь 46 реализует функцию извлечения квадратного корня. В результате на его выходе формируется сигнал

Квадратор 65 имеет единичный коэффициент усиления. В результате на его выходе формируется сигнал f₂ ². Квадратор 68 имеет коэффициент усиления 1/L₃ ². Поэтому на его выходе формируется сигнал

Источник постоянного сигнала 67 имеет на выходе единичный сигнал.A constant signal source 41 generates a signal L ₁ . Both outputs of the adder 42 have unity gain, as a result of the output of this adder produces a signal equal to (R _z - L ₁ ). At the output of the division unit 39, a signal R _y / (R _z -L ₁ ) is generated. A constant source 64 produces a signal f ₂ = −L ₂ / L ₃ . At the output of the multiplication block 43, a signal is generated (R _z - L ₁ ) • f ₂ . Quadrator 44 has a gain of 1 / L ² ₃ . As a result, we have a signal at its output

At the output of the multiplication block 71, we have a signal R _y • f ₂ . Quadrator 72 has a gain of 1 / L ² ₃ . As a result, a signal is generated at its output.

The first and second positive inputs of the adder 45 have a gain of 4, so at its output we have a signal -g ₁ ² + g ₂ ² = 4 (f ₁ ² • f ₂ ² + f ₂ ² • f ₃ ² ). Functional transducer 46 implements the square root extraction function. As a result, a signal is generated at its output.

Quadrator 65 has a unity gain. As a result, a signal f ₂ ² is generated at its output. Quadrator 68 has a gain of 1 / L ₃ ² . Therefore, a signal is generated at its output

The constant signal source 67 has a single signal output.

Первый положительный и второй отрицательный входы сумматора 69 имеют единичные коэффициенты усиления. Поэтому на его выходе формируется сигнал g₃ = 1 - f₁ ² - f₂ ² - f₃ ², а на выходе блока деления 47 сигнал

Функциональный преобразователь 48 реализует функцию arcsin. Поэтому на его выходе формируется сигнал

Функциональный преобразователь 75 реализует функцию

, причем arctg принимает значения от -π до 0 (см. фиг. 3). Сумматор 49 имеет первый положительный и второй отрицательный входы с единичными коэффициентами усиления. На его выходе формируется сигнал

Функциональный преобразователь 50 реализует функцию sin. На выходе блока умножения 51 реализуется сигнал f₂•sin q₂. Первый положительный вход сумматора 52 имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления, равный 1/L₃, поэтому на его выходе формируется сигнал f₁ + f₂•sin q₂. Функциональный преобразователь 53 реализует функцию arcsin.The first, third negative inputs of the adder 66 and its second positive input have unity gain. As a result, a signal equal to (1 - f ₂ ² - f ₁ ² ) is formed at its output. Quadrator 70 has a gain of 1 / L ₃ ² . As a result, a signal is produced at its output.

The first positive and second negative inputs of the adder 69 have unity gain. Therefore, a signal g ₃ = 1 - f ₁ ² - f ₂ ² - f ₃ ² is generated at its output, and a signal at the output of the division unit 47

Functional converter 48 implements the arcsin function. Therefore, a signal is generated at its output

Functional Converter 75 implements the function

, and arctg takes values from -π to 0 (see Fig. 3). The adder 49 has first positive and second negative inputs with unity gain. A signal is generated at its output.

Functional converter 50 implements the sin function. At the output of the multiplication block 51, a signal f ₂ • sin q ₂ is realized. The first positive input of the adder 52 has a unity gain, and its second positive input has a gain of 1 / L ₃ , so a signal f ₁ + f ₂ • sin q ₂ is generated at its output. Functional converter 53 implements the arcsin function.

The first positive and second negative inputs of the adder 54 have unity gain. The output of this adder produces a signal q ₃ = arcsin (f ₁ + f ₂ • sin q ₂ ) - q ₂ . The first and second positive inputs of the adder 55 have unity gain. As a result, the signal φ = q ₂ + q ₃ is formed at the output of this adder. Functional converters 31 and 56 implements the functions cos and sin, respectively. At the output of the multiplication block 32, a signal a _y cos φ is generated. The first and second positive inputs of the adder 33 have unity gain. The signal a _y • cos φ + a _z • sin φ is formed at its output.
Functional Converter 35 implements the arccos function. At its output, we have the signal arccos (a _y • cos φ + a _z • sin φ). This signal determines only the absolute value of the generalized coordinate q ₅ . To determine the sign of q _{5, we} use the following fact. If the angle between the positive direction of the Y axis and the projection of the vector a on the XY plane is greater than the angle between the same direction of the Y axis and the projection of the vector R on the XY plane, then q ₅ has a positive value, otherwise it is negative.

At the output of division block 17, a _x / a _y signal is generated. Functional converter 18 implements the arctg function. When a _y -> 0, the signal at the output of block 17 tends to the maximum possible value. In this case, the functional converter 18 is tuned so that if a _y -> 0, depending on the sign of a _x , the signal π / 2 or (-π / 2) would be output. When generating an a _y signal by block 1, a _y = 0 should be avoided exactly.

В результате на выходе блока 7 умножения формируется сигнал
q₅ = ±arccos(a_ycosφ-a_zsinφ).
Функциональный преобразователь 8 реализует функцию (- sin). Блок 9 деления имеет на выходе сигнал

Функциональный преобразователь 10 реализует функцию arccos. В результате на его выходе сигнал определяет абсолютную величину координаты q₄. Определение знака обобщенной координаты q₄ осуществляется при сравнении координаты вектора e_6z = a_z со значением координаты того же вектора e₆ при q₄ = 0, т.е. e_6z ⁰. При положительном знаке q₅ обобщенная координата q₄ имеет положительный знак, если e_6z ⁰ ≤ a_z, в противном случае - отрицательный. При отрицательном знаке q₅ обобщенная координата q₄ имеет положительный знак, если e_6z ≥ a_z, в противном случае - отрицательный. Функциональный преобразователь 73 реализует функцию cos. На выходе блока 57 умножения формируется сигнал
sinφcosq₅ = e $\genfrac{}{}{0ex}{}{\text{0}}{\text{6z}}$ .
Первый отрицательный и второй положительный входы сумматора 58 имеют единичные коэффициенты усиления. На выходе этого сумматора формируется сигнал k = a_z - e_6z ⁰. Этот сигнал поступает на вход релейного элемента 59, на выходе которого реализует сигнал

На выходе релейного элемента 74 реализует сигнал

В результате на выходе блока 60 умножения формирует сигнал, определяющий знак обобщенной координаты q₄, а на выходе блока 61 умножения формируется сигнал

Элемент слежения - хранения 62 обеспечивает работоспособность устройства при q₅ --> 0, когда на вход делителя блока 9 деления поступает близкий к нулю сигнал, т. е. когда этот блок может работать с недопустимо большой погрешностью. При q₅ --> 0 элемент 62 сохраняет значение q₄ неизменным до тех пор, пока q₅ находится в опасной для блока 9 деления зоне. Управляется элемент 62 релейным элементом 63, имеющем следующему характеристику

где Δ малая положительная величина.Block 3 division has the output signal R _x / R _y . Functional Converter 4 implements the arctg function. The first positive and second negative inputs of the adder 5 have unity gain. As a result, a signal α is formed at its output, which is equal to the difference in angles, which, with the positive direction of the Y axis, make up the projections of the vectors a and R onto the XY plane, respectively. The output of the relay element 6 implements a signal equal to

As a result, a signal is generated at the output of the multiplication unit 7
q ₅ = ± arccos (a _y cosφ-a _z sinφ).
Functional Converter 8 implements the function (- sin). Block 9 division has the output signal

Functional converter 10 implements the arccos function. As a result, at its output, the signal determines the absolute value of the coordinate q ₄ . The sign of the generalized coordinate q ₄ is determined by comparing the coordinate of the vector e _6z = a _z with the coordinate value of the same vector e ₆ when q ₄ = 0, i.e. e _6z ⁰ . With a positive sign of q _{5, the} generalized coordinate q ₄ has a positive sign if e _6z ⁰ ≤ a _z , otherwise it is negative. For a negative sign of q _{5, the} generalized coordinate q ₄ has a positive sign if e _6z ≥ a _z , otherwise it is negative. Functional converter 73 implements the function cos. A signal is generated at the output of the multiplication unit 57
sinφcosq ₅ = e $\genfrac{}{}{0ex}{}{\text{0}}{\text{6z}}$ .
The first negative and second positive inputs of the adder 58 have unity gain. The output of this adder produces a signal k = a _z - e _6z ⁰ . This signal is fed to the input of the relay element 59, the output of which implements a signal

The output of the relay element 74 implements a signal

As a result, a signal defining the sign of the generalized coordinate q ₄ is generated at the output of the multiplication block 60, and a signal is generated at the output of the multiplication block 61

The tracking - storage element 62 ensures the operability of the device at q ₅ -> 0, when a signal close to zero arrives at the input of the divider of the division unit 9, i.e., when this block can operate with an unacceptably large error. When q ₅ -> 0, element 62 keeps the q ₄ value unchanged until q ₅ is in the zone dangerous for block 9 of division. The element 62 is controlled by a relay element 63 having the following characteristic

where Δ is a small positive quantity.

Element 62 is in storage mode when | sinq ₅ | <Δ, and in tracking mode when | sinq ₅ |> Δ.
The generalized coordinate q ₆ can be determined from the scalar product of vectors e ₅ = (sin q ₄ , - cos q ₄ • sin φ, cos q ₄ • cos φ) ^T and b = (b _x , b _y , b _z ) ^T.

Functional Converter 29 implements the function sin. At the output of the multiplication unit 30, a signal is generated equal to b _x • sin q ₄ . Functional converter 11 implements the function cos. At the output of the multiplication block 12, a signal cos q ₄ sin φ is generated, and at the output of the multiplication block 13, a signal b _y cos q ₄ sin φ is generated. The signal cos φ cos q ₄ is generated at the output of the multiplication block 36, and the signal b _z cos φ cos q ₄ is generated at the output of the multiplication block 37. The first negative, second and third positive inputs of the adder 14 have unity gain. As a result, a signal is generated at the output of this adder (b _x sin q ₄ - sin φ cos q ₄ b _y + cos φ cos q ₄ b _z ). Functional converter 15 implements the arccos function. At its output, a signal is formed equal to the absolute value of the generalized coordinate q ₆ .

совпадает с вектором a, то q₆ положительна, а в противном случае - отрицательна. В силу введенных ограничений на обобщенные координаты манипулятора невозможно одновременное обнуление проекций векторов γ и a на оси координат X и Y. В результате о совпадении направлений векторов γ и a можно судить, анализируя только две их проекции: на оси x и y. На выходе блока 25 умножения формируется сигнал sin φ cos q₄b_z.To determine the sign of q _{6, we} use the vector product of the vectors e ₅ and b. If

coincides with the vector a, then q _{6 is} positive, and otherwise negative. Due to the introduced restrictions on the generalized coordinates of the manipulator, it is impossible to simultaneously zero out the projections of the vectors γ and a on the coordinate axes X and Y. As a result, the directions of the vectors γ and a can be judged by analyzing only two of their projections: on the x and y axes. At the output of the multiplication block 25, a signal sin φ cos q ₄ b _z is generated.

В результате на выходе блока 16 умножения формируется сигнал
q₆ = ± arccos (b_xsin q₄ - sin φ cos q₄b_y + cos φ q₄b_z).At the output of the adder 26, which has unity gains at the first and second negative inputs, a signal γ _x = -cos q ₄ sin φ b _z - b _y cos q ₄ cos φ is generated, and at the output of the multiplication unit 28, a signal γ _x • a _x .
At the output of the adder 23, which has unity gains at the first negative and second positive inputs, a signal γ _y = b _x cos q ₄ cos φ - sin q ₄ b _z is generated, and a signal γ _y • a _y is generated at the output of the multiplication unit 19. The first and second positive inputs of the adder 20 have unity gain. As a result, a signal β = γ _x • a _x + γ _y • a _y is formed at its output. If the signal β is positive, then q ₆ has a plus sign, otherwise - minus, because a signal is generated at the output of the relay element 21

As a result, a signal is generated at the output of the multiplication block 16
q ₆ = ± arccos (b _x sin q ₄ - sin φ cos q ₄ b _y + cos φ q ₄ b _z ).

), которые поступают на следящие приводы соответствующих степеней подвижности манипулятора и обеспечивают перемещение точки P схвата в трехмерном пространстве по заданной вектором r(t) траектории с заданной векторами a (t) и b (t) ориентацией схвата. Практическая реализация этого устройства не вызывает затруднений, т. к. оно реализовано с помощью типовых электронных элементов и блоков.Thus, using the proposed device, it was possible to completely solve the inverse kinematics problem for the manipulator shown in Fig. 2. I.e. managed to generate all the signals q _i (t) (i =

), which arrive at the follower drives of the corresponding degrees of mobility of the manipulator and provide for the movement of the gripping point P in three-dimensional space along the trajectory given by the r (t) vector with the gripping orientation given by the vectors a (t) and b (t). The practical implementation of this device is not difficult, because it is implemented using standard electronic elements and units.

Claims (1)

 A device for programmatically controlling the manipulator, comprising a program set block, for each adjustable coordinate — a drive, a first adder connected in series, the first input of which is connected to the first output of the program set block, the first division block, the first functional converter, the second adder, the first relay element, the first a multiplication unit, a second functional converter, a second division unit and a third functional converter connected in series with a fourth functional converter The indexer, the second multiplication block, the third multiplication block, the second input of which is connected to the eighth output of the program setting block, the third adder, the fifth functional converter and the fourth multiplication block, the third division block and the sixth functional converter, the output of which is connected to the second input of the second adder sequentially connected to the fifth multiplication unit, the fourth adder and the second relay element, the output of which is connected to the second input of the fourth multiplication unit, the sequence the sixth multiplication unit, the fifth adder, the output of which is connected to the first input of the fifth multiplication unit and its second input is connected to the output of the seventh multiplication unit, are connected in series with the eighth multiplication unit, the first input of which is connected to the ninth output of the program task unit, the sixth adder, the second input of which, through the ninth multiplication block, is connected to the eighth output of the program task block, the tenth multiplication block, the second input of which is connected to the second output of the program task block, the second input of the first sum ator and with the first input of the third division unit, and its output with the second input of the fourth adder, the seventh functional converter, the input of which is connected to the input of the fourth functional converter, and the eleventh multiplication unit, the second input of which is connected to the first input of the seventh multiplication unit, its output is with the second input of the third adder, and its first input is with the second input of the sixth multiplication block, the eighth functional converter, the twelfth block connected in series multiplication, the seventh adder, the second input of which through the thirteenth multiplication unit is connected to the second input of the second multiplication unit and the ninth functional converter, the output of which is connected to the second input of the first multiplication unit, the fourteenth multiplication unit is connected in series, the first input of which is connected to the output of the eighth functional converter, and the second - with the output of the fourth functional converter, and the fifteenth multiplication block, the output of which is connected to the third input of the third adder, the eighth adder, the first input of which is connected to the third output of the program unit and the second inputs of the third division unit and the fifth multiplication unit, its second input - with the fourth output of the program unit, and the output - with the second input of the first division unit and the first input of the fourth division unit, the ninth adder, the first input of which is connected to the fifth, and the second to the sixth output of the program task unit, characterized in that it is additionally introduced in series with the first constant signal source, the tenth adder, the second input of which is connected to the output of the ninth adder, the sixteenth multiplication unit, the first quadrator, the eleventh adder, the tenth functional converter, the fifth division unit, the eleventh functional converter, the twelfth adder, the twelfth functional converter, the seventeenth multiplication unit, the thirteenth adder, the thirteenth functional converter, fourteenth and the fifteenth adders, the second inputs of which are connected to the output of the twelfth adder, the fourteenth functional pre browser, eighteenth multiplication block, sixteenth adder, the second input of which is connected to the fifth output of the program task block and the second input of the thirteenth multiplication block, the first input of which is connected to the output of the fourteenth functional converter, the third relay element, the nineteenth multiplication block, the twentieth multiplication block, the second input which is connected to the output of the third functional converter, and a tracking-storage element, the second input of which is connected through the fourth relay element to the output of of a functional converter, and its output is to the input of the fourth functional converter, a second constant signal source, a second quadrator, a seventeenth adder connected in series, a second input of which is connected to a third constant signal source, a third input through a third quadrator with the output of the tenth adder and second inputs the fourth division block and the thirteenth adder, the eighteenth adder, the output of which is connected to the second input of the fifth division unit, and its second input through the fourth to adrator - with the second input of the first division block and connected in series with the twenty-first multiplication block, the second input of which is connected to the output of the second constant signal source and the second inputs of the sixteenth and seventeenth multiplication blocks, and the fifth quadrator, the output of which is connected with the second input of the eleventh adder, the fifteenth functional a converter whose input is connected to the inputs of the second functional converter and the fifth relay element, and the output is cleverly connected to the second input of the eighteenth block a sixteenth functional converter, the input of which is connected to the output of the fourth division block, and the output is connected to the second input of the twelfth adder, the second inputs of the sixth and fifteenth multiplication blocks connected to the ninth output of the program task block, the second input of the second division block connected to the second output of the block program task, the second inputs of the seventh and ninth multiplication blocks - to the output of the fourteenth multiplication block, the third output of the program task block - to the second input of the twelfth multiplication block, you one relay element to the second input of the nineteenth multiplication block, the output of the fifteenth adder to the input of the eighth functional converter, the second input of the third multiplication block to the eighth output of the program unit, and the second input of the eighth multiplication block to the output of the second multiplication block, and the outputs of the first, the twelfth and fourteenth adders, as well as the tracking element - storage, the first and fourth multiplication units are connected to the inputs of the drives located respectively in the first to sixth degrees of movement spine manipulator.

Images