RU2348509C1

RU2348509C1 - Robot electrical drive

Info

Publication number: RU2348509C1
Application number: RU2007119863/02A
Authority: RU
Inventors: Владимир Федорович Филаретов (RU); Владимир Федорович Филаретов
Original assignee: Институт автоматики и процессов управления ДВО РАН
Priority date: 2007-05-28
Filing date: 2007-05-28
Publication date: 2009-03-10
Also published as: RU2007119863A

Abstract

FIELD: mechanics; electrical engineering.

SUBSTANCE: design of the robot electrical drive envisages serial connection of the first summator, the first multiplier unit, the second summator, the amplifier and the first speed sensor connected to the electrical motor. The electric motor is connected to the drive gear and the first position sensor via the reduction gear box. The electric drive is additionally equipped with the following components, serially connected: the fourth cosine functional generator and the ninth multiplier unit whose second input and output are connected to the seventh summator output and the sixth summator input accordingly.

EFFECT: complete insusceptibility of the robot electric drive dynamic performances to continuously and quickly varying instantaneous load characteristics.

2 dwg

Description

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

Известен самонастраивающийся электропривод робота, содержащий последовательно соединенные первый сумматор, первый блок умножения, второй сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через репродуктор - с шестерней, приводящей в движение рейку, закрепленную неподвижно на горизонтальном звене робота, и движок первого датчика положения, установленного на вертикальном звене и измеряющего положение характерной точки горизонтального звена относительно вертикального, последовательно соединенные релейный блок и третий сумматор, второй вход которого подключен к выходу первого датчика скорости, входу релейного блока и второму входу первого сумматора, последовательно соединенные первый задатчик сигнала, четвертый сумматор, пятый сумматор, к входу которого подключен второй задатчик сигнала, второй блок умножения, шестой сумматор и третий блок умножения, а также датчик массы, причем второй выход датчика положения соединен с первым входом седьмого сумматора, подключенного вторым входом к входу электропривода, а выходом - к первому входу первого сумматора, выход третьего сумматора соединен с вторым входом второго сумматора, последовательно соединенные второй датчик скорости и квадратор, выход которого соединен с вторым входом третьего блока умножения, выходом подключенного к третьему отрицательному входу третьего сумматора, выход датчика массы соединен с вторыми входами первого и второго блоков умножения, выход датчика положения соединен с вторым входом четвертого сумматора, выход которого подключен к второму входу шестого сумматора, а выход первого сумматора соединен с третьим входом второго сумматора (см. патент РФ №2037173, БИ №16, 1995 г.).Known self-adjusting electric drive of the robot, containing a series-connected first adder, a first multiplication unit, a second adder, an amplifier and an engine connected to the first speed sensor directly and through a reproducer, with a gear driving a rail fixed motionlessly on the horizontal link of the robot, and the engine of the first a position sensor mounted on a vertical link and measuring the position of a characteristic point of the horizontal link relative to the vertical, connected in series the relay unit and the third adder, the second input of which is connected to the output of the first speed sensor, the input of the relay unit and the second input of the first adder, the first signal master, the fourth adder, the fifth adder connected to the input of the second signal master, the second multiplication unit, the sixth an adder and a third multiplication unit, as well as a mass sensor, the second output of the position sensor connected to the first input of the seventh adder connected by the second input to the input of the drive, and the output to the first input ode of the first adder, the output of the third adder is connected to the second input of the second adder, the second speed sensor and a quadrator are connected in series, the output of which is connected to the second input of the third multiplication unit, the output connected to the third negative input of the third adder, the output of the mass sensor is connected to the second inputs of the first and of the second multiplication blocks, the output of the position sensor is connected to the second input of the fourth adder, the output of which is connected to the second input of the sixth adder, and the output of the first adder with union of the third input of the second adder (cm. RF patent No. 2037173, BI No. 16, 1995).

Недостатком данного устройства является отсутствие инвариантности динамических свойств рассматриваемого электропривода к непрерывным и быстрым изменениям его моментных нагрузочных характеристик, поскольку здесь рассматривается другая степень подвижности робота с другой кинематической схемой.The disadvantage of this device is the lack of invariance of the dynamic properties of the drive under consideration to continuous and rapid changes in its moment load characteristics, since here we consider a different degree of mobility of the robot with a different kinematic scheme.

