RU2120850C1 - Устройство для программного управления манипулятором - Google Patents

Abstract

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

Description

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

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

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

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

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

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

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

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

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

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

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

На этих чертежах введены следующие обозначения: q_i - обобщенная координата степени подвижности i манипулятора, это управляющие координаты соответствующих приводов манипулятора

r - вектор, задающий положение характерной точки схвата P в трехмерном пространстве, определяемом неподвижной системой координат xyz; r_x, r_y,r_z - координаты вектора r (характерной точки схвата манипулятора) в трехмерном пространстве; R - вектор, определяющий положение шарнира (точка W) пятой степени подвижности (координата q₅) в трехмерном пространстве xyz; R_x, R_y, R_z -координаты вектора R в трехмерном пространстве xyz; L₁, L₂, L₃, L₄ - длины соответствующих звеньев манипулятора; a, b - единичные векторы, расположенные в плоскости схвата и определяющие его ориентацию в трехмерном пространстве xyz; a_x, a_y, a_z, b_x, b_y, b_z - соответствующие координаты векторов a и b в трехмерном пространстве xyz; e_i - единичные векторы, совпадающие с осями шарниров степеней подвижности манипулятора

Устройство для программного управления манипулятором содержит блок 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: r_x, r_y и r_z формируются соответственно на втором, четвертом и шестом выходах блока 1 задания программы, координаты вектора a: a_x, a_y и a_z - соответственно на первом, третьем и пятом выходах, а координаты вектора b: b_x, b_y и b_z - соответственно на восьмом, девятом и седьмом выходах этого блока. Все указанные координаты являются функциями времени.

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

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

где

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

На фиг. 2 видно, что всегда выполняется равенство
R = r - L₄e₆ = r - L₄a. (1)
Первый отрицательный вход сумматора 2 имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления, равный L₄. В результате на выходе этого сумматора формируется сигнал - R_x. Первые положительные входы сумматоров 39 и 40 имеют единичные коэффициенты усиления, а их вторые отрицательные входы - коэффициенты усиления, равные L₄. В результате на выходах этих сумматоров согласно выражению (1) соответственно будут формироваться сигналы, равные R_y и R_z.

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

Причем R_y ≠ 0 в силу введенных ранее ограничений на q₁ и φ. Источник постоянного сигнала 51 вырабатывает сигнал, равный L₁. Первый отрицательный и второй положительный входы сумматора 41 имеют коэффициенты усиления

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

Источник постоянного сигнала 66 вырабатывает сигнал f₂, равный

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

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

Квадраторы 67 и 71 реализуют функцию y = 4 x², где x - вход, а y - выход. На выходе квадратора 67 формируется сигнал g $\genfrac{}{}{0ex}{}{\text{2}}{\text{1}}$ , где g₁ = 2 f₁f₂, а на выходе квадратора 71 - сигнал g $\genfrac{}{}{0ex}{}{\text{2}}{\text{2}}$ , где g₂ = 2 f₂f₃. Первый и второй положительные входы сумматора 68 имеют единичные коэффициенты усиления. Функциональный преобразователь 69 реализует функцию извлечения квадратного корня. В результате на его выходе будет формироваться сигнал

Квадраторы 48, 49 и 52 реализуют функцию y = x². Первый, третий и четвертый отрицательные входы сумматора 50 имеют единичные коэффициенты усиления, а второй положительный вход имеет коэффициент усиления, равный

В результате на выходе этого сумматора формируется сигнал g₃ = 1-f $\genfrac{}{}{0ex}{}{\text{2}}{\text{1}}$ -f $\genfrac{}{}{0ex}{}{\text{2}}{\text{2}}$ -f $\genfrac{}{}{0ex}{}{\text{2}}{\text{3}}$ .
Функциональный преобразователь 54 реализует функцию arcsin, а функциональный преобразователь 56 - функцию arctg (вида, изображенного на фиг. 3). Первый положительный и второй отрицательный входы сумматора 55 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора формируется сигнал

Функциональный преобразователь 57 реализует функцию sin. Первый и второй положительные входы сумматора 59 имеют единичные коэффициенты усиления. Функциональный преобразователь 60 реализует функцию arcsin. Первый положительный и второй отрицательный входы сумматора 61 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора формируется сигнал q₃ = arcsin (f₁ + f₂ • sin q₂) - q₂.

