УСТРОЙСТВО ДЛЯ УПРАВЛЕНИЯ ПРИВОДОМ РОБОТА Российский патент 2001 года по МПК B25J13/08 

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

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

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

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

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

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

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

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

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

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

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

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

Устройство для управления приводом робота содержит последовательно соединенные первый сумматор 1, второй сумматор 2, первый блок 3 умножения, третий сумматор 4, первый усилитель 5 и электродвигатель 6, связанный непосредственно с первым датчиком 7 скорости и через редуктор 8 с первым датчиком 9 положения, выход которого подключен к первому входу первого сумматора 1, соединенного вторым входом с входом устройства, последовательно подключенные второй датчик 10 скорости, второй блок 11 умножения, третий блок 12 умножения и четвертый сумматор 13, второй вход которого соединен со вторым входом второго сумматора 2 и выходом первого датчика 7 скорости, а третий вход - с выходом релейного элемента 14, подключенного входом ко второму входу третьего блока 12 умножения и выходу первого датчика 7 скорости, последовательно соединенные датчик 15 массы и пятый сумматор 16, второй вход которого подключен к выходу первого задатчика 17 постоянного сигнала, а выход - ко второму входу первого блока 3 умножения, последовательно соединенные второй датчик 18 положения, первый функциональный преобразователь 19, четвертый блок 20 умножения, шестой сумматор 21, второй вход которого подключен к выходу второго задатчика 22 постоянного сигнала, пятый блок 23 умножения, второй вход которого соединен с выходом первого датчика 24 ускорения, а выход - с четвертым входом четвертого сумматора 13, последовательно соединенные третий задатчик 25 постоянного сигнала, седьмой сумматор 26, второй вход которого подключен к выходу датчика 15 массы, и шестой блок 27 умножения, второй вход которого через второй функциональный преобразователь 28 подключен к выходу второго датчика 18 положения, а выход - к второму входу второго блока 11 умножения, седьмой блок умножения 29, последовательно соединенные четвертый задатчик 30 постоянного сигнала, восьмой сумматор 31, второй вход которого подключен к выходу датчика 15 массы и восьмой блок 32 умножения, второй вход которого через третий функциональный преобразователь 33 соединен с выходом первого датчика 9 положения, а его выход - с шестым входом четвертого сумматора 13, последовательно соединенные девятый сумматор 34, первый и второй входы которого подключены соответственно к выходам первого 9 и второго 18 датчиков положения, четвертый функциональный преобразователь 35 и девятый блок 36 умножения, второй вход которого подключен к выходу седьмого сумматора 26, а выход - к седьмому входу четвертого сумматора 13, последовательно соединенные второй усилитель 37, пятый функциональный преобразователь 38, десятый блок 39 умножения, десятый сумматор 40 и одиннадцатый блок 41 умножения, второй вход которого через первый квадратор 42 подключен к выходу третьего датчика 43 скорости, а выход - к восьмому входу четвертого сумматора 13, последовательно соединенные пятый задатчик 44 постоянного сигнала, одиннадцатый сумматор 45 и двенадцатый блок 46 умножения, второй вход которого через последовательно соединенные третий усилитель 47 и шестой функциональный преобразователь 48 подключен к выходу девятого сумматора 34, а его выход - ко второму входу десятого сумматора 40, последовательно соединенные двенадцатый сумматор 49, первый и второй входы которого подключены соответственно к выходам второго датчика 18 положения и второго усилителя 37, седьмой функциональный преобразователь 50 и тринадцатый блок 51 умножения, второй вход которого подключен к выходу седьмого сумматора 26, а его выход - к третьему входу десятого сумматора 40, последовательно соединенные шестой задатчик 52 постоянного сигнала и тринадцатый сумматор 53, второй вход которого подключен ко второму входу одиннадцатого сумматора 45 и выходу датчика 15 массы, а его выход - ко второму входу десятого блока 39 умножения, причем второй вход четвертого блока 20 умножения соединен с выходом седьмого сумматора 26, его выход - с третьим входом пятого сумматора 16, третий вход шестого сумматора 21 соединен с выходом датчика 15 массы, а пятый вход четвертого сумматора 13 через седьмой блок 29 умножения, второй вход которого соединен с выходом второго блока 11 умножения, подключен к выходу второго датчика 10 скорости, вход второго усилителя 37 соединен с выходом первого датчика 9 положения, а выход четвертого сумматора 13 соединен со вторым входом третьего сумматора 4, последовательно соединенные второй датчик 54 ускорения, четырнадцатый блок 55 умножения, второй вход которого соединен с выходом второго блока 11 умножения, а выход - с девятым входом четвертого сумматора 13, последовательно соединенные пятнадцатый блок 56 умножения, первый и второй входы которого соответственно соединены с выходом второго функционального преобразователя 28 и выходом первого датчика ускорения 24, четырнадцатый сумматор 57, шестнадцатый блок 58 умножения, семнадцатый блок 59 умножения, второй вход которого соединен с выходом второго датчика 10 скорости, а выход - с десятым входом четвертого сумматора 13, последовательно соединенные пятнадцатый сумматор 60, первый и второй входы которого соответственно подсоединены к выходу второго задатчика 22 постоянного сигнала и к выходу датчика 15 массы, восемнадцатый блок 61 умножения, второй вход которого через дифференциатор 62 соединен с выходом первого датчика 24 ускорения, а выход - с одиннадцатым входом четвертого сумматора 13, последовательно соединенные второй квадратор 63, подключенный входом ко второму датчику 10 скорости, девятнадцатый блок 64 умножения, шестнадцатый сумматор 65, двадцатый блок 66 умножения, второй вход которого соединен с выходом первого датчика 7 скорости, а выход - с двенадцатым входом четвертого сумматора 13, последовательно соединенные восьмой функциональный преобразователь 67, вход которого соединен с выходом первого датчика 9 положения, двадцать первый блок 68 умножения, второй вход которого подключен к выходу восьмого сумматора 31, двадцать второй блок 69 умножения, второй вход которого соединен с выходом первого датчика 7 скорости, а выход - с тринадцатым входом четвертого сумматора 13, последовательно соединенные семнадцатый сумматор 70, первый и второй входы которого соответственно подключены к выходу первого датчика 7 скорости и к выходу второго датчика 10 скорости, двадцать третий блок 71 умножения, двадцать четвертый блок 72 умножения, второй вход которого соединен с выходом датчика массы 15, а выход - с четырнадцатым входом четвертого сумматора 13, последовательно соединенные девятый функциональный преобразователь 73, вход которого подключен к выходу третьего усилителя 47, двадцать пятый блок 74 умножения, двадцать шестой блок 75 умножения, восемнадцатый сумматор 76, двадцать седьмой блок 77 умножения, второй вход которого подключен к выходу первого квадратора 42, а выход - к пятнадцатому входу четвертого сумматора 13, последовательно соединенные двадцать восьмой блок 78 умножения, первый вход которого подключен через десятый функциональный преобразователь 79 к выходу двенадцатого сумматора 49, а второй - к выходу седьмого сумматора 26, двадцать девятый блок умножения 80, выход которого подключен ко второму входу восемнадцатого сумматора 76, последовательно соединенные одиннадцатый функциональный преобразователь 81, вход которого подключен к выходу второго усилителя 37, тридцатый блок 82 умножения, тридцать первый блок 83 умножения, второй вход которого соединен с выходом первого датчика 7 скорости, а выход - с третьим входом восемнадцатого сумматора 76, последовательно соединенные тридцать второй блок 84 умножения, первый вход которого подключен к выходу десятого сумматора 40, а второй - к выходу третьего датчика 43 скорости, тридцать третий блок 85 умножения, второй вход которого соединен с выходом третьего датчика 86 ускорения, а выход - с шестнадцатым входом четвертого сумматора 13, причем третий вход пятнадцатого сумматора 60 подключен ко второму входу девятнадцатого блока 64 умножения и к выходу четвертого блока 20 умножения, второй вход двадцать девятого блока 80 умножения подключен к выходу первого датчика скорости 7 через девятнадцатый сумматор 87, второй вход которого соединен с выходом второго датчика скорости 10, второй вход двадцать третьего блока 71 умножения через двенадцатый функциональный преобразователь 88 соединен с выходом девятого сумматора 34, семнадцатый вход четвертого сумматора 13 соединен с выходом двадцать третьего блока 71 умножения, второй вход шестнадцатого сумматора 65 соединен с выходом первого датчика 24 ускорения и входом дифференциатора 62 через тридцать четвертый блок 89 умножения, второй вход которого подключен к выходу шестого блока 27 умножения, второй вход четырнадцатого сумматора 57 подключен к выходу первого функционального преобразователя 19 через тридцать пятый блок 90 умножения, второй вход которого соединен с выходом второго квадратора 63, второй вход двадцать пятого блока 74 умножения подключен к выходу одиннадцатого сумматора 45, второй вход тридцатого блока 82 умножения подключен к выходу тринадцатого сумматора 53, второй выход шестнадцатого блока 58 умножения подключен к выходу седьмого сумматора 26, второй вход двадцать шестого блока 75 умножения подключен к выходу семнадцатого сумматора 70, восемнадцатый вход четвертого сумматора 13 соединен с выходом второго датчика ускорения 54. Объект управления 91.

