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

 

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Первый положительный вход сумматора 2 имеет единичный коэффициент усиления, его второй отрицательный вход - коэффициент усиления L₄. В результате на выходе этого сумматора формируется сигнал q₁= R_x= r_x-L₄·a_x.

Первые отрицательные входы сумматоров 38 и 40 имеют коэффициенты усиления L₄, а их вторые положительные входы - коэффициенты усиления, равные единице. В результате на выходах этих сумматоров соответственно будут формироваться сигналы, равные R_y = r_y - L₄a_y и R_z = r_z - L₄a_z.

Источник постоянного сигнала 41 вырабатывает сигнал L₁. Оба выхода сумматора 42 имеют единичные коэффициенты усиления, в результате на выходе этого сумматора вырабатывается сигнал, равный (R_z - L₁). На выходе блока деления 39 формируется сигнал R_y/(R_z-L₁). Источник постоянного сигнала 64 вырабатывает сигнал f₂ = -L₂/L₃. На выходе блока умножения 43 формируется сигнал (R_z - L₁) · f₂. Квадратор 44 имеет коэффициент усиления 1/L²₃. В результате на его выходе имеем сигнал На выходе блока умножения 71 имеем сигнал R_y · f₂. Квадратор 72 имеет коэффициент усиления 1/L²₃. В результате на его выходе формируется сигнал Первый и второй положительные входы сумматора 45 имеют коэффициенты усиления 4, поэтому на его выходе имеем сигнал -g₁² + g₂² = 4 (f₁² · f₂² + f₂² · f₃²). Функциональный преобразователь 46 реализует функцию извлечения квадратного корня. В результате на его выходе формируется сигнал

Квадратор 65 имеет единичный коэффициент усиления. В результате на его выходе формируется сигнал f₂². Квадратор 68 имеет коэффициент усиления 1/L₃². Поэтому на его выходе формируется сигнал

Источник постоянного сигнала 67 имеет на выходе единичный сигнал.

Первый, третий отрицательные входы сумматора 66 и второй положительный его вход имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал, равный (1 - f₂² - f₁²). Квадратор 70 имеет коэффициент усиления 1/L₃². В результате на его выходе получается сигнал

Первый положительный и второй отрицательный входы сумматора 69 имеют единичные коэффициенты усиления. Поэтому на его выходе формируется сигнал g₃ = 1 - f₁² - f₂² - f₃², а на выходе блока деления 47 сигнал Функциональный преобразователь 48 реализует функцию arcsin. Поэтому на его выходе формируется сигнал Функциональный преобразователь 75 реализует функцию , причем arctg принимает значения от -π до 0 (см. фиг. 3). Сумматор 49 имеет первый положительный и второй отрицательный входы с единичными коэффициентами усиления. На его выходе формируется сигнал

Функциональный преобразователь 50 реализует функцию sin. На выходе блока умножения 51 реализуется сигнал f₂·sin q₂. Первый положительный вход сумматора 52 имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления, равный 1/L₃, поэтому на его выходе формируется сигнал f₁ + f₂·sin q₂. Функциональный преобразователь 53 реализует функцию arcsin.

Первый положительный и второй отрицательный входы сумматора 54 имеют единичные коэффициенты усиления. На выходе этого сумматора формируется сигнал q₃ = arcsin (f₁ + f₂ · sin q₂) - q₂. Первый и второй положительные входы сумматора 55 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора формируется сигнал ϕ = q₂ + q₃. Функциональные преобразователи 31 и 56 реализует функции cos и sin соответственно. На выходе блока умножения 32 формируется сигнал a_ycos ϕ. Первый и второй положительные входы сумматора 33 имеют единичные коэффициенты усиления. На его выходе формируется сигнал a_y · cos ϕ + a_z · sin ϕ.
Функциональный преобразователь 35 реализует функцию arccos. На его выходе имеем сигнал arccos (a_y · cos ϕ + a_z · sin ϕ). Этот сигнал определяет лишь абсолютную величину обобщенной координаты q₅. Для определения знака q₅ воспользуемся следующим фактом. Если угол между положительным направлением оси Y и проекцией вектора а на плоскость XY больше, чем угол между тем же направлением оси Y и проекцией вектора R на плоскость XY, то q₅ имеет положительное значение, а в противном случае - отрицательное.

На выходе блока 17 деления формируется сигнал a_x/a_y. Функциональный преобразователь 18 реализует функцию arctg. При a_y --> 0 сигнал на выходе блока 17 стремится к максимальному возможному значению. При этом функциональный преобразователь 18 настраивается так, чтобы при a_y --> 0 в зависимости от знака a_x, на его выходе был бы сигнал π/2 или ( -π/2). При формировании сигнала a_y блоком 1 следует избегать a_y = 0 точно.

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

