Есть ли свет в конце тоннеля? Проверяем путь по данным лидара

Есть ли свет в конце тоннеля? Возможно. Но нас попросили проверить, нет ли по дороге к нему чего-нибудь, во что упрётся поезд :)

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

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

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

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

Это решение нашей команды для кейса Московского транспорта на ЛЦТ2026. Ниже разберу именно геометрическую часть: от ближней пары рельсов до обнаружения объекта. Без классификатора видов препятствий и без заранее построенной карты тоннеля.

Пройдём весь путь на одном эпизоде испытательной записи с синтетическими препятствиями. Начнём со 175 688 точек текущего облака, восстановим рельсы и обнаружим большой квадрат в 96,96 м от лидара. Ещё через 0,30 с новый пакет измерений подтвердит обнаружение. На этом эпизоде разберём основные этапы, а затем отдельно проверим провод сверху.

Один и тот же участок в двух проекциях. В расчёте используются исходные точки; для обзорного рисунка часть точек прорежена.

Начнём с того, куда вообще ставить область поиска. До самого препятствия ещё придётся добраться.

Прямоугольник поставить легко. Правильно поставить сложнее

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

Но путь поворачивает, меняется его положение относительно лидара, у поперечного сечения есть наклон. Габарит должен следовать за рельсами. Иначе на повороте мы начнём проверять одну часть тоннеля, а поезд поедет по другой.

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

Контактный рельс связывает ближний поиск с восстановлением пути дальше по тоннелю.

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

Почему полтора метра, а не два: что говорят нормы

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

Ширина колеи на прямых участках метро - 1520 мм между внутренними гранями головок. На кривых малого радиуса предусмотрено уширение. Отсюда и расстояние порядка полутора метров между рельсами. Эти размеры задаёт СП 120.13330.2022 «Метрополитены», п. 5.7.1.7.

Ширина колеи измеряется между внутренними гранями головок. Оранжевые точки условно показывают опоры, выделяемые алгоритмом.

Контактный рельс расположен сбоку от ходового, а его рабочая поверхность - выше головок пути. В ПТЭ метрополитена, п. 3.26 указаны 690 мм от внутренней грани ближайшей ходовой головки до оси контактного рельса и 160 мм по высоте до его рабочей поверхности. То есть и эту конструкцию мы ищем в определённой области рядом с путём.

Габариты описывают пространство для поезда и расположения оборудования. Их очертания для метрополитена задаёт ГОСТ 23961-2024. Для нашей задачи важна сама идея: восстановив путь, можно определить область, в которой посторонний предмет будет мешать проезду.

Получается понятная последовательность: найти пару рельсов с ожидаемой геометрией, проследить путь и проверить пространство вдоль него. Теперь посмотрим, как это сделать по точкам лидара.

1. Сжимаем первые восемь метров в поперечное сечение

Начнём с системы координат. Обозначим продольную координату через s, поперечную через v, высоту через z. Для первоначального поиска берём точки на первых 8 м. Их продольная координата временно не нужна: смотрим на проекцию в плоскость (v, z).

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

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

$\sigma_{\text{сетка}}=\frac{0{,}025}{0{,}005}=5\text{ ячеек}$

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

Как из пятна получить кандидата

В окрестности максимума радиусом 85 мм собираем точки. Поперечную координату кандидата берём как медиану, высоту - как 75-й процентиль:

$\hat v=\operatorname{median}(v_i),\qquad \hat z=Q_{0{,}75}(z_i)$

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

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

$a=\sum_i\exp\left(-\frac{\lVert p_i-m\rVert^2}{2\cdot 0{,}025^2}\right)$

Здесь pᵢ = (vᵢ, zᵢ), координаты в метрах. Точка в 25 мм от максимума даёт примерно 0,607, в 50 мм - 0,135. Так два соседних скопления не получают одинакового влияния только потому, что оба попали в широкую область поиска.

Затем проверяем саму верхнюю поверхность. В полосе ±55 мм поперёк и ±20 мм по высоте нужны хотя бы восемь точек и непрерывная последовательность из трёх занятых ячеек шириной 10 мм. Ячейка считается занятой при наличии минимум двух точек.

То есть нас интересует небольшой поддержанный участок поверхности. Один удачно торчащий выброс эту проверку не проходит.

Прямоугольниками отмечены полосы поддержки выбранных головок. Серые кружки в нижней панели - остальные кандидаты.

Почему выбираем пару сразу

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

Для левого и правого кандидатов вычисляем:

$\begin{aligned}g&=v_{\text{п}}-v_{\text{л}},\\ c&=\frac{v_{\text{л}}+v_{\text{п}}}{2},\\ h&=\frac{z_{\text{л}}+z_{\text{п}}}{2},\\ \Delta z&=z_{\text{п}}-z_{\text{л}}.\end{aligned}$

g - поперечное расстояние между кандидатами, c - положение середины пары, h - её средняя высота, Δz - перепад высот.

Сначала работают жёсткие ограничения: |c| ≤ 0,30 м, 1,30 ≤ g ≤ 1,80 м, |Δz| ≤ 0,18 м. Среди допустимых пар выбираем наиболее убедительную.

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

