RU2066626C1 - Robot drive control device - Google Patents

Abstract

FIELD: robotics. SUBSTANCE: variations in loading of parameters during operation of manipulators are caused by substantial interference between ladder-type member degrees of freedom by load weight variations and viscous friction. To generate correction signals, control circuit of drive control device is further provided with second amplifier 30, fourth functional converter 31, fifth functional converter 35, sixth functional converter 42, seventh multiplier unit 32, eighth multiplier unit 34, ninth multiplier unit 36, tenth multiplier unit 37, eighth summer 33 and ninth summer 41, third speed detector 38, second squarer and third constant signal setter 40. EFFECT: increased efficiency and enhanced reliability in operation. 2 dwg

Description

Изобретение относится к робототехнике и может быть использовано для создания систем управления приводами роботов. The invention relates to robotics and can be used to create robot drive control systems.

Известно устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен к входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, выход к второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, квадратор, шестой сумматор и с второго по пятый блоки умножения, датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого их который соединен с выходом первого датчика положения, выход датчика массы подключен к второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функционального преобразователя, а их выходы соответственно к второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через квадратор с выходом второго датчика скорости, а выходом с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом датчика ускорения, а вторым входом с выходом шестого сумматора, третий выход которого подключен к выходу второго задатчика сигнала, выход второго сумматора соединен с третьим входом третьего сумматора (авт. св. N 1782721, Б.И. N 47, 1992 г.). A device for controlling a robot drive is known, comprising a series-connected first adder, a second adder, a first multiplication unit, a third adder, an amplifier and an engine connected to the first speed sensor directly and through a gearbox with a first position sensor, the output of which is connected to the first input of the first adder, connected by the second input to the input of the device, the relay element and the fourth adder connected in series, the second input of which is connected to the input of the relay element, the second input of W the next adder and the output of the first speed sensor, the output to the second input of the third adder, the first signal pickup and the fifth adder connected in series, as well as the second speed sensor, the mass sensor, the second signal pickup, the quadrator, the sixth adder and the second to fifth multiplication units, the sensor acceleration, as well as the first and second functional converters, the input of each of which is connected to the output of the first position sensor, the output of the mass sensor is connected to the second input of the first multiplication unit, the first input of the sixth sum pa and the second input of the fifth adder connected by the output to the first inputs of the second and third multiplication units, the second input of each of which is connected respectively to the output of the first and second functional converter, and their outputs, respectively, to the second input of the sixth adder and the first input of the fourth multiplication unit, connected the second input through a quadrator with the output of the second speed sensor, and the output with the third input of the fourth adder, the fourth input of which is connected to the output of the fifth multiplication unit, connected of the first input to an output of the acceleration sensor and the second input to an output of the sixth adder, a third output is connected to the output of second setpoint signal, the output of the second adder connected to a third input of the third adder (auth. St. N 1782721, B.I. N 47, 1992).

Недостатком данного устройства является то, что оно предназначено только для конкретного привода робота с другой кинематической схемой. Для приводов других степеней подвижности других роботов (с другой кинематикой) это устройство не будет обеспечивать требуемую точность и устойчивость работы. The disadvantage of this device is that it is intended only for a specific robot drive with a different kinematic scheme. For drives of other degrees of mobility of other robots (with different kinematics) this device will not provide the required accuracy and stability.

В рассматриваемом устройстве в отличие от прототипа присутствует еще одна поворотная степень подвижности относительно вертикальной оси. При повороте робота относительно этой оси возникают дополнительные возмущающие моментные воздействия на привод третьей степени подвижности. Поэтому устройство-прототип не может быть использовано для качественного управления роботом с поворотом относительно вертикальной оси ввиду неучета дополнительных моментных воздействий на данный привод. In this device, in contrast to the prototype, there is another rotary degree of mobility relative to the vertical axis. When the robot rotates about this axis, additional disturbing moment effects on the drive of the third degree of mobility arise. Therefore, the prototype device cannot be used for high-quality control of the robot with rotation relative to the vertical axis due to the neglect of additional torque effects on this drive.

В результате возникает задача построения такой самонастраивающейся коррекции, которая обеспечила бы высокую динамическую точность работы рассматриваемого привода робота с учетом указанных дополнительных моментных воздействий. As a result, the problem arises of constructing such a self-adjusting correction, which would ensure high dynamic accuracy of the robot drive under consideration, taking into account the indicated additional moment effects.

