Способ определения траектории объекта в окружающем пространстве для построения 3D карты Российский патент 2024 года по МПК G01C21/34 G01S19/00 

ОБЛАСТЬ ТЕХНИКИ

Заявленное техническое решение в общем относится к области вычислительной техники, применяемой в области машинного (компьютерного) зрения, анализа изображений, а в частности к способу определения траектории объекта в окружающем пространстве для построения 3D карты.

УРОВЕНЬ ТЕХНИКИ

В настоящее время активно развивается подход с управлением беспилотным транспортом на основе высокоточного GNSS (Global Navigation Satellite System - Глобальная навигационная спутниковая система). Данный подход дает достаточно высокую (сантиметровую) точность, но только при наличии хорошего сигнала со спутников. В условиях городской застройки, гористой местности или плотных насаждений качество GNSS сигнала может падать. Для компенсации кратковременных потерь точности GNSS сигнала применяются инерциальные датчики, позволяющие сохранять знание о траектории движения на коротком промежутке времени. Но в случае долговременных (десятки секунд, минуты) проблем с точностью GNSS инерциальные датчики уже не справляются.

Существуют подходы по повышению точностей в подобных условиях, но они требуют дополнительного оборудования (ретрансляторы).

С другой стороны, активно развивается направление реконструкции и определения траектории на основе SLAM (simultaneous localization and mapping - одновременная локализация и построение карты) подхода по данным с камер. Но данный подход имеет существенные недостатки в виде накапливающейся ошибки позиционирования и ориентации, которые ведут к ошибкам в определении траектории. Для минимизации данных ошибок в данный момент исследуются решения по добавлению данных от GNSS и от IMU (inertial measurement unit - инерциальный измерительный блок).

В то же время существует ряд задач в автоматизации строительных, горнорудных работ, которые требуют высокоточного позиционирования транспортных средств или ее частей (ковш, нож и т.п.) относительно поверхностей и объектов в окружающем пространстве, равно как и непрерывного обновления трехмерной модели строительной или горнорудной площадки, с целью оценки актуального состояния работ. Эти задачи возможно решать на основе SLAM подхода по данным с камер; однако, как указано выше, данный подход имеет ограничения по точности.

Из уровня техники известен патент RU 2684994 C1 «СИСТЕМА И СПОСОБ НАВИГАЦИИ НАЗЕМНОГО ТРАНСПОРТНОГО СРЕДСТВА» (патентообладатель БЭЙЦЗИН ЮНИСТРОНГ САЙЕНС ЭНД ТЕКНОЛОДЖИ КО., ЛТД., опубл. 16.04.2019).

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

Из уровня техники известна международная заявка WO 2022245246 A1 «Способ комплексирования навигационных данных для позиционирования транспортного средства» (патентообладатель OBSHCHESTVO S OGRANICHENNOI OTVETSTVENNOSTIU EVOKARGO, опубл. 24.11.2022).

В данном решении описан способ комплексирования навигационной информации включает следующие этапы: данные о скорости АТС получают от колесных энкодеров АТС и GNSS приемника, данные о угле поворота АТС вокруг вертикальной оси получают от энкодера на рулевой рейке АТС, и ИНС, производят конвертацию полученных данных от указанных бортовых датчиков, определяют скорость АТС как сумму скоростей от колесных энкодеров и GNSS приемника с учетом весового коэффициента. Определяют расчетный угол поворота АТС вокруг вертикальной оси по углу рулевой рейки с учетом скорости АТС, определенной на основе колесных энкодеров и GNSS приемника, определяют относительные повороты АТС вокруг вертикальной оси по показаниям ИНС и энкодера рулевой рейки, определяют угол поворота АТС вокруг вертикальной оси с учетом весового коэффициента, полученные данные о угле поворота АТС вокруг вертикальной оси и скорости АТС комплексируют с помощью кинематической модели АТС, в результате чего определяют координаты и курс АТС.

Из уровня техники также известен патент RU 2789923 C2 «СПОСОБЫ И СИСТЕМЫ ДЛЯ СИНХРОНИЗАЦИИ ДАТЧИКОВ БЕСПИЛОТНЫХ ТРАНСПОРТНЫХ СРЕДСТВ (SDV) ОНЛАЙН» (патентообладатель Общество с ограниченной ответственностью "Яндекс Беспилотные Технологии", опубл. 14.02.2023).

