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

Классы МПК:B25J9/16 программное управление
Автор(ы):,
Патентообладатель(и):Институт автоматики и процессов управления Дальневосточного отделения РАН
Приоритеты:
подача заявки:
1999-01-05
публикация патента:

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

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

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

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

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

Известно устройство числового программного управления (см. Г.А. Спыну. Промышленные Роботы. Конструирование и применение - Киев: Выща школа, 1991, стр. 165-166), содержащее логический элемент, первый вход которого подключен к пульту обучения, его второй вход - к пульту оператора, третий - к накопителю на магнитной ленте, четвертый через шинный согласователь соединен с выходом микро ЭВМ, пятый и шестой соединены соответственно с формирователями технологических команд, установленных на оборудовании, и с датчиками обратных связей, причем с первого выхода этого элемента подаются сигналы управления следящими приводами, а с его второго выхода - технологические команды на оборудование.

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

Известно также устройство для программного управления манипулятором (см. Патент РФ N 2054349 МКМ 5 B 25 J 9/16, БИ N 5, 1996 г.), содержащее блок задания программы, по каждой регулируемой координате привод, последовательно соединенные первый сумматор, первый вход которого подключен к первому выходу блока задания программы, первый блок деления, первый функциональный преобразователь, второй сумматор, первый релейный элемент, первый блок умножения, второй функциональный преобразователь, второй блок деления и третий функциональный преобразователь, последовательно соединенные четвертый функциональный преобразователь, второй блок умножения, третий блок умножения, второй вход которого подключен к седьмому выходу блока задания программы, третий сумматор, пятый функциональный преобразователь, четвертый блок умножения, последовательно соединенные третий блок деления и шестой функциональный преобразователь, выход которого соединен со вторым входом второго сумматора, последовательно соединенные пятый блок умножения, четвертый сумматор и второй релейный элемент, выход которого соединен со вторым входом четвертого блока умножения, последовательно соединенные шестой блок умножения, пятый сумматор, выход которого соединен с первым входом пятого блока умножения, а его второй вход - с выходом седьмого блока умножения, последовательно соединенные восьмой блок умножения, первый вход которого подключен к девятому выходу блока задания программы, шестой сумматор, второй вход которого через девятый блок умножения соединен с восьмым выходом блока задания программы, десятый блок умножения, второй вход которого соединен со вторым выходом блока задания программы, вторым входом первого сумматора и с первым входом третьего блока деления, а его выход - со вторым входом четвертого сумматора, последовательно соединенные седьмой функциональный преобразователь, вход которого подключен ко входу четвертого функционального преобразователя и одиннадцатый блок умножения, второй вход которого соединен с первым входом седьмого блока умножения, его выход - со вторым входом третьего сумматора, а его первый вход - со вторым входом шестого блока умножения, последовательно соединенные восьмой функциональный преобразователь, двенадцатый блок умножения, седьмой сумматор, второй вход которого через тринадцатый блок умножения подключен ко второму входу второго блока умножения и девятый функциональный преобразователь, выход которого соединен со вторым входом первого блока умножения, последовательно соединенные четырнадцатый блок умножения, первый вход которого соединен с выходом восьмого функционального преобразователя, а второй - с выходом четвертого функционального преобразователя, и пятнадцатый блок умножения, выход которого подключен к третьему входу третьего сумматора, восьмой сумматор, первый вход которого соединен с третьим выходом блока задания программы и вторыми входами третьего блока деления и пятого блока умножения, его второй вход - с четвертым выходом блока задания программы, а выход - со вторым входом первого блока деления и первым входом четвертого блока деления, девятый сумматор, первый вход которого соединен с пятым, а второй - с шестым выходом блока задания программы. (Прототип).

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

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

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

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

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

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

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

На этих чертежах введены следующие обозначения qi - обобщенная координата степени подвижности i манипулятора, это управляющие координаты соответствующих приводов манипулятора устройство для программного управления манипулятором, патент № 2147000; r - вектор, задающий положение характерной точки схвата P в трехмерном пространстве, определяемом неподвижной системой координат XYZ; rx, ry, rz - координаты вектора r (характерной точки схвата манипулятора) в трехмерном пространстве; R - вектор, определяющих положение шарнира (точка W) пятой степени подвижности (координата q5) в трехмерном пространстве XYZ; Ex, Ry, Rz - координаты вектора R в трехмерном пространстве XYZ; L1, L2, L3 - длины соответствующих звеньев манипулятора; a, b - единичные векторы, расположенные в плоскости схвата и определяющие его ориентацию в трехмерном пространстве XYZ; ax, ay, az, bx, by, bz - соответствующие координаты векторов a и b в трехмерном пространстве XYZ; ei - единичные векторы, совпадающие с осями шарниров степеней подвижности манипулятора устройство для программного управления манипулятором, патент № 2147000.

