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

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

Иллюстрации

Показать все

Реферат

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

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

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

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

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

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

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

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

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

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

На фиг.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 умножения, выход которого соединен с шестым входом четвертого сумматора 13, второй датчик 39 ускорения, последовательно соединенные пятый косинусный функциональный преобразователь 40, вход которого подключен к выходу первого датчика 9 положения, одиннадцатый блок 41 умножения, второй вход которого подключен к выходу восьмого сумматора 31, одиннадцатый сумматор 42, второй вход которого через последовательно соединенные шестой косинусный функциональный преобразователь 43 и двенадцатый блок 44 умножения, второй вход которого подключен к выходу седьмого сумматора 26, соединен с выходом девятого сумматора 34, и тринадцатый блок 45 умножения, второй вход которого подключен к выходу третьего датчика 46 ускорения, а выход - к седьмому входу четвертого сумматора 13, последовательно соединенные пятый задатчик 47 сигнала и двенадцатый сумматор 48, второй вход которого соединен с выходом второго датчика 39 ускорения, а выход - со вторым входом десятого блока 38 умножения, объект управления 49.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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