Визуальный одометр Текст научной статьи по специальности «Математика»

УДК 004.896:535.8

Е. А. Девятериков, Б . Б. Михайлов ВИЗУАЛЬНЫЙ ОДОМЕТР

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

E-mail: retegor@mail.ru; robot@bmstu.ru.

Ключевые слова: стереозрение, телекамера, система координат, изображение рабочей зоны, особые точки на изображении, ошибки измерения, измерение пути.

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

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

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

изображения можно находить трехмерные координаты точек пространства. Идея алгоритма визуальной одометрии [1-3] заключается в том, чтобы выделять на стереоизображениях набор точек, обладающих определенным свойством, например углы зданий, темные пятна на асфальте и т. д., наблюдать изменение их положения, а затем пересчитывать это изменение в смещение и угол поворота робота [4].

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

1) ввод 1-й стереопары, удаление искажений, выравнивание эпиполярных линий;

2) поиск особых точек на левом изображении и этих же точек на правом;

3) вычисление пространственных координат этих точек;

4) ввод 2-й стереопары, удаление искажений, выравнивание эпиполярных линий;

5) поиск особых точек из предыдущего левого изображения на текущем;

6) поиск особых точек текущего правого изображения на предыдущем;

7) вычисление трехмерных координат точек текущей стереопары;

8) сравнение координат точек в текущий и предыдущий моменты времени и вычисление матриц поворота и переноса камеры;

9) переход к п. 4.

Рассмотрим основные операции алгоритма.

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

• удаление радиальных искажений;

• совмещение эпиполярных линий двух изображений [6];

• выделение информационной части изображений.

Выравнивание позволяет находить точки на правом изображении,

точно соответствующие точкам левого изображения.

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

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

В методе Гарриса (Harris) [7] для окрестности каждой точки находится матрица вторых производных от интенсивности яркости изображения (I) - матрица Гессе:

д21 д21

дх 2 дхду

д 21 д21

дхду ду 2

Согласно методу Гарриса, точки располагаются там, где автокорреляционная матрица имеет два больших собственных значения, например: если в двух направлениях от точки резко меняется интенсивность яркости, то найден угол. Далее вычисляется функция-критерий R и ее значение сравнивается с пороговым:

R = det(M) -k(trace(M))2, k = 0,04...0,06.

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

В методе ускоренной проверки сегментов (Features from Accelerated Segment Test - FAST) яркости точек, образующих окружность заданного радиуса, сравниваются с яркостью точки, лежащей в центре этой окружности [8] (рис. 1).

Рис. 1. Иллюстрация работы алгоритма FAST

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

ности считается особой точкой. В результате многочисленных тестов авторами метода было установлено, что самым эффективным является модификация FAST-9, в которой N = 9.

Алгоритм FAST позволяет находить точки, инвариантные к повороту. Это один из самых быстрых методов выделения особых точек на сегодняшний день, который позволяет работать в масштабе реального времени. Время обработки изображения алгоритмом FAST в 5 раз меньше, чем у ближайшего аналога SUSAN, и в 20 раз меньше, чем у традиционно используемого в техническом зрении алгоритма Гарриса.

Ускоренный метод нахождения особых точек (Speeded-Up Robust Features - SURF) [9] относится к лидерам по надежности. С его помощью находятся особые точки изображения, инвариантные к масштабированию, повороту, искажениям и освещению. Метод включает нахождение дескриптора особенностей - обобщенной характеристики окрестности точки, позволяющей сопоставлять особенности двух изображений.

В методе SURF используется масштабируемое гауссово пространство изображений f : R2 ^ R, семейство {L(x, у, t): R2 ■ R+ ^ r} постепенно сглаживающихся версий исходного изображения

