Данный способ синхронизации подходит не только для камеры Intel RealSense D455, но также и других камер серии D435/i, D457 и, вероятно, будущей D585 Pro. Более того, из-за известного дефекта D455 у цветной камеры искажающего цвета собирать систему с ней не рекомендуется — этот дефект может быть критичен для нейросетей. По сути, её можно использовать только как монохромную камеру.
Аппаратная синхронизация камеры и лидара позволяет совместить облака точек не только геометрически (чем занимается калибровка), но и по времени их кадров/сканов, что, в свою очередь, даёт возможность объединять их в единую воксельную карту занятости. Рассинхронизация временных меток камеры и лидара будет создавать за движущимися объектами тянущийся след из вокселей, а за неподвижными — при эго-движении самого аппарата.
В интернете есть готовые девайсы для синхронизации, другой вариант, однако они кажутся мне избыточными. Хочется всё сделать внутри своей прошивки на одной плате STM32, которая уже управляет моторами и многим другим, чтобы она не зависала. Синхронизация будет работать независимо от ядра, используя только периферийные устройства МК — таймеры.
К тому же у камер RealSense есть своя специфика работы, но об этом далее.
Я приведу пример синхронизации на своём кастомном драйвере лидара, который работает следующим образом.
Почему 30 Гц?
Камера RealSense отправляет кадры с частотой 30 Гц так, что на один скан лидара (10 Гц) приходится 3 кадра от камеры, но нужен только один кадр из этих трёх. Почему? И почему не запустить камеру и лидар сразу на 10 Гц?
Причина в прошивке камеры, которая ограничивает доступные частоты значениями 5, 15, 30, 60 и 90 Гц. Более того, эти частоты имеют собственные временные сдвиги: 30 Гц камеры в реальности — это 30.015 Гц из-за особенностей работы внутренних часов. Если генератор частоты будет выдавать ровно 30 Гц, начнутся дропы кадров. Поэтому Intel настоятельно рекомендует при мультикамерной синхронизации использовать в качестве генератора частоты для ведомых камер одну из камер, то есть сделать одну камеру ведущей. Из всех доступных режимов наиболее оптимальным и кратным 10 Гц лидара является именно 30 Гц.
Проблема объединения облаков точек
Хорошо, почему просто не запустить два потока и не добавлять облака точек на воксельную карту без синхронизации, по приходу кадров? Если разложить этот процесс объединения облаков точек по времени, получится следующий временной конвейер:
Камера в 0 мс подаёт сигнал о начале создания нового кадра. Он отправляется на счётчик делителя частоты (STM32), и вместе с этим сигналом начинается экспозиция камеры (накопление заряда на CMOS-матрице). Лидар с помощью программных вычислений в нашем драйвере над временными метками начинает от этого момента отсчитывать первый скан своего облака. Поскольку время экспозиции обычно меньше периода обновления между кадрами и варьируется в пределах 1–10 мс, оно укладывается в 33.3 мс временного интервала.
Если мы представим поле зрения (FOV) камеры и лидара наложенными друг на друга, то увидим, что камера с глобальным затвором сканирует свой горизонтальный сектор 90° по курсу «мгновенно», а лидар — постепенно, по мере вращения шпинделя с отражающими зеркалами. При этом лидар имеет FOV в 360° и случайный азимут (к чему мы ещё вернёмся). Для простоты представим, что шпиндель всегда начинает сканирование с левой границы FOV камеры, пока не совершит полный оборот и не вернётся в ту же точку — это и будет наш скан.
Время полного оборота лидара — 100 мс, значит, за 25 мс он пройдёт область FOV камеры и начнёт оцифровывать другие стороны света.
И так как мы решили отправлять кадры лидара и камеры в карту по готовности, то первый кадр идеально совпадает с лидаром по времени: 0 + 10 мс = 10 мс экспозиции — и камера делает кадр, а лидар чуть позже, за 25 мс. Отставание по времени составляет всего 15 мс. Много это или мало? Это минимум, которого можно достичь.
А вот дальше начинаются проблемы. Второй кадр начинает генерироваться с 33.3 мс + время экспозиции (10 мс) = 43.3 мс. Камера и лидар начали сканировать не одновременно — лидар этот сектор уже прошёл. Рассогласование составляет уже 18.3 мс. Если аппарат за это время сместился не сильно, кадр практически полностью перезапишет собой первый на карте. Далее начинает создаваться третий кадр: он начнёт генерироваться в 66.6 мс + 10 мс = 76.6 мс, рассогласование — 51.6 мс, и он перезаписывает собой второй кадр. Через ещё 33.3 мс начинает генерироваться четвёртый кадр, а первый лидарный скан из точек полностью собран и отправлен на воксельную карту, где он объединяется по сути с самым рассогласованным третьим кадром. Мы сразу увидим на экране шлейф за движущимися объектами из кубов, который будет в 3.5 раза больше, чем если бы мы использовали только первый кадр.
Но это ещё не всё. До прихода следующего второго скана у нас ещё целых три кадра впереди — 4, 5 и 6 — и они будут добавлены на карту к первому скану, с каждым разом увеличивая рассогласование, пока второй скан не сбросит эту ошибку. Рассинхрон будет варьироваться от 51.6 мс до 103.2 мс в момент прихода шестого кадра. По этой причине объединять облака точек на воксельной карте по приходу нельзя, и нужна синхронизация, чтобы время рассинхрона между кадрами было:
-
В идеале всегда фиксированным;
-
Стремилось к минимуму.
Случайный азимут лидара
В реальности есть ещё одна проблема: шпиндель лидара обладает случайным азимутом. Что это значит на практике? Это значит, что относительно системы отсчёта координат лидара (условной точки 0) облако точек начинает строиться в случайном произвольном месте и в нём же заканчивает строиться. Связано это с физическими ограничениями моторов: они не идеально синхронны и не могут поддерживать абсолютную точность оборотов. В реальном двигателе обороты всегда плавают, плюс есть дрейф кварцевых часов. Всё это приводит к тому, что после включения точка начала сканирования кадров постоянно плывёт. Начинается сканирование с того места, где мотор остановился в прошлый раз, плюс все физические воздействия, которые привели к смещению этой точки. В общем, точка начала построения каждого нового скана действительно всегда случайна. И по этой причине просто взять первый кадр нельзя.
Разобьём возможные ситуации на три наиболее вероятных случая (остальное — их вариации), чтобы представить картину наложения FOV лидара и горизонтального FOV камеры друг на друга:
-
Начало первого скана совпадает с началом FOV камеры (если ротор крутится по часовой стрелке — это левый угол камеры). В таком случае нам нужен первый кадр, так как он идеально совпадает по времени со сканом лидара: 10 мс от камеры и 25 мс от лидара — это +15 мс рассогласования.
-
Начало первого скана в конце FOV камеры. Это значит, что лидар придёт в конец лишь в последней четверти своего полного оборота на 360°. В таком случае третий кадр, начинающийся в 66.6 мс + 10 мс = 76.6 мс, наиболее точно совпадает со сканом лидара. ¾ пути вращения шпинделя займёт 75 мс, после чего оставшиеся 25 мс он будет сканировать эту часть света. Рассогласование составит 23.4 мс.
-
Начало первого скана ровно на середине FOV камеры. Это значит, что лидар режет кадр пополам. Первую половину он пройдёт почти с идеальной синхронизацией (12.5 мс против 10 мс камеры), а вот вторая половина кадра придёт с задержкой 62.5 мс.
При этом лидар специально не сообщает положение шпинделя. Возможно, его положение можно вычислить из угловых координат каждой новой пришедшей точки, но я пока решил не прибегать к такому сложному способу. Для меня главное, чтобы рассинхрон не уплывал и был стабильным. У идеальной синхронизации есть и другая сторона: она будет продолжением скана лидара, но не добавит на карту новые промежуточные данные. К тому же воксельные карты имеют большой порог округления (у меня — 10 см), и рассогласование в пределах этой величины не будет заметно, так как обе точки в памяти компьютера станут одним вокселем. А вот на величины от 20 см уже появится след из одного кубика и т.д. То есть некоторые допуски в зависимости от точности карты имеются, и небольшие погрешности прощаются.
Выбор в пользу второго кадра
Исходя из вышеизложенного, самым простым и логичным решением я считаю использовать средний, второй, кадр от камеры. Во всех трёх рассмотренных случаях начала случайного сканирования первого скана лидара его рассогласование не может превысить половину периода лидара, то есть ~50 мс.
При этом, чтобы попасть точно в середину экспозиции второго кадра, мы сдвигаем время начала первого скана от метки синхронизации (сброса таймера) на 11.5 мс. Тогда:
-
Начало второго кадра: 33.3 + 11.5 = 44.8 мс
-
Конец второго кадра: 44.8 + 10 = 54.8 мс
-
Середина экспозиции: 44.8 + 5 = 49.8 мс ≈ 50 мс
Случай, когда скан начинается в конце FOV камеры, отметается, так как он не попадёт в этот скан по времени.
-
Максимальная задержка = 49.8 мс
-
Минимальная задержка = 11.7 мс
-
Средняя задержка ≈ 25–30 мс
Таким образом, в худшем случае (когда шпиндель начинает сканирование с правой границы FOV камеры) задержка составляет ~50 мс, что является физическим пределом для половины периода лидара.
Железо
Чтобы объединить камеру и лидар, первое, что понадобится, — это корпус. Мной были протестированы два корпуса, простой:
И с блендой камеры от солнца:
Последний немного лучше, так как защищает камеру от бликов солнца и имеет более сильный угол наклона к земле. Это в дальнейшем при построении карты позволяет значительно снизить уровень помех глубины практически до уровня, что есть в помещениях, так как перед камерой, направленной в землю, всегда есть текстура земли. Это очень важно для алгоритмов построения глубины, таких как Semi-Global Matching (SGM). Направлять камеры смотреть в горизонт (бесконечность) не рекомендуется, потому что на дальности видимости 40 м текстуры не найдётся — алгоритм будет искать случайные совпадения светящихся точек в воздухе, что вызовет помехи, преодолевающие почти любые фильтры постобработки. В общем, перед стереокамерой, использующей алгоритмы (а не нейросети), на максимальной дальности всегда должна быть текстура — либо стены (как в помещении), либо пол/земля (на улице).
Также этот корпус позволяет закрыть хотя бы сверху разъём мультикамерной синхронизации:
Таким образом мы разделяем сферы ответственности: камера преимущественно оцифровывает поверхности в ближнем радиусе по курсу, а лидар — на средних и дальних дистанциях. Зона пересечения всего 7° остаётся только для калибровки относительно друг друга и чтобы цветная камера могла видеть, например, Aruco-маркер прямо перед собой. Кроме того, лидар как наиболее точный прибор может при добавлении новых облаков точек очищать облака камеры от ложных помех (например, от пролетающей пыли) своими лучами, либо наоборот — в зоне пересечения:
Пример построения карты проходимости со старым корпусом без синхронизации на одной RGB-D камере:
Разъём синхронизации
Разъём мультикамерной синхронизации использует специфический коннектор SHR-09V-S, который с трудом находится на Aliexpress.
В нём нам надо оставить всего два провода: 5-й (вход/выход GPIO) и 9-й (земля), и соединить их (в моём случае) с обычным коаксиальным кабелем с волновым сопротивлением 50 Ом. Либо можно использовать двухжильный экранированный кабель, чтобы задействовать ещё и 8-й пин опорного напряжения, что понадобится в дальнейшем.
Настройки камеры
Чтобы запустить генерацию сигнала 30 Гц на камере, можно использовать стандартное приложение realsense-viewer со следующими настройками:
Output Trigger: Enable
Inter Cam Sync Mode: 1
Ну и соответственно запустить поток:
Конвертер логических уровней
Особенность этого вывода — уровень логической единицы 1.8 В, что несовместимо с большинством микроконтроллеров. Поэтому необходимо соорудить конвертер логических уровней.
Мой первый вариант самодельного конвертера оказался неудачным, вероятно, из-за криворукости и желания сэкономить. Я решил просто взять биполярный транзистор KSP2907A (высокочастотный, как делают многие) и через резистор 6.5 кОм завести на базу сигнал от 5-го пина камеры:
В результате фронты сигнала были завалены и вообще он напоминал волну:
Также в такой схеме на базе транзистора оказалось напряжение 5 В, которое он коммутировал, но благодаря резистору 6.5 кОм ток был не столь велик, чтобы сжечь пин камеры. В общем, от этой схемы я отказался и решил делать на китайском конвертере логических уровней:
Для его работы на входы LV и HV также требуется опорное напряжение логического уровня. От МК его взять довольно просто — на плате разведено много пинов, а вот 1.8 В из 8-го пина разъёма камеры брать уже неудобно, поэтому я взял понижающий преобразователь:
На снимке не верное подключение разъёма и коаксиального провода к конвертеру логических уровней!
После чего собрал уже более мудрёную но рабочую схему:
*Проводки на фото укреплены силиконом что-бы не отвались от вибраций и перегибов *
Здесь на LV1 идёт вход с центральной жилы коаксиального кабеля (5-й пин камеры), затем на HV1 по белому проводу через разъём он уходит на вход счётчика в МК. Экран и чёрный провод припаяны к земле (GND). Питание от понижающего преобразователя по фиолетовому проводу заходит на вход LV (1.8 В), а 3.3 В на HV подаётся по оранжевому проводу от МК.
С конвертером фронты стали заметно лучше:
Настройка STM32CubeIDE
Теперь необходимо провести настройку среды STM32CubeIDE. Для конвертации сигнала 30 Гц в сигнал 1 Гц (PPS) придётся использовать два таймера. Первый, в режиме внешнего тактирования, должен принимать сигнал 30 Гц и по достижении 30 отсчётов сбрасываться, подавая сигнал второму таймеру. Второй таймер начинает отсчёт по сигналам шины МК APB2 (240 МГц) и по достижении 12000 тактов (что примерно равно длительности 50 мкс, необходимых лидару для считывания импульса как логической единицы) он сбрасывается и ждёт нового сигнала начала отсчёта. Такой способ независим от ядра МК и работает как полностью аппаратный делитель частоты, что очень хорошо, так как защищает генерацию сигналов от зависаний ядра в очереди прерываний.
Настройка в STM32CubeMX (CubeIDE)
Откройте свой .ioc-файл, переходим в Timers:
TIM1
Clock Source: External Clock Source Mode 2 (ETR2)Channel1..6: отключеныTrigger Output (TRGO): Update EventCounter Settings: Prescaler: 0 Counter Mode: Up Counter Period: 29 Auto-reload preload: EnableNVIC Settings: TIM1 update interrupt: true, 0, 0GPIO: PA12 станет TIM1_ETR (AF1) — вход импульсов камеры
TIM8
Slave Mode: Trigger ModeTrigger Source: ITR0Clock Source: Internal ClockChannel4: PWM Generation CH4 (это PC9) — можно и на любом другом каналеOne Pulse Mode: EnabledCounter Settings: Prescaler: 0 Counter Mode: Up Counter Period: 23999 (полный период) Auto-reload preload: EnablePWM Settings (CH4): Mode: PWM mode 2 Pulse: 11999 (чтобы половина периода была высоким уровнем) Polarity: HighGPIO: PC9 в AF3 (TIM8_CH4) — выход 1PPS
Настройки таймера 1:
Настройки таймера 8:
Можно ещё подтянуть пин к земле:
В область инициализации нужно добавить стандартные HAL-функции:
// 1. Запускаем ведомый TIM8 на выдачу импульса 50 мкс на ножке PC9 HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4); HAL_TIM_Base_Start_IT(&htim1); // Запускаем сам счетчик TIM1
Промежуточный результат если вывести на экран импульс таймера 1:
Как можно заметить по ручкам настройки осциллографа, его период составляет примерно 33.3 мс, потому что он тактируется от генератора сигнала на камере, что значительно больше допустимых для лидара 1–200 мкс. Именно поэтому понадобился второй таймер.
Итоговый импульс 1PPS выглядит вот так:
Итак, у нас получился генератор 1PPS. Для многих лидаров этого уже достаточно — в момент прихода импульса внутренние часы лидара должны сбрасываться. Но не у китайского Livox Mid-360: в штатном Livox Viewer по-прежнему будет No Sync:
NMEA-пакет GPRMC
Причина в том, что лидар также требует отправки текстового пакета со временем в формате GPRMC. По идее, это должно быть время с атомных часов на спутнике, с которым он должен сверяться и вносить поправки, если его часы убегают или отстают. Но в нашем случае мы синхронизируем его не с мировым временем, а конкретно с камерой RealSense, поэтому сделаем просто фейковый пакет. Для его отправки заведём USART (в моём случае второй не занят):
Стандартные настройки:
Mode: AsynchronousBaud Rate: 9600 бодWord Length: 8Parity: NoneStop Bits: 1
DMA Settings: Add → USART2_TX, все стандартные настройки подходят.
После чего добавим код генерации фейковой строки. Используем стандартный HAL-колбэк для прерываний таймера:
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){// measure velocity, position if(htim == &htim4) update_encoder(); if(htim == &htim1) update_lidar_gprmc();} // Loop 1 Hzvoid update_lidar_gprmc(void){ static uint32_t fake_time = 120000; static char packet[80]; fake_time++; if (fake_time % 100 >= 60) fake_time += 40; if ((fake_time / 100) % 100 >= 60) fake_time += 4000; // Собираем строку с $, телом и местом под чек-сумму int len = sprintf(packet, "$GPRMC,%06lu.00,A,0000.0000,N,00000.0000,E,0.0,0.0,060826,,,A", fake_time); // Считаем XOR по содержимому после $ uint8_t checksum = 0; for (int i = 1; i < len; i++) checksum ^= packet[i]; // Дописываем чек-сумму и \r\n len += sprintf(packet + len, "*%02X\r\n", checksum); // Одним вызовом HAL_UART_Transmit_DMA(&huart2, (uint8_t*)packet, len);}
После этого можно подключаться к лидару. У меня получилась такая схема, где жёлтый (8-й пин PPS) и белый (10-й пин GPS input) соединены по цветам. Номера можно увидеть прямо на разъёме M12 и прозвонить с проводами:
И мы должны получить от Livox Viewer вывод GPS Sync:
На этом аппаратная часть синхронизации лидара и камеры завершена. Дальнейшие настройки необходимо делать в Livox SDK2 или ROS.
ссылка на оригинал статьи https://habr.com/ru/articles/1068274/