Наиболее близким по технической сущности к предлагаемому техническому решению является устройство для управления приводом робота, содержащее первый сумматор, последовательно соединенные первый блок умножения и второй сумматор, последовательно соединенные первый усилитель, электродвигатель, связанный с первым датчиком скорости непосредственно и через редуктор - с первым датчиком положения, выход которого подключен к первому входу первого сумматора, второй вход которого образует вход устройства, последовательно соединенные третий сумматор, первый квадратор, второй блок умножения, второй вход которого соединен с выходом датчика массы захваченного груза, четвертый сумматор, второй и третий входы которого подключены соответственно к выходу первого задатчика постоянного сигнала и второго квадратора, последовательно соединенные второй задатчик постоянного сигнала, пятый сумматор, третий блок умножения, шестой сумматор и четвертый блок умножения, второй вход которого подключен к выходу второго датчика скорости, последовательно соединенные второй датчик положения и седьмой сумматор, последовательно соединенные третий датчик положения, первый функциональный преобразователь и третий квадратор, последовательно соединенные второй функциональный преобразователь, вход которого подключен к выходу третьего датчика положения и второму входу седьмого сумматора, и четвертый квадратор, последовательно соединенные третий функциональный преобразователь и пятый квадратор, последовательно соединенные четвертый функциональный преобразователь и шестой квадратор, причем выходы третьего, четвертого, пятого и шестого квадраторов подключены соответственно к четвертому, пятому, шестому и седьмому входам четвертого сумматора, выходы третьего и четвертого функциональных преобразователей соединены с выходом седьмого сумматора, выходы второго и четвертого функциональных преобразователей подключены соответственно к первым и вторым входам третьего и восьмого сумматоров, а выход последнего подключен к входу второго квадратора, последовательно соединенные третий задатчик постоянного сигнала, девятый сумматор, второй вход которого соединен с выходом датчика массы и вторым входом пятого сумматора, пятый блок умножения, десятый сумматор, второй вход которого подключен к выходу третьего блока умножения, шестой блок умножения, второй вход которого соединен с выходом третьего датчика скорости, и одиннадцатый сумматор, второй вход которого подключен к выходу четвертого блока умножения, последовательно соединенные второй усилитель, вход которого соединен с выходом седьмого сумматора, двенадцатый сумматор, второй вход которого подключен к выходу второго датчика положения, пятый функциональный преобразователь и седьмой блок умножения, выход которого подключен к третьему входу десятого сумматора, последовательно соединенные четвертый задатчик постоянного сигнала, тринадцатый сумматор, второй вход которого соединен с выходом датчика массы, восьмой блок умножения, второй вход которого подключен к выходу второго функционального преобразователя, и девятый блок умножения, второй вход которого соединен с выходом третьего функционального преобразователя, а выход - со вторым входом шестого сумматора, последовательно соединенные третий усилитель, вход которого подключен к выходу третьего датчика положения, и шестой функциональный преобразователь, выход которого соединен со вторым входом пятого блока умножения, причем второй вход третьего блока умножения через седьмой функциональный преобразователь подключен к выходу второго усилителя, а выход тринадцатого сумматора - к второму входу седьмого блока умножения, а также десятый и одиннадцатый блоки умножения, выходы которых подключены соответственно к второму и третьему входам второго сумматора, выход релейного элемента подключен к четвертому входу второго сумматора, а вход - к входу первого датчика скорости, первым входам десятого и одиннадцатого блоков умножения и пятому входу второго сумматора, причем второй вход десятого блока умножения подключен к выходу одиннадцатого сумматора, первый вход первого блока умножения соединен с выходом первого сумматора, а его второй вход - с вторым входом одиннадцатого блока умножения и выходом четвертого сумматора, при этом выход второго сумматора подключен к выходу первого усилителя (см. патент РФ №2063866, БИ №20, 1996 г.).The closest in technical essence to the proposed technical solution is a device for controlling a robot drive, comprising a first adder, a first multiplication unit and a second adder connected in series to a first amplifier, an electric motor connected directly to the first speed sensor and through the gearbox with the first position sensor the output of which is connected to the first input of the first adder, the second input of which forms the input of the device, connected in series with the third a matrator, a first quadrator, a second multiplication unit, the second input of which is connected to the output of the mass sensor of the captured cargo, the fourth adder, the second and third inputs of which are connected respectively to the output of the first constant signal generator and the second quadrator, serially connected to the second constant signal generator, fifth adder, a third multiplication unit, a sixth adder and a fourth multiplication unit, the second input of which is connected to the output of the second speed sensor, the second position sensor connected in series with the seventh adder, the third position sensor connected in series, the first functional converter and the third quadrator, the second functional converter connected in series, the input of which is connected to the output of the third position sensor and the second input of the seventh adder, and the fourth quadrator connected in series with the third functional converter and fifth quadrator, in series connected fourth functional converter and sixth quadrator, and the outputs of the third, fourth, fifth and the sixth quadrators are connected respectively to the fourth, fifth, sixth and seventh inputs of the fourth adder, the outputs of the third and fourth functional converters are connected to the output of the seventh adder, the outputs of the second and fourth functional converters are connected respectively to the first and second inputs of the third and eighth adders, and the output of the last is connected to the input of the second quadrator, the third constant signal generator, the ninth adder, the second input of which is connected to the output, are connected in series the mass sensor and the second input of the fifth adder, the fifth multiplication unit, the tenth adder, the second input of which is connected to the output of the third multiplication unit, the sixth multiplication unit, the second input of which is connected to the output of the third speed sensor, and the eleventh adder, the second input of which is connected to the output the fourth multiplication unit, connected in series to the second amplifier, the input of which is connected to the output of the seventh adder, the twelfth adder, the second input of which is connected to the output of the second position sensor, the fifth function an ion converter and a seventh multiplication unit, the output of which is connected to the third input of the tenth adder, a fourth constant signal generator connected in series, a thirteenth adder, the second input of which is connected to the output of the mass sensor, an eighth multiplication unit, the second input of which is connected to the output of the second functional converter, and the ninth multiplication unit, the second input of which is connected to the output of the third functional converter, and the output to the second input of the sixth adder, sequentially the third amplifier, the input of which is connected to the output of the third position sensor, and the sixth functional converter, the output of which is connected to the second input of the fifth multiplication unit, the second input of the third multiplication unit through the seventh functional converter connected to the output of the second amplifier, and the output of the thirteenth adder to the second input of the seventh multiplication block, as well as the tenth and eleventh multiplication blocks, the outputs of which are connected respectively to the second and third inputs of the second adder, output p the relay element is connected to the fourth input of the second adder, and the input to the input of the first speed sensor, the first inputs of the tenth and eleventh multiplication units and the fifth input of the second adder, the second input of the tenth multiplication unit connected to the output of the eleventh adder, the first input of the first multiplication unit is connected to the output of the first adder, and its second input with the second input of the eleventh multiplication unit and the output of the fourth adder, while the output of the second adder is connected to the output of the first amplifier (see RF patent No. 2063866, BI No. 20, 1996).

Недостатком данного устройства также является то, что оно не обеспечивает полной инвариантности динамических свойств рассматриваемого электропривода к непрерывным и быстрым изменениям его моментных нагрузочных характеристик, поскольку оно также предназначено для другой степени подвижности робота с другой кинематической схемой, имеющей только три степени подвижности. В результате это устройство не будет обеспечивать требуемую динамическую точность работы для рассматриваемого привода нового робота.The disadvantage of this device is also that it does not provide complete invariance of the dynamic properties of the drive in question to continuous and rapid changes in its momentary load characteristics, since it is also intended for a different degree of mobility of the robot with another kinematic scheme having only three degrees of mobility. As a result, this device will not provide the required dynamic accuracy for the drive of the new robot under consideration.

Задачей, на решение которой направлено заявляемое техническое решение, является обеспечение полной инвариантности динамических свойств рассматриваемого электропривода к изменениям его динамических моментных нагрузочных характеристик при движении рассматриваемого манипулятора робота по всем четырем степеням подвижности и, тем самым, повышение динамической точности его управления.The task to which the claimed technical solution is directed is to ensure the complete invariance of the dynamic properties of the drive in question to changes in its dynamic moment load characteristics when the robot arm in question moves along all four degrees of mobility and, thereby, increase the dynamic accuracy of its control.

Технический результат, который может быть получен при реализации заявляемого технического решения, выражается в формировании нового сигнала управления, подаваемого на вход привода, который обеспечивает получение нового моментного воздействия, компенсирующего вредное моментное воздействие на качественные показатели работы рассматриваемого привода.The technical result that can be obtained by implementing the proposed technical solution is expressed in the formation of a new control signal supplied to the input of the drive, which provides a new moment effect, compensating for the harmful moment effect on the quality performance of the drive in question.

