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

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

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

Описание изобретения к патенту

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

Известно устройство для управления приводом робота, содержащее последовательно соединенные первый блок умножения и первый сумматор, последовательно подключенные усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого соединен с первым входом второго сумматора, подключенного вторым входом к входу устройства, последовательно соединенные второй датчик положения, третий сумматор, четвертый сумматор, первый квадратор и второй блок умножения, а также пятый блок умножения, седьмой сумматор и последовательно соединенные релейный блок и восьмой сумматор, выход которого подключен к второму входу первого сумматора, соединенного выходом с входом усилителя, второй вход которого подключен к выходу датчика массы и первому входу третьего блока умножения, а выход - к первому входу пятого сумматора, соединенного вторым входом с выходом первого задатчика сигнала, а третьим входом - с выходом второго квадратора, вход которого подключен к выходу третьего сумматора и первому входу шестого сумматора, соединенного выходом с первым входом четвертого блока умножения, а вторым входом - с выходом третьего блока умножения, второй вход которого подключен к выходу четвертого сумматора, соединенного вторым входом с выходом второго задатчика сигнала, выход третьего задатчика сигнала подключен к второму входу третьего сумматора, а выход второго датчика скорости соединен с вторым входом четвертого блока умножения, выход первого датчика скорости подключен к входу релейного блока, к второму входу восьмого сумматора и первому входу седьмого сумматора, второй вход которого соединен с выходом второго сумматора, а выход - с первым входом первого блока умножения, подключенного вторым входом к выходу пятого сумматора, первый вход пятого блока умножения соединен с выходом четвертого блока умножения, второй вход с выходом первого датчика скорости, а выход - с третьим входом восьмого сумматора (см. патент РФ 1484702, БИ 21, 1989 г.).

Известно также устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, первый блок умножения, второй сумматор, усилитель и двигатель с редуктором, кинематически связанный с первым датчиком скорости и первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, а третьим входом - с выходом первого датчика скорости, с входом релейного элемента и первыми входами второго блока умножения и третьего сумматора, второй вход которого подключен к выходу второго блока умножения, третий вход - к выходу релейного элемента, а выход - к второму входу второго сумматора, последовательно соединенные второй датчик положения, четвертый сумматор, пятый сумматор, первый квадратор, третий блок умножения и шестой сумматор, выход которого подключен к второму входу первого блока умножения, второй вход - к выходу первого задатчика сигнала, а третий вход - к выходу второго квадратора, а также последовательно соединенные датчик массы, четвертый блок умножения, седьмой сумматор и пятый блок умножения, подключенный вторым входом к выходу второго датчика скорости, а выходом - к второму входу второго блока умножения, второй вход пятого сумматора соединен с выходом второго задатчика сигнала, выход - с вторым входом четвертого блока умножения, а первый вход - с входом второго квадратора и вторым входом седьмого сумматора, выход датчика массы подключен к второму входу третьего блока умножения, а выход третьего задатчика сигнала соединен с вторым входом четвертого сумматора, последовательно соединенные кинематически связанный с выходным валом двигателя первый датчик ускорения, шестой блок умножения и восьмой сумматор, последовательно подключенные четвертый задатчик сигнала, девятый сумматор, седьмой блок умножения, десятый сумматор и восьмой блок умножения, а также последовательно соединенные второй датчик ускорения и девятый блок умножения, второй вход которого подключен к выходу седьмого сумматора, а выход - к второму входу десятого сумматора, второй вход восьмого сумматора соединен с выходом первого датчика ускорения, выход - с четвертым входом третьего сумматора, а третий вход - с выходом восьмого блока умножения, подключенного вторым входом к выходу первого датчика скорости, второй вход шестого блока умножения соединен с выходом пятого блока умножения, второй вход девятого сумматора подключен к выходу датчика массы, а выход второго датчика скорости через третий квадратор соединен с вторым входом седьмого блока умножения (см. патент РФ 1618642, БИ 1, 1991 г.).

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

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

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

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

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

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

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

