Электропривод робота

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

 

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Рассматриваемый привод управляет обобщенной координатой q₄. Данная степень подвижности позволяет осуществить горизонтальное прямолинейное перемещение робота, схема которого представлена на фиг.2.

Робот в горизонтальной плоскости перемещается с помощью передачи шестерня-рейка (координата q₄). Причем рейка установлена на основании, по которому перемещается робот, а шестерня 8 - на выходном валу редуктора 7 и имеет радиус r.

Датчики 38, 16 и 14 установлены соответственно в первой, второй и третьей степенях подвижности робота (см. фиг.2) и измеряют соответственно обобщенные координаты q₁, q₂, и q₃. Сумматор 15 имеет положительные входы с единичными коэффициентами усиления, поэтому на его выходе формируется сигнал q₂+q₃. На выходе задатчика 20 формируется сигнал , а на выходе датчика 18 - m_Г. Первый (со стороны задатчика 20) положительный вход сумматора 19 имеет единичный коэффициент усиления, а его второй положительный вход - коэффициент усиления, равный 1. В результате на выходе сумматора 19 формируется сигнал . На выходе задатчика 25 сигнала формируется сигнал . Первый (со стороны задатчика 25) и второй положительные входы сумматора 26 соответственно имеют единичный коэффициент усиления и коэффициент усиления, равный l₂. В результате на выходе сумматора 26 формируется сигнал . Таким образом, на выходе блоков 21, 62, 37 и 27 умножения соответственно формируются сигналы Acos(q₂+q₃), Asin(q₂+q₃), Bcosq₂ и Bsinq₂.

Положительные входы сумматоров 23 и 29 имеют единичные коэффициенты усиления. Поэтому на выходах блоков 24, 55, 30 и 60 умножения соответственно формируются сигналы:

b=cosq₁(Acos(q₂+q₃)+Bcosq₂), a=sinq₁(Acos(q₂+q₃)+Bcosq₂),

d=cosq₁(Asin(q₂+q₃)+Bsinq₂) и с=sinq₁(Asin(q₂+q₃)+Bsinq₂).

На выходах блоков 61, 44 и 40 соответственно формируются сигналы: k=Acosq₁cos(q₂+q₃), m=Acosq₁sin(q₂+q₃) и f=Asinq₁sin(q₂+q₃).

Датчики 42, 34 и 31 скорости устанавливаются соответственно в первой, второй и третьей степенях подвижности робота (см. фиг.2.) и измеряют соответственно , , и . Положительные входы сумматора 53 имеют единичные коэффициенты усиления. Поэтому на выходе блока 54 умножения формируется сигнал , а на выходах блоков 50 и 41 умножения - сигналы и соответственно. В результате на выходах блоков 51 и 32 умножения соответственно формируются сигналы и

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

На выходах датчиков 57, 58 и 46 ускорения, установленных соответственно в первой, второй и третьей степенях подвижности робота соответственно, формируются сигналы , , , а на выходах блоков 56, 59 и 45 умножения - сигналы , , , соответственно.

Второй (со стороны блока 45), третий (со стороны блока 49), пятый (со стороны блока 54), шестой (со стороны блока 56) и седьмой (со стороны блока 59) отрицательные входы сумматора 68 имеют единичные коэффициенты усиления, а первый (со стороны блока 32) и четвертый (со стороны блока 51) положительные входы имеют коэффициенты усиления, равные 2. В результате на выходе сумматора 33 формируется сигнал

.

На выходе задатчика 10 формируется сигнал , где J - момент инерции ротора электродвигателя и вращающихся частей редуктора (приведены к валу двигателя), i_Р - передаточное отношение редуктора. Первый (со стороны задатчика 10) и второй положительные входы сумматора 11 имеют соответственно единичный коэффициент усиления и коэффициент усиления, равный r². В результате на выходе этого сумматора формируется сигнал .

На первый положительный вход сумматора 3 (со стороны блока 2) с коэффициентом усиления поступает сигнал (где J_H - суммарный номинальный момент инерции), на его второй положительный вход с коэффициентом усиления К_ω/К_у+К_ВR/(К_уК_м) поступает сигнал , на третий положительный вход (со стороны релейного элемента 13) с коэффициентом усиления R/К_мК_у - сигнал:

,

на четвертый отрицательный вход (со стороны блока 12) с коэффициентом усиления - сигнал , на пятый положительный вход (со стороны сумматора 33) с коэффициентом усиления - сигнал С, где R - активное сопротивление якорной обмотки электродвигателя, K_B - коэффициент вязкого трения, К_у - коэффициент усиления усилителя 4, К_м - коэффициент крутящего момента, К_ω - коэффициент противоЭДС, М_т=const>0 - величина момента сухого трения при движении электродвигателя.

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

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

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

где g - ускорение свободного падения. Учитывая, что

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

,

,

из уравнения Лагранжа 2 рода можно получить силу F, действующую на линейную горизонтальную степень подвижности робота в процессе его движения:

,

которая создает на выходном валу редуктора 7 момент, равный

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

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

Сформированный сигнал U*(1), как несложно убедиться, обеспечивает превращение уравнения (3) с существенно переменными параметрами в уравнение с постоянными желаемыми параметрами, обеспечивающими электроприводу, управляющему координатой q₄, заданные динамические свойства и качественные показатели работы за счет выбора желаемых постоянных значений J_н и К_у.

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

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