RU2792279C1 - Устройство для программного управления манипулятором с шестью степенями подвижности и линейно перемещаемым основанием - Google Patents

Abstract

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

Description

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

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

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

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

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

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

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

Поставленная задача решается тем, что в устройство для программного управления манипулятором с шестью степенями подвижности и линейно перемещаемым основанием, содержащее блок задания программы, последовательно соединенные первый сумматор, первый вход которого подключен к первому выходу блока задания программы со значением проекции r_x на ось х абсолютной системы координат (АСК) вектора r, определяющего текущее положение характерной точки Р схвата манипулятора в этой АСК, а второй вход - ко второму выходу блока задания программы со значением проекции а_х на ось х АСК единичного вектора а, расположенного в плоскости схвата манипулятора и определяющего его ориентацию в АСК, первый блок деления, первый функциональный преобразователь, имеющий функцию преобразования в виде «arctg», второй сумматор, первый релейный элемент, первый блок умножения, второй функциональный преобразователь, имеющий функцию преобразования в виде «sin», второй блок деления, третий функциональный преобразователь, имеющий функцию преобразования в виде «arcsin», четвертый функциональный преобразователь, имеющий функцию преобразования в виде «sin», второй и третий блоки умножения, третий сумматор, пятый функциональный преобразователь, имеющий функцию преобразования в виде «arccos», и четвертый блок умножения, последовательно соединенные третий блок деления и шестой функциональный преобразователь, имеющий функцию преобразования в виде «arctg», выход которого соединен со вторым входом второго сумматора, последовательно соединенный пятый блок умножения, четвертый сумматор и второй релейный элемент, выход которого соединен со вторым входом четвертого блока умножения, последовательно соединенные шестой блок умножения и пятый сумматор, выход которого подключен к первому входу пятого блока умножения, а его второй вход - к выходу седьмого блока умножения, последовательно соединенные восьмой блок умножения, шестой сумматор, второй вход которого через девятый блок умножения соединен с восьмым выходом блока задания программы со значением проекции b_y на ось у АСК единичного вектора b, расположенного в плоскости схвата манипулятора и определяющего его ориентацию в АСК, и десятый блок умножения, второй вход которого соединен со вторым выходом блока задания программы, вторым входом первого сумматора и первым входом третьего блока деления, а его выход - со вторым входом четвертого сумматора, последовательно соединенные седьмой функциональный преобразователь, имеющий функцию преобразования в виде «cos», вход которого подключен ко входу четвертого функционального преобразователя, и одиннадцатый блок умножения, последовательно соединенные восьмой функциональный преобразователь, имеющий функцию преобразования в виде «sin», двенадцатый блок умножения, второй вход которого подключен ко второму выходу блока задания программы, седьмой сумматор, второй вход которого подключен к выходу тринадцатого блока умножения, и девятый функциональный преобразователь, имеющий функцию преобразования в виде «arccos», выход которого соединен со вторым входом первого блока умножения, последовательно соединенные четырнадцатый блок умножения, первый вход которого соединен с выходом четвертого функционального преобразователя, а второй вход - с выходом восьмого функционального преобразователя, и пятнадцатый блок умножения, второй вход которого подключен к восьмому выходу блока задания программы, а его выход - ко второму входу третьего сумматора, третий вход которого соединен с выходом одиннадцатого блока умножения, последовательно соединенные восьмой сумматор и четвертый блок деления, девятый сумматор, первый вход которого соединен с третьим выходом блока задания программы со значением проекции а_у на ось у упомянутого вектора а и вторыми входами третьего блока деления, пятого и тринадцатого блоков умножения, его второй вход - с четвертым выходом блока задания программы со значением проекции r_y на ось у упомянутого вектора r, а выход - со вторым входом первого блока деления, десятый сумматор, первый вход которого соединен с пятым выходом блока задания программы со значением проекции a_z на ось z упомянутого вектора а и вторым входом второго блока деления, а его второй вход - с шестым выходом блока задания программы со значением проекции r_z на ось z упомянутого вектора r, а также десятый функциональный преобразователь, имеющий функцию преобразования в виде «cos», вход которого подключен ко входу восьмого функционального преобразователя, а выход - к первому входу тринадцатого блока умножения, причем первый вход одиннадцатого блока умножения подключен ко второму входу девятого блока умножения и первому входу шестого блока умножения, второй вход которого соединен с седьмым выходом блока задания программы со значением проекции b_x на ось х упомянутого вектора b, первый вход седьмого блока умножения подключен ко второму входу третьего блока умножения, а его второй вход - к первому входу восьмого блока умножения, второй вход одиннадцатого блока умножения соединен с девятым выходом блока задания программы со значением проекции b_z на ось z упомянутого вектора b, последовательно соединенные усилитель, пятый блок деления, одиннадцатый функциональный преобразователь, имеющий функцию преобразования в виде «arctg», одиннадцатый сумматор, второй вход которого через двенадцатый функциональный преобразователь, имеющий функцию преобразования в виде «arcsin», подключен к выходу четвертого блока деления, тринадцатый функциональный преобразователь, имеющий функцию преобразования в виде «sin», двенадцатый сумматор, четырнадцатый функциональный преобразователь, имеющий функцию преобразования в виде «arcsin», тринадцатый и четырнадцатый сумматоры, вторые входы которых подключены к выходу одиннадцатого сумматора, а выход четырнадцатого сумматора подключен ко входу восьмого функционального преобразователя, последовательно соединенные инвертор, первый квадратор, пятнадцатый сумматор, второй вход которого через второй квадратор подключен к первому входу пятого блока деления, второй вход которого соединен с выходом инвертора, и пятнадцатый функциональный преобразователь, имеющий функцию преобразования в виде вычисления корня квадратного, выход которого подключен ко второму входу четвертого блока деления, а также третий квадратор, вход которого подключен ко входу инвертора и выходу девятого сумматора, а выход - к первому входу восьмого сумматора, второй вход которого соединен с выходом первого источника постоянного сигнала, а третий вход через четвертый квадратор - со вторым входом двенадцатого сумматора, выходом первого сумматора и входом усилителя, а также шестнадцатый блок умножения, первый вход которого подключен к девятому выходу блока задания программы, его второй вход - к первому входу второго блока умножения, а выход - к первому входу восьмого блока умножения, второй вход которого соединен с выходом восьмого функционального преобразователя, причем второй вход второго блока умножения подключен к седьмому выходу блока задания программы, а выход десятого функционального преобразователя - ко второму входу третьего блока умножения, причем выходы десятого, одиннадцатого и тринадцатого сумматоров, третьего функционального преобразователя, первого и четвертого блоков умножения выполнены с возможностью подключения к соответствующим следящим приводам степеней подвижности манипулятора, дополнительно введены последовательно соединенные первый ключ, пятый квадратор, шестнадцатый сумматор, второй вход которого через шестой квадратор подключен к выходу девятого сумматора, блок взятия корня квадратного, семнадцатый сумматор, второй вход которого подключен к выходу второго источника постоянного сигнала, первый пороговый элемент и второй ключ, второй вход которого подключен к выходу первого ключа, причем третий вход первого сумматора выполнен с возможностью подключения к выходу датчика положения следящего привода линейного перемещения основания манипулятора, а вход - к выходу второго ключа, последовательно соединенные блок взятия модуля, вход которого подключен к выходу первого сумматора и первому входу первого ключа, второй пороговый элемент и восемнадцатый сумматор, второй вход которого подключен к выходу первого порогового элемента, а выход - ко входу прерывания работы блока задания программы и второму входу первого ключа.

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

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

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

