RU2325268C1 - Control of robotic machine drive - Google Patents

Control of robotic machine drive Download PDF

Info

Publication number
RU2325268C1
RU2325268C1 RU2006129957/02A RU2006129957A RU2325268C1 RU 2325268 C1 RU2325268 C1 RU 2325268C1 RU 2006129957/02 A RU2006129957/02 A RU 2006129957/02A RU 2006129957 A RU2006129957 A RU 2006129957A RU 2325268 C1 RU2325268 C1 RU 2325268C1
Authority
RU
Russia
Prior art keywords
input
output
adder
multiplication unit
multiplication
Prior art date
Application number
RU2006129957/02A
Other languages
Russian (ru)
Other versions
RU2006129957A (en
Inventor
Владимир Федорович Филаретов (RU)
Владимир Федорович Филаретов
Original Assignee
Институт автоматики и процессов управления ДВО РАН
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Институт автоматики и процессов управления ДВО РАН filed Critical Институт автоматики и процессов управления ДВО РАН
Priority to RU2006129957/02A priority Critical patent/RU2325268C1/en
Publication of RU2006129957A publication Critical patent/RU2006129957A/en
Application granted granted Critical
Publication of RU2325268C1 publication Critical patent/RU2325268C1/en

Links

Images

Abstract

FIELD: robotic technology.
SUBSTANCE: idea of invention is installation of additional fourth acceleration sensor, third differentiator, multiplier units and summation units.
EFFECT: improvement of drive control in all modes of operation.
2 cl

Description

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

Известно устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен к входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, выход - к второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, квадратор, шестой сумматор и с второго по пятый блоки умножения, первый датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен к второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функционального преобразователя, а их выходы соответственно - ко второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через квадратор с выходом второго датчика скорости, а выходом - с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом датчика ускорения, а вторым входом - с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход сумматора соединен с третьим входом третьего сумматора, второй датчик положения, седьмой сумматор, второй вход которого подключен к выходу первого датчика положения, третий функциональный преобразователь, шестой блок умножения, второй вход которого подключен к выходу пятого сумматора, и седьмой блок умножения, второй вход которого подключен к выходу второго датчика ускорения, а его выход - к пятому входу четвертого сумматора (см. патент РФ №2193480, БИ №33, 2002 г.).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 is to the second input of the third adder, the first signal pickup and the fifth adder are 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 first 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 input the 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 multiplication unit, connected by a second input through a quadrator to the output of the second speed sensor, and the output to the third input of the fourth adder, the fourth input of which is connected to the output of the fifth multiplication block connected by the first input to the output of the acceleration sensor, and the second input to 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 adder is connected to the third input of the third adder, the second position sensor, the seventh adder, the second input of which is connected to the output of the first position sensor, the third functional converter, the sixth multiplication unit, the second input of which is connected to the output of the fifth adder, and the seventh multiplication unit, the second input of which is connected to the output of the second date acceleration sensor, and its output goes to the fifth input of the fourth adder (see RF patent No. 2193480, BI No. 33, 2002).

Недостатком данного устройства является то, что в нем не учитывается электрическая постоянная времени электродвигателя L. Это во многих случаях может привести к потере устойчивости и снижению динамической точности управления рассматриваемым приводом робота.The disadvantage of this device is that it does not take into account the electric time constant of the electric motor L. In many cases, this can lead to a loss of stability and a decrease in the dynamic accuracy of control of the robot drive in question.

Известно также устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен к входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, выход - к второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, квадратор, шестой сумматор, второй, третий, четвертый и пятый блоки умножения, первый датчик ускорения, а также первый косинусный и второй синусный функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен к второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функциональных преобразователей, а их выходы - соответственно к второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через квадратор с выходом второго датчика скорости, а выходом - с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом первого датчика ускорения, а вторым входом - с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход второго сумматора соединен с третьим входом третьего сумматора, а также второй датчик ускорения, установленный на валу двигателя и выходом подключенный к четвертому входу третьего сумматора, последовательно соединенные седьмой сумматор, первый вход которого подключен к выходу второго датчика скорости, шестой блок умножения, второй вход которого подключен к выходу первого датчика ускорения, седьмой блок умножения, второй вход которого соединен с выходом второго функционального преобразователя, восьмой сумматор, восьмой блок умножения, второй вход которого подключен к выходу пятого сумматора, и девятый сумматор, второй вход которого через последовательно соединенные первый дифференциатор и девятый блок умножения подключен к выходу первого датчика ускорения, а его выход - к пятому входу третьего сумматора, последовательно соединенные десятый и одиннадцатый блоки умножения, причем второй вход девятого блока умножения подключен к выходу шестого сумматора, первый и второй входы десятого блока умножения - соответственно к выходам квадратора и первого функционального преобразователя, второй вход одиннадцатого блока умножения - к выходу первого датчика скорости и второму входу седьмого сумматора, а его выход - к второму входу восьмого сумматора, последовательно соединенные второй датчик положения, установленный во второй степени подвижности манипулятора, десятый сумматор, второй вход которого подключен к выходу первого датчика положения, третий синусный функциональный преобразователь, двенадцатый блок умножения, второй вход которого через второй дифференциатор подключен к выходу третьего датчика ускорения, установленного в первой степени подвижности манипулятора, тринадцатый блок умножения и одиннадцатый сумматор, выход которого подключен к шестому входу третьего сумматора, последовательно соединенные четвертый косинусный функциональный преобразователь, четырнадцатый, пятнадцатый и шестнадцатый блоки умножения, причем выход последнего подключен ко второму входу одиннадцатого сумматора, последовательно соединенные семнадцатый блок умножения, первый вход которого соединен с выходом третьего датчика ускорения и вторым входом шестнадцатого блока умножения, а его второй вход - с выходом третьего функционального преобразователя, и восемнадцатый блок умножения, второй вход которого подключен к выходу пятого сумматора и вторым входам тринадцатого и четырнадцатого блоков умножения, а выход - к пятому входу четвертого сумматора, а также двенадцатый сумматор, первый и второй входы которого подключены соответственно к выходам первого и второго датчиков скорости, а выход - к второму входу пятнадцатого блока умножения (см. патент РФ №2228257, БИ №13, 2004 г.).A device for controlling a robot drive is also known, comprising a 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, 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 ode of the second adder and the output of the first speed sensor, the output is to the second input of the third adder, the first signal pickup and the fifth adder are connected in series, as well as the second speed sensor, mass sensor, the second signal pickup, quadrator, sixth adder, second, third, fourth and the fifth multiplication blocks, the first acceleration sensor, as well as the first cosine and second sine 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 about the multiplication unit, the first input 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 converters, and their outputs, respectively, to the second input of the sixth adder and the first input of the fourth multiplication unit, connected by 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 by the first input to the output of the first acceleration sensor, and the second input to the output of the sixth adder, the third input of which is connected to the output of the second signal setter, and the output of the second adder is connected to the third input of the third adder, as well as the second sensor acceleration mounted on the motor shaft and the output connected to the fourth input of the third adder, the seventh adder connected in series, the first input of which is connected to the output of the second speed sensor, the sixth block multiplication, the second input of which is connected to the output of the first acceleration sensor, the seventh multiplication unit, the second input of which is connected to the output of the second functional converter, the eighth adder, the eighth multiplication unit, the second input of which is connected to the output of the fifth adder, and the ninth adder, the second input of which the first differentiator and the ninth multiplication unit connected in series are connected to the output of the first acceleration sensor, and its output is connected to the fifth input of the third adder, the tenth and od the eleventh multiplication unit, the second input of the ninth multiplication unit connected to the output of the sixth adder, the first and second inputs of the tenth multiplication unit, respectively, to the outputs of the quadrator and the first functional converter, the second input of the eleventh multiplication unit to the output of the first speed sensor and the second input of the seventh adder, and its output - to the second input of the eighth adder, connected in series to the second position sensor installed in the second degree of mobility of the manipulator, the tenth adder, the second input One of which is connected to the output of the first position sensor, the third sine functional converter, the twelfth multiplication unit, the second input of which through the second differentiator is connected to the output of the third acceleration sensor installed in the first degree of manipulator mobility, the thirteenth multiplication unit and the eleventh adder, the output of which is connected to the sixth the input of the third adder, the fourth cosine functional converter, fourteenth, fifteenth and sixteenth blocks are connected in series the output of the latter is connected to the second input of the eleventh adder, the seventeenth multiplication unit is connected in series, the first input of which is connected to the output of the third acceleration sensor and the second input of the sixteenth multiplication unit, and its second input is connected to the output of the third functional converter, and the eighteenth multiplication unit, the second input of which is connected to the output of the fifth adder and the second inputs of the thirteenth and fourteenth multiplication blocks, and the output to the fifth input of the fourth adder, as well as twelve Attiyah adder, the first and second inputs of which are connected respectively to the outputs of the first and second speed sensors and an output - to the second input of the fifteenth multiplication unit (see. RF patent No. 2228257, BI No. 13, 2004).

Недостатком данного устройства является то, что оно предназначено для привода манипулятора, имеющего меньше степеней подвижности. В результате это устройство не будет точно компенсировать все переменные нагрузочные характеристики рассматриваемого привода и обеспечивать требуемую динамическую точность его работы.The disadvantage of this device is that it is designed to drive a manipulator having fewer degrees of mobility. As a result, this device will not accurately compensate for all the variable load characteristics of the drive in question and provide the required dynamic accuracy of its operation.

Задачей, на решение которой направлено предлагаемое техническое решение, является обеспечение высокой динамической точности привода указанной степени подвижности манипулятора с учетом всех его степеней свободы и электрической постоянной времени якорной цепи электродвигателя.The task to which the proposed technical solution is directed is to provide high dynamic accuracy of the drive of the indicated degree of manipulator mobility, taking into account all its degrees of freedom and the electric time constant of the motor anchor chain.

Технический результат, который достигается при реализации изобретения, выражается в формировании дополнительного форсирующего сигнала управления, подаваемого на вход привода, который точнее компенсирует вредное моментное воздействие со стороны других степеней подвижности манипулятора на качественные показатели работы рассматриваемого устройства.The technical result that is achieved during the implementation of the invention is expressed in the formation of an additional boost control signal supplied to the input of the drive, which more accurately compensates for the harmful momentary effect from other degrees of manipulator mobility on the quality performance of the device in question.

Поставленная задача решается тем, что в устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен к входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, выход - к второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, установленный во второй степени подвижности манипулятора, датчик массы, второй задатчик сигнала, квадратор, шестой сумматор, второй, третий, четвертый и пятый блоки умножения, первый датчик ускорения, установленный во второй степени подвижности манипулятора, а также первый косинусный и второй синусный функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен к второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функциональных преобразователей, а их выходы - соответственно к второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через квадратор с выходом второго датчика скорости, а выходом - с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом первого датчика ускорения, а вторым входом - с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход второго сумматора соединен с третьим входом третьего сумматора, а также второй датчик ускорения, установленный на валу двигателя и выходом подключенный к четвертому входу третьего сумматора, последовательно соединенные седьмой сумматор, первый вход которого подключен к выходу второго датчика скорости, шестой блок умножения, второй вход которого подключен к выходу первого датчика ускорения, седьмой блок умножения, второй вход которого соединен с выходом второго функционального преобразователя, восьмой сумматор, восьмой блок умножения, второй вход которого подключен к выходу пятого сумматора, и девятый сумматор, второй вход которого через последовательно соединенные первый дифференциатор и девятый блок умножения подключен к выходу первого датчика ускорения, а его выход - к пятому входу третьего сумматора, последовательно соединенные десятый и одиннадцатый блоки умножения, причем второй вход девятого блока умножения подключен к выходу шестого сумматора, первый и второй входы десятого блока умножения - соответственно к выходам квадратора и первого функционального преобразователя, второй вход одиннадцатого блока умножения - к выходу первого датчика скорости и второму входу седьмого сумматора, а его выход - к второму входу восьмого сумматора, последовательно соединенные второй датчик положения, установленный во второй степени подвижности манипулятора, десятый сумматор, второй вход которого подключен к выходу первого датчика положения, третий синусный функциональный преобразователь, двенадцатый блок умножения, второй вход которого через второй дифференциатор подключен к выходу третьего датчика ускорения, установленного в первой степени подвижности манипулятора, тринадцатый блок умножения и одиннадцатый сумматор, выход которого подключен к шестому входу третьего сумматора, последовательно соединенные четвертый косинусный функциональный преобразователь, подключенный входом к выходу десятого сумматора, четырнадцатый, пятнадцатый и шестнадцатый блоки умножения, причем выход последнего подключен ко второму входу одиннадцатого сумматора, семнадцатый блок умножения, первый вход которого соединен со вторым входом шестнадцатого блока умножения и выходом третьего датчика ускорения, а второй вход - с выходом третьего функционального преобразователя, восемнадцатый блок умножения, второй вход которого подключен к выходу пятого сумматора и вторым входам тринадцатого и четырнадцатого блоков умножения, а выход - к пятому входу четвертого сумматора, а также двенадцатый сумматор, первый и второй входы которого подключены соответственно к выходам первого и второго датчиков скорости, а выход - к второму входу пятнадцатого блока умножения, дополнительно вводятся последовательно соединенные четвертый датчик ускорения, установленный в четвертой степени подвижности манипулятора, третий дифференциатор, девятнадцатый блок умножения, тринадцатый сумматор и двадцатый блок умножения, второй вход которого соединен с выходом пятого сумматора, а выход - с третьим входом одиннадцатого сумматора, последовательно соединенные двадцать первый блок умножения, первый вход которого подключен к выходу третьего функционального преобразователя, и двадцать второй блок умножения, второй вход которого соединен с выходом двенадцатого сумматора, а выход - со вторым входом тринадцатого сумматора, последовательно соединенные двадцать третий блок умножения, первый вход которого подключен к выходу четвертого датчика ускорения и второму входу двадцать первого блока умножения, а второй вход - к второму входу девятнадцатого блока умножения и выходу четвертого косинусного функционального преобразователя, и четырнадцатый сумматор, второй вход которого соединен с выходом семнадцатого блока умножения, а выход - с первым входом восемнадцатого блока умножения.The problem is solved in that in a device for controlling a robot drive, comprising a first adder, a second adder, a first multiplier, a third adder, an amplifier and a motor connected in series with the first speed sensor directly and through a gearbox with the 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, a relay element and a fourth adder are connected in series, the second input of which is connected to the relay input of the second element, the second input of the second adder and the output of the first speed sensor, the output is to the second input of the third adder, the first signal pickup and the fifth adder are connected in series, as well as the second speed sensor installed in the second degree of manipulator mobility, the mass sensor, the second signal pickup, quadrator, sixth adder, second, third, fourth and fifth multiplication blocks, the first acceleration sensor installed in the second degree of manipulator mobility, as well as the first cosine and second sine functionality second 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 adder and the second input of the fifth adder, connected to the first inputs of the second and third multiplication units, the second input of each which are connected respectively to the output of the first and second functional converters, and their outputs are 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 by the first input to the output of the first acceleration sensor, and the second input to the output of the sixth adder, the third input of which connected to the output of the second signal setter, and the output of the second adder connected to the third input of the third adder, as well as a second acceleration sensor mounted on the motor shaft and the output connected to the fourth input tr the fifth adder, the seventh adder connected in series, the first input of which is connected to the output of the second speed sensor, the sixth multiplication unit, the second input of which is connected to the output of the first acceleration sensor, the seventh multiplication unit, the second input of which is connected to the output of the second functional converter, the eighth adder, the eighth a multiplication unit, the second input of which is connected to the output of the fifth adder, and a ninth adder, the second input of which through the first differentiator and the ninth block is connected in series the knife is connected to the output of the first acceleration sensor, and its output is to the fifth input of the third adder, the tenth and eleventh multiplication units are connected in series, the second input of the ninth multiplication unit is connected to the output of the sixth adder, the first and second inputs of the tenth multiplication unit are respectively to the outputs of the quad and the first functional converter, the second input of the eleventh multiplication unit - to the output of the first speed sensor and the second input of the seventh adder, and its output - to the second input of the eighth adder a, a second position sensor installed in a second degree of manipulator mobility in series, a tenth adder, the second input of which is connected to the output of the first position sensor, a third sine function converter, twelfth multiplication unit, the second input of which is connected through the second differentiator to the output of the third acceleration sensor, the manipulator installed in the first degree of mobility, the thirteenth multiplication unit and the eleventh adder, the output of which is connected to the sixth input of the third sum a torus connected in series to a fourth cosine functional converter connected to an input to the output of the tenth adder, fourteenth, fifteenth and sixteenth multiplication units, the output of the latter being connected to the second input of the eleventh adder, a seventeenth multiplication unit, the first input of which is connected to the second input of the sixteenth multiplication unit and the output the third acceleration sensor, and the second input with the output of the third functional converter, the eighteenth multiplication block, the second input of which is connected to the output of the fifth adder and the second inputs of the thirteenth and fourteenth multiplication blocks, and the output to the fifth input of the fourth adder, as well as the twelfth adder, the first and second inputs of which are connected respectively to the outputs of the first and second speed sensors, and the output - to the second input of the fifteenth the multiplication unit, an additional fourth acceleration sensor installed in the fourth degree of manipulator mobility, the third differentiator, the nineteenth multiplication unit, the thirteenth, are additionally introduced in series the adder and the twentieth multiplication unit, the second input of which is connected to the output of the fifth adder, and the output - with the third input of the eleventh adder, the twenty-first multiplication unit, the first input of which is connected to the output of the third functional converter, and the twenty-second multiplication unit, the second input of which connected to the output of the twelfth adder, and the output to the second input of the thirteenth adder, connected in series to the twenty-third multiplication unit, the first input of which is connected to the output of the fourth acceleration sensor and the second input of the twenty-first multiplication block, and the second input is to the second input of the nineteenth multiplication block and the output of the fourth cosine functional converter, and the fourteenth adder, the second input of which is connected to the output of the seventeenth multiplication block, and the output to the first input of the eighteenth block multiplication.

Сопоставительный анализ существенных признаков предлагаемого технического решения с существенными признаками аналогов и прототипа свидетельствует о его соответствии критерию "новизна".A comparative analysis of the essential features of the proposed technical solution with the essential features of analogues and prototype indicates its compliance with the criterion of "novelty."

При этом совокупность существенных признаков отличительной части формулы изобретения позволяет обеспечить полную инвариантность привода к эффектам взаимовлияния между степенями подвижности и моментам трения даже при наличии электрической постоянной времени якорной цепи электродвигателя.Moreover, the set of essential features of the distinguishing part of the claims allows to ensure complete invariance of the drive to the effects of mutual influence between the degrees of mobility and the friction moments even in the presence of an electric time constant of the anchor circuit of the electric motor.

На фиг.2 представлена кинематическая схема исполнительного органа робота. Этот робот имеет пять степеней подвижности. Его первая и четвертая степени подвижности обеспечивают перпендикулярные друг другу линейные перемещения в горизонтальной плоскости (обобщенные координаты q1 и q4), вторая и третья степени (обобщенные координаты q2 и q3) - вращательные движения второго и третьего звеньев манипулятора в горизонтальной плоскости, а пятая степень - вертикальное перемещение вращающихся звеньев (обобщенная координата q5). На фиг.1 представлена блок-схема предлагаемого устройства для управления приводом третьей степени подвижности манипулятора (координата q3).Figure 2 presents the kinematic diagram of the Executive body of the robot. This robot has five degrees of mobility. Its first and fourth degrees of mobility provide linear displacements perpendicular to each other in the horizontal plane (generalized coordinates q 1 and q 4 ), the second and third degrees (generalized coordinates q 2 and q 3 ) - rotational movements of the second and third parts of the manipulator in the horizontal plane, and the fifth degree is the vertical movement of the rotating links (generalized coordinate q 5 ). Figure 1 presents a block diagram of the proposed device for controlling the drive of the third degree of mobility of the manipulator (coordinate q 3 ).

Устройство для управления приводом робота содержит последовательно соединенные первый сумматор 1, второй сумматор 2, первый блок 3 умножения, третий сумматор 4, усилитель 5 и двигатель 6, связанный с первым датчиком скорости 7 непосредственно и через редуктор 8 с первым датчиком 9 положения, выход которого подключен к первому входу первого сумматора 1, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент 10 и четвертый сумматор 11, второй вход которого подключен к входу релейного элемента 10, второму входу второго сумматора 2 и выходу первого датчика 7 скорости, выход - к второму входу третьего сумматора 4, последовательно соединенные первый задатчик 12 сигнала и пятый сумматор 13, а также второй датчик 14 скорости, установленный во второй степени подвижности манипулятора, датчик 15 массы, второй задатчик 16 сигнала, квадратор 17, шестой сумматор 18, второй 19, третий 20, четвертый 21 и пятый 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 ускорения, установленный на валу двигателя и выходом подключенный к четвертому входу третьего сумматора 4, последовательно соединенные седьмой сумматор 27, первый вход которого подключен к выходу второго датчика 14 скорости, шестой блок 28 умножения, второй вход которого подключен к выходу первого датчика 23 ускорения, седьмой блок умножения 29, второй вход которого соединен с выходом второго функционального преобразователя 25, восьмой сумматор 30, восьмой блок 31 умножения, второй вход которого подключен к выходу пятого сумматора 13, и девятый сумматор 32, второй вход которого через последовательно соединенные первый дифференциатор 33 и девятый блок 34 умножения подключен к выходу первого датчика 23 ускорения, а его выход - к пятому входу третьего сумматора 4, последовательно соединенные десятый 35 и одиннадцатый блоки 36 умножения, причем второй вход девятого блока 34 умножения подключен к выходу шестого сумматора 18, первый и второй входы десятого блока 35 умножения - соответственно к выходам квадратора 17 и первого функционального преобразователя 24, второй вход одиннадцатого блока 36 умножения - к выходу первого датчика 7 скорости и второму входу седьмого сумматора 27, а его выход - к второму входу восьмого сумматора 30, последовательно соединенные второй датчик 37 положения, установленный во второй степени подвижности манипулятора, десятый сумматор 38, второй вход которого подключен к выходу первого датчика 9 положения, третий синусный функциональный преобразователь 39, двенадцатый блок 40 умножения, второй вход которого через второй дифференциатор 41 подключен к выходу третьего датчика 42 ускорения, установленного в первой степени подвижности манипулятора, тринадцатый блок 43 умножения и одиннадцатый сумматор 44, выход которого подключен к шестому входу третьего сумматора 4, последовательно соединенные четвертый косинусный функциональный преобразователь 45, подключенный входом к выходу десятого сумматора 38, четырнадцатый 46, пятнадцатый 47 и шестнадцатый 48 блоки умножения, причем выход последнего подключен ко второму входу одиннадцатого сумматора 44, семнадцатый блок 49 умножения, первый вход которого соединен со вторым входом шестнадцатого блока 48 умножения и выходом третьего датчика 42 ускорения, а второй вход - с выходом третьего функционального преобразователя 39, восемнадцатый блок 50 умножения, второй вход которого подключен к выходу пятого сумматора 13 и вторым входам тринадцатого 43 и четырнадцатого 46 блоков умножения, а выход - к пятому входу четвертого сумматора 11, а также двенадцатый сумматор 51, первый и второй входы которого подключены соответственно к выходам первого 7 и второго 14 датчиков скорости, а выход - к второму входу пятнадцатого блока 47 умножения, последовательно соединенные четвертый датчик 52 ускорения, установленный в четвертой степени подвижности манипулятора, третий дифференциатор 53, девятнадцатый блок 54 умножения, тринадцатый сумматор 55 и двадцатый блок 56 умножения, второй вход которого соединен с выходом пятого сумматора 13, а выход - с третьим входом одиннадцатого сумматора 44, последовательно соединенные двадцать первый блок 57 умножения, первый вход которого подключен к выходу третьего функционального преобразователя 39, и двадцать второй блок 58 умножения, второй вход которого соединен с выходом двенадцатого сумматора 51, а выход - со вторым входом тринадцатого сумматора 55, последовательно соединенные двадцать третий блок 59 умножения, первый вход которого подключен к выходу четвертого датчика 52 ускорения и второму входу двадцать первого блока 57 умножения, а второй вход - к второму входу девятнадцатого блока 54 умножения и выходу четвертого косинусного функционального преобразователя 45, и четырнадцатый сумматор 60, второй вход которого соединен с выходом семнадцатого блока 49 умножения, а выход - с первым входом восемнадцатого блока 50 умножения. Объект управления 61 соединен с выходным валом редуктора 8.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 amplifier 5 and the motor 6 connected to the first speed sensor 7 directly and through the gearbox 8 with the first 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, the output is to the second input of the third adder 4, the first signal pickup 12 and the fifth adder 13 are connected in series, as well as the second speed sensor 14 installed in the second degree of manipulator mobility, the mass sensor 15, the second signal adjuster 16, quadrator 17, sixth adder 18, second 19, third 20, fourth 21 and fifth 22 multiplication blocks, the first acceleration sensor 23 installed in the second degree of manipulator mobility, as well as the first 24 cosine and second 25 sine functional second 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 of multiplication, the first input of the sixth adder 18 and the second input of the fifth 13 adder, 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 respectively to the output of the first 24 and second 25 functional converters, and their outputs, respectively, to the second input of the sixth adder 18 and the first input of the fourth multiplication unit 21 I, connected by the second input through the quadrator 17 to the output of the second speed sensor 14, and the output to the third input of the fourth adder 11, the fourth input of which is connected to the output of the fifth multiplication unit 22, connected by the first input to the output of the first acceleration sensor 23, and the second input - with 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, as well as a second acceleration sensor 26 mounted on the motor shaft and the output under connected to the fourth input of the third adder 4, sequentially connected to the seventh adder 27, the first input of which is connected to the output of the second speed sensor 14, the sixth multiplication unit 28, the second input of which is connected to the output of the first acceleration sensor 23, the seventh multiplication unit 29, the second input of which is connected with the output of the second functional converter 25, the eighth adder 30, the eighth multiplication unit 31, the second input of which is connected to the output of the fifth adder 13, and the ninth adder 32, the second input of which is connected through series The first differentiator 33 and the ninth multiplication unit 34 are connected to the output of the first acceleration sensor 23, and its output is connected to the fifth input of the third adder 4, the tenth 35 and eleventh multiplication units 36 are connected in series, and the second input of the ninth multiplication unit 34 is connected to the output of the sixth adder 18, the first and second inputs of the tenth multiplication block 35, respectively, to the outputs of the quadrator 17 and the first functional converter 24, the second input of the eleventh multiplication block 36, to the output of the first speed sensor 7 and the second input two of the seventh adder 27, and its output is to the second input of the eighth adder 30, the second position sensor 37 installed in the second degree of mobility of the manipulator, the tenth adder 38, the second input of which is connected to the output of the first position sensor 9, the third sine functional converter 39 , the twelfth multiplication block 40, the second input of which through the second differentiator 41 is connected to the output of the third acceleration sensor 42 installed in the first degree of manipulator mobility, the thirteenth multiplying block 43 and the eleventh adder 44, the output of which is connected to the sixth input of the third adder 4, connected in series to the fourth cosine functional converter 45, connected by the input to the output of the tenth adder 38, the fourteenth 46, the fifteenth 47, and the sixteenth 48 multiplication units, the output of the latter being connected to the second input the eleventh adder 44, the seventeenth multiplication unit 49, the first input of which is connected to the second input of the sixteenth multiplication unit 48 and the output of the third acceleration sensor 42, and the second input from the output the house of the third functional converter 39, the eighteenth multiplication unit 50, the second input of which is connected to the output of the fifth adder 13 and the second inputs of the thirteenth 43 and fourteenth 46 multiplication units, and the output to the fifth input of the fourth adder 11, as well as the twelfth adder 51, the first and second the inputs of which are connected respectively to the outputs of the first 7 and second 14 speed sensors, and the output to the second input of the fifteenth multiplication unit 47, sequentially connected to the fourth acceleration sensor 52 installed in the fourth step and the mobility of the manipulator, the third differentiator 53, the nineteenth multiplier block 54, the thirteenth adder 55 and the twentieth multiplier block 56, the second input of which is connected to the output of the fifth adder 13, and the output to the third input of the eleventh adder 44, connected in series to the twenty-first multiplier block 57, the first input of which is connected to the output of the third functional converter 39, and the twenty-second multiplication unit 58, the second input of which is connected to the output of the twelfth adder 51, and the output to the second input of the thirteenth sum a matora 55, connected in series to the twenty-third multiplication unit 59, the first input of which is connected to the output of the fourth acceleration sensor 52 and the second input of the twenty-first multiplication unit 57, and the second input to the second input of the nineteenth multiplication unit 54 and the output of the fourth cosine functional converter 45, and the fourteenth adder 60, the second input of which is connected to the output of the seventeenth multiplication block 49, and the output - with the first input of the eighteenth multiplication block 50. The control object 61 is connected to the output shaft of the gearbox 8.

На фиг.1 и 2 введены следующие обозначения: αвх - сигнал желаемого положения третьей степени подвижности манипулятора;

Figure 00000002
Figure 00000003
Figure 00000004
- скорости изменения соответствующих обобщенных координат;
Figure 00000005
Figure 00000006
Figure 00000007
- ускорения соответствующих обобщенных координат; ε - ошибка привода (величина рассогласования); m1, m2, m3, mг - соответственно массы первого, второго, третьего звеньев исполнительного органа и захваченного груза;
Figure 00000008
Figure 00000009
- расстояния от осей вращения соответствующих звеньев до их центров масс; l2, l3 - длины соответствующих звеньев;
Figure 00000010
Figure 00000011
- соответственно, скорость и ускорение вращения ротора двигателя третьей степени подвижности манипулятора; U*, U - соответственно усиливаемый сигнал и сигнал управления двигателем 5.Figure 1 and 2 introduced the following notation: α I - signal of the desired position of the third degree of mobility of the manipulator;
Figure 00000002
Figure 00000003
Figure 00000004
- the rate of change of the corresponding generalized coordinates;
Figure 00000005
Figure 00000006
Figure 00000007
- acceleration of the corresponding generalized coordinates; ε - drive error (mismatch value); m 1 , m 2 , m 3 , m g - respectively, the mass of the first, second, third links of the executive body and the captured cargo;
Figure 00000008
Figure 00000009
- the distance from the axis of rotation of the respective links to their centers of mass; l 2 , l 3 are the lengths of the corresponding links;
Figure 00000010
Figure 00000011
- accordingly, the speed and acceleration of rotation of the rotor of the engine of the third degree of mobility of the manipulator; U * , U - respectively, the amplified signal and the engine control signal 5.

Устройство работает следующим образом. Сигнал ошибки ε с сумматора 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 magnitude 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 of the drive and even leads to a loss of stability of its operation. As a result, a problem arises related to ensuring the invariance of the dynamic properties of the electric drive to continuous and rapid changes in its moment load characteristics, which ensures the stability of a given quality of the control system.

Моментные характеристики привода, управляющего координатой q3, зависят от изменения обобщенных координат и массы захваченного груза. В связи с этим для качественного управления координатой q3 необходимо точно компенсировать отрицательное влияние изменения указанных координат, а также переменной массы груза mг на динамические свойства рассматриваемого привода поворота (координата q3).The moment characteristics of the drive controlling the coordinate q 3 depend on the change in the generalized coordinates and the mass of the captured cargo. In this regard, for quality control of the q 3 coordinate, it is necessary to precisely compensate for the negative effect of changes in the indicated coordinates, as well as the variable mass of the load m g on the dynamic properties of the rotation drive under consideration (q 3 coordinate).

На основе уравнений Лагранжа 2-го рода можно записать, что моментное воздействие на выходной вал привода, управляющего координатой q3, при движении робота (фиг.2) с грузом имеет видBased on the Lagrange equations of the 2nd kind, it can be written that the momentary action on the output shaft of the drive controlling the coordinate q 3 when the robot moves (Fig. 2) with a load has the form

Figure 00000012
Figure 00000012

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

Figure 00000013
и механической
Figure 00000014
Figure 00000015
Figure 00000016
цепей двигателя постоянного тока с постоянными магнитами или независимого возбуждения, рассматриваемый привод, управляющий координатой q3, можно описать следующим дифференциальным уравнениемTaking into account relation (1), as well as the equations of electric
Figure 00000013
and mechanical
Figure 00000014
Figure 00000015
Figure 00000016
chains of a DC motor with permanent magnets or independent excitation, the drive in question, which controls the coordinate q 3 , can be described by the following differential equation

Figure 00000017
Figure 00000017

где R и L - соответственно активное и индуктивное сопротивление якорной цепи двигателя; J - момент инерции якоря двигателя и вращающихся частей редуктора, приведенных к валу двигателя; Kм - коэффициент крутящего момента; Kω - коэффициент противоЭДС; Kв - коэффициент вязкого трения; ip - передаточное отношение редуктора; Мстр - момент сухого трения; Kу - коэффициент усиления усилителя 5; i - ток якоря;

Figure 00000018
- ускорение вращения вала двигателя третьей степени подвижности.where R and L are respectively the active and inductive resistance of the motor armature circuit; J is the moment of inertia of the motor armature and the rotating parts of the gearbox brought to the motor shaft; K m - torque coefficient; K ω is the coefficient of counter-EMF; K in - coefficient of viscous friction; i p - gear ratio; M p is the moment of dry friction; K y - gain of the amplifier 5; i is the armature current;
Figure 00000018
- acceleration of rotation of the motor shaft of the third degree of mobility.

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

Figure 00000019
Figure 00000020
Figure 00000021
Figure 00000022
Figure 00000023
Figure 00000024
Figure 00000025
В результате в процессе работы привода существенно меняются его динамические свойства и для реализации поставленной выше задачи необходимо сформировать такое корректирующее устройство, которое стабилизировало бы параметры привода таким образом, чтобы он описывался дифференциальным уравнением с постоянными желаемыми параметрами.From (2) it can be seen that the parameters of this equation, and therefore the parameters of the drive controlling the coordinate q 3 , are essentially variable, depending on the values of m g , q 2 , q 3 ,
Figure 00000019
Figure 00000020
Figure 00000021
Figure 00000022
Figure 00000023
Figure 00000024
Figure 00000025
As a result, during the operation of the drive, its dynamic properties change significantly and 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) имеют единичные коэффициенты усиления, пятый положительный (со стороны блока 50) - коэффициент усиления l/l2, а его второй положительный вход (со стороны датчика 7) - коэффициент усиления (KмKω/R+Kв). Причем выходной сигнал релейного элемента 10 с нулевой нейтральной точкой имеет видThe first positive input of adder 2 (from the adder 1 side) has a unity gain, and its second negative input has a gain K ω / K у . The first, third and fourth positive inputs of the adder 11 (respectively from the side of the relay element 10, block 21 and block 22) have unit gains, the fifth positive (from the side of block 50) has a gain l / l 2 , and its second positive input ( on the sensor side 7) is the gain (K m K ω / R + K in ). Moreover, the output signal of the relay element 10 with a zero neutral point has the form