Поставленная задача решается тем, что в электропривод робота, содержащий последовательно соединенные первый сумматор, первый блок умножения, второй сумматор, усилитель, электродвигатель, связанный с первым датчиком скорости непосредственно и через редуктор - с шестерней и первым датчиком положения, выход которого подключен к первому входу первого сумматора, второй вход которого соединен с входом устройства, последовательно соединенные первый задатчик сигнала, третий сумматор и второй блок умножения, второй вход которого подключен к выходу первого датчика скорости, второму входу второго сумматора и через релейный элемент - к третьему входу второго сумматора, а выход - к четвертому входу второго сумматора, последовательно соединенные второй датчик положения, четвертый сумматор, второй вход которого подключен к выходу третьего датчика положения, и первый синусный функциональный преобразователь, последовательно соединенные датчик массы груза, пятый сумматор, второй вход которого подключен к выходу второго задатчика сигнала, третий блок умножения, второй вход которого через второй косинусный функциональный преобразователь соединен с выходом четвертого сумматора, шестой сумматор и четвертый блок умножения, последовательно соединенные третий задатчик сигнала, седьмой сумматор, второй вход которого соединен с выходом датчика массы груза и вторым входом третьего сумматора, пятый блок умножения, второй вход которого через третий синусный функциональный преобразователь подключен к выходу третьего датчика положения, восьмой сумматор и шестой блок умножения, последовательно соединенные второй датчик скорости, седьмой блок умножения и девятый сумматор, последовательно соединенные третий датчик скорости и восьмой блок умножения, причем второй вход первого блока умножения подключен к выходу третьего сумматора, дополнительно введены последовательно соединенные четвертый косинусный функциональный преобразователь, вход которого подключен к выходу третьего датчика положения, и девятый блок умножения, второй вход которого подключен к выходу седьмого сумматора, а выход - ко второму входу шестого сумматора, последовательно соединенные четвертый датчик положения, пятый синусный функциональный преобразователь, десятый блок умножения и одиннадцатый блок умножения, второй вход которого подключен к выходу четвертого датчика скорости, а выход - ко второму входу седьмого блока умножения, последовательно соединенные шестой косинусный функциональный преобразователь, вход которого подключен к выходу четвертого датчика положения, двенадцатый блок умножения и тринадцатый блок умножения, второй вход которого подключен к выходу первого датчика ускорения, а выход - ко второму входу девятого сумматора, выход которого подключен к пятому входу второго сумматора, последовательно соединенные первый квадратор, десятый сумматор, второй вход которого через восьмой блок умножения подключен к выходу второго датчика скорости и входу первого квадратора, и четырнадцатый блок умножения, выход которого подключен к третьему входу девятого сумматора, четвертый вход которого через последовательно соединенные пятнадцатый и шестнадцатый блоки умножения соединен с выходом третьего датчика скорости и входом второго квадратора, выход которого через последовательно соединенные одиннадцатый сумматор и семнадцатый блок умножения подключен к пятому входу девятого сумматора, последовательно соединенные восемнадцатый блок умножения, первый вход которого подключен к выходу шестого сумматора и девятнадцатый блок умножения, второй вход которого подключен к выходу второго датчика ускорения, а выход - к шестому входу девятого сумматора, последовательно соединенные третий датчик ускорения и двадцатый блок умножения, второй вход которого подключен к выходу шестого блока умножения, а выход - к седьмому входу девятого сумматора, первый вход двадцать первого блока умножения подключен к выходу восьмого сумматора, его второй вход - ко второму входу восемнадцатого блока умножения и выходу пятого синусного функционального преобразователя, а выход - ко второму входу шестнадцатого блока умножения, первый вход двадцать второго блока умножения соединен с выходом шестого косинусного функционального преобразователя и вторыми входами четвертого и шестого блоков умножения, его второй вход - с выходом третьего блока умножения, а выход - со вторым входом четырнадцатого блока умножения, первый вход двадцать третьего блока умножения соединен с первым входом третьего блока умножения, его второй вход - с выходом первого синусного функционального преобразователя, а выход - со вторыми входами восьмого сумматора, десятого и двенадцатого блоков умножения, причем второй вход одиннадцатого сумматора через третий квадратор подключен к выходу четвертого датчика скорости и второму входу четырнадцатого блока умножения, а второй вход шестнадцатого блока умножения - к выходу четвертого блока умножения.The problem is solved in that in the electric drive of the robot, containing the first adder in series, the first multiplication unit, the second adder, an amplifier, an electric motor connected to the first speed sensor directly and through the gearbox with a gear and the first position sensor, the output of which is connected to the first input the first adder, the second input of which is connected to the input of the device, the first signal generator, the third adder and the second multiplication unit, the second input of which is connected to the first speed sensor, the second input of the second adder and through the relay element to the third input of the second adder, and the output to the fourth input of the second adder, the second position sensor connected in series, the fourth adder, the second input of which is connected to the output of the third position sensor, and the first sine functional converter, series-connected load mass sensor, fifth adder, the second input of which is connected to the output of the second signal generator, the third multiplication unit, the second input of which is A second cosine functional converter is connected to the output of the fourth adder, the sixth adder and the fourth multiplication unit, connected in series with the third signal adjuster, the seventh adder, the second input of which is connected to the output of the load mass sensor and the second input of the third adder, the fifth multiplication unit, the second input of which the third sine functional converter is connected to the output of the third position sensor, the eighth adder and the sixth multiplication unit, connected in series to the second sensor growth, the seventh multiplication unit and the ninth adder, the third speed sensor and the eighth multiplication unit connected in series, the second input of the first multiplication unit being connected to the output of the third adder, the fourth cosine functional converter, the input of which is connected to the output of the third position sensor, is additionally introduced the ninth multiplication unit, the second input of which is connected to the output of the seventh adder, and the output to the second input of the sixth adder, in series connected the fourth position sensor, the fifth sine functional converter, the tenth multiplication unit and the eleventh multiplication unit, the second input of which is connected to the output of the fourth speed sensor, and the output to the second input of the seventh multiplication unit, connected in series with the sixth cosine functional converter, the input of which is connected to the output of the fourth position sensor, the twelfth multiplication unit and the thirteenth multiplication unit, the second input of which is connected to the output of the first acceleration sensor, and the output to the second input the ninth adder, the output of which is connected to the fifth input of the second adder, the first quadrator connected in series, the tenth adder, the second input of which through the eighth multiplication unit is connected to the output of the second speed sensor and the input of the first quadrator, and the fourteenth multiplication unit, the output of which is connected to the third input of the ninth an adder, the fourth input of which through series-connected fifteenth and sixteenth multiplication units is connected to the output of the third speed sensor and the input of the second quadrator, the output where through the eleventh adder and the seventeenth multiplication unit connected in series to the fifth input of the ninth adder, the eighteenth multiplier unit connected in series, the first input of which is connected to the output of the sixth adder and the nineteenth multiplication unit, the second input of which is connected to the output of the second acceleration sensor, and the output to the sixth input of the ninth adder, the third acceleration sensor and the twentieth multiplication block are connected in series, the second input of which is connected to the output of the sixth block knives, and the output is to the seventh input of the ninth adder, the first input of the twenty-first multiplication unit is connected to the output of the eighth adder, its second input to the second input of the eighteenth multiplication unit and the output of the fifth sine functional converter, and the output to the second input of the sixteenth multiplication unit, the first input of the twenty-second multiplication block is connected to the output of the sixth cosine functional converter and the second inputs of the fourth and sixth multiplication blocks, its second input is to the output of the third block The output is with the second input of the fourteenth multiplication block, the first input of the twenty-third multiplication block is connected to the first input of the third multiplication block, its second input is with the output of the first sine functional converter, and the output is with the second inputs of the eighth adder, tenth and twelfth blocks multiplication, and the second input of the eleventh adder through the third quadrator is connected to the output of the fourth speed sensor and the second input of the fourteenth multiplication unit, and the second input of the sixteenth multiplication unit is the output of the fourth multiplier.

