Способ позиционно-силового управления подводным аппаратом с многозвенным манипулятором для выполнения контактных манипуляционных операций с подводными объектами
Владельцы патента RU 2789510:
Федеральное государственное бюджетное учреждение науки Институт проблем морских технологий им. академика М.Д. Агеева Дальневосточного отделения Российской академии наук (RU)
Изобретение относится к автоматическому управлению динамическими объектами в плотной среде, в частности под водой. При выполнении многозвенным манипулятором (ММ), установленным на подводном аппарате (ПА), контактных позиционно-силовых операций над объектом работ компенсируют тягами движителей ПА силовые и моментные реакции на ПА от контактного воздействия рабочего инструмента ММ на поверхность объекта. При этом из измеряемого сигнала результирующего внешнего момента на валу привода каждой степени подвижности ММ вычитают сигнал момента, обусловленного взаимовлиянием степеней подвижности ММ, а также силами сопротивления его движению, включая силы вязкого трения и силы, вызванные присоединенными массами окружающей жидкости. Технический результат направлен на повышение точности отработки требуемых силовых контактных воздействий рабочего инструмента ММ на объект работ в режиме зависания ПА вблизи или над объектом. 1 ил.
Изобретение относится к области автоматического управления динамическими объектами и может быть использовано при автоматическом управлении многозвенными манипуляторами, устанавливаемыми на подводных аппаратах.
Известен способ стабилизации подводного аппарата (ПА) по разомкнутому контуру в режиме его зависания вблизи или над объектом работ в процессе работы установленного на нем манипулятора, включающий подачу на входы двигателей всех степеней подвижности манипулятора сигналов, определяемых желаемой программной траекторией движения его рабочего органа в пространстве и формируемых на основе аналитических соотношений, полученных после решения обратной задачи кинематики для конкретной кинематической схемы манипулятора, а на входы соответствующих движителей ПА - сигналов, компенсирующих силовые и моментные воздействия на этот аппарат со стороны манипулятора, произвольно перемещающегося в вязкой среде, причем сигналы управления движителями ПА формируются в реальном масштабе времени на основе аналитических выражений, определяющих силу и момент, с которыми этот манипулятор действует на ПА, учитывающих все эффекты взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатические и гидродинамические силы сопротивления его движению, включая вязкие трения и присоединенные массы окружающей жидкости [Филаретов В.Ф., Алексеев Ю.К., Лебедев А.В. Системы управления подводными роботами. - М.: Круглый год, 2001, с.171-179; 223-227].
Недостатком указанного способа является то, что он не позволяет обеспечить стабилизацию ПА при выполнении контактных манипуляционных операций, поскольку не идентифицирует и не компенсирует силовые и моментные воздействия, вызванные контактом рабочего инструмента манипулятора с поверхностью объекта работ. Также этот способ не позволяет управлять усилием, оказываемым рабочим инструментом манипулятора на поверхность объекта работ, в условиях неизбежных смещений ПА от заданного положения стабилизации под влиянием динамических воздействий со стороны манипулятора.
Известен способ управления подводным манипулятором в режиме зависания ПА, при реализации которого на борту ПА устанавливают гироскопические датчики и навигационную систему, определяющие линейное и угловое смещения этого аппарата от его исходного положения при работающем манипуляторе, а на следящие приводы всех степеней подвижности манипулятора подают дополнительные сигналы управления, пропорциональные смещению этого ПА от его исходного положения, которые обеспечивают дополнительное перемещение рабочего органа манипулятора в пространстве и тем самым продолжение его точного движения по заранее заданной желаемой программной траектории независимо от произвольных смещений ПА [Филаретов В.Ф., Коноплин А.Ю. Способ управления подводным манипулятором в режиме зависания подводного аппарата // патент РФ № 2475799, Бюл. № 5 от 20.02.2013].
Данный способ по своей сущности является наиболее близким к предлагаемому решению. Его недостатком является то, что при контакте рабочего инструмента манипулятора с поверхностью объекта работ указанный способ не позволяет компенсировать возникающие силовые и моментные воздействия на ПА, смещающие его относительно объекта работ, что приводит к снижению точности выполнения операции. Также этот способ не позволяет управлять усилием, оказываемым рабочим инструментом манипулятора на поверхность объекта работ. В связи с отмеченным этот способ нельзя использовать для решения поставленной задачи, связанной с автоматическим позиционно-силовым управлением ПА с многозвенным манипулятором для выполнения контактных манипуляционных операций с подводными объектами.
Задачей, на решение которой направлено заявляемое изобретение, является обеспечение выполнения ПА, оснащенным многозвенным манипулятором, контактных позиционно-силовых манипуляционных операций в режиме зависания аппарата вблизи или над объектом работ. В процессе проведения упомянутых операций необходимо оказывать желаемые силовые воздействия рабочим инструментом манипулятора на объект работ и одновременно компенсировать тягами движителей ПА вычисляемые силовые и моментные воздействия на этот аппарат, обусловленные силовым воздействием рабочего инструмента манипулятора на поверхность объекта работ.
Технический результат, который может быть получен при реализации заявляемого изобретения, выражается в вычислении величины и направления вектора силы, оказываемой рабочим инструментом манипулятора на объект работ, и подаче на приводы всех степеней подвижности манипулятора сигналов управления, обеспечивающих перемещение рабочего инструмента упомянутого манипулятора в направлении желаемого вектора силы для достижения желаемой величины силового воздействия рабочего инструмента на поверхность объекта работ. Одновременно с помощью тяг, создаваемых движителями ПА, обеспечивается компенсация вычисляемых в реальном масштабе времени силовых и моментных воздействий на ПА со стороны манипулятора, выполняющего силовую операцию.
Поставленная задача решается тем, что при реализации способа управления подводным манипулятором в режиме зависания ПА, при реализации которого на борту ПА устанавливают гироскопические датчики и навигационную систему, определяющие линейное и угловое смещения этого аппарата от его исходного положения при работающем манипуляторе, а на следящие приводы всех степеней подвижности манипулятора подают дополнительные сигналы управления, пропорциональные смещению этого ПА от его исходного положения, которые обеспечивают дополнительное перемещение рабочего органа манипулятора в пространстве и тем самым продолжение его точного движения по заранее заданной желаемой программной траектории независимо от произвольных смещений ПА, дополнительно вычисляют внешние моменты, действующие на выходные валы приводов во всех степенях подвижности манипулятора в результате силового контакта рабочего инструмента этого манипулятора с поверхностью объекта работ, посредством вычитания из величины результирующего внешнего момента, действующего на выходной вал привода каждой степени подвижности манипулятора и измеряемого датчиком или наблюдателем момента нагрузки соответствующего привода, величины момента, обусловленного эффектами взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатическими и гидродинамическими силами сопротивления его движению, включая вязкие трения и присоединенные массы окружающей жидкости, и вычисляемого с помощью аналитических выражений решения обратной задачи динамики для упомянутого подводного манипулятора, затем для конкретной кинематической схемы манипулятора на основе вычисленных величин внешних моментов, действующих на выходные валы приводов во всех степенях подвижности манипулятора в результате силового контакта рабочего инструмента этого манипулятора с поверхностью объекта работ, рассчитывают величину и направление вектора силы, оказываемой рабочим инструментом манипулятора на объект работ, после чего на приводы всех степеней подвижности манипулятора подают сигналы управления, обеспечивающие перемещение рабочего инструмента упомянутого манипулятора в направлении желаемого вектора силы для достижения желаемой величины силового воздействия рабочего инструмента на поверхность объекта работ, в то же время на входы соответствующих движителей ПА подают сигналы, компенсирующие силовые и моментные воздействия на этот аппарат со стороны манипулятора, перемещающегося в вязкой среде и оказывающего силовое воздействие на объект работ, причем сигналы управления движителями ПА формируются в реальном масштабе времени на основе аналитических выражений, определяющих силу и момент, с которыми этот манипулятор действует на ПА, учитывающих вычисляемые величину и направление вектора силы, оказываемой рабочим инструментом манипулятора на объект работ, эффекты взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатические и гидродинамические силы сопротивления его движению, включая вязкие трения и присоединенные массы окружающей жидкости.
Сопоставительный анализ признаков заявляемого способа с признаками аналогов и прототипа свидетельствует о соответствии этого способа критерию «новизна».
Признак «вычисляют внешние моменты, действующие на выходные валы приводов во всех степенях подвижности манипулятора в результате силового контакта рабочего инструмента этого манипулятора с поверхностью объекта работ, посредством вычитания из величины результирующего внешнего момента, действующего на выходной вал привода каждой степени подвижности манипулятора и измеряемого датчиком или наблюдателем момента нагрузки соответствующего привода, величины момента, обусловленного эффектами взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатическими и гидродинамическими силами сопротивления его движению, включая вязкие трения и присоединенные массы окружающей жидкости, и вычисляемого с помощью аналитических выражений решения обратной задачи динамики для упомянутого подводного манипулятора» обеспечивает вычисление внешних моментов, действующих на выходные валы приводов во всех степенях подвижности манипулятора в результате силового контакта рабочего инструмента этого манипулятора с поверхностью объекта работ.
Признак «для конкретной кинематической схемы манипулятора на основе вычисленных величин внешних моментов, действующих на выходные валы приводов во всех степенях подвижности манипулятора в результате силового контакта рабочего инструмента этого манипулятора с поверхностью объекта работ, рассчитывают величину и направление вектора силы, оказываемой рабочим инструментом манипулятора на объект работ,» позволяет вычислять величину и направление вектора силы, оказываемой рабочим инструментом манипулятора на объект работ в процессе выполнения контактной операции.
Признак «на приводы всех степеней подвижности манипулятора подают сигналы управления, обеспечивающие перемещение рабочего инструмента упомянутого манипулятора в направлении желаемого вектора силы для достижения желаемой величины силового воздействия рабочего инструмента на поверхность объекта работ,» позволяет оказывать желаемые силовые воздействия рабочим инструментом манипулятора на объект работ.
Признак «в то же время на входы соответствующих движителей ПА подают сигналы, компенсирующие силовые и моментные воздействия на этот аппарат со стороны манипулятора, перемещающегося в вязкой среде и оказывающего силовое воздействие на объект работ, причем сигналы управления движителями ПА формируются в реальном масштабе времени на основе аналитических выражений, определяющих силу и момент, с которыми этот манипулятор действует на ПА, учитывающих вычисляемые величину и направление вектора силы, оказываемой рабочим инструментом манипулятора на объект работ, эффекты взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатические и гидродинамические силы сопротивления его движению, включая вязкие трения и присоединенные массы окружающей жидкости» позволяет компенсировать вычисляемые в реальном масштабе времени силовые и моментные воздействия на ПА со стороны манипулятора, выполняющего силовую операцию. В результате чего обеспечивается стабилизация ПА в режиме его зависания вблизи или над объектом работ в процессе работы установленного на нем манипулятора, при этом создается необходимый упор в точке крепления манипулятора к ПА, позволяющий рабочему инструменту этого манипулятора оказывать желаемые силовые воздействия на поверхность объекта работ.
Заявленное изобретение иллюстрируется чертежом, где на фиг. 1 схематически показан ПА 1, оснащенный многозвенным манипулятором 2, а также направления воздействия векторов сил и моментов при контакте рабочего инструмента 3 с поверхностью объекта работ 4.
Заявленный способ реализуется следующим образом.
Для выполнения контактных операций с объектом работ ПА 1 стабилизируется в режиме зависания над или вблизи этого объекта 4 с помощью тяг его движителей [Патент РФ № 2547039. Способ стабилизации подводного аппарата в режиме зависания // В.Ф. Филаретов, А.Ю. Коноплин. Бюл. № 10 от 10.04.2015; Филаретов В.Ф., Алексеев Ю.К., Лебедев А.В. Системы управления подводными роботами. - М.: Круглый год, 2001, с.171-179; 223-227].
Чтобы скомпенсировать неизбежные ошибки системы стабилизации ПА 1, приводящие к отклонениям рабочего инструмента 3 манипулятора 2 от поверхности объекта работ 4, на следящие приводы всех степеней подвижности манипулятора 2 подают дополнительные сигналы управления, пропорциональные смещению этого ПА 1 от его исходного положения, которые обеспечивают дополнительное перемещение рабочего органа манипулятора 2 в пространстве и тем самым продолжение его точного движения по заранее заданной желаемой программной траектории независимо от произвольных смещений ПА 1 [Филаретов В.Ф., Коноплин А.Ю. Способ управления подводным манипулятором в режиме зависания подводного аппарата // патент РФ № 2475799, Бюл. № 5 от 20.02.2013; Филаретов В.Ф., Коноплин А.Ю. Система коррекции траектории движения манипулятора // патент РФ № 2462745, Бюл. № 27 от 27.09.2012].
При движении рабочего инструмента 3 подводного манипулятора 2 по поверхности объекта работ 4 на выходной вал редуктора привода i-й степени подвижности манипулятора (
, где n - количество степеней подвижности манипулятора) действует внешний момент iMdz, направленный вдоль оси шарнира i. iMdz = iMwz + iMez , где iMwz - момент, обусловленный эффектами взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатическими и гидродинамическими силами сопротивления его движению, включая вязкие трения и присоединенные массы окружающей жидкости, iMez – момент, обусловленный влиянием силы реакции опоры поверхности объекта работ 
, которая равна по величине и противоположна по направлению силе 
воздействия рабочего инструмента на этот объект. Индексом G отмечены векторы, заданные в системе координат (СК), связанной с рабочим инструментом манипулятора.
Величина iMdz измеряется с помощью датчика или наблюдателя момента нагрузки привода [Филаретов В. Ф. и др. Метод синтеза систем высокоточного управления перемещениями подводных манипуляторов // Подводные исследования и робототехника. – 2020. – №. 4. – С. 31-37.; Katsura S., Matsumoto Y., Ohnishi K. Modeling of force sensing and validation of disturbance observer for force control // IEEE Transactions on industrial electronics. – 2007. – Т. 54. – №. 1. – С. 530-538.]. При этом величины моментов iMwz вычисляются с помощью аналитических выражений решения обратной задачи динамики (ОЗД) для подводного манипулятора [Филаретов В.Ф., Коноплин А.Ю. Система автоматической стабилизации подводного аппарата в режиме зависания при работающем многозвенном манипуляторе. Часть 1 // Мехатроника, автоматизация, управление. 2014. №6. С. 53-56.].
Учитывая вышеизложенное, величина момента iMez вычисляется с помощью выражения:
iMez = iMdz – iMwz.
Далее для конкретной кинематической схемы манипулятора на основе вычисленных величин внешних моментов iMez, действующих на выходные валы приводов во всех степенях подвижности манипулятора в результате силового контакта рабочего инструмента этого манипулятора с поверхностью объекта работ, определяются элементы действующего на инструмент манипулятора вектора силы реакции 
. Для типовой кинематической схемы подводного многозвенного манипулятора (см. фиг. 1), включающей три переносных и одну ориентирующую степени подвижности, вычисление упомянутого вектора осуществляется следующим образом.
С каждым звеном манипулятора жестко связывается правая СК с ортами XiYiZi. Для построения осей СК, связанных со звеньями манипулятора, используется одна из наиболее эффективных разновидностей подхода Денавита-Хартенберга [Медведев В. С., Лесков А. Г., Ющенко А. С. Системы управления манипуляционных роботов. – 1978.; Попов Е. П., Верещагин А. Ф., Зенкевич С. Л. Манипуляционные роботы. – 1978.]. Очевидно, что при 
в шарнире четвертой степени подвижности манипулятора возникает дополнительный момент силы, вычисляемый с помощью рекуррентных соотношений [Филаретов В.Ф., Коноплин А.Ю. Система автоматической стабилизации подводного аппарата в режиме зависания при работающем многозвенном манипуляторе. Часть 1 // Мехатроника, автоматизация, управление. 2014. №6. С. 53-56.]:
, (1)
где 
– вектор, направленный вдоль продольной оси звена i и определяющий положение СК, связанной со звеном i+1 в СК i-го звена;
– матрица поворота векторов из СК, связанной со звеном i+1, в СК, связанную с основанием i-го звена; (
) – векторное произведение векторов.
На выходной вал редуктора привода четвертой степени подвижности манипулятора будет действовать составляющая 
момента 
, направленная вдоль оси вращения сочленения. Поэтому с учетом выражения (1) составляющую 
силы 
, действующую вдоль оси ZG, можно определить с помощью выражения:
. (2)
Действующие на рабочий инструмент манипулятора составляющие 
вектора 
определяются аналогично на основе измеряемых значений моментов 
и 
, возникающих в приводах третьей и первой степеней подвижности соответственно и обусловленных воздействием вектора на упомянутый рабочий инструмент. Для используемой кинематической схемы манипулятора вектор 
с учетом направления осей СК, расположенных в степенях подвижности манипулятора будет иметь вид:
, (3)
где 
.
В таком случае с учетом (1) и (3) вектор 
будет иметь вид:
. (4)
Из (4) нетрудно получить:
,
откуда с учетом выражения (2) можно выразить:
, (5)
Вектор 
в СК, связанной с основанием второго звена манипулятора, определяется в виде:
, (6)
где 
.
С учетом (6) определим вектор 
в СК, связанной с основанием первого звена манипулятора:
. (7)
где 
.
Из (7) нетрудно получить:
,
откуда выразим:
. (8)
C учетом (2), (5), (8) получим:
. (9)
Чтобы рабочий инструмент манипулятора воздействовал на поверхность объекта работ с желаемым усилием 
, необходимо подавать дополнительные управляющие сигналы на приводы его степеней подвижности, которые будут обеспечивать перемещение инструмента в направлении вектора 
, причем 
. Причем величина k указанного дополнительного перемещения рабочего инструмента формируется регулятором пропорционально длине (модулю) вектора 
. В результате если разница 
, то в направлении вектора формируется величина k дополнительного смещения рабочего инструмента пропорционально величине полученной разницы. Если 
, тогда величина k дополнительного смещения инструмента будет формироваться в направлении от поверхности объекта. Таким образом обеспечивается непрерывное оказание заданного силового воздействия рабочим инструментом манипулятора на поверхность объекта работ.
Чтобы создать необходимый упор в точке крепления манипулятора к ПА, позволяющий рабочему органу этого манипулятора оказывать желаемые силовые воздействия на поверхность объекта работ, на входы соответствующих движителей ПА должны подаваться сигналы, которые будут компенсировать оказываемые на этот ПА со стороны манипулятора силовые и моментные воздействия.
Для создания в реальном масштабе времени дополнительных сигналов управления движителями ПА в СК XcYcZc, жестко связанной с корпусом ПА и имеющей начало в центре C величины (водоизмещения) аппарата, рассчитываются векторы силы 
и момента
, с которыми работающий манипулятор воздействует этот аппарат. Эти векторы обусловлены величиной и направлением вектора силы 
(9), оказываемой на рабочий инструмент манипулятора объектом работ, эффектами взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатическими и гидродинамическими силами сопротивления движению звеньев, включая вязкие трения и присоединенные массы окружающей жидкости. Расчет упомянутых динамических воздействий и с учетом вычисляемой величины (9) осуществляется с помощью аналитических выражений, описывающих решение ОЗД манипулятора [Филаретов В.Ф., Коноплин А.Ю. Система автоматической стабилизации подводного аппарата в режиме зависания при работающем многозвенном манипуляторе. Часть 1 // Мехатроника, автоматизация, управление. 2014. №6. С. 53-56.].
Вычисленные воздействия компенсируются соответствующими тягами движителей ПА с помощью разомкнутого контура системы стабилизации [Филаретов В.Ф., Алексеев Ю.К., Лебедев А.В. Системы управления подводными роботами. - М.: Круглый год, 2001, с.171-179; Филаретов В.Ф., Коноплин А.Ю. Система автоматической стабилизации подводного аппарата в режиме зависания при работающем многозвенном манипуляторе. Часть 2 // Мехатроника, автоматизация, управление. 2014. №7. С. 29-34]. В результате чего обеспечивается стабилизация ПА в режиме его зависания вблизи или над объектом работ в процессе работы установленного на нем манипулятора, при этом создается необходимый упор в точке крепления манипулятора к ПА, позволяющий рабочему инструменту этого манипулятора оказывать желаемые силовые воздействия на поверхность объекта работ.
Способ позиционно-силового управления подводным аппаратом (ПА) с многозвенным манипулятором для выполнения контактных манипуляционных операций с подводными объектами, при реализации которого на борту ПА устанавливают гироскопические датчики и навигационную систему, определяющие линейное и угловое смещения этого аппарата от его исходного положения при работающем манипуляторе, а на следящие приводы всех степеней подвижности манипулятора подают дополнительные сигналы управления, пропорциональные смещению этого ПА от его исходного положения, которые обеспечивают дополнительное перемещение рабочего органа манипулятора в пространстве и тем самым продолжение его точного движения по заранее заданной желаемой программной траектории независимо от произвольных смещений ПА, отличающийся тем, что дополнительно вычисляют внешние моменты, действующие на выходные валы приводов во всех степенях подвижности манипулятора в результате силового контакта рабочего инструмента этого манипулятора с поверхностью объекта работ, посредством вычитания из величины результирующего внешнего момента, действующего на выходной вал привода каждой степени подвижности манипулятора и измеряемого датчиком или наблюдателем момента нагрузки соответствующего привода, величины момента, обусловленного эффектами взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатическими и гидродинамическими силами сопротивления его движению, включая силы вязкого трения и силы, вызванные присоединенными массами окружающей жидкости, и вычисляемого с помощью аналитических выражений решения обратной задачи динамики для упомянутого подводного манипулятора, затем для конкретной кинематической схемы манипулятора на основе вычисленных величин внешних моментов, действующих на выходные валы приводов во всех степенях подвижности манипулятора в результате силового контакта рабочего инструмента этого манипулятора с поверхностью объекта работ, рассчитывают величину и направление вектора силы, оказываемой рабочим инструментом манипулятора на объект работ, после чего на приводы всех степеней подвижности манипулятора подают сигналы управления, обеспечивающие перемещение рабочего инструмента упомянутого манипулятора в направлении желаемого вектора силы для достижения желаемой величины силового воздействия рабочего инструмента на поверхность объекта работ, одновременно на входы соответствующих движителей ПА подают сигналы, компенсирующие силовые и моментные воздействия на этот аппарат со стороны манипулятора, перемещающегося в вязкой среде и оказывающего силовое воздействие на объект работ, причем сигналы управления движителями ПА формируются в реальном масштабе времени на основе аналитических выражений, определяющих силу и момент, с которыми этот манипулятор действует на ПА, учитывающих вычисляемые величину и направление вектора силы, оказываемой рабочим инструментом манипулятора на объект работ, эффекты взаимовлияния между всеми степенями подвижности манипулятора, а также гидростатические и гидродинамические силы сопротивления его движению, включая силы вязкого трения и силы, вызванные присоединенными массами окружающей жидкости.
