Построение карты проходимости на GPU по данным RGB-D камеры & лидара

Sencis3 часа назад

Построение карты проходимости на GPU по данным RGB-D камеры & лидара

Время на прочтение13 минОхват и читатели3.2KАлгоритмы*РобототехникаАннотация

Рассматривается практический опыт построения карты проходимости для автономного наземного робота, работающего в условиях пересечённой местности и смешанной indoor/outdoor-среды. Описывается эволюция подходов: от универсальной трёхмерной воксельной карты к двухмерной карте высот, восстанавливаемой из воксельного представления, и далее — к карте проходимости, вычисляемой полностью на GPU. Отдельное внимание уделено проблеме выбора сенсора: сравниваются лидар и RGB-D камера, обосновывается необходимость их совместного использования и описывается алгоритм синхронизации облаков точек.

1. Введение

Изначальной целью было создание универсальной трёхмерной воксельной карты, пригодной как для беспилотных летательных аппаратов, так и для наземных роботов. Однако планирование движения непосредственно по воксельной карте требует алгоритмов, способных обрабатывать большие объёмы данных за короткое время. Доступные opensource-решения, работающие на CPU, даже на процессорах уровня Intel Core i7 не обеспечивают необходимой производительности. Кроме того, анализ проходимости участка (уклоны, ямы, габаритные ограничения) ложится на планировщик, который вынужден выполнять эту работу внутри себя, что дополнительно увеличивает время планирования.

В случае двухмерной карты значительную часть этой информации можно рассчитать заранее массово параллельно ещё на этапе построения карты. Так, для каждой ячейки 2D карты известно, может ли робот в ней находиться (помещается ли по габаритам, не проваливается ли в яму). Благодаря инфляционному слою в большинстве ситуаций удаётся отказаться от проверки коллизий всего периметра корпуса, ограничиваясь одной точкой, которая детектирует приближение к препятствию по инфляционному слою. Расчёт подобной карты проходимости в трёхмерном виде под силу далеко не каждому GPU: для карты 400×400×100 объём вычислений возрастает примерно в 100 раз по сравнению с 400×400.

Двухмерные карты проходимости удобнее и с точки зрения обмена данными CPU–GPU, котрый зачастую является узким горлышком и добавляет не мало наклодных расходов CUDA ядрам, 2D карта представляют собой компактное описание окружающего пространства, тогда как в трёхмерной карте камеры и лидары видят лишь поверхности, а около 90 % объёма занимает пустота, не несущая полезной информации.

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

2. Способы переноса трёхмерной карты в двухмерное представление

Известны два основных подхода.

2.1. Горизонтальный срез на уровне робота

Первый способ — срез участка карты на высоте робота, как это делается у роботов-пылесосов. У пылесоса срез представляет собой одну линию горизонтальной развёртки; иногда срез делают на всю высоту робота, что позволяет учитывать препятствия типа натянутого через дорогу троса на произвольной высоте, которые одномерный срез может пропустить.

Недостаток метода в том, что он не представляет поверхность, по которой движется робот. На ней могут быть ямы и неровности, которых робот в таком варианте просто не видит и едет фактически над пустотой. Это порождает неопределённость при планировании: неясно, куда ставить узлы графа, если под роботом пустота, и как планировать путь через пустоту. Стандартные алгоритмы двухмерной карты ROS для заполнения пустоты используют лучи (raycast): лучи запускаются из текущей позиции робота во все видимые лидаром точки на поверхностях предметов (облако точек), и по мере прохождения луча от робота к точке на поверхности препятствия пройденный путь заполняется проходимыми ячейками, формируя свободные зоны. Это решает задачу заполнения пространства между стенами, но неровности и ямы на дороге также отмечаются проходимыми, что является лукавством. Метод плохо работает на наклонных поверхностях при подъёме по крутому склону, так как впереди робот может вообще ничего не видеть. Область применения ограничена indoor-средой.

2.2. Вертикальная проекция

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

Недостаток метода в том, что некоторые поверхности, например потолоки в помещении, навесы могут перекрывать поверхность движения и делать многие зоны недоступными. Проблема усугубляется с лидарами типа Livox MID-360, большая часть лучей которого направлена вверх, а не параллельно горизонту, как у моделей Velodyne, Hesai и других. Из-за этого потолки особенно часто попадают в скан робота даже на ровной поверхности.