В данном решении описан способ и система для синхронизации датчиков, используемых беспилотным транспортным средством (SDV) для осуществления навигации, в частности к синхронизации лидара (LIDAR) и камеры. Способ для синхронизации данных для работы беспилотного транспортного средства содержит: побуждение, посредством электронного устройства, камерной системы и лидарной системы обеспечивать данные изображений и картографические данные в электронное устройство в общем начале отсчета времени, определение, посредством электронного устройства, первой временной метки для картографических данных, второй временной метки для данных изображений и временного смещения между первой временной меткой и второй временной меткой. Временное смещение используется для инициирования задержки между последующими моментальными снимками, сформированными посредством системы камер.

Из уровня техники известна международная заявка WO 2019079511 А1 «Lidar and camera synchronization)) (патентообладатель DEEPMAP INC, опубл. 25.04.2019).

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

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

Кроме того, существует потребность в реализации решения для точного отслеживания траектории и построении 3D карт, которое не зависит от окружающей среды и может надежно обеспечивать оценку движения и положения практически в реальном времени. Это связано с тем, что различные датчики могут не работать при различных условиях (например, GNSS в тоннеле, или в условиях городской застройки, или камеры в темноте, или осадках).

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

В зависимости от качества (а также стоимости и размера) IMU эти ошибки могут быть довольно большими по сравнению с фактическими регистрируемыми измерениями. Навигация в трехмерном пространстве с помощью IMU фактически представляет собой суммирование (или интеграцию) сотен (или тысяч) отсчетов в секунду, в течение которых также накапливаются ошибки. Это означает, что неисправленная система ИНС будет смещаться относительно истинного положения сразу же при отсутствии внешних данных.

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

СУЩНОСТЬ ИЗОБРЕТЕНИЯ

В заявленном техническом решении предлагается новый подход к определению траектории объекта в окружающем пространстве для построения 3D карты.

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

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

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

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

- считывают одновременно посредством вычислителя данные с инерциального датчика (IMU), данные о положении объекта с GNSS приемника и синхронизированные кадры со стереокамеры, состоящей из левой и правой камер;

- обрабатывают посредством вычислителя полученные данные для вычисления показаний одометрии и получения 3D модели окружающего пространства, для чего одновременно учитываются показания инерциального датчика (IMU), GNSS приемника, стереокамеры, при этом во время обработки осуществляют:

• обработку последовательности кадров левой и правой камеры, полученных в текущий момент времени (t1) и в предыдущий момент времени (t0), синхронизированных по времени съемки;

• осуществляют автоматический поиск на каждом полученном кадре особых областей, и получают для этих областей дескрипторы и координаты с помощью эвристического алгоритма ORB;

• выполняют сопоставление полученных дескрипторов, как набор пар наиболее похожих между собой дескрипторов на всех полученных с левой и правой камер кадрах в моменты времени t0 и t1;

• вычисляют позу Р0 стереокамеры и 3D координаты точки в пространстве, соответствующей каждой паре дескрипторов в сопоставлении по паре кадров, полученных в момент времени t0 по сопоставлению при помощи триангуляции, используя калибровочные данные;

• вычисляют позу Р1 стереокамеры и 3D координаты точки в пространстве, соответствующей каждой паре дескрипторов в сопоставлении по паре кадров, полученных в момент времени t1 по сопоставлению при помощи триангуляции, используя калибровочные данные;

• для вычисленных поз Р0 и Р1 вычисляют ошибки триангуляции еР0, еР1 как среднеквадратическое отклонение координат проекций найденных 3D точек от координат их дескрипторов на левом и правом кадрах;

• берут замеры с датчика IMU по ускорению перемещения с акселерометра, и угловому ускорению вращения с гироскопа с момента времени t0 до момента времени t1, для каждого замера берут время между замерами, умножают его на ускорение вращения и получают изменение скорости вращения для данного замера;

• берут для каждого замера время между замерами, умножают его на ускорение перемещения и получают изменение скорости перемещения для данного замера;

• полагая скорость равной нулю в момент времени t0, вычисляют скорость в каждый замер инкрементально складывая изменения скорости до момента времени t1;

• вычисляют перемещения между всеми замерами, как произведение скорости на время между замерами;

