Описание решения. 
Разработка мехатронной системы ориентирования с использованием SLAM технологии

Для устранения искажений возникающих в результате работы алгоритма необходимо учитывать не только особые точки — разнообразные углы объектов, но и сами прямые объекты — стены и другие длинные прямые предметы. Для этого необходимо сначала выделить эти объекты во входных данных. Для поиска прямых линий обычно используется преобразование Хафа [7], входными данными для него являются двумерные изображения. Двумерное изображение можно построить, основываясь на информации от сканирующего лазерного дальномера Алгоритм должен отслеживать окружающие робота прямые в течение времени работы робота и на основании параметров p и и прямых определять положение робота. Параллельно с этим должна обновляться карта видимого окружающего пространства.

Для реализации SLAM-метода, основанного на анализе прямых, окружающих робота необходимо следовать алгоритму, представленному на рисунке (Рисунок 2). Этот алгоритм раскрывает этап вычисления перемещения из алгоритма на рисунке 1. и выполняется на каждой итерации рабочего цикла робота. Последним этапом алгоритма является обновление карты окружающего пространства.

Рисунок 2. Алгоритм вычисления перемещения робота.

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

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

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

Выходными данными преобразования Хафа является двумерный массив H (p, и), называемый накопительным пространством. Координаты локального максимума в этом пространстве определяют найденные прямые линии в виде:

(1).

где p и и — определенны, с помощью преобразования Хафа параметры прямой.

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

(2).

Изменяя иi от -90 до 90 градусов с шагом Ди и округляя значения p_{x, y}(иi) до ближайшего p’i=n*Дp, где n — целое, а Дp — шаг расчета по расстоянию, получаем массив (pi', иi), содержащий набор параметров прямых, проходящих через точку A_{x, y}.

Далее значение каждой точки H (p_i, и_i) инкрементируется. Таким образом осуществляется голосование точками входного изображения A за проходящие через них прямые.

Найдя локальные максимумы в H (p, и), определим все найденные прямые. Каждому локальному максимуму под номером j с координатами (p_j, и_j) соответствует прямая на изображении, где p=p_j, и=и_j

Перемещение между i и j измерениями вычисляются по формулам:

(4).

(5).

где p_{i, 1}, и_{i, 1} и p_{i, 2,} и_{i, 2} — параметры 1 и 2-ой прямых в измерениях под номером i, а p_{j, 1}, и_{j, 1} и p_{j, 2}, и_{j, 2} — параметры 1 и 2-ой прямых в измерениях под номером j. Дx, Дy, Дц — изменение положения робота (координат и направления) между двумя моментами i и j. Эти уравнения выведены из соотношений изменения параметров двух пересекающихся прямых при перемещении начала координат.

2. Проектно-конструкторская часть