Способ управления многостепенным подводным манипулятором с помощью откалиброванной монокулярной видеокамеры и лазерного дальномера Российский патент 2025 года по МПК G06T7/50 G06V10/14 B25J9/16 

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

Известен способ наведения рабочего органа манипулятора лесной машины на объект оператором, в котором оператор указывает положение места захвата или спила объекта пятном луча дальномера и одновременно с расстоянием до объекта автоматически измеряются углы поворота дальномера, с дальнейшим компьютерными расчетом и управлением движениями лесной машины, манипулятора и рабочего органа, где оператор указывает несколько объектов, не ожидая начала операции или окончания текущей, с последующей автоматизированной обработкой всех указанных объектов, либо оператор, для оптимизации точки захвата, указывает на каждом объекте, по меньшей мере, две точки, например, для определения длины сортамента и/или оптимальной раскряжевки, либо оператор указывает на каждом объекте в заданном порядке комель и вершину или точки близкие к ним. (Патент РФ №2 468 573, кл. A01G 23/08; B25J 13/00. Бюл. №34, 2012).

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

Известен также способ захвата объектов с помощью роботизированного устройства, выполняемый по меньшей мере одним вычислительным устройством, содержащий этапы, на которых получают цветное изображение и карту глубины, осуществляют поиск объектов для захвата на изображении с учетом данных карты глубины, выбирают объект для захвата, на основе данных карты глубины формируют облако точек выбранного объекта, осуществляют поиск в облаке точек заданного количества ключевых точек, определяют арифметический центр между упомянутыми ключевыми точками, определяют точку среди ключевых точек, наиболее близкую к арифметическому центру, на основе координат ключевых точек и точки, наиболее близкой к арифметическому центру, определяют положение выбранного объекта, на основе данных о положении объекта и данных модели, построенной в результате роботизированного устройства; - осуществляют захват выбранного объекта с помощью роботизированного устройства. Кроме того, способ дополнительно содержит этап, на котором в случае неудачного захвата выбирают новую позу захвата роботизированного устройства; он же дополнительно содержит этапы, на которых сегментируют изображение на отдельные изображения объектов, полученные изображения объектов ранжируют для определения порядка захвата объектов; причем для захвата объекта выбирают первый объект согласно определенному порядку захвата объектов. Кроме того, известный способ характеризуется тем, что этап поиска в облаке точек заданного количества ключевых точек включает этапы, на которых преобразуют облако точек в матричное представление; рассчитывают матрицу ковариации на основе матричного представления; осуществляют поиск собственных векторов матрицы ковариации; выбирают два первых собственных вектора матрицы ковариации; осуществляется поиск заданного количества ключевых точек, имеющих минимальные и максимальные координаты вдоль осей, базисом которых являются собственные векторы. Способ дополнительно содержит этап, на котором формируют список наблюдаемых параметров, в который заносятся данные об упомянутых ранее ключевых точках и точке, наиболее близкой к арифметическому центру, для передачи сформированного списка в алгоритм обучения. Система захвата объекта с помощью роботизированного устройства включает в себя роботизированное устройство, содержащее по меньшей мере один сенсор глубины и захватное устройство; вычислительное устройство, соединенное с роботизированным устройством; по меньшей мере одну память, содержащую машиночитаемые инструкции, которые при их исполнении по меньшей мере одним вычислительным устройством выполняют способ по любому из перечисленных этапов. (Патент РФ №2745380, кл. G06K 9/20, G06T 7/50, B25J 9/16. Бюл. №9, 2021).

Описанный способ является наиболее близким к заявляемому изобретению и выбран в качестве прототипа.

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

Задачей заявляемого технического решения является устранение указанных недостатков прототипа, а именно, значительное увеличение скорости и упрощение автоматического подвода схвата ММ к ОР с использованием откалиброванной подводной монокулярной видеокамеры (MB) и подводного лазерного дальномера (ЛД).