• складывают все перемещения для получения сдвига dPimu1;

• берут для каждого замера время между замерами, умножают его на ускорение вращения и получают изменение скорости вращения для данного замера;

• полагая скорость равной нулю в момент времени t0, вычисляют вращения между всеми замерами, как произведение скорости на время между замерами;

• складывают все вращения для получения изменения ориентации oPimu1;

• объединяют dPimu1 и oPimu1, получая изменение позы в пространстве pPimu1 для датчика IMU;

• получают с GNSS приемника 3D координаты С0, С1 для моментов времени t0, t1 и ошибки вычисления координат еС0, еС1;

• осуществляют построение позы fP0_0 и fP1_0 как позы Р0, Р1 и вычисляют функцию потери F(fP0_0, fP1_0, Р0, P1, pPimu1, С0, С1, еР0, eP1, eImu, еС0, eC1)=Y0;

• находят позы fP0, fP1 которые минимизируют функцию потерь F с помощью метода градиентного спуска, используя начальное приближение fP0_0 и fP1_0;

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

для каждой полученной позы fPi берут каждый пиксель первого изображения и ищут его на втором изображении по максимальной похожести окружающей области этого пикселя;

получают 3D координаты точки в пространстве, которая соответствует сопоставленным пикселям на обоих кадрах;

полученный набор 3D координат является плотным облаком Mi, соответствующим позе fPi; - составляют 3D карту объединяя множества облаков Mi, полученных при вычислении траектории.

В частном варианте реализации описываемого решения, данные GNSS приемника представляют собой метки времени и координаты токи в пространстве, которую занимает датчик в момент опроса.

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

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

КРАТКОЕ ОПИСАНИЕ ЧЕРТЕЖЕЙ

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

Фиг. 1 - иллюстрирует конструктивные элементы устройства;

Фиг. 2 - иллюстрирует диаграмму потока данных.

ДЕТАЛЬНОЕ ОПИСАНИЕ ИЗОБРЕТЕНИЯ

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

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

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

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

• Плотная городская застройка может приводить к ухудшению точности оценки расположения с помощью GNSS приемника, однако для визуального SLAM (англ. simultaneous localization and mapping - одновременная локализация и построение карты) такие условия (особенно - в случае самопересекающихся и (или) многократно проходящих по некоторым участкам траектории) приводят к меньшей ошибке абсолютного позиционирования. В этой ситуации GNSS приемник играет роль абсолютной привязки, без требования к большой точности единичной оценки расположения;

• Движение по открытой местности с малым количеством визуально-разнообразных особенностей, по траекториям не имеющим самопересечений и (или) повторных прохождений по участкам траектории, приводят к уменьшению точности абсолютного позиционирования при использовании визуального SLAM; однако в подобном применении точность позиционирования по GNSS приемнику максимальна. В этой ситуации точность определения расположения во многом определяется GNSS приемников, а использование визуального SLAM сводится к оценке ориентации;

• В случае движения по объектам, с отсутствующим сигналом GNSS приемника, не обладающим визуально-разнообразными особенностями (например тоннели и другие инженерные сооружения) позиционирование как по GNSS приемнику, так и по SLAM затруднено. В этой ситуации использование инерциального датчика (IMU) позволит продолжить позиционирование.

Для работы заявленного решения используются такие технические средства (см. фиг. 1), как:

- инерциальный датчик IMU (3);

- стереокамера, состоящая из левой (1) и правой камер (2);

- GNSS приемник (4);

- вычислитель (5).

Входными данными являются (см. фиг. 2):

- данные с инерциального датчика IMU (3);

- данные со стереокамеры (1)/(2);

- данные с GNSS приемника (4).

Выходными данными являются (см. фиг. 2):

- траектория движения;

- 3D карта.

Ниже представлен общий процесс выполнения заявленного способа определения траектории объекта в окружающем пространстве для построения 3D карты. На этапе (101) выполняется подключение стереокамеры (1)/(2), инерциального датчика IMU (3) и приемника GNSS (4) к вычислителю (5).

После чего на этапе (102) считывают одновременно посредством вычислителя (5) данные с инерциального датчика (IMU), данные о положении объекта с GNSS приемника (4) и синхронизированные кадры со стереокамеры, состоящей из левой (1) и правой камер (2).

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

