самонастраивающийся электропривод манипуляционного робота

Формула изобретения

Самонастраивающийся электропривод манипуляционного робота, содержащий последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, первый усилитель и электродвигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом со входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен ко входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, а выход - ко второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, первый квадратор, шестой сумматор, второй, третий, четвертый и пятый блоки умножения, первый датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен ко второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функциональных преобразователей, а их выходы - соответственно ко второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через первый квадратор с выходом второго датчика скорости, а выходом - с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом первого датчика ускорения, а вторым входом - с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход второго сумматора соединен с третьим входом третьего сумматора, последовательно соединенные второй датчик положения, седьмой сумматор, второй вход которого подключен к выходу первого датчика положения, третий функциональный преобразователь и шестой блок умножения, второй вход которого подключен к выходу пятого сумматора, а выход - к пятому входу четвертого сумматора, последовательно соединенные второй усилитель, четвертый функциональный преобразователь, седьмой блок умножения, восьмой сумматор и восьмой блок умножения, последовательно соединенные пятый функциональный преобразователь, девятый и десятый блоки умножения, последовательно соединенные третий датчик скорости и второй квадратор, выход которого подключен ко второму входу восьмого блока умножения, выход которого соединен с шестым входом четвертого сумматора, последовательно соединенные третий задатчик постоянного сигнала и девятый сумматор, второй вход которого подключен к выходу второго задатчика постоянного сигнала, его третий вход - к выходу датчика массы, а выход - ко второму входу седьмого блока умножения, причем второй вход десятого блока умножения через шестой функциональный преобразователь подключен к выходу седьмого сумматора и входу второго усилителя, а его выход - ко второму входу восьмого сумматора, второй вход девятого блока умножения соединен с выходом пятого сумматора, а вход пятого функционального преобразователя - с выходом второго датчика положения, отличающийся тем, что в него дополнительно введены последовательно соединенные третий датчик положения, седьмой функциональный преобразователь, одиннадцатый блок умножения, второй вход которого подключен к выходу пятого сумматора, двенадцатый блок умножения, второй вход которого подключен к выходу второго датчика ускорения, и тринадцатый блок умножения, второй вход которого подключен к выходу шестого функционального преобразователя, а выход - к седьмому входу четвертого сумматора.

Описание изобретения к патенту

Изобретение относится к робототехнике и может быть использовано для создания систем управления приводами манипуляционных роботов.

Известно устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен к входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, выход - к второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, квадратор, шестой сумматор и с второго по пятый блоки умножения, датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен к второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функционального преобразователя, а их выходы - соответственно к второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через квадратор с выходом второго датчика скорости, а выходом с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом датчика ускорения, а вторым входом с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, выход второго сумматора соединен с третьим входом третьего сумматора (А.с. СССР № 1782721, БИ № 47, 1992 г.).

Недостатком данного устройства является то, что оно предназначено только для конкретного электропривода манипулятора с другой кинематической схемой. Для приводов рассматриваемого манипулятора это устройство не будет обеспечивать требуемую точность и устойчивость работы.

Известно также устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, первый усилитель и электродвигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом со входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен ко входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, а выход - ко второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, первый квадратор, шестой сумматор, второй, третий, четвертый и пятый блоки умножения, первый датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен ко второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функциональных преобразователей, а их выходы - соответственно ко второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через первый квадратор с выходом второго датчика скорости, а выходом - с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом первого датчика ускорения, а вторым входом - с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход второго сумматора соединен с третьим входом третьего сумматора, последовательно соединенные второй датчик положения, седьмой сумматор, второй вход которого подключен к выходу первого датчика положения, третий функциональный преобразователь и шестой блок умножения, второй вход которого подключен к выходу пятого сумматора, а выход - к пятому входу четвертого сумматора, последовательно соединенные второй усилитель, четвертый функциональный преобразователь, седьмой блок умножения, восьмой сумматор и восьмой блок умножения, последовательно соединенные пятый функциональный преобразователь, девятый и десятый блоки умножения, последовательно соединенные третий датчик скорости и второй квадратор, выход которого подключен ко второму входу восьмого блока умножения, выход которого соединен с шестым входом четвертого сумматора, последовательно соединенные третий задатчик постоянного сигнала и девятый сумматор, второй вход которого подключен к выходу второго задатчика постоянного сигнала, его третий вход - к выходу датчика массы, а выход - ко второму входу седьмого блока умножения, причем второй вход десятого блока умножения через шестой функциональный преобразователь подключен к выходу седьмого сумматора и входу второго усилителя, а его выход - ко второму входу восьмого сумматора, второй вход девятого блока умножения соединен с выходом пятого сумматора, а вход пятого функционального преобразователя - с выходом второго датчика положения (А.с. СССР № 2066626, БИ № 26, 1996 г.).

