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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

- скорость изменения третьей обобщенной координаты;

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

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

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

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

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

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

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

Устройство работает следующим образом.

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

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

Движение робота по координате q₁ перпендикулярно плоскости вращения звеньев 2 и 3.

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

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

где

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

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

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

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

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

Датчик 18 измеряет обобщенную координату q₃ робота, а функциональный преобразователь 19 реализует функцию cosq₃. В результате на выходе блока 20 умножения формируется сигнал .

Второй положительный вход сумматора 16 имеет единичный коэффициент усиления, а задатчик 17 подает на него сигнал где J_H - суммарное номинальное (желаемое) значение момента инерции, приведенного к выходному валу электродвигателя. Его третий положительный вход имеет коэффициент усиления , а на первый положительный, имеющий коэффициент усиления , датчик 15 массы подает сигнал m_г. В результате на выходе сумматора 16 формируется сигнал

а на выходе блока 3 - сигнал . Функциональный преобразователь 28 реализует функциональную зависимость sinq₃. В результате на выходе блока 27 формируется сигнал .

Датчик 10 измеряет скорость изменения обобщенной координаты ₃ и как и датчик 18 установлен в третьей степени подвижности робота. В результате на первый отрицательный вход сумматора 13 (со стороны блока 12) с коэффициентом усиления поступает сигнал , а на пятый отрицательный вход этого сумматора (со стороны блока 29) с коэффициентом усиления - сигнал .

Первый и второй положительные входы сумматора 21 (соответственно со стороны блока 20 и задатчика 22) имеют единичные коэффициенты усиления, а его третий положительный вход - коэффициент усиления . Задатчик 22 формирует сигнал , а датчик 24 измеряет ускорение обобщенной координаты q₃ и установлен в третьей степени подвижности робота. В результате на выходе блока 23 формируется сигнал , который поступает на четвертый положительный вход сумматора 13, имеющий коэффициент усиления . Третий и второй положительные входы сумматора 13 (соответственно со стороны релейного элемента 14 и датчика 7 скорости) соответственно имеют единичный коэффициент усиления и коэффициент усиления, равный K_MK/R+K_B. Выходной сигнал релейного элемента 14 с нулевой нейтральной точкой имеет вид

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

Первый и второй положительные входы сумматора 34 имеют единичные коэффициенты усиления, а функциональный преобразователь 35 реализует функциональную зависимость sin(q₂+q₃). В результате на выходе блока 36 формируется сигнал , который поступает на седьмой положительный вход сумматора 13 с коэффициентом усиления .

Задатчик 30 вырабатывает сигнал и подает его на первый положительный вход сумматора 31, имеющий единичный коэффициент усиления, второй положительный вход этого сумматора имеет коэффициент усиления l₂. Функциональный преобразователь 33 реализует функциональную зависимость sinq₂. В результате на выходе блока 32 формируется сигнал , который поступает на шестой положительный вход сумматора 13, имеющий коэффициент усиления .

Датчик 41 установлен в первую степень подвижности робота и измеряет ускорение . На выходе блоков умножения 38 и 43, соответственно, формируются сигналы и . Первый положительный вход сумматора 39 (со стороны блока 38) имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления, равный . В результате на выходе блока 40 формируется сигнал

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

Исходя из отмеченного, на выходе сумматора 13 формируется сигнал

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

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

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