На чертежах введены следующие обозначения:
α_ВХ- сигнал желаемого положения;
q₁, q₂, q₃ - соответствующие обобщенные координаты исполнительного органа робота;
скорости изменения соответствующих обобщенных координат;
ускорения соответствующих обобщенных координат;
ε- ошибка привода (величина рассогласования);
m₁, m₂, m₃, m_r - соответственно массы первого, второго, третьего звеньев исполнительного органа и захваченного груза;
l₂^*, l₃^* - расстояния от осей вращения соответствующих звеньев до их центров масс;
l₂, l₃ - длины соответствующих звеньев;
скорость вращения ротора двигателя;
U^*, U - соответственно усиливаемый сигнал и сигнал управления двигателем 6.

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

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

Рассматриваемый привод управляет обобщенной координатой q₂. Конструкция робота (см. фиг. 2) является наиболее типовой для отечественных и зарубежных промышленных роботов.

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

Для определения моментных воздействий на рассматриваемый привод (обобщенных моментов неконсервативных сил) воспользуемся уравнением Лагранжа II рода. Кинетическая энергия T всех движущихся масс исполнительного органа (фиг. 2) представляется в виде

где J_Si, J_Ni - соответственно моменты инерции i-го звена относительно продольной и поперечной осей, проходящих через центр масс звена
Потенциальная энергия имеет вид
П = m₂gl₂^*(1 - cosq₂) + m₃g[l₂(1 - cosq₂) + l₃^*(1 - cos(q₂ + q₃))] + m_гg[l₂(1 - cosq₂) + l₃(1 - cos(q₂ + q₃))],
где g - ускорение свободного падения.

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

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

