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

Классы МПК:B25J9/16 программное управление
Автор(ы):,
Патентообладатель(и):Дальневосточный государственный технический университет
Приоритеты:
подача заявки:
1997-12-23
публикация патента:

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

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

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

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

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

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

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

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

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

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

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

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

Сопоставительный анализ признаков заявляемого решения с признаками аналогов и прототипа свидетельствует о его соответствии критерию "новизна".

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

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

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

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

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

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

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

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

где устройство для программного управления манипулятором, патент № 2120850 , устройство для программного управления манипулятором, патент № 2120850 = q2 + q3.

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

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

R = r - L4e6 = r - L4a. (1)

Первый отрицательный вход сумматора 2 имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления, равный L4. В результате на выходе этого сумматора формируется сигнал - Rx. Первые положительные входы сумматоров 39 и 40 имеют единичные коэффициенты усиления, а их вторые отрицательные входы - коэффициенты усиления, равные L4. В результате на выходах этих сумматоров согласно выражению (1) соответственно будут формироваться сигналы, равные Ry и Rz.

Функциональный преобразователь 4 реализует функцию arctg. В результате на выходе этого преобразователя формируется сигнал, равный устройство для программного управления манипулятором, патент № 2120850 Причем Ry устройство для программного управления манипулятором, патент № 2120850 0 в силу введенных ранее ограничений на q1 и устройство для программного управления манипулятором, патент № 2120850. Источник постоянного сигнала 51 вырабатывает сигнал, равный L1. Первый отрицательный и второй положительный входы сумматора 41 имеют коэффициенты усиления устройство для программного управления манипулятором, патент № 2120850 В результате на выходе сумматора 41 формируется сигнал устройство для программного управления манипулятором, патент № 2120850

Источник постоянного сигнала 66 вырабатывает сигнал f2, равный устройство для программного управления манипулятором, патент № 2120850

Функциональный преобразователь 10 реализует функцию cos. Усилитель 47 имеет коэффициент усиления устройство для программного управления манипулятором, патент № 2120850 и в результате на его выходе формируется сигнал устройство для программного управления манипулятором, патент № 2120850

Квадраторы 67 и 71 реализуют функцию y = 4 x2, где x - вход, а y - выход. На выходе квадратора 67 формируется сигнал g21, где g1 = 2 f1f2, а на выходе квадратора 71 - сигнал g22, где g2 = 2 f2f3. Первый и второй положительные входы сумматора 68 имеют единичные коэффициенты усиления. Функциональный преобразователь 69 реализует функцию извлечения квадратного корня. В результате на его выходе будет формироваться сигнал устройство для программного управления манипулятором, патент № 2120850

Квадраторы 48, 49 и 52 реализуют функцию y = x2. Первый, третий и четвертый отрицательные входы сумматора 50 имеют единичные коэффициенты усиления, а второй положительный вход имеет коэффициент усиления, равный устройство для программного управления манипулятором, патент № 2120850 В результате на выходе этого сумматора формируется сигнал g3 = 1-f21-f22-f23.

Функциональный преобразователь 54 реализует функцию arcsin, а функциональный преобразователь 56 - функцию arctg (вида, изображенного на фиг. 3). Первый положительный и второй отрицательный входы сумматора 55 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора формируется сигнал устройство для программного управления манипулятором, патент № 2120850

Функциональный преобразователь 57 реализует функцию sin. Первый и второй положительные входы сумматора 59 имеют единичные коэффициенты усиления. Функциональный преобразователь 60 реализует функцию arcsin. Первый положительный и второй отрицательный входы сумматора 61 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора формируется сигнал q3 = arcsin (f1 + f2 устройство для программного управления манипулятором, патент № 2120850 sin q2) - q2.

Первый и второй положительные входы сумматора 62 имеют единичные коэффициенты усиления. На выходе этого сумматора формируется сигнал устройство для программного управления манипулятором, патент № 2120850 = q2 + q3.

Функциональный преобразователь 5 реализует функцию sin. Функциональные преобразователи 63 и 72 реализуют функции cos и sin, соответственно. Первый и второй положительные и третий отрицательный входы сумматора 26 имеют единичные коэффициенты усиления. Функциональный преобразователь 13 реализует функцию arccos. В результате на его выходе формируется сигнал arccos(-axsinq1cosустройство для программного управления манипулятором, патент № 2120850 + aycosq1cosустройство для программного управления манипулятором, патент № 2120850 + azsinустройство для программного управления манипулятором, патент № 2120850). Этот сигнал определяет абсолютную величину изменения координаты q5, но не определяет ее направления.

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

На выходе блока 7 деления формируется сигнал устройство для программного управления манипулятором, патент № 2120850 Функциональный преобразователь 8 реализует функцию arctg. Если ay _устройство для программного управления манипулятором, патент № 2120850 0, то на выходе блока 7 деления сигнал стремится к максимально возможному. При этом функциональный преобразователь 8 настраивается так, чтобы при ay _устройство для программного управления манипулятором, патент № 2120850 0 в зависимости от знака ax на его выходе формировался сигнал устройство для программного управления манипулятором, патент № 2120850 или устройство для программного управления манипулятором, патент № 2120850 Очевидно, что максимально возможный сигнал на выходе блока 7 деления будет определять статическую точность манипулятора при ay _устройство для программного управления манипулятором, патент № 2120850 0. При формировании сигнала ay блоком 1 необходимо избегать ay = 0 точно.

