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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

На фиг. 1 представлена блок-схема предлагаемого устройства управления; на фиг. 2 кинематическая схема манипулятора (эта схема соответствует кинематической схеме типового манипулятора, работающего в цилиндрической системе координат).

На чертежах введены следующие обозначения: q_i обобщенная координата степени подвижности i манипулятора, это управляющие координаты соответствующих приводов манипулятора i=; r вектор, задающий положение характерной точки схвата в трехмерном пространстве, определяемом неподвижной системой координат xyz; r_x, r_y, r_z координаты вектора r (характерной точки схвата манипулятора) в трехмерном пространстве; R вектор, определяющий положение шарнира (точка О) пятой степени подвижности (координата q₅) в трехмерном пространстве xyz R_x, R_y, R_z координаты вектора R в трехмерном пространстве xyz; L₁, L₂ длины соответствующих звеньев манипулятора; А, В единичные векторы, распространенные в плоскости схвата и определяющие его ориентацию в трехмерном пространстве xyz; a_x, a_y, a_z, b_x, b_y, b_z соответствующие координаты векторов А и В в трехмерном пространстве xyz; e_i единичные векторы, совпадающие с осями шарниров и призматических пар степеней подвижности i манипулятора i=.

Устройство для прогpаммного управления манипулятором содержит блок 1 задания программы, а также по каждой регулируемой координате привод, последовательно соединенные первый сумматор 2, подключенные первым входом к второму выходу блока задания программы, первый блок 3 деления, первый функциональный преобразователь 4, второй функциональный преобразователь 5, первый блок 6 умножения, второй сумматор 7, третий функциональный преобразователь 8, второй блок 9 умножения, четвертый функциональный преобразователь 10, второй блок 11 деления, пятый функциональный преобразователь 12, шестой функциональный преобразователь 13, третий блок 14 умножения, четвертый блок 5 умножения, третий сумматор 16, седьмой функциональный преобразователь 17 и пятый блок 18 умножения, последовательно соединенные четвертый сумматор 19, первый вход которого подключен к четвертому выходу блока 1 задания программы, пятый сумматор 20 и третий блок 21 деления, второй вход которого соединен со вторыми входами третьего блока 14 умножения и пятого сумматора 20, выходом восьмого функционального преобразователя 22 и первым входом шестого блока 23 умножения, выход которого подключен к второму входу второго сумматора 7, последовательно соединенные седьмой блок 24 умножения, первый вход которого соединен с выходом второго функционального преобразователя 5, а его второй вход с выходом шестого функционального преобразователя 13, восьмой блок 25 умножения, шестой сумматор 26, девятый блок 27 умножения, седьмой сумматор 28, второй вход которого подключен к выходу десятого блока 29 умножения, и первый релейный элемент с вторым входом пятого блока 18 умножения, последовательно соединенные одиннадцатый блок 31 умножения, первый вход которого подключен к выходу третьего блока 14 умножения, восьмой сумматор 32, второй вход которого через двенадцатый блок 33 умножения соединен с восьмым выходом блока 1 задания программы и вторым входом четвертого блока 15 умножения, последовательно соединенные девятый функциональный преобразователь 34, вход которого подключен к выходу пятого функционального преобразователя 12 и тринадцатый блок 35 умножения, выход которого соединен с вторым входом шестого сумматора 26, последовательно соединенные четвертый блок 36 деления, первый вход которого соединен с первым выходом блока 1 задания программы, вторым входом первого сумматора 2, вторым входом первого блока 6 умножения и вторым входом девятого блока 27 умножения, а его второй вход с третьим выходом блока 1 задания программы, вторым входом шестого блока 23 умножения, первым входом десятого блока 29 умножения, второй вход которого подключен к выходу восьмого сумматора 32 и вторым входом четвертого сумматора 19, выход которого соединен с вторым входом первого блока 39 деления, десятый функциональный преобразователь 37, девятый сумматор 38, второй вход которого соединен с выходом первого функционального преобразователя 4 и входом восьмого функционального преобразователя 22, и второй релейный элемент 39, выход которого подключен ко второму входу второго блока 9 умножения, а также четырнадцатый блок 40 умножения, первый вход которого соединен с выходом девятого функционального преобразователя 34 и вторым входом двенадцатого блока 33 умножения, его второй вход с девятым выходом блока 1 задания программы и вторыми входами восьмого 25 и одиннадцатого 31 блоков умножения, а его выход с вторым входом третьего сумматора 16, третий вход которого соединен с выходом пятнадцатого блока 41 умножения. Первый вход последнего подключен к выходу седьмого блока 24 умножения, а его второй вход к седьмому выходу блока 1 задания программы и второму входу тринадцатого блока 35 умножения. Первый вход десятого сумматора 42 соединен с шестым выходом блока 1 задания программы, а его второй вход с пятым выходом этого блока и вторым входом второго блока 11 деления, причем выходы первого функционального преобразователя 4, десятого сумматора 42, третьего блока 21 деления, пятого функционального преобразователя 12, второго блока 9 умножения и пятого блока 18 умножения подключены к входам приводов, расположенных соответственно в первой, второй, третьей, четвертой, пятой и шестой степенях подвижности манипулятора.

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