Первый и второй положительные входы сумматора 62 имеют единичные коэффициенты усиления. На выходе этого сумматора формируется сигнал φ = q₂ + q₃.
Функциональный преобразователь 5 реализует функцию sin. Функциональные преобразователи 63 и 72 реализуют функции cos и sin, соответственно. Первый и второй положительные и третий отрицательный входы сумматора 26 имеют единичные коэффициенты усиления. Функциональный преобразователь 13 реализует функцию arccos. В результате на его выходе формируется сигнал arccos(-a_xsinq₁cosφ + a_ycosq₁cosφ + a_zsinφ). Этот сигнал определяет абсолютную величину изменения координаты q₅, но не определяет ее направления.

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

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

Функциональный преобразователь 8 реализует функцию arctg. Если a_y _→ 0, то на выходе блока 7 деления сигнал стремится к максимально возможному. При этом функциональный преобразователь 8 настраивается так, чтобы при a_y _→ 0 в зависимости от знака a_x на его выходе формировался сигнал

или

Очевидно, что максимально возможный сигнал на выходе блока 7 деления будет определять статическую точность манипулятора при a_y _→ 0. При формировании сигнала a_y блоком 1 необходимо избегать a_y = 0 точно.

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

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

В результате на выходе блока 12 умножения формируется сигнал
q₅ = ∓arccos(-a_xsinq₁cosφ+a_ycosq₁cosφ+a_zsinφ).
Первый и второй отрицательные входы сумматора 44 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора реализуется сигнал - a_x cos q₁ - a_y sin q₁. Функциональный преобразователь 14 реализует функцию sin. На выходе блока 15 деления формируется сигнал (-a_x cos q₁ - a_y sin q₁)/sin q₅. Функциональный преобразователь 16 реализует функцию аrccos, и на его выходе формируется сигнал

Этот сигнал определяет абсолютную величину координат q₄, но не определяет ее направления.

Определение знака обобщенной координаты q₄ осуществляется при сравнении координаты вектора e_6z(e_6z = a_z) со значением рассчитываемой координаты того же вектора e₆ при q₄ = 0, т.е. e $\genfrac{}{}{0ex}{}{\text{0}}{\text{6z}}$ . При положительном знаке q₅ обобщенная координата q₄ имеет положительный знак, если e $\genfrac{}{}{0ex}{}{\text{0}}{\text{6z}}$ ≤ a_z, в противном случае - отрицательный. При отрицательном знаке q₅ обобщенная координата q₄ имеет положительный знак, если e $\genfrac{}{}{0ex}{}{\text{0}}{\text{6z}}$ ≥ a_z, в противном случае - отрицательный. В случае равенства q₅ = 0 обобщенная координата q₄ принимает предыдущее значение.

Функциональный преобразователь 74 реализует функцию cos. На выходе блока 75 умножения формируется сигнал e $\genfrac{}{}{0ex}{}{\text{0}}{\text{6z}}$ = sinφ•cosq₅. Первый отрицательный и второй положительный входы сумматора 76 имеют единичные коэффициенты усиления. На выходе этого сумматора формируется сигнал k = a_z - e $\genfrac{}{}{0ex}{}{\text{0}}{\text{6z}}$ . Этот сигнал поступает на вход релейного элемента 77, на выходе которого формируется сигнал

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

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

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

где
δ - малая величина, близкая к нулю.

Элемент слежения-хранения 82 сохраняет значение q₄ неизменным при q₅__→ 0, когда на вход делителя блока деления 15 поступает близкий к нулю сигнал, т.е. когда этот блок может работать с недопустимой погрешностью.

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

на вектор b = (b_x, b_y, b_z).

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

Первый и второй положительные входы сумматора 86 имеют единичные коэффициенты усиления. В результате на выходе блока 38 умножения формируется сигнал b_x•(cosq₁•sinq₄ + sinq₁•sinφ•cosq₄).
Первый отрицательный и второй положительный входы сумматора 88 имеют единичные коэффициенты усиления. В результате на выходе блока 36 умножения формируется сигнал b_y•(sinq₁•sinq₄ - cosq₁•sinφ•cosq₄). На выходе блока 33 умножения формируется сигнал b_z•cosφ•cosq₄.
Все три положительных входа сумматора 37 имеют единичные коэффициенты усиления. Функциональный преобразователь 31 реализует функцию arccos. В результате на его выходе формируется сигнал, равный

Этот сигнал определяет только абсолютную величину обобщенной координаты q₆.

Для определения направления изменения координаты q₆ можно воспользоваться векторным произведением векторов e₅ и b. Если вектор γ = e₅ b совпадает с вектором a, то направление изменения q₆ является положительным, а в противном случае - отрицательным.

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

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

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

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

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

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

.

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

которые поступают на следящие приводы соответствующих степеней подвижности манипулятора и обеспечивают перемещение точки P схвата в трехмерном пространстве по заданной вектором r(t) траектории с заданной векторами a(t) и b(t) ориентацией схвата.

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

Claims (1)

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

Images