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

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

Реферат

 

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Потенциальная энергия робота имеет вид П=m₂gl₂^*[(l-cosq₂)+m₃g (l₂(lcosg₂)+l*₃(l-cos(q₂+q₃))]+m_rg [l₂(l-cosq₂)+l₃(l-cos(q₂+q₃))], где g - ускорение свободного падения.

Учитывая, что На основе уравнения Лагранжа 2-го рода можно записать, что моментное воздействие на выходной вал привода, управляющего координатой q₃, при движении робота (фиг. 2) с грузом имеет вид С учетом соотношения (1), а также уравнений электрической и механической цепей электродвигателя постоянного тока с постоянными магнитами или независимого возбуждения рассматриваемый привод, управляющий координатой q₃, можно описать следующим дифференциальным уравнением: где R - активное сопротивление якорной цепи двигателя; L - индуктивность якорной цепи двигателя; I - момент инерции якоря двигателя и вращающихся частей редуктора, приведенных к валу двигателя; K_м - коэффициент крутящего момента; K - - коэффициент противоЭДС; K_в - коэффициент вязкого трения; i_р - передаточное отношение редуктора; M_стр - момент сухого трения; K_у - коэффициент усиления усилителя 5; i - ток якоря; ускорение вращения вала двигателя третьей степени подвижности.

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

Полагается, что первый положительный вход сумматора 2 (со стороны сумматора 1) единичный, а его второй отрицательный вход имеет коэффициент усиления K/K_у. Первый, третий и шестой положительные входы сумматора 11 (соответственно со стороны релейного элемента 10, блока 16 умножения и блока 36 умножения) единичные, его второй положительный вход (со стороны датчика 7 скорости) имеет коэффициент усиления (K_мK/R+K_в), пятый положительный вход (со стороны блока 25 умножения) - коэффициент усиления g/l₂, а четвертый отрицательный (со стороны блока 21 умножения) - коэффициент усиления 1/2. Причем выходной сигнал релейного элемента 10 с нулевой нейтральной точкой имеет вид где величина момента сухого трения при движении.

Второй положительный вход сумматора 13 (со стороны датчика 39) имеет коэффициент усиления l₂l₃/i_р, а его первый положительный вход (со стороны задатчика 12) - единичный коэффициент усиления. Сигнал с выхода задатчика 12 сигнала равен m₃l₂l₃^*/i_р, а с выхода задатчика 17 сигнала - (i_N3+m₃l₃^*2)/i_р. Первый (со стороны блока 14 умножения) и третий (со стороны задатчика 17 сигнала) положительные входы сумматора 15 имеют единичные коэффициенты усиления, а его второй положительный вход (со стороны датчика 39 массы) - коэффициент усиления l₃²/i_р.

Таким образом, на выходе сумматора 13 формируется сигнал l₂(m₃l₃^*+m_гl₃)/i_р. Поскольку функциональный преобразователь 42 формирует сигнал cos q₃, то на выходе блока 14 умножения появляется сигнал l₂(m₃l₃^*+m_гl₃)cos(q₃)/i_р, а на выходе сумматора 15 - сигнал [I_N3+m₃l₃^*2+m_гl₃² +l₂(m₃l₃^*+m_гl₃)cos(q₃)]/i_р.

Датчик 40 ускорения измеряет ускорение вращения второй степени подвижности робота (координату ), а дифференцирующее устройство 48 формирует сигнал поэтому на выходе блока 16 умножения формируется сигнал [I_N3+m₃l₃^*2+m_гl₃²+l₂(m₃l₃^*+m_гl₃)cos(q₃)]/i_р, а на выходе блока 49 умножения формируется сигнал Датчик 34 скорости измеряет скорость вращения во второй степени подвижности (координату ), а функциональный преобразователь 37 формирует сигнал sinq₃. Поэтому на выходе блока 38 умножения появляется сигнал l₂(m₃l₃^*+m_гl₃)sin(q₃)/i_р, а на выходе блока 36 умножения - сигнал Датчик 32 положения измеряет угол поворота во второй степени подвижности (координату q₂), функциональный преобразователь 24 формирует сигнал sin (q₂+q₃). В результате на выходе блока 25 умножения формируется сигнал l₂(m₃l₃^*+m_гl₃) sin(q₂+q₃)/i_р.

Датчик 22 скорости измеряет скорость вращения первой степени подвижности робота (координата q₁). С выхода задатчика 41 постоянного сигнала на третий отрицательный вход сумматора 18 с единичным коэффициентом усиления поступает сигнал I_S3/i_р. Первый (со стороны задатчика 17) положительный вход этого сумматоры единичный, а второй положительный (со стороны датчика 39 массы) имеет коэффициент усиления l₃²/i_р. В результате на выходе сумматора 18 формируется сигнал (I_N3-I_S3+m₃l₃^*2+m_гl₃²)/i_р. Усилитель 26 имеет коэффициент усиления, равный 2. Функциональный преобразователь 27 реализует функцию sin. В результате на выходе блока 19 умножения формируется сигнал (I_S3+m₃l₃^*2+m_гl₃²)sin2(q₂+q₃)/i_р.

Функциональный преобразователь 28 реализует функцию sin, а функциональный преобразователь 30 - функцию cos. В результате на выходе блока 31 умножения формируется сигнал ₂(m₃l₃^*+m_гl₃) sin(q₂)cos(q₂+q₃)/i_р, а на выходе сумматора 20, первый положительный вход которого (со стороны блока 19 умножения) единичный, а второй положительный вход имеет коэффициент усиления 2, формируется сигнал [(I_N3-I_S3+m₃l₃^*2+ m_гl₃²)sin2(q₂+q₃)+2l₂ (m₃l₃^*+m_гl₃)sin(q₂)cos(q₂+q₃)/i_р.

С учетом отмеченных выше коэффициентов усиления соответствующих входов сумматора 11 на его выходе формируется сигнал Так как второй положительный вход сумматора 50 (со стороны датчика 7 скорости) имеет коэффициент усиления l/i_р, а его первый положительный вход - единичный коэффициент усиления, то на его выходе формируется сигнал а на выходе блока 52 умножения - сигнал Второй отрицательный вход сумматора 43 (со стороны датчика 7 скорости) имеет коэффициент усиления l/i_р, а его первый положительный вход (со стороны датчика 34) коэффициент усиления, равный 2. В результате на его выходе формируется сигнал а на выходе блока 45 умножения - сигнал На выходе блока 68 умножения формируется сигнал Второй положительный вход сумматора 46 (со стороны блока 69 умножения) имеет коэффициент усиления l/i_р, а его первый положительный вход - единичный коэффициент усиления. В результате на выходе этого сумматора формируется сигнал а на выходе блока 47 умножения - сигнал Датчик 53 ускорения измеряет ускорение вращения первой степени подвижности робота (координату поэтому на выходе блока 54 умножения формируется сигнал Второй положительный вход сумматора 57 (со стороны датчика 32 положения) имеет коэффициент усиления 2, а его первый положительный вход - единичный коэффициент усиления, поэтому на выходе этого сумматора формируется сигнал (q₃+-2q₂). Функциональный преобразователь 58 реализует функцию cos. Второй положительный вход сумматора 72 (со стороны датчика 7 скорости) имеет коэффициент усиления l/i_р, а его первый положительный вход - коэффициент усиления, равный 2, поэтому на выходе этого сумматора формируется сигнал В результате на выходе блока 59 умножения формируется сигнал Второй отрицательный вход сумматора 60 (со стороны блока 71 умножения) имеет коэффициент усиления l/i_р, а его первый положительный вход - единичный коэффициент усиления. В результате, на его выходе формируется сигнал Третий усилитель 64 имеет коэффициент усиления, равный 2. Функциональный преобразователь 65 реализует функцию cos. В результат на выходе блока 66 умножения формируется сигнал а на выходе блока 67 умножения формируется сигнал На выходе блока 61 умножения формируется сигнал Первый положительный вход сумматора 62 (со стороны блока 61 умножения) имеет коэффициент усиления 1/2, а его второй положительный вход - единичный коэффициент усиления. В результате на выходе блока 63 умножения - сигнал Первый (со стороны блока 54 умножения) и второй входы сумматора 55 имеют единичный коэффициент усиления. В результате, на выходе этого сумматора формируется сигнал На выходе сумматора 2 формируется сигнал а на выходе блока 3 умножения - сигнал Третий датчик 56 ускорения измеряет ускорение вращения двигателя 6 третьей степени подвижности робота (сигнал Первый положительный йход сумматора 4 (со стороны блока 3 умножения) имеет коэффициент усиления l₃²/(l_нi_р²), второй положительный (со стороны сумматора 11) - коэффициент усиления R/(K_мK_у), третий положительн