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

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

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

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

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

Известно устройство для управления приводом робота, содержащее последовательно соединенные инерционное дифференцирующее звено, первый блок умножения и первый сумматор, второй вход которого соединен с выходом апериодического звена, вход которого подключен ко входу инерционного дифференцирующего звена, второй вход первого блока умножения соединен с выходом блока деления, последовательно соединенные первый усилитель и двигатель, кроме того, оно содержит второй блок умножения и вычислительный блок, первый вход второго блока умножения соединен с выходом первого сумматора, выход с выходом первого усилителя, а второй вход с первым выходом вычислительного блока, второй и третий выходы которого соединены соответственно с первым и вторым входами блока деления, а входы вычислительного блока являются входами устройства. Вычислительный блок выполнен в виде последовательно соединенных второго сумматора, первого квадратора, третьего блока умножения, третьего сумматора, второй вход которого соединен с первым входом вычислительного блока, а третий с выходом второго квадратора, вход которого соединен с первым входом четвертого сумматора и выходом пятого сумматора, первый вход которого соединен с первым входом второго сумматора и вторым входом вычислительного блока, а второй с вторым входом второго сумматора и третьим входом вычислительного блока, четвертый вход которого соединен с третьим входом второго сумматора, пятый с первым входом шестого сумматора, а шестой с вторым входом третьего блока умножения и первым входом четвертого блока умножения, второй вход которого соединен с выходом второго сумматора, а выход с вторым входом четвертого сумматора, выход которого соединен с первым входом пятого блока умножения, второй вход которого соединен с седьмым входом вычислительного блока, а выход с вторым входом шестого сумматора, выход которого соединен с входом второго усилителя, при этом выходы третьего сумматора, шестого сумматора и второго усилителя соответственно являются третьим, вторым и первым выходами вычислительного блока /см. авт. свид. СССР N1352450, Б.И. N 42, 1987/.

Известно также устройство для управления приводом робота, содержащее последовательно соединенные блок коррекции, первый блок умножения, первый сумматор, второй блок умножения, первый усилитель и электродвигатель, а также вычислительный блок, семь входов которого подключены к семи входам устройства, первый выход ко второму входу второго блока умножения, а второй и третий выходы соответственно к первому и второму входам первого блока деления, соединенного выходом со вторым входом первого блока умножения. Кроме того, оно содержит последовательно соединенные датчик положения и второй сумматор, а также последовательно подключенные третий сумматор, второй блок деления и третий блок умножения, второй вход которого соединен со вторым выходом блока коррекции, а выход со вторым входом первого сумматора, подключенного третьим входом к третьему выходу блока коррекции, вход которого соединен с выходом второго сумматора, подключенного вторым входом к восьмому входу устройства. Датчик положения кинематически связан с валом электродвигателя. Второй вход второго блока деления соединен с четвертым выходом вычислительного блока, пятый и шестой выходы которого подключены к первому и второму входам третьего сумматора соответственно /см. авт. свид. СССР N 1579770, Б.И. N 27, 1990/.

Недостатком данных устройств является то, что они сохраняют достаточно высокую точность работы только для конкретной степени подвижности конкретного типа робота. Для других степеней подвижности других роботов с помощью указанного устройства высокая точность работы обеспечена не будет. В частности для типовых роботов РUМА, ТУР-10, Гранат-10 и многих других с однотипной кинематикой для достижения высокого качества /точности/ управления следует разрабатывать иные устройства управления приводом, которые обеспечивали бы точную компенсацию переменных параметров нагрузки, характерных для манипуляторов указанного типа.

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

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

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

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

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

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

Блок-схема предлагаемого устройства для управления приводом робота представлена на фиг.1. На фиг.2 представлена кинематическая схема робота.

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

_вх сигнал с выхода программного устройства;

сигнал ошибки;

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

q₁, q₂, q₃ обобщенные координаты трех степеней подвижности;

m₁, m₂, m₃, m_г массы соответствующих звеньев робота и груза;

l₂, l₃ длины соответствующих звеньев;

l^*₂,, l^*₃ расстояния от осей вращения соответствующих звеньев до их центров масс;

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

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

Кроме того полагается, что

J_si моменты инерции соответствующих звеньев робота относительно их продольных осей /i 1, 3/;

J_Ni моменты инерции соответствующих звеньев робота относительно поперечных осей, проходящих через их центры масс /i 2, 3/.

Устройство работает следующим образом. На вход подается управляющее воздействие _вх, обеспечивающее требуемый закон управления объектом. На выходе сумматора 1 вырабатывается сигнал ошибки , который после коррекции в элементах 2 и 3, усиливаясь, поступает на электродвигатель 5 с редуктором, приводя его вал во вращательное движение с направлением и скоростью /ускорением/, зависящим от величины поступающего сигнала U и внешнего моментного воздействия M_в на привод.

В изобретении рассматривается устройство для управления приводом робота относительно вертикальной оси исполнительного органа робота, схема которого представлена на фиг. 2. Этот привод управляет обобщенной координатой q₁.