Устройство для управления манипулятором (см. фиг. 2) содержит блок 1 задания программы, по каждой регулируемой координате привод, последовательно соединенные первый сумматор 2, первый вход которого подключен к первому выходу блока 1 задания программы, первый блок 3 деления, первый функциональный преобразователь 4, второй сумматор 5, первый релейный элемент 6, первый блок 7 умножения, второй функциональный преобразователь 8, второй блок 9 деления и третий функциональный преобразователь 10, последовательно соединенные четвертый функциональный преобразователь 11, второй 12 и третий 13 блоки умножения, третий сумматор 14, пятый функциональный преобразователь 15 и четвертый блок 16 умножения, последовательно соединенные третий блок 17 деления и шестой функциональный преобразователь 18, выход которого соединен со вторым входом второго сумматора 5, последовательно соединенный пятый блок 19 умножения, четвертый сумматор 20 и второй релейный элемент 21, выход которого соединен со вторым входом четвертого блока 16 умножения, последовательно соединенные шестой блок 22 умножения и пятый сумматор 23, выход которого подключен к первому выходу пятого блока 19 умножения, а его второй вход - к выходу седьмого блока 24 умножения, последовательно соединенные восьмой блок 25 умножения, первый вход которого подключен к девятому выходу блока 1 задания программы, шестой сумматор 26, второй вход которого через девятый блок 27 умножения соединен с восьмым выходом блока 1 задания программы, и десятый блок 28 умножения, второй вход которого соединен со вторым выходом блока 1 задания программы, вторым входом первого сумматора 2, и первым входом третьего блока 17 деления, а его выход - со вторым входом четвертого сумматора 20, последовательно соединенные седьмой функциональный преобразователь 2, вход которого подключен ко входу четвертого функционального преобразователя 11, и одиннадцатый блок 30 умножения, последовательно соединенные восьмой функциональный преобразователь 31, двенадцатый блок 32 умножения, седьмой сумматор 33, второй вход которого через тринадцатый блок 34 умножения подключен ко второму входу второго блока 12 умножения, и девятый функциональный преобразователь 35, выход которого соединен со вторым входом первого блока 7 умножения, последовательно соединенные четырнадцатый блок 36 умножения, первый вход которого соединен с выходом четвертого функционального преобразователя 31, и пятнадцатый блок 37 умножения, восьмой сумматор 38, первый вход которого соединен с третьим выходом блока 1 задания программы и вторыми входами третьего блока 17 деления и пятого блока 19 умножения, его второй вход - с четвертым выходом блока 1 задания программы, а выход - со вторым входом первого блока 3 деления, девятый сумматор 39, первый вход которого соединен с пятым, а второй - с шестым выходами блока 1 задания программы, десятый функциональный преобразователь 40, вход которого подключен к выходу первого 4 и входу восьмого 3 функциональных преобразователей, четвертый блок 41 деления и десятый сумматор 42, последовательно соединенные шестнадцатый блок 43 умножения, первый вход которого подключен ко второму выходу блока 1 задания программы, и одиннадцатый сумматор 44, второй вход которого через последовательно соединенные одиннадцатый функциональный преобразователь 45 и семнадцатый блок 46 умножения подключен к выходу первого функционального преобразователя 4, а его выход - ко второму входу второго блока 9 деления, причем второй вход семнадцатого блока 46 умножения соединен с третьим выходом блока 1 задания программы, а его первый вход - с первым входом восемнадцатого блока 47 умножения, второй вход которого подключен ко второму выходу блока 1 задания программы, а его выход - ко второму входу тринадцатого блока 34 умножения, последовательно соединенные девятнадцатый блок 48 умножения, первый вход которого соединен с выходом десятого функционального преобразователя 40, второй вход - с выходом четвертого блока 41 деления, двенадцатый функциональный преобразователь 49, первый элемент 50 слежения - хранения, второй вход которого через третий релейный элемент 51 соединен с выходом первого сумматора 2 и первым входом четвертого блока 41 деления, тринадцатый функциональный преобразователь 52 и пятый блок 53 деления, второй вход которого соединен с выходом девятого сумматора 39 и вторым входом четвертого блока 41 деления, а его выход - с первым входом десятого сумматора 42, второй вход которого соединен с выходом первого источника 54 постоянного сигнала, последовательно соединенные четырнадцатый функциональный преобразователь 55, вход которого подключен ко входу тринадцатого функционального преобразователя 52, и двадцатый блок 56 умножения, первый вход которого также соединен со вторым входом второго блока 12 умножения, второй вход - с третьим выходом блока 1 задания программы, а его выход - со вторым входом двенадцатого блока 32 умножения, последовательно соединенные четвертый релейный элемент 57 и второй элемент 58 слежения - хранения, второй вход которого подключен к выходу десятого сумматора 42, последовательно соединенные пятнадцатый функциональный преобразователь 59, двадцать первый блок 60 умножения, двенадцатый сумматор 61, пятый релейный элемент 62, двадцать второй блок 63 умножения, второй вход которого через шестой релейный элемент 64 соединен с выходом первого блока 7 умножения и входом пятнадцатого функционального преобразователя 59, двадцать третий блок 65 умножения, второй вход которого подключен к выходу третьего функционального преобразователя 10, и третий элемент слежения - хранения 66, второй вход которого через седьмой релейный элемент 67 подключен к выходу второго функционального преобразователя 8, а выход - ко входу четвертого функционального преобразователя 11, последовательно соединенные двадцать четвертый блок 68 умножения, первый вход которого соединен с выходом седьмого функционального преобразователя 29, тринадцатый сумматор 69, второй вход которого подключен к выходу пятнадцатого блока 37 умножения, и двадцать пятый блок 70 умножения, первый вход которого подключен также ко второму входу восьмого блока 25 умножения, его второй вход - к восьмому выходу блока 1 задания программы, а его выход - ко второму входу третьего сумматора 14, последовательно соединенные двадцать шестой блок 71 умножения, первый вход которого подключен ко второму входу двадцать четвертого блока 68 умножения и выходу одиннадцатого функционального преобразователя 45, двадцать седьмой блок 72 умножения, второй вход которого соединен с выходом четвертого функционального преобразователя 11, четырнадцатый сумматор 73, второй вход которого подключен к выходу одиннадцатого блока 30 умножения, второй вход которого соединен с выходом восьмого функционального преобразователя 31 и вторыми входами пятнадцатого 37 и шестнадцатого 43 блоков умножения, и двадцать восьмой блок 74 умножения, второй вход которого соединен с седьмым выходом блока 1 задания программы и первым входом седьмого блока 24 умножения, а выход - с третьим входом третьего сумматора 14, причем первый вход шестого блока 22 умножения подключен к выходу четырнадцатого сумматора 73, а его второй вход - к девятому выходу блока 1 задания программы и ко второму входу третьего блока 13 умножения, третий вход девятого сумматора 39 соединен с выходом второго источника 75 постоянного сигнала, второй вход двадцать шестого блока 71 умножения подключен ко второму входу четырнадцатого блока 36 умножения, входу четвертого релейного элемента 57, выходу тринадцатого функционального преобразователя 52, второму входу двадцать первого блока 60 умножения и первому входу двадцать девятого блока 76 умножения, второй вход которого подключен к пятому выходу блока 1 задания программы и второму входу двенадцатого сумматора 61, а его выход - к третьему входу седьмого сумматора 33, вторые входы седьмого 24 и девятого 27 блоков умножения подключены к выходу второго блока 12 умножения.

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