На фиг.1 представлена схема предлагаемого устройство для управления приводом робота. На фиг. 2 представлена кинематическая схема исполнительного органа робота, а на фиг.3 показан вид сверху этой кинематической схемы.

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

На чертежах введены следующие обозначения:

_вх - сигнал желаемого положения;

q_i - соответствующие обобщенные координаты исполнительного органа робота ;

- скорости изменения соответствующих обобщенных координат ;

- ускорения изменения соответствующих обобщенных координат ;

m₂, m₃, m_r - соответственно массы второго, третьего звеньев исполнительного органа и захваченного груза;

l₃*=const - исходное расстояние от оси вращения горизонтального звена до его центра масс при q₃=0;

l₃= const - расстояние от центра масс горизонтального звена до средней точки схвата;

- скорость вращения ротора двигателя первой степени подвижности;

U*, U - соответственно усиливаемый сигнал и сигнал управления двигателем 5.

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

Сигнал ошибки = _вх-q₁ после коррекции в блоках 1, 2 и 3, усиливаясь в усилителе 4, поступает на электродвигатель с редуктором 5, приводя его вал во вращательное движение с направлением и скоростью (ускорением), зависящими от величины поступающего сигнала U, моментов трения и внешнего моментного воздействия М_в. Электропривод 5 при работе манипулятора с различными грузами, а также за счет взаимовлияния степеней подвижности исполнительного органа обладает переменными моментными характеристиками, которые могут меняться в широких пределах. Это снижает качественные показатели работы указанного электропривода и даже приводит к потере устойчивости его работы. В результате возникает задача, связанная с обеспечением инвариантности динамических свойств электропривода к непрерывным и быстрым изменениям его моментных нагрузочных характеристик, что позволяет обеспечить стабильность заданного качества системы управления.

Рассматриваемый привод управляет перемещением вокруг вертикальной оси (обобщенная координата q₁). Конструкция робота позволяет осуществлять также вертикальное прямолинейное перемещение груза (обобщенная координата q₂) и его горизонтальные прямолинейные перемещения (обобщенные координаты q₃ и q₄).

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

Моментное воздействие на выходной вал привода поворота со стороны движущихся масс исполнительного органа робота и груза, определенное с помощью уравнения Лагранжа 2 рода, имеет вид



H(q₃, m_г)=m₃(l₃*+q₃)²+m_г(l₃*+l₃+q₃)²+I_N+I_s,





где I_s и I_N - соответственно моменты инерции вертикального звена исполнительного органа относительно продольной оси и горизонтального звена относительно поперечной оси, проходящей через его центр масс.

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



где R - активное сопротивление якорной цепи;

L - индуктивность якорной цепи;

J - момент инерции якоря двигателя и вращающихся частей редуктора, приведенных к валу двигателя;

k_м - коэффициент крутящего момента;

k - коэффициент противо-ЭДС;

k_в - коэффициент вязкого трения;

i_p - передаточное отношение редуктора;

М_cтр - момент сухого трения;

k_y - коэффициент усиления усилителя 4;

i - ток якоря;





Очевидно, что изменение h, H, М и , вызванное изменением существенно влияет на динамические свойства, а следовательно, и качественные показатели рассматриваемого привода. Причем из (3) следует, что при большом по модулю h* отрицательного знака привод даже может потерять устойчивость, так как появляются положительные корни характеристического уравнения (3).

Поскольку при движении привода |M_стр| const, то в выражении (3) будем полагать, что В процессе эксплуатации привода можно также полагать, что k_в const,, a

