Робот в тумане: лошадь оставить, туман удалить

Утром наш уличный SpeedyBot отказался ехать: вокруг препятствия. Перед ним свободно. Протёрли лидар. Лучше не стало. Зато на облаке точек обнаружилось почти аккуратное кольцо вокруг корпуса. Получился «Ёжик в тумане», которому вместо лошади привиделась круговая оборона.

Дальше были миллионы точек, поездки к скамейке, виртуальные сетки и фильтр, который сначала научился убирать туман, а потом слишком увлёкся. Рассказываю, как мы вернули ему чувство меры.

Лидар чистый. Призраки настоящие

Первое подозрение пало на грязь, корпус робота и старые препятствия на карте. Поэтому пошли по цепочке назад: костмапа → облако препятствий → облако после локализации → сырые измерения RoboSense Airy Lite.

Выбрали 731 подозрительную точку, попавшую в слой статических препятствий. Все проследились до сырых измерений. У всех отражательная способность оказалась 2 или 3. Призраков приносил сам сенсор; искать их источник в интерфейсе уже не имело смысла.

На улице был лёгкий туман. Для лидара капли могут стать источником ложных отражений; такую остановку робота описывают и в работе Heinzler и соавторов о погодном шуме. В наших данных подозрительное поле ещё и заметно менялось между кадрами. Гипотеза складывалась.

Пока туман не ушёл, записали минуту: 598 сканов, 5,81 миллиона точек. Теперь утро можно было переигрывать сколько угодно, не заказывая погоду заново.

97% — отличный результат. Только робот об этом не знает

Начали с простого: убирать слабые ближние отражения без убедительной поддержки соседей. Затем добавили поиск линий и плоскостей, чтобы оставлять точки предметов. Вторая версия подавила 97,19% слабых кандидатов в области 0,4–2,2 метра.

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

Возник соблазн: если основную массу уже признали туманом, дочистим одиночные остатки. Заодно отдельно исключили отражения собственного корпуса по его геометрическим границам. Картинка улучшилась. А потом мы подсунули фильтру сетку.

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

Пока писали фильтр, туман закончился

Зато появился отличный контрольный эксперимент. Повторили запись почти из той же позы, с прежним лидаром и настройками. Утром медиана слабых внешних точек была 809 на кадр. После рассеивания тумана стала нулевой; за целую минуту нашлось всего 45 таких точек. Кольцо исчезло вместе с погодой.

За реальными предметами отправили робота в две точки: к скамейке и на площадку. Записали подъезд, повороты и остановки. Ориентации отличаются почти на 180 градусов, рядом фасад, ступени, опоры и вещи у крыльца. Фильтру предстояло сохранить эту геометрию при обоих ракурсах.

«Матрица» для робота: загрузи мне столб

Для повторяемых опытов подмешивали предметы прямо в записанное облако после LIO, то есть после оценки движения по лидару и IMU. Столб, доска, сетка, горизонтальная перекладина и диагональная проволока. Двигали их в данных, добавляли шум, совмещали с настоящим утренним туманом. У каждого предмета заранее известны координаты: исчезновение нельзя списать на неудачный ракурс.

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

Кольцо — улика, четыре точки — алиби

Полезную идею подсказала сама форма шума: туман окружает робота широким облаком, тогда как предметы чаще собираются в отдельные структуры. Разделили круг на 24 сектора и стали оценивать заполнение ближней области. В финальном профиле анализируем сферу 0,4–1,65 метра; для кольцевого контекста нужны минимум 32 точки, 13 занятых секторов и 9 секторов хотя бы с двумя точками.

Но круг сам по себе не даёт права всё стереть: круглыми бывают и реальные конструкции. Поэтому внутри распознанного кольца отдельно ищем геометрию предметов. Пара случайных точек легко изображает отрезок. Для защиты линии потребовали минимум четыре пространственно различные опоры, протяжённость от 20 сантиметров и отклонение от прямой до 1,2 сантиметра.

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

Изменения собрали в классификаторе препятствий. Исходный поток лидара, IMU и LIO сохранили: за шум для навигации теперь отвечает тот участок цепочки, где формируется облако препятствий. Радиусы и пороги вынесли в изменяемый профиль, чтобы следующий эксперимент обходился без пересборки бинарника.

Последний босс — костмапа

Финальную проверку делали настоящими ARM64-бинарниками классификатора и штатной костмапы в QEMU, воспроизводя записанные сообщения и преобразования координат. В полном туманном прогоне все 590 кадров дали ноль выходных точек препятствий в исследуемой сфере. На двух ясных поездках сохранились все 877 наблюдений контрольных геометрических компонентов.

Затем пропустили результат через два DepthLayer и InflationLayer. Для наглядности взяли свободную статическую подложку: на ней виден именно вклад текущих препятствий. Во всех 28 опубликованных костмапах туманного фрагмента внутри кольца 0,4–1,65 метра осталось ноль занятых ячеек. А все пять добавленных типов предметов создавали препятствие на каждой из 19 костмап, выпущенных во время их присутствия.

Получилось 57 тестов и 2489 входных кадров в финальных нативных прогонах. Но главная картинка здесь последняя: у робота больше нет стены из воздуха прямо под носом.

Утреннего «Ёжика в тумане» мы научили отличать облако капель от предмета внутри него. Лошадь оставляем. Туман убираем.