Устройство для программного управления манипулятором с шестью степенями подвижности и линейно перемещаемым основанием содержит блок 1 задания программы, последовательно соединенные первый сумматор 2, первый вход которого подключен к первому выходу блока 1 задания программы со значением проекции r_x на ось х абсолютной системы координат (АСК) вектора r, определяющего текущее положение характерной точки Р схвата манипулятора в этой АСК, а второй вход - ко второму выходу блока 1 задания программы со значением проекции а_х на ось х АСК единичного вектора а, расположенного в плоскости схвата манипулятора и определяющего его ориентацию в АСК, первый блок 3 деления, первый функциональный преобразователь 4, имеющий функцию преобразования в виде «arctg», второй сумматор 5, первый релейный элемент 6, первый блок 7 умножения, второй функциональный преобразователь 8, имеющий функцию преобразования в виде «sin», второй блок 9 деления, третий функциональный преобразователь 10, имеющий функцию преобразования в виде «arcsin», четвертый функциональный преобразователь 11, имеющий функцию преобразования в виде «sin», второй 12 и третий 13 блоки умножения, третий сумматор 14, пятый функциональный преобразователь 15, имеющий функцию преобразования в виде «arccos», и четвертый блок 16 умножения, последовательно соединенные третий блок 17 деления и шестой функциональный преобразователь 18, имеющий функцию преобразования в виде «arctg», выход которого соединен со вторым входом второго сумматора 5, последовательно соединенный пятый блок 19 умножения, четвертый сумматор 20 и второй релейный элемент 21, выход которого соединен со вторым входом четвертого блока 16 умножения, последовательно соединенные шестой блок 22 умножения и пятый сумматор 23, выход которого подключен к первому входу пятого блока 19 умножения, а его второй вход - к выходу седьмого блока 24 умножения, последовательно соединенные восьмой блок 25 умножения, шестой сумматор 26, второй вход которого через девятый блок 27 умножения соединен с восьмым выходом блока 1 задания программы со значением проекции b_y на ось у АСК единичного вектора b, расположенного в плоскости схвата манипулятора и определяющего его ориентацию в АСК, и десятый блок 28 умножения, второй вход которого соединен со вторым выходом блока 1 задания программы, вторым входом первого сумматора 2 и первым входом третьего блока 17 деления, а его выход - со вторым входом четвертого сумматора 20, последовательно соединенные седьмой функциональный преобразователь 29, имеющий функцию преобразования в виде «cos», вход которого подключен ко входу четвертого функционального преобразователя 11, и одиннадцатый блок 30 умножения, последовательно соединенные восьмой функциональный преобразователь 31, имеющий функцию преобразования в виде «sin», двенадцатый блок 32 умножения, второй вход которого подключен ко второму выходу блока 1 задания программы, седьмой сумматор 33, второй вход которого подключен к выходу тринадцатого блока 34 умножения, и девятый функциональный преобразователь 35, имеющий функцию преобразования в виде «arccos», выход которого соединен со вторым входом первого блока 7 умножения, последовательно соединенные четырнадцатый блок 36 умножения, первый вход которого соединен с выходом четвертого функционального преобразователя 11, а второй вход - с выходом восьмого функционального преобразователя 31, и пятнадцатый блок 37 умножения, второй вход которого подключен к восьмому выходу блока 1 задания программы, а его выход - ко второму входу третьего сумматора 14, третий вход которого соединен с выходом одиннадцатого блока 30 умножения, последовательно соединенные восьмой сумматор 38 и четвертый блок 39 деления, девятый сумматор 40, первый вход которого соединен с третьим выходом блока 1 задания программы со значением проекции а_у на ось у упомянутого вектора а и вторыми входами третьего блока 17 деления, пятого 19 и тринадцатого 34 блоков умножения, его второй вход - с четвертым выходом блока 1 задания программы со значением проекции r_y на ось у упомянутого вектора r, а выход - со вторым входом первого блока 3 деления, десятый сумматор 41, первый вход которого соединен с пятым выходом блока 1 задания программы со значением проекции a_z на ось z упомянутого вектора а и вторым входом второго блока 9 деления, а его второй вход - с шестым выходом блока 1 задания программы со значением проекции r_z на ось z упомянутого вектора r, а также десятый функциональный преобразователь 42, имеющий функцию преобразования в виде «cos», вход которого подключен ко входу восьмого функционального преобразователя 31, а выход - к первому входу тринадцатого блока 34 умножения, причем первый вход одиннадцатого блока 30 умножения подключен ко второму входу девятого блока 27 умножения и первому входу шестого блока 22 умножения, второй вход которого соединен с седьмым выходом блока 1 задания программы со значением проекции b_x на ось х упомянутого вектора b, первый вход седьмого блока 24 умножения подключен ко второму входу третьего блока 13 умножения, а его второй вход - к первому входу восьмого блока 25 умножения, второй вход одиннадцатого блока 30 умножения соединен с девятым выходом блока 1 задания программы со значением проекции b_z на ось z упомянутого вектора b, последовательно соединенные усилитель 43, пятый блок 44 деления, одиннадцатый функциональный преобразователь 45, имеющий функцию преобразования в виде «arctg», одиннадцатый сумматор 46, второй вход которого через двенадцатый функциональный преобразователь 47, имеющий функцию преобразования в виде «arcsin», подключен к выходу четвертого блока 39 деления, тринадцатый функциональный преобразователь 48, имеющий функцию преобразования в виде «sin», двенадцатый сумматор 49, четырнадцатый функциональный преобразователь 50, имеющий функцию преобразования в виде «arcsin», тринадцатый 51 и четырнадцатый 52 сумматоры, вторые входы которых подключены к выходу одиннадцатого сумматора 46, а выход четырнадцатого сумматора 52 подключен ко входу восьмого функционального преобразователя 31, последовательно соединенные инвертор 53, первый квадратор 54, пятнадцатый сумматор 55, второй вход которого через второй квадратор 56 подключен к первому входу пятого блока 44 деления, второй вход которого соединен с выходом инвертора 53, и пятнадцатый функциональный преобразователь 57, имеющий функцию преобразования в виде вычисления корня квадратного, выход которого подключен ко второму входу четвертого блока 39 деления, а также третий квадратор 58, вход которого подключен ко входу инвертора 53 и выходу девятого сумматора 40, а выход - к первому входу восьмого сумматора 38, второй вход которого соединен с выходом первого источника 59 постоянного сигнала, а третий вход через четвертый квадратор 60 - со вторым входом двенадцатого сумматора 49, выходом первого сумматора 2 и входом усилителя 43, а также шестнадцатый блок 61 умножения, первый вход которого подключен к девятому выходу блока 1 задания программы, его второй вход - к первому входу второго блока 12 умножения, а выход - к первому входу восьмого блока 25 умножения, второй вход которого соединен с выходом восьмого функционального преобразователя 31, причем второй вход второго блока 12 умножения подключен к седьмому выходу блока 1 задания программы, а выход десятого функционального преобразователя 42 - ко второму входу третьего блока 13 умножения, причем выходы десятого 41, одиннадцатого 46 и тринадцатого 51 сумматоров, третьего функционального преобразователя 10, первого 7 и четвертого 16 блоков умножения выполнены с возможностью подключения к соответствующим следящим приводам степеней подвижности манипулятора, последовательно соединенные первый ключ 62, пятый квадратор 63, шестнадцатый сумматор 64, второй вход которого через шестой квадратор 65 подключен к выходу девятого сумматора 40, блок 66 взятия корня квадратного, семнадцатый сумматор 67, второй вход которого подключен к выходу второго источника 68 постоянного сигнала, первый пороговый элемент 69 и второй ключ 70, второй вход которого подключен к выходу первого ключа 62, причем третий вход первого сумматора 2 выполнен с возможностью подключения к выходу датчика положения следящего привода 71 линейного перемещения основания манипулятора, а вход - к выходу второго ключа 70, последовательно соединенные блок 72 взятия модуля, вход которого подключен к выходу первого сумматора 2 и первому входу первого ключа 62, второй пороговый элемент 73 и восемнадцатый сумматор 74, второй вход которого подключен к выходу первого порогового элемента 69, а выход - ко входу прерывания работы блока 1 задания программы и второму входу первого ключа 62.

