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

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

 

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

Известно устройство для программного управления манипулятором, содержащее блок задания программы, последовательно соединенные элемент И и коммутатор, а также по каждой регулируемой координате привод, связанный с датчиком положения, выход каждого из которых подключен к соответствующему входу элемента И. Кроме того, оно содержит датчик технологического оборудования, реверсивный счетчик по каждой регулируемой координате триггер, выход которого подключен к выходу привода, а вход каждого из которых к соответствующему первому выходу блока задания программы, соединенного входами с выходами реверсивного счетчика и вторым выходом и входом датчика технологического оборудования, первый и второй выходы которого подключены соответственно к второму и третьему входам коммутатора, соединенного первым и вторым выходами с соответствующими входами реверсивного счетчика, третий вход которого подключен к выходу элемента И (А.с. СССР №1442392, кл. B25J 9/00, B25J 9/16; 1988).

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

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

Это устройство по своей технической сущности является наиболее близким к предлагаемому изобретению и принято за прототип.

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

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

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

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

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

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

На фиг. 1 представлена блок-схема предлагаемого устройства управления; на фиг. 2 изображена кинематическая схема манипулятора (эта схема соответствует кинематической схеме типового манипулятора).

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

На чертежах введены следующие обозначения: q_i - обобщенная координата степени подвижности i манипулятора, это управляющие координаты соответствующих приводов манипулятора ; r - вектор, задающий положение характерной точки схвата P в трехмерном пространстве, определяемом абсолютной системой координат (АСК) xyz; r_x, r_y и r_z координаты вектора r (характерной точки P схвата манипулятора) в АСК; R - вектор, определяющий положение шарнира (точка O) пятой степени подвижности (координата q₅) в АСК; R_x, R_y и R_z - координаты вектора R в АСК; L₁, L₂ длины соответствующих звеньев манипулятора; A, B - единичные векторы, распроложенные в плоскости схвата и определяющие его ориентацию в АСК; a_x, a_y, a_z, b_x, b_y, b_z - соответствующие координаты векторов A и B в АСК; e_i - единичные векторы, совпадающие с осями шарниров и призматических пар степеней подвижности i манипулятора

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

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

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

Отсчет обобщенных координат манипулятора начинается из его положения, показанного на фиг. 2. На некоторые обобщенные координаты накладываются ограничения , , , -π<q₆<π. Движение против часовой стрелки вращательных степеней считается положительным. Направление вращения определяется относительно соответствующих векторов е_i, если взгляд направлен от стрелки к основанию вектора e_i. Из фиг. 2 видно, что всегда выполняется равенство

Первый отрицательный вход сумматора 2 имеет единичный коэффициент усиления, а его второй положительный вход коэффициент усиления, равный L₂. В результате на выходе этого сумматора формируется сигал R_x. Первые положительные входы сумматоров 19 и 42 имеют единичные коэффициенты усиления, а их вторые отрицательные входы коэффициенты усиления, равные L₂. В результате на выходах этих сумматоров согласно выражению (1), соответственно, формируются сигналы, равные R_y и R_z. Причем q₂=R_z.

Если объекты работ все время находятся в рабочей зоне манипулятора, то его основание всегда покоится и совпадает с центром АСК, ключ 57 замкнут, ключ 50 разомкнут, ключ 58 соединяет выход блока 21 со входом третьей степени подвижности, a q₇=q_7П=0.

По координате Y все объекты работ всегда находятся в рабочей зоне манипулятора, определяемой его кинематической схемой (фиг. 2), а по координате X эти объекты могут находиться на значительном расстоянии от начала АСК. Поэтому для выполнения требуемых рабочих операций основание манипулятора может быть перемещено по координате q₇ к объекту работ. При формировании этого перемещения должен учитываться факт, что после его окончания манипулятор должен быть способен выполнить все требуемые виды работ с объектами, находящимися после перемещения его основания по оси X в его рабочей зоне с требуемой ориентацией рабочих инструментов.