Блок 1 задания программы вырабатывает координаты вектора r, характеризующего текущее положение средней точки P схвата манипулятора в трехмерном пространстве XYZ, а также текущие координаты векторов a и b, характеризующих ориентацию этого схвата в указанном пространстве в конкретный момент времени. Координаты вектора r: rx, ry, rz формируются соответственно на первом, четвертом и шестом выходах блока 1 задания программы, координаты вектора a: ax, ay, az - соответственно на втором, третьем и пятом выходах, а координаты вектора b: bx, by, bz - соответственно на седьмом, восьмом и девятом выходах этого блока. Все указанные координаты являются функциями времени. Предлагаемое устройство должно обеспечивать формирование требуемых сигналов управления qi устройство для программного управления манипулятором, патент № 2147000, которые необходимо подать одновременно на все шесть приводов манипулятора, установленные в соответствующие степени подвижности для обеспечения задаваемого блоком 1 закона перемещения схвата в пространстве XYZ, т.е. для обеспечения закона изменения векторов r = r(t), a = a(t), b = b(t), где t - текущий момент времени.

На обобщенные координаты манипулятора накладываются следующие ограничения устройство для программного управления манипулятором, патент № 2147000 (i = 1, 2, 4, 5, 6). Отсчет вращательных степеней подвижности начинается из положения манипулятора, показанного на фиг. 1. При этом движение против часовой стрелки считается положительным, а по часовой стрелке - отрицательным. Направление вращения определяется относительно соответствующих векторов ei, если взгляд направлен от стрелки к основанию вектора ei.

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

