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

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

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

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

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

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

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

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

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

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

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

Связь между координатами одной и той же точки M_i в системе координат нулевого звена (корпус манипулятора, земля) и последнего звена (рабочий орган, грузовая площадка) определяется зависимостью:

(1) где y_i1, y_i2, y_i3 - координаты точки M_i в системе координат нулевого звена;

x_i1, x_i2, x_i3 - координаты той же точки в системе координат последнего звена (рабочего органа);

А( ₁, ₂, . . . , ₆) - матрица преобразования 4 х 4 из одной системы в другую, зависящая от геометрических параметров манипулятора и координат его звеньев ₁, ₂, . . . , ₆.

Воспользовавшись зависимостью в векторно-матричной форме преобразование (1) имеет вид:

Y = A , (2) где А, Х, Y - матрица и векторы, определенные выше.

Формулу (2) можно использовать для описания преобразования координат не только одной точки, но и системы, например n точек. Для этого следует считать/ что X и Y являются матрицами из n столбиков/ в которых каждый столбец является выбором координат одной точки: (3) Сокращенная запись преобразования (3) соответствует формуле (2), в сокращенной форме (2) требуется, чтобы за счет изменения координат звеньев манипулятора _j (j = 1, 6) было осуществлено совмещение системы реперных точек рабочего органа с их требуемыми положениями:

A( ₁, ₂, . . . ₆) ^тр = А ₁^тр,

₂^тр, . . . , ₆^тр) (4) где _тр - матрица требуемых координат реперных точек в системе координат последнего звена при условии, что звенья манипулятора имеют координаты ₁, ₂, . . . , ₆;

- матрица координат реперных точек также в системе координат последнего звена.

Матрица преобразования А ( ₁, ₂, . . . , ₆) переводит в систему координат нулевого звена требуемые координаты, а матрица А ( ₁^т, ₂^т, . . . , ₆^т) координаты реперных точек. Равенство этих координат означает совпадение положения реперных точек с требуемыми.

Координаты реперных точек , их требуемые значения ^тр и погрешность позиционирования связаны между собой соотношением:

^тр = + . (5)

Координаты реперных точек на последнем звене манипулятора являются его конструктивными параметрами. Таким образом, измерив при каком-либо положении звеньев манипулятора ₁, ₂, . . . , ₆ погрешность позиционирования , по соотношению (5) можем определить требуемые координаты реперных точек ^тр, а с помощью соотношения (4) - требуемые для точного позиционирования координаты звеньев манипулятора ₁^тр, ₂^тр, . . . , ₆^тр. Определение погрешности позиционирования ², как разности требуемых и фактических координат реперных точек, и применение этого определения для управления манипулятором (формула 4 и последующие) являются оригинальными.

Соотношение (4) представляют собой матричное уравнение относительно неизвестной матрицы А ( ₁^тр, ₂^тр, . . . , ₆^тр). В этом уравнении шесть неизвестных ₁^тр, ₂^тр, . . . , ₆^тр, а количество скалярных уравнений равно количеству элементов в трех первых строках матриц - произведений А ^тр и А . Если в матрицах и _триспользуется по одному столбцу, то получим три скалярных уравнений с шестью неизвестными (неопределенная система). При двух столбцах в матрице получим шесть уравнений относительно шести неизвестных.

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

Система уравнений (4) для трех реперных точек определяет зависимость требуемых координат звеньев манипулятора _j^т от требуемых координат реперных точек:

_j^тр = f_j( ^тр). (6)

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

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

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

С помощью программного управления рабочий орган подводится к конечному положению возможно ближе, но с погрешностью, не меньшей погрешности позиционирования манипулятора относительно объекта (ПМО). Далее управление осуществляется по закону точного позиционирования (6) до достижения конечного положения. Приближение рабочего органа к конечному положению снимает оба ограничения, на применение закона управления (6) упомянутые выше. Во-первых, ограничивается диапазон действия датчиков, измеряющих погрешность позиционирования и, следовательно, упрощается проблема выбора датчиков. Во-вторых, ограничивается диапазон значений переменных, при которых необходимо решать систему уравнений (4).

На малом расстоянии от конечного положения рабочего органа решение (6) системы уравнений (4) допускает линеаризацию по своим аргументам:

^т_j= f_j()+ x_k, (7) где - матрица координат реперных точек рабочего органа при таком положении звеньев манипулятора ₁, ₂, . . . , ₆, которое обеспечивает достаточную малость их расстояния до заданных координат ^тр;

х_к - элемент матрицы х, одна из координат реперных точек;

- частные производные решений системы (4) по координатам реперных точек. Согласно (4):

f_j(X) = _j, тогда:

, или

^т_j^p= x_k . (8)

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

