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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

_вх - сигнал желаемого значения координаты q₁ ;

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

q_i - соответствующие обобщенные координаты исполнительного органа робота

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

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

=const - расстояние от оси вращения горизонтального звена до его центра масс;

=const - расстояние от оси вращения горизонтального звена до груза;

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

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

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

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

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

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

Моментное воздействие на выходной вал привода поворота со стороны движущихся масс исполнительного органа робота и груза, определенное с помощью уравнения Лагранжа 2 рода, имеет вид

J_S и J_N - соответственно моменты инерции вертикального звена исполнительного органа относительно продольной оси и горизонтального звена относительно поперечной оси, проходящей через его центр масс.

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

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

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

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

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

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

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

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

k_У - коэффициент усиления усилителя 3;

j - ток якоря;

Очевидно, что изменение H, M и , вызванное изменением и m_г, существенно влияет на динамические свойства, а следовательно, и качественные показатели рассматриваемого привода.

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

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

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

Первый положительный вход сумматора 9 (со стороны сумматора 8) имеет единичный коэффициент усиления, а его второй отрицательный вход - коэффициент усиления k/k _У, первый (со стороны блока 12) и третий (со стороны релейного элемента 16) положительные входы сумматора 13 имеют единичные коэффициенты усиления, а его второй положительный вход - коэффициент усиления {k_Mk/R+k _B).

Первый положительный вход сумматора 2 (со стороны блока 1) имеет единичный коэффициент усиления, а его второй положительный - коэффициент усиления R/(k_Mk _У].

Первый (со стороны датчика 14) и второй положительные входы сумматора 15 имеют коэффициенты усиления соответственно, где J_H - номинальное (желаемое) значение приведенного момента инерции, обеспечивающее приводу поворота заданные динамические свойства и качественные показатели. Причем на выходе задатчика 10 формируется сигнал, равный J_S +J_N+Ji_p ²+m₁l ².

Первый (со стороны источника 10) и второй положительные входы сумматора 11 имеют коэффициенты усиления и l_г/i_p соответственно.

Таким образом, на выходе сумматора 11 формируется сигнал, равный (m₁ l+m_гl_г)/i_p, на выходе сумматора 15 - сигнал а на выходе блока 1 умножения - сигнал

Датчики 19 и 23 соответственно установлены во второй и третьей степенях подвижности манипулятора и измеряют ускорения и соответственно. В результате на выходе сумматора 20, имеющего первый отрицательный (со стороны блока 18) и второй положительный входы с единичными коэффициентами усиления, формируется сигнал . Четвертый положительный вход сумматора 13 имеет коэффициент усиления k_BL/R и на его вход с датчика 24 поступает сигнал ускорения вращения вала двигателя . Первый положительный (со стороны дифференциатора 25) вход сумматора 26 имеет единичный коэффициент усиления, а его второй отрицательный вход - коэффициент усиления 1/i_p. В результате на выходе блока 28 умножения формируется сигнал . Первый положительный (со стороны дифференциатора 31) вход сумматора 30 имеет единичный коэффициент усиления, а его второй отрицательный вход - коэффициент усиления 1/i_p. В результате на выходе блока 32 умножения формируется сигнал .

Первый отрицательный (со стороны блока 32) и третий положительный (со стороны блока 28) входы сумматора 33 имеют коэффициенты усиления L/R, а его второй положительный вход - единичный коэффициент усиления. В результате на выходе сумматора 33 формируется сигнал а на выходе сумматора 13 - сигнал

Окончательно на выходе сумматора 2 имеем

Подставив U из (4) в (3), имеем

Очевидно, что уравнение (5) имеет постоянные желаемые параметры, а сам рассматриваемый привод обладает постоянными желаемыми динамическими свойствами и качественными показателями.