Figure 00000026
Figure 00000026

где |Мт| - величина момента сухого трения при движении.where | M t | - the value of the moment of dry friction during movement.

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

Figure 00000027
второй положительный (со стороны сумматора 11) - коэффициент усиления R/(KмKу), а третий положительный (со стороны сумматора 2) - коэффициент усиления
Figure 00000028
где Jн - номинальное (желаемое) значение приведенного момента инерции, обеспечивающее рассматриваемому приводу заданные динамические свойства и показатели качества.The first positive input of the adder 4 (from the side of unit 3) has a gain
Figure 00000027
the second positive (from adder 11) is the gain R / (K m K у ), and the third positive (from adder 2) is the gain
Figure 00000028
where J n is the nominal (desired) value of the reduced moment of inertia, which provides the drive with the specified dynamic properties and quality indicators.

Первый положительный вход сумматора 13 (со стороны датчика 15) имеет коэффициент усиления l2l3/ip, а его второй положительный вход (со стороны задатчика 12) - единичный коэффициент усиления. Сигнал с выхода задатчика 12 равен

Figure 00000029
а задатчика 16 сигнала -
Figure 00000030
Первый (со стороны блока 19) и третий (со стороны задатчика 16) положительные входы сумматора 18 имеют единичные коэффициенты усиления, а второй положительный вход (со стороны датчика 15) - коэффициент усиления
Figure 00000031
Таким образом, на выходе сумматора 13 формируется сигнал
Figure 00000032
Функциональный преобразователь 24 формирует сигнал cosq3, поэтому на выходе блока 19 появляется сигнал
Figure 00000033
а на выходе сумматора 18 - сигнал
Figure 00000034
The first positive input of the adder 13 (from the sensor 15 side) has a gain l 2 l 3 / i p , and its second positive input (from the setpoint 12) has a unity gain. The signal from the output of the setter 12 is equal to
Figure 00000029
and the setter 16 signal
Figure 00000030
The first (from the side of the block 19) and third (from the setter 16) positive inputs of the adder 18 have unity gain, and the second positive input (from the side of the sensor 15) is the gain
Figure 00000031
Thus, at the output of the adder 13, a signal is generated
Figure 00000032
The functional Converter 24 generates a signal cosq 3 , therefore, at the output of block 19, a signal appears
Figure 00000033
and at the output of the adder 18 is a signal
Figure 00000034

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