$\begin{aligned}P_g&=0{,}12\left(\frac{g-1{,}56745}{0{,}07}\right)^2+2\left(\frac{e}{0{,}07}\right)^2,\\e&=\max(1{,}48-g;\;0;\;g-1{,}62).\end{aligned}$

Здесь 1,56745 м - ориентир расстояния между опорами головок, выбранный по данным для настройки детектора и округлённый для записи формулы. Первое слагаемое мягко предпочитает близкие к нему расстояния, второе сильнее штрафует выход из диапазона 1,48-1,62 м. Найденное по облаку расстояние g при этом не меняется: штраф влияет только на выбор пары.

Посчитаем. При g = 1,60 м штраф около 0,026. При g = 1,70 м он уже около 3,04. Обе пары ещё проходят широкий жёсткий фильтр, но второй понадобилась бы гораздо более сильная поддержка, чтобы выиграть.

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

Поддержки a и b ограничены сверху значением 80; q₁, q₂ - качества верхних поверхностей от 0 до 1; σ₁, σ₂ - разбросы скоплений в метрах:

$\begin{aligned}S={}&0{,}4\,[\ln(1+a)+\ln(1+b)]\\&+0{,}15\,\frac{\min(a,b)}{\max(a,b)}+0{,}4\,(q_1+q_2)\\&-P_g-0{,}12\left(\frac{c}{0{,}30}\right)^2\\&-0{,}08\left(\frac{h+1{,}09348}{0{,}35}\right)^2\\&-0{,}10\left(\frac{\Delta z}{0{,}18}\right)^2-0{,}25\,\frac{\sigma_1+\sigma_2}{0{,}05}.\end{aligned}$

Качество поверхности равно произведению двух насыщаемых множителей: min(1, N / 20) · min(1, K / 6), где N - число точек в полосе, K - длина непрерывной цепочки занятых ячеек. Разброс скопления - квадратный корень из наибольшего собственного значения ковариации его точек.

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

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

$\begin{aligned}(v_{\text{л}},z_{\text{л}})&=(-0{,}77309;\;-1{,}09447)\text{ м},\\(v_{\text{п}},z_{\text{п}})&=(0{,}78769;\;-1{,}08519)\text{ м},\\g&=1{,}56079\text{ м},\qquad \Delta z=9{,}27\text{ мм}.\end{aligned}$

В полосах верхних поверхностей находятся 408 и 412 точек. Итоговая оценка пары S = 4,2103, отрыв от альтернативы ΔS = 1,1884. Дальше передаётся именно эта пара.

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

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

$\kappa=\left(1-\exp\left(-\frac{a_{\min}}{20}\right)\right)\left(1-\exp\left(-\frac{\Delta S}{0{,}25}\right)\right)$

a_min - меньшая из исходных поддержек двух головок, до ограничения сверху; ΔS - отрыв от лучшей отличающейся пары. Если такой пары нет, берётся ΔS = 3. При κ < 0,25 результат отвергается. Это эвристический показатель поддержки и различимости, не вероятность правильного ответа.

Если эта проверка не пройдена, ближнюю пару не принимаем. В нашем эпизоде опора есть. Теперь можно искать рядом с ней контактный рельс - именно он поможет продолжить путь дальше.

2. Узнаём контактный профиль среди соседних деталей

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

В этой части алгоритма шаблон - набор точек T = {tⱼ}. Для каждой стороны переводим облако в координаты относительно найденной головки, причём боковую ось направляем наружу от пути. Тогда один шаблон можно искать и слева, и справа.

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

В ближнем поиске оптимизируются два смещения, без свободного вращения шаблона. Обозначим их a = (d, h): боковое и вертикальное относительно ходового рельса.

Начальное ожидаемое положение и масштабы отклонений:

$\begin{aligned}\mu&=(0{,}63344;\;0{,}25559)\text{ м},\\\sigma&=(0{,}035;\;0{,}055)\text{ м}.\end{aligned}$

Допустимая область шире: боковое смещение от 0,49 до 1,29 м, высота от −0,12 до 0,58 м. Ожидаемое положение помогает поиску, но не заставляет ответ совпасть с одной заранее заданной точкой.

Сначала решаем задачу подгонки

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

Для наблюдаемой точки pᵢ расстояние до смещённого шаблона:

$\delta_i(a)=\min_j\lVert p_i-(t_j+a)\rVert$

Нормируем его на 15 мм. Добавляем два остатка, которые удерживают положение около ожидаемого:

$\begin{aligned}r_i(a)&=\frac{\delta_i(a)}{0{,}015},\\r_d(a)&=0{,}1\sqrt N\,\frac{d-\mu_d}{\sigma_d},\\r_h(a)&=0{,}1\sqrt N\,\frac{h-\mu_h}{\sigma_h}.\end{aligned}$

N здесь - число точек в области подгонки. Множитель √N удерживает масштаб позиционного ограничения относительно меняющегося числа наблюдений.

