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

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

 

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Движение робота по координате q₅ перпендикулярно, а по координатам q₁, q₄ параллельно плоскости вращения звеньев 2 и 3.

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

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

где

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

где , , , R - активное сопротивление якорной цепи двигателя; J - момент инерции якоря двигателя и вращающихся частей редуктора, приведенных к валу двигателя; J₂, J₃ - моменты инерции соответствующих звеньев робота относительно осей, проходящих через их центры масс; Км - коэффициент крутящего момента; К_м - коэффициент противо ЭДС; К_В - коэффициент вязкого трения; i_p - передаточное отношение редуктора; М_стр - момент сухого трения; К_у - коэффициент усиления усилителя 5; i - ток якоря.

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

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

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

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

Второй положительный вход сумматора 16 имеет единичный коэффициент усиления, а задатчик 17 подает на него сигнал где J_H - суммарное номинальное (желаемое) значение момента инерции, приведенного к выходному валу электродвигателя. Его третий положительный вход имеет коэффициент усиления , а на первый положительный, имеющий коэффициент усиления датчик 15 подает сигнал m_Г. В результате на выходе сумматора 16 формируется сигнал , а на выходе блока 3 - сигнал . Функциональный преобразователь 28 реализует функциональную зависимость sinq₃. В результате на выходе блока 27 формируется сигнал .

Датчик 10 измеряет скорость изменения обобщенной координаты q₃ и как и датчик 18 установлен в третьей степени подвижности робота. В результате на первый отрицательный вход сумматора 13 (со стороны блока 12) с коэффициентом усиления 2/i_p ² поступает сигнал , а на пятый отрицательный вход этого сумматора (со стороны блока 29) с коэффициентом усиления l/i_p - сигнал .

Первый и второй положительные входы сумматора 21 (соответственно со стороны блока 20 и задатчика 22) имеют единичные коэффициенты усиления, а его третий положительный вход - коэффициент усиления . Задатчик 22 формирует сигнал , а датчик 24 измеряет ускорение обобщенной координаты q₃ и установлен в третьей степени подвижности робота. В результате на выходе блока 23 формируется сигнал , который поступает на четвертый положительный вход сумматора 13, имеющий коэффициент усиления 1/i_p. Третий и второй положительные входы сумматора 13 (соответственно, со стороны релейного элемента 14 и датчика 7), соответственно, имеют единичный коэффициент усиления и коэффициент усиления, равный К_МК_ω/R+К_В. Выходной сигнал релейного элемента 14 с нулевой нейтральной точкой имеет вид

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

Первый и второй положительные входы сумматора 34 имеют единичные коэффициенты усиления, функциональный преобразователь 35 реализует функциональную зависимость sin(q₂+q₃), а функциональный преобразователь 43 - зависимость cos(q₂+q₃). В результате на выходе блока 36 формируется сигнал а на выходе блока 44 - сигнал .

Задатчик 30 вырабатывает сигнал и подает его на первый положительный вход сумматора 31, имеющий единичный коэффициент усиления, второй положительный вход этого сумматора имеет коэффициент усиления l₂. Функциональный преобразователь 33 реализует функциональную зависимость sinq₂, а функциональный преобразователь 40 - зависимость cosq2. В результате на выходе блока 32 формируется сигнал , а на выходе блока 41 - сигнал .

Первые положительные входы сумматоров 37 (со стороны блока 32) и 42 (со стороны блока 41) имеют единичные коэффициенты усиления, а их вторые положительные входы - коэффициенты усиления 1/l₂. Шестой и седьмой положительные входы сумматора 13 имеют коэффициенты усиления, равные 1/i_p. Датчики 39 и 46, установленные соответственно в первой и четвертой степенях подвижности робота измеряют, соответственно и .

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

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

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

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

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