Технической задачей изобретения является устранение указанного выше недостатка, т.е. обеспечение высокой динамической точности привода робота с другой кинематической схемой построения исполнительного органа за счет обеспечения инвариантности к переменным параметрам нагрузки. An object of the invention is to eliminate the above drawback, i.e. ensuring high dynamic accuracy of the robot drive with a different kinematic scheme for constructing the executive body by ensuring invariance to variable load parameters.

Технический результат, который может быть получен при реализации заявляемого технического решения, выражается в формировании дополнительного сигнала управления, подаваемого на вход привода, который обеспечивает получение дополнительного моментного воздействия, компенсирующего вредное моментное воздействие со стороны первой поворотной степени подвижности робота (см. координату q₁) на качественные показатели работы рассматриваемого электропривода.The technical result that can be obtained by implementing the claimed technical solution is expressed in the formation of an additional control signal supplied to the input of the drive, which provides an additional momentary effect that compensates for the harmful momentary effect from the side of the first rotary degree of mobility of the robot (see coordinate q ₁ ) on quality indicators of the operation of the drive in question.

Блок-схема предлагаемого устройства для управления приводом робота представлена на фиг. 1. На фиг. 2 представлена кинематическая схема исполнительного органа робота, которая соответствует типовой схеме робота типа PUMA. A block diagram of a device for controlling a robot drive is shown in FIG. 1. In FIG. 2 is a kinematic diagram of a robot actuator, which corresponds to a typical robot circuit of the PUMA type.