Инерциальный датчик IMU (3) выдает поток измерений 9-мерного вектора, а именно ускорений перемещений по 3 осям, ускорений вращений по 3 осям, ориентаций 3 осей магнитометра.

GNSS приемник (4) выдает положение с RTK коррекцией с частотой 10 Гц по протоколу NMEA. RTK (Real Time Kinematic) - услуга, позволяющая получать поправки к измерениям и устанавливать местоположение с сантиметровой точностью в режиме реального времени с помощью GNSS приемника (4) в сети постоянно действующих референцных GNSS станций.

На этапе (103) обрабатывают посредством вычислителя (5) полученные данные из Стерео камеры (1) и (2) для вычисления ее относительной позы, при этом во время обработки осуществляют:

• обработку последовательности кадров левой (1) и правой камеры (2), полученных в текущий момент времени (t1) и в предыдущий момент времени (t0), синхронизированных по времени съемки. Кадры, синхронизированные по времени съемки, включают в себя:

Клт=кадр с левой камеры в момент времени t1,

Клп=кадр с левой камеры в момент времени t0,

Кпт=кадр с правой камеры в момент времени t1,

Кпп=кадр с правой камеры в момент времени t0.

* осуществляют автоматический поиск на каждом полученном кадре особых областей, и получают для этик областей дескрипторы и координаты с помощью эвристического алгоритма ORB (Oriented FAST and Rotated BRIEF). Обозначим, что для кадров Клт, Елп, Кпт, Кпп получают наборы дескрипторов такие, как Олт, Олп, Опт, Опп, соответственно.

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

Работает алгоритм так:

1) Особые точки обнаруживаются при помощи быстрого древовидного FAST на исходном изображении и на нескольких изображениях из пирамиды уменьшенных изображений.

2) Для обнаруженных точек вычисляется мера Харриса, кандидаты с низким значением меры Харриса отбрасываются.

3) Вычисляется угол ориентации особой точки. Для этого, сначала вычисляются моменты яркости для окрестности особой точки:

х,у - пиксельные координаты, I - яркость. И затем угол ориентации особой точки:

θ=atan2(m₀₁, m₁₀)

В итоге получаем некоторое направление для окрестности особой точки.

4) Имея угол ориентации особой точки, последовательность точек для бинарных сравнений в дескрипторе BRIEF поворачивается в соответствие с этим углом.

5) По полученным точкам вычисляется бинарный дескриптор BRIEF.

* выполняют сопоставление полученных дескрипторов, как набор пар наиболее похожих между собой дескрипторов на всех полученных с левой (1) и правой (2) камер кадрах в моменты времени t0 и t1. Определяют сопоставление Si, i=1…4, как набор пар наиболее похожих между собой дескрипторов одного и второго кадра. Сопоставление осуществляют следующим образом S1=Олт с Олп, S2=Олп с Опп, S3=Опп с Опт, S4=Олт с Опт.

* вычисляют позу Р0 стереокамеры и 3D координаты точки в пространстве, соответствующей каждой паре дескрипторов в сопоставлении по паре кадров, полученных в момент времени t0 по сопоставлению при помощи триангуляции, используя калибровочные данные (6);

• вычисляют позу P1 стереокамеры и 3D координаты точки в пространстве, соответствующей каждой паре дескрипторов в сопоставлении по паре кадров, полученных в момент времени t1 по сопоставлению при помощи триангуляции, используя калибровочные данные;

• для вычисленных поз Р0 и Р1 вычисляют ошибки триангуляции еР0, еР1 как среднеквадратическое отклонение координат проекций найденных 3D точек от координат их дескрипторов на левом и правом кадрах.

На этапе (104) берут замеры с датчика IMU (3) по ускорению перемещения с акселерометра, и угловому ускорению вращения с гироскопа с момента времени t0 до момента времени t1, для каждого замера берут время между замерами, умножают его на ускорение вращения и получают изменение скорости вращения для данного замера. При этом, следует отметить, что данные инерциального датчика IMU (3) представляют собой метки времени, компонент угловой скорости и ускорения объекта в декартовой системе координат в момент опроса датчика.

Берут для каждого замера время между замерами на этапе (105), умножают его на ускорение перемещения и получают изменение скорости перемещения для данного замера. Полагая скорость равной нулю в момент времени t0, скорость в каждый замер инкрементально складывая изменения скорости до момента времени t1.

