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

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

Иллюстрации

Показать все

Реферат

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

Известно устройство для программного управления манипулятором, содержащее блок задания программы, последовательно соединенные элемент И и коммутатор, а также по каждой регулируемой координате привод, связанный с датчиком положения, выход каждого из которых подключен к соответствующему входу элемента И. Кроме того, оно содержит датчик технологического оборудования, реверсивный счетчик по каждой регулируемой координате триггер, выход которого подключен к выходу привода, а вход каждого из которых к соответствующему первому выходу блока задания программы, соединенного входами с выходами реверсивного счетчика и вторым выходом и входом датчика технологического оборудования, первый и второй выходы которого подключены соответственно к второму и третьему входам коммутатора, соединенного первым и вторым выходами с соответствующими входами реверсивного счетчика, третий вход которого подключен к выходу элемента И (А.с. СССР №1442392, кл. 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, десятый функциональный преобразователь 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, первый блок 46 извлечения квадратного корня, двенадцатый сумматор 47, второй вход которого подключен к выходу первого задатчика 48 постоянного сигнала, шестнадцатый блок 49 умножения, второй вход которого подключен к выходу пятого блока 50 деления, и первый ключ 51, выход которого является входом привода седьмой степени подвижности манипулятора, последовательно соединенные шестой блок 52 деления, семнадцатый блок 53 умножения, второй вход которого подключен к выходу двенадцатого сумматора 47, и второй ключ 54, выход которого является входом привода восьмой степени подвижности манипулятора, последовательно соединенные третий квадратор 55, тринадцатый сумматор 56, второй вход которого подключен к выходу четвертого квадратора 57, второй блок 58 извлечения квадратного корня, четырнадцатый сумматор 59, второй вход которого подключен к выходу первого блока 46 извлечения квадратного корня и к первым входам пятого 50 и шестого 52 блоков деления, и пороговый элемент 60, выход которого подключен к управляющим входам первого 51 и второго 54 ключа, третьего ключа 61, выход которого подключен к первому входу первого блока 3 деления, а второй сигнальный вход через пятнадцатый сумматор 62 - к выходу первого сумматора 2, к входу первого квадратора 43 и к второму входу пятого блока 50 деления, четвертого ключа 63, выход которого подключен к первому входу пятого сумматора 20, а второй сигнальный вход через шестнадцатый сумматор 64 - к выходу четвертого сумматора 19, к входу второго квадратора 45 и к второму входу шестого блока 52 деления, и пятого ключа 65, второй и третий сигнальные входы которого подключены соответственно к выходам второго задатчика 66 постоянного сигнала и третьего блока 21 деления, а выход является входом привода третьей степени подвижности манипулятора, причем вход третьего 55 квадратора подключен к выходу привода седьмой степени подвижности манипулятора и к второму входу пятнадцатого сумматора 62, а вход четвертого 57 квадратора - к выходу привода восьмой степени подвижности манипулятора и к второму входу шестнадцатого сумматора 64.

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

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

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

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

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

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

На выходе блока 46 формируется сигнал который определяет программное расстояние от начала АСК, в которой задается вектор r, до проекции точки О манипулятора на горизонтальную плоскость, в которой располагается СК ХУ. На выходе задатчика 48 формируется постоянный сигнал, равный L₁+q_3C, где q_3C - некоторое среднее значение, которое может принимать обобщенная координата q₃ манипулятора. В рзультате на выходе блоков 49 и 53 соответственно формируются сигналы и которые в СК ХУ задают желаемую координату основания (вертикального звена) манипулятора, когда q₃=q_3C.

На выходе блока 58 формируется сигнал который определяет реальное положение основания манипулятора в СК ХУ (сигналы q_7П и q_8П соответствуют реальным перемещениям в соответствующих степенях подвижности манипулятора). Если объекты работ все время находятся в рабочей зоне манипулятора, то его основание всегда покоится и

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

Релейный элемент 60 имеет характеристику

где величина К>0, определяемая кинематическими особенностями манипулятора, характеризует нахождение объектов работ в его рабочей зоне. Если объекты работ в текущий момент времени находятся в рабочей зоне манипулятора (сигналы r_x и r_y достаточно малы), то m<К и на выходе элемента 60 формируется нулевой сигнал, который приводит к размыканию ключей 51 и 54, замыканию ключей 61 и 63 и замыканию ключа 65 на вход блока 21. В этом случае q₇=q₈=0 и на выходах приводов седьмой и восьмой степеней подвижности манипулятора q_7П=q_8П=0.

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

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

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

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

Первый положительный (со стороны функционального преобразователя 37) и второй отрицательный входы сумматора 38 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал α, равный разности углов, которые составляют с положительным направлением оси Y соответственно проекции вектора А и 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₆ определяется из скалярного произведения вектора е₅=(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₆ используется векторное произведение векторов е₅ и В. Если вектор γ=е₅×В совпадает с вектором А, то направление изменения q₆ является положительным, а в противном случае отрицательным. В силу введенных ограничений на обобщенные координаты манипулятора невозможно одновременное обнуление проекций векторов γ и А на оси координат X и Y. В результате о совпадении направлений векторов γ и А можно судить, анализируя только их проекции на оси X и Y.

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

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

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

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

Если сигналы R_x и/или R_y становятся такими, при которых манипулятору не удается выполнить требуемые рабочие операции с объектами при покоящемся основании (объекты работ начинают находиться вне рабочей зоны манипулятора), то сигнал m на выходе сумматора 59 начинает превышать пороговое значение К. В результате на выходе релейного элемента 60 появляется ненулевой сигнал, который приведет к замыканию ключей 51, 54, размыканию ключей 61, 63 и замыканию входа ключа 65 на выход задатчика 66. После этого начнутся соответствующие перемещения в степенях 7 и 8 подвижности манипулятора и его основание переместится в новое положение q_7П и q_8П, определяемое сигналами q₇ и q₈, сформированными на выходах ключей 51 и 54. При этом координата q₃ для удобства последующей настройки требуемого положение и ориентации схвата манипулятора примет некоторое среднее значение q_3С, формируемое на выходе задатчика 66.

После того как основание манипулятора займет новое положение в плоскости ХУ, позволяющее расположить объект работ в рабочей зоне этого манипулятора, то на выходе сумматора 59 появится сигнал m<К, который приведет к обнулению сигнала на выходе релейного элемента 60. Это приведет к размыканию ключей 51, 54, замыканию ключей 61, 63 и замыканию входа ключа 66 на выход блока 21. После этого рабочие манипуляционные операции будут продолжены по уже описанному выше алгоритму.

Если значения сигналов R_x и/или R_y опять станут такими, при которых новый объект работ не будет находиться из рабочей зоне манипулятора, то на выходе релейного элемента 60 опять появится ненулевой сигнал m, который приведет к замыканию ключей 51, 54, размыканию ключей 61, 63 и замыканию входа ключа 66 на выход задатчика 66.

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

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

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

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