Данное устройство по своей технической сущности является наиболее близким к предлагаемому решению.

Недостатком данного устройства является то, что оно предназначено для электропривода манипуляционного робота, имеющего меньше степеней подвижности. В результате это устройство не будет точно компенсировать все переменные нагрузочные характеристики рассматриваемого электропривода и обеспечивать требуемую динамическую точность его работы. Поэтому возникает задача построения такой самонастраивающейся коррекции, которая обеспечила бы высокую динамическую точность работы рассматриваемого электропривода с учетом указанных дополнительных моментных воздействий.

Технической задачей, на решение которой направлено заявляемое техническое решение, является обеспечение полной инвариантности динамических свойств рассматриваемого электропривода к непрерывным и быстрым изменениям его динамических моментных нагрузочных характеристик при движении манипулятора одновременно по всем его степеням подвижности.

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

Поставленная задача решается тем, что в самонастраивающийся электропривод манипуляционного робота, содержащий последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, первый усилитель и электродвигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом со входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен ко входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, а выход - ко второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, первый квадратор, шестой сумматор, второй, третий, четвертый и пятый блоки умножения, первый датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен ко второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функциональных преобразователей, а их выходы - соответственно ко второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через первый квадратор с выходом второго датчика скорости, а выходом - с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом первого датчика ускорения, а вторым входом - с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход второго сумматора соединен с третьим входом третьего сумматора, последовательно соединенные второй датчик положения, седьмой сумматор, второй вход которого подключен к выходу первого датчика положения, третий функциональный преобразователь и шестой блок умножения, второй вход которого подключен к выходу пятого сумматора, а выход - к пятому входу четвертого сумматора, последовательно соединенные второй усилитель, четвертый функциональный преобразователь, седьмой блок умножения, восьмой сумматор и восьмой блок умножения, последовательно соединенные пятый функциональный преобразователь, девятый и десятый блоки умножения, последовательно соединенные третий датчик скорости и второй квадратор, выход которого подключен ко второму входу восьмого блока умножения, выход которого соединен с шестым входом четвертого сумматора, последовательно соединенные третий задатчик постоянного сигнала и девятый сумматор, второй вход которого подключен к выходу второго задатчика постоянного сигнала, его третий вход - к выходу датчика массы, а выход - ко второму входу седьмого блока умножения, причем второй вход десятого блока умножения через шестой функциональный преобразователь подключен к выходу седьмого сумматора и входу второго усилителя, а его выход - ко второму входу восьмого сумматора, второй вход девятого блока умножения соединен с выходом пятого сумматора, а вход пятого функционального преобразователя - с выходом второго датчика положения, дополнительно вводятся последовательно соединенные третий датчик положения, седьмой функциональный преобразователь, одиннадцатый блок умножения, второй вход которого подключен к выходу пятого сумматора, двенадцатый блок умножения, второй вход которого подключен к выходу второго датчика ускорения, и тринадцатый блок умножения, второй вход которого подключен к выходу шестого функционального преобразователя, а выход - к седьмому входу четвертого сумматора.

Сопоставительный анализ заявляемого технического решения с его аналогами и прототипом свидетельствует о его соответствии критерию «Новизна».

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