Одним из решений является метод поиска проходимого пространства на основе окон (Map-Conversion-3D-Voxel-Map-to-2D-Occupancy-Map). Алгоритм анализирует в вертикальном столбце свободное пространство — окна, в которые по высоте может поместиться робот, — а затем пытается собрать из этих рамок коридор: если следующее свободное окно по высоте примерно равно текущему, а порог между ними робот может преодолеть, нижние точки окна (подоконник) добавляются на карту как проходимые точки земной поверхности; если смещение слишком большое, они отсеиваются. Предполагается, что алгоритм должен работать как в уличных условиях, так и под землёй в пещере, расширяя возможности обычной вертикальной проекции.

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

Вполне вероятно, что более тонкой настройкой пороговой функции их можно было бы сделать непроходимыми, как и стены, но у меня сделать этого не получилось. Хотя карта на снимках была построена с помощью RGB-D камеры, установленной горизонтально, часть точек потолка всё же попала на воксельную карту, и на итоговой карте они не видны, что говорит о том, что область отмечена как проходимая. В целом, если настроить пороговую функцию по полученной карте высот, результат можно назвать приемлемым. Однако первичное построение участка карты на i7 занимало около 50 секунд и по 1-6 секунд на дальнейшее обновление новыми участками (карта внутри разбита на чанки для ускорения обработки новых регионов). Это довольно медленно с учётом частоты обновления данных лидара 10 Гц.

3. Собственная реализация на CUDA

На основе указанного репозитория была сделана собственная версия карты проходимости, работающая на CUDA:

Первые результаты оказались неудовлетворительными:

Алгоритм окон плохо поддавался переносу на CUDA, а собственное ядро, являвшееся первой попыткой решить проблему в лоб, плохо работало из-за шума RGB-D камеры и скачков лидарной одометрии по оси Y, то есть по высоте:

// Основное ядро обработки
__global__ void processLayerKernel(uint8_t* data, int16_t layer_y, int16_t robot_height, int16_t current_y) {
// Вычисляем координаты x и z для текущего потока
uint16_t x = blockIdx.x * blockDim.x + threadIdx.x;
uint16_t z = blockIdx.y * blockDim.y + threadIdx.y;

// Проверяем границы if (x >= 400 || z >= 400) return;

// 1. Обработка верхней области (y от layer_y + 1 до 100)
uint32_t base_idx = get_value(x, layer_y, z);

// Пропускаем свободные ячейки if (data[base_idx] == 0) return;

// Проверяем наложение блоков overlay filter
if(data[get_value(x, layer_y + 1, z)]){ data[base_idx] = 0; return; }

bool is_contour = (data[base_idx] > 1 && data[base_idx] < 100);
bool has_occupied_in_range = false;
uint8_t occupied_count = 0;
int16_t first_occupied_height = -1;
robot_height += 1;

// Проверяем ячейки выше
for (int16_t y = layer_y + 1; y <= layer_y + robot_height; y++) {
if (y >= 100) break;

uint32_t idx = get_value(x, y, z);
if (data[idx] != 0) {
has_occupied_in_range = true;
occupied_count++;
if (first_occupied_height == -1) {
first_occupied_height = y;
}
}
}

// Обработка в зависимости от типа ячейки
if (is_contour) {
// Случай 4: Ячейка контура
if (has_occupied_in_range) {
// 4.1: Есть занятые ячейки выше
data[base_idx] = 254;

// Удаляем занятые ячейки в диапазоне
for (int16_t y = layer_y + 1; y <= layer_y + robot_height; y++) {
if (y >= 100) break;
uint32_t idx = get_value(x, y, z);
data[idx] = 0;
}
} else {
// 4.2: Нет занятых ячеек выше
data[base_idx] = 1 + calculate_height_value(layer_y, current_y);
}
} else {
// Обычная занятая ячейка
if (!has_occupied_in_range) {
// Случай 1: Нет занятых ячеек на высоте робота
data[base_idx] = 1;
} else if (occupied_count == 1 && first_occupied_height == layer_y + 1) {
// Случай 2: Ровно одна занятая ячейка сразу над текущей
uint32_t above_idx = get_value(x, layer_y + 1, z);
data[above_idx] = 0;
data[base_idx] = 1 + calculate_height_value(layer_y, current_y);
} else {
// Случай 3: Несколько занятых ячеек выше
data[base_idx] = 254;

// Удаляем все занятые ячейки в диапазоне
for (int16_t y = layer_y + 1; y <= layer_y + robot_height; y++) {
if (y >= 100) break;
uint32_t idx = get_value(x, y, z);
data[idx] = 0;
}
}
}

// Удаляем ячейки выше robot_height
for (int16_t y = layer_y + robot_height + 1; y < 100; y++) {
uint32_t idx = get_value(x, y, z);
data[idx] = 0;
}

// 2. Обработка нижней области (y от 0 до current_y - 1)
for (int16_t y = 0; y < layer_y - 1; y++) {
uint32_t idx = get_value(x, y, z);
data[idx] = 0;
}
}