Технический результат, получаемый при решении поставленной задачи, заключается в реализации способа, обеспечивающего точное управление подводным ММ, перемещаемым и стабилизируемым в подводном пространстве с помощью подводного аппарата (ПА), в процессе выполнения этим ММ технологических операций на ОР с использованием подвижного ЛД и MB (она откалибрована на основе патента РФ №2785952, кл. G01C 11/00. Бюл. №4, 2023). Заявляемый способ вначале позволяет точно и быстро определять координаты характерных точек ОР в системе координат (СК) подводного ММ, основание которого перемещают и стабилизируют в водной среде над указанным ОР с помощью (ПА), а затем точно выполнять все движения этого ММ в указанной СК даже при наличии значительных искажений первичной видеоинформации, получаемой с помощью типовой MB.

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

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

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

Признак «…основание манипулятора фиксируют на подводном аппарате, способном перемещаться в пространстве с помощью его движителей…» позволяет зафиксировать в водном пространстве СК, связанную с основанием ММ, для точного решения обратной задачи кинематики и точного вывода его рабочего органа в заданную точку рабочего пространства.

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

Признак «…при перемещении подводного аппарата к объекту работ оптическую ось дальномера устанавливают в транспортное положение с минимальным гидродинамическим сопротивлением движению этого аппарата…» позволяет минимизировать потери энергии при движении ПА к ОР.

Признак «…откалиброванную монокулярную видеокамеру устанавливают на подводном аппарате также с возможностью обзора всей рабочей зоны манипулятора…» позволяет получить координаты проекции любой характерной точки ОР на светочувствительной матрице во всей рабочей зоне ММ.

Признак «…этот аппарат с помощью вычислительного устройства, а также его системы управления и движителей подводят к объекту работ и стабилизируют над ним так, чтобы указанный объект располагался в рабочей зоне манипулятора, и проекция характерной точки объекта появилась на светочувствительной матрице видеокамеры…» позволяет определить координаты второй точки в системе координат ММ для прямой, на которой будет располагаться характерная точка ОР.

Признак «…после стабилизации аппарата оптическую ось дальномера с помощью указанного позиционера устанавливают в исходное положение перпендикулярно продольной оси подводного аппарата, затем включают этот дальномер и с помощью вычислительного устройства в системе координат манипулятора определяют пространственную координату исходного пятна от луча дальномера на грунте или на любом другом объекте…» позволяет обеспечить дальнейшие построения для перемещения луча ЛД в пространстве к характерной точке ОР.

Признак «...после этого в той же системе координат, используя вычислительное устройство и изменяя углы степеней подвижности позиционера, смещают луч дальномера из его исходного положения так, чтобы он прошел через точку пересечения прямой, на которой расположен фокус видеокамеры и проекция на светочувствительной матрице видеокамеры характерной точки объекта работ, и плоскости, перпендикулярной оптической оси видеокамеры, в которой располагалось исходное пятно от луча дальномера...» позволяет автоматически навести луч ЛД на точку, лежащую на прямой, проходящей через характерную точку ОР и фокус MB.

Признак «…если в текущем пространственном положении дальномера определенное с помощью вычислительного устройства расстояние от его кромки до точки пересечения указанной прямой с обозначенной плоскостью меньше реального показания дальномера, то есть расстояния от указанной кромки до нового пятна от луча этого дальномера, то обозначенную точку пересечения, определенную вычислительным устройством, а вместе с ней и луч дальномера с помощью позиционера смещают по прямой, проходящей через фокус видеокамеры и проекцию характерной точки объекта работ на ее светочувствительной матрице, в сторону фокуса до совмещения изображения текущего положения указанного пятна с изображением характерной точки объекта на светочувствительной матрице, в противном случае указанную точку пересечения смещают в противоположном направлении по указанной прямой также до совпадения изображения текущего положения перемещающегося пятна с изображением характерной точки объекта на светочувствительной матрице видеокамеры, контроль этого совмещения осуществляют с помощью видеокамеры…» позволяет автоматически с помощью вычислительного устройства направить луч ЛД на характерную точку ОР.

Признак «…после указанного совмещения, учитывая расположение позиционера и манипулятора на аппарате, в системе координат манипулятора с помощью дальномера и вычислительного устройства определяют все пространственные координаты характерной точки объекта работ, используя окончательные значения углов двух шарниров позиционера этого дальномера и измеренное им расстояние до характерной точки…» позволяет определить три пространственные координаты характерной точки ОР, которую наблюдает MB.

Признак «…затем указанные три координаты этой точки передают в систему управления манипулятора, и она смещает его рабочий орган в указанную характерную точку объекта работ автоматически.» позволяет вывести рабочий орган ММ в характерную точку объекта работ без участия оператора.

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