Для самой оптимизации берём уникальные точки рядом с ожидаемым профилем: его ограничивающий прямоугольник расширяем на 15 мм, затем ещё на 40 мм для фиксированного контекста. Этот набор выбирается до оптимизации и не меняется при движении шаблона. Для последующей оценки кандидата используем все уникальные точки допустимой области поиска.

Оптимизатор минимизирует сумму потерь Коши по всем этим остаткам:

$a^*=\underset{a}{\operatorname{arg\,min}}\;\frac12\sum_k\ln\left(1+r_k(a)^2\right)$

Это относится и к расстояниям, и к двум остаткам положения. Отдельного чисто квадратичного штрафа на положение в этой подгонке нет.

Зачем Коши? При ошибке 15 мм нормированный остаток равен 1. При ошибке 60 мм - уже 4. В квадратичной потере вклад вырос бы с 0,5 до 8, а здесь с 0,347 до 1,417. Далёкая посторонняя точка всё ещё влияет на решение, но не командует им.

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

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

Хорошо приложить шаблон ещё недостаточно

Оптимизатор нашёл смещение. Теперь нужно разобраться, заслуживает ли этот вариант доверия. Для этого проверяем положение профиля, близость точек к его форме и полноту покрытия.

Начнём с положения: насколько далеко пришлось уйти от ожидаемого места?

$G=\exp\left\{-\frac12\left[\left(\frac{d-\mu_d}{\sigma_d}\right)^2+\left(\frac{h-\mu_h}{\sigma_h}\right)^2\right]\right\}$

В ожидаемом положении G = 1. Смещение на 35 мм по боковой координате при неизменной высоте даёт G ≈ 0,607. На 70 мм - 0,135.

Вторая составляющая - насколько наблюдения похожи на профиль. Берём точки внутри ограничивающего прямоугольника шаблона с небольшим запасом, сортируем расстояния и оставляем лучшие 60%:

$\begin{aligned}m&=\max\left(1,\left\lfloor0{,}6N_{\text{внутри}}\right\rfloor\right),\\D&=\frac1m\sum_{j=1}^{m}\min\left(\delta_{(j)},0{,}5\right),\\O&=\exp\left(-\frac{D}{0{,}015}\right).\end{aligned}$

δ₍ⱼ₎ - расстояния в порядке возрастания, D - их усечённое среднее в метрах. Ограничение 0,5 м относится к каждому расстоянию в этой сумме.

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

Поэтому третья составляющая смотрит в обратную сторону - от шаблона к наблюдениям:

$\begin{aligned}D_{\text{обр}}&=\frac1M\sum_j\min_i\lVert t_j+a-p_i\rVert,\\\tau&=\max(\Delta,1{,}5\ell),\qquad C=\exp\left(-\frac{D_{\text{обр}}}{\tau}\right).\end{aligned}$

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

Итоговая оценка:

Теперь подставим значения нашего кадра. Контактный рельс найден справа. Относительно ходовой головки его смещение:

$a^*=(0{,}622967;\;0{,}269962)\text{ м}$

Усечённое среднее расстояние наблюдений до шаблона D = 1,543 мм, а масштаб покрытия τ = 7,050 мм. Получаем:

$\begin{aligned}G&=0{,}92417,\quad O=0{,}90224,\quad C=0{,}76738,\\S&=1{,}12\cdot0{,}92417+0{,}90224+0{,}35\cdot0{,}76738\\&\approx2{,}20589.\end{aligned}$

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

Профиль поддерживают 1233 исходных отражения. В поперечной проекции среди них 635 уникальных точек: разные исходные измерения могут иметь одинаковые поперечные координаты. При оценке формы дубликаты не превращаются в новую геометрическую информацию.

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

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

3. Продолжаем профиль, каждый раз проверяя стык

Ближний контактный рельс найден. Теперь хочется вытянуть его линию вдоль тоннеля. Одна длинная прямая здесь быстро перестаёт соответствовать повороту пути.

Вместо этого идём перекрывающимися окнами длиной 8 м с обычным шагом 4 м. В каждом следующем окне снова ищем профиль. Основную оценку направления получаем по уже подтверждённым точкам текущего облака на последних восьми метрах. Для разреженных участков предусмотрена повторная проверка направления - к ней вернёмся чуть ниже.

Для оценки направления используем главную компоненту:

$\begin{aligned}\bar p&=\frac1N\sum_i p_i,\\Q&=\frac1N\sum_i(p_i-\bar p)(p_i-\bar p)^{\mathsf T}.\end{aligned}$

t - собственный вектор Q для наибольшего собственного значения.

Вытянутое скопление задаёт продольную ось. Знак t согласуется с предыдущим направлением. Резкий поворот локального направления больше 5° не принимается как обычное продолжение.

Бирюзовым показаны принятые измерения контактного рельса, золотистым - последовательность опор. Фон и масштабы одинаковы во всех трёх панелях.

Новый профиль обязан объяснить старые точки

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

В перекрытии считаем расстояния старых поддерживающих точек до нового профиля. Обозначим их dᵢ. Основной показатель - 90-й процентиль:

$d_{90}=Q_{0{,}90}\left(\{d_i\}\right)$