R = r - L3e6 = r - L3a.

Первый отрицательный вход сумматора 2 имеет единичный коэффициент усиления, его второй положительный вход - коэффициент усиления L3. В результате на выходе этого сумматора формируется сигнал - Rx = -rx + L3ax.

Первые отрицательные входы сумматоров 38 и 39 имеют коэффициенты усиления L3, а их вторые положительные входы - коэффициенты усиления, равные единице. В результате на выходах этих сумматоров, соответственно, будут формироваться сигналы, равные Ry = ry - L3устройство для программного управления манипулятором, патент № 2147000ay и Rz - L1 = rz - L3az - L1, т.к. с источника постоянного сигнала 75 на третий отрицательный единичный вход сумматора 39 поступает сигнал L1. Функциональный преобразователь 4 реализует функцию arctg. В результате на выходе этого функционального преобразователя формируется сигнал

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

Причем Ry устройство для программного управления манипулятором, патент № 2147000 0 в силу введенных ранее ограничений на q1.

Функциональный преобразователь 40 реализует функцию sin, а функциональный преобразователь 49 - функцию arctg. В результате на выходе функционального преобразователя 49 будет формироваться сигнал

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

Поскольку Rx может принимать значение Rx = 0, то в этом случае необходимо исключать ситуацию, когда аргумент arctg становится равным устройство для программного управления манипулятором, патент № 2147000. Это удается сделать с помощью релейного элемента 51, имеющего характеристику

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

где устройство для программного управления манипулятором, патент № 21470001 малая положительная величина.

Когда устройство для программного управления манипулятором, патент № 2147000 элемент слежения-хранения 50 по нулевому сигналу с релейного элемента 51 сохраняет последнее значение q2 до тех пор, пока при движении манипулятора устройство для программного управления манипулятором, патент № 2147000 опять не станет устройство для программного управления манипулятором, патент № 2147000устройство для программного управления манипулятором, патент № 21470001.

При Rx>устройство для программного управления манипулятором, патент № 21470001 единичный сигнал с релейного элемента 51 обеспечивает обновление текущего значения выходного сигнала элемента слежения - хранения 50.

Функциональный преобразователь 52 реализует функцию sin, а на выходе источника постоянного сигнала 54 формируется сигнал L2, который поступает на второй отрицательный вход сумматора 42. Все входы этого сумматора имеют одиночные коэффициенты усиления, поэтому на его выходе формируется сигнал устройство для программного управления манипулятором, патент № 2147000. Поскольку sin q2 может принимать нулевые значения, то для исключения недопустимо больших ошибок в получении сигнала q3, как и раньше, вводятся элемент слежения - хранения 58 и релейный элемент 57, управляющий элементом слежения хранения, с характеристикой

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

где устройство для программного управления манипулятором, патент № 21470002 - малая положительная величина.

Если sin q2 ---> 0 релейный элемент 57 нулевым сигналом сохраняет в элементе 58 постоянное значение ранее вычисленной величины q3 до тех пор пока значение устройство для программного управления манипулятором, патент № 2147000 не превысит устройство для программного управления манипулятором, патент № 21470002. При устройство для программного управления манипулятором, патент № 2147000 значение q3 точно соответствует текущему (истинному) значению.