В результате на выходе блока 7 умножения формируется сигнал
q₅ = ±arccos(a_ycosϕ-a_zsinϕ).
Функциональный преобразователь 8 реализует функцию (- sin). Блок 9 деления имеет на выходе сигнал

Функциональный преобразователь 10 реализует функцию arccos. В результате на его выходе сигнал определяет абсолютную величину координаты q₄. Определение знака обобщенной координаты q₄ осуществляется при сравнении координаты вектора e_6z = a_z со значением координаты того же вектора e₆ при q₄ = 0, т.е. e_6z⁰. При положительном знаке q₅ обобщенная координата q₄ имеет положительный знак, если e_6z⁰ ≅ a_z, в противном случае - отрицательный. При отрицательном знаке q₅ обобщенная координата q₄ имеет положительный знак, если e_6z ≥ a_z, в противном случае - отрицательный. Функциональный преобразователь 73 реализует функцию cos. На выходе блока 57 умножения формируется сигнал
sinϕcosq₅ = e⁰_6z.
Первый отрицательный и второй положительный входы сумматора 58 имеют единичные коэффициенты усиления. На выходе этого сумматора формируется сигнал k = a_z - e_6z⁰. Этот сигнал поступает на вход релейного элемента 59, на выходе которого реализует сигнал

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

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

Элемент слежения - хранения 62 обеспечивает работоспособность устройства при q₅ --> 0, когда на вход делителя блока 9 деления поступает близкий к нулю сигнал, т. е. когда этот блок может работать с недопустимо большой погрешностью. При q₅ --> 0 элемент 62 сохраняет значение q₄ неизменным до тех пор, пока q₅ находится в опасной для блока 9 деления зоне. Управляется элемент 62 релейным элементом 63, имеющем следующему характеристику

где Δ малая положительная величина.

Элемент 62 находится в режиме хранения, когда |sinq₅|<Δ, и в режиме слежения, когда |sinq₅|>Δ.
Обобщенную координату q₆ можно определить из скалярного произведения векторов e₅ = (sin q₄, - cos q₄ · sin ϕ, cos q₄ · cos ϕ )^T и b = (b_x, b_y, b_z)^T.

Функциональный преобразователь 29 реализует функцию sin. На выходе блока 30 умножения формируется сигнал, равный b_x · sin q₄. Функциональный преобразователь 11 реализует функцию cos. На выходе блока 12 умножения формируется сигнал cos q₄ sin ϕ, а на выходе блока 13 умножения - сигнал b_y cos q₄ sin ϕ. На выходе блока 36 умножения формируется сигнал cos ϕ cos q₄, а на выходе блока 37 умножения сигнал b_z cos ϕ cos q₄. Первый отрицательный, второй и третий положительные входы сумматора 14 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора формируется сигнал (b_xsin q₄ - sin ϕ cos q₄b_y + cos ϕ cos q₄b_z). Функциональный преобразователь 15 реализует функцию arccos. На его выходе формируется сигнал, равный абсолютному значению обобщенной координаты q₆.

Для определения знака q₆ воспользуемся векторным произведением векторов e₅ и b. Если совпадает с вектором a, то q₆ положительна, а в противном случае - отрицательна. В силу введенных ограничений на обобщенные координаты манипулятора невозможно одновременное обнуление проекций векторов γ и a на оси координат X и Y. В результате о совпадении направлений векторов γ и a можно судить, анализируя только две их проекции: на оси x и y. На выходе блока 25 умножения формируется сигнал sin ϕ cos q₄b_z.

На выходе сумматора 26, имеющего на первом и втором отрицательных входах единичные коэффициенты усиления, формируется сигнал γ_x = -cos q₄sin ϕ b_z - b_y cos q₄cos ϕ, а на выходе блока 28 умножения - сигнал γ_x·a_x.
На выходе сумматора 23, имеющего на первом отрицательном и втором положительном входах единичные коэффициенты усиления, формируется сигнал γ_y = b_x cos q₄cos ϕ - sin q₄b_z, а на выходе блока 19 умножения формируется сигнал γ_y·a_y. Первый и второй положительные входы сумматора 20 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал β = γ_x·a_x+γ_y·a_y. Если сигнал β положительный, то q₆ имеет знак "плюс", в противном случае - "минус", т.к. на выходе релейного элемента 21 формируется сигнал

В результате на выходе блока 16 умножения формируется сигнал
q₆ = ± arccos (b_xsin q₄ - sin ϕ cos q₄b_y + cos ϕ q₄b_z).

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

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

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

РИСУНКИ

Рисунок 1, Рисунок 2, Рисунок 3