Figure 00000035
поэтому на выходе блока 22 формируется сигнал
Figure 00000036
Датчик 14 измеряет скорость вращения второй степени подвижности (координату
Figure 00000037
а функциональный преобразователь 25 формирует сигнал sinq3. Поэтому на выходе блока 20 появляется сигнал
Figure 00000038
а на выходе блока 21 - сигнал
Figure 00000039
The sensor 23 measures the acceleration of rotation of the second degree of mobility (coordinate
Figure 00000035
therefore, at the output of block 22, a signal is generated
Figure 00000036
The sensor 14 measures the speed of rotation of the second degree of mobility (coordinate
Figure 00000037
and the functional Converter 25 generates a sinq signal 3 . Therefore, a signal appears at the output of block 20
Figure 00000038
and at the output of block 21, a signal
Figure 00000039

Датчик 37 измеряет обобщенную координату q2. Первый и второй положительные входы сумматора 38 имеют единичные коэффициенты усиления, поэтому на выходе функционального преобразователя 39 формируется сигнал sin(q2+q3), а на выходе функционального преобразователя 45 - сигнал cos(q2+q3). Датчик 42 измеряет ускорение

Figure 00000040
В результате на выходе блока 43 формируется сигнал
Figure 00000041
The sensor 37 measures the generalized coordinate q 2 . The first and second positive inputs of the adder 38 have unity gain, therefore, the signal sin (q 2 + q 3 ) is generated at the output of the functional converter 39, and the signal cos (q 2 + q 3 ) is output at the output of the functional converter 45. Sensor 42 measures acceleration
Figure 00000040
As a result, a signal is generated at the output of block 43
Figure 00000041

Датчик 52 измеряет ускорение

Figure 00000042
В результате на выходе блока 59 формируется сигнал
Figure 00000043
Входы сумматора 60 имеют единичные коэффициенты усиления, поэтому на выходе блока 50 формируется сигнал
Figure 00000044
Sensor 52 measures acceleration
Figure 00000042
As a result, a signal is generated at the output of block 59
Figure 00000043
The inputs of the adder 60 have unit gains, so a signal is generated at the output of block 50
Figure 00000044

С учетом отмеченных выше коэффициентов усиления соответствующих входов сумматора 11 на его выходе формируется сигналTaking into account the gains noted above, the corresponding inputs of the adder 11 at its output, a signal

Figure 00000045
Figure 00000045

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

Figure 00000046
а на выходе блока 3 - сигнал
Figure 00000047
At the output of adder 2, a signal is generated
Figure 00000046
and at the output of block 3 - a signal
Figure 00000047

Датчик 26 измеряет ускорение вращения выходного вала электродвигателя

Figure 00000048
четвертый положительный вход сумматора 4 (со стороны датчика 26) имеет коэффициент усиления
Figure 00000049
Первый положительный вход сумматора 27 (со стороны датчика 14) имеет коэффициент усиления 2, а его второй отрицательный вход - коэффициент усиления l/ip. В результате на выходе блока 29 формируется сигнал
Figure 00000050
На выходе блока 34 формируется сигнал
Figure 00000051
На выходе блока 36 формируется сигнал
Figure 00000052
Первый (со стороны блока 29) положительный вход сумматора 30 имеет единичный коэффициент, а его второй положительный вход - коэффициент усиления l/ip. В результате на выходе блока 31 формируется сигнал
Figure 00000053
The sensor 26 measures the acceleration of rotation of the output shaft of the electric motor
Figure 00000048
the fourth positive input of the adder 4 (from the sensor 26) has a gain
Figure 00000049
The first positive input of the adder 27 (from the side of the sensor 14) has a gain of 2, and its second negative input has a gain of l / i p. As a result, a signal is generated at the output of block 29
Figure 00000050
At the output of block 34, a signal is generated
Figure 00000051
At the output of block 36, a signal is generated
Figure 00000052
The first (from the side of block 29) positive input of the adder 30 has a unity coefficient, and its second positive input has a gain l / i p . As a result, a signal is generated at the output of block 31
Figure 00000053

Первый положительный вход сумматора 51 (со стороны датчика 7) имеет коэффициент усиления l/ip, а его второй положительный вход - единичный коэффициент усиления. В результате на его выходе формируется сигнал

Figure 00000054
The first positive input of the adder 51 (from the side of the sensor 7) has a gain l / i p , and its second positive input has a unity gain. As a result, a signal is generated at its output.
Figure 00000054

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

Figure 00000055
на выходе блока 48 - сигнал
Figure 00000056
а на выходе блока 54 - сигнал
Figure 00000057
Первый положительный (со стороны блока 54) и второй отрицательный входы сумматора 55 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал
Figure 00000058
а на выходе блока 56 - сигнал
Figure 00000059
Все положительные входы сумматора 44 имеют коэффициенты усиления l/l2, поэтому на его выходе формируется сигнал
Figure 00000060
At the output of block 58, a signal is generated
Figure 00000055
at the output of block 48, a signal
Figure 00000056
and at the output of block 54, a signal
Figure 00000057
The first positive (from the side of block 54) and the second negative inputs of the adder 55 have unity gain. As a result, a signal is generated at its output.
Figure 00000058
and at the output of block 56, a signal
Figure 00000059
All positive inputs of the adder 44 have gains l / l 2 , therefore, a signal is generated at its output
Figure 00000060

Первый и второй положительные входы сумматора 32 имеют единичные коэффициенты усиления, а пятый и шестой (со стороны сумматоров 32 и 44) положительные входы сумматора 4 - коэффициенты усиления L/(KуKм). Таким образом, с учетом указанных коэффициентов усиления соответствующих входов сумматора 4 на его выходе окончательно формируется сигналThe first and second positive inputs of adder 32 have unit gains, and the fifth and sixth (from adders 32 and 44) the positive inputs of adder 4 have gain L / (K at K m ). Thus, taking into account the indicated gains of the corresponding inputs of the adder 4, a signal is finally formed at its output

Figure 00000061
Figure 00000061

Figure 00000062
Figure 00000062

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

Figure 00000063
при движении привода достаточно точно соответствует Мстр, то, подставив полученное значение U*(3) в соотношение (2), получим уравнение
Figure 00000064
которое имеет постоянные желаемые параметры, т.е. привод, управляющий координатой q3, будет обладать постоянными желаемыми динамическими свойствами и качественными показателями.It is easy to show that since
Figure 00000063
when the drive moves quite accurately corresponds to M p , then, substituting the obtained value U * (3) in relation (2), we obtain the equation
Figure 00000064
which has constant desired parameters, i.e. the drive controlling the coordinate q 3 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, an amplifier and a motor 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, 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 the second the sensor and the output of the first speed sensor, and the output to the second input of the third adder, connected in series with the first signal pickup and the fifth adder, as well as the second speed sensor installed in the second degree of manipulator mobility, the mass sensor, the second signal pickup, the quadrator, the sixth adder, the second, third, fourth and fifth blocks of multiplication, the first acceleration sensor installed in the second degree of mobility of the manipulator, as well as the first cosine and second sine function 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 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 the second functional converters, and their outputs, respectively, to the second input of the sixth adder and the first input of the fourth multiplication unit, connected by the second input through the quadrator to the output ohm 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 by the first input to the output of the first 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 master signal, and the output of the second adder is connected to the third input of the third adder, as well as the second acceleration sensor mounted on the motor shaft and the output is connected to the fourth input of the third adder, in series with the seventh adder, the first input of which is connected to the output of the second speed sensor, the sixth multiplication unit, the second input of which is connected to the output of the first acceleration sensor, the seventh multiplication unit, the second input of which is connected to the output of the second functional converter, the eighth adder, the eighth multiplication unit, the second the input of which is connected to the output of the fifth adder, and the ninth adder, the second input of which is connected in series through the first differentiator and the ninth multiplication unit to the output of the first acceleration sensor, and its output to the fifth input of the third adder, the tenth and eleventh multiplication units connected in series, the second input of the ninth multiplication unit connected to the output of the sixth adder, the first and second inputs of the tenth multiplication unit, respectively, to the outputs of the quadrator and the first functional converter, the second input of the eleventh multiplication block - to the output of the first speed sensor and the second input of the seventh adder, and its output - to the second input of the eighth adder, connected in series to the second the th position sensor installed in the second degree of manipulator mobility, the tenth adder, the second input of which is connected to the output of the first position sensor, the third sine function converter, the twelfth multiplication unit, the second input of which is connected through the second differentiator to the output of the third acceleration sensor installed in the first degree the mobility of the manipulator, the thirteenth multiplication unit and the eleventh adder, the output of which is connected to the sixth input of the third adder, connected in series to a fourth cosine functional converter connected to the input of the tenth adder, fourteenth, fifteenth and sixteenth multiplication units, the output of the latter being connected to the second input of the eleventh adder, the seventeenth multiplication unit, the first input of which is connected to the second input of the sixteenth multiplication unit and the output of the third acceleration sensor, and the second input - with the output of the third functional converter, the eighteenth multiplication unit, the second input of which is connected to the output of the fifth adder and the first inputs of the thirteenth and fourteenth multiplication units, and the output to the fifth input of the fourth adder, as well as the twelfth adder, the first and second inputs of which are connected respectively to the outputs of the first and second speed sensors, and the output to the second input of the fifteenth multiplication unit, characterized in that a fourth acceleration sensor installed in the fourth degree of manipulator mobility, a third differentiator, a nineteenth multiplication unit, a thirteenth adder, and the twentieth multiplication unit, the second input of which is connected to the output of the fifth adder, and the output is the third input of the eleventh adder, the twenty-first multiplication unit, the first input of which is connected to the output of the third functional converter, and the twenty-second multiplication unit, the second input of which is connected to the output of the twelfth adder, and the output with the second input of the thirteenth adder connected in series to the twenty-third multiplication block, the first input of which is connected to the output of the fourth acceleration sensor and the second input of the twenty-first multiplication block, and the second input to the second input of the nineteenth multiplication block and the output of the fourth cosine functional converter, and the fourteenth adder, the second input of which is connected to the output of the seventeenth multiplication block, and the output - with the first input of the eighteenth multiplication block .
RU2006129957/02A 2006-08-18 2006-08-18 Control of robotic machine drive RU2325268C1 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
RU2006129957/02A RU2325268C1 (en) 2006-08-18 2006-08-18 Control of robotic machine drive

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
RU2006129957/02A RU2325268C1 (en) 2006-08-18 2006-08-18 Control of robotic machine drive

Publications (2)

Publication Number Publication Date
RU2006129957A RU2006129957A (en) 2008-02-27
RU2325268C1 true RU2325268C1 (en) 2008-05-27

Family

ID=39278520

Family Applications (1)

Application Number Title Priority Date Filing Date
RU2006129957/02A RU2325268C1 (en) 2006-08-18 2006-08-18 Control of robotic machine drive

Country Status (1)

Country Link
RU (1) RU2325268C1 (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2562403C1 (en) * 2014-12-10 2015-09-10 Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) Self-tuning electric drive of manipulator
RU2562400C1 (en) * 2014-12-10 2015-09-10 Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) Self-tuning electric drive of manipulator
RU2565779C1 (en) * 2014-12-10 2015-10-20 Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) Self-adjusting electric drive of manipulator

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2562403C1 (en) * 2014-12-10 2015-09-10 Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) Self-tuning electric drive of manipulator
RU2562400C1 (en) * 2014-12-10 2015-09-10 Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) Self-tuning electric drive of manipulator
RU2565779C1 (en) * 2014-12-10 2015-10-20 Федеральное государственное бюджетное учреждение науки Институт автоматики и процессов управления Дальневосточного отделения Российской академии наук (ИАПУ ДВО РАН) Self-adjusting electric drive of manipulator

