Сообщений в теме: 187

[erbol]

Использование алгоритма "точка схода" для ориентирования робота-квадрокоптера в лабиринте

 

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

 

angle1 < fabs(m) < angle2

 

где m = -ctg(teta), где teta это угол между прямой линией и осью х

 

У меня angle1 = 0.3, angle2 =10.0

 

То есть отбрасываются линии которые близки к горизонтальным и вертикальным направлениям на картинке

 

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

 

Если х-координата "точки схода" совпадает с центром картинки, то камера параллельна стенкам лабиринта

 

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

[erbol]

Качество картинки получаемое от камеры робота зависит от скорости движения камеры относительно "сцены". На некоторых кадрах объекты сцены смазаны, неразличимы. .Поэтому прежде чем искать "точку схода" надо проверить качество картинки. 

 

Для того чтобы оценить качество картинки используем нижние допустимые значения пороговых параметров в функциях cvCanny и cvHoughLines2

 

Для cvCanny используется параметр threshold1 - нижнее пороговое значения яркости пиксела попадающего в результат

 

Для cvHoughLines2 параметр threshold - линия возвращается в том случае если аккумулирующий параметр больше порогового значения

[erbol]

Третьим параметром влияющим на поиск "точки схода" является количество возвращаемых параметров линий которые возвращает функция cvHoughLines2. Для хранения параметров линий используется структура данных которая называется в OpenCV последовательностью.

 

В нашем случае каждый элемент последовательности хранит два числа - угол прямой к оси х и длину перпендикуляра от начала координат к прямой

 

CvSeq *lines = cvHoughLines2(canny_out, storage, CV_HOUGH_STANDARD, 1, CV_PI/180, th3);

 

Количество возвращаемых элементов задано с помощью функции MIN( a, b ), она возвращает минимальное значение из пары величин a и b

 

Для обработки параметров найденных линий используем цикл

 

for (int i = 0; i < MIN(lines->total,20); i++ )

 

где lines->total это число элементов в последовательности lines 

[erbol]

Улучшение качества картинки для поиска прямых предполагает сглаживание

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

В opencv для сглаживания используется функция cvSmooth. Эта функция может работать в нескольких режимах. Для того чтобы при сглаживании не расплывались линии на картинке можно использовать режим CV_BILATERAL

Вот две картинки полученные с помощью разных режимов

1.  CV_BILATERAL
2.  CV_GAUSSIAN

Видно, что в режиме CV_BILATERAL линии не расплываются

Исходная картинка



Картинка обработанная в режиме gaussian



Картинка обработанная в режиме bilateral



А это сами функции

double smoothness = 30.0;

int kernel  = 11;

cvSmooth(src, dst, CV_GAUSSIAN, kernel , kernel );
cvSmooth(src, dst1, CV_BILATERAL, kernel , kernel , smoothness, smoothness);

[erbol]

Угловой обзор горизонтальной камеры - 92 градуса. Камера робота как и глаз человека измеряет именно угловые размеры объекта

Допустим мы имеем картинку от робота размером n*m пикселов, где n - ширина картинки, m - высота картинки

Пусть q - ширина объекта на картинке, тогда угловой размер объекта равен q*92/n

 

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

 

Зная координату х точки схода можно определить расстояние от точки схода до центра картинки следующим образом

 

d = x - x0

 

x0 - координата центра картинки

x - координата точки схода

 

Пусть 

 

psi = d * 92 / width

 

где width - ширина картинки в пикселах

 

тогда угол psi будет давать отклонение камеры от оси коридора лабиринта

[erbol]

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

С другой стороны с помощью анализа картинок алгоритмом "точка схода" тоже можно получить некоторую информацию о положении и ориентации робота

Задача состоит в том, чтобы корректировать данные получаемые с датчиков с помощью данных получаемых с камеры робота 

 

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

Разберем простой двухмерный пример