Блок 1 задания программы вырабатывает координаты вектора r, характеризующего текущее положение средней точки Р схвата манипулятора в трехмерном пространстве xyz, а также текущие координаты векторов А и В, характеризующих ориентацию этого схвата в указанном пространстве (см. фиг. 2) в конкретный момент времени. Причем координаты вектора r: r_x, r_yи r_z формируются соответственно на втором, четвертом и шестом выходах блока 1 задания программы, координаты вектора А: a_x, a_y и a_z соответственно на первом, третьем и пятом выходах, а координаты вектора В: b_x, b_y и b_z соответственно на восьмом и девятом выходах этого блока. Все указанные координаты являются функциями времени.

Предлагаемое устройство должно обеспечить формирование требуемых сигналов управления, которые необходимо подать одновременно на все шесть приводов манипулятора (см. фиг. 2), установленных в соответствующие степени подвижности для обеспечения задаваемого блоком 1 закона перемещения схвата в пространстве xyz, т.е. для обеспечения задаваемого временного закона изменения векторов r r(t), A A(t) и B B(t), где t текущий момент времени.

Отметим, что на обобщенные координаты манипулятора накладываются следующие ограничения:

< q₁ < < q₄< < q₅ < - < q₆< .

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

Из фиг. 2 видно, что всегда выполняется равенство

R r L₂b₂ r L₂A. (1)

Первый отрицательный вход сумматора 2 имеет единичный коэффициент усиления, а его второй положительный вход коэффициент усиления, равный L₂. В результате на выходе этого сумматора формируется сигал R_x. Первые положительные входы сумматоров 19 и 42 имеют единичные коэффициенты усиления, а их вторые отрицательные входы коэффициенты усиления, равные L₂. В результате на выходах этих сумматоров согласно выражению (1) соответственно будут формироваться сигналы, равные R_y и R_z. Причем q₂ R_z.

Функциональный преобразователь 4 реализует функцию arctg. В результате на выходе этого преобразователя формируется сигнал, равный q₁ -arctg,причем R_y 0 в силу введенных ранее ограничений на q_i.

Первый положительный вход сумматора 20 имеет единичный коэффициент усиления, а его второй отрицательный вход коэффициент усиления L₁. Функциональный преобразователь 22 реализует функцию cos. В результате на выходе блока 21 деления формируется сигнал q₃ Причем cos q₁0 в силу введенных ранее ограничений на координату q₁.

Функциональный преобразователь 5 реализует функцию sin. Первый отрицательный и второй положительный входы сумматора 7 имеют единичные коэффициенты усиления. Функциональный преобразователь 8 реализует функцию arccos. В результате на его выходе будет формироваться сигнал arccos(a_y cos q₁ a_x sin q₁). Этот сигнал определяет абсолютную величину изменения координаты q₅, но не определяет ее напpавления.

Для определения знака q₅ можно воспользоваться следующим фактом. Если угол между положительным направлением оси Y и проекцией вектора А на плоскость XY больше, чем угол между тем же направлением оси Y и проекций вектора R на плоскость XY, то q₅ имеет положительное значение, а в противном случае отрицательное.