Сопоставительный анализ заявляемого решения с его аналогами и прототипом свидетельствует о его соответствии критерию "новизна".A comparative analysis of the proposed solution with its 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 to increase the dynamic accuracy of control of the robot drive in question under conditions of rapid changes in its parameters, due to the effects of mutual influence between its degrees of mobility in the presence of cargo and the moment of dry friction.

Блок-схема предлагаемого электропривода робота представлена на фиг.1. На фиг.2 представлена кинематическая схема робота.A block diagram of the proposed robot electric drive is presented in figure 1. Figure 2 presents the kinematic diagram of the robot.

Электропривод робота содержит последовательно соединенные первый сумматор 1, первый блок 2 умножения, второй сумматор 3, усилитель 4, электродвигатель 5, связанный с первым датчиком 6 скорости непосредственно и через редуктор 7 - с шестерней 8 и первым датчиком 9 положения, выход которого подключен к первому входу первого сумматора 1, второй вход которого соединен с входом устройства, последовательно соединенные первый задатчик 10 сигнала, третий сумматор 11 и второй блок 12 умножения, второй вход которого подключен к выходу первого датчика 6 скорости, второму входу второго сумматора 3 и через релейный элемент 13 - к третьему входу второго сумматора 3, а выход - к четвертому входу второго сумматора 3, последовательно соединенные второй датчик 14 положения, четвертый сумматор 15, второй вход которого подключен к выходу третьего датчика 16 положения, и первый синусный функциональный преобразователь 17, последовательно соединенные датчик 18 массы груза, пятый сумматор 19, второй вход которого подключен к выходу второго задатчика 20 сигнала, третий блок 21 умножения, второй вход которого через второй косинусный функциональный преобразователь 22 соединен с выходом четвертого сумматора 15, шестой сумматор 23 и четвертый блок 24 умножения, последовательно соединенные третий задатчик 25 сигнала, седьмой сумматор 26, второй вход которого соединен с выходом датчика массы 18 груза и вторым входом третьего сумматора 11, пятый блок 27 умножения, второй вход которого через третий синусный функциональный преобразователь 28 подключен к выходу третьего датчика 16 положения, восьмой сумматор 29 и шестой блок 30 умножения, последовательно соединенные второй датчик 31 скорости, седьмой блок 32 умножения и девятый сумматор 33, последовательно соединенные третий датчик 34 скорости и восьмой блок 35 умножения, причем второй вход первого блока 2 умножения подключен к выходу третьего сумматора 11, последовательно соединенные четвертый косинусный функциональный преобразователь 36, вход которого подключен к выходу третьего датчика положения 16, и девятый блок 37 умножения, второй вход которого подключен к выходу седьмого сумматора 26, а выход - ко второму входу шестого сумматора 23, последовательно соединенные четвертый датчик 38 положения, пятый синусный функциональный преобразователь 39, десятый блок 40 умножения и одиннадцатый блок 41 умножения, второй вход которого подключен к выходу четвертого датчика 42 скорости, а выход - ко второму входу седьмого блока 32 умножения, последовательно соединенные шестой косинусный функциональный преобразователь 43, вход которого подключен к выходу четвертого датчика 38 положения, двенадцатый блок 44 умножения и тринадцатый блок 45 умножения, второй вход которого подключен к выходу первого датчика 46 ускорения, а выход - ко второму входу девятого сумматора 33, выход которого подключен к пятому входу второго сумматора 3, последовательно соединенные первый квадратор 47, десятый сумматор 48, второй вход которого через восьмой блок 35 умножения подключен к выходу второго датчика 31 скорости и входу первого квадратора 47, и четырнадцатый блок 49 умножения, выход которого подключен к третьему входу девятого сумматора 33, четвертый вход которого через последовательно соединенные пятнадцатый 50 и шестнадцатый 51 блоки умножения соединен с выходом третьего датчика 34 скорости и входом второго квадратора 52, выход которого через последовательно соединенные одиннадцатый сумматор 53 и семнадцатый блок 54 умножения подключен к пятому входу девятого сумматора 33, последовательно соединенные восемнадцатый блок 55 умножения, первый вход которого подключен к выходу шестого сумматора 23 и девятнадцатый блок 56 умножения, второй вход которого подключен к выходу второго датчика 57 ускорения, а выход - к шестому входу девятого сумматора 33, последовательно соединенные третий датчик 58 ускорения и двадцатый блок 59 умножения, второй вход которого подключен к выходу шестого блока 30 умножения, а выход - к седьмому входу девятого сумматора 33, первый вход двадцать первого блока 60 умножения подключен к выходу восьмого сумматора 29, его второй вход - ко второму входу восемнадцатого блока 55 умножения и выходу пятого синусного функционального преобразователя 39, а выход - ко второму входу шестнадцатого блока 51 умножения, первый вход двадцать второго блока 61 умножения соединен с выходом шестого косинусного функционального преобразователя 43 и вторыми входами четвертого 24 и шестого 30 блоков умножения, его второй вход - с выходом третьего блока 21 умножения, а выход - со вторым входом четырнадцатого блока 49 умножения, первый вход двадцать третьего блока 62 умножения соединен с первым входом третьего блока 21 умножения, его второй вход - с выходом первого синусного функционального преобразователя 17, а выход - со вторыми входами восьмого сумматора 29, десятого 40 и двенадцатого 44 блоков умножения, причем второй вход одиннадцатого сумматора 53 через третий квадратор 63 подключен к выходу четвертого датчика 42 скорости и второму входу пятнадцатого блока 50 умножения, а второй вход семнадцатого блока 54 умножения - к выходу четвертого блока 24 умножения.The electric drive of the robot contains a series-connected first adder 1, a first multiplication unit 2, a second adder 3, an amplifier 4, an electric motor 5 connected to the first speed sensor 6 directly and through a reducer 7 with a gear 8 and a first position sensor 9, the output of which is connected to the first the input of the first adder 1, the second input of which is connected to the device input, the first signal adjuster 10, the third adder 11 and the second multiplier 12, the second input of which is connected to the output of the first speed sensor 6, are connected in series springs, the second input of the second adder 3 and through the relay element 13 to the third input of the second adder 3, and the output to the fourth input of the second adder 3, serially connected to the second position sensor 14, the fourth adder 15, the second input of which is connected to the output of the third sensor 16 position, and the first sine functional converter 17, serially connected to the load mass sensor 18, the fifth adder 19, the second input of which is connected to the output of the second signal setter 20, the third multiplication unit 21, the second input of which is via W a swarm cosine functional converter 22 is connected to the output of the fourth adder 15, the sixth adder 23 and the fourth multiplication unit 24, connected in series with the third signal adjuster 25, the seventh adder 26, the second input of which is connected to the output of the load mass sensor 18 and the second input of the third adder 11, the fifth a multiplication unit 27, the second input of which through the third sine functional converter 28 is connected to the output of the third position sensor 16, the eighth adder 29 and the sixth multiplication unit 30, connected in series to a speed sensor 31, a seventh multiplication unit 32 and a ninth adder 33 connected in series to a third speed sensor 34 and an eighth multiplication unit 35, the second input of a first multiplying unit 2 being connected to an output of a third adder 11 connected in series to a fourth cosine function converter 36, the input of which connected to the output of the third position sensor 16, and the ninth multiplication unit 37, the second input of which is connected to the output of the seventh adder 26, and the output to the second input of the sixth adder 23, connected in series the fourth position sensor 38, the fifth sine function converter 39, the tenth multiplication unit 40 and the eleventh multiplication unit 41, the second input of which is connected to the output of the fourth speed sensor 42, and the output - to the second input of the seventh multiplication unit 32, connected in series with the sixth cosine functional converter 43, the input of which is connected to the output of the fourth position sensor 38, the twelfth multiplication unit 44 and the thirteenth multiplication unit 45, the second input of which is connected to the output of the first acceleration sensor 46 and the output goes to the second input of the ninth adder 33, the output of which is connected to the fifth input of the second adder 3, the first quadrator 47 is connected in series, the tenth adder 48, the second input of which is connected through the eighth multiplier 35 to the output of the second speed sensor 31 and the input of the first quad 47, and the fourteenth multiplication unit 49, the output of which is connected to the third input of the ninth adder 33, the fourth input of which is connected in series through the fifteenth 50 and sixteenth 51 multiplication units to the output of the third sensor as the speed 34 and the input of the second quadrator 52, the output of which through the eleventh adder 53 and the seventeenth multiplier 54 are connected in series to the fifth input of the ninth adder 33, the eighteenth multiplier unit 55 is connected in series, the first input of which is connected to the output of the sixth adder 23 and the nineteenth block 56 multiplication, the second input of which is connected to the output of the second acceleration sensor 57, and the output to the sixth input of the ninth adder 33, connected in series to the third acceleration sensor 58 and the twentieth block 59 mind a knife, the second input of which is connected to the output of the sixth multiplication unit 30, and the output to the seventh input of the ninth adder 33, the first input of the twenty-first multiplication unit 60 is connected to the output of the eighth adder 29, its second input to the second input of the eighteenth multiplication unit 55 and the output the fifth sine functional converter 39, and the output to the second input of the sixteenth multiplication block 51, the first input of the twenty-second multiplication block 61 is connected to the output of the sixth cosine functional converter 43 and the second inputs of four grated 24 and sixth 30 multiplication blocks, its second input is the output of the third multiplication block 21, and the output is the second input of the fourteenth multiplication block 49, the first input of the twenty-third multiplication block 62 is connected to the first input of the third multiplication block 21, its second input is with the output of the first sine functional converter 17, and the output with the second inputs of the eighth adder 29, tenth 40 and twelfth 44 multiplication blocks, and the second input of the eleventh adder 53 through the third quadrator 63 is connected to the output of the fourth sensor 42 speed growth and the second input of the fifteenth multiplication block 50, and the second input of the seventeenth multiplication block 54 to the output of the fourth multiplication block 24.