Устройство для управления приводом робота содержит последовательно соединенные первый сумматор 1, второй сумматор 2, первый блок умножения 3, третий сумматор 4, первый усилитель 5 и двигатель 6, связанный с первым датчиком 7 скорости непосредственно и через редуктор 8 с датчиком 9 положения, выход которого подключен к первому входу первого сумматора 1, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент 10 и четвертый сумматор 11, второй вход которого подключен к входу релейного элемента 10, второму входу второго сумматора 2 и выходу первого датчика 7 скорости, выход к второму входу третьего сумматора 4, последовательно соединенные первый задатчик 12 сигнала и пятый сумматор 13, а также второй датчик 14 скорости, датчик 15 массы, второй задатчик 16 сигнала, первый квадратор 17, шестой сумматор 18 и с второго по пятый блоки умножения 19-22, датчик ускорения 23, а также первый 24 и второй 25 функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика 9 положения, выход датчика 15 массы подключен к второму входу первого блока 3 умножения, первому входу шестого сумматора 18 и второму входу пятого сумматора 13, соединенного выходом с первыми входами второго 19 и третьего 20 блоков умножения, второй вход каждого из которых подключен к выходу первого 24 и второго 25 функционального преобразователя, а их выходы к второму входу шестого сумматора 18 и первому входу четвертого блока умножения 21, соединенного вторым входом через первый квадратор 17 с выходом второго датчика 14 скорости, а выходом с третьим входом четвертого сумматора 11, четвертый вход которого подключен к выходу пятого блока 22 умножения, соединенного первым входом с выходом датчика 23 ускорения, а вторым входом с выходом шестого сумматора 18, третий вход которого подключен к выходу второго задатчика 16 сигнала, а выход второго сумматора 2 соединен с третьим входом третьего сумматора 4, последовательно соединенные второй датчик 26 положения, седьмой сумматор 27, второй вход которого подключен к выходу первого датчика 9 положения, третий функциональный преобразователь 28 и шестой блок 29 умножения, второй вход которого подключен к выходу пятого сумматора 13, а выход к пятому входу четвертого сумматора 11, последовательно соединенные второй усилитель 30, четвертый функциональный преобразователь 31, седьмой блок 32 умножения, восьмой сумматор 33 и восьмой блок 34 умножения, последовательно соединенные пятый функциональный преобразователь 35, девятый 36 и десятый 37 блоки умножения, последовательно соединенные третий датчик 38 скорости и второй квадратор 39, выход которого подключен к второму входу восьмого блока 34 умножения, выход которого соединен с шестым отрицательным входом четвертого сумматора 11, последовательно соединенные третий задатчик 40 постоянного сигнала и девятый сумматор 41, второй вход которого подключен к выходу второго задатчика 16 постоянного сигнала, его третий вход к выходу датчика 15 массы, а его выход к второму входу седьмого блока 32 умножения, причем второй вход десятого блока 37 умножения через шестой функциональный преобразователь 42 подключен к выходу седьмого сумматора 27 и входу второго усилителя 30, а его выход к второму положительному входу восьмого сумматора 33, второй вход девятого блока 36 умножения соединен с выходом пятого сумматора 13, а вход пятого функционального преобразователя 35 с выходом второго датчика положения 26, объект управления 43. The device for controlling the robot drive contains serially connected the first adder 1, the second adder 2, the first multiplication unit 3, the third adder 4, the first amplifier 5 and the motor 6, connected directly to the first speed sensor 7 and through the gearbox 8 with the position sensor 9, the output of which connected to the first input of the first adder 1, connected by the second input to the input of the device, a relay element 10 and a fourth adder 11 connected in series, the second input of which is connected to the input of the relay element 10, the second input at the second adder 2 and the output of the first speed sensor 7, an output to the second input of the third adder 4, the first signal generator 12 and the fifth adder 13, as well as the second speed sensor 14, the mass sensor 15, the second signal controller 16, the first quadrator 17, connected in series the sixth adder 18 and the second to fifth multiplication blocks 19-22, the acceleration sensor 23, as well as the first 24 and second 25 functional converters, the input of each of which is connected to the output of the first position sensor 9, the output of the mass sensor 15 is connected to the second input of the first block 3 mind The first input of the sixth adder 18 and the second input of the fifth adder 13, connected by the output to the first inputs of the second 19 and third 20 multiplication units, the second input of each of which is connected to the output of the first 24 and second 25 functional converters, and their outputs to the second input of the sixth the adder 18 and the first input of the fourth multiplication block 21, connected to the second input through the first quadrator 17 with the output of the second speed sensor 14, and the output with the third input of the fourth adder 11, the fourth input of which is connected to the output of the heel of the second multiplication unit 22, connected by the first input to the output of the acceleration sensor 23, and by the second input to the output of the sixth adder 18, the third input of which is connected to the output of the second signal setter 16, and the output of the second adder 2 is connected to the third input of the third adder 4, sequentially connected to the second the position sensor 26, the seventh adder 27, the second input of which is connected to the output of the first position sensor 9, the third functional converter 28 and the sixth multiplication unit 29, the second input of which is connected to the output of the fifth adder 13, and the output to the fifth input of the fourth adder 11, the second amplifier 30, the fourth functional converter 31, the seventh multiplication unit 32, the eighth adder 33 and the eighth multiplication unit 34, the fifth functional converter 35, the ninth 36 and the tenth 37 multiplication units connected in series connected in series a third speed sensor 38 and a second quadrator 39, the output of which is connected to the second input of the eighth block 34 of the multiplication, the output of which is connected to the sixth negative input of the fourth adder 11, therefore, the connected third constant signal master 40 and the ninth adder 41, the second input of which is connected to the output of the second constant signal master 16, its third input is the output of the mass sensor 15, and its output is the second input of the seventh multiplication unit 32, the second input of the tenth block 37 multiplication through the sixth functional Converter 42 is connected to the output of the seventh adder 27 and the input of the second amplifier 30, and its output to the second positive input of the eighth adder 33, the second input of the ninth block 36 multiplication is connected to Odom fifth adder 13 and the input of the fifth inverter 35 from the functional output of the second position sensor 26, the control object 43.

На рисунках приведены следующие обозначения:
α_вх сигнал желаемого положения;
q₁,q₂,q₃= α_н соответствующие обобщенные координаты исполнительного органа робота;

скорости изменения соответствующих обобщенных координат;
ε ошибка привода (величина рассогласования);
m₁, m₂, m₃, m_г соответственно массы первого, второго, третьего звеньев исполнительного органа и захваченного груза;
l $\genfrac{}{}{0ex}{}{\text{*}}{\text{2}}$ ,l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ расстояния от осей вращения соответствующих звеньев до их центров масс;
l₂, l₃ длины соответствующих звеньев;

скорость вращения ротора двигателя;
U^*, U соответственно усиливаемый сигнал и сигнал управления двигателем 6.The following symbols are shown in the figures:
α _in signal of the desired position;
q ₁ , q ₂ , q ₃ = α _n corresponding generalized coordinates of the executive body of the robot;