На выходе блока 36 деления формируется сигнал a_x/a_y, функциональный преобразователь 37 реализует функцию arctg. Если a_y__0, то на выходе блока 36 деления сигнал стремится к максимально возможному. При этом функциональный преобразователь 37 настраивается так, чтобы при a__0 в зависимости от знака a_x на его выходе формировался сигнал или Очевидно, что максимально возможный сигнал на выходе блока 36 деления будет определять статическую точность манипулятора при a_y__0. При формировании сигнала a_y блоком 1 необходимо избегать a_y 0 точно.

Первый положительный и второй отрицательный входы сумматора 38 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал равный разности углов, которые составляют с положительным направлением оси Y соответственно проекции вектора Ф и R на плоскость XY.

На выходе релейного элемента 39 реализуется сигнал, равный

U_{вых 39} если _< ^0,_0.

В результате на выходе блока 9 умножения формируется сигнал q₅ arccos(a_y cos q₁- a_x sin q₁).

Функциональный преобразователь 10 реализует функцию sin. В результате на выходе блока 11 деления формируется сигнал a_z/sin q₅, а на выходе функционального преобразователя 12, реализующего функцию arcsin сигнал q₄ arcsin(a_z/sin q₅). Поскольку обнуление sin q₅ возможно только при одновременном (см. фиг. 2) a_z 0, то зоны нечувствительности входов блока 11 деления должны настраиваться с учетом этого факта. При этом при a_z sin q0 на выходе блока 11 деления естественно должен формироваться нулевой сигнал.

Обобщенную координату q₆ можно определить из скалярного произведения вектора l₅ (cos q₁sin q₄, sin q₁ sin q₄, cos q₄)^Т, на вектор B (b_x, b_y, b_z)^Т.

Функциональный преобразователь 13 реализует функцию sin, преобразователь 34 функцию cos, в результате на выходе блока 41 умножения формируется сигнал sin q₁ sin q₄ b_y, на выходе блока 15 умножения сигнал cos q₁ sin q₄ b_x, а на выходе блока 40 умножения сигнал cos q₄ b_z. Все положительные входы сумматора 16 имеют единичные коэффициенты усиления. Функциональный преобразователь 17 реализует функцию arccos. В результате на его выходе формируется сигнал, равный

arccos(cosq₁ sinq₄b_x+sinq₁ x sinq₄b_y + cosq₄ b_z)

Для определения направления изменения координаты q₆ можно воспользоваться векторным произведением векторов l₅ и В. Если вектор l₅ x B совпадает с вектором А, то направление изменения q₆ является положительным, а в противном случае отрицательным.

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

На выходе блока 35 умножения формируется сигнал b_y cos q₄, а на выходе блока 25 умножения сигнал b_z sin q₁ sin q₄.

Первый положительный и второй отрицательный входы сумматора 26 имеют единичные коэффициенты усиления, поэтому на выходе этого сумматора формируется сигнал _x b_z sin q₁sin q₄ b_y cos q₄, а на выходе блока 17 умножения сигнал _x a_x.

Первый отрицательный и второй положительный входы сумматора 32 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора формируется сигнал _y b_x cos q₄ b_z cos q₁ sin q₄, а на выходе блока 29 умножения сигнал _y a_y.

На выходе сумматора 28, имеющего положительные входы с единичными коэффициентами усиления формируется сигнал =_x a_x + _y a_y. Если этот сигнал положительный, то угол q₆ положителен, в противном случае от отрицателен.

На выходе релейного элемента 30 формируется сигнал

U_{вых 30} если _< ^0,_0.

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

q₆ arc (b_xcos q₁sin q₄ + b_ysin q₁sin q₄ + b_zcos q₄).

Таким образом, с помощью предложенного устройства полностью удалось решить обратную задачу кинематики для манипулятора, изображенного на фиг. 2, т. е. удалось сформировать все сигналы q_i(t) i=, которые поступают на следящие приводы соответствующих степеней подвижности манипулятора и обеспечивают перемещение точки Р схвата в трехмерном пространстве по заданной вектором (t) траектории с заданной векторами A(t) и В(t) ориентацией схвата.

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