Also Published As

Publication number Publication date
RU2006129957A (en) 2008-02-27

Similar Documents

Publication Publication Date Title
RU2394674C2 (en) Self-adaptive electric drive of robot
RU2372186C1 (en) Self-tuning electric drive of manipulation robot
RU2325268C1 (en) Control of robotic machine drive
RU2423224C2 (en) Robot electric drive
RU2312007C1 (en) Robot drive control apparatus
RU2423225C1 (en) Robot electric drive
RU2359306C2 (en) Self-adapting electric drive of robot
RU2311283C1 (en) Device for controlling drive of a robot
RU2363972C2 (en) Robot self-adaptive electric drive
RU2311284C1 (en) Device for controlling drive of a robot
RU2345885C1 (en) Robot drive control device
RU2066626C1 (en) Robot drive control device
RU2063867C1 (en) Self-tuning electric motor drive of robot
RU2212329C1 (en) Device for control of robot drive
RU2274884C1 (en) Device for controlling motor of robot
RU2193480C1 (en) Device controlling robot drive
RU2380215C1 (en) Self-tuning electric drive of robot
RU2335389C2 (en) Robot drive control device
RU2398672C1 (en) Robot electric drive
RU2705737C1 (en) Self-tuning electric manipulator drive
RU2309444C2 (en) Self-adjusting electrical drive for robot
RU2228257C1 (en) Apparatus for controlling drive unit of robot
RU2424894C1 (en) Robot electric drive
RU2163190C1 (en) Device for control of robot drive
RU2443543C1 (en) Manipulator electric drive

Legal Events

Date Code Title Description
MM4A The patent is invalid due to non-payment of fees

Effective date: 20080819