Результат интеллектуальной деятельности: Способ детектирования доильного робота

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

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

Известен патент US 6532892 В1, 18.03.2003 «Method and apparatus for recognizing and determining a position and robot including such an apparatus», согласно которому раскрыт способ обнаружения сосков вымени коровы на основе последовательных движений сканирующей головки в различных направлениях. Сканирующая головка перемещается в фиксированное исходное положение, которое находится под животным перед выменем, после чего сканирующая головка перемещается определенными шагами, а именно: вверх к животному, вниз, назад к вымени, вверх, вниз и т.д. Во время вышеуказанных движений выполняется процедура сканирования.

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

Известно изобретение «Automatic milking installation» (публ. US4867103A, 11.09.1987), согласно которому координаты сосков определяются с помощью датчика, который расположен в фиксированном месте на одной стороне стойла и имеет сканирующее устройство, позволяющее формировать изображение сосков, и устройство обработки для определения мгновенных координат сосков. Также предусмотрены четыре независимых манипулятора, каждый из которых несет соответствующий доильный стакан, и каждая управляется независимо от других, чтобы поднести переносимый доильный стакан к соответствующему соску животного, не мешая движению других манипуляторов.

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

Известен патент US 7895972 В2, 01.03.2011 «Arrangement and method for visual detection in a milking system», согласно которому двумерная камера располагается рядом с коровой и многократно записывает двумерные изображения сбоку животного.

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

Известно изобретение WO 2014/014341 А1, 23.01.2014 «Milking arrangement», согласно которому детектирование соска вымени коровы производится за счет поиска цилиндрического объекта диаметром примерно 2-3 см и длиной примерно 2-8 см, с закругленным концом на нижней стороне и соединенным на верхней стороне к гораздо большей сферической структуре, и, кроме того, имеющей четырехкратную трапециевидную симметрию.

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

Известно изобретение WO 2007/104124, 20.09.2007 «Time of flight teat location system», в котором раскрыта система определения местоположения сосков для автоматизированных систем доения, содержащая корпус датчика, установленный на манипуляторе робота для определения положения сосков на вымени. Выходной сигнал датчика используется для управления применением автоматизированного доильного аппарата, который включает в себя доильные стаканы, которые также устанавливаются на манипуляторе робота. Доильный аппарат приспособлен для приема инструкций, характеризующих расположение сосков, для перемещения манипулятора робота в такое место, чтобы доильные чашки входили в соски.

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

Известно изобретение (публ. WO 2009093965 А1, 30.07.2009 «Arrangement and method for determining positions of the teats of a milking animal»), заключающееся в определении положений сосков вымени коровы в ротационной доильной системе, включающей стойло для доения для содержания животного во время доения, при этом с помощью подвижного робота-манипулятора автоматически прикрепляют доильные стаканы к соскам животного и управляют движением манипулятора робота в зависимости от положения сосков животного. При этом на манипулятор устанавливают трехмерную камеру, предназначенную для многократной записи трехмерных изображений вымени животного в реальном времени, и используют средства обработки изображений, предназначенные для многократного обнаружения сосков животного и определения их положения на основе многократно записываемых трехмерных изображений.

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

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

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

Это достигается тем, что в известном способе детектирования доильного робота, заключающемся в определении положений сосков вымени коровы при помощи камеры, согласно изобретению, в качестве камеры используют трехмерную времяпролетную камеру, устанавливают ее на корпус роботизированного манипулятора доильного робота, производят детектирование вымени коровы и запись всех его точек в матрицу N_obj[i,j], формируют матрицу S[4,3] координат вершин сосков вымени коровы в пространстве относительно трехмерной времяпролетной камеры, производят выработку управляющего воздействия u, которое подают в систему управления драйверами роботизированного манипулятора доильного робота.

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

Камера может быть установлена на корпус манипулятора любого доильного робота, при этом место крепления должно обеспечивать возможность произвести снимок вымени коровы в рабочем положении манипулятора. Информация о глубине в виде матрицы данных размером [i,j] (i - количество столбцов, j - количество строк) анализируется алгоритмом и выводит координаты вершин сосков вымени животного относительно системы координат времяпролетной камеры.

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

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

Трехмерная времяпролетная камера 1 и блок 2 задания апертурных углов соединены с блоком 3 формирования матрицы значений координат X_k[i,j], Y_k[i,j] и Z_k[i,j]. Блок 3 формирования матрицы значений координат X_k[i,j] Y_k[i,j] и Z_k[i,j] и блок 4 задания угла наклона трехмерной времяпролетной камеры к горизонту соединены с блоком 5 формирования матрицы значений координат X_h[i,j], Y_h[i,j] и Z_h[i,j]. Блок 5 формирования матрицы значений координат X_h[i,j], Y_h[i,j] и Z_h[i,j] и блок 6 задания шага соединены с блоком 7 расчета длины S_x, блоком 8 расчета длины S_y, блоком 9 расчета длины S_xy, блоком 10 расчета длины S_yx. Блок 7 расчета длины S_x, блок 8 расчета длины S_y, блок 9 расчета длины S_xy, блок 10 расчета длины S_yx и блок 11 задания глубины исследования соединены с блоком 12 формирования матрицы данных N[i,j], который последовательно соединен с блоком 13 формирования матрицы облака точек obj[i,j], блоком 14 формирования матрицы данных N_obj[i,j], блоком 15 фильтрации, блоком 16 детектирования вершин сосков вымени коровы, блоком 17 выработки управляющего воздействия и системой управления драйверами роботизированного манипулятора доильного робота 18.

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

