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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Устройство для управления приводом робота содержит последовательно соединенные первый сумматор 1, второй сумматор 2, первый блок 3 умножения, третий сумматор 4, усилитель 5 и двигатель 6, связанный с первым датчиком 7 скорости непосредственно и через редуктор 8 с датчиком 9 положения, выход которого подключен к первому входу первого сумматора 1, соединенного вторым входом с входом устройства, последовательно подключенные релейный элемент 10 и четвертый сумматор 11, второй вход которого подключен к входу релейного элемента 11, второму входу второго сумматора 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 ускорения, установленный на выходном валу двигателя 6 и выходом подключенный к четвертому входу третьего сумматора 4, последовательно соединенные седьмой сумматор 27, первый вход которого подключен к выходу второго датчика 14 скорости, шестой блок 28 умножения, второй вход которого подключен к выходу первого датчика 23 ускорения, седьмой блок 29 умножения, второй вход которого соединен с выходом второго функционального преобразователя 25, восьмой сумматор 30, восьмой блок 31 умножения, второй вход которого подключен к выходу пятого сумматора 13, и девятый сумматор 32, второй вход которого через последовательно соединенные дифференциатор 33 и девятый блок 34 умножения подключен к выходу первого датчика 23 ускорения, а его выход к пятому входу третьего сумматора 4, последовательно соединенные десятый 35 и одиннадцатый 36 блоки умножения, причем второй вход девятого блока 34 умножения подключен к выходу шестого сумматора 18, первый и второй входы десятого блока 35 умножения соответственно к выходам квадратора 17 и первого функционального преобразователя 24, а второй вход одиннадцатого блока 36 умножения к выходу первого датчика 7 скорости и второму входу седьмого сумматора 27. Объект управления соединен с выходным валом редуктора 8.

На фиг.1 и 2 введены следующие обозначения:

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

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

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

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

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

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

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

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

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

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

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

Рассматриваемый привод управляет обобщенной координатой q₃. Конструкция робота (фиг.2) является наиболее типовой для отечественных и зарубежных промышленных роботов, например, роботы типа Skilam (SR-2,SR-3, SR-4), ТУР, Гранат-2,5 и т.д.

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

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

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

+ где I₂, I₃ моменты инерции соответствующих горизонтальных звеньев относительно их центров масс.

Потенциальная энергия является функцией только координаты q₁.

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

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

l)sin(q₃)+

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

q₃)+ (1)

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

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

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

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

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

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

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

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

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

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

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

i ток якоря;

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

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

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

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

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

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

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

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

+ m_гl₃) cos(q₃)]/i_p.

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

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

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

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

q₃)/i_p+

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

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

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

[I₃+m₃l^*₃²+m_гl²₃+l₂(m₃l^*₃+m_гl₃)cos(q₃)]/i_p

Первый положительный вход седьмого сумматора 27 (со стороны датчика 14) имеет коэффициент усиления 2, а его второй отрицательный вход коэффициент усиления 1/i_p. В результате на выходе седьмого блока 29 умножения формируется сигнал sin(q₃)(2-).

На выходе одиннадцатого блока 36 умножения формируется сигнал cos(q₃). Первый (со стороны блока 29) и второй положительные входы восьмого сумматора 30 имеют соответственно единичный коэффициент усиления и коэффициент усиления, равный 1/i_p. В результате на выходе восьмого блока 31 умножения формируется сигнал

_.

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

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

+ (3)

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

LI+RI+K_мK K_уK_м

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