устройство для управления приводом робота

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

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

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

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

Известно устройство для управления приводом робота, содержащее последовательно соединенные первый блок умножения и первый сумматор, последовательно подключенные усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого соединен с первым входом второго сумматора, подключенного вторым входом к входу устройства, последовательно соединенные второй датчик положения, третий сумматор, четвертый сумматор, первый квадратор и второй блок умножения, второй вход которого подключен к выходу датчика массы и первому входу третьего блока умножения, а выход - к первому входу пятого сумматора, соединенного вторым входом с выходом первого задатчика сигнала, а третьим входом - с выходом второго квадратора, вход которого подключен к выходу третьего сумматора и первому входу шестого сумматора, соединенного выходом с первым входом четвертого блока умножения, а вторым входом - с выходом третьего блока умножения, второй вход которого подключен к выходу четвертого сумматора, соединенного вторым входом с выходом второго задатчика сигнала, выход третьего задатчика сигнала подключен к второму входу третьего сумматора, а выход второго датчика скорости соединен с вторым входом четвертого блока умножения. Кроме того, оно содержит пятый блок умножения, седьмой сумматор и последовательно соединенные релейный блок и восьмой сумматор, выход которого подключен к второму входу первого сумматора, соединенного выходом с входом усилителя, выход первого датчика скорости подключен к входу релейного блока, к второму входу восьмого сумматора и первому входу седьмого сумматора, второй вход которого соединен с выходом второго сумматора, а выход - с первым входом первого блока умножения, подключенного вторым входом к выходу пятого сумматора, первый вход пятого блока умножения соединен с выходом четвертого блока умножения, второй вход - с выходом первого датчика скорости, а выход - с третьим входом восьмого сумматора (см. а.с. СССР 1484702, МКИ В 25 J 13/00, 1989 г.).

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

Известно также устройство для управления приводом робота, содержащее последовательно соединенные первый и второй сумматоры, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого соединен с первым отрицательным входом первого сумматора, подключенного вторым положительным входом ко входу устройства, последовательно соединенные релейный блок и четвертый сумматор, второй вход которого соединен с выходом первого датчика скорости и входом релейного блока, последовательно соединенные первый задатчик сигнала и пятый сумматор, второй вход которого подключен к выходу датчика массы, а выход - к второму входу первого блока умножения, последовательно соединенные второй датчик скорости, второй блок умножения и третий блок умножения, второй вход которого соединен с выходом первого датчика скорости, а выход - с третьим входом четвертого сумматора, а также второй датчик положения, причем второй вход второго сумматора соединен с выходом первого датчика скорости, а выход четвертого сумматора подключен ко второму входу третьего сумматора, последовательно соединенные второй задатчик сигнала, шестой сумматор, четвертый блок умножения, второй вход которого через первый функциональный преобразователь соединен с выходом второго датчика положения, седьмой сумматор, второй вход которого соединен с выходом третьего задатчика сигнала, и пятый блок умножения, второй вход которого соединен с выходом первого датчика ускорения, а выход подключен к четвертому входу четвертого сумматора, последовательно соединенные второй функциональный преобразователь, вход которого соединен со входом первого функционального преобразователя, и шестой блок умножения, второй вход которого подключен к выходу шестого сумматора, а выход - ко второму входу второго блока умножения, пятый вход четвертого сумматора подключен к выходу седьмого блока умножения, первый вход которого соединен с выходом второго датчика скорости, а второй вход - с выходом второго блока умножения, третий вход пятого сумматора соединен с выходом четвертого блока умножения, третий вход седьмого сумматора подключен к выходу датчика массы и второму входу шестого сумматора (см. патент РФ 1816684, БИ 19, 1993 г.).

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

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

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

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

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

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

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

На фиг.1 дана блок-схема предлагаемого устройства для управления приводом робота; на фиг.2 - кинематическая схема исполнительного органа робота.