Блок-схема предлагаемого самонастраивающегося электропривода манипуляционного робота представлена на фиг.1. На фиг.2 представлена кинематическая схема манипулятора.

Самонастраивающийся электропривод манипуляционного робота содержит последовательно соединенные первый сумматор 1, второй сумматор 2, первый блок 3 умножения, третий сумматор 4, первый усилитель 5 и электродвигатель 6, связанный с первым датчиком 7 скорости непосредственно и через редуктор 8 с первым датчиком 9 положения, выход которого подключен к первому входу первого сумматора 1, соединенного вторым входом со входом устройства, последовательно подключенные релейный элемент 10 и четвертый сумматор 11, второй вход которого подключен ко входу релейного элемента 10, второму входу второго сумматора и выходу первого датчика 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 положения, седьмой сумматор 27, второй вход которого подключен к выходу первого датчика 9 положения, третий функциональный преобразователь 28 и шестой блок 29 умножения, второй вход которого подключен к выходу пятого сумматора 13, а выход - к пятому входу четвертого сумматора 11, последовательно соединенные второй усилитель 30; четвертый функциональный преобразователь 31, седьмой блок 32 умножения, восьмой сумматор 33 и восьмой блок 34 умножения, последовательно соединенные пятый функциональный преобразователь 35, девятый 36 и десятый 37 блоки умножения, последовательно соединенные третий датчик 38 скорости и второй квадратор 39, выход которого подключен ко второму входу восьмого блока 34 умножения, выход которого соединен с шестым входом четвертого сумматора 11, последовательно соединенные третий задатчик 40 постоянного сигнала и девятый сумматор 41, второй вход которого подключен к выходу второго задатчика 16 постоянного сигнала, его третий вход - к выходу датчика 15 массы, а выход - ко второму входу седьмого блока 32 умножения, причем второй вход десятого блока 37 умножения через шестой функциональный преобразователь 42 подключен к выходу седьмого сумматора 27 и входу второго усилителя 30, а его выход - ко второму входу восьмого сумматора 33, второй вход девятого блока 36 умножения соединен с выходом пятого сумматора 13, а вход пятого функционального преобразователя 35 - с выходом второго датчика 26 положения, последовательно соединенные третий датчик 43 положения, седьмой функциональный преобразователь 44, одиннадцатый блок 45 умножения, второй вход которого подключен к выходу пятого сумматора 13, двенадцатый блок 46 умножения, второй вход которого подключен к выходу второго датчика 47 ускорения, и тринадцатый блок 48 умножения, второй вход которого подключен к выходу шестого функционального преобразователя 42, а выход - к седьмому входу четвертого сумматора 11, объект управления 49.

На рисунках приведены следующие обозначения:

_вх - сигнал с выхода программного устройства; - сигнал ошибки электропривода; U*, U - соответственно усиливаемый сигнал и сигнал управления двигателем; q_i - обобщенные координаты соответствующих степеней подвижности манипулятора ; m_i, m_г, - массы соответствующих звеньев манипулятора и груза ; l₂, l₃ - длины соответствующих звеньев; l₂ , l₃ - расстояния от осей вращения соответствующих звеньев манипулятора до их центров масс; - скорости изменения соответствующих обобщенных координат манипулятора; - скорость вращения ротора электродвигателя третьей степени подвижности манипулятора; , ускорения во второй и четвертой степенях подвижности манипулятора.

Самонастраивающийся электропривод работает следующим образом. На вход подается воздействие _вх, обеспечивающее требуемый закон управления рассматриваемой степенью подвижности манипулятора. На выходе сумматора 1 вырабатывается сигнал ошибки , который после коррекции в элементах 2 и 3, усиливаясь, поступает на вход электродвигателя 5 с редуктором, приводя его вал во вращательное движение с направлением и скоростью (ускорением), зависящими от величины поступающего сигнала U и внешнего моментного воздействия М_в на привод.

В изобретении рассматривается электропривод, управляющий обобщенной координатой q₃ манипулятора (см. фиг.2).

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