На указанных чертежах введены следующие обозначения: α_ВХ - сигнал с выхода программного устройства; ε - сигнал ошибки; U*, U - усиливаемый сигнал и сигнал управления двигателем соответственно; q₁, q₂, q₃, q₄ - обобщенные координаты четырех степеней подвижности; m₁, m₂, m₃, m_Г - массы соответствующих звеньев робота и груза; l₂, l₃ - длины соответствующих звеньев;

,

- расстояния от осей вращения соответствующих звеньев до их центров масс;

,

- скорости изменения соответствующих обобщенных координат;

- скорость вращения ротора двигателя четвертой степени подвижности;

,

- ускорения изменения соответствующих обобщенных координат;

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

; J_ni - моменты инерции соответствующих звеньев робота относительно поперечных осей, проходящих через их центры масс

.The following notation is introduced in the indicated drawings: α _ВХ - signal from the output of the software device; ε is the error signal; U *, U - amplified signal and engine control signal, respectively; q ₁ , q ₂ , q ₃ , q ₄ - generalized coordinates of four degrees of mobility; m ₁ , m ₂ , m ₃ , m _G - masses of the corresponding links of the robot and cargo; l ₂ , l ₃ are the lengths of the corresponding links;

,

- the distance from the axis of rotation of the respective links to their centers of mass;

,

- the rate of change of the corresponding generalized coordinates;

- the speed of rotation of the rotor of the engine of the fourth degree of mobility;

,

- accelerate changes in the corresponding generalized coordinates;

- acceleration of rotation of the rotor of the engine of the fourth degree of mobility; J _si - moments of inertia of the corresponding links of the robot relative to their longitudinal axes

; J _ni - moments of inertia of the corresponding links of the robot relative to the transverse axes passing through their centers of mass

.

Устройство работает следующим образом. На вход подается управляющее воздействие α_ВХ, обеспечивающее требуемый закон управления приводом. На выходе сумматора 1 вырабатывается сигнал ошибки ε, который после коррекции в элементах 2 и 3, усиливаясь, поступает на электродвигатель 5 с редуктором 7, приводя его вал во вращательное движение с направлением и скоростью (ускорением), зависящим от величины поступающего сигнала U и внешнего моментного воздействия на привод.The device operates as follows. At the input, a control action α _VX is applied, which provides the required drive control law. At the output of adder 1, an error signal ε is generated, which, after correction in elements 2 and 3, amplifies, enters the electric motor 5 with gear 7, bringing its shaft into rotational motion with direction and speed (acceleration), depending on the value of the incoming signal U and external moment impact on the drive.