На чертежах введены следующие обозначения: q_i - обобщенная координата степени подвижности i манипулятора - это управляемые координаты соответствующих приводов манипулятора

; r(t) - вектор, задающий положение характерной точки схвата Р в трехмерном пространстве, определяемом абсолютной системой координат (АСК) xyz; x₁y₁z₁ - связанная с манипулятором система координат (ССК), ось х₁ которой всегда совпадает с осью х АСК, а оси у₁ и z₁ всегда параллельны соответствующим осям у и z АСК; R - вектор, определяющий положение шарнира (точка W) пятой степени подвижности (координата q₅) в АСК; R_x, R_y и R_z - координаты вектора R(t) в АСК; L₂, L₃, L₄ - длины соответствующих звеньев манипулятора; a, b - единичные векторы, расположенные в плоскости схвата и определяющие его ориентацию в АСК; а_х, а_у, a_z, b_x, b_y, b_z - соответствующие координаты векторов а и b в АСК; e_i - единичные векторы, совпадающие с осями шарниров и призматических пар степеней подвижности i манипулятора

; t - время.

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

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

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

, где

.

Из Фиг. 2 видно, что в АСК всегда выполняется равенство R=r-L₄e₆=r-L₄a.

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

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

Если объекты работ все время находятся в рабочей зоне манипулятора, то АСК и ССК совпадают, а его основание покоится и совпадает с началом этих систем координат (q₇=0). Нулевым сигналом с выхода элемента 69, имеющего характеристику

