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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Электропривод робота содержит последовательно соединенные первый сумматор 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 положения, седьмой сумматор 27, второй вход которого подключен к выходу первого датчика 9 положения, третий синусный функциональный преобразователь 28, шестой блок 29 умножения, второй вход которого подключен к выходу пятого сумматора 13, и седьмой блок 30 умножения, выход которого подключен к пятому входу четвертого сумматора 11, второй датчик 31 ускорения, последовательно соединенные четвертый косинусный функциональный преобразователь 32, подключенный вводом к выходу седьмого сумматора 27, восьмой блок 33 умножения, второй вход которого соединен с выходом пятого сумматора 13, и девятый блок 34 умножения, второй вход которого соединен с выходом третьего датчика 35 ускорения, а его выход - с шестым входом четвертого сумматора 11, последовательно соединенные третий задатчик 36 сигнала и восьмой сумматор 37, второй вход которого подключен к выходу второго датчика 31 ускорения, а выход - к второму входу седьмого блока 30 умножения, объект управления 38.

На чертежах приведены следующие обозначения: _вх - сигнал желаемого положения третьей степени подвижности робота; q₁, q ₂, q₃, q₄, q₅ - соответствующие обобщенные координаты исполнительного органа робота; - скорость изменения второй обобщенной координаты; - ускорения соответствующих обобщенных координат; = _вх-q₃ - ошибка привода (величина рассогласования); m₁, m₂, m₃, m _г - соответственно массы первого, второго, третьего звеньев исполнительного органа робота и захваченного груза; - расстояния от осей вращения соответствующих звеньев до их центров масс; l₂, l ₃ - длины соответствующих звеньев; - скорость вращения ротора двигателя; U*, U - соответственно усиливаемый сигнал и сигнал управления двигателем 5.

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

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

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

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

где J₂, J ₃ - соответственно моменты инерции второго и третьего звеньев относительно их центров масс, g - ускорение свободного падения.

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

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

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

Из (2) видно, что параметры этого уравнения, а следовательно, и параметры привода, управляющего координатой q₃, являются существенно переменными, зависящими от величин q₂, q ₃, , , , , m_г.

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

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

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

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

Второй (со стороны блока 19) и третий (со стороны задатчика 16) положительные входы сумматора 18 имеют единичные коэффициенты усиления, а первый положительный вход (со стороны датчика 15) -коэффициент усиления

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

Датчик 23 измеряет ускорение вращения второй степени подвижности робота (координату ), поэтому на выходе блока 22 формируется сигнал

Датчик 14 измеряет скорость вращения второй степени подвижности (координату а функциональный преобразователь 25 формирует сигнал sin q₃. Поэтому на выходе блока 20 появляется сигнал а на выходе блока 21 - сигнал

Датчик 26 измеряет угол поворота во второй степени подвижности (координату q₂). Сумматор 27 имеет положительные входы с единичными коэффициентами усиления, а функциональные преобразователи 28 и 29 соответственно формируют сигналы sin(q₂+q₃ ) и cos(q₂+q₃). Датчики 31 и 35 соответственно установлены и измеряют ускорения движения в первой (координата и четвертой (координата степенях подвижности робота.

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

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

на выходе сумматора 2 - сигнал а на выходе блока 3 - сигнал

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

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

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