На выходе блока 46 формируется сигнал , который определяет программное расстояние от начала АСК, в которой задается вектор r, до проекции точки O манипулятора на горизонтальную плоскость XY. На выходе задатчика 48 формируется постоянный сигнал, равный L₁+q_3C, где q_3C - некоторое значение (часто среднее), которое может принимать обобщенная координата q₃ манипулятора. В результате на выходе блока 49 формируются сигналы , которые на оси X задают желаемую координату основания (вертикального звена) манипулятора для его перемещения по этой оси к объекту, когда q₃=q_3C. При этом учитывается, что R_x>R_y.

На выходе блока 55 формируется сигнал , а на выходе сумматора 54, первый положительный (со стороны блока 46) и второй отрицательный входы которого имеют единичные коэффициенты усиления - сигнал , который оценивает текущее расстояние от основания манипулятора до проекции точки O на горизонтальную плоскость АСК (сигнал q_7П соответствует реальному текущему положению седьмой степеней подвижности манипулятора, определяемому по выходной координате привода этой степени подвижности).

В начале цикла работы манипулятора q_7П=0, и если объект работ находится в рабочей зоне манипулятора (сигнал r_x достаточно мал), то на выходе привода седьмой степени подвижности формируется сигнал q_7П=0, ключ 57 замкнут, ключ 50 разомкнут, а ключ 57 замыкается на вход блока 21.

В этом случае устройство работает следующим образом.

На выходе функционального преобразователя 4 формируется сигнал, равный , причем R_y≠0 в силу введенных ограничений на q_i.

Первый положительный вход сумматора 20 (со стороны сумматора 19) имеет единичный коэффициент усиления, а его второй отрицательный вход - коэффициент усиления L₁. В результате на выходе блока 21 формируется сигнал . Причем cosq₁≠0 в силу введенных ранее ограничений на координату q₁.

Первый отрицательный (со стороны блока 6) и второй положительный входы сумматора 7 имеют единичные коэффициенты усиления. В результате на его выходе функционального преобразователя 8 формируется сигнал arccos(a_ycosq₁-a_xsinq₁), который определяет абсолютную величину изменения координаты q₅, но не определяет ее направления. Для определения знака q₅ учтен следующий факт. Если угол между положительным направлением оси Y и проекцией вектора A на плоскость XY больше, чем угол между тем же направлением оси Y и проекций вектора R на плоскость XY, то q₅ имеет положительное значение, а в противном случае отрицательное.

На выходе блока 36 формируется сигнал a_x/a_y. Если a_y→0, то этот сигнал стремится к максимально возможному. При этом, функциональный преобразователь 37 настраивается так, чтобы при a_y→0 в зависимости от знака a_x на его выходе формировался сигнал или . Очевидно, что максимально возможный сигнал на выходе блока 36 будет определять статическую точность манипулятора при a_y→0. При формировании сигнала a_y блоком 1 необходимо избегать a_y=0.

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

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

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

На выходе блока 11 формируется сигнал a_z/sinq₅, а на выходе функционального преобразователя 12 - сигнал q₄=arcsin(a_z/sinq₅). Поскольку обнуление sinq₅ возможно только тогда, когда одновременно a_z=0 (фиг. 2), то зоны нечувствительности входов блока 11 должны настраиваться с учетом этого факта. При этом, при a_z=sinq₅≅0 на выходе блока 11 должен формироваться нулевой сигнал.

Обобщенная координата q₆ определяется из скалярного произведения вектора e₅=(cosq₁sinq₄, sinq₃sinq₄, cosq₄)^T на вектор B=(b_x, b_y, b_z)^T.

