Lidar для роботов: что даёт скан и как из него получается карта

Введение

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

Оба края уже рассмотрены здесь: с одной стороны, определение расстояния до одной точки, с другой — SLAM и A* по сетке занятости. Но всё это предполагает, что сетка уже существует. Нигде не сказано, откуда она взялась, а ответ — трассировка луча по ячейкам — одновременно прост и полон ловушек.

2D lidar — это датчик ToF на поворотном столике

Это самое полезное знание, потому что оно означает, что вторую половину, связанную с измерением расстояния, вы уже понимаете.

Внутри любого любительского 2D lidar находится один оптический модуль измерения времени пролёта, точно такой же, как на модуле серии VL53, направленный вбок и вращающийся. Угловой энкодер сообщает, куда он смотрел при каждом измерении. Питание и данные проходят через вращающуюся границу с помощью контактного кольца или оптической/индуктивной связи.

Поэтому «скан» — не фотография. Это список пар (angle, range), собранных по одному за полный оборот. Все ограничения ниже следуют из этого предложения.

Это также объясняет, почему ультразвуковые датчики не превращают во вращающийся lidar. Ультразвуковой луч имеет ширину в десятки градусов, поэтому его вращение почти ничего не даёт: вы лишь размазываете по комнате и без того широкий луч. Узкий оптический луч делает угловое разрешение осмысленным. Сканирование единственного датчика ToF с помощью сервопривода, как в руководстве по сканированию ультразвуком, действительно является медленным lidar с низким разрешением.

Бюджет отсчётов фиксирован, поэтому разрешение и свежесть конкурируют

Вот часть, которая удивляет людей, и её стоит понять до покупки чего-либо.

У модуля измерения расстояния есть одно значение производительности: сколько измерений в секунду он может выполнить. У RPLIDAR A1 это примерно 8000 отсчётов в секунду, и оно не меняется при изменении частоты вращения. Частоту вращения вы выбираете сами, обычно 1–10 Hz.

Поэтому число точек за оборот просто равно бюджету, делённому на частоту вращения:

points per revolution = samples per second ÷ spin rate
angular step          = 360° ÷ points per revolution

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

Быстро движущийся робот в открытом коридоре: вращайте быстро, вам нужна свежесть, а стены большие. Медленно движущийся робот, составляющий карту загромождённой комнаты: вращайте медленно, вам нужны детали, и задержка допустима.

Лучи расходятся, поэтому дальние препятствия попадают между ними

Угловой шаг — это угол, а не расстояние. Два соседних луча расходятся по мере распространения:

gap between beams ≈ range × angular step (in radians)

При 0.45° это промежуток 7.9 mm на расстоянии одного метра и 31 mm на расстоянии четырёх метров. Пространство между ними не измеряется.

Возьмём ножку стула шириной 20 mm. При вращении с частотой 10 Hz промежуток между лучами достигает 20 mm примерно на расстоянии 2.5 m — дальше ножка может целиком оказаться между двумя лучами и не вернуть сигнал. При 5.5 Hz то же происходит примерно на 4.6 m.

Это объясняет поведение, иначе похожее на неисправность датчика: тонкие препятствия появляются, когда робот приближается, и возникают на карте за один-два метра до него. Lidar не отказывал. Ножку просто никогда не измеряли.

Два следствия, которые стоит учесть при проектировании:

• Не доверяйте одному скану для тонких препятствий. Накопление нескольких сканов в сетке, как показано ниже, действительно находит ножки столов: каждый скан измеряет немного другие углы.
• Ножки стульев и столов — классический отказ домашнего робота, потому что они тонкие, находятся ровно на высоте lidar, а робот узнаёт о них слишком поздно.

Скан — не снимок

Точки за один оборот измерялись не в один момент. При 5.5 Hz обход занимает 182 ms, а робот не станет вежливо ждать.

• При движении со скоростью 0.5 m/s робот проезжает 91 mm за один оборот.
• При повороте с частотой 1 rad/s скан размазывается на 10.4°.

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

Именно поэтому sensor_msgs/LaserScan несёт scan_time и time_increment вместе с расстояниями: они позволяют потребителю определить, когда был выполнен каждый луч. Коррекция этого — преобразование каждой точки с использованием позы, которую робот занимал в момент отметки времени соответствующего луча, — называется компенсацией движения (deskewing), и хорошие программные реализации SLAM делают это за вас.

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

Вы получаете только один горизонтальный срез

