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

 

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

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

Известны устройства, применяемые для управления манипуляторами роботов (см., например, а.с. СССР №278371, БИ №25, 1970 г.; № 470791, БИ.№18, 1975 г.; патенты США 2679940, 3425775; ФРГ 1127561; Франции 2011610; Японии 47-32764), ЭВМ и сервопривод.

В известных условиях (см. а.с. СССР №675400, БИ №27, 1979 г.; №746419, БИ №25, 1980 г.) для обеспечения быстродействия отработки заданных траекторий движения исполнительного органа применяют блоки программного управления, что существенно усложняет эти устройства.

Из известных устройств наиболее близким по технической сущности является устройство, содержащее последовательно соединенные блок сравнения, первый усилитель, сумматор, сервопривод, исполнительный орган, датчик угла поворота, своим выходом подключенный ко второму входу блока сравнения, выход которого подключен также через последовательно соединенные блок дифференцирования и второй усилитель ко второму входу сумматора (см. книгу Р. Пол. Моделирование, планирование траекторий и управление движением робота-манипулятора. М.: Наука, 1976, с.61, 62, рис. 6.1).

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

В ряде случаев исполнительный орган - звено манипулятора должно перемещаться из начального положения Q₁ в целевое Q₂ с заданной скоростью v к целевому положению. По введенным указанным данным специализированный вычислитель, в качестве которого обычно применяют ЦВМ, должен за некоторое время рассчитать относительно сложный управляющий сигнал и затем генерировать его, что и обуславливает его относительную сложность и малое быстродействие. Кроме того, в процессе отработки сервосистемой устройства входного сигнала имеют место непрерывные переходные процессы, на подавление которых тратится дополнительное время, снижающее быстродействие известного устройства.

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

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

В предлагаемое устройство введены новые блоки, такие как два нелинейных элемента, два полупериодных выпрямителя, схема И, схема НЕ, шесть ключей и три усилителя, которые являются стандартными и известными из литературы (см., например, А.Г. Алексенко, Е.А. Коломбет, Г.И. Стародуб. Применение прецизионных аналоговых ИС. М.: Радио и связь, 1981, с.77-82, 92-101, 105-110, 148-152. 167-177. 190-194; Ю.С.Забродин. Промышленная электроника. М.: Высшая школа, 1982, с. 335; Справочник по интегральным микросхемам / Б.В.Тарабрина. М.: Энергия, 1981, с.568-770). За счет введения новых блоков и установления новых связей между ними и блоками устройства-прототипа предлагаемое устройство приобретает свойство автоматического перестраивания структуры с исключением ЭВМ, благодаря которому достигается увеличение быстродействия и упрощение устройства.

На чертеже представлена функциональная схема устройства, на которой указаны: первый, второй и третий усилители 1, 2 и 3 соответственно, два сумматора 4 и 26 соответственно, сервопривод 5, исполнительный орган 6, датчик 7 угла поворота, блоки сравнения 8 и 10 соответственно, блок 9 дифференцирования, нелинейные элементы 11 и 21 соответственно, первый и второй блоки выделения модуля 12 и 13 соответственно, схема И 14, схема НЕ 15, инвертор 25, первый, второй, третий, четвертый, пятый и шестой ключи 16, 17, 18, 19, 20 и 24 соответственно, два задающих блока 22 и 23 соответственно.

