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

Формула изобретения

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

Описание изобретения к патенту

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

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

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

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

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

Известно также устройство для управления движителем подводного аппарата (патент РФ 2147985, БИ 12, 2000), содержащее последовательно соединенные первый сумматор, второй сумматор, первый блок умножения, третий сумматор, усилитель и двигатель, связанный непосредственно с первым датчиком скорости, отличающееся тем, что в него дополнительно введены последовательно соединенные четвертый сумматор, второй блок умножения, пятый сумматор, второй вход которого соединен с выходом квадратора, первый функциональный преобразователь, второй функциональный преобразователь, первый блок деления, второй вход которого через шестой сумматор соединен с выходом первого сумматора и входом квадратора, третий блок умножения, седьмой сумматор, второй вход которого соединен с выходом второго сумматора, четвертый блок умножения, третий функциональный преобразователь, второй блок деления, второй вход которого через восьмой сумматор, своим вторым отрицательным входом подключенный к выходу первого блока умножения, соединен с выходом задатчика входного сигнала,

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

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

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

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

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

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

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

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

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

На чертеже введены следующие обозначения:

_вх - входной сигнал, задающий требуемую силу тяги движителя;

- ошибка привода (разность между задаваемой и реальной силой тяги);

- угловая скорость вращения вала двигателя 6;

s, D - промежуточные сигналы;

u* - усиливаемый сигнал;

u - сигнал управления двигателем 10;

Т_m - сигнал на выходе модели;

е - ошибка между желаемой (сигнал с модели) и реальной силой тяги.

Устройство для управления движителем подводного робота, содержит последовательно соединенные первый датчик 1 скорости, первый блок 12 взятия модуля, первый блок 3 умножения, первый сумматор 4, второй вход которого подключен к выходу первого датчика 1 скорости, второй сумматор 5, второй вход которого соединен с выходом второго блока 6 умножения, третий блок 7 умножения, третий сумматор 8, усилитель 9, двигатель 10, связанный непосредственно со вторым датчиком 11 скорости, второй блок взятия модуля 12, четвертый сумматор 13, четвертый блок 14 умножения, пятый сумматор 15, первый нелинейный элемент 16, второй нелинейный элемент 17, вход которого подключен ко второму входу второго блока 11 умножения, первый блок 18 деления, второй вход которого соединен с выходом шестого сумматора 19, первый вход которого подключен к выходу первого сумматора 4 и через квадратор 20 - ко второму входу пятого сумматора 15, пятый блок 21 умножения, второй вход которого соединен с выходом второго блока 12 взятия модуля, вторым входом третьего блока 7 умножения и первым входом шестого блока 22 умножения, выход которого подключен ко второму входу третьего сумматора 8, седьмой сумматор 23, второй вход которого соединен с выходом второго сумматора 5, а третий вход - с третьим входом второго сумматора 5, вторым входом шестого сумматора 19, вторым входом шестого блока 22 умножения и выходом четвертого сумматора 13, седьмой блок 24 умножения, второй вход которого подключен ко второму входу второго блока 6 умножения, второму входу первого блока 3 умножения и через первый релейный элемент 25 - к выходу второго датчика 11 скорости, третьему входу третьего сумматора 8, четвертому входу седьмого сумматора 23, второму входу четвертого сумматора 13, второму входу четвертого блока 14 умножения и третьему входу первого сумматора 4, третий нелинейный элемент 26.