Если d₉₀ ≤ 30 мм, перекрытие считается хорошим. Допускается и более слабое согласование: d₉₀ ≤ 50 мм, если одновременно ограничено среднеквадратичное расстояние:

$\sqrt{\frac1N\sum_i\min(d_i,0{,}06)^2}\le0{,}04\text{ м}$

Внутри корня каждое расстояние ограничено 60 мм. Это уменьшает влияние отдельных далёких точек, но проверка 90-го процентиля не позволяет оправдать массовое расхождение.

Например, d₉₀ = 24 мм даёт хорошее перекрытие. 42 мм при ограниченном среднеквадратичном расстоянии 31 мм - слабое, но допустимое. Те же 42 мм при среднеквадратичном 43 мм уже не проходят. Это примеры пороговых решений, а не статистика записи.

Одновременно проверяется и новый участок: минимум три уникальные поддерживающие точки на расстоянии до 20 мм от профиля, ограничение ошибки формы, смещения и отрыва от конкурирующего варианта.

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

Что делать со слабым участком

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

Для подтверждения последовательности опор по последним подтверждённым и предварительным точкам строится квадратичная пространственная кривая. Для каждой координаты это обычная модель:

u - нормированная продольная координата. Коэффициенты находятся методом наименьших квадратов. Для подтверждения нужны хотя бы три опоры на протяжении минимум 2 м, поддержка уникальными измерениями и отклонения всех использованных опор от кривой не больше 50 мм.

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

Если между отражениями оказался пробел

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

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

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

Совмещаем историю, прежде чем пользоваться ею

Когда текущего облака недостаточно, добавляем доступные предыдущие - до 12 кадров в окне истории. Между ними датчик перемещается, поэтому координаты сначала нужно привести к одной системе.

Пусть Hᵢ - матрица позы, переводящая точки кадра i в общую систему. В однородных координатах:

$p_T=H_T^{-1}H_i p_i$

Сначала переводим точку старого кадра в общую систему, потом - в систему текущего датчика.

Для простого случая без вращения: раньше объект находился в 20 м впереди. Датчик проехал 2 м к нему. В новом кадре старая точка должна оказаться на 18 м. Если оставить 20 м, мы сами растянем объект вдоль движения.

Для сравнения использованы одни и те же точки первого нового окна. Цвет обозначает возраст измерения, а не класс объекта.

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

В выбранном эпизоде поиск сделал 20 шагов. Вместе с начальной получилась 21 опора. Контактную линию поддерживают 8742 исходных измерения: 1948 из текущего облака и 6794 из предыдущих. Самая дальняя подтверждённая точка находится примерно в 109,44 м от лидара.

Вот здесь история перестаёт быть абстрактным «можно накопить несколько кадров». На этом участке большая часть поддержки действительно пришла из предыдущих наблюдений.

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

4. Превращаем опоры в гладкую пространственную кривую

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

Поэтому между опорами и восстановлением рельсов есть отдельная задача сглаживания.

Близкие продольные опоры сначала объединяем в пределах 25 см. Затем представляем кривую кубическим B-сплайном с внутренними узлами через 4 м:

$c(s)=\sum_j B_j(s)a_j$

Bⱼ(s) - базисные функции сплайна, aⱼ - неизвестные трёхмерные коэффициенты. Они и определяют форму кривой.

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

Что именно минимизируется

Для одной координаты обозначим y вектор измерений, a вектор коэффициентов, Xᵢⱼ = Bⱼ(sᵢ) матрицу значений базиса в опорах. Тогда прогноз равен Xa.

Вторая и третья производные сплайна вычисляются на сетке через 0,5 м. Для двух горизонтальных координат штраф устроен так:

$\lVert La\rVert^2=\left\lVert\frac{D_2a}{0{,}025}\right\rVert^2+\left\lVert\frac{D_3a}{0{,}004}\right\rVert^2$

D₂, D₃ - матрицы вторых и третьих производных базисных функций. Для вертикальной координаты обе производные нормируются на свой настроенный масштаб около 0,00405 в соответствующих единицах производной.

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

Полная задача на одной итерации:

$\begin{aligned}a^*&=\underset{a}{\operatorname{arg\,min}}\left[\lVert W(Xa-y)\rVert^2+\lVert La\rVert^2\right],\\W_{ii}&=\frac{\sqrt{w_i}}{\sigma_i}.\end{aligned}$

σᵢ задаёт масштаб неопределённости измерения, wᵢ уменьшает влияние плохо согласованной опоры. В начальной модели измерений здесь используется стандартное отклонение 35 мм по каждой оси.

Её удобно решать одним методом наименьших квадратов, дописав к системе строки регуляризации:

$\left[\begin{matrix}WX\\ L\end{matrix}\right]a\approx\left[\begin{matrix}Wy\\ 0\end{matrix}\right]$

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

Как не дать одной плохой опоре искривить путь

После подгонки вычисляем нормированную пространственную ошибку каждой опоры:

$\begin{aligned}r_i&=\sqrt{\sum_{k\in\{x,y,z\}}\left(\frac{\hat c_{ik}-p_{ik}}{\sigma_{ik}}\right)^2},\\w_i&=\max\left(10^{-5},\min\left(1,\frac{1{,}5}{\max(r_i,\varepsilon)}\right)\right).\end{aligned}$