http://edu.dvgups.ru...frame/4_1_3.htm

Хотя на самом деле поворачивается вертолет, можно представить что поворачивается МИР относительно вертолета. Нам нужно рассчитать как меняются координаты точки текущего положения при повороте.

Положительным считается поворот против часовой стрелки



Найдем координаты точки Р` после поворота.

Пусть f  - угол между вектором r и осью X
g - угол между векторами P и P`

P = [ x   y ] = [ r * cos(f)     r * sin(f) ]

P` = [ x`  y` ] = [ r * cos (f + g)   r * sin (f + g) ]

r - длина вектора

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

P` = [ x`  y` ] = [ r * (cos(f) * cos(g) - sin(f) * sin(g))  r * (cos(f) * sin(g) + sin(f) * cos(g)) ]

Используя определение для x и y можно переписать P` так

P` = [ x`  y` ] = [ x * cos(g) - y * sin(g)  x * sin(g) + y * cos(g) ]

x` = x * cos(g) - y * sin(g)
y` = x * sin(g) + y * cos(g)

в матричном виде


где угол тета это угол g в наших обозначениях

[erbol]

Для полета в лабиринте необходимо уметь корректировать курс движения робота относительно оси коридора.

Пусть имеется вычисляемая по картинке с камеры величина d, которая равна отклонению направления движения от оси коридора в градусах

Тогда можно построить алгоритм корректировки курса. Для реализации алгоритма используем флаг mkey.
1. Флаг имеет значение 1, если запущена процедура корректировки
2. Значение 0, если корректировка не нужна, то есть вертолет движется параллельно оси коридора

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

Алгоритм корректировки

1. Сначала проверяем условие abs(d) > 3.0

d - величина отклонения курса от оси коридора в градусах, abs(d) - абсолютное значение величины d

Если условие истинно , то выполняем процедуру корректировки курса

Если условие ложно - заканчиваем процедуру. Для этого присваиваем флагу mkey значение 0

2. Если условие из первого пункта ( abs(d) > 3.0 ) оказалось истинным, тогда переходим к процедуре корректировки. Для этого проверяем условие : mkey == 1. Если mkey равно 0 , тогда присваиваем значения флагу и счетчику : nkey = 0 и mkey = 1

3. Если условие mkey == 1 является истинным тогда выполняем подряд несколько действий. Первое - увеличиваем на единицу текущее значение величины nkey. Второе - проверяем кратно ли текущее значение числу 10. Если кратно тогда в зависимости от того положительным или отрицательным является угол отклонения - даем импульс движения по часовой стрелке или против часовой.

if (abs(d) > 3.0) {

if(mkey == 1){
nkey++;

if (nkey % 10 == 0) {

if (d > 3) r = -1.0;
else
r = 1.0;
}

}
else {
nkey = 0;
mkey = 1;
}
}
else if (mkey == 1) mkey = 0;

[erbol]

Смещения коптера относительно вертикальной оси называются рысканьем. Угол "рысканья" соответствует углу Эйлера "пси" и соответствует углу поворота в горизонтальной плоскости от выбранного направления. Выбранным направлением в нашем случае является ось коридора лабиринта.



http://ru.wikipedia.org/wiki/Рыскание

Задача состоит в вычислении текущего угла поворота "пси". Для этого может быть использован компас коптера. Компас дает ошибку плюс/минус 5 процентов и его нуль плавает. Поэтому показания компаса надо корректировать с помощью картинки получаемой с камеры коптера.

Для того чтобы получить значение угла поворота по вертикали относительно направления на север (угла рысканья) от компаса коптера используем функцию getYaw() класса ardrone пакета cvdrone созданного puku0x

Ссылка на архив пакета
https://github.com/puku0x/cvdrone

Для привязки к местности используем алгоритм Vanishing point detection

Ссылка на описание алгоритма
http://dasl.mem.drex...rs_and_hallways

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

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

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

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