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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

На этих чертежах введены следующие обозначения: 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 - единичные векторы, совпадающие с осями шарниров степеней подвижности манипулятора

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

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

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

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

R = r - L₄e₆ = r - L₄a. (1)

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

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

Усилитель 43 и инвертор 53 имеют коэффициенты усиления 2L₂/L₃². В результате на их выходах, соответственно, формируются сигналы и Функциональный преобразователь 45 реализует функцию arctg. Причем arctg принимает значение от - до 0 (см. фиг. 3). На выходе источника постоянного сигнала 58 формируется сигнал 1-L₂²/L₃². Первый (со стороны квадратора 57) и третий (со стороны квадратора 59) отрицательные входы сумматора 38 имеют коэффициенты усиления 1/L₃², а его второй положительный вход - единичный коэффициент усиления. В результате на входе этого сумматора формируется сигнал Положительные входы сумматора 55 имеют единичные коэффициенты усиления, а функциональный преобразователь 57 реализует функцию корня квадратного. В результате на выходе этого функционального преобразователя будет формироваться сигнал а на выходе функционального преобразователя 47, реализующего функцию arcsin - сигнал Поскольку первый отрицательный и второй положительный входы сумматора 46 имеют одиночные коэффициенты усиления, то на его выходе будет формироваться сигнал



Функциональный преобразователь 48 реализует функцию sin. Первый (со стороны функционального преобразователя 48) и второй отрицательные входы сумматора 49 имеют, соответственно, коэффициенты усиления L₂/L₃ и 1/L₃. В результате на его выходе формируется сигнал - на выходе функционального преобразователя 50, реализующего функцию arcsin - сигнал на входе сумматора 51, имеющего первый положительный и второй отрицательный входы с единичными коэффициентами усиления, - сигнал а на выходе сумматора 52, положительные входы которого также имеют единичные коэффициенты усиления, - сигнал = q₂+q₃.

Функциональные преобразователи 31 и 42 реализуют функции sin и cos, соответственно. В результате на выходе сумматора 33, имеющего первый отрицательный и второй положительный входы с единичными коэффициентами усиления, формируется сигнал a_ycos-a_xsin, а на выходе функционального преобразователя 35, реализующего функцию arccos - сигнал arccos(a_ycos-a_xsin). Этот сигнал определяет лишь абсолютную величину обобщенной координаты q₅. Для определения знака q₅ воспользуемся следующим фактором. Если угол между положительным направлением оси Y и проекцией вектора a на плоскость 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_xsin).

Функциональный преобразователь 8 реализует функцию sin. Блок 9 деления имеет на выходе сигнал Функциональный преобразователь 10 реализует функцию arcsin. В результате на его выходе будет формироваться сигнал q₄ = arcsin(a_z/sinq₅).

Обобщенную координату q₆ можно определять из скалярного произведения векторов e₅= (cossinq₄,sinsinq₄,cosq₄) и b = (b_x, b_y, b_z).

Функциональный преобразователь 29 реализует функцию cos. В результате на выходе блока умножения 30 формируется сигнал b_zcosq₄. Функциональный преобразователь 11 реализует функцию sin. В результате на выходе блока умножения 13 формируется сигнал b_xsinq₄cos, а на выходе блока умножения 37 сигнал Сумматор 14 на всех трех положительных выходах имеет единичные коэффициенты усиления, а функциональный преобразователь 15 реализует функцию arccos. В результате на его выходе формируется сигнал arccos(b_xsinq₄cos+b_ysinq₄sin+b_zcosq₄), равный абсолютному значению обобщенной координаты q₆.

Для определения знака q₆ воспользуемся векторным произведением векторов e₅ и b. Если = e₅b совпадает с вектором a, то q₆ положительна, а в противном случае - отрицательна. В силу введенных ограничений на обобщенные координаты манипулятора невозможно одновременное обнуление проекций векторов и a на оси координат X и Y. В результате о совпадении направлений векторов и а можно судить, анализируя только две их проекции: на оси X и Y.

На выходе блока умножения 27 формируется сигнал b_ycosq₄, а на выходе блока умножения 25 - сигнал b_zsinq₄sin. В результате на выходе сумматора 26, имеющего первый положительный и второй отрицательный входы с единичными коэффициентами усиления формируется сигнал _x= b_zsinq₄sin-b_ycosq₄.

На выходе блока умножения 22 формируется сигнал b_xcosq₄, а на выходе блока умножения 24 - сигнал b_zsinq₄cos. В результате на выходе сумматора 23, имеющего первый положительный и второй отрицательный входы с единичными коэффициентами усиления формируется сигнал _y= b_zcosq₄-b_zsinq₄cos. На выходе блоков умножения 19 и 28 формируются сигналы a_xx и a_yy, соответственно. Первый и второй положительные входы сумматора 20 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал = _xa_x+_ya_y. Если сигнал положительный, то q₆ имеет знак "плюс", в противном случае - "минус", т.к. на выходе релейного элемента 21 формируется сигнал



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

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