3.1. Идея приоритетного обхода воксельной карты

Алгоритм этой карты решал проблему переноса трёхмерной карты в двухмерную проекцию по другому принципу. Рассмотрим помещение наподобие парковки или дома хоббита:

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

Для решения этой проблемы был придуман способ преобразования карты на основе приоритетов. Воксельная карта, которая строится на GPU, имеет размер 400×400×100 блоков и состоит из 100 вертикальных слоёв; робот всегда находится в центре карты, то есть в позиции (199, 199, 49). Карта движется вокруг него как скользящее окно по мере движения робота, что обеспечивает приход новых свободных областей от просканированного пространства и удаление старых, вышедших за пределы карты; таким образом карта может строиться бесконечно.

От центра карты (199, 199, 49), высота которого совпадает с высотой робота (все лучи лидара и камеры также идут из центра), опускается луч, определяющий высоту земной поверхности под роботом. Если там пустота, берётся значение по умолчанию, равное высоте робота. Допустим, робот высотой 50 см, и высота до земли в слоях получается 44. Этот слой самый приоритетный, далее алгоритм обходит все 100 слоёв в порядке 44, 45, 43, 46, 42, 47, 41 и так далее. На каждом слое он анализирует столбец по высоте Y и относительно высоты текущего слоя (например, 44) до высоты робота в слоях (5 слоёв, то есть до слоя 49) выбирает в столбце 44–49 самую высокую точку. Пусть это будет точка 47; тогда от неё в обе стороны по оси Y удаляются все остальные блоки. Таким образом трёхмерная воксельная карта схлопывается в одну поверхность — 2.5D карту высот, по которой и едет робот:

// Основное ядро обработки
__global__ void processLayerKernel(uint8_t* data, int16_t layer_y, int16_t robot_height, int16_t current_y)
{
int x = blockIdx.x * blockDim.x + threadIdx.x;
int z = blockIdx.y * blockDim.y + threadIdx.y;
if (x >= Map_X || z >= Map_Z) return;

// Проверяем слой layer_y
if (data[get_value(x, layer_y, z)] == 0) return;

// Ищем самый верхний occupied в [layer_y, layer_y + robot_height]
int y_top = layer_y;
int y_max = min(layer_y + robot_height, Map_Y - 1);
for (int yy = layer_y + 1; yy <= y_max; yy++) {
if (data[get_value(x, yy, z)] != 0) {
y_top = yy;
}
}

// Удаляем всё выше и ниже y_top
for (int yy = 0; yy < Map_Y; yy++) {
if (yy != y_top) {
data[get_value(x, yy, z)] = 0;
}
}
}

3.2. Преобразование карты высот в карту проходимости

Далее метод наименьших квадратов с решением системы нормальных уравнений по правилу Крамера, заимствованный из статьи Map-Conversion-3D-Voxel-Map-to-2D-Occupancy-Map, позволяет преобразовать карту высот в карту проходимости. Вычисляются локальный и глобальный уклоны, а также размах высот в окне; итоговое значение берётся как максимум из них. Затем уклон отображается в стоимость ячейки: unknown, lethal, free или промежуточные значения:

__device__ void getSlopeOfPoints( const float* heights, const float* xs, const float* zs, int n, float& slopeX, float& slopeZ)
{
if (n < 3) { slopeX = 0.0f; slopeZ = 0.0f; return; }

float sumX = 0, sumZ = 0, sumH = 0;
float sumX2 = 0, sumZ2 = 0, sumXZ = 0;
float sumXH = 0, sumZH = 0;

for (int i = 0; i < n; ++i) {
float x = xs[i], z = zs[i], h = heights[i];
sumX += x; sumZ += z; sumH += h;
sumX2 += x * x; sumZ2 += z * z; sumXZ += x * z;
sumXH += x * h; sumZH += z * h;
}

float det = sumX2 * (sumZ2 * n - sumZ * sumZ)
- sumXZ * (sumXZ * n - sumZ * sumX)
+ sumX * (sumXZ * sumZ - sumZ2 * sumX);

if (fabsf(det) < 1e-10f) { slopeX = 0.0f; slopeZ = 0.0f; return; }

float detA = sumXH * (sumZ2 * n - sumZ * sumZ)
- sumXZ * (sumZH * n - sumZ * sumH)
+ sumX * (sumZH * sumZ - sumZ2 * sumH);

float detB = sumX2 * (sumZH * n - sumZ * sumH)
- sumXH * (sumXZ * n - sumZ * sumX)
+ sumX * (sumXZ * sumH - sumZH * sumX);

slopeX = detA / det; slopeZ = detB / det; }