Выход блока сравнения 8 через последовательно соединенные первый усилитель 1, сумматор 4, сервопривод 5 и исполнительный орган 6 соединен со входами датчика 7 угла поворота, блок 9 дифференцирования подключен через второй усилитель 2 ко второму входу сумматора 4, выход первого компаратора 10 через последовательно соединенные нелинейный элемент 21 и первый блок выделения модуля 12, схему И 14, схему НЕ 15 соединен с первым входом первого ключа 16, второй вход которого соединен параллельно с первым входом блока сравнения 10, выход датчика 7 угла поворота подключен ко второму входу второго блока сравнения 10, к первому входу второго ключа 17, выход которого параллельно с выходом третьего ключа 18 соединен со входом блоком 9 дифференцирования, и выход датчика 7 подключен также через четвертый ключ 19 к первому входу блока 8 сравнения, выход которого соединен с первым входом третьего ключа 18, выход второго нелинейного элемента 11 через второй блок выделения модуля 13 подключен ко второму входу схемы И 14, с первым входом пятого ключа 20 соединен выход третьего усилителя 3, вход которого соединен с выходом сумматора 26, первый вход которого соединен с выходом инвертора 25, вход которого соединен с первым выходом ключа 24, второй выход которого подсоединен ко второму входу сумматора 26, входы ключа 24 соединены с входом нелинейного элемента 11 и с выходом элемента 21, выход пятого ключа 20 параллельно с выходом первого ключа 16 соединен со вторым входом блока 8 сравнения, выход схемы НЕ 15 соединен также со вторыми входами третьего и четвертого ключей 18 и 19 соответственно, а выход схемы И 14 соединен также со вторыми входами второго и пятого ключей 17 и 20 соответственно.

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

R(t)=R₁,

означающий, что звено должно быть повернуто на такой угол, что соответствующий ему выходной сигнал с датчика угла поворота 7 Q(t) должен быть равен

Q(t)=Q₁=R₁.

На втором входе устройства, то есть на входе нелинейного элемента 11, сигнал, задающий целевую скорость, равен нулю. В результате на входе нелинейного элемента 11 имеет место нулевое напряжение, которое затем через блок выделения модуля 13 поступает на второй вход схемы И 14. На входе схемы И присутствует поэтому логический ноль. Этот ноль воздействует непосредственно на управляющие входы ключей 17, 20, которые в результате этого закрыты. Одновременно этот ноль инвертируется схемой НЕ 15, выходной единичный сигнал которой поступает на управляющие выходы ключей 16, 18, 19. Назначенные ключи находятся вследствие этого в открытом состоянии. На выходе блока сравнения 8 получится сигнал ошибки положения, равный разности входного сигнала, поступающий через открытый ключ 16, и текущего сигнала положения, поступающего с датчика угла поворота 7 через открытый ключ 19. Сигнал ошибки по скорости находится в результате дифференцирования сигнала ошибки положения, поступающий через открытый ключ 18 на вход блока дифференцирования 9. Полученные сигналы усиливаются усилителями 1, 2, суммируются сумматором 4 и поступают на вход сервопривода 5. Сервопривод вращает звено 6 шарнирного уравновешенного манипулятора, с которым механически связан датчик угла поворота 7. Таким образом, в устройстве реализуется преобразование входного сигнала и его скоростной составляющей в управляющий механический момент, приводящий во вращательное движение звено манипулятора, и за счет сигналов отрицательной обратной связи по положению и скорости звена, останавливающий звено в положении, соответствующем значению входного сигнала.