Функциональный преобразователь 45 реализует функцию sin, а функциональный преобразователь 55 - функцию cos. В результате на выходе блока умножения 34 сформируется сигнал axsinq1cosq2. Функциональный преобразователь 31 реализует функцию cos. В результате на выходе блока умножение 32 формируется сигнал aycosq1cosq2, а на выходе блока умножения 76 - сигнал azsinq2.

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

arccos(azsinq2 + aycosq1cosq2 - axsinq1cosq2).

Этот сигнал определяет лишь абсолютную величину координаты q5.

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

На выходе блока 17 деления формируется сигнал ax/ay, функциональный преобразователь 18 реализует функцию arctg. При ay ---> 0 сигнал на выходе блока 17 стремится к максимально возможному. При этом функциональный преобразователь 18 настраивается так, чтобы при ay ---> 0 в зависимости от знака ax на его выходе был бы сигнал устройство для программного управления манипулятором, патент № 2147000.

При формировании сигнала ay блоком 1 следует избегать ay = 0 точно.

Блок деления 3 имеет на выходе сигнал устройство для программного управления манипулятором, патент № 2147000. Функциональный преобразователь 4 реализует функцию arctg. Первый и второй отрицательные выходы сумматора 5 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал a, равный разности углов, которые составляют с положительным направлением оси Y соответственно проекции векторов a и R на плоскость XY.

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

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

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

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

q5 = устройство для программного управления манипулятором, патент № 2147000arccos(azsinq2 + aycosq1cosq2 - axsinq1cosq2).

Функциональные преобразователи 8 и 45 реализует функцию sin. На выходе блоков умножения 43 и 46 соответственно формируются сигналы axcosq1 и aysinq1. Первый и второй отрицательные входы сумматора 44 имеют единичные коэффициенты усиления. Функциональный преобразователь 10 реализует функцию arccos. В результате на его выходе будет формироваться сигнал

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

определяющий модуль координаты q4. Определение знака q4 осуществляется при сравнении координаты вектора e6z = az со знаком той же координаты вектора e6 при q4 = 0, т.е. e6z0. При положительном знаке q5 обобщенная координата q4 имеет положительный знак, если e6z0 устройство для программного управления манипулятором, патент № 2147000 az, в противном случае - отрицательный. При отрицательном знаке q5 обобщенная координата q4 имеет положительный знак, если e6z0 устройство для программного управления манипулятором, патент № 2147000 az, в противном случае - отрицательный.

Функциональный преобразователь 59 реализует функцию cos. На выходе блока 57 умножения формируется сигнал sinq2cosq5 = e6z0. Первый отрицательный и второй положительный входы сумматора 61 имеют единичные коэффициенты усиления. На выходе этого сумматора формируется сигнал k = az - e6z0. Этот сигнал поступает на вход релейного элемента 62, на выходе которого реализует сигнал

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

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

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

В результате на выходе блока 63 умножения формируется сигнал, определяющий знак обобщенной координаты q4, а на выходе блока умножения 65 формируется сигнал

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

Элемент слежения - хранения 66 обеспечивает работоспособность устройства при q5 ---> 0, когда на вход делителя блока деления 9 поступает близкий к нулю сигнал, т. е. когда этот блок может работать с недопустимо большой погрешностью. При q5 ---> 0 элемент 66 сохраняет значение q4 неизменным до тех пор, пока q5 находится в опасной для блока деления 9 зоне. Элемент 66 управляется релейным элементом 67, имеющим следующую характеристику

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

где устройство для программного управления манипулятором, патент № 21470003 - малая положительная величина.

Элемент 66 находится в режиме хранения, когда устройство для программного управления манипулятором, патент № 2147000, и в режиме слежения, когда устройство для программного управления манипулятором, патент № 2147000.

Обобщенную координату q6 можно определить из скалярного произведения векторов

e5 = (cosq1sinq4 + sinq1sinq2cosq4, sinq1sinq4 - cosq1sinq2cosq4, cosq2cosq4) и

b = (bx, by, bz).

Функциональный преобразователь 11 реализует функцию cos. В результате на выходе блока умножения 72 формируется сигнал sinq1sinq2cosq4. Функциональный преобразователь 29 реализует функцию sin. В результате на выходе блока умножения 30 формируется сигнал cosq1sinq4. Поскольку все положительные входы сумматора 73 имеют единичные коэффициенты усиления, то на выходе блока умножения 74 формируется сигнал (cosq1sinq4 + sinq1sinq2cosq4)bx. На выходе блока умножения 68 формируется сигнал sinq1sinq4, а на выходе блока умножения 37 - сигнал cosq1sinq2cosq4. Поскольку первый положительный и второй отрицательный входы сумматора 69 имеют единичные коэффициенту усиления, то на выходе блока умножения 70 формируется сигнал (sinq1sinq4 - cosq1sinq2cosq4)by. На выходе блока умножения 13 формируется сигнал (cosq2cosq4)bz.

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