На фиг. 1 введены следующие обозначения: 1 - подводный аппарат, с закрепленным на нем многостепенным манипулятором 2, имеющим шесть степеней подвижности и может произвольно менять свою конфигурацию в пространстве при одновременном изменении всех его обобщенных координат СК O₁ жестко связана с основанием ММ 2 и корпусом ПА 1; 3 - монокулярная видеокамера, жестко закрепленная в корпусе ПА 1, с которой жестко связана СК О₂, соответствующие оси которой всегда параллельны осям СК O₁; 4 - лазерный дальномер, установленный в пространственном позиционере 5, кинематическая схема которого имеет две вращательные степени свободы, которые определяются углами q_i (i=7,8) и с первым звеном которого жестко связана СК О₃; 6 - движители ПА 1; 7 - рабочий орган (РО) манипулятора; 8 - вершина сектора обзора MB 3; 9 - оптическая ось MB 3, которая совпадает с осью z₂ СК O₂, 10 - объект работ; 11 - характерная точка ОР 10.

На фиг. 2 введены обозначения: 12 - луч ЛД 4, который упирается в точку 13, расположенную на грунте дна или на поверхности ОР; 14 - плоскость светочувствительной матрицы MB 3, которая всегда перпендикулярна оси z₂, 15 - плоскость, в которой расположена характерная точка 11 ОР, перпендикулярная оси Z2 и пересекающая ее в точке 16; 17 - оптическая проекция на плоскость 14 точки 11, расположенной на плоскости 15; 18 -плоскость, перпендикулярная оси z₂ и пересекающая ее в точке 19, на которой расположены точка 13 и точка 20 пересечения этой плоскости с прямой 21, проходящей через точки 8, 11 и 17; l₁, l₂ и l₃ - длины соответствующих звеньев позиционера 5 и луча ЛД 4; I₀ - расстояние между СК O₁ и О₃; С корпусом ЛД 4 жестко связана СК О4, ось z₄ которой совпадает с его оптической осью; Ось шарнира первой степени свободы позиционера 5 (угол вращения q₇) совпадает с осью z₃, а второй степени (угол вращения q₈) - с осью x₄. Поэтому позиционер 5 может направлять луч в любую точку рабочего пространства под ПА.

Для решения поставленной задачи в процессе реализации заявляемого способа вначале с помощью движителей 6 и штатной системы управлении (СУ) осуществляют подвод ПА 1 к ОР 10, а затем и его стабилизацию над этим ОР таким образом, чтобы точка 11 одновременно находилась и в секторе обзора MB 3, и в рабочей зоне ММ (фиг. 1). Для обеспечения минимального гидродинамического сопротивления продольному движению ПА во время его подхода к ОР позиционер 5 располагает ось ЛД 4 так, чтобы она была параллельна продольной оси ПА 1. После стабилизации ПА над ОР в заданной точке пространства с помощью позиционера 5 ЛД 4 поворачивают в исходное положение таким образом, чтобы все соответствующие оси СК О₂, О₃ и О₄ были параллельны (фиг. 2), а углы q₇ и q₈ обнулились.

Для достижения главной цели способа - автоматического подвода схвата 7 ММ 2 к характерной точке 11, а далее и к другим целевым точкам, все координаты этих точек определяют в СК O₁, поскольку управление ММ осуществляют именно в этой СК.

После стабилизации ПА 1 над ОР 10 для определения координаты точки 11 в СК O₁ вначале луч 12 ЛД 4 наводят из исходной точки 13 (фиг. 2) на точку 20 (точку пересечения плоскости 18 и прямой 21), определяя ее координаты также в СК О_1. Координату z₂₀ точки 20 определяют из равенства

z₂₀=z₁₃=l₁+l₂+l₃, т.к. z₁=z₂=z₃. Для определения координат х₂₀ и у₂₀ также в СК O₁ используют известные уравнения