где а - малая отрицательная величина, ключ 70 не замыкает выход сумматора 2 и вход привода 71, а ключ 62 замыкает нулевым сигналом с выхода сумматора 74 вход квадратора 63 с выходом сумматора 2. То есть нулевым сигналом с выхода сумматора 74 блоку 1 разрешена работа по программе.

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

Сумматор 64 имеет положительные входы с единичными коэффициентами усиления. Поэтому на выходе блока 66 после начала движения манипулятора формируется сигнал

, который определяет программное расстояние от начала АСК, пока совмещенного с ССК, до проекции точки W манипулятора на плоскость ху, когда сигналы R_x и R_y начинают изменяться. На выходе источника 68 формируется постоянный сигнал L₂+L₃. В результате на выходе сумматора 67, первый положительный (со стороны блока 66) и второй отрицательный входы которого имеют единичные коэффициенты усиления, формируется сигнал

, который является индикатором приближения точки W к своему предельному значению в ССК на границе рабочей зоны манипулятора. Пока в начале работы манипулятора обрабатываемые объекты находятся в его исходной рабочей зоне и q₇=0, на выходе сумматора 67 формируется сигнал меньше σ, на выходе элемента 69 устанавливается сигнал 0 и ключ 70 остается разомкнутым. В этом состоянии ССК и АСК совпадают.