Устройство для управления приводом робота содержит последовательно соединенные первый 1 и второй 2 сумматоры, первый блок 3 умножения, третий сумматор 4, усилитель 5 и двигатель 6, связанный с первым датчиком 7 скорости непосредственно и через редуктор 8 с первым датчиком 9 положения, выход которого соединен с первым отрицательным входом первого сумматора 1, подключенного вторым положительным входом ко входу устройства, последовательно соединенные релейный блок 10 и четвертый сумматор 11, второй вход которого соединен с выходом первого датчика 7 скорости и входом релейного блока 10, последовательно соединенные первый задатчик 12 сигнала и пятый сумматор 13, второй вход которого подключен к выходу датчика 14 массы, а выход - к второму входу первого блока 3 умножения, последовательно соединенные второй датчик 15 скорости, второй блок 16 умножения и третий блок 17 умножения, второй вход которого соединен с выходом первого датчика 7 скорости, а выход - с третьим входом четвертого сумматора 11, а также второй датчик 18 положения, причем второй вход второго сумматора 2 соединен с выходом первого датчика 7 скорости, а выход четвертого сумматора 11 подключен ко второму входу третьего сумматора 4, последовательно соединенные второй задатчик 19 сигнала, шестой сумматор 20, четвертый блок 21 умножения, второй вход которого через первый функциональный преобразователь 22 соединен с выходом второго датчика 18 положения, седьмой сумматор 23, второй вход которого соединен с выходом третьего задатчика 24 сигнала, и пятый блок 25 умножения, второй вход которого соединен с выходом первого датчика 26 ускорения, а выход подключен к четвертому входу четвертого сумматора 11, последовательно соединенные второй функциональный преобразователь 27, вход которого соединен со входом первого функционального преобразователя 23, и шестой блок 28 умножения, второй вход которого подключен к выходу шестого сумматора 20, а выход - ко второму входу второго блока 16 умножения, пятый вход четвертого сумматора 11 подключен к выходу седьмого блока 29 умножения, первый вход которого соединен с выходом второго датчика 15 скорости, а второй вход - с выходом второго блока 16 умножения, третий вход пятого сумматора 13 соединен с выходом четвертого блока 21, третий вход седьмого сумматора 23 подключен к выходу датчика 14 массы и второму входу шестого сумматора 20, последовательно соединенные третий датчик 30 положения, восьмой сумматор 31, второй вход которого подключен к выходу второго датчика 18 положения, третий функциональный преобразователь 32, восьмой блок 33 умножения, девятый сумматор 34 и девятый блок 35 умножения, второй вход которого подключен к выходу второго датчика 36 ускорения, а его выход - к шестому входу четвертого сумматора 11, последовательно соединенные четвертый задатчик 37 сигнала, десятый сумматор 38, десятый блок 39 умножения, второй вход которого через четвертый функциональный преобразователь 40 подключен к выходу третьего датчика 30 положения, а его выход - ко второму входу девятого сумматора 34, последовательно соединенные пятый задатчик 41 сигнала и одиннадцатый сумматор 42, второй вход которого подключен к выходу датчика 14 массы и ко второму входу десятого сумматора 38, а его выход - ко второму входу восьмого блока 33 умножения.

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

_вх - сигнал желаемого положения;

q₁, q₂, q₃, q₄ - соответствующие обобщенные координаты исполнительного органа робота;

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

- ошибка привода (величина рассогласования);

m₁, m₂, m₃, m_г - соответственно массы первого, второго, третьего звеньев исполнительного органа и захваченного груза;

l*₂, l*₃ - расстояния от осей вращения горизонтальных звеньев до их центров масс;

l₂, l₃ - длины соответствующих горизонтальных звеньев;

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

U*, U - соответственно усиливаемый сигнал и сигнал управления двигателем 6.

Устройство работает следующим образом. Сигнал ошибки сумматора 1 после коррекции в блоках 4, 2, 3, усиливаясь, поступает на электродвигатель 6, приводя его вал во вращательное движение с направлением и скоростью (ускорением), зависящими от величины поступающего сигнала U, моментов трения и внешнего моментного воздействия M_в. Электропривод при работе с различными грузами, а также за счет взаимовлияния степеней подвижности исполнительного органа обладает переменными моментными характеристиками, которые могут меняться в широких пределах. Это снижает качественные показатели электропривода и даже приводит к потере устойчивости его работы. В результате возникает задача, связанная с обеспечением инвариантности динамических свойств электропривода к непрерывным и быстрым изменениям его моментных нагрузочных характеристик, что позволяет обеспечить стабильность заданного качества системы управления.

Рассматриваемый привод управляет обобщенной координатой q₂. Конструкция робота (фиг.2) является наиболее типовой для отечественных и зарубежных промышленных роботов (см., например, роботы типа skilam, SR-2, SR-3, SR-4, ТУР-2.5, гранат-2.5 и т.д.).

Эта конструкция позволяет осуществлять вертикальное прямолинейное перемещение груза (координата q₄), два вращательных движения в горизонтальной плоскости (координаты q₂ и q₃) и линейное перемещение в горизонтальной плоскости (координата q₁).

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

На основе уравнений Лагранжа II рода можно записать, что моментное воздействие на выходной вал привода, управляющего координатой q₂, с грузом имеет вид



где







С учетом соотношений (1) и (2), а также уравнений электрической и механической цепей электродвигателя постоянного тока с постоянными магнитами или независимого возбуждения рассматриваемый привод, управляющий координатой q₂, можно описать следующим дифференциальным уравнением:



где H* = H/i_p ²; h* = h/i_p ²; М*_вн = М_вн/i_р;

R - активное сопротивление якорной цепи двигателя;

J - момент инерции якоря двигателя и вращающихся частей редуктора, приведенных к валу двигателя;

k_м - коэффициент крутящего момента;

k - коэффициент противо-ЭДС;

k_в - коэффициент вязкого трения;

i_p - передаточное отношение редуктора;

М_стр - момент сухого трения;

К_у - коэффициент усиления усилителя 5;

i - ток якоря;

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

Из уравнения (3) видно, что его параметры, а следовательно, и параметры привода, управляющего координатой q₂, являются существенно переменными, зависящими от величин В результате в процессе работы привода меняются (притом существенно) его динамические свойства. Для реализации поставленной выше задачи необходимо сформировать такое корректирующее устройство, которое застабилизировало бы параметры привода таким образом, чтобы он описывался дифференциальным уравнением с постоянными параметрами.