L( х У,t) = g(х У,t )* f (х y\

где * - операция свертки; g (х, у, t) - двумерная функция Гаусса со

стандартным отклонением t = ст2, называемым масштабом. В результате выделяются особые точки, инвариантные к масштабированию изображения.

Детектор особых точек основан на матрице Гессе:

H (х,а) =

Lxx ( х,а) Lxy (х,а) Lxy ( х,а) Lyy (х,а)

где (х,*) .М^ * I (х,у); Ь,, (,,*) = * I (х,у),

дхдх дхду

Критерием выделения особенностей является определитель матрицы Гессе

\2

det(H) = L^ -(со^) ,

где Ьу (Ьхх, Ьху, Ьуу) - свертки для текущей точки в текущем масштабе; (О - коэффициент, компенсирующий сжатие при аппроксимации (для сжатия в 1,2 раза ( = 0,9).

Дескриптор состоит из 64 значений. Окрестность особой точки разбивается на 16 квадрантов. Для каждого квадранта находят четыре значения: суммы производных интенсивности в двух направлениях и суммы модулей производных интенсивности в двух направлениях.

Затем простым перебором, методом ЯА^АС или построением к-ё-дерева сравниваются значения дескрипторов особых точек двух изображений.

Слежение за особыми точками методом Люкаса и Кенеда.

Для вычисления пути робота находят смещение особых точек между текущим и предыдущим изображением. Алгоритм [10] имеет следующие ограничения: постоянство яркости и когерентное смещение всех точек. При этом само смещение должно быть небольшим. Условия выполняются при достаточно высокой частоте обработки изображений.

Пусть х - особенность первой функции ¥, найдем такую точку х+И функции О, в которой расстояние между окрестностями этих точек минимально.

Расстояние между окрестностями записывается в виде

E = Z[ (х + h) - G( x)]2

xeR

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

д

^ (х + И) = ^ (х) + И—Г ( х),

дх

д

где--градиент (для изображений по площади). Далее ищем ми-

дх

нимум Е:

0 = АE dh dh

F (x) + h ^ - G (x) dx

= E2—

. dx

F (x) + h dF - G( x) dx

Отсюда смещение И можно получить как

h =

Z

dF_ dx

[G (x) - G( x) ]

-1

2

Определение расстояния до особых точек. Рассмотрим самый простой случай, когда камеры расположены параллельно друг другу. Фокусные расстояния / известны и вычисляются при калибровке камер [8]. Расстояние 2 до точки Р обратно пропорционально разнице между координатами проекций этой точки на левом и правом изображениях й = х1 - хг:

Z =

fT

l r X - X

(1)

где T - расстояние между камерами (база); f - фокусное расстояние; x1 и xr - координаты проекций точки на левом и правом изображениях.

Величина d называется рассогласованием (disparity). Так как расстояние до точек обратно пропорционально d, то очевидно, что зависимость нелинейна. Когда d близко к 0, значительно увеличивается ошибка вычислений, поэтому до геометрических расчетов допускаются точки с относительно большим d (>5 % ширины изображения, т. е. близкие точки).

Формула (1), по которой вычисляется расстояние до точки про-

1 r

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

\Л[

\

\ X2

о т L.

N

С>9

N

Рис. 2. Геометрическая схема для оценки ошибки вычисления расстояния

Здесь Ox и O2 - оптические центры левой (первой) и правой (второй) телекамер. Положение точки пространства Ai -1 определяется как пересечение лучей O1Ai-1 и O2 Ai-1, направление которых (углы o"i и <т"2 ) известно из положения проекций точки на изображениях. Пусть за некоторый интервал времени ti...ti-1 робот повернулся на угол у. Пусть также в i-й момент времени, когда был сохранен кадр с первой телекамеры, точка A в системе координат робота находилась в положении Ai. Вторая телекамера зафиксировала точку A в (i - 1)-й

момент времени в другом положении Ai-1. Тогда ее положение ошибочно будет определено как A'.

Результаты геометрического расчета показывают, что при повороте робота с угловой скоростью о = п /2 рад / с и рассогласовании времени захвата изображений 0,033 с (отставание на один кадр при

частоте видеозахвата 30 с-1 ) расстояние до точки пространства вычисляется с ошибкой порядка 35 %. Ошибка 5 % возникает при повороте робота с угловой скоростью о = п /20 рад / с.

Интерлейсинг и смазывание изображений также негативно влияют на работу визуального одометра. Когда робот перемещается вперед, вдоль оси камер, изображения не искажаются. Но при повороте робота появляется эффект «гребенки» (интерлейсинг): четные строки изображения отстают от нечетных, что является следствием использования в видеосистемах чересстрочной развертки. При повороте робота со скоростью 15 °/с выделение особых точек становится невозможным. Данную проблему можно решить двумя способами:

• вычислить величину смещения строк и совместить их. Но эта задача по ресурсоемкости соизмерима со всей задачей визуальной одометрии;

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

После применения чересстрочного фильтра метод FAST нормально функционирует при повороте робота со скоростью до 30°/с. При повороте робота с большей скоростью даже после фильтрации негативного эффекта интерлейсинга изображение остается «смазанным» из-за недостаточного быстродействия системы технического зрения, а алгоритмы поиска особых точек не функционируют.

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

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

ни относительно его положения в момент запуска алгоритма. Все предшествующие вычисления направлены на получение трехмерных координат точек пространства, в котором находится робот, относительно робота. Задача блока вычисления перемещения - определить по этим координатам величину перемещения робота. На рис. 3 показано перемещение системы координат робота, где система координат

пР ъ-ОлгОгуО НС^гС^СгуС пР^^Р^Р^Р

О X 1 А неподвижна; О X 1 А и О X 1 А - положение системы координат робота в текущий и в некоторый предыдущий моменты; М1 - неподвижные особые точки пространства.

Рис. 3. Перемещение системы координат робота

Пусть координаты 1-й особой точки пространства в системе ко-

(С С С\Т

Х1 У1 ) ,

координаты той же точки пространства в некоторый предыдущий момент времени р = (хр Ур Ар) . Пусть также найдено N особых точек. Тогда данные образуют выборку пар векторов Ж = ((рСД... ,(PN,CN)) объемом N. Изменение положения системы координат робота будем описывать в виде аффинного преобразования

/: Я3 ^ Я3, /(х) = Ях + Т,

Г /11 r12 r13^

r21 r22 r23

V r31 r32 r33J

где Т = (АХ АУ АА)Т - вектор переноса; Я =

матрица поворота.

Задача заключается в получении оценки вектора переноса Т и матрицы поворота Я преобразования системы координат робота на основе выборки Ж

Поскольку изменение положения системы координат робота описывается аффинным преобразованием, координаты 1-й точки пространства в текущий и предыдущий момент времени связаны выражением

С, = Щ + T,

или

' Xе ^ ( Г r11 r12 Г \ r13 ( Xp л (AX 1

= r21 r22 r23 Yf + AY

V zc , v Г31 r32 r33 J V Zf j VAZ J

Выполнив перемножение и сложение матриц, для 1-й особой точки получим

( хС >

zc

V А i

(

Г11 Xf + rnYP + r13 Zf

■AX

Л

r2iXf + r22TiP + r23 Zf +AY V ГяXf + ^Yf + Гзз Zf + AZ j

Для нахождения неизвестных решим три системы линейных алгебраических уравнений:

XC = rnXf + rnY? + ri3 Zf + AX, i = 1,N;

Yc = r2iXf + r2Y + r23 Zf + AY, i = 1, N;

ZC = r,1 Xf + ^Yf + гзз Zf + AZ, i = 1,N.

(2)

(3)

(4)

Системы линейных алгебраических уравнений (2-4) имеют общий вид:

ХС = г Xf + r2Yf + г Zf + At, i = 1,N.

(5)

Система (5) содержит четыре неизвестных, поэтому для ее решения необходимо иметь данные по крайней мере о четырех точках. Запишем (5) в матричной форме:

Аа = у, (6)

где A =

( Xf Yf Zf X2 Y2 Z 2

XP YP ZP

VXN yN zN

CO =

(гъ ^ ^ At); y = (xc, , •••, xN)T.

Здесь матрица A не является квадратной, поэтому утверждение с = A_1 y неверно. Речь идет о задаче регрессионного анализа. Необходимо на основе всей выборки W объемом N получить неизвестные параметры r1, r2, r3, At регрессионной модели (гипотезы), удовлетворяющей некоторому критерию качества.

В системе уравнений (5) заданы линейная регрессионная модель (функция восстановления значений зависимой переменной)

f (, Г2, Гз, At, Xf, Yf, Zf, 1) = rXX + rY + г Zp +At-1

с параметрами с, а также вектор фактических значений y и выборка значений аргументов A.

Один из способов получения неизвестных параметров гь г^, г и At, приближающих значения правых частей всех уравнений системы (5) к значениям левых, - метод наименьших квадратов, который позволяет найти такие оптимальные параметры с линейной регрессии, чтобы сумма ошибок (регрессионных остатков) была минимальна. Метод заключается в минимизации евклидова расстояния |Ac - y|

между двумя векторами: вектором восстановленных значений зависимой переменной Ac и вектором фактических значений зависимой переменной y .

Запишем критерий качества модели (функция невязки)

N

S = £(f (с, A) - y)2 =|Ac- y|2 ^ min.

i=1

Запишем функцию невязки в виде

S = \Ac- y|2 = (Ac - y)T (Ac - y) = yTy - yTAc --CATy -cT AT Ac = yTy - 2yT Arn+aT AT Arn.

Чтобы найти минимум функции невязки, найдем ее производную и приравняем к нулю:

dS

— = -2 ATy + 2 ATAc = 0;

dc

2 ATAc = 2 ATy; (7)

(ATA)c = (ATy).

Таким образом, вместо системы уравнений (6) с неквадратной матрицей А будем решать систему уравнений (7) с квадратной матрицей (АТ А), для которой

©=( АТА)-1 (АТу).

ЗБ-модель для отладки программы. На основе алгоритма визуальной одометрии с использованием приведенных в статье процедур выравнивания и фильтрации изображений, поиска особых точек и вычисления перемещения робота была написана программа. Для ее предварительного тестирования и отладки была создана трехмерная модель в среде БоНё'^гкв. Модель представляет собой сцену (рис. 4), состоящую из двух виртуальных камер, стола и простых объектов (кубиков, «обтянутых» текстурами).

Рис. 4. Модель системы

Сначала программно камеры смещались на равные интервалы по координате X и рассчитывалось среднее квадратичное отклонение вычисленных перемещений, которое при обработке 100 ключевых точек составило 1,8 % всего пути. Затем камеры перемещались по замкнутой траектории, представляющей собой ромб. Погрешность выхода в начальную точку составила 5 % относительно суммарного пройденного пути. Таким образом, была установлена работоспособность алгоритма.

Экспериментальное исследование системы. Для экспериментальной проверки программы, реализующей алгоритм визуальной одометрии, был изготовлен макет робота (рис. 5).

Состав системы:

• видеокамеры Sony 640x480 пикс. (2 шт.);

• компьютер Intel i5 2.4GHz, 2Gb DDR3 (используется 1 поток);

• устройства видеозахвата Pinnacle DVC100 (2 шт.);

• мобильная платформа.

Камеры были закреплены параллельно полу и параллельно друг другу, расстояние между осями объективов (база) - 120 мм.

Вычислялись только две координаты - пройденный путь и угол поворота (курс) робота.

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

Рис. 5. Система визуальной одо-метрии на мобильном роботе

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

c =

1

N

Е ( z - z ')2

i=0

N

-100 %

L

где - найденное перемещение за один сдвиг макета; Z' - действительное перемещение за один сдвиг макета; N - число кадров (обработанных стереопар), N = 52; Ь - действительная длина пути.

48 N, пгг.

Рис. 6. Результаты измерения при смещении робота на равные интервалы — пройденный путь)

Затем робот несколько раз перемещался по замкнутой траектории: 1,5 м вперед, 1,5 м назад, после чего оценивалась средняя ошибка выхода в исходную точку [11] (рис. 7).

612 N, шт.

Рис. 7. Результаты измерения при перемещении робота по замкнутой траектории

В табл. 1 приведены скорость обработки и ошибки при использовании трех методов поиска особых точек.

Таблица 1

Сводная таблица результатов эксперимента

Метод выделения особых точек Скорость обработки, стереопар/с Ошибка выхода в исходную точку, % Среднее квадратичное отклонение при смещении на равные интервалы, %

FAST 37,2 7,2 4,8

Гарриса 17,9 7,6 5,2

SURF 10,0 3,8 4,1

Затем обрабатывались записи, полученные при повороте робота со скоростью около 25 °/с, анализировалась средняя точность выхода угла в начальное значение для каждой итерации. На рис. 8 представлены графики изменения вычисленного угла с применением различных фильтров, а в табл. 2 - численные значения результатов эксперимента.

Таблица 2

Результаты при нормальной скорости поворота робота

Фильтр изображения Скорость обработки (БР8), кадров/с Ошибка выхода в исходную точку, %

Без фильтров 37,2 20,1

Чересстрочный 33,0 10,8

С бинаризацией 22,0 12,6

Рис. 8. Результаты измерения угла поворота робота (а - угол поворота робота; N - количество обработанных пар изображений)

Результаты эксперимента показывают, что метод визуальной одометрии позволяет достаточно точно измерить пройденный путь и угол поворота робота. При ошибке 3,8 % самую высокую точность и низкую производительность (10 кадров/с) обеспечивает метод SURF. В режиме реального времени может работать метод FAST, дающий возможность обрабатывать 37 пар кадров за секунду, но дающий ошибку 7,2 %. Система позволяет также измерить угол поворота робота с ошибкой 10 %.

Эксперимент показал, что удаление четных/нечетных строк из изображения незначительно снижает точность измерений при медленном перемещении робота, когда нет негативного эффекта интер-лейсинга. Но способ чересстрочной фильтрации принципиально улучшает точность работы системы при повороте робота с рабочей скоростью, поскольку без представленного фильтра ошибка измерения достигает 20 %. Усиление контуров изображения с помощью адаптивной бинаризации снижает точность системы, но дает возможность вычислять угол поворота робота при вращении со скоростью более 35 °/c.

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

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

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

СПИСОК ЛИТЕРАТУРЫ

1. Jin Xu, Minyi Shen, Weiqiang Wang, Li Yang, Jilin Liu, K a l y a e v I. Robust stereo visual odometry for autonomous rover. Proceedings of 6th WSEAS International Conference on Signal, Speech and Image Processing, 2006.

2. Nist'er D., Naroditsky O., Bergen J. Visual odometry for ground vehicle applications // Journal of Field Robotics, 2006. Iss. 23(1). Р. 3-20.

3.Beets E.,Boukir S., Suter D. Aircraft Pose Estimation from Homography. Technical report MECSE-1-2004, Monash University, 2004.

4. DeMenthon D.F., D av is L.S. Model-based object pose in 25 lines of code. Proceedings of the European Conference on Computer Vision, 1992.

5. Bradski G., Kaehler A. Learning OpenCV. Newgen Publishing and Data Services - 2008.

6. Конушин А. Геометрические свойства нескольких изображений // Компьютерная графика и мультимедиа. - 2006. - Вып. № 4(3). - http://cgm. computergraphics.ru/content/view/141.

7. Гаганов В. Инвариантные алгоритмы сопоставления точечных особенностей на изображениях // Компьютерная графика и мультимедиа. - 2009. -Вып. № 7(1). - http://cgm.computergraphics.ru/issues/issue17/invariant_features.

8. Rosten E., Drummond T. Points and Lines for High Performance Tracking. International Conference on Computer Vision. 2005. ISS 2 (Oct.). Р. 1508-1511.

9. Bay H., Ess A., Tuytelaars T., Gool V. Speeded-Up Robust Features (SURF). Computer vision and image understanding (CVIU). 2008. Vol. 110(3). Р. 346-359.

10. Конушин А. Слежение за точечными особенностями сцены (Point feature tracking), 2003. http://cgm.computergraphics.ru/content/view/54.

11. Jang G., Kim S., Lee W., Kweon I. Color Landmark Based Self-Localization for Indoor Mobile Robots. Korea Advanced Institute of Science and Technology, 2001.

Статья поступила в редакцию 28.06.2012