где H = J_N2 + J_N3 + m₂l₂^*2 + m₃l₃^*2 + (m₃ + m_г)l₂² + m_гl₃² + 2l₂(m₃l₃^* + m_гl₃)cosq₃,


С учетом соотношений (1) и (2), а также уравнений электрической

и механической

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

где


соответственно активное сопротивление и индуктивность якорной обмотки электродвигателя, J - момент инерции якоря двигателя и вращающихся частей редуктора, приведенный к валу двигателя, К_м - коэффициент крутящего момента, K_ω- коэффициент противоЭДС, К_В - коэффициент вязкого трения, i_р - передаточное отношение редуктора, М_СТР - момент сухого трения, К_у - коэффициент усиления усилителя 5, i - ток якоря, ускорение вращения вала двигателя второй степени подвижности.

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

Предполагается, что первый положительный вход сумматора 2 (со стороны сумматора 1) единичный, а его второй отрицательный вход имеет коэффициент усиления K_ω/K_У, следовательно, на выходе сумматора 2 формируется сигнал

Первый вход сумматора 26 единичный, задатчик 25 постоянного сигнала подает на него сигнал l₂l₃^*m₃. Второй вход этого сумматора имеет коэффициент усиления l₂l₃. В результате на выходе этого сумматора формируется сигнал l₂(m₃l₃^* + m_гl₃).

Второй датчик 18 положения измеряет обобщенную координату q₃ робота, а функциональный преобразователь 19 реализует cosq₃. В результате на выходе блока 20 умножения формируется сигнал l₂(m₃l₃^* + m_гl₃)cosq₃.