(стр. 99, Письменный Д.Т. Конспект лекций по высшей математике: - М.:Айрис-пресс, 2011. - 608 с.), задающие координаты искомой точки пересечения, где x₈, y₈, z₈ - координаты точки 8; x₁₇, y₁₇, z₁₇ - координаты точки 17; z₁₃ - координата точки 13; х₂₀, y₂₀, z₂₀ - координаты точки пересечения плоскости 18 и прямой 21 в СК O₁. Из приведенных выше уравнений следует, что z₂₀=l₁+l₂+l₃ в СК O₁.

Метод перевода координат всех необходимых точек из СК О₂ в СК O₁ известен и прост, поскольку оси этих СК всегда параллельны. В этом случае будут различаться только их координаты у на величину смещения центров СК О₂ и О₁. Но для перевода координат соответствующих точек из СК О₃ и O₄ в СК O₁, а также для определения обобщенных координат позиционера 5, направленного луч ЛД 4 на точку 20, необходимо использование дополнительных преобразований.

С учетом кинематической схемы позиционера 5 матрицы однородных преобразований, переводящие любой вектор из СК O₄ в СК О₃, и из СК О₃ в СК O₁, имеют вид:

В результате матрица, переводящая вектор из СК О₄ в СК O₁, имеет вид:

Получив искомые координаты x₂₀, y₂₀, z₂₀, определяют обобщенные координаты q₇ и q₈ с помощью выражений

и направляют луч 12 ЛД 4 в точку 20, устанавливая вычисленные обобщенные координаты позиционера 5.

Поскольку в общем случае точки 11 и 20 не совпадают, то после определения координат х₂₀, y₂₀, z₂₀ приступают к определению с помощью MB 3 и ЛД 4 реальных значений координат х_11, у_11, z₁₁ сначала в СК О₃.

На фиг. 3 (а и б) показано, что после направления луча 12 ЛД 4 в точку 20, лежащую на прямой 21, пятно 13 от этого луча может появиться в различных местах на поверхности грунта 22 или ОР. Если плоскость 15 находится ниже плоскости 18 (фиг. 3, а), то ЛД, измеряя расстояние от своей кромки 23 до точки 13, покажет большее расстояние, чем рассчитанное от точки 23 до точки 20. В этом случае, используя выражения (1) и (2), луч 12 ЛД с помощью позиционера 5 смещают по прямой 21 от фокуса 8 MB 3, непрерывно наблюдая с помощью MB 3 уменьшение расстояния между точками 13 и 11 до их полного совпадения. После совпадения этих точек фиксируют длину луча 12 ЛД 4, а также - текущие значения обобщенных координат q₇ и q₈ (с помощью датчиков углов) позиционера 5. Зная текущее значение , в СК О₄ определяют все координаты точки 11, учитывая, что оптическая ость ЛД 4 совпадает с осью z₄ (фиг. 2), координаты x₄=у₄=0, а Затем с использованием полученных значений q₇ и q₈, а также матриц определяют координаты точки 11 в СК O₁, в которой осуществляется управление ММ 2 для перемещения его РО к точке 11.

Если измеренное расстояние между точками 23 и 13 меньше, чем рассчитанное расстояние между точками 23 и 20 (фиг. 3, б), то луч 12 ЛД с помощью позиционера 5 в соответствии с выражениями (1) и (2) смещают по прямой 21 в сторону фокуса 8 MB 3, также непрерывно наблюдая с помощью MB 3 уменьшение расстояния между точками 13 и 11, до их полного совпадения. После совпадения указанных точек также определяют длину луча 12 ЛД 4, а также - текущие значения обобщенных координат q₇ и q₈ (с помощью датчиков углов) позиционера 5. Зная текущее значение СК О₄ определяют все координаты точки 11, учитывая, что оптическая ость ЛД 4 совпадает с осью Z4 (фиг. 2), координаты x₄=у₄=0, а Затем с использованием полученных значений q₇ и q₈, а также матриц также определяют координаты точки 11 в СК О_1.

При необходимости после подхода схвата ММ к текущей точке 11 определяют расположение новой точки 11. Это позволяет непрерывно перемещать ММ рабочий орган 6 по любой непрерывно формируемой пространственной траектории с помощью вычислительного устройства и штатной СУ ММ. При этом описанный выше способ подхода схвата ММ к очередной характерной точке не меняется. Данная процедура повторяется для всех характерных точек ОР.

Практическая реализация предложенного нового способа точного управления подводным ММ с помощью MB и ЛД не требует присутствия оператора.

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

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