На выходе блока 41 формируется сигнал b_ysinq₁sinq₄, на выходе блока 15 - сигнал b_xcosq₁sinq₄, а на выходе блока 40 - сигнал b_zcosq₄. Все положительные входы сумматора 16 имеют единичные коэффициенты усиления. В результате на выходе функционального преобразователя 17 формируется сигнал, равный arccos(b_xcosq₁sinq₄+b_ysinq₁sinq₄+b_zcosq₄).

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

На выходе блока 35 формируется сигнал b_ycosq₄, а на выходе блока 25 сигнал b_zsinq₁sinq₄. Первый положительный (со стороны блока 25) и второй отрицательный входы сумматора 26 имеют единичные коэффициенты усиления, поэтому на выходе этого сумматора формируется сигнал γ_x=b_zsinq₁sinq₄-b_ycosq₄, а на выходе блока 17 - сигнал γ_xa_x. Первый отрицательный (со стороны блока 31) и второй положительный входы сумматора 32 имеют единичные коэффициенты усиления. В результате на выходе этого сумматора формируется сигнал γ_y=b_xcosq₄-b_zcosq₁sinq₄, а на выходе блока 29 - сигнал γ_ya_y.

На выходе сумматора 28, имеющего положительные входы с единичными коэффициентами усиления формируется сигнал β=γ_xa_x+γ_ya_y. Если этот сигнал положительный, то угол q₆ положителен, в противном случае он отрицателен.

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

В результате на выходе блока 18 формируется требуемое значение сигнала q₆=±arccos(b_xcosq₁sinq₄+b_ysinq₁sinq₄+b_zcosq₄).

Если сигнал R_X становится таким, при котором манипулятору не удается выполнить требуемые рабочие операции с объектами при покоящемся основании (объекты работ начинают находиться вне рабочей зоны манипулятора), то сигнал начинает превышать некоторое пороговое значение K, определяемое кинематическими особенностями манипулятора. В результате на выходе релейного элемента 56, имеющего характеристику

появляется ненулевой сигнал, который приведет к замыканию ключа 50, размыканию ключа 57 и замыканию входа ключа 58 на выход задатчика 59. В результате начнется соответствующее перемещение в степени 7 подвижности манипулятора и его основание переместится в новое положение q_7П, определяемое сигналом q₇, сформированным на выходе ключа 50. При этом координата q₃ для удобства последующей настройки требуемого положение и ориентации схвата манипулятора примет некоторое среднее значение q_3C, формируемое на выходе задатчика 59.

После того, как основание манипулятора займет новое положение вдоль оси X, позволяющее расположить объект работ в рабочей зоне этого манипулятора, сигнал на выходе сумматора 54 станет таким, что на выходе релейного элемента 56 сигнал обнулится. Это приведет к размыканию ключа 50, замыканию ключа 57 и замыканию входа ключа 58 на выход блока 21. После этого рабочие манипуляционные операции будут продолжены по уже описанному выше алгоритму, но при этом по сигналу о текущем значении q_7П, поступающему на вход блока 1 задания программы, параметры векторов A и B, формирующиеся на соответствующих выходах этого блока, автоматически должны быть скорректированы в соответствии со значением q_7П.

Если, учитывая текущее значение сигнала R_X, объект работ опять выйдет из рабочей зоны манипулятора, то на выходе релейного элемента 56 опять появится ненулевой сигнал, который приведет к замыканию ключа 50, размыканию ключа 57 и замыканию входа ключа 58 на выход задатчика 59.

Указанная выше процедура будет повторяться до выполнения манипулятором всех работ, определяемых блоком 1 задания программы.

Таким образом, с помощью предложенного устройства полностью удалось решить обратную задачу кинематики для манипулятора, изображенного на фиг. 2, т.е. удалось сформировать все сигналы которые поступают на следящие приводы соответствующих степеней подвижности манипулятора и обеспечивают перемещение точки p схвата в трехмерном пространстве по заданной вектором r(t) траектории с заданной векторами A(t) и B(t)ориентацией схвата.

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

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