Первый отрицательный (со стороны датчика 6) и второй положительный входы сумматора 1 единичные, а третий отрицательный вход (со стороны датчика 7) имеет коэффициент усиления k/k_у. Положительные входы сумматора 3 имеют единичные коэффициенты усиления. Все входы сумматора 9 положительные. Его первый вход (со стороны датчика 7) имеет коэффициент усиления (k+k_вR/k_м)/k_у, второй (со стороны блока 10) и третий (со стороны релейного элемента 8) - коэффициенты усиления R/(k_мk_y) четвертый (со стороны сумматора 28) - коэффициент усиления L/(k_мk_y), а пятый - коэффициент усиления i_pR/(2k_мk_y).

Причем выходной сигнал релейного элемента 8 с нулевой нейтральной точкой имеет вид



где |M_т| - величина момента сухого трения при движении.

Все входы сумматора 28 положительные. Его первый вход (со стороны блока умножения 27) имеет коэффициент усиления 2, второй вход (со стороны датчика 26) - коэффициент усиления _y, пятый (со стороны блока 49) - коэффициент усиления 1/i_р, а третий и четвертый входы - единичные коэффициенты усиления. Положительные входы сумматоров 12 и 13 имеют единичные коэффициенты усиления. Задатчик 24 сигнала вырабатывает сигнал l₃, а задатчик 25 - сигнал l₃*.

Первый и второй положительные входы сумматора 16 (со стороны блока 15 умножения и задатчика 17 сигнала) соответственно имеют коэффициенты усиления 1/(J_нi_p ²), а третий положительный (со стороны квадратора 18) - коэффициент усиления m₃/(J_нi_p), где J_н - номинальное (желаемое) значение приведенного момента инерции, обеспечивающее приводу поворота заданные динамические свойства и качественные показатели. Причем с выхода задатчика 17 сигнала на второй вход сумматора 16 поступает сигнал J_S+J_N+Ji_p ².

Первый и второй положительные входы сумматора 21 (со стороны блока 20 умножения и сумматора 12) соответственно имеют коэффициенты усиления 2/i_p ² и 2m₃/i_p ²

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

Первый положительный вход сумматора 32 (со стороны блока 31) имеет коэффициент усиления 2/i_p ², а его второй положительный вход (со стороны блока 35) - единичный коэффициент усиления.

Первый отрицательный (со стороны блока 41) и второй положительный входы сумматора 43 имеют соответственно коэффициенты усиления, равные 1/2 и i_p/2.

Таким образом, на выходе сумматора 12 формируется сигнал, равный l₃*+q₃, а на сумматора 13 - сигнал l₃+l₃*+q₃. На выходе блока 15 формируется сигнал m_г(l₃+l₃*+q₃)², а на выходе квадратора 18 - сигнал (l₃+q₃)². В результате на выходе сумматора 16 появляется сигнал, равный



Таким образом, на выходе блока 2 формируется сигнал



На выходе блока 20 формируется сигнал m_г(l₃+l₃*+q₃), а на выходе сумматора 21 - сигнал 2m_г(l₃+l₃*+q₃)/i_p ²+2m₃(l₃+q₃)/i_p ².

Таким образом, на выходе блока 22 формируется сигнал, равный

На выходе сумматора 30 формируется сигнал m_г+m₃, на выходе блока 31 - сигнал а на выходе сумматора 32 - сигнал .

Датчик 45 измеряет ускорение , поэтому на выходе блока 39 формируется сигнал

На выходе блока 41 формируется сигнал а на выходе блока 47 - сигнал . В результате на выходе сумматора 43 формируется сигнал а на выходе блока 44 - сигнал

На выходе блока 49 формируется сигнал на выходе сумматора 28 - сигнал а на выходе сумматора 9 - сигнал



Окончательно на выходе сумматора 3 имеем



Несложно показать, что, подставив U* из (4) в (3), имеем



Таким образом, уравнение (5) имеет постоянные желаемые параметры, которые определяются выбором желаемых значений k_y и J_н, а сам привод обладает постоянными желаемыми динамическими свойствами и качественными показателями при любых законах изменения параметров нагрузки.