Датчики 24 и 22 установлены соответственно во второй и третьей степенях подвижности робота /см. фиг. 2/ и измеряют соответственно обобщенные координаты q₂, q₃. Сумматор 23 имеет положительные входы с единичными коэффициентами усилия, поэтому на его выходе формируется сигнал q₂ + q₃. Усилители 41 и 49 имеют коэффициенты усиления 2. Сумматор 42 имеет единичные коэффициенты усиления, но его второй вход /со стороны датчика 22/ отрицательный. Поэтому на выходе сумматора 42 формируется сигнал 2 q₂ + q₃. Функциональные преобразователи 25, 29, 43, 50 и 51 реализуют функцию sin, а функциональные преобразователи 27 и 31 функцию cos. В результате на выходе третьего 26, четвертого 28, пятого 30 и шестого 32 квадраторов соответственно формируются сигналы sin²q₂, cos²q₂, sin²(q₂ + q₃) и cos²(q₂ + q₃).

Первый /со стороны функционального преобразователя 27/ и второй положительные входы сумматора 33 соответственно имеют коэффициенты усиления l₂ и l^*₃. В результате на выходе квадратора 15 формируется сигнал [l₂cosq₂+l^*₃cos(q₂+q₃)]². Первый /со стороны функционального преобразователя 27/ и второй положительные входы сумматора 9 соответственно имеют коэффициента усиления l₂ и l₃. В результате на выходе блока 11 умножения формируется сигнал m_г[l₂cosq₂ + l₃cos(q₂ + q₃)]², т.к. датчик 12 измеряет массу захваченного груза m_г.

На выходе задатчика 14 постоянного сигнала формируется сигнал Ji²_p+J_s1, где J момент инерции ротора электродвигателя и вращающихся частей редуктора /приведены к валу двигателя/, i_р передаточное отношение редуктора. Первый /со стороны блока 11/ и второй /со стороны задатчика 14/ положительные входы сумматора 13 имеют единичные коэффициенты и усиления. Его третий /со стороны квадратора 15/, четвертый /со стороны квадратора 26/, пятый /со стороны квадратора 28/, шестой /со стороны квадратора 30/ и седьмой /со стороны квадратора 32/ положительные входы соответственно имеют коэффициенты усиления m₃, J_S2, J_N2+m₂l^*₂², J_S3 и J_N3. В результате на выходе сумматора формируется сигнал

.

Датчики скорости 39 и 21 устанавливаются соответственно во второй и третьей степенях подвижности робота /см. фиг. 2/ и измеряют соответственно и .

На выходе задатчика 34 формируется сигнал m₃l²₂+J_N2-J_s2+m₂l^*₂². Первый /со стороны задатчика 34 / и второй положительные входы сумматора 35 соответственно имеют единичный коэффициент усиления и единичный коэффициент усиления и коэффициент усиления, равный l²₂. В результате на выходе блока умножения 36 формируется сигнал

[J_N2-J_s2+ m₂l^*₂²+ (m₃+m_г)l²₂]sin 2q₂.

Задатчик 16 формирует сигнал J_N3-J_s3+ m₃l^*₃². Первый /со стороны задатчика 16/ и второй положительные входы сумматора 17 соответственно имеют единичный коэффициент усиления и коэффициент усиления l²₃. В результате на выходе блока 18 умножения формируется сигнал (J_N3-J_s3+ m₃l^*₃² +m_гl²₃]sin 2(q₂+q₃).

Задатчик 45 формирует сигнал 2l₂l^*₃m₃. Первый /со стороны задатчика 45/ и второй положительные входы сумматора 46 соответственно имеют единичный коэффициент усиления и коэффициент усиления 2l₂l₃. В результате на выходе блока 44 умножения формируется сигнал 2l₂(m₃l^*₃ +m_гl₃)sin(2q₂+q₃).

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



На выходе блока умножения 47 формируется сигнал

2l₂(m₃l^*₃ +m_гl₃)cosq₂,

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



Оба отрицательных входа сумматора 40 имеют единичные коэффициенты усиления, поэтому на его выходе формируется сигнал -/A+B/. На первый положительный вход сумматора 3 /со стороны блока 2/ с коэффициентом усиления поступает сигнал (P+Ji²_p), на его второй положительный вход /со стороны блока 52/ с коэффициентом усиления сигнал , на третий отрицательный вход /со стороны блока 53/ с коэффициентом усиления K/(K_уJ_Ni²_p) - сигнал , на четвертый положительный вход с коэффициентом усиления R/(K_мK_у /со стороны релейного элемента 54/ сигнал

,

а на пятый положительный вход с коэффициентом усиления сигнал , где R активное сопротивление якорной обмотки электродвигателя, K_в коэффициент вязкого трения, K_у коэффициент усиления усилителя 4, K_м коэффициент крутящего момента, K коэффициент противо-ЭДС, M_т const величина момента сухого трения при движении электродвигателя.

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



Кинетическая энергия всех движущихся масс исполнительного органа робота представляется в виде



а потенциальная энергия имеет вид



Учитывая, что



Из уравнения Лагранжа 2 рода несложно получить



Учитывая, что U K_уU^*, q₁i_p=₁, а также уравнения электрической цепи и механической цепи /с учетом соотношения /2//

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



Поскольку при движении электропривода достаточно точно соответствует M_стр, то сформированный сигнал U^* /1/, как несложно убедиться, обеспечивает превращение уравнения /3/ с существенно переменными параметрами в уравнение с номинальными постоянными /желаемыми/ параметрами



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