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

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

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

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

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

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

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

Известно также устройство для управления приводом робота, содержащее последовательно соединенные первый и второй сумматоры, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор - с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен к входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, а выход - ко второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, квадратор, шестой сумматор и со второго по пятый блоки умножения, датчик ускорения, а также первый косинусный и второй синусный функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен к второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходам первого и второго функциональных преобразователей, а их выходы - соответственно к второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через квадратор с выходом второго датчика скорости, а выходом - с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом датчика ускорения, а вторым входом - с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход второго сумматора соединен с третьим входом третьего сумматора, последовательно соединенные второй датчик положения, седьмой сумматор, второй вход которого подключен к выходу первого датчика положения, третий синусный функциональный преобразователь и шестой блок умножения, второй вход которого подключен к выходу пятого сумматора, а выход - к пятому входу четвертого сумматора (авт. св. №2028930, кл. В 25 J 13/00, 1995).

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

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

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

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

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

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

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

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

q_ВХ - сигнал желаемого положения;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

i - ток якоря.

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

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

,

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

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

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

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

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

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

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

Датчик 33 установлен в первой степени подвижности и измеряет ускорение . В результате на входе блока 32 формируется сигнал .

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

На выходе сумматора 2 формируется сигнал , а на выходе блока 3 – сигнал m_Г .

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

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

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