Устройство также содержит второй блок 27 деления, выход которого соединен с четвертым входом третьего сумматора 8, а второй вход - с выходом восьмого сумматора 28, первый вход которого подключен к выходу задатчика 29 входного сигнала, а второй вход - к выходу третьего блока 7 умножения, последовательно соединенные девятый сумматор 30, первый вход которого подключен к выходу задатчика 29 входного сигнала, интегратор 31, выход которого соединен со вторым входом девятого сумматора 30, десятый сумматор 32, второй релейный элемент 33, восьмой блок 34 умножения, второй вход которого через третий блок 35 взятия модуля подключен к выходу девятого сумматора 30, а выход - к третьему входу восьмого сумматора 28, последовательно соединенные одиннадцатый сумматор 36, первый вход которого соединен с выходом шестого блока 22 умножения, а второй вход - со вторым входом десятого сумматора 32 и выходом третьего блока 7 умножения, четвертый блок 37 взятия модуля, девятый блок 38 умножения, второй вход которого подключен к выходу второго релейного элемента 33, а выход - к пятому входу третьего сумматора 8. На выходном валу двигателя 10 установлен винт 39.

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

Сигнал ошибки с сумматора 28 после коррекции в блоках 27 и 8, а также с учетом сигналов самонастройки с выходов элементов 34 и 38 усиливаясь поступает на электродвигатель 10, приводя его вал во вращательное движение с частотой, зависящей от величины поступающего сигнала и момента вязкого трения, возникающего при вращении гребного винта в жидкости и нелинейно зависящего от величин и v. Сила тяги (упор) движителя также нелинейно зависит от частоты вращения винта и скорости движения окружающей среды относительно подводного робота вдоль оси винта. При изменении режима работы движителя существенно изменяются и его параметры, что приводит к ухудшению качества работы системы и, как следствие, к значительному отклонению траекторий движения подводного робота от желаемых в большинстве режимов его эксплуатации.

Зависимость силы тяги и момента, действующего на винт со стороны жидкости, от частоты вращения винта и скорости перемещения окружающей среды относительно подводного робота в тех случаях, когда >>1 рад/с, определяется системой уравнений:





где

k₁=F"_t/(4A_s),











H₁ - гидродинамический шаг винта; Н - геометрический шаг винта; Н - гидродинамическая поправка на шаг винта; F_t, F_m - обобщенные коэффициенты упора и момента; m - величина, характеризующая профильные потери винта; - плотность окружающей среды; A_s - площадь диска винта; Т - упор, создаваемый винтом; М - момент на валу движителя; - скорость вращения винта; v - скорость движения окружающей среды относительно подводного робота вдоль оси движителя; s(, v_p) - абсолютное скольжение винта.

В качестве исполнительного элемента использован электродвигатель постоянного тока, описываемый линейным дифференциальным уравнением первого порядка:



где J_д - момент инерции вращающихся частей движителя с учетом присоединенных моментов инерции; R - сопротивление якорной цепи двигателя; k_m, k_w - коэффициенты момента и противо-ЭДС; k_у - коэффициент усиления усилителя мощности.

В различных режимах работы параметры движителя могут меняться в следующих пределах:

F"_mminF"_mF"_mmin, (2)

J_дminJ_дJ_дmax (3)

Датчик 1 скорости установлен на корпусе подводного робота и измеряет величину скорости перемещения окружающей среды относительно подводного робота. Релейные элементы 25 и 33 имеют следующую характеристику:

u_вых=sign(u_вx).

Коэффициенты усиления входов сумматора 4 имеют следующие значения: коэффициент усиления первого отрицательного входа (со стороны блока умножения 3) равен 0,25, второго отрицательного входа (со стороны датчика скорости 1) - 0,25, третьего положительного входа (со стороны релейного элемента 25) - k₁. В результате на выходе сумматора 4 получается сигнал (k₁-v_p/2).

Коэффициент усиления первого положительного входа сумматора 13 (со стороны блока 12 взятия модуля) равен H/(2), а второго положительного входа (со стороны датчика 11 скорости) равен Н/(2). Таким образом сигнал на выходе сумматора 13 равен H"₁. После преобразования сигнала с выхода сумматора 4 в квадраторе 20 и после суммирования его с сигналом, поступающим с выхода блока 14 умножения, в сумматоре 15, коэффициент усиления которого по первому положительному входу (со стороны блока 14 умножения) равен 2k₁, а по второму положительному входу (со стороны квадратора 20) равен единице, на выходе сумматора 15 формируется сигнал Затем в нелинейном элементе 16 происходит вычисление квадратного корня из сигнала, поступающего с выхода сумматора 15.