rate of change of the corresponding generalized coordinates;
ε drive error (mismatch value);
m ₁ , m ₂ , m ₃ , m _g, respectively, the mass of the first, second, third links of the executive body and the captured cargo;
l $\genfrac{}{}{0ex}{}{\text{*}}{\text{2}}$ , l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ the distance from the axis of rotation of the respective links to their centers of mass;
l ₂ , l _{3 the} length of the corresponding links;

rotor speed of the engine;
U ^* , U respectively, the amplified signal and the engine control signal 6.

Устройство работает следующим образом. Сигнал ошибки ε с сумматора 1 после коррекции в блоках 2, 3, 4, усиливаясь, поступает на электродвигатель 6, приводя его вал во вращательное движение с направлением и скоростью (ускорением), зависящими от величины поступающего сигнала U, моментов трения и внешнего моментного воздействия М_в. Электропривод при работе с различными грузами, а также за счет взаимовлияния степеней подвижности исполнительного органа обладает переменными моментными характеристиками, которые могут меняться в широких пределах. Это снижает качественные показатели электропривода и даже приводит к потере устойчивости его работы.The device operates as follows. The error signal ε from the adder 1 after correction in blocks 2, 3, 4, amplifying, enters the electric motor 6, bringing its shaft into rotational motion with direction and speed (acceleration), depending on the value of the incoming signal U, the friction moments and external torque M _century The electric drive when working with various loads, as well as due to the mutual influence of the degrees of mobility of the executive body, has variable torque characteristics that can vary widely. This reduces the quality indicators of the electric drive and even leads to a loss of stability of its operation.

Рассматриваемый привод управляет обобщенной координатой q₃. Конструкция робота (см. фиг. 2) является наиболее типовой для отечественных и зарубежных промышленных роботов. Эта конструкция позволяет осуществлять любое перемещение груза в трехмерном пространстве.The drive in question controls the generalized coordinate q ₃ . The design of the robot (see Fig. 2) is the most typical for domestic and foreign industrial robots. This design allows any movement of cargo in three-dimensional space.

Моментные характеристики привода, управляющего координатой q₃, существенно зависят от изменения координат

. В связи с этим для качественного управления координатой q₃ необходимо точно компенсировать отрицательное влияние изменения координат

, а также переменной массы груза m_г на динамические свойства рассматриваемого привода поворота (координата q₃).The moment characteristics of the drive controlling the coordinate q ₃ substantially depend on the change in coordinates

. In this regard, for quality control of the q ₃ coordinate, it is necessary to precisely compensate for the negative influence of the coordinate change

, as well as a variable mass of the cargo m _g on the dynamic properties of the rotation drive in question (coordinate q ₃ ).

Для определения моментных воздействий на рассматриваемый привод (обобщенных моментов неконсервативных сил) воспользуемся уравнением Лагранжа 2-го рода. Кинетическая энергия Т всех движущихся масс исполнительного органа (фиг. 2) представляется в виде

где I_Si, I_Ni (i 1, 3) соответственно моменты инерций относительно продольной и поперечной осей, проходящих через центр масс звена i.To determine the moment effects on the drive in question (generalized moments of non-conservative forces), we use the second-order Lagrange equation. The kinetic energy T of all moving masses of the executive body (Fig. 2) is presented in the form

where I _Si , I _Ni (i 1, 3), respectively, the moments of inertia relative to the longitudinal and transverse axes passing through the center of mass of the link i.

Потенциальная энергия робота имеет вид

где g ускорение свободного падения.The potential energy of the robot has the form

where g is the acceleration due to gravity.

Учитывая, что

На основе уравнения Лагранжа 2-го рода можно записать, что моментное воздействие на выходной вал привода, управляющего координатой q₃, при движении робота (фиг. 2) с грузом имеет вид

С учетом соотношения (1), а также уравнений электрической

и механической

цепей электродвигателя постоянного тока с постоянными магнитами или независимого возбуждения рассматриваемый привод, управляющий координатой q₃, можно описать следующим дифференциальным уравнением:

где R активное сопротивление якорной цепи двигателя;
I момент инерции якоря двигателя и вращающихся частей редуктора, приведенных к валу двигателя;
K_м коэффициент крутящего момента;
K_ω коэффициент противоЭДС;
K_в коэффициент вязкого трения;
i_р передаточное отношение редуктора;
М_стр момент сухого трения;
К_у коэффициент усиления усилителя 5;
i ток якоря;