От времяпролетной камеры (блок 1) в блок 3 поступает матрица A[i,j], которая содержит i⋅j значений глубины, коротких расстояний до объектов, располагающихся перед ней. Из блока 2 также в блок 3 поступает значение апертурных углов α и β. В блоке 3 формируются матрицы значений координат X_k[i,j], Y_k[i,j] и Z_k[i,j], согласно следующим выражениям:

В блоке 4 задается угол наклона трехмерной времяпролетной камеры к горизонту γ. В блоке 5 на основе данных от блоков 3 и 4 формируют матрицы значений координат X_h[i,j], Y_h[i,j] и Z_h[i,j], согласно следующим выражениям:

В блоке 6 задают шаг h, который поступает в блоки 7-10, где анализируют элементы матриц X_k[i,j], Y_k[i,j] и Z_k[i,j]. В блоке 7 производится расчет длины S_x между точками по оси 0Х, по следующей формуле:

В блоке 8 производится расчет длины S_y между точками по оси 0Y, по следующей формуле:

В блоке 9 производится расчет длины S_xy между точками в плоскости 0XY, по следующей формуле:

В блоке 10 производится расчет длины S_yx между точками в плоскости 0XY, по следующей формуле:

В блоке 11 оператор задает глубину исследования g, которая, совместно с матрицами S_x[i,j], S_y[i,j], S_xy[i,j] и S_yx[i,j], поступающих от блоков 7-10, в блоке 12 формирует матрицу данных N[i,j], состоящую из «0» и «1», где «0» обозначаются ячейки которые не удовлетворяют, а «1» удовлетворяют следующему неравенству:

S_x[i,j]≤g, тогда N[i,j]=1, N[i,j+h]=1,

S_x[i, j]>g, тогда N[i, j]=0, N[i, j+h]=0,

S_y[i,j]≤g, тогда N[i,j]=1,N[i+h,j]=1,

S_y[i,j]>g, тогда N[i,j]=0, N[i+h,j]=0,

S_xy[i,j]≤g, тогда N[i,j]=1, N[i+h, j+h]=1,

S_xy[i,j]>g, тогда N[i,j]=0, N[i+h, j+h]=0,

S_yx[i,j]≤g, тогда N[i,j]=1, N[i-h, j+h]=1,

S_yx[i,j]>g, тогда N[i,j]=0, N[i-h, j+h]=0.

В блок 13 поступает матрица данных N[i,j] из блока 12. В блоке 13 формируют матрицу obj [i,j] в которой в первом столбце фиксируется номер облака точек, а во втором столбце количество точек, входящих в данное облако точек. Под облаком точек понимается скопление точек, каждая точка из которого должна удовлетворять следующему неравенству хотя бы с одной из точек текущего облака точек:

или

или

или

В блоке 14 формируют матрицу данных N_obj[i,j], состоящую из «0» и «1». Изначально на программном уровне создается матрица N_obj[i,j], состоящая из «0». Далее данная матрица заполняется значениями «1» в тех местах, которые соответствуют точкам самого большого облака точек из матрицы obj[i,j].

В блоке 15 производят следующие операции:

1) фильтрация данных и удаление данных, ошибочно вошедших в состав данного облака;

2) выделения периметра Р облака, система автоматически по крайним точкам фиксирует периметр облака точек, в котором, по определению, заключены все точки текущего облака точек;

3) производится сопоставление второго по величине облака точек из массива данных obj[i,j] с периметром Р облака точек. Если более 50% точек второго по величине облака точек входит в периметр Р, то два вышеупомянутых облака точек сливаются в одно, при этом обновляется информация в матрице N_obj[i,j].

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

В блоке 16 производят детектирование вершин сосков вымени коровы. Алгоритм детектирования основан на определении четырех облаков точек которые записываются в матрицы V₁[n₁,3], V₂[n₂,3], V₃[n₃,3] и V₄[n₄,3], каждое из которых содержит точки, лежащие на определенном соске вымени коровы, после, последовательно, в каждой матрице определяется точка, с минимальной координатой по высоте. На выходе из блока 16 формируют массив данных S[4,3], который состоит из координат вершин сосков вымени коровы в пространстве относительно времяпролетной камеры:

Заполнение матрицы данных S[4,3] производится по мере выявления точек вершин сосков вымени коровы с высоким приоритетом. Низкий приоритет присваивается тем точкам вершин сосков вымени коровы, координаты которых ранее вовсе не детектировались, либо количество их детектирования не значительно относительно высокого приоритета. Высокий приоритет присваивается тем точкам вершин сосков вымени коровы, координаты которых имеют высокий процент совпадения при повторном анализе данных, полученных с пространственной времяпролетной камеры.

В блоке 17 производят выработку управляющего воздействия u, которое подают в систему управления драйверами роботизированного манипулятора доильного робота (блок 18). В случае если матрица S[4,3] содержит не все координаты вершин сосков вымени коровы на выходе управляющее воздействие и будет способствовать выполнить следующие действия:

1) При наличии вакантных координат в матрице S[4,3] подать управляющее воздействие u, с целью позиционирования рабочего органа роботизированного манипулятора доильного робота возле вакантных координат с целью надевания стакана на сосок вымени коровы;

2) При отсутствии вакантных координат в матрице S[4,3] подать управляющее воздействие u, с целью произвести перемещение пространственной времяпролетной камеры в другую точку для повторного снятия матрицы данных A[i,j].

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

В случае если матрица S[4,3] содержит все координаты вершин сосков вымени коровы на выходе управляющее воздействие u будет способствовать выполнить следующие действия:

1) При наличии вакантных координат в матрице S[4,3] подать управляющее воздействие u, с целью позиционирования рабочий орган роботизированного манипулятора доильного робота возле вакантных координат с целью надевания стакана на сосок вымени коровы;

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

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