2D lidar измеряет плоскость. Всё, что находится выше или ниже неё, для него не существует.

Классические три случая:

• Выступы. Столешница на высоте 700 mm невидима для lidar на высоте 150 mm. Робот уверенно проезжает под ней и застревает.
• Низкие препятствия. Порог двери, кабель, носок. Всё ниже плоскости.
• Обрывы. Спуск лестницы — пустота во всех направлениях, доступных lidar. Он читается как безупречно ровный пол.

Никакая обработка скана это не исправит, потому что информация не была получена. Реальные роботы сохраняют бампер, датчики обрыва и часто ещё направленный вниз или вперёд дальномер рядом с lidar — не потому, что lidar плох, а потому, что плоскость остаётся плоскостью. Когда вы читаете, что у робота-пылесоса «есть lidar и всё равно есть бамперы», вот почему.

От скана к сетке: трассировка лучей

Теперь вторая половина — картографирование. Сетка занятости делит мир на квадратные ячейки и для каждой хранит, насколько мы уверены, что она занята.

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

Поэтому каждый луч даёт три вердикта:

1. Ячейки, через которые прошёл луч → свидетельство свободы. Пройдите по линии от датчика к столкновению и слегка сдвиньте каждую ячейку в сторону пустого состояния.
2. Ячейка, в которой оказалось измерение → свидетельство занятости. Сдвиньте её в сторону полного состояния.
3. Ячейки за столкновением → ничего. Луч остановился. Вы ничего не узнали о том, что за этой стеной, поэтому эти ячейки нужно оставить нетронутыми.

Проход по линии — это просто растеризация отрезка, алгоритм Брезенхэма (Bresenham), тот же, что рисует линию на экране.

Измерение на максимальной дальности требует отдельной обработки. Многие драйверы сообщают максимум или бесконечность, когда ничего не вернулось. Это не «стена на расстоянии 12 m», а «ничего не найдено». Пометьте свободными все ячейки вдоль луча до максимальной дальности и не помечайте никакой занятой ячейки в конце.

Log-odds (логарифмическое отношение шансов): почему складывают, а не умножают

Можно хранить вероятность для каждой ячейки и перемножать обновления Байеса. На практике все хранят log-odds (логарифмическое отношение шансов):

l = ln( p / (1 - p) )          p = 1 / (1 + e^(-l) )

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

При p = 0.5 (полное незнание), соответствующем l = 0, выберите значение для попадания и значение для промаха — например, p_occ = 0.7 (l = +0.847) и p_free = 0.4 (l = −0.405) — и обновление будет:

l_cell += l_occ     # this beam hit this cell
l_cell -= 0.405     # this beam passed through this cell

Ячейка, начавшая с l = 0, после трёх попаданий:

Свидетельства накапливаются, а противоречие оттягивает оценку назад. Вот и весь алгоритм.

Ограничивайте итоговые значения. Это важнее, чем кажется. Двадцать последовательных попаданий без ограничения доведут ячейку до l = 16.9, то есть p = 0.999999. Теперь кто-то переставил стул. Для отмены этой уверенности нужны 42 наблюдения свободного пространства. Стул оставляет постоянный фантом, и планировщик продолжает объезжать пустоту.

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

Неизвестное — третье состояние, а не синоним свободного

Это ошибка, которая приводит к уверенным и опасным планам.

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

Оба варианта неверны; сохраняйте три состояния различимыми и принимайте решение осознанно:

• Занято — не планировать проход.
• Свободно — планировать проход.
• Неизвестно — планировать проход только с затратами или только при намеренном исследовании.

Фронтирное исследование (исследование границы между свободной и неизвестной областями), с помощью которого робот выбирает, куда смотреть дальше, полностью построено на этой границе. Если объединить эти состояния, вы выбросите всю идею.

Выбор размера ячейки

Обычный ответ для роботов в помещении — 5 cm, и есть причина не делать ячейки мельче только потому, что это возможно.

Ячейка меньше промежутка между лучами на типичной дальности изобретает точность, которой у вас нет: на 4 m лучи находятся на расстоянии 31 mm, поэтому ячейки 1 cm оставляют постоянные дыры между лучами, которые никогда не заполнятся. Ячейки, намного большие, чем запас по проходимости робота, наоборот, делают дверные проёмы непроходимыми, потому что одна занятая ячейка может поглотить весь просвет.

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

Когда что-то идёт не так

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