ускорение вращения вала двигателя третьей степени подвижности.Given that

Based on the Lagrange equation of the second kind, it can be written that the momentary action on the output shaft of the drive controlling the coordinate q ₃ when the robot moves (Fig. 2) with a load has the form

Given the relation (1), as well as the equations of electric

and mechanical

chains of a DC motor with permanent magnets or independent excitation, the drive in question, which controls the coordinate q ₃ , can be described by the following differential equation:

where R is the active resistance of the engine armature circuit;
I moment of inertia of the motor armature and rotating parts of the gearbox, brought to the motor shaft;
K _m torque coefficient;
K _ω counter-emf coefficient;
K _{in the} coefficient of viscous friction;
i _p gear ratio;
M _{p the} moment of dry friction;
To _y the gain of the amplifier 5;
i armature current;

acceleration of rotation of the motor shaft of the third degree of mobility.

Из (2) видно, что параметры этого уравнения, а следовательно, и параметры привода, управляющего координатой q₃, являются существенно переменными, зависящими от величины

. В результате в процессе работы привода меняются (притом существенно) его динамические свойства. В результате для реализации поставленной выше задачи необходимо сформировать такое корректирующее устройство, которое застабилизировало бы параметры привода таким образом, чтобы он описывался дифференциальным уравнением с постоянными желаемыми параметрами.From (2) it can be seen that the parameters of this equation, and therefore the parameters of the drive controlling the coordinate q ₃ , are essentially variable, depending on the value

. As a result, during the operation of the drive, its dynamic properties change (and substantially). As a result, for the implementation of the above task, it is necessary to form such a corrective device that would stabilize the drive parameters so that it is described by a differential equation with constant desired parameters.

Полагается, что первый положительный вход сумматора 2 (со стороны сумматора 1) единичный, а его второй отрицательный вход имеет коэффициент усиления K_ω/K_у. Первый, третий, четвертый положительные входы сумматора 11 (соответственно со стороны релейного элемента 10, блока 21 умножения и блока 22 умножения) единичные, второй его положительный вход (со стороны датчика 7 скорости) имеет коэффициент усиления (K_мK_ω/R+K_в), его пятый положительный вход (со стороны блока 29 умножения) коэффициент усиления g/l₂, а шестой отрицательный коэффициент усиления 1/2. Причем выходной сигнал релейного элемента 10 с нулевой нейтральной точкой имеет вид

где IМ_тI величина момента сухого трения при движении.It is believed that the first positive input of adder 2 (from the adder 1 side) is single, and its second negative input has a gain K _ω / K _у . The first, third, fourth positive inputs of the adder 11 (respectively from the side of the relay element 10, the multiplying unit 21 and the multiplying unit 22) are single, its second positive input (from the side of the speed sensor 7) has a gain (K _m K _ω / R + K _c ), its fifth positive input (from the side of the multiplication unit 29) is the gain g / l ₂ , and the sixth negative gain is 1/2. Moreover, the output signal of the relay element 10 with a zero neutral point has the form

where IM _t I is the moment of dry friction during movement.

Первый положительный вход сумматора 4 (со стороны блока 3 умножения) имеет коэффициент усиления l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ /(I_нi $\genfrac{}{}{0ex}{}{\text{2}}{\text{p}}$ ), второй положительный (со стороны сумматора 11) коэффициент усиления R/(K_м K_у), а третий положительный (со стороны сумматора 2) коэффициент усиления [I+(I_N3+m₃l $\genfrac{}{}{0ex}{}{\text{*2}}{\text{3}}$ )/i $\genfrac{}{}{0ex}{}{\text{2}}{\text{p}}$ ]J_н, где I_н - номинальное (желаемое) значение приведенного момента инерции, обеспечивающее рассматриваемому приводу робота заданные динамические свойства и показатели качества.The first positive input of the adder 4 (from the side of the block 3 multiplication) has a gain l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ / (I _n i $\genfrac{}{}{0ex}{}{\text{2}}{\text{p}}$ ), the second positive (from the adder 11) gain R / (K _m K _y ), and the third positive (from the adder 2) gain [I + (I _N3 + m ₃ l $\genfrac{}{}{0ex}{}{\text{* 2}}{\text{3}}$ ) / i $\genfrac{}{}{0ex}{}{\text{2}}{\text{p}}$ ] J _n , where I _n is the nominal (desired) value of the reduced moment of inertia, which provides the given robot drive with the specified dynamic properties and quality indicators.