Второй вход сумматора 16 имеет единичный коэффициент усиления, а задатчик 17 постоянного сигнала подает на этот вход сигнал [J + (J_N2 + J_N3 + m₂l₂^*2 + m₃l₃^*2 + m₃l₂²)/i_p²] /J_H. На первый его вход с коэффициентом усиления (l₂² + l₃²)/(i_p²J_H) датчик 15 массы подает сигнал m_r. Третий вход сумматора 16 имеет коэффициент усиления 2/(i_p²J_H). В результате на его выходе формируется сигнал {J + [J_N2 + J_N3 + m₂l₂^*2 + m₃l₃^*2 + m₃l₂² + m_г(l₂² + l₃²) + 2l₂(m₃l₃^* + m_гl₃)cosq₃]/i_p²}/J_H = A, а на выходе блока 3 умножения - сигнал

Функциональный преобразователь 28 реализует функциональную зависимость sinq₃. В результате на выходе блока 27 умножения формируется сигнал l₂(m₃l₃^* + m_гl₃)sinq₃.

Датчик 10 скорости измеряет скорость изменения обобщенной координаты q₃ и, как и датчик 18 положения, установлен в третьей степени подвижности робота. В результате на первый отрицательный вход сумматора 13 (со стороны блока 12 умножения) с коэффициентом усиления 2/i_p² поступает сигнал а на пятый отрицательный вход этого сумматора (со стороны блока 29 умножения) с коэффициентом усиления l/i_p - сигнал
Первый и второй входы сумматора 21 (соответственно со стороны блока 20 умножения и задатчика 22 постоянного сигнала) имеют единичные коэффициенты усиления, а его третий вход - коэффициент усиления l₃². Задатчик 22 постоянного сигнала формирует сигнал J_N3 + m₃l₃^*2, а датчик 24 ускорения измеряет ускорение обобщенной координаты q₃ и установлен в третьей степени подвижности робота. В результате на выходе блока 23 умножения формируется сигнал который поступает на четвертый положительный вход сумматора 13, имеющий коэффициент усиления l/i_p. Третий и второй положительные входы сумматора 13 (соответственно со стороны релейного элемента 14 и датчика 7 скорости) соответственно имеют единичный коэффициент усиления и коэффициент усиления, равный K_MK_ω/R+K_в.
Выходной сигнал релейного элемента 14 с нулевой нейтральной точкой имеет вид

где М_т - величина момента сухого трения при движении (М_т > 0).

Первый и второй положительные входы сумматора 34 имеют единичные коэффициенты усиления, а функциональный преобразователь 35 реализует функциональную зависимость sin(q₂ + q₃). В результате на выходе блока 36 умножения формируется сигнал l₂(m₃l₃^* + m_гl₃)sin(q₂ + q₃), который поступает на седьмой положительный вход сумматора 13 с коэффициентом усиления g/(l₂i_p).

Задатчик 30 постоянного сигнала вырабатывает сигнал m₂l₂^* + m₃l₂ и подает его на первый положительный вход сумматора 31, имеющий единичный коэффициент усиления, второй положительный вход этого сумматора имеет коэффициент усиления l₂. Функциональный преобразователь 33 реализует функциональную зависимость sinq₂. В результате на выходе блока 32 умножения формируется сигнал [m₂l₂^* + (m₃ + m_г)l₂]sinq₂, который поступает на шестой положительный вход сумматора 13, имеющий коэффициент усиления g/i_p.

Первый и второй положительные входы сумматора 49 имеют единичные коэффициенты усиления. Усилители 37 и 47 имеют коэффициенты усиления, равные 2. Функциональные преобразователи 38, 48, 50 реализуют функцию sin. В результате на выходе блока 51 умножения формируется сигнал l₂(m₃l₃^* + m_гl₃)sin(2q₂ + q₃).

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

Первый (со стороны задатчика 52 постоянного сигнала) положительный вход сумматора 53 имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления, равный l₂². Задатчик 52 постоянного сигнала вырабатывает сигнал, равный J_N2 - J_S2 + m₂l₂^*2 + m₃l₂². В результате на выходе блока 39 умножения вырабатывается сигнал
[J_N2 - J_S2 + m₂l₂^*2 + (m₃ + m_г)l₂²]sin(2q₂).

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

Датчик 43 скорости измеряет скорость вращения первой степени подвижности робота (координата q₁). В результате на выходе блока 41 умножения формируется сигнал

Восьмой отрицательный вход сумматора 13 имеет коэффициент усиления, равный 1/(2i_p).

