Утром SpeedyBot остановился перед пустой дорожкой: в облаке лидара возникло кольцо препятствий примерно в метре от корпуса. Протирка сенсора не помогла. Получился «Ёжик в тумане», только с запретом на движение.
Я Лев Ларин, инженер Unibot. Мы подключаем робота-доставщика Alfa Robotics SpeedyBot к своей платформе UNIBOT Fleet, сохраняя штатную навигацию. Это вторая часть серии: фильтрация погодного шума без удаления тонких препятствий.
Начало серии · ← Часть 1: Робот доставщик дороже автомобиля, мокрая трава и открытый JDWP · Часть 3: 25 FPS с видеокамеры по 3G. Модем, извини →
Лидар и тракт обработки
На роботе установлен RoboSense Airy Lite, выбранный драйвером как RSAIRYLITE_ETH. Характеристики Lite по руководству производителя:
Что | Значение и смысл для робота |
|---|---|
Каналы | 24 вертикальных направления сканирования |
Поле зрения | 360° по горизонтали, от −11,5° до +33,5° по вертикали |
Угловой шаг | 0,6° по горизонтали, около 1,96° по вертикали |
Частота и поток | 10 Гц, паспортные 144 000 точек/с |
Дальность | До 60 м; по мишени с отражением 10% — 40 м в центральных каналах и 30 м в крайних |
Интерфейс | 100BASE-TX, пакеты UDP по Ethernet |
Питание и корпус | 9–24 В, около 6 Вт, меньше 250 г, IP67 |
Режим отражений | Только Strongest Return — самый сильный отклик |
У Lite есть только Strongest Return. Переключить его на последнее отражение, чтобы попытаться увидеть предмет за каплей, нельзя.
На расстоянии 2 м шаг 0,6° даёт около 2,1 см между горизонтальными лучами (2 × tan(0,6°)), вертикальный шаг — около 6,8 см. Проволока или край сетки могут дать лишь несколько точек: удалять все редкие отражения опасно.
Лидар передаёт UDP-пакеты через внутреннюю Ethernet-сеть в rslidar_sdk_node на плате ZYSJ 2288K v01 с RK3588. Штатный ROS-тракт:
Airy Lite → UDP/Ethernet → rslidar_sdk_node → /rslidar_points сырые точки → laserLocation + IMU оценка движения, LIO → /cloud_registered совмещённое облако → obstacle_cloud_classifier выделение препятствий → /static_obstacle, /dynamic_obstacle → два DepthLayer → InflationLayer → локальная костмапа
LIO оценивает движение по лидару и IMU. Костмапа хранит занятое пространство с запасом для габаритов робота. Вход камеры глубины /remembered_points в начальных записях был пуст: кольцо пришло по лидарной ветке.
Откуда взялись точки
Проследили 731 подозрительную точку из статических препятствий до сырых измерений. Все имели reflectivity 2 или 3. Сопоставляли близкие по времени сообщения, приводили координаты с учётом положения сенсора и сравнивали XYZ. Между static и registered координаты совпали точно; медианное расхождение с raw — 0,131 мм. Допуски составляли 3 мм для registered и 30 мм для raw.
Для проверки ROS1-топиков:
rostopic hz /rslidar_points rostopic echo -n 1 /rslidar_points/fields rostopic echo -n 1 /cloud_registered/fields rosbag record -O fog.bag /rslidar_points /cloud_registered \ /static_obstacle /dynamic_obstacle /remembered_points /tf /tf_static
После LIO поле яркости в нашем облаке всегда равно нулю. Реальную reflectivity нашли только в raw. Порог intensity < 4 после LIO удалил бы всё облако. Шкала Lite 1–255 не означает проценты отражения.
На улице был туман, поле слабых отражений менялось между кадрами. Погодный шум такого рода описан в работе Heinzler и соавторов. Записали минуту: 598 сканов, 5,81 млн точек.
Почему подавления 97% недостаточно
Первый фильтр удалял слабые ближние отражения без поддержки соседей. Добавили поиск линий и плоскостей для сохранения предметов. Вторая версия подавила 97,19% слабых кандидатов в области 0,4–2,2 м.
Но inflation_radius=0,8 м раздувал оставшиеся точки в запретные зоны. При fade_away_time=2 с обновляющиеся отражения продолжали перекрывать проезд. Исключение отражений собственного корпуса улучшало картину, агрессивная очистка остатка удаляла настоящие препятствия.
В контрпримере одна и та же сетка сохраняла 15 отражений на 2,5 м и теряла все на 2 м.
Контрольные записи
После рассеивания тумана повторили запись почти из той же позы, с прежними настройками. Медиана слабых внешних точек снизилась с 809 до нуля на кадр; за минуту нашлось всего 45 таких точек.
Для проверки реальной геометрии записали две поездки: к скамейке и на площадку. Ракурсы отличаются почти на 180°; в облаках есть фасад, ступени, опоры и предметы у крыльца.
В записанное облако после LIO добавили пять моделей с известными координатами: столб, доску, сетку, горизонтальную перекладину и диагональную проволоку. Перемещали их, добавляли шум и совмещали с утренним туманом. Проверяли первую видимость и сохранение предмета в следующих кадрах.
Обнаружение кольца
Фильтр встроен в obstacle_cloud_classifier. Сначала он обнаруживает широкое кольцевое поле, затем защищает геометрию предметов и удаляет неподдержанный остаток.
В сферическом слое 0,4–1,65 м считаем x²+y²+z² и распределяем точки по 24 азимутальным секторам по 15°. Для включения режима нужны минимум 32 точки, 13 занятых секторов и 9 секторов с двумя точками. Эти сектора анализа независимы от вертикальных каналов лидара. Фрагмент ring.c:
int one = 0, two = 0; for (int b = 0; b < 24; ++b) { one += bins[b] > 0; two += bins[b] >= 2; } if (!s->ring_enabled || count < s->ring_min_points || one < s->ring_min_sectors || two < (int)(s->ring_min_sectors * 2 / 3) + 1) return 0; // кольцевой режим в этом кадре не включаем
При пороге 14 один из 590 кадров не включал режим: секторов с двумя точками было девять при требуемых десяти. Порог 13 даёт девять. После изменения повторили туман, ясные сцены и круглые конструкции. Радиус 1,65 м подобран для нашего записанного кольца; отражения корпуса исключаются отдельно по его границам.
Сохранение предметов
Вертикальной опоре нужны минимум четыре точки с разбросом по высоте от 12 см и горизонтальным радиусом 4 см. Для линий и плоскостей вычисляем ковариацию координат и собственные значения. Поддержанные точки объединяем по соседству до 28 см, оставляем группы минимум из четырёх точек.
Обычная PCA по всем соседям теряла тонкую перекладину: туман менял оценку направления. Поэтому перебираем пары точек, строим гипотезы прямых и ищем согласованные точки в радиусе 45 см. Это похоже на RANSAC, но пары выбираются перебором. В коде a — вектор от начала прямой к точке, v — единичное направление:
double d = a[0]*a[0] + a[1]*a[1] + a[2]*a[2]; double t = a[0]*v[0] + a[1]*v[1] + a[2]*v[2]; if (d > reach2) continue; if (d - t*t > tolerance2) continue; // t — координата вдоль прямой; // d - t*t — квадрат расстояния поперёк неё
Поперечный допуск — 1,2 см. Точки сортируем по продольной координате t, ищем последовательности с промежутками до 16 см. Требуются четыре разные опоры с разносом вдоль линии хотя бы 4 см и общей протяжённостью от 20 см.
Совмещение с временным фильтром
Штатный фильтр использовал XY-сетку 10 см, историю из трёх кадров и минимум два подтверждения с проверкой соседних ячеек. Уменьшение клетки до 4 см ослабило кольцо, но движущийся виртуальный столб сохранился лишь в 20 из 80 кадров вместо исходных 55.
Геометрию поэтому защищаем по текущему кадру. В кольцевом режиме маска проверяет и выход прежнего временного фильтра, и точки, возвращаемые защитой геометрии. За пределами кольца работают временной фильтр с клеткой 4 см и отдельная защита геометрии.
Параметры профиля:
{ "enabled": true, "grid_resolution_m": 0.04, "history_frames": 3, "min_votes": 2, "preserve_geometry": true, "ring_enabled": true, "ring_radius_m": 1.65, "ring_min_points": 32, "ring_min_sectors": 13, "ring_line_radius_m": 0.45, "ring_line_tolerance_m": 0.012, "ring_line_min_span_m": 0.20, "ring_line_max_gap_m": 0.16 }
Профиль читается на каждом скане, пороги меняются без перезапуска. C-код встроен в ARM64-классификатор на месте штатной функции, использует его выделение памяти и добавление точек. Обработка находится после LIO и до слоёв препятствий: сырые /rslidar_points, IMU и локализация сохраняют исходные измерения. Работа с бинарником описана в части про ARM.
Результат
Классификатор и штатную костмапу запускали как ARM64-бинарники в QEMU, воспроизводя сообщения и преобразования координат. Во всех 590 кадрах туманного прогона в исследуемой сфере осталось ноль выходных точек препятствий. На двух ясных поездках сохранились 877 наблюдений контрольных геометрических компонентов.
Выход пропустили через два DepthLayer и InflationLayer на свободной статической подложке. Во всех 28 костмапах туманного фрагмента внутри кольца 0,4–1,65 м осталось ноль занятых ячеек. Каждый из пяти добавленных типов предметов создавал препятствие на всех 19 костмапах за время своего присутствия.
Всего в финальных нативных прогонах — 57 тестов и 2489 входных кадров. Число 877 относится к наблюдениям компонентов в кадрах, не к отдельным размеченным предметам. Производительность QEMU с RK3588 не сравнивали.
18 октября фильтр установили на робота и проверили выходные топики: около 10 Гц. Нового проезда в настоящем тумане после установки в этой проверке не было. На записи ложное кольцо исчезло из костмапы, контрольные предметы сохранились.
Все 10 историй SpeedyBot
Робот доставщик дороже автомобиля, мокрая трава и открытый JDWP
Робот в тумане: лошадь оставить, туман удалить — вы здесь.
