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

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

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

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

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

• Хабр
• Toward Data Science
• Medium
• Hackster
  

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

• Отфильтровать по цветам.
• Выделить грани
• Отсечь ненужные части картинки
• Детектировать линии с помощью преобразования Хафа.

Я наклеил на пол белую изоленту и приступил к делу.



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

Геометрия

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

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

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



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



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



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

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

Свет

Вторая проблема была с освещением. Я очень удачно проложил одну сторону трассы в тени дивана и совершенно невозможно было обрабатывать фото всей трассы одними и теми же настройками. В итоге, пришлось реализовать динамическую отсечку на черно-белом фильтре. Алгоритм такой — если после применения фильтра на картинке слишком много белого (больше 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")

Результаты

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



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

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

## 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

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