Поскольку усилитель 43 и инвертор 53 имеют коэффициенты усиления

, то в результате на их выходах, соответственно, формируются сигналы

. Функциональный преобразователь 45 реализует функцию arctg. Причем arctg принимает значение от -π до 0. На выходе источника постоянного сигнала 59 формируется сигнал

. Первый (со стороны квадратора 58) и третий (со стороны квадратора 60) отрицательные входы сумматора 38 имеют коэффициенты усиления

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

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

, а на выходе функционального преобразователя 47, реализующего функцию arcsin, - сигнал

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

.

Функциональный преобразователь 48 реализует функцию sin. Первый (со стороны функционального преобразователя 48) и второй отрицательные входы сумматора 49 имеют, соответственно, коэффициенты усиления L₂/L₃ и 1/L₃. В результате на его выходе формируется сигнал

, на выходе функционального преобразователя 50, реализующего функцию arcsin, - сигнал

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

, а на выходе сумматора 52, положительные входы которого также имеют единичные коэффициенты усиления, - сигнал ϕ=q₂+q₃.

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

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

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

.

В результате на выходе блока 7 умножения формируется сигнал q₅=±arccos(a_ycosϕ+a_xsinϕ).

Функциональный преобразователь 8 реализует функцию sin. Блок 9 деления имеет на выходе сигнал

. Функциональный преобразователь 10 реализует функцию arcsin. В результате на его выходе будет формироваться сигнал

.

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

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

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

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

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

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

Если в процессе описанной выше работы манипулятора его точка W при некотором значении R_x подходит к границе рабочей зоны, и на выходе сумматора 67 формируется сигнал больше а, то на выходе элемента 69, появляется сигнал, равный единице, который замыкает ключ 70, и на вход привода 71 с выхода ключа 62 поступает сигнал R_x-q₇. Одновременно на второй положительный вход сумматора 74 (с выхода элемента 69) с коэффициентом усиления, равным единице, тоже поступает единичный сигнал, а на его второй отрицательный вход с единичным коэффициентом усиления - нулевой сигнал с выхода элемента 73, имеющего характеристику

где σ₁>0 - малая величина, зависящая от особенностей манипулятора и статической точности работы привода 71. Сигнал на выходе элемента 73 равен нулю, поскольку в указанное время q₇=0, a

и сигнал на выходе блока 72 больше σ₁.

Единичный сигнал с выхода сумматора 74, поступая на вход блока 1, прерывает изменение во времени всех девяти его выходных сигналов и размыкает ключ 62. В результате на выходе ключа 62 сохраняется текущий (исходный) сигнал R_x-q₇=const, который через замкнутый ключ 70 поступает на вход привода 71 и начинает смещать основание манипулятора вместе с его ССК и рабочей зоной вдоль оси х АСК на расстояние, равное q₇. По мере изменения q₇ конфигурация манипулятора начинает изменяться, сохраняя неизменные координаты его точки Р и векторов а и b в АСК.

Процесс отработки приводом 71 входного сигнала R_x-q₇=const (при q₇=0), сохраненного на выходе ключа 62, продолжается до тех пор, пока