Второй положительный вход сумматора 13 (со стороны датчика 15) имеет коэффициент усиления l₂l₃/i_р, а его первый положительный вход (со стороны задатчика 12) единичный коэффициент усиления. Сигнал с выхода задатчика 12 сигнала равен m₃l₂l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ /i_p, а с выхода задатчика 16 сигнала - (I_N3+m₃l $\genfrac{}{}{0ex}{}{\text{*2}}{\text{3}}$ )/i_p/ . Второй (со стороны блока 19 умножения) и третий (со стороны задатчика 16 сигнала) положительные входы сумматора 18 имеют единичные коэффициенты усиления, а первый положительный вход (со стороны датчика 15) коэффициент усиления l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ /i_p.The second positive input of the adder 13 (from the sensor 15) has a gain l ₂ l ₃ / i _p , and its first positive input (from the setpoint 12) has a unity gain. The signal from the output of the setter 12 of the signal is m ₃ l ₂ l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ / i _p , and from the output of the signal setter 16 - (I _N3 + m ₃ l $\genfrac{}{}{0ex}{}{\text{* 2}}{\text{3}}$ ) / i _{p /} . The second (from the side of the multiplication block 19) and the third (from the side of the signal setter 16) the positive inputs of the adder 18 have unit gains, and the first positive input (from the side of the sensor 15) gain l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ / i _p .

Таким образом, на выходе сумматора 13 формируется сигнал l₂(m₃l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ +m_гl₃)/i_p. Поскольку функциональный преобразователь 24 формирует сигнал cos q₃, то на выходе блока 19 умножения появляется сигнал l₂(m₃l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ +m_гl₃)cos(q₃)/i_p, а на выходе сумматора 18 сигнал
[I_N3+m₃l $\genfrac{}{}{0ex}{}{\text{*2}}{\text{3}}$ +m_гl $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ +l₂(m₃l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ +m_гl₃)cosq₃]/i_p.
Датчик 23 ускорения измеряет ускорение вращения второй степени подвижности робота (координату

), поэтому на выходе блока 22 умножения формируется сигнал

Датчик 14 скорости измеряет скорость вращения во второй степени подвижности (координату

), а функциональный преобразователь 25 формирует сигнал sin q₃. Поэтому на выходе блока 20 умножения появляется сигнал l₂(m₃l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ +m_гl₃)sinq₃/i_p, а на выходе блока 21 умножения сигнал

.Thus, at the output of the adder 13, a signal l ₂ (m ₃ l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ + m _g l ₃ ) / i _p . Since the functional Converter 24 generates a signal cos q ₃ , then at the output of the multiplication unit 19, a signal l ₂ (m ₃ l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ + m _g l ₃ ) cos (q ₃ ) / i _p , and at the output of the adder 18 a signal
[I _N3 + m ₃ l $\genfrac{}{}{0ex}{}{\text{* 2}}{\text{3}}$ + m _g l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ + l ₂ (m ₃ l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ + m _g l ₃ ) cosq ₃ ] / i _p .
The acceleration sensor 23 measures the acceleration of rotation of the second degree of robot mobility (coordinate

), therefore, at the output of the multiplication block 22, a signal is generated

The speed sensor 14 measures the speed of rotation in the second degree of mobility (coordinate

), and the functional converter 25 generates a signal sin q ₃ . Therefore, at the output of the multiplication unit 20, a signal l ₂ (m ₃ l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ + m _g l ₃ ) sinq ₃ / i _p , and at the output of block 21 of the multiplication signal

.