На этапе (106) вычисляют перемещения между всеми замерами, как произведение скорости на время между замерами.

Начальная скорость=0 m/с. Зная скорость по всем 6 осям для предыдущего замера, осуществляют вычисление скорости по всем 6 осям для текущего замера. Зная скорость и время, вычисляют перемещение по формуле S=V*t, где S - расстояние, V - скорость, t - время, для каждой из 3-х осей.

Далее на этапе (107) складывают все перемещения для получения сдвига dPimu1. После чего на этапе (108) берут для каждого замера время между замерами, умножают его на ускорение вращения и получают изменение скорости вращения для данного замера. И на этапе (109) полагая скорость равной нулю в момент времени t0, вычисляют вращения между всеми замерами, как произведение скорости на время между замерами. Далее на этапе (110) складывают все вращения для получения изменения ориентации oPimu1 и на тапе (111) объединяют dPimu1 и oPimu1, получая изменение позы в пространстве pPimu1 для датчика IMU.

На этапе (112) получают с GNSS приемника (4) 3D координаты С0, С1 для моментов времени t0, t1 и ошибки вычисления координат еС0, еС1. Данные GNSS приемника (4) представляют собой также метки времени и координаты токи в пространстве, которую занимает датчик в момент опроса.

Далее осуществляют построение позы fP0_0 и fP1_0 как позы Р0, Р1 и вычисляют функцию потери F(fP0_0, fP1_0, Р0, P1, pPimu1, С0, С1, еР0, eP1, eImu, еС0, eC1)=Y0 на этапе (113). Принцип построения функции F: функция должна быть всюду гладкая, по каждому источнику данных (fP0_0, fP1_0, Р0, P1, dPimu1, С0, С1, еР0, eP1, eImu, еС0, eC1) потеря должна быть 0, если отклонение его смещения от вычисленного нами значения смещения тоже 0. Потеря должна возрастать согласно закону распределения ошибок Гаусса. Итоговая функция потерь будет состоять из суммы потерь по каждому источнику данных.

На этапе (114) находят позы fP0, fP1 которые минимизируют функцию потерь F с помощью метода градиентного спуска, используя начальное приближение fP0_0 и fP1_0. Следует отметить, что последовательность поз fP0, fP1…fPi и есть траектория объекта.

На этапе (115) вычисляют посредством вычислителя (5) траекторию движения объекта путем добавления вычисленных на предыдущем шаге поз в список ранее вычисленных поз получая последовательность поз fP0, fP1…fPi для моментов времени t0, t1, …ti.

На этапе (116) получают 3D облако точек для позы fPi:

для каждой полученной позы fPi берут каждый пиксель первого изображения и ищут его на втором изображении по максимальной похожести окружающей области этого пикселя;

получают 3D координаты точки в пространстве, которая соответствует сопоставленным пикселям на обоих кадрах;

полученный набор 3D координат является плотным облаком Mi, соответствующим позе fPi. После чего на этапе (117) составляют 3D карту объединяя множества облаков Mi.

Траектория и 3D модель выводятся на дисплей вычислителя (5) и сохраняются в виде файлов, пригодных для дальнейшего анализа.

Преимуществом заявленного решения перед известными из уровня техники решениями заключается в том, что заявленное решение вычисляет показания одометрии, одновременно учитывая показания GNSS приемника (4), стереокамеры (1)/(2) и датчика IMU (3) и может работать либо только на основе стереокамеры (1)/(2), либо на основе GNSS, дополненного дополнительным входом датчиков для более точной и стабильной локализации.

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

Изобретение относится к способу определения траектории объекта в окружающем пространстве для построения 3D карты. Для определения траектории объекта считывают одновременно посредством вычислителя данные с инерциального датчика (IMU), данные о положении объекта с GNSS приемника и синхронизированные кадры со стереокамеры, состоящей из левой и правой камер, обрабатывают определенным образом посредством вычислителя полученные данные для вычисления показаний одометрии и получения 3D модели окружающего пространства, для чего одновременно учитываются показания инерциального датчика (IMU), GNSS приемника, стереокамеры. Обеспечивается повышение точности отслеживания объекта в окружающем пространстве для построения 3D карты. 4 з.п. ф-лы, 2 ил.