, но когда это условие прекращает выполняться, на выходе элемента 73 появляется сигнал, равный 1, а на выходе сумматора 74 - сигнал, равный 0. Этот сигнал вновь запускает работу блока 1, на всех выходах которого в реальном масштабе времени начинают изменяться соответствующие сигналы, и замыкает ключ 62, на выходе которого появляется новый уменьшенный сигнал R_x-q₇ (поскольку уже q₇≠0), который приводит к размыканию ключа 70 и фиксации значения q₇ во время выполнения манипулятором очередных операций по описанному выше алгоритму в уже смещенной вместе со ССК новой (в АСК) рабочей зоне манипулятора по изменяемым во времени выходным сигналам блока 1. При этом значение R_x в АСК изменяется по программе независимо от текущего значения q₇.

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

Таким образом, с помощью предложенного устройства удается не только полностью автоматически решать обратную задачу кинематики для манипулятора, изображенного на Фиг. 2, то есть формировать все сигналы q₁(t)

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

Claims (1)

1. Устройство для программного управления манипулятором с шестью степенями подвижности и линейно перемещаемым основанием, содержащее блок задания программы, последовательно соединенные первый сумматор, первый вход которого подключен к первому выходу блока задания программы со значением проекции «r_x» на ось «х» абсолютной системы координат (АСК) вектора «r», определяющего текущее положение характерной точки «Р» схвата манипулятора в этой АСК, а второй вход - ко второму выходу блока задания программы со значением проекции «а_х» на ось «х» АСК единичного вектора «а», расположенного в плоскости схвата манипулятора и определяющего его ориентацию в АСК, первый блок деления, первый функциональный преобразователь, имеющий функцию преобразования в виде «arctg», второй сумматор, первый релейный элемент, первый блок умножения, второй функциональный преобразователь, имеющий функцию преобразования в виде «sin», второй блок деления, третий функциональный преобразователь, имеющий функцию преобразования в виде «arcsin», четвертый функциональный преобразователь, имеющий функцию преобразования в виде «sin», второй и третий блоки умножения, третий сумматор, пятый функциональный преобразователь, имеющий функцию преобразования в виде «arccos», и четвертый блок умножения, последовательно соединенные третий блок деления и шестой функциональный преобразователь, имеющий функцию преобразования в виде «arctg», выход которого соединен со вторым входом второго сумматора, последовательно соединенные пятый блок умножения, четвертый сумматор и второй релейный элемент, выход которого соединен со вторым входом четвертого блока умножения, последовательно соединенные шестой блок умножения и пятый сумматор, выход которого подключен к первому входу пятого блока умножения, а его второй вход - к выходу седьмого блока умножения, последовательно соединенные восьмой блок умножения, шестой сумматор, второй вход которого через девятый блок умножения соединен с восьмым выходом блока задания программы со значением проекции «b_y» на ось «у» АСК единичного вектора «b», расположенного в плоскости схвата манипулятора и определяющего его ориентацию в АСК, и десятый блок умножения, второй вход которого соединен со вторым выходом блока задания программы, вторым входом первого сумматора и первым входом третьего блока деления, а его выход - со вторым входом четвертого сумматора, последовательно соединенные седьмой функциональный преобразователь, имеющий функцию преобразования в виде «cos», вход которого подключен к входу четвертого функционального преобразователя, и одиннадцатый блок умножения, последовательно соединенные восьмой функциональный преобразователь, имеющий функцию преобразования в виде «sin», двенадцатый блок умножения, второй вход которого подключен ко второму выходу блока задания программы, седьмой сумматор, второй вход которого подключен к выходу тринадцатого блока умножения, и девятый функциональный преобразователь, имеющий функцию преобразования в виде «arccos», выход которого соединен со вторым входом первого блока умножения, последовательно соединенные четырнадцатый блок умножения, первый вход которого соединен с выходом четвертого функционального преобразователя, а второй вход - с выходом восьмого функционального преобразователя, и пятнадцатый блок умножения, второй вход которого подключен к восьмому выходу блока задания программы, а его выход - ко второму входу третьего сумматора, третий вход которого соединен с выходом одиннадцатого блока умножения, последовательно соединенные восьмой сумматор и четвертый блок деления, девятый сумматор, первый вход которого соединен с третьим выходом блока задания программы со значением проекции «а_у» на ось «у» АСК упомянутого вектора «а» и вторыми входами третьего блока деления, пятого и тринадцатого блоков умножения, его второй вход - с четвертым выходом блока задания программы со значением проекции «r_y» на ось «у» АСК упомянутого вектора «r», а выход - со вторым входом первого блока деления, десятый сумматор, первый вход которого соединен с пятым выходом блока задания программы со значением проекции «a_z» на ось «z» АСК упомянутого вектора «а» и вторым входом второго блока деления, а его второй вход - с шестым выходом блока задания программы со значением проекции «r_z» на ось «z» АСК упомянутого вектора «r», десятый функциональный преобразователь, имеющий функцию преобразования в виде «cos», вход которого подключен к входу восьмого функционального преобразователя, а выход - к первому входу тринадцатого блока умножения, причем первый вход одиннадцатого блока умножения подключен ко второму входу девятого блока умножения и первому входу шестого блока умножения, второй вход которого соединен с седьмым выходом блока задания программы со значением проекции «b_x» на ось «x» АСК упомянутого вектора «b», первый вход седьмого блока умножения подключен ко второму входу третьего блока умножения, а его второй вход - к первому входу восьмого блока умножения, второй вход одиннадцатого блока умножения соединен с девятым выходом блока задания программы со значением проекции «b_z» на ось «z» АСК упомянутого вектора «b», последовательно соединенные усилитель, пятый блок деления, одиннадцатый функциональный преобразователь, имеющий функцию преобразования в виде «arctg», одиннадцатый сумматор, второй вход которого через двенадцатый функциональный преобразователь, имеющий функцию преобразования в виде «arcsin», подключен к выходу четвертого блока деления, тринадцатый функциональный преобразователь, имеющий функцию преобразования в виде «sin», двенадцатый сумматор, четырнадцатый функциональный преобразователь, имеющий функцию преобразования в виде «arcsin», тринадцатый и четырнадцатый сумматоры, вторые входы которых подключены к выходу одиннадцатого сумматора, а выход четырнадцатого сумматора подключен к входу восьмого функционального преобразователя, последовательно соединенные инвертор, первый квадратор, пятнадцатый сумматор, второй вход которого через второй квадратор подключен к первому входу пятого блока деления, второй вход которого соединен с выходом инвертора, и пятнадцатый функциональный преобразователь, имеющий функцию преобразования в виде вычисления квадратного корня, выход которого подключен ко второму входу четвертого блока деления, а также третий квадратор, вход которого подключен к входу инвертора и выходу девятого сумматора, а выход - к первому входу восьмого сумматора, второй вход которого соединен с выходом первого источника постоянного сигнала, а третий вход через четвертый квадратор - со вторым входом двенадцатого сумматора, выходом первого сумматора и входом усилителя, шестнадцатый блок умножения, первый вход которого подключен к девятому выходу блока задания программы, его второй вход - к первому входу второго блока умножения, а выход - к первому входу восьмого блока умножения, второй вход которого соединен с выходом восьмого функционального преобразователя, причем второй вход второго блока умножения подключен к седьмому выходу блока задания программы, а выход десятого функционального преобразователя - ко второму входу третьего блока умножения, причем выходы десятого, одиннадцатого и тринадцатого сумматоров, третьего функционального преобразователя, первого и четвертого блоков умножения выполнены с возможностью подключения к соответствующим следящим приводам степеней подвижности манипулятора, отличающееся тем, что в него дополнительно введены последовательно соединенные первый ключ, пятый квадратор, шестнадцатый сумматор, второй вход которого через шестой квадратор подключен к выходу девятого сумматора, блок вычисления квадратного корня, семнадцатый сумматор, второй вход которого подключен к выходу второго источника постоянного сигнала, первый пороговый элемент и второй ключ, второй вход которого подключен к выходу первого ключа, последовательно соединенные блок вычисления модуля, вход которого подключен к выходу первого сумматора и первому входу первого ключа, второй пороговый элемент и восемнадцатый сумматор, второй вход которого подключен к выходу первого порогового элемента, а выход - к входу прерывания работы блока задания программы и второму входу первого ключа, причем третий вход первого сумматора выполнен с возможностью подключения к выходу датчика положения следящего привода линейного перемещения основания манипулятора, а выход второго ключа выполнен с возможностью подключения к входу следящего привода линейного перемещения основания манипулятора.

Images