Алгоритм формирования локальной карты местности

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

Поэтому возникает необходимость составления карты местности в некоторой небольшой окрестности вокруг робота. Размеры этой окрестности должны быть небольшими, во-первых, из-за ограниченного объема памяти для хранения карты местности, а во-вторых, из-за возможности возникновения на этой карте "препятствий-призраков".

В вычислительном плане удобно организовать локальную карту местности в виде массива переменной длины (n). Каждый i-ый элемент этого массива содержит двухмерный вектор декартовых координат (M_i,X, M_i,Y) i-ой дискреты карты. Координаты M_i,X, M_i,Y заданы в связанной с роботом системе координат.

Для формирования карты местности на каждом такте расчета необходимо применять следующий алгоритм:

1. Каждую i-ую дискрету локальной карты местности необходимо повернуть вокруг центра координат на величину азимутального поворота (Da) робота за такт расчета; далее учтем линейное перемещение (DL) робота за такт расчета. Тогда можно записать:

Знак ":=" в этих выражениях следует понимать как оператор присвоения. При расчете данных выражений величины sin(Da) и cos(Da) необходимо вычислить перед началом цикла. x₀, y₀ - промежуточные переменные.

2. Удалить все дискреты, вышедшие за пределы локальной зоны:

Здесь:

W_map - ширина локальной зоны (будем считать, что зона симметрична относительно курсовой оси робота) (Рис. 1, а);

L_перед и L_зад - передняя и задняя граница локальной зоны (Рис. 1, а).

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

Для этого определим границы видимой зоны. Вычислим величину "мертвой зоны" Y_min (Рис. 1. б). Из треугольника AOC выразим величину D:

Тогда:

Здесь: X_С, Y_С, Z_С - положение камеры относительно центра координат робота; FOV_x, FOV_y - углы обзора камеры по горизонтали и вертикали соответственно.

Для каждой i-ой дискреты локальной карты местности совершим следующие действия:

ЕСЛИ M_i,Y>Y_min, ТО:

Рассчитаем для этой точки граничное значение X_m:
Значение K=tg(FOV_x/2) следует вычистить перед началом цикла.
ЕСЛИ | M_i,X - X_C | < X_m, то точка локальной карты попадает в видимую зону и ее следует удалить.

4. Удалить одну из двух дискрет карты, расстояние между которыми мало (удалить совпавшие дискреты).

5. На полную локальную карту местности следует наложить карту расположения препятствий в видимой зоне.

Экспериментальные исследования алгортима формирования локальной карты местности

Для проведения экспериментальных исследований сформируем перед роботом какие-либо препятствия. После чего повернем робота. Можно заметить, что на локальной карте препятствия, которые были в видимой (белой) области до поворота (рис 6, а) повернулись и попали в невидимую (серую) зону (рис 6, б). Это показывает работоспособность описанных алгоритмов.

а)
б)