Первый и второй отрицательные входы сумматора 9 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал устройство для программного управления манипулятором, патент № 2120850, равный разности углов, которые составляют с положительным направлением оси y соответственно проекции вектора a и R на плоскость xy.

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

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

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

q5 = устройство для программного управления манипулятором, патент № 2120850arccos(-axsinq1cosустройство для программного управления манипулятором, патент № 2120850+aycosq1cosустройство для программного управления манипулятором, патент № 2120850+azsinустройство для программного управления манипулятором, патент № 2120850).

Первый и второй отрицательные входы сумматора 44 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора реализуется сигнал - ax cos q1 - ay sin q1. Функциональный преобразователь 14 реализует функцию sin. На выходе блока 15 деления формируется сигнал (-ax cos q1 - ay sin q1)/sin q5. Функциональный преобразователь 16 реализует функцию аrccos, и на его выходе формируется сигнал устройство для программного управления манипулятором, патент № 2120850 Этот сигнал определяет абсолютную величину координат q4, но не определяет ее направления.

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

Функциональный преобразователь 74 реализует функцию cos. На выходе блока 75 умножения формируется сигнал e06z = sinустройство для программного управления манипулятором, патент № 2120850устройство для программного управления манипулятором, патент № 2120850cosq5. Первый отрицательный и второй положительный входы сумматора 76 имеют единичные коэффициенты усиления. На выходе этого сумматора формируется сигнал k = az - e06z. Этот сигнал поступает на вход релейного элемента 77, на выходе которого формируется сигнал

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

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

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

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

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

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

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

где

устройство для программного управления манипулятором, патент № 2120850 - малая величина, близкая к нулю.

Элемент слежения-хранения 82 сохраняет значение q4 неизменным при q5__устройство для программного управления манипулятором, патент № 2120850 0, когда на вход делителя блока деления 15 поступает близкий к нулю сигнал, т.е. когда этот блок может работать с недопустимой погрешностью.

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

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

на вектор b = (bx, by, bz).

Функциональный преобразователь 17 реализует функцию sin, а функциональный преобразователь 18 - функцию cos.

Первый и второй положительные входы сумматора 86 имеют единичные коэффициенты усиления. В результате на выходе блока 38 умножения формируется сигнал bxустройство для программного управления манипулятором, патент № 2120850(cosq1устройство для программного управления манипулятором, патент № 2120850sinq4 + sinq1устройство для программного управления манипулятором, патент № 2120850sinустройство для программного управления манипулятором, патент № 2120850устройство для программного управления манипулятором, патент № 2120850cosq4).

Первый отрицательный и второй положительный входы сумматора 88 имеют единичные коэффициенты усиления. В результате на выходе блока 36 умножения формируется сигнал byустройство для программного управления манипулятором, патент № 2120850(sinq1устройство для программного управления манипулятором, патент № 2120850sinq4 - cosq1устройство для программного управления манипулятором, патент № 2120850sinустройство для программного управления манипулятором, патент № 2120850устройство для программного управления манипулятором, патент № 2120850cosq4). На выходе блока 33 умножения формируется сигнал bzустройство для программного управления манипулятором, патент № 2120850cosустройство для программного управления манипулятором, патент № 2120850устройство для программного управления манипулятором, патент № 2120850cosq4.

Все три положительных входа сумматора 37 имеют единичные коэффициенты усиления. Функциональный преобразователь 31 реализует функцию arccos. В результате на его выходе формируется сигнал, равный устройство для программного управления манипулятором, патент № 2120850 Этот сигнал определяет только абсолютную величину обобщенной координаты q6.

Для определения направления изменения координаты q6 можно воспользоваться векторным произведением векторов e5 и b. Если вектор устройство для программного управления манипулятором, патент № 2120850 = e5 b совпадает с вектором a, то направление изменения q6 является положительным, а в противном случае - отрицательным.

В силу введенных ограничений на обобщенные координаты манипулятора невозможно одновременное обнуление проекций векторов устройство для программного управления манипулятором, патент № 2120850 и a на оси координат x и y. В результате o совпадении направлений векторов устройство для программного управления манипулятором, патент № 2120850 и a можно судить, анализируя только их проекции на оси x и y.

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

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

На выходе сумматора 27, имеющего положительные входы с единичными коэффициентами усиления, формируется сигнал устройство для программного управления манипулятором, патент № 2120850 = устройство для программного управления манипулятором, патент № 2120850xустройство для программного управления манипулятором, патент № 2120850ax + устройство для программного управления манипулятором, патент № 2120850yустройство для программного управления манипулятором, патент № 2120850ay. Если этот сигнал положительный, то угол q6 положителен. В противном случае он отрицателен.

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

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

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

Таким образом, с помощью предложенного устройства полностью удалось решить обратную задачу кинематики для манипулятора, изображенного на фиг. 2. То есть удалось сформировать все сигналы устройство для программного управления манипулятором, патент № 2120850 которые поступают на следящие приводы соответствующих степеней подвижности манипулятора и обеспечивают перемещение точки 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)
Наверх