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

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

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

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

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

Известно устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен к входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, выход к второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, квадратор, шестой сумматор и с второго по пятый блоки умножения, датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого их который соединен с выходом первого датчика положения, выход датчика массы подключен к второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходу первого и второго функционального преобразователя, а их выходы соответственно к второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через квадратор с выходом второго датчика скорости, а выходом с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом датчика ускорения, а вторым входом с выходом шестого сумматора, третий выход которого подключен к выходу второго задатчика сигнала, выход второго сумматора соединен с третьим входом третьего сумматора (авт. св. N 1782721, Б.И. N 47, 1992 г.).

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

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

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

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

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

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

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

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

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

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

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

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

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

l^*₂,l^*₃ расстояния от осей вращения соответствующих звеньев до их центров масс;

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

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

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

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

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

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

Для определения моментных воздействий на рассматриваемый привод (обобщенных моментов неконсервативных сил) воспользуемся уравнением Лагранжа 2-го рода. Кинетическая энергия Т всех движущихся масс исполнительного органа (фиг. 2) представляется в виде



где I_Si, I_Ni (i 1, 3) соответственно моменты инерций относительно продольной и поперечной осей, проходящих через центр масс звена i.

Потенциальная энергия робота имеет вид



где g ускорение свободного падения.

Учитывая, что





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



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



и механической



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



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

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

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

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

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

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

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

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

i ток якоря;

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

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

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



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

Первый положительный вход сумматора 4 (со стороны блока 3 умножения) имеет коэффициент усиления l²₃/(I_нi²_p), второй положительный (со стороны сумматора 11) коэффициент усиления R/(K_м K_у), а третий положительный (со стороны сумматора 2) коэффициент усиления [I+(I_N3+m₃l^*₃²)/i²_p]J_н, где I_н - номинальное (желаемое) значение приведенного момента инерции, обеспечивающее рассматриваемому приводу робота заданные динамические свойства и показатели качества.

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

Таким образом, на выходе сумматора 13 формируется сигнал l₂(m₃l^*₃+m_гl₃)/i_p. Поскольку функциональный преобразователь 24 формирует сигнал cos q₃, то на выходе блока 19 умножения появляется сигнал l₂(m₃l^*₃+m_гl₃)cos(q₃)/i_p, а на выходе сумматора 18 сигнал

[I_N3+m₃l^*₃²+m_гl²₃+l₂(m₃l^*₃+m_гl₃)cosq₃]/i_p.

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



Датчик 14 скорости измеряет скорость вращения во второй степени подвижности (координату ), а функциональный преобразователь 25 формирует сигнал sin q₃. Поэтому на выходе блока 20 умножения появляется сигнал l₂(m₃l^*₃+m_гl₃)sinq₃/i_p, а на выходе блока 21 умножения сигнал .

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

l₂(m₃l^*₃+m_гl₃)sin(q₂+q₃)/i_p.

Третий датчик 38 скорости измеряет скорость вращения первой степени подвижности робота (координата ). С выхода третьего задатчика 40 постоянного сигнала на первый положительный единичный вход девятого сумматора 41 поступает сигнал (-I_S3/i_р). Второй (со стороны задатчика 16) положительный вход этого сумматора единичный, а третий положительный (со стороны датчика 15 массы) имеет коэффициент усиления l²₃/i_p. В результате на выходе девятого сумматора 41 формируется сигнал

(I_N3-I_S3+m₃l^*₃²+m_гl²₃)/i_p.

Второй усилитель 30 имеет коэффициент усиления, равный 2. Четвертый функциональный преобразователь 31 реализует функцию sin. В результате на выходе седьмого блока 32 умножения формируется сигнал

(I_N3-I_S3+m₃l^*₃²+m_гl²₃)sin2(q₂+q₃)/i_p.

Пятый функциональный преобразователь 35 реализует функцию sin, а шестой функциональный преобразователь 42 функцию cos. В результате на выходе десятого блока умножения 37 формируется сигнал

l₂(m₃l^*₃+m_гl₃)sin(q₂)cos(q₂+q₃)/i_p,

а на выходе восьмого сумматора 33, первый положительный вход которого (со стороны блока 32 умножения) единичный, а второй положительный имеет коэффициент усиления 2, формируется сигнал



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



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

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



Несложно показать, что поскольку при движении привода достаточно точно соответствует М_стр, то, подставив полученное значение U^* (3) в соотношение (2), получим уравнение



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