Сигнал с датчика 54 ускорения, измеряющего ускорение вращения вала двигателя, поступает на восемнадцатый положительный вход сумматора 13, имеющий коэффициент усиления
На выходе блока 55 умножения формируется сигнал который поступает на девятый отрицательный вход сумматора 13, имеющий коэффициент усиления
Функциональный преобразователь 67 реализует функциональную зависимость cosq₂, на второй вход блока 68 умножения (со стороны сумматора 31) поступает сигнал m₂l₂^* + m₃l₂ + m_гl₂. В результате на выходе блока 69 умножения формируется сигнал который подается на тринадцатый положительный вход сумматора 13, имеющий коэффициент усиления
Первый положительный вход сумматора 70 (со стороны датчика 7 скорости) имеет коэффициент усиления а его второй положительный вход - единичный коэффициент усиления. Функциональный преобразователь 88 реализует функцию cos(q₂ + q₃). В результате на выходе блока 71 умножения формируется сигнал который поступает на семнадцатый положительный вход сумматора 13, имеющий коэффициент усиления На выходе блока 72 умножения формируется сигнал который подается на четырнадцатый положительный вход сумматора 13, имеющий коэффициент усиления
Функциональный преобразователь 73 реализует функцию cos2(q₂ + q₃). На выходе блока 75 умножения формируется сигнал

Функциональный преобразователь 79 реализует функциональную зависимость cos(2q₂ + q₃). Первый положительный вход сумматора 87 (со стороны датчика 7 скорости), имеет коэффициент усиления а его второй положительный вход - единичный коэффициент усиления. В результате на его выходе формируется сигнал а на выходе блока 80 умножения - сигнал
Функциональный преобразователь 81 реализует функциональную зависимость cos(2q₂). В результате на выходе блока 82 умножения формируется сигнал [J_N2 - J_S2 + m₂l₂^*2 + (m₃ + m_г)l₂²]cos(2q₂), а на выходе блока 83 умножения сигнал Первый и второй положительные входы сумматора 76 имеют единичные коэффициенты усиления, а его третий положительный вход - коэффициент усиления В результате на выходе блока 77 умножения формируется сигнал

который поступает на пятнадцатый положительный вход сумматора 13, имеющий коэффициент усиления L/(Ri_p).

Датчик 86 ускорения измеряет ускорение обобщенной координаты q₁. В результате на выходе блока 85 умножения формируется сигнал

который поступает на шестнадцатый положительный вход сумматора 13, имеющий коэффициент усиления L/(Ri_p).

На первый положительный единичный вход сумматора 65 (со стороны блока 64 умножения) подается сигнал а на его второй положительный единичный вход - сигнал В результате на выходе блока 66 умножения формируется сигнал который поступает на двенадцатый отрицательный вход сумматора 13, имеющий коэффициент усиления
Первый и третий положительные входы сумматора 60 (соответственно со стороны задатчика 22 постоянного сигнала и блока 20 умножения) имеют единичные коэффициенты усиления, а его второй положительный вход - коэффициент усиления l₃². В результате на выходе этого сумматора формируется сигнал [J_N3 + m₃l₃^*2 + m_гl₃² + (m₃l₃^* + m_гl₃)l₂cosq₃], а на выходе блока 61 умножения - сигнал который поступает на одиннадцатый положительный вход сумматора 13, имеющий коэффициент усиления L/(Ri_p).

Первый положительный вход сумматора 57 (со стороны блока 56 умножения) имеет коэффициент усиления 3, а его второй положительный вход имеет единичный коэффициент усиления. В результате на выходе блока 59 умножения формируется сигнал который подается на десятый отрицательный вход сумматора 13, имеющий коэффициент усиления L/(Ri_p).

Исходя из отмеченного на выходе сумматора 13 формируется сигнал

Первый положительный вход сумматора 4 (со стороны блока 3 умножения) имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления R/(K_мK_у). В результате на выходе сумматора 4 формируется сигнал U^*, равный

Поскольку при движении привода достаточно точно соответствует М_стр, то сигнал U^* (4), как несложно убедиться, обеспечивает превращение уравнения (3) с существенно переменными параметрами в уравнение с постоянными желаемыми параметрами, обеспечивающими приводу заданные динамические свойства и качественные показатели

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

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

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