ĉᵢ - текущая оценка кривой в опоре, pᵢ - измеренная опора, ε = 10⁻¹² защищает деление на ноль. Это веса Хьюбера с нижним численным ограничением 10⁻⁵. Они общие для трёх координат одной опоры.

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

При r = 1 вес остаётся единичным. При r = 3 получаем 0,5, при r = 6 - 0,25. Пересчитываем веса, снова решаем взвешенную задачу. В реализации выполняется шесть таких итераций.

Согласованные опоры сохраняют полный вес. Влияние опоры с большой ошибкой уменьшается на следующей подгонке.

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

Медиана расстояния от опор до сглаженной кривой - 9,77 мм, максимум - 31,86 мм. Это величина изменения при подгонке, а не ошибка относительно эталонного пути.

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

5. По контактному рельсу восстанавливаем два ходовых

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

Конструкция пути ограничивает задачу. Две ходовые головки и контактный профиль связаны поперечными расстояниями и высотами. Поэтому каждый новый срез не приходится разбирать с нуля. Но и просто прибавить одно «стандартное число» недостаточно: сечение может быть наклонено, а сам путь поворачивает.

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

Посмотрим на одно сечение без пространственной перспективы. В нашем примере от контактной опоры до ближайшей головки нужно отложить 0,622 м в сторону пути и 0,271 м вниз по нормали сечения. Вторая головка находится ещё на 1,583 м дальше поперёк пути. Здесь обе головки получились на одной высоте; в общей формуле их высотные смещения учитываются отдельно.

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

Это и есть маленький геометрический шаблон, который дальше едет вдоль контактной линии. В каждом новом месте у него меняются положение и ориентация. Для этого и нужен следующий расчёт.

Как повернуть сечение вместе с путём

Контактная линия известна. Но для восстановления ходовых рельсов надо ответить на вопрос: в какую сторону от этой линии откладывать боковое смещение?

На прямом участке легко не заметить подвох: откладывай одно и то же смещение по глобальному X - и всё выглядит нормально. Но на повороте «поперёк пути» поворачивается вместе с путём. Значит, нам нужны оси, которые будут следовать за линией.

В каждой позиции строим ортонормированный базис (t, e₁, e₂): касательную и две поперечные оси. Чтобы оси не крутились вокруг касательной без причины, переносим их минимальным поворотом между соседними направлениями. Это базис Бишопа.

Пусть t₀, t₁ - предыдущая и новая единичные касательные:

$\begin{aligned}v&=t_0\times t_1,\qquad c=t_0\cdot t_1,\\R&=I+K+\frac{K^2}{1+c},\qquad Kx=v\times x.\end{aligned}$

K - кососимметричная матрица векторного произведения, I - единичная матрица. Такой поворот переводит t₀ в t₁. Им поворачиваем старую поперечную ось, затем убираем небольшую численную составляющую вдоль новой касательной и нормируем. Вторую ось получаем векторным произведением.

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

Наклон поперечного сечения задаём отдельно углом α:

$\begin{aligned}b&=\cos\alpha\cdot e_1+\sin\alpha\cdot e_2,\\n&=-\sin\alpha\cdot e_1+\cos\alpha\cdot e_2.\end{aligned}$

b - боковая ось сечения, n - перпендикулярная ей ось высоты в той же поперечной плоскости. Так мы разделяем изгиб самой линии и наклон расположения рельсов относительно переносимого базиса.

Записываем это через векторы

Ближние наблюдения дают пять параметров: наклон α, боковое расстояние d до ближайшей головки, две разности высот h₁, h₂ и расстояние g между головками.

Эти параметры тоже нужно измерить. Найденные ближние головки проецируем в поперечные плоскости сглаженной контактной линии. Из вектора между головками убираем составляющую вдоль касательной, выбираем знак по стороне контактного рельса и нормируем. Получаем боковую ось b, затем n = t × b.

Если r₁, r₂ - ближняя и дальняя головки, а c - контактная точка этого сечения, обратное вычисление параметров выглядит так:

$\begin{aligned}\alpha&=\operatorname{atan2}(b\cdot e_2,b\cdot e_1),\\d&=q(c-r_1)\cdot b,\qquad g=q(r_1-r_2)\cdot b,\\h_1&=(c-r_1)\cdot n,\qquad h_2=(c-r_2)\cdot n.\end{aligned}$

Знак q равен +1 для контактного рельса справа и −1 для рельса слева. Параметры считаются по ближним сечениям, после чего берётся покоординатная медиана. Тенденция изменения угла оценивается линейной подгонкой по продольным позициям.

Для точки контактной линии c:

$\begin{aligned}r_1&=c-qd\cdot b-h_1\cdot n,\\r_2&=c-q(d+g)\cdot b-h_2\cdot n.\end{aligned}$

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

Круги в поперечных сечениях - ходовые головки, квадраты - контактная опора. Все координаты здесь относятся к текущему лидару.

Для нашего кадра после приведения ближних наблюдений к сглаженной линии получились параметры:

Величина	Значение
Начальный наклон сечения α₀	0,349°
Боковое расстояние d	0,62194 м
Вертикальные смещения h₁ и h₂	0,27127 м
Расстояние между головками g	1,58306 м

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

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

Почему наклон нельзя продолжать бесконечной прямой

На ближнем участке оцениваем начальный наклон α₀ и тенденцию его изменения $v_{\alpha}$ . Постоянные боковые и вертикальные смещения дальше сохраняются, а изменение наклона постепенно затухает.

Сначала отсчитываем расстояние от конца ближнего участка:

$\begin{aligned}\ell&=\max(s-8,0),\\\eta&=\frac{\ell^3}{\ell^2+2^2},\quad \xi=\min\left(\frac\eta L,1\right),\quad L=50\text{ м},\\\alpha(s)&=\alpha_0+v_{\alpha}\cdot L\left(\xi-\xi^2+\frac{\xi^3}{3}\right).\end{aligned}$

В выражении для η расстояния заданы в метрах; 2² означает квадрат масштаба 2 м. ξ - безразмерный аргумент функции.

У границы ближнего участка η растёт как ℓ³. Поэтому значение наклона и его первые две производные плавно стыкуются с постоянной ближней оценкой.

Дальше вступает в работу кубическая функция f(ξ) = ξ − ξ² + ξ³/3. Её производная:

$f'(\xi)=(1-\xi)^2$

При ξ → 1 изменение затухает, а суммарная добавка ограничивается $v_{\alpha}L/3$ . Это не бесконечное продолжение ближней тенденции.

Например, для условного $v_{\alpha}=0{,}02^\circ/\text{м}$ предельная добавка равна примерно 0,333°. Линейное продолжение за 100 м добавило бы уже 2°.

Начальная тенденция не должна бесконечно накапливать поворот сечения за пределами ближних измерений.

6. Насколько сместятся рельсы, если мы ошиблись в угле

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

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

Соберём параметры сечения в вектор x = (α, d, h₁, g, h₂). В производных угол α измеряется в радианах. Для ближней головки производные положения:

$\begin{aligned}\frac{\partial r_1}{\partial\alpha}&=-qd\cdot n+h_1\cdot b,\\\frac{\partial r_1}{\partial d}&=-q\cdot b,\qquad \frac{\partial r_1}{\partial h_1}=-n,\\\frac{\partial r_1}{\partial g}&=0,\qquad \frac{\partial r_1}{\partial h_2}=0.\end{aligned}$

Для дальней:

$\begin{aligned}\frac{\partial r_2}{\partial\alpha}&=-q(d+g)\cdot n+h_2\cdot b,\\\frac{\partial r_2}{\partial d}&=\frac{\partial r_2}{\partial g}=-q\cdot b,\\\frac{\partial r_2}{\partial h_1}&=0,\qquad \frac{\partial r_2}{\partial h_2}=-n.\end{aligned}$

Столбцы этих производных составляют матрицу J. Через неё ковариация параметров Σₓ переносится в ковариацию положения головки:

$\Sigma_{\text{рельса}}\approx\Sigma_{\text{линии}}+J\Sigma_xJ^{\mathsf T}+\sigma_{\text{напр}}^2AA^{\mathsf T}$

Последнее слагаемое учитывает неопределённость направления. Если r - вектор от контактной линии до головки, то A = [b × r, n × r] содержит смещения от двух малых поворотов касательной. σ_напр - стандартное отклонение этих углов.

Возьмём расстояния из нашего сечения и ошибку угла сечения 0,1° ≈ 0,001745 рад. Только угловая составляющая даёт характерные линейные смещения:

$\begin{aligned}\delta_1&\approx\sqrt{0{,}622^2+0{,}271^2}\cdot0{,}001745\approx1{,}18\text{ мм},\\\delta_2&\approx\sqrt{2{,}205^2+0{,}271^2}\cdot0{,}001745\approx3{,}88\text{ мм}.\end{aligned}$

Одна ошибка угла, но у дальней головки эффект больше чем втрое. Остальные источники неопределённости в этом маленьком расчёте пока не учитываем.

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

На графике нашего кадра видны ступени около 30 и 60 м. Они связаны с принятыми в модели нижними границами неопределённости для разных диапазонов дальности. Сама траектория в этих местах остаётся гладкой.

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

7. Не разрешаем красивой кривой уехать в стену

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

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

Продолжение за наблюдаемым участком проверяем секциями примерно по 2 м в продольной координате датчика. Для каждой предполагаемой головки интерполируем положение и выбираем сырые точки в окрестности:

$\begin{aligned}\lvert v_i-v_{\text{рельса}}(s_i)\rvert&\le0{,}055\text{ м},\\\lvert z_i-z_{\text{рельса}}(s_i)\rvert&\le0{,}030\text{ м}.\end{aligned}$

В каждой секции для обеих головок одновременно нужны:

Условие	Что оно отсекает
Не меньше трёх точек	Единичные попадания
Не меньше трёх пространственных ячеек размером 3 см	Несколько почти совпадающих измерений
Продольный размах минимум 0,5 м	Маленькое пятно вместо протяжённой головки