__global__ void heightmap_to_slope_cuda( const float* __restrict__ dev_heightmap, float* __restrict__ dev_slope, int estimation_size)
{
int x = blockIdx.x * blockDim.x + threadIdx.x;
int z = blockIdx.y * blockDim.y + threadIdx.y;
if (x >= Map_X || z >= Map_Z) return;

const float cellSize = 1.0f; // 1 воксель = 0.1 м, но мы работаем в вокселях

// --- Локальный slope (окно 3×3) ---
float hl[9], xl[9], zl[9];
int nl = 0;
float raw_min = 1e30f, raw_max = -1e30f;

for (int dr = -1; dr <= 1; ++dr) {
for (int dc = -1; dc <= 1; ++dc) {
int nr = z + dr;
int nc = x + dc;
if (nr < 0 || nr >= Map_Z || nc < 0 || nc >= Map_X) continue;

float h = dev_heightmap[nr * Map_X + nc];
if (h < 0.0f) continue; // unknown

hl[nl] = h;
xl[nl] = dc * cellSize;
zl[nl] = dr * cellSize;
nl++;

if (h < raw_min) raw_min = h;
if (h > raw_max) raw_max = h;
}
}

if (nl < 3) { dev_slope[z * Map_X + x] = -1.0f; return; }

float sx, sz;
getSlopeOfPoints(hl, xl, zl, nl, sx, sz);
float localSlope = sqrtf(sx * sx + sz * sz);

float heightRange = (raw_max - raw_min) / cellSize;

// --- Глобальный slope (окно estimation_size) ---
float globalSlope = 0.0f;
if (estimation_size > 1) {
// Динамический массив плохо, ограничим размер
const int MAX_N = 121; // (2*5+1)^2 = 121 для estimation_size=5
float hg[MAX_N], xg[MAX_N], zg[MAX_N];
int ng = 0;

for (int dr = -estimation_size; dr <= estimation_size; ++dr) {
for (int dc = -estimation_size; dc <= estimation_size; ++dc) {
if (ng >= MAX_N) break;
int nr = z + dr;
int nc = x + dc;
if (nr < 0 || nr >= Map_Z || nc < 0 || nc >= Map_X) continue;

float h = dev_heightmap[nr * Map_X + nc];
if (h < 0.0f) continue;

hg[ng] = h;
xg[ng] = dc * cellSize;
zg[ng] = dr * cellSize;
ng++;
}
}

if (ng >= 3) {
float gx, gz;
getSlopeOfPoints(hg, xg, zg, ng, gx, gz);
globalSlope = sqrtf(gx * gx + gz * gz);
}
}

// --- Итог = max(локальный, глобальный, размах) ---
float result = localSlope;
if (globalSlope > result) result = globalSlope;
if (heightRange > result) result = heightRange;

dev_slope[z * Map_X + x] = result; }

__global__ void slope_to_costmap_cuda(const float* __restrict__ dev_slope, uint8_t* __restrict__ dev_costmap, float max_slope, float flat_slope)
{
int x = blockIdx.x * blockDim.x + threadIdx.x;
int z = blockIdx.y * blockDim.y + threadIdx.y;
if (x >= Map_X || z >= Map_Z) return;

int idx = z * Map_X + x; float s = dev_slope[idx]; uint8_t cost;

if (s < 0.0f) cost = 255; // unknown
else if (s > max_slope) cost = 254; // lethal
else if (s < flat_slope) cost = 0; // free
else {
float norm = (s - flat_slope) / (max_slope - flat_slope);
float c = 252.0f * norm;
cost = (uint8_t)(c < 1.0f ? 1.0f : (c > 252.0f ? 252.0f : c));
}

dev_costmap[idx] = cost;
}Получившаяся карта проходимости аналогична результату, получаемому в 3D Voxel Map to 2D Occupancy Map Conversion Using Free Space Representation, работает полностью на GPU со скоростью обновления данных от лидара и устраняет ошибки предыдущего ядра:

4. Выбор сенсора: лидар или RGB-D камера