1. Компьютерно-реализуемый способ определения траектории объекта в окружающем пространстве для построения 3D карты, содержащий этапы, на которых:

- считывают одновременно посредством вычислителя данные с инерциального датчика (IMU), данные о положении объекта с GNSS приемника и синхронизированные кадры со стереокамеры, состоящей из левой и правой камер;

- обрабатывают посредством вычислителя полученные данные для вычисления показаний одометрии и получения 3D модели окружающего пространства, для чего одновременно учитываются показания инерциального датчика (IMU), GNSS приемника, стереокамеры, при этом во время обработки осуществляют:

• обработку последовательности кадров левой и правой камеры, полученных в текущий момент времени (t1) и в предыдущий момент времени (t0), синхронизированных по времени съемки;

• осуществляют автоматический поиск на каждом полученном кадре особых областей и получают для этих областей дескрипторы и координаты с помощью эвристического алгоритма ORB;

• выполняют сопоставление полученных дескрипторов, как набор пар наиболее похожих между собой дескрипторов на всех полученных с левой и правой камер кадрах в моменты времени t0 и t1;

• вычисляют позу Р0 стереокамеры и 3D координаты точки в пространстве, соответствующей каждой паре дескрипторов в сопоставлении по паре кадров, полученных в момент времени t0 по сопоставлению при помощи триангуляции, используя калибровочные данные;

• вычисляют позу Р1 стереокамеры и 3D координаты точки в пространстве, соответствующей каждой паре дескрипторов в сопоставлении по паре кадров, полученных в момент времени t1 по сопоставлению при помощи триангуляции, используя калибровочные данные;

• для вычисленных поз Р0 и Р1 вычисляют ошибки триангуляции еР0, еР1 как среднеквадратическое отклонение координат проекций найденных 3D точек от координат их дескрипторов на левом и правом кадрах;

• берут замеры с датчика IMU по ускорению перемещения с акселерометра, и угловому ускорению вращения с гироскопа с момента времени t0 до момента времени t1, для каждого замера берут время между замерами, умножают его на ускорение вращения и получают изменение скорости вращения для данного замера;

• берут для каждого замера время между замерами, умножают его на ускорение перемещения и получают изменение скорости перемещения для данного замера;

• полагая скорость равной нулю в момент времени t0, вычисляют скорость в каждый замер инкрементально, складывая изменения скорости до момента времени t1;

• вычисляют перемещения между всеми замерами, как произведение скорости на время между замерами;

• складывают все перемещения для получения сдвига dPimu1;

• берут для каждого замера время между замерами, умножают его на ускорение вращения и получают изменение скорости вращения для данного замера;

• полагая скорость равной нулю в момент времени t0, вычисляют вращения между всеми замерами, как произведение скорости на время между замерами;

• складывают все вращения для получения изменения ориентации oPimu1;

• объединяют dPimu1 и oPimu1, получая изменение позы в пространстве pPimu1 для датчика IMU;

• получают с GNSS приемника 3D координаты С0, С1 для моментов времени t0, t1 и ошибки вычисления координат еС0, еС1;

• осуществляют построение позы fP0_0 и fP1_0 как позы Р0, Р1 и вычисляют функцию потери F(fP0_0, fP1_0, Р0, P1, pPimu1, С0, C1, еР0, eP1, eImu, еС0, еС1)=Y0;

• находят позы fP0, fP1, которые минимизируют функцию потерь F с помощью метода градиентного спуска, используя начальное приближение fP0_0 и fP1_0;

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

для каждой полученной позы fPi берут каждый пиксель первого изображения и ищут его на втором изображении по максимальной похожести окружающей области этого пикселя;

получают 3D координаты точки в пространстве, которая соответствует сопоставленным пикселям на обоих кадрах;

полученный набор 3D координат является плотным облаком Mi, соответствующим позе fPi;

- составляют 3D карту, объединяя множества облаков Mi, полученных при вычислении траектории.

2. Способ по п. 1, характеризующийся тем, что данные инерциального датчика (IMU) представляют собой метки времени, компонент угловой скорости и ускорения объекта в декартовой системе координат в момент опроса датчика.

3. Способ по п. 1, характеризующийся тем, что данные GNSS приемника представляют собой метки времени и координаты точки в пространстве, которую занимает датчик в момент опроса.

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

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