где J_si, - моменты инерции соответствующих звеньев манипулятора относительно их продольных осей ; J_Ni - моменты инерции соответствующих звеньев манипулятора относительно поперечных осей, проходящих через их центры масс (i=2, 3), g - ускорение свободного падения. С учетом соотношения (1), а также уравнений электрической и механической

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

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

Из уравнения (2) видно, что параметры этого уравнения, а следовательно, и параметры электропривода, управляющего координатой q₃, являются существенно переменными, зависящими от величин m_г, q₂ , q₃, q₁, q₂, q₂, q₄. В результате в процессе работы рассматриваемого электропривода меняются (притом существенно) его динамические свойства. Поэтому для реализации поставленной выше задачи необходимо сформировать такое корректирующее устройство, которое застабилизировало бы параметры этого электропривода таким образом, чтобы он описывался дифференциальным уравнением с постоянными желаемыми параметрами.

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

где |М_Г| - величина момента сухого трения при движении.

Второй положительный вход сумматора 13 (со стороны датчика 15) имеет коэффициент усиления l₂l₃/i_р, а его первый положительный вход (со стороны задатчика 12) - единичный коэффициент усиления. Сигнал с выхода задатчика 12 равен m₂l₂ l₃/i_P, а с выхода задатчика 16 - (J _N3+m₃l₃ ²)/i_р. Второй (со стороны блока 19) и третий (со стороны задатчика 16) положительные входы сумматора 18 имеют единичные коэффициенты усиления, а его первый положительный вход (со стороны датчика 15) - коэффициент усиления .

Таким образом, на выходе сумматора 13 формируется сигнал . Функциональный преобразователь 24 формирует сигнал cosq ₃. Датчик 23 измеряет ускорение Поэтому на выходе блока 19 появляется сигнал , на выходе сумматора 18 - сигнал , а на выходе блока 22 - сигнал .

Датчик 14 измеряет скорость , а функциональный преобразователь 25 формирует сигнал sinq₃. Поэтому на выходе блока 21 появляется сигнал .

Датчик 26 измеряет угол функциональный преобразователь 28 формирует сигнал sin(q ₂+q₃). В результате на выходе блока 29 формируется сигнал .

Датчик 38 измеряет скорость . С выхода задатчика 40 на первый положительный вход сумматора 41 с единичным коэффициентом усиления поступает сигнал (-J _S3/i_p)второй (со стороны задатчика 16) и третий (со стороны датчика 15) положительные входы имеют соответственно единичный коэффициентом усиления и коэффициент усиления, равный . В результате на выходе сумматора 41 появляется сигнал .

Усилитель 30 имеет коэффициент усиления, равный 2. Функциональный преобразователь 31 реализует функцию sin. В результате на выходе блока 32 формируется сигнал .

Поскольку функциональный преобразователь 35 реализует функцию sin, а функциональный преобразователь 42 - функцию cos, то на выходе блока 37 формируется сигнал , а на выходе сумматора 33, первый (со стороны блока 32) и второй положительные входы которого имеют соответственно единичный коэффициент усиления и коэффициент усиления, равный 2, формируется сигнал .

Датчик 43 измеряет угол поворота q ₁. Функциональный преобразователь 44 реализует функцию cosq₁. Датчик 47 измеряет ускорение . Поэтому на выходе блока 48 формируется сигнал .

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

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

Первый положительный вход сумматора 4 (со стороны блока 3) имеет коэффициент усиления , второй положительный (со стороны сумматора 11) - коэффициент усиления R/(K_M, К_у), а третий положительный (со стороны сумматора 2) - коэффициент усиления , где J_H - желаемое значение приведенного момента инерции, обеспечивающее рассматриваемому электроприводу требуемые динамические свойства и показатели качества. В результате на выходе сумматора 4 будет сформирован сигнал

Несложно показать, что поскольку при движении привода достаточно точно соответствует M _CTP, то, подставив полученное значение U* (3) в соотношение (2), получим уравнение которое имеет постоянные желаемые параметры. То есть электропривод, управляющий координатой q₃, будет обладать постоянными желаемыми динамическими свойствами и качественными показателями.