Сумматор 5 имеет единичный коэффициент усиления по первому (со стороны сумматора 4) и второму (со стороны сумматора 13) положительным и третьему (со стороны блока 6 умножения) отрицательному входу. Поэтому сигнал на его выходе равен:



Нелинейный элемент 17 описывается следующим выражением:



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

Первый положительный вход сумматора 19 (со стороны сумматора 4) имеет единичный коэффициент усиления, а второй положительный вход (со стороны сумматора 13) имеет коэффициент усиления, равный 2. После деления в блоке 18 деления сигнала с выхода сумматора 19 на сигнал с выхода функционального преобразователя 17 сигнал с выхода блока 18 деления домножается на || в блоке 21 умножения.

Первый отрицательный вход (со стороны блока 21 умножения) и четвертый положительный вход (со стороны датчика 11 скорости) сумматора 23 имеют коэффициент усиления, равный k₁, второй (со стороны сумматора 5) и третий (со стороны сумматора 13) положительные входы имеют единичные коэффициенты усиления. После домножения сигнала с выхода сумматора 23 на величину sign() в блоке 24 умножения формируется сигнал:



Для того чтобы избежать деления на 0, сигнал D преобразуется в функциональном преобразователе 26, который описывается выражением:



Сумматор 30 и интегратор 31 формируют желаемый эталонный сигнал. Первый положительный вход сумматора 30 (со стороны задатчика входного сигнала 29) имеет коэффициент усиления k_d/Т_d, а второй отрицательный вход (со стороны интегратора 31) имеет коэффициент усиления 1/T_d. Таким образом, на выходе сумматора 30 формируется сигнал:



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

Первый положительный вход сумматора 32 (со стороны интегратора 31) имеет единичный коэффициент усиления, а второй отрицательный вход (со стороны блока 7 умножения) - коэффициент усиления, равный F"_t. На выходе сумматора 32 формируется сигнал ошибки между желаемым и реальным значениями упора:



На выходе блока 34 умножения - сигнал, равный

Первый положительный вход сумматора 36 (со стороны блока умножения 22) имеет коэффициент усиления J_д0K_h2m, где J_д0 - номинальное значение момента инерции движителя из диапазона (2), К_h2 - коэффициент самонастройки, который определяется выражением: а второй положительный вход (со стороны блока 7 умножения) имеет коэффициент усиления J_д0К_h2. В результате на выходе блока 38 умножения формируется сигнал

Коэффициент усиления первого положительного входа сумматора 28 (со стороны задатчика 29 входного сигнала) равен k_d, его второй отрицательный вход (со стороны блока 7 умножения) имеет коэффициент усиления, равный F"_t, а третий положительный вход (со стороны блока 34 умножения) - коэффициент усиления K_h1T_dF"_t, где К_h1 - коэффициент самонастройки, определяемый выражением:



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



Первый положительный вход сумматора 8 (со стороны блока умножения 7) имеет коэффициент усиления, равный

второй положительный вход (со стороны блока 26 умножения) имеет коэффициент усиления, равный

где F"_m0 - номинальное значение коэффициента момента из диапазона (3), третий положительный вход (со стороны датчика 11 скорости) - коэффициент усиления, равный k_w/k_у, четвертый положительный вход (со стороны блока 27 деления) - коэффициент усиления



а пятый положительный вход (со стороны блока 38 умножения) имеет коэффициент усиления

.

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

(4)

Первая часть уравнения (4):



позволяет представить динамику движителя в виде уравнения:



где k_d, Т_d - желаемые коэффициент усиления и постоянная времени. Это слагаемое обеспечивает желаемую динамику движителя только при каких-то постоянных номинальных значениях его параметров. Для обеспечения постоянства динамических свойств движителя при изменении его параметров (см. вторую часть выражения (4)) вводятся дополнительные сигналы cамонастройки



и



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

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