Индекс ячейки для точки p = (x, y, z) считается покоординатно:

$k(p)=\left(\left\lfloor\frac{x}{0{,}03}\right\rfloor,\left\lfloor\frac{y}{0{,}03}\right\rfloor,\left\lfloor\frac{z}{0{,}03}\right\rfloor\right)$

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

Проверка останавливается на первой неудачной секции. Дальний хвост отрезается; через разрыв к следующему подходящему скоплению не перескакиваем.

Бирюзовым показан принятый путь, пунктиром - предложенное продолжение. Красная полоса обозначает первую дальнюю секцию, которая не получила поддержки.

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

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

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

В нашем эпизоде исходный путь заканчивается примерно на 109,33 м вперёд, а предложенное продолжение доходит до 129,31 м. Уже в первой секции, от 109,33 до 111,43 м, ни у одной предполагаемой головки не нашлось точек в допустимой полосе.

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

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

8. Строим габарит и проверяем каждую точку

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

Разбиваем путь на короткие сегменты. Пусть a и b - середины пар головок на соседних сечениях. Тогда направление сегмента и его длина:

$f=\frac{b-a}{\lVert b-a\rVert},\qquad L=\lVert b-a\rVert$

Вектор между правой и левой головками обозначим v_пары. Убираем из него продольную составляющую и нормируем:

$\begin{aligned}e&=\frac{v_{\text{пары}}-(v_{\text{пары}}\cdot f)f}{\lVert v_{\text{пары}}-(v_{\text{пары}}\cdot f)f\rVert},\\n&=f\times e.\end{aligned}$

Знак нормали n выбирается вверх.

Для проверяемой точки p получаем локальные координаты:

$\begin{aligned}\ell&=(p-a)\cdot f,\\u&=(p-a)\cdot e,\qquad h=(p-a)\cdot n.\end{aligned}$

Это три скалярных произведения. Первое отвечает на вопрос «находится ли точка между концами сегмента», остальные - «где она в его поперечном сечении».

В эксперименте габарит имеет ширину 2,1 м, высоту 3 м. С учётом внутренних отступов проверяем:

$\begin{aligned}0&\le\ell\le L,\\\lvert u\rvert&<1{,}05-0{,}03-\varepsilon_{\text{гр}},\\0{,}10+\varepsilon_{\text{гр}}&<h<3{,}00-0{,}03-\varepsilon_{\text{гр}}.\end{aligned}$

Снизу исключаем 10 см, по бокам и сверху - 3 см. Здесь ε_гр = 10⁻⁶ м - дополнительный численный запас на границе. Получаются точные пределы |u| < 1,019999 м и 0,100001 < h < 2,969999 м. На границе поперечного сечения попадание не засчитывается.

Срез в районе объекта. Красные точки прошли пространственную проверку попадания в сегменты габарита; здесь они показаны в общей поперечной проекции.

Для сегмента длиной 1 м:

Локальные координаты (ℓ, u, h), м	Решение
(0,4; 0,8; 1,2)	Внутри
(0,4; 1,03; 1,2)	За боковой границей
(0,4; 0,8; 0,06)	Ниже проверяемой области
(1,2; 0,8; 1,2)	За концом этого сегмента

Последняя точка ещё может попасть в соседний сегмент. Мы проверяем весь принятый путь, а не один прямоугольник.

Чтобы не сравнивать все точки со всеми сегментами, используется пространственная сетка с ячейками 0,5 м. Ограничивающие параллелепипеды подсказывают возможные пересечения, затем выполняются точные проверки в локальных координатах.

В свежем пакете нашего эпизода 521 978 точек из трёх последовательных облаков. Внутрь габарита попали 90 точек. Ближайшая из них находится в 96,956 м от лидара.

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

9. Собираем попадания и не считаем одно измерение дважды

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

Сначала разбиваем пространство на ячейки по 5 см. Каждую занятую ячейку представляем средним положением её точек. Соединяем такие представители, если расстояние между ними не больше 24 см, и находим связные компоненты. Затем возвращаем каждой компоненте её исходные точки. Для принятия нужны минимум три точки и две занятые ячейки.

Дальше учитываем дальность каждой точки r от лидара:

$\begin{aligned}w(r)&=\min\left(1,\left(\frac{70}{r}\right)^2\right),\\W_{\text{объекта}}&=\sum_iw(r_i),\qquad W_{\text{объекта}}\ge3.\end{aligned}$

До 70 м точка даёт единицу. На 100 м - 0,49. Три точки на этой дальности дадут только 1,47; шесть - 2,94, всё ещё ниже порога. Семь дадут 3,43 и пройдут именно эту проверку.

Одинаковое количество близких и далёких попаданий даёт разную суммарную поддержку.

На этой дальности квадрат ещё не собирается в одну компоненту. Между соседними столбцами отражений больше 24 см, поэтому получаются шесть групп по 15 точек. У каждой сумма весов около 7,815, то есть каждая отдельно проходит порог 3. Общая сумма весов всех попаданий - 46,890, но порог проверяется именно для каждой компоненты.

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