На чертеже представлена функциональная схема устройства для управления автоматическим транспортно-погрузочным манипулятором. Она содержит реверсивный счетчик 1, коммутатор 2, датчик 3 технологического оборудования, первый 4 и второй 5 блоки задания программы, первую 6 и вторую 7 группы триггеров, первую 8 и вторую 9 группы переключателей, таймер 10, первую 11 и вторую 12 группы приводов, первую 13 и вторую 14 группы датчиков положения, первый 15 и второй 16 элементы И, первую 17 и вторую 18 группы сумматоров и измеритель 19 погрешности позиционирования.

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

режим I программного позиционирования, соответствующий работе прототипа;

режим II, предлагаемый для решений задачи точного позиционирования.

Переключение с режима I на режим II выполняется сигналом таймера 10, который включается сигналом второго блока 5 задания программы на время, необходимое для выполнения точного позиционирования и необходимых технологических операций в этом режиме.

В режиме I переключатели первой 8 и второй 9 групп пропускают на приводы первой 11 и второй 12 групп сигналы управления с триггеров первой 6 и второй 7 групп, а на второй элемент И 16 таймер 10 выдает сигнал логической единицы. В режиме II сигнал таймера 10 переключает приводы первой 11 и второй 12 групп с первой 6 и второй 7 групп триггеров на первую 17 и вторую 18 группы сумматоров и блокирует сигналом логического нуля работу второго элемента И 16.

В режиме I обеспечивается отработка манипулятором программы, которую ему задают первый 4 и второй 5 блоки задания программы (в предлагаемом устройстве для управления количество приводов увеличено по сравнению с прототипом с 3 до 6, чтобы обеспечить управление ориентацией рабочего органа). Эта программа представляет собой ряд фиксированных наборов координат звеньев манипулятора.

Отработка этих фиксированных наборов осуществляется следующим образом. Все наборы фиксированных координат занумерованы: 0, 1, 2, . . . Первый 4 и второй 5 блоки задания программы выдают на приводы первой 13 и второй 14 групп тот набор координат, номер которого указывает реверсивный счетчик 1. В зависимости от состояния датчика 3 технологического оборудования и коммутатора 2 для реверсивного счетчика возможны два режима работы: суммирующий и вычитающий. Группы 6, 7, 13 и 14 и элементы 15 и 16 ограничивают скорость прохождения программы так, чтобы каждый набоp фиксированных координат успевал отрабатываться с необходимой точностью.

Для осуществления такого контроля при выдаче каждого очередного набора координат устанавливают триггеры первой 6 и второй 7 групп в единичное состояние тех приводов, координаты которых изменяются. Приводы отрабатывают новые координаты до тех пор, пока датчики положения первой 13 и второй 14 групп не зафиксируют, что все приводы пришли на заданные координаты. Сигналы с групп 13 и 14 датчиков положения устанавливают триггеры первой 6 и второй 7 групп в нулевое состояние и передаются на первый 15 и второй 16 элементы И. Элементы И 15 и 16 после окончания отработки всех заданных координат разрешает коммутатору 2 произвести изменение состояния реверсивного счетчика 1 (+1 или -1) и перейти к отработке очередного набора координат.

Режим I работы манипулятора для некоторых приложений является неприемлемым из-за наличия погрешности позиционирования манипулятора относительно объекта. Этот недостаток устраняется при работе в режиме II следующим образом. В точках программы, в которых необходимо обеспечить более точное позиционирование, второй блок 5 задания программы запускают таймер 10, который сигналом логического "0" переключает с помощью переключателей первой 8 и второй 9 групп все приводы первой 11 и второй 12 групп от системы грубого позиционирования на систему точного позиционирования, т. е. подключает к выходам коммутаторов первой 17 и второй 18 групп. Это позволяет реализовать формулу (8) для вычисления погрешностей координат звеньев.

Каждый из сумматоров вычисляет поправку координаты _j^тр для соответствующего звена манипулятора в соответствии с зависимостью (8). Каждый из приводов, на который поступает сигнал > _j ( - зона нечувствительности привода), отрабатывает ошибку положения соответствующего звена. В результате все соответствующие ошибки позиционирования х_к стремятся к нулю, а при условии:

< _j, j =

все составляющие ошибки позиционирования оказываются достаточно малыми. Их отработка прекращается по сигналу таймера 10, который сигналом логической "1" вновь возвращает устройство в режим программного управления (режим I).

Изобретение позволяет повысить точность позиционирования рабочего органа манипулятора относительно объекта, на который доставляется груз. Моделирование на ЭЦВМ показывает, что погрешность позиционирования автоматического транспортно-погрузочного манипулятора для некоторых случаев применения может быть уменьшена в 5 . . . 10 раз (от 50 до 5 мм). Это, в свою очередь, приводит к повышению производительности, надежности и безопасности работы манипулятора. (56) Авторское свидетельство СССР N 1442392, кл. B 25 J 9/00, 1987.