Проект «робот-грузчик»: определение ориентации

    Месяц назад я писал об определении моим роботом-грузчиком собственного положения. (Жаль, ту статью я запостил в неудачное время в ночь на субботу, так что её мало кто увидел.) Как я отметил, показания колёсных датчиков позволяют роботу определять своё положение достаточно точно — медленно накапливающаяся ошибка корректируется, как только робот сканирует баркод на любой из полок склада. С другой стороны, накапливающуюся ошибку направления корректировать было нечем.

    Я обсудил свои затруднения с девушкой-гуманитарием, и спросил, какие ей известны способы ориентации в пространстве. По её словам, в Лондонском музее науки она застала экспозицию, посвящённую ориентации муравьёв по виду вертикально вверх над головой. Посетителям предлагалось взять зеркало и идти по комнате, разглядывая в это зеркало узоры на потолке и ориентируясь лишь по ним. (Карта потолка прилагалась.)

    Я решил проверить: что видит на потолке склада мой робот?


    Даже на такой низкокачественной записи отлично различимы длинные прямоугольные окна. Все они, естественно, параллельны одной из осей склада, а значит, параллельны и рядам полок. Получается, если «в кадр» попадёт окно и я смогу определить его направление — то я получаю и направление робота, с точностью до 180°. Окна видно не из любой точки склада, но для периодической коррекции курса — они попадают в кадр достаточно часто.

    В распознавании образов я не мастак, и первым делом я спросил на Q&A — нет ли для распознавания прямоугольных окон чего-нибудь уже готового, например, применим ли OpenCV? К сожалению, ничего толкового мне не подсказали — люди, которые «в теме», сочли ниже своего достоинства разжёвывать чайнику азы.

    Американский форум: задал вопрос — получишь ответ.
    Израильский форум: задал вопрос — получишь вопрос.
    Русский форум: задал вопрос — тебе объяснят, какой ты мудак.

    Тем более, что робот управлялся из-под Microsoft Robotics Development Studio, а готовой .NET-сборки для работы с OpenCV я не нашёл — я решил писать собственное распознавание с нуля. Не ракетная наука, чай — всего-то распознавать ярко-белые прямоугольники.

    Потолок склада роботом видится примерно так:


    Для начала отделим ярко-белое окно от тёмного фона. Переводим из RGB в YCbCr и разделяем по Y=227 (порог выбран опытным путём, и в идеальном мире подбирался бы адаптивно по яркости изображения в целом). Попутно уменьшаем исходное изображение 640х480 до 320х240 — нам этого достаточно, а обработка ускорится вчетверо.

    Код:
    byte[] bytes = response.Frame;
    int stride = bytes.Length / height;
    
    byte[,] belong = (byte[,])Array.CreateInstance(typeof(byte), new int[] { 326, 246 }, new int[] { -3, -3 });
    
    int Ythreshold = settings.Ythreshold;
    for (int y = 0; y < 240; y++)
    {
        int offset = stride * y * 2;
        for (int x = 0; x < 320; x++)
        {
            int blu = bytes[offset] + bytes[offset + 3] + bytes[offset + stride] + bytes[offset + stride + 3]; offset++;
            int grn = bytes[offset] + bytes[offset + 3] + bytes[offset + stride] + bytes[offset + stride + 3]; offset++;
            int red = bytes[offset] + bytes[offset + 3] + bytes[offset + stride] + bytes[offset + stride + 3]; offset += 4;
    
            belong[x, y] = (0.299 / 4 * red + 0.587 / 4 * grn + 0.114 / 4 * blu > Ythreshold ? (byte)1 : (byte)0);
        }
    }
    


    В результате разделения получается такое изображение:


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

    Удаляем шум медианным фильтром (хотя в QA мне отчего-то советовали гауссовское размытие. Ну вот зачем здесь размытие?) с радиусом 3 пиксела, и порогом в 5 светлых пикселей из 21 рассмотренных. Такой фильтр «склоняется» к соединению светлых областей, между которыми есть тёмные пиксели — так, чтобы наше окно из трёх стёкол объединилось в один прямоугольник.

    private bool[,] filtered = (bool[,])Array.CreateInstance(typeof(bool), new int[] { 326, 246 }, new int[] { -3, -3 });
    
    int medianThreshold = settings.MedianThreshold;
    for (int x = 0; x < 320; x++)
        for (int y = 0; y < 240; y++)
                  filtered[x, y] = belong[x - 1, y - 2] + belong[x, y - 2] + belong[x + 1, y - 2] +
            belong[x - 2, y - 1] + belong[x - 1, y - 1] + belong[x, y - 1] + belong[x + 1, y - 1] + belong[x + 2, y - 1] +
            belong[x - 2, y * 1] + belong[x - 1, y * 1] + belong[x, y * 1] + belong[x + 1, y * 1] + belong[x + 2, y * 1] +
            belong[x - 2, y + 1] + belong[x - 1, y + 1] + belong[x, y + 1] + belong[x + 1, y + 1] + belong[x + 2, y + 1] +
                                   belong[x - 1, y + 2] + belong[x, y + 2] + belong[x + 1, y + 2] > medianThreshold;
    

    Размерность для массивов belong и filtered — [-3..322, -3..242] — выбрана нарочно с «полями» по три пиксела с каждой стороны, чтобы работать с этими массивами, не заморачиваясь проверками индексов.

    После фильтрации на изображении остаются белыми только окно, объединённое из трёх стёкол, и несколько бликов на полке:


    Постановим, что самое большое белое пятно в кадре — это непременно окно. Зальём (floodfill) каждое белое пятно, и возьмём наибольшее по площади.

    int biggest_area = 0;
    byte area_id = 1, biggest_id = 0; // areas start from 2
    Rectangle bounds = new Rectangle();
    PointF cg = new PointF(); // center
    Point[] stack = new Point[320*200]; 
    
    for (int x = 0; x < 320; x++)
        for (int y = 0; y < 240; y++)
            if (filtered[x, y] && belong[x, y] <= 1)
            {
                int area = 0, left = 320, top = 240, right = 0, bottom = 0;
                int sx = 0, sy = 0;
                ++area_id;
                // FloodFill 
                int sp = 0;
                stack[0] = new Point(x, y);
                while (sp >= 0)
                {
                    Point next = stack[sp--];
                    area++;
                    sx += next.X;
                    sy += next.Y;
                    belong[next.X, next.Y] = area_id;
    
                    if (next.X < left) left = next.X;
                    if (next.X > right) right = next.X;
                    if (next.Y < top) top = next.Y;
                    if (next.Y > bottom) bottom = next.Y;
    
                    if (filtered[next.X - 1, next.Y] && belong[next.X - 1, next.Y] <= 1) stack[++sp] = new Point(next.X - 1, next.Y);
                    if (filtered[next.X, next.Y - 1] && belong[next.X, next.Y - 1] <= 1) stack[++sp] = new Point(next.X, next.Y - 1);
                    if (filtered[next.X, next.Y + 1] && belong[next.X, next.Y + 1] <= 1) stack[++sp] = new Point(next.X, next.Y + 1);
                    if (filtered[next.X + 1, next.Y] && belong[next.X + 1, next.Y] <= 1) stack[++sp] = new Point(next.X + 1, next.Y);
                }
                if (area > biggest_area)
                {
                    biggest_area = area;
                    biggest_id = area_id;
                    bounds = new Rectangle(left, top, right - left, bottom - top);
                    cg = new PointF((float)sx / area, (float)sy / area);
                }
            }
    

    Изображение для распознавания готово! Двухуровневое — белый прямоугольник на чёрном фоне. Мы даже вычислили во время заливки координаты его «центра масс» cg (на изображениии — красная точка) и границ (зелёная точка — центр bounding box). Можем теперь проверить, насколько наше белое пятно похоже на окно: площадь biggest_area должна быть не меньше 2000 пикселей, расстояние между двумя центрами — не больше 20 пикселей, иначе фигура слишком несимметрична для прямоугольника. Но как дальше будем определять его ориентацию?


    Ракетные учёные, может быть, применили бы преобразование Хафа, перевели бы изображение в 4-мерное пространство вероятностных параметров прямоугольника (ширина, длина, угол наклона, смещение от начала координат), и искали бы там максимум. Но я подошёл к задаче по рабоче-крестьянски, и для начала составил «гистограмму» удаления точек прямоугольника от его геометрического центра:

    PointF c = new PointF(bounds.Left + (float)bounds.Width / 2, bounds.Top + (float)bounds.Height / 2);
    
    int[] hist = new int[400];
    
    for (int i = 0; i < 400; i++) hist[i] = 0;
    
    int maxdist = 0;
    for (int x = bounds.Left; x <= bounds.Right; x++)
        for (int y = bounds.Top; y <= bounds.Bottom; y++)
            if (belong[x, y] == biggest_id)
            {
                int dist = (int)Math.Sqrt(Sqr(x - c.X) + Sqr(y - c.Y));
                hist[dist]++;
                if (dist > maxdist) maxdist = dist;
            }
    




    Серый график — это и есть гистограмма, на ней тёмная полоса — максимум, т.е. наибольшее число точек находится именно на таком расстоянии от центра прямоугольника. (На прямоугольнике проведена тёмная окружность, соответствующая этому расстоянию.) Легко понять, что это расстояние как раз и есть полуширина прямоугольника: до неё — окружности целиком помещаются внутри прямоугольника, и гистограмма линейно (с коэффициентом π) растёт; потом гистограмма медленно убывает, пока не дойдёт до полудлины прямоугольника; с этого момента внутри прямоугольника помещаются только четыре «уголка» окружностей, и гистограмма резко спадает. К сожалению, найти длину через этот «обрыв» на гистограмме не удаётся — края прямоугольника оказываются слишком рваными, и зашумляют конец гистограммы. Мы пойдём другим путём, и рассмотрим круг на 5 пикселей шире прямоугольника. В нём будут две чёрные «боковушки» как раз на поперечной оси прямоугольника:



    Аккуратно найдём центры масс этих «боковушек»: сложность здесь в том, чтобы их разделить. Считаем отдельно центр масс в каждом «квадранте», потом объединяем «квадранты» в две пары по близости центров.
    int r1 = 0; // incircle radius
    for (int x = maxdist; x >= 3; x--)
    {
        if (hist[x] > hist[r1]) r1 = x;
    }
    
    int rSample = r1 + 5;
    int[] voters = new int[4];
    Point[] sums = new Point[4];
    Point sampleOrg = new Point(Math.Max((int)(c.X - rSample), 0),
                                Math.Max((int)(c.Y - rSample), 0));
    Rectangle sample = new Rectangle(sampleOrg, new Size(
                                     Math.Min((int)(c.X + rSample), 319) - sampleOrg.X,
                                     Math.Min((int)(c.Y + rSample), 239) - sampleOrg.Y));
    for (int x = sample.Left; x <= sample.Right; x++)
        for (int y = sample.Top; y <= sample.Bottom; y++)
            if (belong[x, y] != biggest_id)
            {
                int dist = (int)Math.Sqrt(Sqr(x - c.X) + Sqr(y - c.Y));
                if (dist > r1 && dist <= rSample)
                {
                    int idx = y < c.Y ? (x < c.X ? 1 : 0)
                                      : (x < c.X ? 2 : 3);
                    voters[idx]++;
                    sums[idx].X += x;
                    sums[idx].Y += y;
                }
            }
    
    PointF adjusted = new PointF();
    int vAbove = voters[0] + voters[1],
        vBelow = voters[2] + voters[3],
         vLeft = voters[2] + voters[1],
        vRight = voters[0] + voters[3],
        allVoters = vAbove + vBelow;
    if (allVoters == 0)
    {
        // abort: no black pixels found
    }
    else
    {
        if (vAbove > 0 && vBelow > 0)
        {
            // split vertically
            PointF above = new PointF((float)(sums[0].X + sums[1].X) / vAbove - c.X, (float)(sums[0].Y + sums[1].Y) / vAbove - c.Y),
                   below = new PointF((float)(sums[2].X + sums[3].X) / vBelow - c.X, (float)(sums[2].Y + sums[3].Y) / vBelow - c.Y);
            double dAbove = Math.Sqrt(above.X * above.X + above.Y * above.Y),
                   dBelow = Math.Sqrt(below.X * below.X + below.Y * below.Y);
            if (dAbove >= r1 && dAbove <= rSample && dBelow >= r1 && dBelow <= rSample)
                // the split is valid
                adjusted = new PointF((above.X * vAbove - below.X * vBelow) / allVoters, (above.Y * vAbove - below.Y * vBelow) / allVoters);
        }
        if (adjusted.X == 0 && adjusted.Y == 0 &&
            vLeft > 0 && vRight > 0)
        {
            // split horizontally
            PointF toleft = new PointF((float)(sums[2].X + sums[1].X) / vLeft - c.X, (float)(sums[2].Y + sums[1].Y) / vLeft - c.Y),
                  toright = new PointF((float)(sums[0].X + sums[3].X) / vRight - c.X, (float)(sums[0].Y + sums[3].Y) / vRight - c.Y);
            double dLeft = Math.Sqrt(toleft.X * toleft.X + toleft.Y * toleft.Y),
                  dRight = Math.Sqrt(toright.X * toright.X + toright.Y * toright.Y);
            if (dLeft >= r1 && dLeft <= rSample && dRight >= r1 && dRight <= rSample)
                // the split is valid
                adjusted = new PointF((toleft.X * vLeft - toright.X * vRight) / allVoters, (toleft.Y * vLeft - toright.Y * vRight) / allVoters);
        }
    }
    

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


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

    Продемонстрирую работу на ещё одном примере — когда окно попадает в кадр не полностью. Благодаря тому, что мы не пользуемся «концом» гистограммы для распознавания — нас неполное окно нисколько не сбивает с толку.

    Второй пример
    Исходное изображение:


    После разделения:


    После фильтрации:


    Гистограмма:


    Круг с боковушками:


    Поперечная ось:


    Написанный мной код был облачён в MRDS-сервис WindowDetector — по образу и подобию стандартного Technologies\Vision\ColorSegment. Мой сервис привязывается к видеокамере, и по каждому обновлению кадра высылает подписчикам UpdateFoundWindow с углом наклона окна в кадре. MRDS-сервис, управляющий роботом, привязывается к WindowDetector точно так же, как привязывается к сканеру баркодов, и совершенно аналогично обрабатывает сообщения от обоих — корректируя в первом случае курс, в другом случае положение.

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


    Дальнейшая судьба робота печальна. Всего через час после съёмки этого видео колёсный датчик забился складской пылью, и робот перестал следить за своим положением. При изначальной сборке робота этот датчик ставится самым первым, т.е. для его замены нужно по сути всё разобрать и потом собрать по-новой. Я решил попробовать заменить датчик «на живом» роботе, но по неуклюжести закоротил батарею обо что-то внутри робота, и ездить он перестал совсем. Пришлось его оставить на складской полке рядом с позапрошлогодним роботом — тем, что на основе Roomba. Прямо робокладбище, а не полка.

    Так я перед отъездом из Британии и не успел сделать Eddie действующим роботом-грузчиком. Но уже этой весной я, скорее всего, вернусь туда с запчастями, оживлю его, и продолжу испытания.
    Ads
    AdBlock has stolen the banner, but banners are not teeth — they will be back

    More

    Comments 26

      0
      А если робот не грузчик, а фрезеровщик — нужно ли ему менять ориентацию? )
        +4
        Станет грузчиком после смены )
        +3
        Медианная фильтрация в таких случаях обычно нужна как вспомогательная, чтобы проредить сильный высокочастотный шум, а в данном случае, как мне кажется, проще и удобнее было бы сразу использовать операции математической морфологии(opening/closing) и получить нужный прямоугольник. Но это технические мелочи.

        А с Robot Operating System не приходилось иметь дел? Сам просто как раз с подобной задачей столкнулся, поэтому интересно.
          0
          Сам я с ROS не работал, но слышал весьма хвалебные отзывы, по сравнению с MRDS, которой я остался в целом недоволен.

          А как вы через opening/closing получите из белого пятна — прямоугольник? Там же чётких углов нет.
            0
            ROS — занятная штука , хоть немного и монстрообразная, на первый взгляд.
            +1
            А не проще ли для корректировки направления использовать гироскоп, а на потолке лишь изредка поцеплять какие-то маркеры для устранения накопившейся погрешности?
              0
              Прошлая статья как раз заканчивалась фразой «Лобовым решением было бы установить гироскоп; но готового гироскопа для Eddie не существует, а таинств с паяльником и написанием прошивки мне хотелось бы избежать.»
              +2
              А в тёмное время суток как?
                0
                А в тёмное время суток склад всё равно закрыт и не работает :-)
                  0
                  Даже зимой, когда рассветает поздно и темнеет рано? :)
                    0
                    Да. Тестировалось в конце ноября, окна на потолке различимы до 16 часов. По удивительному совпадению, как раз в 16 часов склад и закрывается.
              • UFO just landed and posted this here
                  +1
                  Компасу, скорее всего, не понравятся массивные стальные полки и проезжающие рядом огромные погрузчики.

                  Полосы на полу — во-первых, уязвимы в складских условиях (затопчут, обдерут), во-вторых, их трудоёмко переносить, если придётся перемещать стеллажи (что происходит несколько раз в году).
                    0
                    Ну, на самом деле, вы несколько предвзяты к компасам. Сейчас доступны достаточно дешёвые модели (недавно работал с Freescale Xtrinsic, например, очень доволен; у них к тому же ещё и акселлерометр встроен) — можно купить несколько, поставить по окружности и фильтровать любые сильные помехи на раз-два (с которыми столкнуться достаточно сложно, на самом деле; стальные балки точно не будут источником проблем, только моторы погруздчиков, I guess).
                      0
                      Звучит заманчиво. Можете подробнее про фильтрацию помех?
                        0
                        Ну, я это выдвигаю как теорию, сам я их использовал для несколько другой задачи (ставил по углам фанерного листа, водил магнитом по нему и определял позицию in real-time), но так как вам не нужно знать идеальную ориентацию робота в любой момент, а только изредка калиброваться — задача сводится просто к отбрасыванию выделяющихся результатов считывания датчиков (если будет какая-то помеха — они будут показывать сильно различающиеся значения, так как магнитный поток ослабевает по экспоненте в зависимости от расстояния до источника; могу выражаться неправильно, не бейте сильно :)).
                  –4
                  rocket science → ракетостроение
                    +4
                    «Rocket science» — не ракетостроение. Это устойчивое фразеологическое выражение, означающее «нечто, весьма сложное для понимания».
                      +1
                      По-русски говорят «Сдал сопромат — можно жениться».
                    0
                    Как видно из первого видео, на потолке видны еще и балки. Они присутствуют на изображении практически всегда. Может попробовать дополнительно ориентироваться и по ним?
                      0
                      Они намного менее контрастные, и вдобавок их от полок отличать сложнее.
                      +1
                      Имхо, можно попробовать без медианного фильтра и прочих наворотов немного модернизировать классический алгоритм заливки, чтобы он заглядывал не только в соседние точки, а чуть дальше. И попутно, помимо площади, вычислял координаты самой нижней, верхней, левой и правой точек. По полученному четырехугольнику не составит труда определить ориентацию.
                        0
                        И попутно, помимо площади, вычислял координаты самой нижней, верхней, левой и правой точек.
                        Обратите внимание на изображение ещё раз. Самых левых точек — четыре, самых правых — пять. От того, которую из них принять за угол, получающаяся ориентация зависит достаточно сильно. Тем более, что в моём случае ни одна из «крайних» точек в действительности не в угле — все углы «скруглились» фильтром.
                        +1
                        Hough Transform есть в openCV
                        docs.opencv.org/doc/tutorials/imgproc/imgtrans/hough_lines/hough_lines.html
                        соответственно можно выделить все прямые линии, которые видны роботу, а по их гистограмме (кроме окон попадут и предметы интерьера) — корректировать направление.

                        Но оно затратно достаточно.

                          +1
                          Если есть видеокамера ориентированная на потолок и неравномерный рисунок на потолке (полосы какие-то), я бы попробовал обойтись без колесного датчика и вычислять перемещение примерно как делает оптическая мышь, т.е. через автокорреляцию изображений через малые промежутки времени.
                            0
                            Вопрос, возможно, дилетантский, но нельзя ли сделать ориентацию по визуальным маркерам, которые легко бы распознавались? Наклеить на стене или потолке узор типа того, что используется в системах дополненной реальности и, если робот его засёк, то однозначно определил свою позицию по тому, что за узор и своем относительному положению.

                            Only users with full accounts can post comments. Log in, please.