Устройство для управления приводом робота Советский патент 1992 года по МПК B25J13/00 

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

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

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

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

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

VJ 00

ю XI ho

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

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

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

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

5 с выходом второго сумматора, а выход - с первым входом первого блока умножения, подключенного вторым входом к выходу пятого сумматора, первый вход пятого блока умножения соединен с выходом четвертого

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

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

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

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

Целью изобретения является устранение указанного выше недостатка, то есть

обеспечение высокой точности привода другой степени подвижности другого робота.

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

На фиг.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 функционального

преобразователя, а их выходы - соответственно ко второму входу шестого сумматора 1.8 и первому входу четвертого блока 21 умножения, соединенного вторым входом через квадратор 17с выходом второго датчика

14 скорости, а выходом - с третьим входом четвертого сумматора 11, четвертый вход которого подключен к выходу пятого блока 22 умножения, соединенного первым входом с выходом датчика 23 ускорения, а вторым входом - с выходом шестого сумматора 18, третий вход которого подключен к выходу второго задатчика 16 сигнала, а выход второго сумматора 2 соединен с третьим входом третьего сумматора 4. Объект управления 26 соединен с выходным валом редуктора 8.

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

Овх - сигнал желаемого положения; qi.Q2,Q3 - соответствующие обобщенные координаты исполнительного органа робота;

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

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

т1,гт)2,тз,плг - соответственно массы первого, второго, третьего звеньев исполнительного органа и захваченного груза;

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

12,1з-длины соответствующих горизонтальных звеньев;

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

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

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

Рассматриваемый привод управляет обобщенной координатой дз. Конструкция робота является наиболее типовой для отечественных и зарубежных промышленных роботов (см.,например, роботы типа skilam (SR-2,SR-3,SR-4,TyP, гранат-2,5 и т.д.). Эта конструкция позволяет осуществлять вертикальное прямолинейное перемещение груза (координата qi и два вращательных в горизонтальной плоскости (координаты qa и

Q3).

Моментные характеристики привода, управляющего координатой рз существенно зависят от изменения координат дз,р2.С|2 и mr. В связи с этим для качественного управ- ления координатой qs необходимо точно компенсировать отрицательное влияние изменения координат qs,q2 и , а также переменной массы груза mr на динамические свойства рассматриваемого привода пово- рота (координата qs).

Для определенных моментных воздействий на рассматриваемый привод (обобщенных моментов неконсервативных сил)

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

+ т2122)д3 ,

Оз+тз -ЬтггёК +д з)2

+

(тэ+тг)6& 4 п вЬяфй Ы (mi-f ГП2+тз+mr)q

2

где 12, b - моменты инерции соответствующих горизонтальных звеньев относительно их центров масс.

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

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

л Т

(1з + + пМз2} (Q2 + о.з) + 12( + 0q3

+ пМз) cos (qs) qa,

(|Т ,. ((3 +

тИз ) Q3 d+t 4dqa

-12(тз1 з) sin (qs)q2q3 + + + +12тзГ + ) cos qs q2

f

- 12( + ) sin (qsXqa2 + Э2-дз),

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

MB (1з + + ппг1з2) qs + 12( + +пМз) sin (q3)q22+(з + 2 + + h (m2 з+ + ) cos дз 92(1)

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

U iR + Kw «з и механической

+{Ь + ) /)2р аз + Кваз +

+Мсгр + + + tefmai з + ) qsjq 2 /ip + 12( + mrlajsinqsq 2 /Ip.

cos

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

KyKMU + (13 + + тг1з2) + ККтКш + РКв)«з + RMCTp + R 1з + + + 12( + тг1з) cosqs q 2 ip+ Rl2( + ) sin q3)q22/ip(2)

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

