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

 

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Из фиг. 1 видно, что всегда выполняется равенство R = r - L₃e₆ = r - L₃a.

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

Первые отрицательные входы сумматоров 38 и 39 имеют коэффициенты усиления L₃, а их вторые положительные входы - коэффициенты усиления, равные единице. В результате на выходах этих сумматоров, соответственно, будут формироваться сигналы, равные R_y = r_y - L₃·a_y и R_z - L₁ = r_z - L₃a_z - L₁, т.к. с источника постоянного сигнала 75 на третий отрицательный единичный вход сумматора 39 поступает сигнал L₁. Функциональный преобразователь 4 реализует функцию arctg. В результате на выходе этого функционального преобразователя формируется сигнал Причем R_y ≠ 0 в силу введенных ранее ограничений на q₁.

Функциональный преобразователь 40 реализует функцию sin, а функциональный преобразователь 49 - функцию arctg. В результате на выходе функционального преобразователя 49 будет формироваться сигнал Поскольку R_x может принимать значение R_x = 0, то в этом случае необходимо исключать ситуацию, когда аргумент arctg становится равным ∞. Это удается сделать с помощью релейного элемента 51, имеющего характеристику где Δ₁ малая положительная величина.

Когда элемент слежения-хранения 50 по нулевому сигналу с релейного элемента 51 сохраняет последнее значение q₂ до тех пор, пока при движении манипулятора опять не станет ≥Δ₁.

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

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

Если sin q₂ ---> 0 релейный элемент 57 нулевым сигналом сохраняет в элементе 58 постоянное значение ранее вычисленной величины q₃ до тех пор пока значение не превысит Δ₂. При значение q₃ точно соответствует текущему (истинному) значению.

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

Поскольку второй отрицательный, а первый и третий положительные входы сумматора 33 имеют единичные коэффициенты усиления, функциональный преобразователь 35 реализует функцию arccos, то на выходе этого функционального преобразователя будет формироваться сигнал
arccos(a_zsinq₂ + a_ycosq₁cosq₂ - a_xsinq₁cosq₂).

Этот сигнал определяет лишь абсолютную величину координаты 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 на его выходе был бы сигнал .

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

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

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

В результате на выходе блока 7 умножения формируется сигнал
q₅ = ±arccos(a_zsinq₂ + a_ycosq₁cosq₂ - a_xsinq₁cosq₂).

Функциональные преобразователи 8 и 45 реализует функцию sin. На выходе блоков умножения 43 и 46 соответственно формируются сигналы a_xcosq₁ и a_ysinq₁. Первый и второй отрицательные входы сумматора 44 имеют единичные коэффициенты усиления. Функциональный преобразователь 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, в противном случае - отрицательный.

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

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

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

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

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

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

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

Функциональный преобразователь 11 реализует функцию cos. В результате на выходе блока умножения 72 формируется сигнал sinq₁sinq₂cosq₄. Функциональный преобразователь 29 реализует функцию sin. В результате на выходе блока умножения 30 формируется сигнал cosq₁sinq₄. Поскольку все положительные входы сумматора 73 имеют единичные коэффициенты усиления, то на выходе блока умножения 74 формируется сигнал (cosq₁sinq₄ + sinq₁sinq₂cosq₄)b_x. На выходе блока умножения 68 формируется сигнал sinq₁sinq₄, а на выходе блока умножения 37 - сигнал cosq₁sinq₂cosq₄. Поскольку первый положительный и второй отрицательный входы сумматора 69 имеют единичные коэффициенту усиления, то на выходе блока умножения 70 формируется сигнал (sinq₁sinq₄ - cosq₁sinq₂cosq₄)b_y. На выходе блока умножения 13 формируется сигнал (cosq₂cosq₄)b_z.

Функциональный преобразователь 15 реализует функцию arccos, а все три положительные выходы сумматора 14 имеют единичные коэффициенты усиления. В результате на выходе преобразователя 15 будет сформирован сигнал
arccos((cosq₁sinq₄ + sinq₁sinq₂cosq₄)b_x + (sinq₁sinq₄ - cosq₁sinq₂cosq₄)b_y + cosq₂cosq₄b_z) = A,
равный абсолютному значению обобщенной координаты q₆.

Для определения знака q₆ воспользуемся векторным произведением векторов e₅ и b. Если γ = e₅·b совпадает с вектором a, то q₆ положительна, а в противном случае - отрицательна. В силу введенных ограничений на обобщенные координаты манипулятора невозможно одновременное обнуление проекций векторов γ и a на оси координат X и Y. В результате о совпадении направлений векторов γ и a можно судить, анализируя только две их проекции: на оси X и Y. На выходе сумматора 26, первый положительный и второй отрицательный входы которого имеют единичные коэффициенты усиления, формируется сигнал
γ_x = (sinq₁sinq₄ - cosq₁sinq₂cosq₄)b_z - (cosq₂cosq₄)b_y,
а на выходе блока умножения 28 - сигнал γ_x·α_x.

На выходе сумматора 23, первый отрицательный и второй положительный входы которого имеют единичные коэффициенты усиления, формируется сигнал
γ_y = -(cosq₁sinq₄ + sinq₁sinq₂cosq₄)b_z + (cosq₂cosq₄)b_x,
а на выходе блока умножения 19 - сигнал γ_y·a_x.

Первый и второй положительные входы сумматора 20 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал β = γ_y·a_x+γ_ya_y..

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

В результате на выходе блока 16 формируется сигнал q₆ = ±A.

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

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

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

РИСУНКИ

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