Датчик 26 положения измеряет угол поворота во второй степени подвижности (координату q₂), третий функциональный преобразователь 28 формирует сигнал sin (q₂+q₃). В результате на выходе блока 29 умножения формируется сигнал
l₂(m₃l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ +m_гl₃)sin(q₂+q₃)/i_p.
Третий датчик 38 скорости измеряет скорость вращения первой степени подвижности робота (координата

). С выхода третьего задатчика 40 постоянного сигнала на первый положительный единичный вход девятого сумматора 41 поступает сигнал (-I_S3/i_р). Второй (со стороны задатчика 16) положительный вход этого сумматора единичный, а третий положительный (со стороны датчика 15 массы) имеет коэффициент усиления l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ /i_p. В результате на выходе девятого сумматора 41 формируется сигнал
(I_N3-I_S3+m₃l $\genfrac{}{}{0ex}{}{\text{*2}}{\text{3}}$ +m_гl $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ )/i_p.
Второй усилитель 30 имеет коэффициент усиления, равный 2. Четвертый функциональный преобразователь 31 реализует функцию sin. В результате на выходе седьмого блока 32 умножения формируется сигнал
(I_N3-I_S3+m₃l $\genfrac{}{}{0ex}{}{\text{*2}}{\text{3}}$ +m_гl $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ )sin2(q₂+q₃)/i_p.
Пятый функциональный преобразователь 35 реализует функцию sin, а шестой функциональный преобразователь 42 функцию cos. В результате на выходе десятого блока умножения 37 формируется сигнал
l₂(m₃l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ +m_гl₃)sin(q₂)cos(q₂+q₃)/i_p,
а на выходе восьмого сумматора 33, первый положительный вход которого (со стороны блока 32 умножения) единичный, а второй положительный имеет коэффициент усиления 2, формируется сигнал

С учетом отмеченных выше коэффициентов усиления соответствующих входов сумматора 11 на его выходе формируется сигнал

На выходе сумматора 2 формируется сигнал

, а на выходе блока 3 умножения сигнал

.The position sensor 26 measures the angle of rotation in the second degree of mobility (coordinate q ₂ ), the third functional converter 28 generates a signal sin (q ₂ + q ₃ ). As a result, a signal is generated at the output of the multiplication unit 29
l ₂ (m ₃ l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ + m _g l ₃ ) sin (q ₂ + q ₃ ) / i _p .
The third speed sensor 38 measures the speed of rotation of the first degree of robot mobility (coordinate

) From the output of the third constant adjuster 40, a signal (-I _S3 / i _p ) is supplied to the first positive single input of the ninth adder 41. The second (from the setpoint 16) positive input of this adder is single, and the third positive (from the side of the mass sensor 15) has a gain l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ / i _p . As a result, a signal is generated at the output of the ninth adder 41
(I _N3 -I _S3 + m ₃ l $\genfrac{}{}{0ex}{}{\text{* 2}}{\text{3}}$ + m _g l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ ) / i _p .
The second amplifier 30 has a gain of 2. The fourth functional converter 31 implements the sin function. As a result, a signal is generated at the output of the seventh multiplication block 32
(I _N3 -I _S3 + m ₃ l $\genfrac{}{}{0ex}{}{\text{* 2}}{\text{3}}$ + m _g l $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ ) sin2 (q ₂ + q ₃ ) / i _p .
The fifth functional converter 35 implements the sin function, and the sixth functional converter 42 the cos function. As a result, a signal is generated at the output of the tenth multiplication block 37
l ₂ (m ₃ l $\genfrac{}{}{0ex}{}{\text{*}}{\text{3}}$ + m _g l ₃ ) sin (q ₂ ) cos (q ₂ + q ₃ ) / i _p ,
and at the output of the eighth adder 33, the first positive input of which (from the side of the multiplication unit 32) is single, and the second positive has a gain of 2, a signal is generated

Taking into account the gains noted above for the corresponding inputs of the adder 11, a signal is generated at its output

At the output of adder 2, a signal is generated

, and at the output of block 3 of the multiplication signal

.

Таким образом, с учетом указанных выше коэффициентов усиления соответствующих входов сумматора 4 на его выходе окончательно будет сформирован сигнал вида

Несложно показать, что поскольку

при движении привода достаточно точно соответствует М_стр, то, подставив полученное значение U^* (3) в соотношение (2), получим уравнение

которое имеет постоянные желаемые параметры. То есть сам привод, управляющий координатой q₃, будет обладать постоянными желаемыми динамическими свойствами и качественными показателями.Thus, taking into account the above gain factors of the corresponding inputs of the adder 4, a signal of the form

It is easy to show that since

when the drive moves quite accurately corresponds to M _p , then, substituting the obtained value U ^* (3) in relation (2), we obtain the equation