В дальнейшем на первый вход устройства поступает задающий целевой сигнал положения R₂, а на второй вход устройства сигнал целевой скорости V подхода к целевому положению Q₂. На втором входе компаратора 10 продолжает действовать сигнал текущего положения Q₁, не равный вновь заданному, поэтому на выходе компаратора появляется ненулевое выходное напряжение, которое после блока выделения модуля 12 воздействует в виде логической единицы на первый вход схемы И 14. Поскольку значение целевой скорости подхода V не равно нулю, на выходе нелинейного элемента 11, выполненного на основе усилителя-ограничителя, появляется выходное напряжение, воздействующее после прохождения блока выделения модуля 13 в виде логической единицы на второй вход схемы И 14. На выходе схемы И появляется логическая единица, переводящая работу устройства в режим отработки целевой скорости подхода. Названная единица воздействует на управляющие входы ключей 17, 20, которые открываются. Одновременно эта единица инвертируется схемой НЕ 15, выходной нулевой сигнал который поступает на управляющие входы ключей 16, 18, 19, в результате чего ключи закрываются. На выходе блока 8 сравнения получается инвертированный по второму входу сигнал, который равен произведению целевой скорости V на значение коэффициента усиления усилителя 3. Этот сигнал поступает на второй вход блока 8 через открытый ключ 20, а первый вход сигналы не поступают, так как ключ 19 закрыт. Текущий сигнал положения с выхода датчика угла поворота 7 через открытый ключ 17 поступает на вход блока дифференцирования 9, на входе которого получается сигнал входной скорости вращения исполнительного органа 6. Выходной же сигнал с блока 8 на вход блока 9 не поступает, так как ключ 18 закрыт. Далее названные сигналы усиливаются усилителями 1, 2, суммируются сумматором 4 и поступают на вход сервопривода 5, вращающий исполнительный механизм 6. За счет сигнала отрицательной обратной связи по выходной скорости скорость вращения поддерживается равной заданной целевой скорости V. Такое движение происходит до тех пор, пока исполнительный орган не достигает положения, примерно равного целевому, то есть выходной сигнал Q₂ датчика 7 угла поворота станет примерно равен входному целевому сигналу R₂. На выходе компаратора 10 напряжение станет нулевым, и на первом входе схемы И 14 станет действовать логический ноль, и на выходе схемы И также появится логический ноль. В результате структура устройства перестраивается в первоначальное положение, в нем снова имеет место формирование управляющего воздействия по ошибке положения и ошибке скорости, и звено поэтому замедляет свое движение, останавливается и удерживается в заданном положении Q₂. Если звено манипулятора из-за набранной скорости V по инерции проскочит несколько заданное целевое положение, структура устройства не перейдет вновь во второе состояние. Блок сравнения 10 выполнен двухпороговым, то есть он является дискриминатором. Состояние его выхода изменяется два раза при изменении разности входных сигналов, он имеет нижний и верхний уровни разности сравниваемых входных сигналов, при которых происходит мгновенное изменение его входного напряжения. Иными словами, компаратор имеет зону нечувствительности к некоторому значению разности входных сигналов, определяемую параметрами его элементов (см., например, Алексенко А.Д., Коломбет Е.А., Стародуб Г.И. Применение прецизионных аналоговых ИС. М.: Радио и связь, 1981, с.173...175, таблица 7.4,а). Поэтому при проскоке звеном целевого положения при разности входных сигналов блока сравнения 10, не равной нулю, его входное напряжение останется нулевым, и структура устройства не изменится. Введение названных новых блоков и установление новых связей значительно упрощает устройство за счет исключения из его состава сложной цифровой вычислительной машины. Одновременно повышается быстродействие устройства, так как при достижении целевого положения с целевой скоростью не тратится время, как ранее, на подавление переходных процессов, а непосредственно используется сам переходный процесс определенного вида, сформированный посредством соответствующих перестроений структуры устройства. Сказанное подтверждается математическими выкладками, описывающими работу устройства.

Блоки устройства имеют следующие передаточные функции в изображении по Лапласу. Усилители 1, 2, 3 имеют коэффициенты усиления соответственно Ке, Kv, Kv/Ke. Блоки 4 и 7 имеют коэффициенты передачи, равные 1. Сервопривод 5 осуществляет линейное преобразование электрического сигнала в механический вращающий момент. Коэффициент передачи сервопривода равен 1 и не зависит от скорости вращения звена, так как оно выполняется на электродвигателе постоянного тока, питаемого преобразователем напряжение - ток, либо на электродвигателе, питаемом усилителем напряжения с соответствующей положительной обратной связью по выходной скорости вращения, которая компенсирует влияние скоростной противоЭДС электродвигателя. Передаточная функция исполнительного органа 6 равна 1/JS², а блока 9 дифференцирования S. На выходе блока сравнения 8 имеет место сигнал ошибки E(s), равный разности выходного сигнала Q(s) и выходного сигнала R(s). Поскольку исполнительным органом является малоскоростной уравновешенный шарнирный манипулятор, то эффективный момент инерции J постоянен, а внешний возмущающий момент равен нулю. В процессе удержания исполнительного органа в заданном положении или останова в целевом положении в структуре устройства имеет место формирование управляющего воздействия по ошибке положения и демпфирование по ошибке скорости. Выражение для изображения динамической ошибки E(s) критического коэффициента демпфирования К и установившейся статической ошибки положения Е имеет следующий вид (см. формулы 6.1, 6.2 материала прототипа):