I - момент инерции якоря двигателя и Первый положительный вход сумматора вращающихся частей редуктора, приведен-13 (со стороны датчика 15) имеет коэффици- ных к валу двигателя; 5ент усиления Ыз/lp, а его второй положи- Км - коэффициент крутящего момента;тельный вход (со стороны задатчика 12) - К - коэффициент противо-ЭДС;единичный коэффициент усиления. Сигнал с Кв - коэффициент вязкого трения;выхода задатчика сигнала 16 - (з + )/1р. IP - передаточное отношение редукто-Первый (со стороны блока 19 умноже- ра; 10ния) и третий (со стороны задатчика 16 сиг- Метр - момент сухого трения;нала) положительные входы сумматора 18 Ку - коэффициент усиления усилителя 5;имеют единичные коэффициенты усиления. I - ток якоря;а второй положительный вход (со стороны аз - ускорение вращения вала двигате-датчика 15) - коэффициент усиления 1з Лр. ля третьей степени подвижности. . 15 Таким образом, на выходе сумматора 13 Из (2) видно, что параметры этого урав-формируется сигнал 12( + тг1з)/1р. По- нения, а следовательно, и параметры приво-скольку функциональный преобразователь да, управляющего координатой qs,24 формирует сигнал cos цз, то на выходе являются существенно переменными, зави-блока+умножения 19 появляется сигнал сящими от величин mr,q3.q2,q2. В результате 20h (тз з + пМз) cos (чз)/1р. а на выходе сумма- в процессе работы привода меняютсятора 18-сигнал 1з+ msks 2+гпгЬ2-Н2тз1 з + (притом существенно) его динамические -ИтЫз) cos (qsjl/lp.

свойства. В результате для реализации JДатчик 23 ускорения измеряет ускорепоставленной выше задачи необходимоние вращения второй степени подвижности

сформировать такое корректирующее уст- 25робота (координату 92), поэтому на выходе

ройство, которое застабилизировало бы па-блока 22 умножения формируется сигнал

раметры привода таким образом, чтобы + -НгИз2+ $т$ з + mrtejxcos qsjcfe/ip. описывался дифференциальным уравнени-Датчик 14 скорости измеряет скорость

ем с постоянными желаемыми параметра-вращения во второй степени подвижности

ми.30(координату Q2), а функциональный преобПолагается, что первый положительныйразователь 25 формирует сигнал sin qs. Повход сумматора 2 (со стороны сумматора 1)этому на выходе блока 20 умножения

единичный, а его второй отрицательныйпоявляется сигнал te ( + тИз) sin (qs) /Ip,

вход имеет коэффициент усиления К# /Ку.а на выходе блока 21 умножения - сигнал

Первый, третий и четвертый положительные ( + пМз) sin (qs) cj22/fp. входы сумматора 11 (соответственно со сто-С учетом отмеченных выше коэффицироны релейного элемента 10, блока 21 умно-ентов усиления соответствующих входов

жения и блока 22 умножения) единичные,сумматора 11 на его выходе формируется

второй его положительный вход (со сторонысигнал

датчика 7 скорости) имеет коэффициент уем- 40 ( + Кв) аз + Мт sign 03 +J2 ( +

ления ( + Кв). Причем выходной сиг--НтЫз) jsln (qa) q22/ip + з + +

нал релейного элемента 10 с нулевой+12(тз з + ) cosqs cJ2 /Ip нейтральной точкой имеет видНа выходе сумматора 2 формируется

ивыхю при § 0- 45сигнал е -КГ а На ВЫХ°яе блока 3

, Коз ч

где |МТ| -ветчина момента сухого тренияУмножения - сигнал mr ( -од).

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

ра 4 (со стороны блока умножения 3) имеет 50входов сумматора 4 на его выходе окончакоэффициентусиления з /(Ыр ), второй по-тельно будет сформирован сигнал U вида ложительный (со стороны сумматора 11)-, , ,. . ,2 v,a . m ,2/л

коэффициент усиления Р/КмКу),атретийпо-U - U3f тз з ;/IP ч-тг i3/IP x ложительный (со стороны сумматора 2) ,-iL-

коэффициент усиления р + 0з + )/ 55х( Б -- оз )+Утг- г- ( + Кв) аз +

где Ун - номинальное (желаемое) значение+Мт,п 2h ( Шг з) (дз) {- Р t f 3 +

приведенного момента инерции, обеспечи-+тз1, +тг 3 + 3 + mrl3)cosq3 2/ h.

вающее рассматриваемому приводу роботам Несложно показать, что поскольку

MTsign az при движении привода достаточно точно соответствует Метр, то подставив полученное значение U(3) в соотношение (2). получим уравнение

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

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

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

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

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

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

(рЈ/г1

Фиг. Z