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

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

УСТРОЙСТВО ДЛЯ УПРАВЛЕНИЯ ПРИВОДОМ РОБОТА, содержащее последовательно соединенные первый и второй сумматоры, первый блок умножения, третий сумматор, усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент и четвертый сумматор, второй вход которого подключен к входу релейного элемента, второму входу второго сумматора и выходу первого датчика скорости, выход - к второму входу третьего сумматора, последовательно соединенные первый задатчик сигнала и пятый сумматор, а также второй датчик скорости, датчик массы, второй задатчик сигнала, квадратор, шестой сумматор и с второго по пятый блоки умножения, датчик ускорения, а также первый и второй функциональные преобразователи, вход каждого из которых соединен с выходом первого датчика положения, выход датчика массы подключен к второму входу первого блока умножения, первому входу шестого сумматора и второму входу пятого сумматора, соединенного выходом с первыми входами второго и третьего блоков умножения, второй вход каждого из которых подключен соответственно к выходам первого и второго функционального преобразователя, а их выходы - соответственно к второму входу шестого сумматора и первому входу четвертого блока умножения, соединенного вторым входом через квадратор с выходом второго датчика скорости, а выходом - с третьим входом четвертого сумматора, четвертый вход которого подключен к выходу пятого блока умножения, соединенного первым входом с выходом датчика ускорения, а вторым входом - с выходом шестого сумматора, третий вход которого подключен к выходу второго задатчика сигнала, а выход второго сумматора соединен с третьим входом третьего сумматора, отличающееся тем, что в него дополнительно введены последовательно соединенные второй датчик положения, седьмой сумматор, второй вход которого подключен к выходу первого датчика положения, третий функциональный преобразователь и шестой блок умножения, второй вход которого подключен к выходу пятого сумматора, а выход - к пятому входу четвертого сумматора.

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

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

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

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

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

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

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

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

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

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

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

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

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

На фиг. 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.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

T = + +

+ (+)² +

+ (m₃l^*₃+m_гl₃)l₂cos(q₃)(+) где J₂, J₃ - соответственно моменты инерции второго и третьего звеньев относительно их центров масс.

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

n=m₂gl₂*(1-cosq₂) + m₃g(l₂(1 - cosq₂) + + l₃*(1-cos(q₂+q₃))) + m_гg(l₂(1 - cosq₂) + + l₃(1-cos(q₂+q₃))), где g - ускорение свободного падения.

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

= (J₃+m₃l^*₃²+m_гl²₃)(+)+

+l₂(m₃l^*₃+m_гl₃)cos(q₃)

= (J+m₃l^*₃²+m_гl²₃)+

[J₃+m₃l^*₃²+m_гl²₃+l₂(m₃l^*₃+m_гl₃)cosq₃]-

-l₂(m₃l^*₃+m_гl₃)sin(q₃)q₃

= -l₂(m₃l^*₃+m_гl₃)sin(q₃)(+)

= 0, = (m₃l^*₃+m_гl₃)sin(q₂+q₃)g

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

M_В=(J₃+m₃l^*₃²+m_гl²₃)+[J₃+m₃l^*₃²+m_гl²₃+

+ l₂(m₃l^*₃+m_гl₃)cosq₃]+l₂(m₃l^*₃+m_гl₃)sin(q₃)+

+ +g(m₃l^*₃+m_гl₃)sin(q₂+q₃) (1)

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

U=iR+K и механической

iK_М[J+(J₃+m₃l^*₃²+m_гl²₃)/[i²_р]+

+ K+M_стр+[J₃+m₃l^*₃²+m_гl²₃+l₂(m₃l^*₃+m_гl₃)cosq₃]/i_р+

+ l₂(m₃l^*₃+m_гl₃)sin(q₃)q²₂/i_p+g(m₃l^*₃+m_гl₃)sin(q₂+q₃)/i_р Цепей электродвигателя постоянного тока с постоянными магнитами или независимого возбуждения, рассматриваемый привод, управляющий координатой q₃, можно описать следующим дифференциальным уравнением

R[J+(J₃+m₃l^*₃²+m_гl²₃)/i²_р]+(K_МK+RK_В)+

+RM_стр+R[J₃+m₃l^*₃²+m_гl²₃+l₂(m₃l^*₃+m_гl₃)cosq₃]/i_р+

+ Rl₂(m₃l^*₃+m_гl₃)sin(q₃)/i_р+R_д(m₃l^*₃+m_гl₃)sin(q₂+q₃)/i_р= =K_уK_МU^* (2) где R - активное сопротивление якорной цепи двигателя;

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

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

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

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

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

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

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

i - ток якоря;

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

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

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

U_вых.10= при , где M - величина момента сухого трения при движении.

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

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

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

сигнал cosq₃, то на выходе блока умножения 19 появляется сигнал l₂(m₃l₃*+m_гl₃)cos(q)₃/i_р, а на выходе сумматора 18 - сигнал

[J₃+m₃l₃*²+m_гl₃²+l₂(m₃l₃*+m_гl₃)cosq₃]/i_р.

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

[J₃+m₃l^*₃²+m_гl²₃+l₂(m₃l^*₃+m_гl₃)cosq₃]/i_р

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

l₂(m₃l^*₃+m_гl₃)sin(q₃)/i_р

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

В результате на выходе блока 29 умножения формируется сигнал

l₂(m₃l₃*+m_гl₃)sin(q₂+q₃)/i_р.

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

(K_МK/R+K_В)+Msign+l₂(m₃l^*₃+m_гl₃)sin(q₃)/i_р+ + g(m₃l^*₃+m_гl₃)sin(q₂+q₃)/i_р+[J₃+m₃l^*₃²+m_гl²₃+l₂(m₃l^*₃+m_гl₃)cosq₃]

/i_р

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

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

U^* = - +

+ [(K_МK/R+K_В)+Msign+

+l₂(m₃l^*₃+m_гl₃)sin(g₃)/i_р+

+[J₃+m₃l^*₃²+m_гl²₃+l₂(m₃l^*₃+m_гl₃)cosq₃]/i_р+

+q(m₃l^*₃+m_гl₃)sin(q₂+q₃)/i_р] (3)

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

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

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