E=0.

Таким образом, с течением времени звено манипулятора с нулевой ошибкой останавливается или удерживается в заданном положении.

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

JS2+KvS=KeR(s).

Изображение по Лапласу решения этого уравнения Q₁ имеет (см., например, Г. Корн, Т. Корн. Справочник по математике. - М.: Наука, 1977, с.228, табл. 8.4-1) такой вид:

Управляющий сигнал R(t) поступающий с выхода ключа 20, постоянен, пропорционален единичному скачку и имеет изображение

Преобразованное по таблице обратных преобразований Лапласа выражение (1) с учетом (2) принимает вид

С течением времени после разгона звена 6 разность, находящаяся в скобках в (3), становится равной нулю, и звено манипулятора совершает движение с заданной скоростью V от начального положения Q₁ к целевому.

Структура устройства выполнена таким образом, что в режиме отработки целевой скорости V подхода к целевому положению R, формируемой задатчиком скорости, знак целевой скорости меняется в зависимости от знака рассогласования целевого и текущего положения. Это происходит следующим образом. Целевой сигнал R₂ сравнивается в блоке сравнения 10. Целевой сигнал R₂ и сигнал текущий (Qτ) с датчика 7 положения всегда неотрицательны, что обусловлено выполнением данных блоков. Если названная разность сигналов положительна, то выходной сигнал блока сравнения 10 после прохождения нелинейного элемента 21 имеет знак "+", который поступает на управляющий вход ключа 24 таким образом, что положительный сигнал скорости с задатчика скорости 23 пропускается ключом 24 непосредственно на первый вход сумматора 26 и на вход усилителя 3 соответственно. И устройство передвигает звено манипулятора с заданной целевой скоростью в сторону уменьшения рассогласования целевого и текущего положения согласно уравнению (3).

Если в результате инерции произойдет перебег звеном заданного целевого положения R₂, выходной сигнал нелинейных элементов 21 изменит знак на противоположный и переключит ключ 24, сигнал задатчика скорости 23 будет проходить через инвертор 25, изменит свой знак на отрицательный и поступит на второй вход сумматора 26 и на вход усилителя 3 соответственно. Тем самым движение, согласно уравнению (3), начнет осуществляться в обратную сторону. То есть в сторону уменьшения рассогласования между целевым R₂, и текущим Q_t положением.

Предлагаемое устройство выполнено из стандартных блоков дифференцирования 9, сравнения 8, 10, сумматора 4, 26, усилителей 1, 2, 3, сервопривод 5 на основе электродвигателя постоянного тока и преобразователя напряжение - ток или усилителя напряжения с положительной обратной связью по выходной скорости вращения вала электродвигателя, информацию о которой несет выходной сигнал встроенного в электродвигатель тахогенератора, датчик 7 угла поворота на основе потенциометра, нелинейные элементы 11 и 21, блок сравнения 10, блоки выделения модуля 12, 13, схемы И 14, схемы НЕ 15, задатчики скорости и положения 22, 23, ключей 16, 17, 18, 19, 20 и 24 (см.. А.Г. Алексенко, Е.А. Коломбет, Г.И. Стародуб. Применение прецизионных аналоговых ИС. М.: Радио и связь, 1981. С.77-82, 92-101, 105-110, 148-152, 167-177, 190-194, Ю.C. Забродин. Промышленная электроника. М.: Высшая школа, 1982, с.159, 160, 170, 175, 184-199, 254-296, В.Г. Гусев, Ю.М. Гусев. Электроника. М.: Высшая школа, 1982, с.335, Справочник по интегральным микросхемам. Б.В. Тарабрин, С.В. Якубовский, Н.А. Барканов и др. Под ред. Б.В. Тарабрина. М.: Энергия, 1981, с.568-770).

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

На кафедре "Робототехнические системы" СамГТУ собран и испытан лабораторный макет устройства.

Для доведения изобретения до промышленного использования требуется выполнение НИР сроком 2 года и последующее проведение ОКР сроком 1 год.

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

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

РИСУНКИ

Рисунок 1