RU2524034C1 - Устройство для управления подводным роботом - Google Patents

Abstract

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

Description

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

Известно устройство для управления движителем подводного робота, содержащее три сумматора, два из которых по входам соединены с задатчиками, последовательно соединенные блок умножения, первый сумматор, усилитель и двигатель, соединенный непосредственно с датчиком скорости, а также блок деления и блок вычисления модуля, причем выход второго сумматора соединен с первым входом блока деления, а его второй вход - с выходом блока умножения, выход датчика скорости соединен с первым входом блока умножения, входом блока вычисления модуля и вторым входом первого сумматора, выход блока вычисления модуля соединен со вторым входом блока умножения и вторым входом третьего сумматора, выход которого соединен со вторым входом блока деления, выход которого соединен с третьим входом первого сумматора (см. пат. РФ №2147001, БИ №9, 2000).

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

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

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

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

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

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

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

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

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

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

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

На чертеже введены следующие обозначения; φ_вх, θ_вх, ψ_вх - входные сигналы, формируемые на выходах задатчиков 4, 5 и 15 соответственно, и задающие изменения угловых координат ПР; φ, θ, ψ - углы курса, крена и дифферента ПР в абсолютной системе координат (СК), формируемые на выходах датчиков 19, 21 и 20 соответственно; ε_φ, ε_ψ - ошибки (величины рассогласований) по координатам φ и ψ соответственно; ω_x, ω_y, ω_z - проекции вектора угловой скорости вращательного движения ПР на оси связанной с ним СК, измеряемые датчиками 25, 7 и 22 соответственно; $$u_{\varphi }^{*}$$

, $$u_{\theta }^{*}$$

, $$u_{\psi }^{*}$$

- усиливаемые сигналы в каждом канале управления ПР; u_φ, u_θ, u_ψ - сигналы управления движителями 9, 18 и 14 подводного робота соответственно.

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

Сигналы ошибок ε_φ и ε_ψ, а также сигнал θ_вх после коррекции в блоках 1, 6, 11, 12 и 16, усиливаясь, поступают на движители 9, 14 и 18 соответственно, приводя во вращение их гребные винты и осуществляя требуемые повороты (изменение ориентации) ПР ввиду изменения его угловых координат φ, θ, ψ в абсолютной СК. При этом проекции ω_x, ω_y, ω_z угловой скорости движения ПР на оси, связанной с ним СК, зависят от величин поступающих сигналов u_φ, u_θ, u_ψ, от моментов вязкого трения, возникающих при движении ПР в жидкости, а также от взаимовлияния между всеми его каналами управления. Указанные факторы приводят к снижению точности работы традиционных систем управления в большинстве режимов эксплуатации ПР.

С учетом динамического взаимовлияния между всеми тремя каналами управления ПР, имеющего нейтральную плавучесть, динамика его вращательного движения описывается системой, состоящей из трех нелинейных дифференциальных уравнений второго порядка каждое:

$$\ddot{\varphi }=A_y\cos \theta /\cos \psi -A_z\sin \theta /\cos \psi$$

,

$$\ddot{\theta }=A_x-A_{y}tg\psi \cos \theta +A_{z}tg\psi \sin \theta ,(1)$$

$$\ddot{\psi }=A_y\sin \theta +A_z\cos \theta$$

,

где $$A_x=-\dot{\psi }\dot{\varphi }\cos \psi +J_x^{-1}(k_{y1}{k}_{\partial 1}{u}_{\theta }^{*}-k_{m1}{\omega }_x-(J_z-J_y){\omega }_y{\omega }_z)$$

;

$$\begin{array}{l}{A}_y=\dot{\psi }\dot{\varphi }\cos \theta \sin \psi +\\ +\dot{\theta }\dot{\varphi }\sin \theta \cos \psi -\dot{\theta }\dot{\psi }\cos \theta +J_y^{-1}(k_{y2}{k}_{\partial 2}{u}_{\varphi }^{*}-k_{m2}{\omega }_y-(J_x-J_z){\omega }_x{\omega }_z)\end{array}$$

;

$$\begin{array}{l}{A}_z=-\dot{\psi }\dot{\varphi }\sin \theta \sin \psi +\\ +\dot{\theta }\dot{\varphi }\cos \theta \cos \psi +\dot{\theta }\dot{\psi }\sin \theta +J_z^{-1}(k_{y3}{k}_{\partial 3}{u}_{\psi }^{*}-k_{m3}{\omega }_z-(J_y-J_x){\omega }_x{\omega }_y)\end{array}$$

;

J_x, J_y, J_z - моменты инерции ПР относительно его главных центральных осей инерции (с учетом присоединенных моментов инерции жидкости); k_m1, k_m2, k_m3 - коэффициенты вязкого трения при вращательном движении ПР; k_y1, k_y2, k_y3 - коэффициенты усиления усилителей мощности 17, 8 и 13 соответственно; k_∂1, k_∂2, k_∂3 - коэффициенты усиления движителей 18, 9 и 14 соответственно.

Очевидно, что качественно управлять всеми режимами работы нелинейной многосвязной системы (1) при использовании традиционной коррекции невозможно.

Первые положительные (со стороны задатчиков 4 и 5 соответственно), и вторые отрицательные входы сумматоров 2 и 3 имеют единичные коэффициенты усиления. В результате на их выходах соответственно формируются сигналы ε_φ=φ_вх-φ и ε_ψ=ψ_вх-ψ, а на выходах блоков 6, 10, 11, 23 и 24 - соответственно сигналы ε_φcosθcosψ, ε_ψsinθ, ε_ψcosθ, ε_φsinθcosψ, ε_φsinψ. Первый положительный (со стороны квадратора 31) и второй отрицательный входы сумматора 32 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал $${\omega }_y^2-{\omega }_z^2$$