Рассматриваемый привод управляет обобщенной координатой q₄. Данная степень подвижности позволяет осуществить горизонтальное прямолинейное перемещение робота, схема которого представлена на фиг.2.The drive in question controls the generalized coordinate q ₄ . This degree of mobility allows for horizontal rectilinear movement of the robot, a diagram of which is presented in figure 2.

Робот в горизонтальной плоскости перемещается с помощью передачи шестерня-рейка (координата q₄). Причем рейка установлена на основании, по которому перемещается робот, а шестерня 8 - на выходном валу редуктора 7 и имеет радиус r.The robot moves in the horizontal plane by means of a gear-rack transmission (coordinate q ₄ ). Moreover, the rail is installed on the basis on which the robot moves, and the gear 8 on the output shaft of the gearbox 7 and has a radius r.

Датчики 38, 16 и 14 установлены соответственно в первой, второй и третьей степенях подвижности робота (см. фиг.2) и измеряют соответственно обобщенные координаты q₁, q₂, и q₃. Сумматор 15 имеет положительные входы с единичными коэффициентами усиления, поэтому на его выходе формируется сигнал q₂+q₃. На выходе задатчика 20 формируется сигнал

, а на выходе датчика 18 - m_Г. Первый (со стороны задатчика 20) положительный вход сумматора 19 имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления, равный 1. В результате на выходе сумматора 19 формируется сигнал

. На выходе задатчика 25 сигнала формируется сигнал

. Первый (со стороны задатчика 25) и второй положительные входы сумматора 26 соответственно имеют единичный коэффициент усиления и коэффициент усиления, равный l₂. В результате на выходе сумматора 26 формируется сигнал

. Таким образом, на выходе блоков 21, 62, 37 и 27 умножения соответственно формируются сигналы Acos(q₂+q₃), Asin(q₂+q₃), Bcosq₂ и Bsinq₂.Sensors 38, 16 and 14 are installed respectively in the first, second and third degrees of mobility of the robot (see figure 2) and measure, respectively, the generalized coordinates q ₁ , q ₂ , and q ₃ . The adder 15 has positive inputs with unity gain, therefore, a q ₂ + q ₃ signal is generated at its output. At the output of the setter 20, a signal is generated

, and at the output of the sensor 18 - m _G. The first (from the master 20) positive input of the adder 19 has a unity gain, and its second positive input has a gain of 1. As a result, a signal is generated at the output of the adder 19

. At the output of the signal setter 25, a signal is generated

. The first (from the setpoint 25) and the second positive inputs of the adder 26 respectively have a unity gain and a gain equal to l ₂ . As a result, a signal is generated at the output of the adder 26

. Thus, at the output of multiplication blocks 21, 62, 37, and 27, signals Acos (q ₂ + q ₃ ), Asin (q ₂ + q ₃ ), Bcosq _2, and Bsinq _{2 are} formed respectively.

Положительные входы сумматоров 23 и 29 имеют единичные коэффициенты усиления. Поэтому на выходах блоков 24, 55, 30 и 60 умножения соответственно формируются сигналы:The positive inputs of the adders 23 and 29 have unity gain. Therefore, the outputs of the blocks 24, 55, 30 and 60 of the multiplication, respectively, the signals are generated:

b=cosq₁(Acos(q₂+q₃)+Bcosq₂), a=sinq₁(Acos(q₂+q₃)+Bcosq₂),b = cosq ₁ (Acos (q ₂ + q ₃ ) + Bcosq ₂ ), a = sinq ₁ (Acos (q ₂ + q ₃ ) + Bcosq ₂ ),

d=cosq₁(Asin(q₂+q₃)+Bsinq₂) и с=sinq₁(Asin(q₂+q₃)+Bsinq₂).d = cosq ₁ (Asin (q ₂ + q ₃ ) + Bsinq ₂ ) and c = sinq ₁ (Asin (q ₂ + q ₃ ) + Bsinq ₂ ).

На выходах блоков 61, 44 и 40 соответственно формируются сигналы: k=Acosq₁cos(q₂+q₃), m=Acosq₁sin(q₂+q₃) и f=Asinq₁sin(q₂+q₃).At the outputs of blocks 61, 44 and 40, respectively, signals are generated: k = Acosq ₁ cos (q ₂ + q ₃ ), m = Acosq ₁ sin (q ₂ + q ₃ ) and f = Asinq ₁ sin (q ₂ + q ₃ ) .

Датчики 42, 34 и 31 скорости устанавливаются соответственно в первой, второй и третьей степенях подвижности робота (см. фиг.2.) и измеряют соответственно

,

, и

. Положительные входы сумматора 53 имеют единичные коэффициенты усиления. Поэтому на выходе блока 54 умножения формируется сигнал

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

и

соответственно. В результате на выходах блоков 51 и 32 умножения соответственно формируются сигналы

и

Sensors 42, 34 and 31 of the speed are installed respectively in the first, second and third degrees of mobility of the robot (see figure 2.) and measure, respectively

,

, and

. The positive inputs of the adder 53 have unity gain. Therefore, a signal is generated at the output of the multiplication block 54

, and at the outputs of blocks 50 and 41 of the multiplication - signals

and

respectively. As a result, signals are generated at the outputs of the multiplication blocks 51 and 32, respectively.

and

На выходе блока 35 умножения формируется сигнал

. Поэтому, поскольку первый (со стороны блока 35) положительный вход сумматора 48 имеет коэффициент усиления, равный 2, а его второй положительный вход - единичный коэффициент усиления, то на выходе блока 49 умножения формируется сигнал

.A signal is generated at the output of the multiplication block 35

. Therefore, since the first (from the side of block 35) positive input of the adder 48 has a gain of 2, and its second positive input has a unity gain, a signal is generated at the output of the multiplication unit 49

.

На выходах датчиков 57, 58 и 46 ускорения, установленных соответственно в первой, второй и третьей степенях подвижности робота соответственно, формируются сигналы

,

, а на выходах блоков 56, 59 и 45 умножения - сигналы

,

, соответственно.At the outputs of the acceleration sensors 57, 58 and 46, respectively installed in the first, second and third degrees of mobility of the robot, signals are generated

,

, and at the outputs of blocks 56, 59 and 45 of the multiplication - signals

,

, respectively.

