Устройство для управления приводом робота
Изобретение относится к робототехнике, в частности к средствам управления приводами роботов и манипуляторов. Сущность изобретения заключается в том, что в устройство дополнительно введены последовательно соединенные косинусный функциональный преобразователь, десятый и одиннадцатый блоки умножения, причем выход последнего подключен к пятому входу третьего сумматора, последовательно соединенные синусный функциональный преобразователь, вход которого подключен к входу косинусного функционального преобразователя и выходу первого датчика положения, двенадцатый блок умножения, второй вход которого через тринадцатый блок умножения соединен с выходом первого датчика скорости, одиннадцатый сумматор и четырнадцатый блок умножения, второй вход которого подключен к выходу седьмого сумматора и второму входу одиннадцатого блока умножения, а выход - к четвертому входу восьмого сумматора, последовательно соединенные третий датчик ускорения, дифференциатор и пятнадцатый блок умножения, второй вход которого соединен с выходом косинусного функционального преобразователя, а выход - со вторым входом одиннадцатого сумматора, последовательно соединенные шестнадцатый блок умножения, первый и второй входы которого соответственно подключены к выходам второго датчика скорости и десятого блока умножения, и семнадцатый блок умножения, второй вход которого соединен с выходом девятого сумматора, а выход - с пятым входом восьмого сумматора. Вторые входы десятого и тринадцатого блоков умножения подключены к выходу третьего датчика ускорения. Техническим результатом изобретения является повышение динамической точности управления. 3 ил.
Изобретение относится к робототехнике и может быть использовано при создании приводов роботов.
Известно устройство для управления приводом робота, содержащее последовательно соединенные первый блок умножения и первый сумматор, последовательно подключенные усилитель и двигатель, связанный с первым датчиком скорости непосредственно и через редуктор с первым датчиком положения, выход которого соединен с первым входом второго сумматора, подключенного вторым входом к входу устройства, последовательно соединенные второй датчик положения, третий сумматор, четвертый сумматор, первый квадратор и второй блок умножения, а также пятый блок умножения, седьмой сумматор и последовательно соединенные релейный блок и восьмой сумматор, выход которого подключен к второму входу первого сумматора, соединенного выходом с входом усилителя, второй вход которого подключен к выходу датчика массы и первому входу третьего блока умножения, а выход - к первому входу пятого сумматора, соединенного вторым входом с выходом первого задатчика сигнала, а третьим входом - с выходом второго квадратора, вход которого подключен к выходу третьего сумматора и первому входу шестого сумматора, соединенного выходом с первым входом четвертого блока умножения, а вторым входом - с выходом третьего блока умножения, второй вход которого подключен к выходу четвертого сумматора, соединенного вторым входом с выходом второго задатчика сигнала, выход третьего задатчика сигнала подключен к второму входу третьего сумматора, а выход второго датчика скорости соединен с вторым входом четвертого блока умножения, выход первого датчика скорости подключен к входу релейного блока, к второму входу восьмого сумматора и первому входу седьмого сумматора, второй вход которого соединен с выходом второго сумматора, а выход - с первым входом первого блока умножения, подключенного вторым входом к выходу пятого сумматора, первый вход пятого блока умножения соединен с выходом четвертого блока умножения, второй вход с выходом первого датчика скорости, а выход - с третьим входом восьмого сумматора (см. патент РФ 1484702, БИ 21, 1989 г.). Известно также устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, первый блок умножения, второй сумматор, усилитель и двигатель с редуктором, кинематически связанный с первым датчиком скорости и первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, а третьим входом - с выходом первого датчика скорости, с входом релейного элемента и первыми входами второго блока умножения и третьего сумматора, второй вход которого подключен к выходу второго блока умножения, третий вход - к выходу релейного элемента, а выход - к второму входу второго сумматора, последовательно соединенные второй датчик положения, четвертый сумматор, пятый сумматор, первый квадратор, третий блок умножения и шестой сумматор, выход которого подключен к второму входу первого блока умножения, второй вход - к выходу первого задатчика сигнала, а третий вход - к выходу второго квадратора, а также последовательно соединенные датчик массы, четвертый блок умножения, седьмой сумматор и пятый блок умножения, подключенный вторым входом к выходу второго датчика скорости, а выходом - к второму входу второго блока умножения, второй вход пятого сумматора соединен с выходом второго задатчика сигнала, выход - с вторым входом четвертого блока умножения, а первый вход - с входом второго квадратора и вторым входом седьмого сумматора, выход датчика массы подключен к второму входу третьего блока умножения, а выход третьего задатчика сигнала соединен с вторым входом четвертого сумматора, последовательно соединенные кинематически связанный с выходным валом двигателя первый датчик ускорения, шестой блок умножения и восьмой сумматор, последовательно подключенные четвертый задатчик сигнала, девятый сумматор, седьмой блок умножения, десятый сумматор и восьмой блок умножения, а также последовательно соединенные второй датчик ускорения и девятый блок умножения, второй вход которого подключен к выходу седьмого сумматора, а выход - к второму входу десятого сумматора, второй вход восьмого сумматора соединен с выходом первого датчика ускорения, выход - с четвертым входом третьего сумматора, а третий вход - с выходом восьмого блока умножения, подключенного вторым входом к выходу первого датчика скорости, второй вход шестого блока умножения соединен с выходом пятого блока умножения, второй вход девятого сумматора подключен к выходу датчика массы, а выход второго датчика скорости через третий квадратор соединен с вторым входом седьмого блока умножения (см. патент РФ 1618642, БИ 1, 1991 г.). По своей технической сущности данное устройство является наиболее близким к предлагаемому изобретению. Недостатком этих устройств (аналога и прототипа) является то, что они эффективны только для исполнительного органа робота, имеющего три степени подвижности. Однако при трех степенях подвижности у робота значительно сокращается рабочая зона (зона обслуживания). Например, при работе на конвейере желательно, чтобы робот мог перемещаться вдоль этого конвейера, сопровождая движущееся изделие и выполняя требуемые технологические операции. Однако при введении четвертой линейной степени подвижности q4, в рассматриваемом приводе появляются дополнительные возмущающие моментные воздействия, значительно ухудшающие его показатели качества. В результате возникает задача компенсации этих вредных моментных воздействий за счет введения дополнительных сигналов коррекции. Задачей, на решение которой направлено заявляемое техническое решение, является обеспечение полной инвариантности динамических свойств электропривода к непрерывным и быстрым изменениям его динамических моментных нагрузочных характеристик при движении манипулятора по всем четырем степеням подвижности и, тем самым, повышение динамической точности управления. Технический результат, который может быть получен при реализации заявляемого технического решения, выражается в формировании дополнительного сигнала управления, подаваемого на вход привода, который обеспечивает получение дополнительного моментного воздействия, компенсирующего вредное моментное воздействие со стороны четвертой степени подвижности (см. координату q4) на качественные показатели работы рассматриваемого привода. Поставленная задача решается тем, что устройство для управления приводом робота, содержащее последовательно соединенные первый сумматор, первый блок умножения, второй сумматор, усилитель и двигатель с редуктором, кинематически связанный с первым датчиком скорости и первым датчиком положения, выход которого подключен к первому входу первого сумматора, соединенного вторым входом с входом устройства, а третьим входом - с выходом первого датчика скорости, с входом релейного элемента и первыми входами второго блока умножения и третьего сумматора, второй вход которого подключен к выходу второго блока умножения, третий вход - к выходу релейного элемента, а выход - к второму входу второго сумматора, последовательно соединенные второй датчик положения, четвертый сумматор, пятый сумматор, первый квадратор, третий блок умножения и шестой сумматор, выход которого подключен к второму входу первого блока умножения, второй вход - к выходу первого задатчика сигнала, а третий вход - к выходу второго квадратора, а также последовательно соединенные датчик массы, четвертый блок умножения, седьмой сумматор и пятый блок умножения, подключенный вторым входом к выходу второго датчика скорости, а выходом - к второму входу второго блока умножения, второй вход пятого сумматора соединен с выходом второго задатчика сигнала, выход - с вторым входом четвертого блока умножения, а первый вход - с входом второго квадратора и вторым входом седьмого сумматора, выход датчика массы подключен к второму входу третьего блока умножения, а выход третьего задатчика сигнала соединен с вторым входом четвертого сумматора, последовательно соединенные кинематически связанный с выходным валом двигателя первый датчик ускорения, шестой блок умножения и восьмой сумматор, последовательно подключенные четвертый задатчик сигнала, девятый сумматор, седьмой блок умножения, десятый сумматор и восьмой блок умножения, а также последовательно соединенные второй датчик ускорения и девятый блок умножения, второй вход которого подключен к выходу седьмого сумматора, а выход - к второму входу десятого сумматора, второй вход восьмого сумматора соединен с выходом первого датчика ускорения, выход - с четвертым входом третьего сумматора, а третий вход - с выходом восьмого блока умножения, подключенного вторым входом к выходу первого датчика скорости, второй вход шестого блока умножения соединен с выходом пятого блока умножения, второй вход девятого сумматора подключен к выходу датчика массы, а выход второго датчика скорости через третий квадратор соединен с вторым входом седьмого блока умножения, отличается тем, что в него дополнительно введены последовательно соединенные косинусный функциональный преобразователь, десятый и одиннадцатый блоки умножения, причем выход последнего подключен к пятому входу третьего сумматора, последовательно соединенные синусный функциональный преобразователь, вход которого подключен к входу косинусного функционального преобразователя и выходу первого датчика положения, двенадцатый блок умножения, второй вход которого через тринадцатый блок умножения соединен с выходом первого датчика скорости, одиннадцатый сумматор и четырнадцатый блок умножения, второй вход которого подключен к выходу седьмого сумматора и второму входу одиннадцатого блока умножения, а выход - к четвертому входу восьмого сумматора, последовательно соединенные третий датчик ускорения, дифференциатор и пятнадцатый блок умножения, второй вход которого соединен с выходом косинусного функционального преобразователя, а выход - со вторым входом одиннадцатого сумматора, последовательно соединенные шестнадцатый блок умножения, первый и второй входы которого соответственно подключены к выходам второго датчика скорости и десятого блока умножения, и семнадцатый блок умножения, второй вход которого соединен с выходом девятого сумматора, а выход - с пятым входом восьмого сумматора, причем вторые входы десятого и тринадцатого блоков умножения подключены к выходу третьего датчика ускорения. Сопоставительный анализ существенных признаков предлагаемого технического решения с существенными признаками аналогов и прототипа свидетельствует о его соответствии критерию "новизна". При этом отличительные признаки формулы изобретения обеспечивают высокую точность и устойчивость привода роботов в условиях существенного изменения параметров нагрузки. На фиг.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 ускорения. На чертежах введены следующие обозначения:![](https://img.poleznayamodel.ru/chr/945.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655379.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655380.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655381.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655382.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655383.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655384.gif)
![](https://img.poleznayamodel.ru/chr/949.gif)
![](https://img.poleznayamodel.ru/chr/945.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655385.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655386.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655387.gif)
H(q3, mг)=m3(l3*+q3)2+mг(l3*+l3+q3)2+IN+Is,
![](https://img.poleznayamodel.ru/img_pat/65/655388.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655389.gif)
где Is и IN - соответственно моменты инерции вертикального звена исполнительного органа относительно продольной оси и горизонтального звена относительно поперечной оси, проходящей через его центр масс. С учетом соотношений (1) и (2), а также уравнений электрической
![](https://img.poleznayamodel.ru/img_pat/65/655390.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655391.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655392.gif)
где R - активное сопротивление якорной цепи;
L - индуктивность якорной цепи;
J - момент инерции якоря двигателя и вращающихся частей редуктора, приведенных к валу двигателя;
kм - коэффициент крутящего момента;
k
![](https://img.poleznayamodel.ru/chr/969.gif)
kв - коэффициент вязкого трения;
ip - передаточное отношение редуктора;
Мcтр - момент сухого трения;
ky - коэффициент усиления усилителя 4;
i - ток якоря;
![](https://img.poleznayamodel.ru/img_pat/65/655393.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655394.gif)
Очевидно, что изменение h, H, М и
![](https://img.poleznayamodel.ru/img_pat/65/655395.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655396.gif)
![](https://img.poleznayamodel.ru/chr/8773.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655397.gif)
![](https://img.poleznayamodel.ru/chr/8773.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655398.gif)
Первый отрицательный (со стороны датчика 6) и второй положительный входы сумматора 1 единичные, а третий отрицательный вход (со стороны датчика 7) имеет коэффициент усиления k
![](https://img.poleznayamodel.ru/chr/969.gif)
![](https://img.poleznayamodel.ru/chr/969.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655399.gif)
где |Mт| - величина момента сухого трения при движении. Все входы сумматора 28 положительные. Его первый вход (со стороны блока умножения 27) имеет коэффициент усиления 2, второй вход (со стороны датчика 26) - коэффициент усиления y, пятый (со стороны блока 49) - коэффициент усиления 1/iр, а третий и четвертый входы - единичные коэффициенты усиления. Положительные входы сумматоров 12 и 13 имеют единичные коэффициенты усиления. Задатчик 24 сигнала вырабатывает сигнал l3, а задатчик 25 - сигнал l3*. Первый и второй положительные входы сумматора 16 (со стороны блока 15 умножения и задатчика 17 сигнала) соответственно имеют коэффициенты усиления 1/(Jнip 2), а третий положительный (со стороны квадратора 18) - коэффициент усиления m3/(Jнip), где Jн - номинальное (желаемое) значение приведенного момента инерции, обеспечивающее приводу поворота заданные динамические свойства и качественные показатели. Причем с выхода задатчика 17 сигнала на второй вход сумматора 16 поступает сигнал JS+JN+Jip 2. Первый и второй положительные входы сумматора 21 (со стороны блока 20 умножения и сумматора 12) соответственно имеют коэффициенты усиления 2/ip 2 и 2m3/ip 2
Первый и второй положительные входы сумматора 30 имеют единичные коэффициенты усиления, а задатчик 29 сигнала вырабатывает сигнал, равный m3. Первый положительный вход сумматора 32 (со стороны блока 31) имеет коэффициент усиления 2/ip 2, а его второй положительный вход (со стороны блока 35) - единичный коэффициент усиления. Первый отрицательный (со стороны блока 41) и второй положительный входы сумматора 43 имеют соответственно коэффициенты усиления, равные 1/2 и ip/2. Таким образом, на выходе сумматора 12 формируется сигнал, равный l3*+q3, а на сумматора 13 - сигнал l3+l3*+q3. На выходе блока 15 формируется сигнал mг(l3+l3*+q3)2, а на выходе квадратора 18 - сигнал (l3+q3)2. В результате на выходе сумматора 16 появляется сигнал, равный
![](https://img.poleznayamodel.ru/img_pat/65/655400.gif)
Таким образом, на выходе блока 2 формируется сигнал
![](https://img.poleznayamodel.ru/img_pat/65/655401.gif)
На выходе блока 20 формируется сигнал mг(l3+l3*+q3), а на выходе сумматора 21 - сигнал 2mг(l3+l3*+q3)/ip 2+2m3(l3+q3)/ip 2. Таким образом, на выходе блока 22 формируется сигнал, равный
![](https://img.poleznayamodel.ru/img_pat/65/655402.gif)
На выходе сумматора 30 формируется сигнал mг+m3, на выходе блока 31 - сигнал
![](https://img.poleznayamodel.ru/img_pat/65/655403.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655404.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655405.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655406.gif)
На выходе блока 41 формируется сигнал
![](https://img.poleznayamodel.ru/img_pat/65/655407.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655408.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655409.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655410.gif)
На выходе блока 49 формируется сигнал
![](https://img.poleznayamodel.ru/img_pat/65/655411.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655412.gif)
![](https://img.poleznayamodel.ru/img_pat/65/655413.gif)
Окончательно на выходе сумматора 3 имеем
![](https://img.poleznayamodel.ru/img_pat/65/655414.gif)
Несложно показать, что, подставив U* из (4) в (3), имеем
![](https://img.poleznayamodel.ru/img_pat/65/655415.gif)
Таким образом, уравнение (5) имеет постоянные желаемые параметры, которые определяются выбором желаемых значений ky и Jн, а сам привод обладает постоянными желаемыми динамическими свойствами и качественными показателями при любых законах изменения параметров нагрузки.
Формула изобретения
РИСУНКИ
Рисунок 1, Рисунок 2, Рисунок 3