, а на выходах блоков 34, 35, 53 - сигналы $$({\omega }_y^2-{\omega }_z^2)\sin \theta \cos \theta$$

, $$({\omega }_y^2-{\omega }_z^2){\sin }^2\theta \cos \theta$$

и $$({\omega }_y^2-{\omega }_z^2)\sin \theta {\cos }^2\theta$$

соответственно.

На выходах блоков 52, 54, 51 формируются сигналы $${\omega }_y^2\cos \theta$$

, ω_yω_zcosθ и $${\omega }_z^2\sin \theta$$

соответственно, а на выходах блоков 43, 48 и 49 - сигналы ω_yω_zcos³θ, ω_yω_zsin²θ и ω_yω_zsin³θ соответственно.

Первый и третий положительные входы сумматора 36 (со стороны блоков 35 и 52 соответственно) имеют единичные коэффициенты усиления, а его второй отрицательный - коэффициент усиления 2. В результате на его выходе формируется сигнал $$g_1={\omega }_y^2\cos \theta +({\omega }_y^2-{\omega }_z^2){\sin }^2\theta \cos \theta -2{\omega }_y{\omega }_z{\sin }^3\theta$$

.

Второй отрицательный и третий положительный входы сумматора 44 (со стороны блоков 51 и 53 соответственно) имеют единичные коэффициенты усиления, а его первый отрицательный вход - коэффициент усиления 2. В результате на его выходе формируются сигнал $$g_2=({\omega }_y^2-{\omega }_z^2)\sin \theta {\cos }^2\theta --{\omega }_z^2\sin \theta -2{\omega }_y{\omega }_z{\cos }^3\theta$$

.

На выходе блока 27 формируется сигнал tgψ, поэтому на выходах блоков 37 и 45 соответственно формируются сигналы f₁=g₁tgψ, f₂=g₂tgψ.

Все входы сумматора 1 положительные. Его первый и третий входы (со стороны блоков 6 и 10) имеют коэффициенты усиления k_py=k_uJ_y/(k_y2k_∂2), второй (со стороны датчика 7), четвертый (со стороны блока 39) и пятый - коэффициенты усиления k_νy=(k_m2-k_u1J_y)/(k_y2k_∂2), k_wy=(J_x+J_y-J_z)/(k_y2k_∂2), k_jy=J_y/(k_y2k_∂2), соответственно, где k_u, k_u1 - желаемые постоянные коэффициенты. В результате на выходе этого сумматора формируется сигнал $$u_{\varphi }^{*}=k_{py}({\epsilon }_{\varphi }\cos \theta \cos \psi +{\epsilon }_{\psi }\sin \theta )+k_{\nu y}{\omega }_y+k_{wy}{\omega }_x{\omega }_z+k_{jy}{f}_2$$

.

Первый положительный и третий отрицательный входы сумматора 12 (со стороны блоков 11 и 23 соответственно) имеют коэффициенты усиления k_pz==k_uJ_z/(k_y3k_∂3), его второй (со стороны датчика 22), четвертый (со стороны блока 37) и пятый (со стороны блока 38) положительные входы - коэффициенты усиления k_νz=(k_m3-k_u1J_z)/(k_y3k_∂3), k_jz=J_z/(k_y3k_∂3), k_wz=(J_y-J_x-J_z)/(k_y3k_∂3), соответственно. В результате на его выходе формируется сигнал $$u_{\psi }^{*}=k_{pz}({\epsilon }_{\psi }\cos \theta -{\epsilon }_{\varphi }\sin \theta \cos \psi )+k_{\nu z}{\omega }_z+k_{wz}{\omega }_x{\omega }_y+k_{jx}{f}_1$$

.

Второй и пятый входы сумматора 16 отрицательные, а остальные положительные. Его первый, второй и четвертый входы (со стороны задатчика 15, датчика 21 и блока 24 соответственно) имеют коэффициенты усиления k_px=k_uJ_x/(k_y1k_∂1), третий (со стороны датчика 25), пятый (со стороны блока 48), шестой (со стороны блока 34) и седьмой входы - коэффициенты усиления k_νx=(k_m1-k_u1J_x)/(k_y1k_∂1), 2k_jx, k_jx=J_x(k_y1k_∂1), k_wx=(J_x+J_z-J_y)/(k_y1k_∂1) соответственно. В результате на выходе этого сумматора формируется сигнал $$u_{\theta }^{*}=k_{px}({\theta }_{вх}-\theta +{\epsilon }_{\varphi }\sin \psi )+k_{\nu x}{\omega }_x+k_{wx}{\omega }_y{\omega }_z+k_{jx}(({\omega }_y^2-{\omega }_z^2)\cos \theta -2{\omega }_y{\omega }_z\sin \theta )\sin \theta$$

Подставив полученные значения $$u_{\varphi }^{*}$$

, $$u_{\theta }^{*}$$

, $$u_{\psi }^{*}$$

в уравнения системы (1) и выполнив простые преобразования, получим выражения: $$\ddot{\varphi }=k_u({\varphi }_{вх}-\varphi )-k_{u1}\dot{\varphi }$$

, $$\ddot{\theta }=k_u({\theta }_{вх}-\theta )-k_{u1}\dot{\theta }$$

, $$\ddot{\psi }=k_u({\psi }_{вх}-\psi )-k_{u1}\dot{\psi }$$

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

Claims (1)

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