Второй (со стороны блока 45), третий (со стороны блока 49), пятый (со стороны блока 54), шестой (со стороны блока 56) и седьмой (со стороны блока 59) отрицательные входы сумматора 68 имеют единичные коэффициенты усиления, а первый (со стороны блока 32) и четвертый (со стороны блока 51) положительные входы имеют коэффициенты усиления, равные 2. В результате на выходе сумматора 33 формируется сигналThe second (from the side of block 45), the third (from the side of block 49), the fifth (from the side of block 54), the sixth (from the side of block 56) and the seventh (from the side of block 59) negative inputs of the adder 68 have unity gain, and the first (from the side of block 32) and the fourth (from the side of block 51) the positive inputs have gains equal to 2. As a result, a signal is generated at the output of the adder 33

.

На выходе задатчика 10 формируется сигнал

, где J - момент инерции ротора электродвигателя и вращающихся частей редуктора (приведены к валу двигателя), i_Р - передаточное отношение редуктора. Первый (со стороны задатчика 10) и второй положительные входы сумматора 11 имеют соответственно единичный коэффициент усиления и коэффициент усиления, равный r². В результате на выходе этого сумматора формируется сигнал

.At the output of the setter 10, a signal is generated

where J is the moment of inertia of the rotor of the electric motor and the rotating parts of the gearbox (given to the motor shaft), i _P is the gear ratio of the gearbox. The first (from the setpoint 10) and the second positive inputs of the adder 11 have respectively a unity gain and a gain equal to r ² . As a result, a signal is generated at the output of this adder

.

На первый положительный вход сумматора 3 (со стороны блока 2) с коэффициентом усиления

поступает сигнал

(где J_H - суммарный номинальный момент инерции), на его второй положительный вход с коэффициентом усиления К_ω/К_у+К_ВR/(К_уК_м) поступает сигнал

, на третий положительный вход (со стороны релейного элемента 13) с коэффициентом усиления R/К_мК_у - сигнал:At the first positive input of the adder 3 (from the side of unit 2) with a gain

signal is coming

(where J _H is the total nominal moment of inertia), a signal is received at its second positive input with a gain of K _ω / K _y + K _V R / (K _y K _m )

, to the third positive input (from the side of the relay element 13) with a gain of R / K _m K _y - signal:

,

на четвертый отрицательный вход (со стороны блока 12) с коэффициентом усиления

- сигнал

, на пятый положительный вход (со стороны сумматора 33) с коэффициентом усиления

- сигнал С, где R - активное сопротивление якорной обмотки электродвигателя, K_B - коэффициент вязкого трения, К_у - коэффициент усиления усилителя 4, К_м - коэффициент крутящего момента, К_ω - коэффициент противоЭДС, М_т=const>0 - величина момента сухого трения при движении электродвигателя.to the fourth negative input (from the side of block 12) with a gain

- signal

, to the fifth positive input (from the adder 33) with a gain

- signal C, where R is the active resistance of the armature winding of the electric motor, K _B is the viscous friction coefficient, K _y is the gain of the amplifier 4, K _m is the torque coefficient, K _ω is the counter-emf coefficient, M _t = const> 0 is the moment dry friction when moving an electric motor.

В результате на выходе сумматора 3 формируется сигналAs a result, at the output of the adder 3, a signal is generated

Кинетическая энергия всех движущихся масс робота имеет вид:The kinetic energy of all moving masses of the robot has the form:

а его потенциальная энергия имеет вид:and its potential energy has the form:

где g - ускорение свободного падения. Учитывая, чтоwhere g is the acceleration of gravity. Given that

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

,

,

,

из уравнения Лагранжа 2 рода можно получить силу F, действующую на линейную горизонтальную степень подвижности робота в процессе его движения:from the Lagrange equation of the 2nd kind, one can obtain the force F acting on the linear horizontal degree of mobility of the robot during its movement:

,

которая создает на выходном валу редуктора 7 момент, равныйwhich creates on the output shaft of the gearbox 7 a moment equal to

С учетом соотношения (2), а также уравнения механическойTaking into account relation (2), as well as the mechanical equation

и электрической

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

and electrical

chains of DC motors with permanent magnets or independent excitation, it is easy to show that the drive in question, which controls the coordinate q _{4 of the} robot, can be described by the differential equation:

Сформированный сигнал U*(1), как несложно убедиться, обеспечивает превращение уравнения (3) с существенно переменными параметрами в уравнение

с постоянными желаемыми параметрами, обеспечивающими электроприводу, управляющему координатой q₄, заданные динамические свойства и качественные показатели работы за счет выбора желаемых постоянных значений J_н и К_у.The generated signal U * (1), as is easy to verify, provides the transformation of equation (3) with substantially variable parameters into the equation

with constant desired parameters, providing the electric drive that controls the coordinate q ₄ , the specified dynamic properties and quality performance due to the choice of the desired constant values of J _n and K _y .

Таким образом, благодаря введению дополнительных элементов и новых связей удалось обеспечить полную инвариантность рассматриваемого электропривода робота ко всем силовым воздействиям. Это позволяет получить стабильно высокую точность управления в любых режимах его работы.Thus, thanks to the introduction of additional elements and new connections, it was possible to ensure the complete invariance of the considered electric drive of the robot to all power influences. This allows you to get a consistently high accuracy control in any modes of its operation.

Claims