Здесь легко случайно обмануть самого себя. Мы уже накапливали старые облака ради геометрии рельсов. Если старые точки препятствия снова подать как новые, одна и та же горстка отражений начнёт многократно подтверждать существование объекта.

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

Через 0,30 с в другом, непересекающемся пакете снова получаем 90 попаданий, ближайшее на 96,487 м. Трекер сопоставляет шесть групп с предыдущими наблюдениями и подтверждает каждую. Так большой квадрат обнаруживается ещё до того, как отражения от него успели собраться в одну сплошную компоненту.

При дальнейшем сближении отражений становится больше: на 74,98 м в одну компоненту собираются 324 точки, на 61,60 м - уже 495.

Четыре выбранных момента сближения с одним объектом. Красным выделены точки обнаруженных компонент; масштаб всех панелей одинаковый.

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

Провод сверху: сначала одна точка, потом объект

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

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

В центральной части сечения видна цепочка отражений, которая тянется вниз от потолка.

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

Через 0,40 с, на расстоянии около 21,87 м, нижний конец провода даёт 6 связанных точек. Ещё два одиночных попадания в габарит не образуют компонент. Шесть точек провода проходят порог по сумме весов:

$W=6\cdot1=6\ge3$

Большая часть провода выше проверяемой области. Красным выделены шесть точек нижнего конца, прошедшие геометрическую проверку.

Ещё через 0,40 с новый пакет даёт 9 точек на расстоянии 15,19 м. Трекер связывает его с предыдущим и подтверждает объект. В следующем показанном пакете, примерно в 8,54 м, точек уже 23.

Золотистая точка ещё не образует объект. Красные точки в следующих пакетах уже входят в выделенные компоненты; пакеты не пересекаются по исходным облакам.

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

Эти два эпизода можно рассмотреть в 3D: повращать облако, отключить окружение и отдельно посмотреть точки рельсов, контактного рельса и препятствия. Восстановленные линии пути включаются отдельно от измеренных точек.

От облака до подтверждённого объекта

В начале у нас были точки тоннеля и неизвестная область движения поезда. В выбранном эпизоде цепочка получилась такой:

Этап	Результат
Ближняя пара	Две поддержанные головки
Контактный профиль	Правая сторона, 1233 исходных отражения
Продолжение	21 опора, 8742 поддерживающих измерения
Сглаживание	Пространственная кривая для переноса сечений
Восстановление	Две ходовые линии с оценкой неопределённости
Проверка продолжения	Конец принятого пути на 109,33 м вперёд
Проверка габарита	90 точек внутри, шесть компонент
Новое наблюдение через 0,30 с	Объект подтверждён трекером

Теперь посмотрим на тот же эпизод в движении. Слева виден весь путь, справа - поперечное сечение у найденного объекта. Первое обнаружение появляется в 96,96 м, а через 0,30 с его подтверждает новый пакет измерений.

Красные точки - обнаруженные компоненты. Воспроизведение замедляется на первом обнаружении и подтверждении; время исходной записи показано справа вверху.

В исходном хакатонном прогоне были обнаружены все шесть препятствий из визуальной разметки испытательной записи.

Что получилось

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

Вся цепочка работает на CPU. Python связывает этапы, вычислительные участки вынесены в C++ через pybind11, для параллельного перебора используется OpenMP. Есть загрузка записи и отдельный вход ROS 2 для потока облаков. Интерфейс позволяет открыть конкретное обнаружение и посмотреть, какие точки его образовали.

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

Так задача «найти что-нибудь постороннее в тоннеле» превращается в последовательность геометрических вопросов. Сначала - где рельсы. Затем - где пройдёт поезд. И только после этого - что ему мешает.

Источник: https://habr.com/ru/articles/1098160/

Вернуться к списку

Интересные статьи

Для здоровья полезно заниматься разнообразно

Совсем свежее исследование, январь 2026. https://pubmed.ncbi.nlm.nih.gov/41574252/ и Physical activity types, variety, and mortality: results from two prospective cohort studies.70 725 женщин и 40 74...

Пора двигаться быстро. Даже если ты на Битриксе (часть 3)

Привет, хабровчане! На связи Алиса — тимлид в e-commerce агентстве KISLOROD.Кто о чем, а я продолжаю рассказывать, как сшипперить Bitrix и Laravel. В первой части я рассказывала, как по...

Интеграция онлайн-кассы Атол с Битрикс24: облачное решение на .NET

В современном бизнесе интеграция фискальных регистраторов с CRM-системами перестала быть опцией — это необходимость для эффективного ведения дел. Однако существующие решения часто требуют дорогостояще...

Битрикс. Оптимизация сервиса, который помогает устранять опасности

Представьте: горячий металл, мощные машины, сотни работников — наше производство постоянно подвержено различным рискам. И как мы превратили эти вызовы в возможности? Этот рассказ будет о том, как мы в...

Personal (jesus) стенд — решаем проблему тестовых контуров в компании

Всем привет, меня зовут Захаров Антон, и я DevOps-инженер в компании Bimeister! Весь свой опыт я получил в этой компании, за 5 лет прошел путь от эникея до того, кем я работаю сейчас (большое сп...