Следование линии на основе OpenCV

Сейчас очень популярны курсы по созданию автопилотов для машин. Вот эта нано-степень от Udacity — самый наверное известный вариант.

Много людей по нему учатся и выкладывают свои решения. Я тоже не смог пройти мимо и увлекся.

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


Все у них довольно просто и схема работы сводится к нескольким пунктам:
Я наклеил на пол белую изоленту и приступил к делу.

8vddnwlx9odevming4sba03ddjc.jpeg

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

Геометрия


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

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

После применения Canny получилось вот что:

zi6ntwcuu0t4zwtw9y3nevjg5je.jpeg

А линии Хафа были такими:

image

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

tue1czduaydztjue0g4aq0oe0so.jpeg

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

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

qcxrbwggmkm6wkt-wzxq8xbtbkw.jpeg

Свет


Вторая проблема была с освещением. Я очень удачно проложил одну сторону трассы в тени дивана и совершенно невозможно было обрабатывать фото всей трассы одними и теми же настройками. В итоге, пришлось реализовать динамическую отсечку на черно-белом фильтре. Алгоритм такой — если после применения фильтра на картинке слишком много белого (больше 10%) — то порог следует поднять. Если слишком мало (меньше 3%) — опустить. Практика показала, что в среднем за 3–4 итерации удается найти оптимальную отсечку.

Магические числа вынесены в отдельный конфиг (см ниже), можно с ними играться в поисках оптимума.

def balance_pic(image):
    global T
    ret = None
    direction = 0
    for i in range(0, tconf.th_iterations):
        rc, gray = cv.threshold(image, T, 255, 0)
        crop = Roi.crop_roi(gray)
        nwh = cv.countNonZero(crop)
        perc = int(100 * nwh / Roi.get_area())
        logging.debug(("balance attempt", i, T, perc))
        if perc > tconf.white_max:
            if T > tconf.threshold_max:
                break
            if direction == -1:
                ret = crop
                break
            T += 10
            direction = 1
        elif perc < tconf.white_min:
            if T < tconf.threshold_min:
                break
            if  direction == 1:
                ret = crop
                break
            T -= 10
            direction = -1
        else:
            ret = crop
            break  
    return ret      


Наладив машинное зрение, можно было переходить к собственно движению. Алгоритм был такой:

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


Сокращенный вариант кода (Полный — на Гитхабе):

def check_shift_turn(angle, shift):
    turn_state = 0
    if angle < tconf.turn_angle or angle > 180 - tconf.turn_angle:
        turn_state = np.sign(90 - angle)
    shift_state = 0
    if abs(shift) > tconf.shift_max:
        shift_state = np.sign(shift)
    return turn_state, shift_state

def get_turn(turn_state, shift_state):
    turn_dir = 0
    turn_val = 0
    if shift_state != 0:
        turn_dir = shift_state
        turn_val = tconf.shift_step if shift_state != turn_state else tconf.turn_step
    elif turn_state != 0:
        turn_dir = turn_state
        turn_val = tconf.turn_step
    return turn_dir, turn_val                


def follow(iterations):
    tanq.set_motors("ff")   
    try:
        last_turn = 0
        last_angle = 0 
        for i in range(0, iterations):
            a, shift = get_vector()
            if a is None:
                if last_turn != 0:
                    a, shift = find_line(last_turn)
                    if a is None:
                        break
                elif last_angle != 0:
                    logging.debug(("Looking for line by angle", last_angle))
                    turn(np.sign(90 - last_angle), tconf.turn_step)
                    continue
                else:
                    break
            turn_state, shift_state = check_shift_turn(a, shift)
            turn_dir, turn_val = get_turn(turn_state, shift_state)
            if turn_dir != 0:
                turn(turn_dir, turn_val)
                last_turn = turn_dir
            else:
                time.sleep(tconf.straight_run)
                last_turn = 0
            last_angle = a
    finally:
        tanq.set_motors("ss")


Результаты


Неровно, но уверенно танк ползет по траектории:

cm1cugubbk4nny-wavpxpqwa5eq.gif

А вот собрал гифку из отладочной графики:

x8iyrvqh5r5zrulvgnyn5ipbysu.gif

Настройки алгоритма

## Picture settings
# initial grayscale threshold
threshold = 120
# max grayscale threshold
threshold_max = 180
#min grayscale threshold
threshold_min = 40
# iterations to find balanced threshold
th_iterations = 10
# min % of white in roi
white_min=3
# max % of white in roi
white_max=12
## Driving settings
# line angle to make a turn
turn_angle = 45
# line shift to make an adjustment
shift_max = 20
# turning time of shift adjustment
shift_step = 0.125
# turning time of turn
turn_step = 0.25
# time of straight run
straight_run = 0.5
# attempts to find the line if lost
find_turn_attempts = 5
# turn step to find the line if lost
find_turn_step = 0.2
# max # of iterations of the whole tracking
max_steps = 100


Код на Гитхабе.

© Habrahabr.ru