При работе над предыдущими версиями карт возникала дилемма: что лучше использовать для карты проходимости — лидар или RGB-D камеру? Практика показала, что камера довольно ненадёжна и не может использоваться как единственный основной источник данных. Её единственное преимущество — плотные, большие сканы, позволяющие за один кадр оцифровать всю поверхность по курсу движения. Из-за особенностей алгоритмов камера всегда должна видеть перед собой на расстоянии около 5 м текстуры или хотя бы поверхность (текстуру наложит ИК-проектор), на которой она будет сопоставлять светящиеся точки (пиксели); иначе алгоритм построения глубины начинает разваливаться, выдавая случайный шум. Кроме того, на камеру сильно влияет солнечный свет, усиливая этот шум вплоть до полного развала алгоритма:

Чтобы эти ограничения надёжно выполнялись на роботе, единственный способ, который мне помог, — направить камеру под углом к земле; тогда она будет строить сканы земной поверхности. Для комбинирования с лидаром Livox MID-360 это хорошо из-за малых отрицательных углов склонения лазерного луча (всего 7°), из-за чего на скорости земная поверхность не успевает сканироваться. Для имитации этого эффекта изображение от камеры глубины обрезается так что-бы на кадр попала только земля как можно заметить нависающих помех теперь нет с оставшимися справляется лучевой фильтр благодоря постоянному эффекту окклюзии:

В то же время сам лидар по сравнению с камерой работает очень медленно: если камера примерно за 5 мс способна запечатлеть облако точек шириной 90° по курсу робота, то лидар делает это за 100 мс, из-за чего образуется сильный эффект rolling shutter — все предметы кажутся смазанными из-за движения. Кроме того, лидар видит пролетающих насекомых и пыль, которые остаются на снимках, поэтому использовать сырые данные лидара нельзя:

Лучевой фильтр без эффекта окклюзии на лидаре, возникающем только при наличи препяствий, по курсу движения робота (и противоположно ему) работать не будет в 90% ситуаций outdoor, ведь лучи не достигают цели и гаснут в пустоте. Таким образом, необходимо использовать облака точек, прошедшие обработку в алгоритме одометрии, который очистит их от эффекта rolling shutter, во всяком случае от собственного (эго) движения робота, и удалит движущиеся точки. Но эти облака точек, называемые ключевыми кадрами (keyframe’ами), обновляются довольно медленно, так как на очистку (естественно, в open-source это обычно делается на CPU, как и вообще все расчёты одометрии) требуется время, и, чтобы экономить ресурсы CPU, они обновляются лишь по мере движения робота. В частности, в используемой мной DLIO ключевой кадр обновляется при повороте робота относительно прошлого обновления более чем на 45° или при смещении более чем на 30 см; впрочем, эти параметры можно настроить:

Для Livox MID-360 — лидара с неповторяющимся сканированием — это плохо, ведь его физическая плотность лучей в вертикальной развёртке невелика и компенсируется благодаря многократному прохождению одного и того же места с разными углами зенита вертикальной развёртки. Таким образом, редкие keyframe’ы не позволяют отсканировать поверхность без пропусков.

Как решить эти проблемы? Можно объединить снимки лидара и камеры на одной карте: от камеры получить скан земной поверхности для карты проходимости, с одной стороны, и скан окружающего пространства (деревья, здания и т. д.) от лидара — с другой. Тем более что снимки лидара (keyframe’ы) обновляются довольно медленно, что для сканирования земной поверхности особенно плохо, но достаточно, чтобы просто зафиксировать наличие препятствия для алгоритма расчёта коллизий в планировщике: там достаточно и одной точки на периметре робота.

По перечисленным причинам был выбран именно такой подход:

В предыдущей статье алгоритм синхронизации камеры глубины и лидара уже был описан; их облака точек добавляются на воксельную карту для построения на её основе двухмерной карты проходимости. Я ожидал, что сгенерированная на GPU карта будет аналогична той, что была получена мной с использованием этого алгоритма в самодельном симуляторе.

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

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

6. Заключение

Фактический результат, полученный на роботе, действительно напоминает поведение, ранее воспроизведённое в симуляторе, и по сравнению с первой версией карты проходимости стал значительно лучше. Карта в целом корректно отображает проходимые и непроходимые области. При этом сохраняются проблемы: падение приложения и баг в шейдерной программе визуализатора, из-за которого модель робота смещается без необходимости, скачки одометрии. Эти проблемы не отменяют корректности самой карты, но требуют отдельной доработки.Теги:• cuda
• lidar
• стереокамераХабы:• Алгоритмы
• Робототехника