Можно ли отслеживать полёт дрона с земли в условиях, когда спутниковая навигация недоступна, а камера теряет цель? На складе инвентаризационный дрон летит вдоль стеллажей и считывает маркировку. Чтобы проверить, где он отклонился от маршрута, нужны координаты вдоль всего пролёта, а внутри здания GPS ловит плохо. На открытой площадке за дроном можно следить по видео, но в дождь или туман детектор может начать пропускать его на отдельных кадрах.

Я решил проверить, можно ли наблюдать за дроном с помощью неподвижного наземного лидара. Он даёт трёхмерные координаты отражений без бортовой телеметрии и при слабом освещении.

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

На чём я проверял

Я взял запись с Livox Horizon, установленного на штативе у дороги. Пока квадрокоптер взлетал и летел на разном удалении от датчика, в поле зрения оставались дорога и трава, отражения от которых потом пришлось отделять. В топик /livox/lidar поступало около 100 сообщений PointCloud2 в секунду, по 2 400 записей в каждом. Для обработки я брал из них координаты XYZ.

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

Как из сканов получить кандидатов на дрон

1. Что делать, если в одном скане дрона почти не видно

Даже если дрон всё время остаётся в поле зрения Livox Horizon, его след в облаке от скана к скану выглядит по-разному. Развёртка у Horizon неповторяющаяся, поэтому за время одного скана лучи могут лишь несколько раз попасть на корпус или вовсе пройти мимо. Я собираю 50 последовательных сканов в одно облако, чтобы отражения из разных моментов дополняли друг друга и после фильтрации от дрона могло остаться скопление точек. Руководство Livox Horizon, раздел Product Characteristics

На графике показано окно, которое начинается примерно через две секунды после первого лидарного сообщения. В первом скане окна все 2 400 записей имели координаты (0, 0, 0), и лишь в четвёртом появились первые 90 ненулевых точек. Когда я объединил все 50 сообщений, их оказалось 10 425. По двум проекциям видно, насколько больше участков сцены оставляют след в накопленном облаке. В этот подсчёт входят и земля, и остальные объекты.

На обеих проекциях показаны ненулевые точки, нанесённые по азимуту и углу места в системе координат лидара
На обеих проекциях показаны ненулевые точки, нанесённые по азимуту и углу места в системе координат лидара

В voxel_grid_filter.py каждое сообщение добавляется в буфер, а когда в нём набирается 50 сканов, массивы координат объединяются и буфер освобождается для следующего окна. Здесь и дальше привожу сокращённые фрагменты кода ROS-нод.

points = self.ros_to_numpy(cloud_msg)
if len(points) == 0:
    return

self.raw_buffer.append(points)
if len(self.raw_buffer) >= self.accumulation_frames:
    dense_points = np.vstack(self.raw_buffer)
    self.raw_buffer = []

При частоте около 100 Гц такое окно занимает примерно полсекунды. Квадрокоптер, летящий со скоростью 5 м/с, за это время успел бы пройти около 2,5 м, так что отражения от корпуса могут вытянуться вдоль траектории. Накопленное облако получает метку последнего скана, хотя часть точек соответствует более ранним положениям. Поэтому центроид скопления может отставать от положения дрона на момент, указанный в сообщении.

2. Как проредить облако и сохранить точки дрона

За одно окно набирается около 120 000 записей, причём несколько сканов могут дать десятки близких точек на одном участке дороги и всего несколько на соседнем. При поиске плоскости по числу согласованных измерений густо просканированный участок получает больший вес, даже если занимает меньшую площадь. Чтобы сократить число повторных измерений, я разбиваю пространство на кубические ячейки, или воксели. Open3D заменяет точки в каждой занятой ячейке их центроидом, и на следующий этап от неё уходит одна точка со средними координатами.

o3d_cloud = o3d.geometry.PointCloud()
o3d_cloud.points = o3d.utility.Vector3dVector(points.astype(np.float64))
downsampled = o3d_cloud.voxel_down_sample(self.leaf_size)
return np.asarray(downsampled.points, dtype=np.float32)
Визуализация работы Voxel Grid Filter: точки, накопленные за 50 кадров (зеленые), точки, полученные с помощью вокселизации (красные)
Визуализация работы Voxel Grid Filter: точки, накопленные за 50 кадров (зеленые), точки, полученные с помощью вокселизации (красные)

Чтобы посмотреть, сколько точек теряется при укрупнении сетки, я взял те же 50 сканов и убрал нулевые тройки, как при построении графика. Для обеих настроек я использовал один и тот же набор из 10 425 точек. При ребре вокселя 30 см их осталось примерно втрое меньше, чем при 5 см.

Ребро вокселя

Точек после фильтрации

5 см

7 802

30 см

2 600

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

3. Как убрать землю