which has constant desired parameters. That is, the drive itself, controlling the coordinate q ₃ , will have constant desired dynamic properties and quality indicators.

Claims (1)

Устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, первый усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом со входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен ко входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, выход ко второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, первый квадратор, шестой сумматор, второй, третий, четвертый и пятый блоки умножения, датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен ко второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функционального преобразователя, а их выходы - соответственно ко второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через первый квадратор с выходом второго датчика скорости, а выходом с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом датчика ускорения, а вторым входом с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход второго сумматора соединен с третьим входом третьего сумматора, отличающееся тем, что в него дополнительно введены последовательно соединенные второй датчик положения, седьмой сумматор, второй вход которого подключен к выходу первого датчика положения, третий функциональный преобразователь и шестой блок умножения, второй вход которого подключен к выходу пятого сумматора, а выход к пятому входу четвертого сумматора, последовательно соединенные второй усилитель, четвертый функциональный преобразователь, седьмой блок умножения, восьмой сумматор и восьмой блок умножения, последовательно соединенные пятый функциональный преобразователь, девятый и десятый блоки умножения, последовательно соединенные третий датчик скорости и второй квадратор, выход которого подключен ко второму входу восьмого блока умножения, выход которого соединен с шестым отрицательным входом четвертого сумматора, последовательно соединенные третий задатчик постоянного сигнала и девятый сумматор, второй вход которого подключен к выходу второго задатчика постоянного сигнала, его третий вход к выходу датчика массы, а его выход ко второму входу седьмого блока умножения, причем второй вход десятого блока умножения через шестой функциональный преобразователь подключен к выходу седьмого сумматора и входу второго усилителя, а его выход ко второму положительному входу восьмого сумматора, второй вход девятого блока умножения соединен с выходом пятого сумматора, а вход пятого функционального преобразователя с выходом второго датчика положения. A device for controlling a robot drive, comprising a first adder, a second adder, a first multiplication unit, a third adder, a first amplifier and an engine connected to the first speed sensor directly and through a gearbox with a first position sensor, the output of which is connected to the first input of the first adder, connected by a second input to the input of the device, a relay element and a fourth adder connected in series, the second input of which is connected to the input of the relay element, the second input of W the next adder and the output of the first speed sensor, the output to the second input of the third adder, the first signal pickup and the fifth adder connected in series, as well as the second speed sensor, mass sensor, the second signal pickup, the first quad, sixth adder, the second, third, fourth and fifth multiplication units, acceleration sensor, as well as the first and second functional converters, the input of each of which is connected to the output of the first position sensor, the output of the mass sensor is connected to the second input of the first multiplication unit, the first at the entrance of the sixth adder and the second input of the fifth adder, connected by the output to the first inputs of the second and third multiplication units, the second input of each of which is connected respectively to the output of the first and second functional converter, and their outputs, respectively, to the second input of the sixth adder and the first input of the fourth a multiplication unit connected by a second input through the first quadrator to the output of the second speed sensor, and an output with a third input of the fourth adder, the fourth input of which is connected to the output the multiplication unit connected by the first input to the output of the acceleration sensor, and the second input with the output of the sixth adder, the third input of which is connected to the output of the second signal generator, and the output of the second adder is connected to the third input of the third adder, characterized in that it is additionally introduced in series connected by a second position sensor, a seventh adder, the second input of which is connected to the output of the first position sensor, a third functional converter and a sixth multiplication unit, the second input of which is sub the output of the fifth adder, and the output to the fifth input of the fourth adder, the second amplifier, the fourth functional converter, the seventh multiplication unit, the eighth adder and the eighth multiplication unit, the fifth functional converter, the ninth and tenth multiplication units connected in series to the third sensor speed and a second quadrator, the output of which is connected to the second input of the eighth block of multiplication, the output of which is connected to the sixth negative input of the fourth an adder connected in series to the third constant signal master and the ninth adder, the second input of which is connected to the output of the second constant signal master, its third input to the output of the mass sensor, and its output to the second input of the seventh multiplication unit, the second input of the tenth multiplication unit through the sixth functional the converter is connected to the output of the seventh adder and the input of the second amplifier, and its output to the second positive input of the eighth adder, the second input of the ninth multiplication unit is connected to the output the house of the fifth adder, and the input of the fifth functional converter with the output of the second position sensor.

Images