Полагается, что первый положительный вход сумматора 2 (со стороны сумматора 1) единичный, а его второй отрицательный вход имеет коэффициент усиления k/k_у. Следовательно, на выходе сумматора 2 формируется сигнал

Первый положительный вход сумматора 20 единичный, а задатчик 19 сигнала подает на него сигнал l₂l*₃m₃. Второй положительный вход этого сумматора имеет коэффициент усиления l₂l₃. В результате на выходе этого сумматора формируется сигнал ₂(m₃l*₃ + m_гl₃).

Датчик 18 положения измеряет обобщенную координату q₃, a функциональный преобразователь 22 реализует функцию cos ₃. В результате на выходе блока 21 умножения формируется сигнал l₂(m₃l*₃ + m_гl₃)cosq₃.

Первый положительный вход сумматора 13 имеет единичный коэффициент усиления, а задатчик 12 сигнала подает на этот вход сигнал На второй его вход с коэффициентом усиления (l₂ ² + l₃ ² )/(i_p ²J_н) датчик 14 массы подает сигнал m_г. Третий вход этого сумматора 13 имеет коэффициент усиления 2/(i_p ²J_н). В результате на его выходе формируется сигнал



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

Функциональный преобразователь 27 реализует функциональную зависимость sinq₃. В результате на выходе блока 28 умножения формируется сигнал l₂(m₃l*₃ + m_гl₃)sinq₃.

Датчик 15 скорости измеряет скорость изменения обобщенной координаты q₃ и, как и датчик 18 положения, установлен в третьей степени подвижности робота. В результате на третий отрицательный вход сумматора 11 (со стороны блока 17 умножения) с коэффициентом усиления 2/i_p ² поступает сигнал а на пятый отрицательный вход этого сумматора (со стороны блока 29 умножения) с коэффициентом усиления 1/i_p - сигнал

Первый и второй положительные входы сумматора 23 (соответственно со стороны блока 21 умножения и задатчика 24 сигнала) имеют единичные коэффициенты усиления, а его третий положительный вход - коэффициент усиления l₃ ². Задатчик сигнала 24 формирует сигнал J₃+m₃l^*2₃, а датчик 26 ускорения измеряет ускорение изменения обобщенной координаты q₃ и установлен в третьей степени подвижности робота. В результате на выходе блока 25 умножения формируется сигнал который поступает на четвертый положительный вход сумматора 11, имеющий коэффициент усиления 1/i_p. Первый и второй положительные входы сумматора 11 (соответственно со стороны релейного элемента 10 и датчика 7 скорости) соответственно имеют единичный коэффициент усиления и коэффициент усиления, равный k_мk/R+k_в.

Первый положительный вход сумматора 38 имеет единичный коэффициент усиления и задатчик сигнала 37 подает на него сигнал (m₂l*₂ + m₃l₂)/i_p, а его второй положительный вход - коэффициент усиления l₂/i_p. В результате на выходе этого сумматора формируется сигнал [m₂l*₂ + (m₃ + m_г)l₂ ]/i_p. Функциональный преобразователь 40 реализует функцию sinq₂. В результате на выходе блока 39 умножения формируется сигнал [m₂l*₂ + (m₃ +m_г)l₂]sinq₂/i_p. Задатчик сигнала 41 подает на первый положительный вход сумматора 42 с единичным коэффициентом усиления сигнал m₃l*₃/i_p, eго второй положительный вход имеет коэффициент усиления, равный l₃/i_p. В результате на выходе этого сумматора формируется сигнал (m₃l*₃+m_гl₃)/i_р. Сумматор 31 имеет положительные входы с единичными коэффициентами усиления. В результате на его выходе формируется сигнал q₂+q₃. Функциональный преобразователь 32 реализует функцию sin(q₂+q₃). В результате на выходе блока 33 умножения формируется сигнал (m₃l*₃+m_гl₃)sin(q₂+q₃)/i_р.

Сумматор 34 имеет положительные входы с единичными коэффициентами усиления, поэтому на шестой положительный вход сумматора 11 с единичным коэффициентом усиления поступает сигнал



Выходной сигнал релейного элемента 10 с нулевой нейтральной точкой имеет вид



где |M_т| - величина момента сухого трения при движении. В результате на выходе сумматора 11 формируется сигнал



Первый положительный вход сумматора 4 (со стороны блока 3 умножения) имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления R/k_мk_y. В результате на выходе сумматора 4 формируется сигнал U*, равный



Поскольку при движении привода М_т sign достаточно точно соответствует М_стр, то сигнал U* (4), как несложно убедиться, обеспечивает превращение уравнения (3) с существенными переменными параметрами в уравнение с постоянными номинальными желаемыми параметрами, обеспечивающими приводу заданные динамические свойства и качественные показатели



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

Практическая реализация предлагаемого устройства не вызывает затруднений, так как в нем использованы только типовые электронные элементы.