Если передать прореженное облако сразу в кластеризацию, тысячи точек дороги могут образовать плотную группу, к которой присоединятся расположенные рядом объекты. На моей записи дорогу можно приближённо описать плоскостью, поэтому для её оценки я использую RANSAC. Он строит гипотезы по случайным тройкам точек и сравнивает, сколько измерений лежит достаточно близко к каждой плоскости. Благодаря этому отражения от травы и дрона, которые лежат в стороне, слабее влияют на выбор модели. Для инициализации я объединяю первые четыре вокселизированных облака и принимаю модель, если ей соответствуют как минимум 500 точек и не менее 5% исходной выборки. Оригинальная работа Фишлера и Боллса о RANSAC

В ангаре или на складе алгоритм может сначала найти стену, если рядом с ней оказалось больше точек, чем возле пола. Поэтому в ransac_filter.py я проверяю направление нормали. Для плоскости, которая близка к горизонтальной в системе лидара, модуль компоненты нормали вдоль оси Z должен быть больше 0,8. Если условие не выполнено, точки найденной плоскости исключаются из поиска и RANSAC пробует снова, пока оставшихся точек достаточно. При наклонной установке датчика пришлось бы отдельно учесть, куда направлена вертикаль.

while True:
    plane_model, inliers = o3d_cloud.segment_plane(
        distance_threshold=self.distance_threshold,
        ransac_n=3,
        num_iterations=self.max_iterations,
    )
    _, _, c, _ = plane_model
    if abs(c) > 0.8:
        break

    o3d_cloud = o3d_cloud.select_by_index(inliers, invert=True)
    if len(o3d_cloud.points) < 1000:
        self.plane_model = None
        return

Когда плоскость найдена, я сохраняю её коэффициенты, поскольку неподвижный датчик продолжает видеть дорогу в той же системе координат. В каждом следующем окне достаточно измерить расстояние от точек до сохранённой плоскости. Точки ближе 10 см относятся к земле, остальные передаются в DBSCAN. Если при инициализации подходящую плоскость найти не удалось, кластеризация получает всё облако.

a, b, c, d = self.plane_model
distances = np.abs(np.dot(points, [a, b, c]) + d)
plane_mask = distances < self.distance_threshold
return points[~plane_mask], points[plane_mask]

4. Как найти отдельные скопления

Даже после удаления земли в облаке могут остаться и дрон, и трава, и фрагменты других объектов, поэтому я не задаю число групп заранее - DBSCAN находит их по плотности. При текущих настройках точка становится ядром кластера, если в её окрестности радиусом 15 см есть хотя бы 30 точек, считая её саму. Соседние ядра объединяются, к ним присоединяются граничные точки, а те, которые не вошли ни в одну группу, получают метку -1. Работа Эстера и соавторов о DBSCAN

labels = DBSCAN(
    eps=self.eps,
    min_samples=self.min_samples,
).fit_predict(points)

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

В /dbscan_clusters точки одной группы получают общий цвет, а шумовые точки остаются серыми. Так в RViz удобно смотреть, какие отражения попали в одну группу, однако DBSCAN обрабатывает каждое окно отдельно, поэтому номер и цвет кластера могут измениться, даже если он снова соответствует тому же объекту.

Что получилось на записи полёта

Первая проверка на сегментации земли

В первом опыте я взял воксели с ребром 30 см, и после такой фильтрации в одном окне из 120 000 записей осталось 2 615 точек. Порог RANSAC тогда составлял 1 м, чтобы в полосу вокруг плоскости попадали и дорога, и трава, которая находилась на другой высоте. С моделью согласовались 10 428 из 10 432 точек, использованных для её поиска, а в RViz над полосой земли было видно светлое скопление, которое я визуально связал с квадрокоптером. Такой порог захватывает неровности поверхности, но при низком полёте вместе с ними могут исчезнуть и отражения от корпуса.

Сегментация полевой записи в RViz. Точки плоскости показаны красным, точки вне плоскости светлым
Сегментация полевой записи в RViz. Точки плоскости показаны красным, точки вне плоскости светлым
Фото с полевого эксперимента
Фото с полевого эксперимента

Повторный прогон с кластеризацией

Для второго прогона я обработал всю запись офлайн, читая сообщения из rosbag по порядку и сохраняя нулевые тройки так же, как при штатном запуске. Из 15 325 сканов за 153,2 с получилось 306 полных окон по 50 сканов, а ещё 25 сообщений остались в буфере. По первым четырём окнам я оценил плоскость, зафиксировав начальное состояние генератора Open3D значением 0. По остальным 302 окнам посчитал медианы для вокселя с ребром 5 см и порога RANSAC 10 см.

Этап

Медиана числа точек за окно

После накопления сканов

120 000

После воксельной фильтрации

7 694

В полосе вокруг плоскости земли

2 996

Вне полосы, на входе DBSCAN

4 740

При этих настройках хотя бы один кластер нашёлся в 119 из 302 окон (39,4%), причём в 89 из них кластер был единственным. В 183 окнах DBSCAN отнёс все точки вне полосы земли к шуму. Эти числа описывают всю сцену, поэтому для оценки детекции нужно разметить окна, в которых дрон виден, и проверить, совпадает ли его положение хотя бы с одной из найденных групп.

Что можно сделать после детекции

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

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

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