Электропривод робота, содержащий последовательно соединенные первый сумматор, первый блок умножения, второй сумматор, усилитель и первый датчик скорости, связанный с электродвигателем, который через редуктор связан с шестерней и первым датчиком положения, выход которого подключен к первому входу первого сумматора, второй вход которого соединен с входом устройства, последовательно соединенные первый задатчик сигнала, третий сумматор и второй блок умножения, второй вход которого подключен к выходу первого датчика скорости, второму входу второго сумматора и через релейный элемент - к третьему входу второго сумматора, а выход - к четвертому входу второго сумматора, последовательно соединенные второй датчик положения, четвертый сумматор, второй вход которого подключен к выходу третьего датчика положения и первый синусный функциональный преобразователь, последовательно соединенные датчик массы груза, пятый сумматор, второй вход которого подключен к выходу второго задатчика сигнала, третий блок умножения, второй вход которого через второй косинусный функциональный преобразователь соединен с выходом четвертого сумматора, шестой сумматор и четвертый блок умножения, последовательно соединенные третий задатчик сигнала, седьмой сумматор, второй вход которого соединен с выходом датчика массы груза и вторым входом третьего сумматора, пятый блок умножения, второй вход которого через третий синусный функциональный преобразователь подключен к выходу третьего датчика положения, восьмой сумматор и шестой блок умножения, последовательно соединенные второй датчик скорости, седьмой блок умножения и девятый сумматор, последовательно соединенные третий датчик скорости и восьмой блок умножения, причем второй вход первого блока умножения подключен к выходу третьего сумматора, отличающийся тем, что в него дополнительно введены последовательно соединенные четвертый косинусный функциональный преобразователь, вход которого подключен к выходу третьего датчика положения и девятый блок умножения, второй вход которого подключен к выходу седьмого сумматора, а выход - ко второму входу шестого сумматора, последовательно соединенные четвертый датчик положения, пятый синусный функциональный преобразователь, десятый блок умножения и одиннадцатый блок умножения, второй вход которого подключен к выходу четвертого датчика скорости, а выход - ко второму входу седьмого блока умножения, последовательно соединенные шестой косинусный функциональный преобразователь, вход которого подключен к выходу четвертого датчика положения, двенадцатый блок умножения и тринадцатый блок умножения, второй вход которого подключен к выходу первого датчика ускорения, а выход - ко второму входу девятого сумматора, выход которого подключен к пятому входу второго сумматора, последовательно соединенные первый квадратор, десятый сумматор, второй вход которого через восьмой блок умножения подключен к выходу второго датчика скорости и входу первого квадратора, и четырнадцатый блок умножения, выход которого подключен к третьему входу девятого сумматора, четвертый вход которого через последовательно соединенные пятнадцатый и шестнадцатый блоки умножения соединен с выходом третьего датчика скорости и входом второго квадратора, выход которого через последовательно соединенные одиннадцатый сумматор и семнадцатый блок умножения подключен к пятому входу девятого сумматора, последовательно соединенные восемнадцатый блок умножения, первый вход которого подключен к выходу шестого сумматора и девятнадцатый блок умножения, второй вход которого подключен к выходу второго датчика ускорения, а выход - к шестому входу девятого сумматора, последовательно соединенные третий датчик ускорения и двадцатый блок умножения, второй вход которого подключен к выходу шестого блока умножения, а выход - к седьмому входу девятого сумматора, первый вход двадцать первого блока умножения подключен к выходу восьмого сумматора, его второй вход - ко второму входу восемнадцатого блока умножения и выходу пятого синусного функционального преобразователя, а выход - ко второму входу шестнадцатого блока умножения, первый вход двадцать второго блока умножения соединен с выходом шестого косинусного функционального преобразователя и вторыми входами четвертого и шестого блоков умножения, его второй вход - с выходом третьего блока умножения, а выход - со вторым входом четырнадцатого блока умножения, первый вход двадцать третьего блока умножения соединен с первым входом третьего блока умножения, его второй вход - с выходом первого синусного функционального преобразователя, а выход - со вторыми входами восьмого сумматора, десятого и двенадцатого блоков умножения, причем второй вход одиннадцатого сумматора через третий квадратор подключен к выходу четвертого датчика скорости и второму входу пятнадцатого блока умножения, а второй вход семнадцатого блока умножения - к выходу четвертого блока умножения. An electric drive of the robot, comprising a first adder, a first multiplication unit, a second adder, an amplifier and a first speed sensor connected in series with an electric motor, which is connected through a gearbox to a gear and a first position sensor, the output of which is connected to the first input of the first adder, the second input of which is connected with the input of the device, the first signal setter, the third adder and the second multiplication unit are connected in series, the second input of which is connected to the output of the first speed sensor, the second input at the second adder and through the relay element to the third input of the second adder, and the output to the fourth input of the second adder, the second position sensor connected in series, the fourth adder, the second input of which is connected to the output of the third position sensor and the first sine functional converter, connected in series to the sensor cargo mass, the fifth adder, the second input of which is connected to the output of the second signal generator, the third multiplication unit, the second input of which is through the second cosine functional pre the developer is connected to the output of the fourth adder, the sixth adder and the fourth multiplication unit, sequentially connected to the third signal master, the seventh adder, the second input of which is connected to the output of the load mass sensor and the second input of the third adder, the fifth multiplication unit, the second input of which is through the third sine functional converter connected to the output of the third position sensor, the eighth adder and the sixth multiplication unit, the second speed sensor, the seventh multiplication unit and the ninth sum an ator, a third speed sensor and an eighth multiplication unit connected in series, the second input of the first multiplying unit being connected to the output of the third adder, characterized in that a fourth cosine functional converter is additionally introduced into it, the input of which is connected to the output of the third position sensor and the ninth block multiplication, the second input of which is connected to the output of the seventh adder, and the output to the second input of the sixth adder, the fourth floor sensor connected in series the fifth sine functional converter, the tenth multiplication unit and the eleventh multiplication unit, the second input of which is connected to the output of the fourth speed sensor, and the output to the second input of the seventh multiplication unit, connected in series with the sixth cosine functional converter, the input of which is connected to the output of the fourth position sensor , the twelfth multiplication block and the thirteenth multiplication block, the second input of which is connected to the output of the first acceleration sensor, and the output to the second input of the ninth sum a, the output of which is connected to the fifth input of the second adder, the first quadrator is connected in series, the tenth adder, the second input of which through the eighth multiplication unit is connected to the output of the second speed sensor and the input of the first quadrator, and the fourteenth multiplication unit, the output of which is connected to the third input of the ninth adder the fourth input of which, through the fifteenth and sixteenth multiplication units connected in series, is connected to the output of the third speed sensor and the input of the second quadrator, the output of which is through the eleventh adder and the seventeenth multiplication unit connected in series are connected to the fifth input of the ninth adder, the eighteenth multiplier unit is connected in series, the first input of which is connected to the output of the sixth adder and the nineteenth multiplication unit, the second input of which is connected to the output of the second acceleration sensor, and the output to the sixth input the ninth adder, connected in series to the third acceleration sensor and the twentieth multiplication unit, the second input of which is connected to the output of the sixth multiplication unit, and the output - to the seventh input of the ninth adder, the first input of the twenty-first multiplication block is connected to the output of the eighth adder, its second input to the second input of the eighteenth multiplication block and the output of the fifth sine functional converter, and the output to the second input of the sixteenth multiplication block, the first input of the twenty-second the multiplication block is connected to the output of the sixth cosine functional converter and the second inputs of the fourth and sixth multiplication blocks, its second input is the output of the third multiplication block, and the output is with the second input of the fourteenth multiplication block, the first input of the twenty-third multiplication block is connected to the first input of the third multiplication block, its second input is with the output of the first sine functional converter, and the output is with the second inputs of the eighth adder, tenth and twelfth multiplication blocks, the second input through the third quadrator, the eleventh adder is connected to the output of the fourth speed sensor and the second input of the fifteenth multiplication block, and the second input of the seventeenth multiplication block to the fourth output multiplication block.