RU2781926C1 - Способ управления многозвенным манипулятором необитаемого подводного аппарата для выполнения манипуляционных операций с подводными объектами - Google Patents

Abstract

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

Description

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

Известен способ управления подводным манипулятором в режиме зависания подводного аппарата, при реализации которого на борту подводного аппарата устанавливают гироскопические датчики и навигационную систему, определяющие линейное и угловое смещения этого аппарата от его исходного положения при работающем манипуляторе, а на следящие приводы всех степеней подвижности манипулятора подают дополнительные сигналы управления, пропорциональные смещению этого подводного аппарата от его исходного положения, которые обеспечивают дополнительное перемещение рабочего органа манипулятора в пространстве и тем самым продолжение его точного движения по заранее заданной желаемой программной траектории независимо от произвольных смещений подводного аппарата [Филаретов В.Ф., Коноплин А.Ю. Способ управления подводным манипулятором в режиме зависания подводного аппарата // патент РФ №2475799, Бюл. №5 от 20.02.2013].

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

Известен также способ формирования желаемых программных траекторий движения рабочего органа многозвенного манипулятора, установленного на подводном аппарате, по поверхностям известного подводного объекта, для определения местоположения и пространственной ориентации которого выполняется совмещение модели поверхности объекта в виде облака точек, получаемого от бортовых систем технического зрения (СТЗ) аппарата с моделью поверхности объекта в виде облака точек, построенным на основе заранее построенной трехмерной модели этого объекта [Коноплин А.Ю., Коноплин Н.Ю., Шувалов Б.В. Подход к выполнению АНПА технологических манипуляционных операций с различными подводными объектами // Подводные исследования и робототехника. - 2019. - №. 1. - С. 31-37.].

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

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

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

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

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

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

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

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

Заявленное изобретение иллюстрируется чертежами, где:

на фиг. 1 схематически показано предварительное совмещение модели поверхности объекта в виде облака точек, полученной с помощью бортовых СТЗ необитаемого подводного аппарата, с моделью поверхности объекта в виде облака точек, построенной на основе заданной исходной трехмерной модели этого объекта;

на фиг. 2 показаны отфильтрованное (зеленое) и неотфильтрованное (красное) облака точек рядом с облаком точек поверхности объекта;

на фиг. 3 показан общий вид проверочной траектории, векторов проецирования и триангуляционных поверхностей;

на фиг. 4 показаны спроецированные на поверхности объекта (красная) и его модели (зеленая) траектории.

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

Определение формы и расположения объекта относительно необитаемого подводного аппарата осуществляется в режиме стабилизируемого зависания, жесткой фиксации вблизи объекта или при посадке аппарата на грунт. Подводное окружение сканируется средствами СТЗ - многолучевыми сонарами и стереокамерами, формирующими модель поверхности объекта в виде облака точек, принадлежащих поверхностям дна и объекта. Упомянутые облака точек задаются в прямоугольной системе координат (СК) Oxyz, жестко связанной с корпусом необитаемого подводного аппарата и началом в центре величины (водоизмещения) аппарата, ось Ox совпадает с горизонтальной - продольной осью аппарата, ось Oz - с его вертикальной осью и направлена вниз, а ось Oy составляет с ними правую тройку.

Чтобы определить форму и расположение объекта относительно необитаемого подводного аппарата в работе [Коноплин А.Ю., Коноплин Н.Ю., Шувалов Б.В. Подход к выполнению АНПА технологических манипуляционных операций с различными подводными объектами // Подводные исследования и робототехника. - 2019. - №. 1. - С. 31-37] предложено использовать трехмерную модель этого объекта, предварительно обработанную и преобразованную в облако точек, заданное в СК Oxyz. Это облако, состоящее из m точек

⋅(

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

, величина и направление которого определяются известными системами обнаружения подводных объектов, выделяющихся из окружения [10. Guerneve T., Subr K., Petillot Y. Three-dimensional reconstruction of underwater objects using wide-aperture imaging SONAR // Journal of Field Robotics. - 2018. - Vol. 35, - No. 6. - P. 890-905.]. Затем смещенное облако, заданное точками

, совмещается с моделью поверхности объекта в виде облака точек, полученной с помощью бортовых СТЗ аппарата, с использованием алгоритма Iterative Closest Point, реализованного в открытой библиотеке Point Cloud Library [Interactive Iterative Closest Point: Documentation of Point Cloud Library. - URL: http://pointclouds.org/documentation/tutorials/interactive_icp.php (дата обращения: 14.07.2021)].

В случае совмещения облаков точек стандартными средствами библиотеки Point Cloud Library (см. фиг. 1) определяются вектор

и матрица

, соответственно описывающие линейное смещение и поворот модели поверхности объекта в виде облака точек, построенной на основе заданной исходной трехмерной модели этого объекта, относительно осей СК

, необходимые для совмещения облаков. Следовательно, для любой точки

, принадлежащей облаку точек модели объекта, можно получить точку

, определяющую положение точки

после совмещения двух облаков:

где

- средняя точка (центр масс) облака точек модели объекта,

- вектор, определяющий положение любой точки

относительно точки

этого облака.

Однако, поскольку на каждой итерации алгоритма Iterative Closest Point оценка точности совмещения облаков выполняется методом наименьших квадратов для ближайших точек этих облаков, наличие в перемещаемом облаке модели объекта «лишних» точек (которые отсутствуют на скане реального объекта из-за однонаправленного характера сканирования СТЗ) часто приводит к ошибкам совмещения центров масс двух облаков. При этом исследования показали, что в таких случаях обеспечивается правильная пространственная ориентация облака модели объекта (фиг. 1).

Для повышения точности совмещения облаков точек необходимо отбросить часть точек

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

вычисляется угол

лежащий между вектором нормали

, соединяющим центр величины

облака с точкой

, и вектором

, где S

- координаты точки начала СК СТЗ в СК Oxyz. Отбросим точки

, для которых выполняется неравенство

. Результат такого исключения «лишних» точек показан на фиг. 2.

После отбрасывания лишних точек снова выполняется совмещение облаков алгоритмом Iterative Closest Point и определяются вектор

и матрица

, описывающие последующее линейное смещение и поворот облака точек модели относительно осей СК

. В результате аналогично (1) координаты точек

совмещенного облака модели объекта будут определяться согласно выражению:

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

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

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

, где

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

,

.

Для проецирования на обе триангуляционные поверхности построенной в СК

проверочной траектории сначала задается направление проецирующих лучей единичным вектором

, направленным от точки S начала СК СТЗ к центру масс

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

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

⋅(

) и найдем пересечения лучей, выходящих из этих точек в направлении вектора

, с обеими триангуляционными поверхностями (см. фиг. 3).

В результате пересечения образуются наборы точек

и

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

Координаты точек

и

можно вычислить с помощью модификации алгоритма Моллера-Трумбора в виде:

где (

) - скалярное произведение векторов; (

) - векторное произведение векторов.

Проекции траектории состоят только из координат точек

и

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

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

,

(см. фиг. 4).

Пороговое значение критерия Q зависит от размеров объекта и разрешающей способности СТЗ.

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

⋅(

s - количество точек) с соответствующими единичными векторами

и

, задающими желаемую ориентацию рабочего инструмента в j-й точке. С учетом выражения (2) заданную траекторию, перенесенную на реальный объект, можно представить в виде:

где X_j - точки, задающие желаемые положения рабочего инструмента на поверхности объекта, с соответствующими векторами ориентации

и

,

- центр масс облака исходных точек

модели объекта.

В случаях, когда точная форма объекта не может быть известна заранее, или велика вероятность его деформации, обрастания или заиливания, желаемая траектория рабочего инструмента (4) может быть дополнительно спроецирована на построенную с помощью СТЗ триангуляционную поверхность этого объекта. Для этого аналогично выражению (3) выполняется проецирование точек X_j в положительном и отрицательном направлениях вектора

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

, которые будут лежать на поверхности реального объекта:

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

Claims (1)

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

Images