arccos((cosq1sinq4 + sinq1sinq2cosq4)bx + (sinq1sinq4 - cosq1sinq2cosq4)by + cosq2cosq4bz) = A,

равный абсолютному значению обобщенной координаты q6.

Для определения знака q6 воспользуемся векторным произведением векторов e5 и b. Если устройство для программного управления манипулятором, патент № 2147000 = e5устройство для программного управления манипулятором, патент № 2147000b совпадает с вектором a, то q6 положительна, а в противном случае - отрицательна. В силу введенных ограничений на обобщенные координаты манипулятора невозможно одновременное обнуление проекций векторов устройство для программного управления манипулятором, патент № 2147000 и a на оси координат X и Y. В результате о совпадении направлений векторов устройство для программного управления манипулятором, патент № 2147000 и a можно судить, анализируя только две их проекции: на оси X и Y. На выходе сумматора 26, первый положительный и второй отрицательный входы которого имеют единичные коэффициенты усиления, формируется сигнал

устройство для программного управления манипулятором, патент № 2147000x = (sinq1sinq4 - cosq1sinq2cosq4)bz - (cosq2cosq4)by,

а на выходе блока умножения 28 - сигнал устройство для программного управления манипулятором, патент № 2147000xустройство для программного управления манипулятором, патент № 2147000устройство для программного управления манипулятором, патент № 2147000x.

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

устройство для программного управления манипулятором, патент № 2147000y = -(cosq1sinq4 + sinq1sinq2cosq4)bz + (cosq2cosq4)bx,

а на выходе блока умножения 19 - сигнал устройство для программного управления манипулятором, патент № 2147000yустройство для программного управления манипулятором, патент № 2147000ax.

Первый и второй положительные входы сумматора 20 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал устройство для программного управления манипулятором, патент № 2147000 = устройство для программного управления манипулятором, патент № 2147000yустройство для программного управления манипулятором, патент № 2147000ax+устройство для программного управления манипулятором, патент № 2147000yay..

Если сигнал устройство для программного управления манипулятором, патент № 2147000 положительный, то q6 имеет знак "плюс", в противном случае - "минус", т.к. на выходе релейного элемента 21 формируется сигнал

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

В результате на выходе блока 16 формируется сигнал q6 = устройство для программного управления манипулятором, патент № 2147000A.

Таким образом, с помощью предложенного устройства удалось полностью решить обратную задачу кинематики для манипулятора, изображенного на фиг. 1, т. е. удалось сформировать все сигналы qi(t) устройство для программного управления манипулятором, патент № 2147000, которые поступают на следящие приводы соответствующих степеней подвижности манипулятора и обеспечивают перемещение точки P схвата в трехмерном пространстве по заданной вектором r(t) траектории с заданной векторами a(t) и b(t) ориентацией схвата. Практическая реализация этого устройства не вызывает затруднений, т.к. оно реализовано с помощью типовых электронных элементов и блоков.

Класс B25J9/16 программное управление

способ выполнения синхронизированной роботизированной технологической операции на конструкции, имеющей ограниченное пространство, в частности на кессоне крыла летательного аппарата, соответствующее компьютерное устройство и роботизированная установка -  патент 2509681 (20.03.2014)
способ управления стабилизацией шагающего робота -  патент 2507061 (20.02.2014)
управление перемещением упругих структур робота -  патент 2487796 (20.07.2013)
электропривод манипулятора -  патент 2443543 (27.02.2012)
электропривод манипулятора -  патент 2443542 (27.02.2012)
рабочая станция с многосторонним держателем деталей и способ управления такой станцией -  патент 2404043 (20.11.2010)
устройство управления строительной машиной (варианты) -  патент 2399576 (20.09.2010)
манипуляционная система -  патент 2389599 (20.05.2010)
исполнительный механизм -  патент 2381099 (10.02.2010)
интеллектуальное устройство управления стрелой -  патент 2344923 (27.01.2009)
Наверх