СПОСОБ И СИСТЕМА ОБНАРУЖЕНИЯ И СОПРОВОЖДЕНИЯ ДВИЖУЩИХСЯ ОБЪЕКТОВ НА ОСНОВЕ ДАННЫХ ТРЕХМЕРНОГО ДАТЧИКА
METHOD AND SYSTEM FOR DETECTING AND TRACKING OF MOVING OBJECTS BASED ON THREE-DIMENSIONAL SENSOR DATA

ОБЛАСТЬ ТЕХНИКИ
[0001] Данное техническое решение относится, в общем, к вычислительным системам и способам, а в частности к системам и способам информационного обеспечения автономных и автоматизированных подвижных объектов.
УРОВЕНЬ ТЕХНИКИ
[0002] В настоящее время технические решения по детектированию и сопровождению движущихся объектов базируются на видеокамерах, фиксирующих некий искусственный паттерн.
[0003] Из уровня техники известен патент № US 20110026008 A1 «Lidar Measurement Device with Target Tracking and Method for Use of Same», патентообладатель: Gammenthaler Robert S, опубликовано: 03.02.2011.
[0004] В данном решении получают трехмерное изображение от лазерного сканера для наблюдения автомобильного трафика. Изобретение акцентируется на детекции движущихся автомобилей, при этом основным результатом работы изобретения является получение планарной скорости движения определяемого объекта. Недостатком исходя из изложенного подхода растрирования и способа оценки смежных по времени растровых изображений является низкая точность определения скорости объекта, движущегося с малой скоростью (сравнимой со скоростью пешехода).
СУЩНОСТЬ ИЗОБРЕТЕНИЯ
[0005] Данное техническое решение направлено на устранение недостатков, свойственных решениям, известным из уровня техники.
[0006] Технической задачей, решаемой в данном техническом решении, является обеспечение качества обнаружения и сопровождения подвижных объектов. Указанное качество измеряется вероятностью ошибок первого и второго рода (ложных срабатываний и пропуска события соответственно), а так же, повышением качества сопровождения объекта, которое сводится к уменьшению числа потерь слежения и обеспечению высокой точности определения характеристик объекта (координаты, скорости его движения и его размер).
[0007] Первая проблема обусловлена тем, что при движении подвижного объекта, например, робота, точки, соответствующие неподвижным объектам инфраструктуры, движутся, причем, это движение не сводится к планарному движению робота.
[0008] Вторая проблема заключается в том, что объект может иметь нечеткую отражающую поверхность сложной формы, и при больших расстояниях трехмерный датчик фиксирует его как малое количество точек. Это создает неоднозначность в определении перемещения объекта за время между двумя последовательными лазер-сканами трехмерного датчика.
[0009] В данном техническом решении предложен способ, позволяющий надежно обнаруживать движущиеся объекты, и определять их характеристики с требуемой точностью, а также совмещать облака точек объекта, накопленные за всю историю наблюдения и восстанавливать форму отражающей поверхности объекта.
[00010] Техническим результатом, достигаемым при решении указанной задачи, является повышение точности обнаружения движущихся объектов и повышение надежности их сопровождения.
[00011] Для планарного сканера кругового обзора установленного на складском роботе-погрузчике были получены следующие результаты. Для последовательности, состоящей из 16771 кадров, на которой вручную было отмечено 7467 событий присутствия подвижной складской техники была получена вероятность не обнаружения 1.3% и вероятность ложного обнаружения 2.1%, что значительно меньше, чем в уровне техники. Примеры сцены показаны на Фиг. 6.
[00012] Указанный технический результат достигается благодаря способу обнаружения и сопровождения движущихся объектов на основе данных трехмерного датчика, в котором получают последовательность кадров в реальном времени от трехмерного датчика, установленного на подвижном объекте; выделяют на полученной последовательности кадров движущиеся точки посредством применения статического критерия гладкости движения; формируют по меньшей мере один кластер движущихся точек, который соответствует движущемуся объекту; преобразуют каждый кластер, сформированный на предыдущем шаге в облако точек движущегося объекта на текущем кадре; совмещают облака точек одних и тех же движущихся объектов на текущем и предыдущем кадрах последовательности кадров, полученной от трехмерного датчика, тем самым получая смещение движущихся объектов; формируют накопленное облако точек по меньшей мере одного движущегося объекта посредством совмещения облака точек движущегося объекта текущего кадра и накопленного облака точек, полученного на предыдущем шаге; формируют статистическую модель по меньшей мере одного движущегося объекта на основании накопленного облака точек по меньшей мере одного движущегося объекта, сформированного на предыдущем шаге; определяют координаты, скорость и размеры движущегося объекта по сформированной на предыдущем шаге статистической модели по меньшей мере одного движущегося объекта и полученного смещения движущихся объектов.
[00013] В некоторых вариантах осуществления трехмерным датчиком является времяпролетный датчик или трехмерная камера или стереокамера или триангуляционный сканер или лазерная сканирующая система или их совокупность в различных сочетаниях.
[00014] В некоторых вариантах осуществления при получении последовательности кадров осуществляют компенсацию движения датчика посредством переноса координат в глобальную систему координат.
[00015] В некоторых вариантах осуществления при выделении на полученной последовательности кадров движущихся точек используют критерий видимости.
[00016] В некоторых вариантах осуществления при формировании кластеров движущихся точек, рекурсивно добавляют все движущиеся точки, расстояния от которых до ближайшей точки каждого кластера меньше некоторого порога.
[00017] В некоторых вариантах осуществления совмещают облака точек одних и тех же движущихся объектов на текущем и предыдущем кадрах посредством корреляции.
[00018] В некоторых вариантах осуществления при формировании статистической модели объекта выполняют удаление точек из облака точек.
[00019] Также указанный технический результат достигается благодаря системе обнаружения и сопровождения движущихся объектов на основе данных трехмерного датчика, которая содержит по меньшей мере один трехмерный датчик, установленный на подвижном объекте и выполненный с возможностью формирования последовательности кадров в реальном времени; по меньшей мере один компонент обработки; память для хранения инструкций, выполняемых посредством компонента обработки, причем компонент обработки выполнен с возможностью выполнения шагов, описанных в п. 1 способа.
КРАТКОЕ ОПИСАНИЕ ЧЕРТЕЖЕЙ
[00020] Признаки и преимущества настоящего технического решения станут очевидными из приводимого ниже подробного описания и прилагаемых чертежей, на которых:
[00021] На Фиг. 1 показана блок-схема осуществления способа обнаружения и сопровождения движущихся объектов на основе данных трехмерного датчика;
[00022] На Фиг. 2 блок-схема осуществления выделения движущихся точек на последовательности кадров;
[00023] На Фиг. 3 показана блок-схема осуществления преобразования каждого кластера в облако точек движущегося объекта;
[00024] На Фиг. 4 показан пример совмещения облака точек движущегося объекта текущего кадра и накопленного облака точек;
[00025] На Фиг. 5 показан пример осуществления системы обнаружения и сопровождения движущихся объектов на основе данных трехмерного датчика;
[00026] На Фиг. 6 показаны примеры сцен планарного сканера кругового обзора, установленного на складском роботе-погрузчике;
[00027] На Фиг. 7 показан пример двух кадров с облаками точек в локальной и глобальной системе координат;
[00028] На Фиг. 8 показан пример двух кадров с облаками точек в локальной и глобальной системах координат с пунктирными окрестностями точек;
[00029] На Фиг. 9 показан пример объединения полученных движущихся точек в кластеры.
ПОДРОБНОЕ РАСКРЫТИЕ ТЕХНИЧЕСКОГО РЕШЕНИЯ
[00030] Ниже будут описаны понятия и определения, необходимые для подробного раскрытия осуществляемого технического решения.
[00031] Техническое решение может быть реализовано в виде распределенной компьютерной системы.
[00032] В данном решении под системой подразумевается компьютерная система, ЭВМ (электронно-вычислительная машина), ЧПУ (числовое программное управление), ПЛК (программируемый логический контроллер), компьютеризированные системы управления и любые другие устройства, способные выполнять заданную, четко определенную последовательность операций (действий, инструкций).
[00033] Под устройством обработки команд подразумевается электронный блок либо интегральная схема (микропроцессор), исполняющая машинные инструкции (программы).
[00034] Устройство обработки команд считывает и выполняет машинные инструкции (программы) с одного или более устройства хранения данных. В роли устройства хранения данных могут выступать, но, не ограничиваясь, жесткие диски (HDD), флеш-память, ПЗУ (постоянное запоминающее устройство), твердотельные накопители (SSD), оптические носители (CD, DVD и т.п.).
[00035] Программа - последовательность инструкций, предназначенных для исполнения устройством управления вычислительной машины или устройством обработки команд.
[00036] Подвижный объект - автономно перемещающий технический объект. К таким подвижным объектам могут относиться транспортные тележки, транспортировщики паллет, погрузочная техника различных кинематических схем, оснащенные различными силовыми агрегатами, беспилотные летательные аппараты самолетного, вертолетного или многороторного типа, легковые и грузовые автомобили на различных типах шасси, трактора, подвижная сельскохозяйственная техника, специальная техника на гусеничном или колесно-гусеничном шасси, суда на воздушной подушке, железнодорожный или трамвайный транспорт.
[00037] Статистический критерий - строгое математическое правило, по которому принимается или отвергается та или иная статистическая гипотеза с известным уровнем значимости. Построение критерия представляет собой выбор подходящей функции от результатов наблюдений (ряда эмпирически полученных значений признака), которая служит для выявления меры расхождения между эмпирическими значениями и гипотетическими.
[00038] Облако точек - набор вершин в трехмерной системе координат. Эти вершины, как правило, определяются координатами X, Y и Z и, как правило, предназначены для представления внешней поверхности объекта.
[00039] Фиг. 1 представляет собой блок-схему, показывающую способ обнаружения и сопровождения движущихся объектов на основе данных трехмерного датчика, который содержит следующие шаги.
[00040] Шаг 101: получают последовательность кадров в реальном времени от трехмерного датчика, установленного на подвижном объекте.
[00041] В качестве трехмерного датчика могут использоваться времяпролетные датчики, трехмерные камеры, стереокамеры, триангуляционные сканеры, лазерные сканирующие системы, например лидары и т.д., не ограничиваясь.
[00042] Трехмерный датчик может в процессе работы перемещаться в пространстве (например, датчик может быть установлен на автомобиле, движущемся по ровному горизонтальному дорожному полотну). В этом случае для осуществления технического решения необходимо компенсировать движение датчика, которое может осуществляться, например, посредством переноса координат в глобальную систему координат (неподвижную и фиксированную на протяжении цикла непрерывной работы настоящей системы).
[00043] Шаг 102: выделяют на полученной последовательности кадров движущиеся точки посредством применения статического критерия гладкости движения.
[00044] На Фиг. 2 показана блок-схема осуществления выделения на последовательности кадров движущихся точек.
[00045] Перед выделением движущихся точек, полученные кадры обрабатывают посредством выделения облаков точек каждого кадра вышеуказанной последовательности.
[00046] Определение точек как движущихся выполняется посредством статического критерия гладкости движения. В некоторых вариантах осуществления дополнительно может использоваться критерий видимости (или наблюдаемости) точек. Статический критерий гладкости движения состоит в статистической оценке облаков точек на смежных по времени кадрах полученной последовательности кадров и определения набора элементарных смещений точек по заданному заранее порогу. Для компенсации недостатка потенциально ограниченного угла обзора трехмерного датчика, а также учета эффекта затенения может использоваться критерий видимости точек. Критерий видимости статистически оценивает вероятность наблюдаемости окрестности данной точки на предыдущих кадрах, и определяет вероятность наблюдения точки в окрестности на следующих кадрах. При использовании данного критерия уже на основе этих двух вероятностей выполняется классификация точки (движущаяся/неподвижная) по критерию видимости. Критерий видимости в целом является более слабым статистическим классификатором, и, поэтому, может применяться только для пополнения уже полученных наборов движущихся точек по статическому критерию гладкости.
[00047] Шаг 103: формируют по меньшей мере один кластер движущихся точек, который соответствует движущемуся объекту.
[00048] После выделения всех движущихся точек выполняется их кластеризация в соответствии с принадлежностью точек к различным движущимся объектам. Начиная с произвольной движущейся точки к кластеру, рекурсивно добавляются все движущиеся точки, расстояния от которых до ближайшей точки кластера меньше некоторого порога. Точка, ранее не помеченная как движущаяся, добавляется к существующему кластеру при условии, что она находится на расстоянии меньшем порогового от точки кластера и классифицирована как движущаяся по статическому критерию гладкости и критерию видимости. Кластеры формируются последовательно и, по построению, не могут пересекаться. Т.е. в первый кластер рекурсивно добавляются все точки расположенные не далее, чем L от любой точки кластера. Соответственно, когда построение первого кластера закончено и начато построение второго кластера, не существует не помеченных точек, которые могут быть отнесены к первому кластеру.
[00049] Шаг 104: преобразуют каждый кластер, сформированный на предыдущем шаге в облако точек движущегося объекта на текущем кадре.
[00050] На Фиг. 3 показана блок-схема осуществления преобразования каждого кластера в облако точек движущегося объекта.
[00051] Шаг 105: совмещают облака точек одних и тех же движущихся объектов на текущем и предыдущем кадрах последовательности кадров, полученной от трехмерного датчика, тем самым получая смещение движущихся объектов.
x
y
x
y
[00052] Первым шагом слежения за подвижным объектом является грубое совмещение облаков точек объекта соответствующих текущему и предыдущему кадрам соответственно по всем кадрам последовательности. Совмещение осуществляется посредством корреляции. Для этого облака точек объектов переводятся в растровое представление (изображение). Изображение точек объекта предыдущего кадра расширяется на s пикселей в каждую сторону нулями. Смещение объекта между соседними кадрами определяется как ar gmax. Где S=-s:s, S=-s:s - смещения по X и Y; X - изображение объекта на предыдущем кадре, Y - изображение объекта на текущем кадре; S=k*(S,S) - искомый вектор смещения; k - коэффициент масштабирования выбранный для растеризации изображения.
Перемещение может измеряться в сантиметрах, дециметрах или метрах, не ограничиваясь. После грубого определения вектора перемещения объекта, точки, лежащие в окрестности заранее определенного движущегося объекта, добавляются к облаку точек данного объекта. Величина допустимой окрестности может задаваться заранее. Например, величина допустимой окрестности может быть множеством окружностей радиусом 0.05 м с центрами, соответствующими точкам из облака точек объекта. Это позволяет существенно сократить число точек объекта ошибочно помеченных как неподвижные. На Фиг. 4 показано облако точек объекта (черные круговые маркеры) полученное на стадии кластеризации, и облако точек объекта с добавленными к нему точками (серые круговые маркеры).
[00053] Шаг 106: формируют накопленное облако точек по меньшей мере одного движущегося объекта посредством совмещения облака точек движущегося объекта текущего кадра и накопленного облака точек, полученного на предыдущем шаге.
[00054] Накопленное облако точек движущегося объекта, соответствующее текущему кадру, совмещается с моделью объекта или так называемым накопленным облаком точек (Фиг. 4). Данное совмещение позволяет существенно повысить точность первоначальной оценки перемещения, поскольку в нем используется больше точек текущего кадра и модель объекта с данными, полученными за всю предыдущую историю наблюдения объекта. С учетом полученной оценки перемещения, облако точек объекта, соответствующее текущему кадру, добавляется к ранее накопленному облаку точек объекта.
[00055] Шаг 107: формируют статистическую модель по меньшей мере одного движущегося объекта на основании накопленного облака точек по меньшей мере одного движущегося объекта, сформированного на предыдущем шаге.
[00056] Статистическая модель является моделью распределения точек объекта как двумерной случайной величины (x, y), так и трехмерной (x, y, z). В данном техническом решении может использоваться модель распределения в виде набора гауссиан (GMM, Gaussian Mixture Model). Но может быть также применена другая статистическая модель, достаточно точно описывающая распределение точек, не ограничиваясь.
[00057] В некоторых вариантах осуществления выполняют выбраковку (или удаление) некоторых точек из сформированного облака точек, которое используется для предотвращения размывания объекта за счет случайных выбросов (больших смещений точек) и за счет неточного совмещения облаков. Выбраковку могут осуществлять по порогу расстояния до существующих точек накопленного облака точек объекта. Возможен также вариант выбраковки по порогу значения вероятности в данной точке Р'(x, y), где P'() есть статистическая модель объекта, т.е. приближенное распределение точек.
[00058] Накопленное облако точек кластеризуется, например, посредством алгоритма K-means, для построения статистической модели объекта. Это позволяет более компактно описать объект и ускорить выполнение операций с моделью объекта (по сравнению с операциями со всеми точками облака, накопленного за всю историю наблюдения объекта).
[00059] Шаг 108: определяют координаты, скорость и размеры движущегося объекта по сформированной на предыдущем шаге статистической модели по меньшей мере одного движущегося объекта и полученного смещения движущихся объектов.
[00060] Скорость определяется путем временной фильтрации временной последовательности «мгновенных скоростей». Мгновенная скорость на текущем кадре определяется как оценка перемещения объекта (вектор) между текущем и предыдущим кадром, деленная на время съемки между кадрами. Размеры и форма объекта определяются размерами и формой накопленного облака точек. Координаты объекта определяются суперпозицией аффинных преобразований (смещений) объекта с момента его обнаружения. Точка облака, относительно которой вычисляются координаты и перемещения, может быть выбрана произвольно в момент обнаружения объекта. В данном техническом решении используется центр облака точек объекта на момент его обнаружения. В процессе слежения эта точка сохраняет свое положение относительно объекта, но может перестать быть его центром. Т.е. если в момент обнаружения был виден передний край машины, то отслеживаются координаты середины переднего края машины.
[00061] Фиг 5 представляет собой блок-схему, показывающую систему обнаружения и сопровождения движущихся объектов на основе данных трехмерного датчика, которая содержит:
[00062] В некоторых вариантах осуществления, система 500 может быть мобильным телефоном, компьютером, устройством для обмена сообщениями, планшетом и персональным цифровым помощником и т.д.
[00063] Ссылаясь на Фигуру 5, система 500 может включать один или более следующих компонент: компонент 502 обработки, трехмерный датчик 503, память 504, компонент 506 питания, компонент 508 мультимедиа, интерфейс 512 ввода/вывода (I/О), сенсорный компонент 514, компонент 516 передачи данных.
[00064] В некоторых вариантах, компонент 502 обработки в основном управляет всеми операциями системы 500, например, дисплеем, передачей данных, работой видеокамеры и операцией записи. Компонент 502 обработки может включать в себя один или более процессоров 518, реализующих инструкции для завершения всех или части шагов из указанных выше способов. Кроме того, компонент 902 обработки может включать в себя один или более модулей для удобного процесса взаимодействия между компонентом 502 обработки и другими компонентами. Например, компонент 502 обработки может включать в себя мультимедийный модуль для удобного облегченного взаимодействия между компонентом 508 мультимедиа и компонентом 502 обработки.
[00065] В качестве трехмерного датчика 503 может использоваться любая известная из уровня техники технология получения и обработки информации об удаленных объектах с помощью активных и пассивных оптических систем, использующих явления отражения света и его рассеяния в прозрачных и полупрозрачных средах. Например, это может быть времяпролетный датчик, триангуляционный сканер, лазерная сканирующая система, лидар, датчик глубины, стереокамера.
[00066] Память 504 выполнена с возможностью хранения различных типов данных для поддержки работы системы 500. Примеры таких данных включают в себя инструкции из любого приложения или способа, изображения, видео, и т.д. Память 504 может быть реализована в виде любого типа энергозависимого запоминающего устройства, энергонезависимого запоминающего устройства или их комбинации, например, Статического Оперативного Запоминающего Устройства (СОЗУ), Электрически Стираемого Программируемого Постоянного Запоминающего Устройства (ЭСППЗУ), Стираемого Программируемого Постоянного Запоминающего Устройства (СППЗУ), Программируемого Постоянного Запоминающего Устройства (ППЗУ), Постоянного Запоминающего Устройства (ПЗУ), магнитной памяти, флэш-памяти, магнитного или оптического диска.
[00067] В некоторых вариантах, компонент 506 питания обеспечивает электричеством различные компоненты системы 500. Компонент 506 питания может включать систему управления электропитанием, один или более источник питания, и другие узлы для генерации, управления и распределения электроэнергии к системе 500.
[00068] В некоторых вариантах, мультимедийный компонент 508 включает в себя экран, обеспечивающий выходной интерфейс между системой 500 и пользователем. В некоторых вариантах осуществления, экран может быть жидкокристаллическим дисплеем (ЖКД) или сенсорной панелью (СП). Если экран включает в себя сенсорную панель, экран может быть реализован в виде сенсорного экрана для приема входного сигнала от пользователя. Сенсорная панель включает один или более сенсорных датчиков в смысле жестов, прикосновения и скольжения сенсорной панели. Сенсорный датчик может не только чувствовать прикосновение или жест перелистывания, но и определять длительность времени и давления, связанных с режимом работы прикосновения и скольжения.
[00069] Интерфейс 512 ввода/вывода обеспечивает интерфейс между компонентом 502 обработки и периферийным интерфейсным модулем.
[00070] Сенсорный компонент 514 содержит один или более сенсоров и выполнен с возможностью обеспечения различных аспектов оценки состояния системы 500. Например, сенсорный компонент 514 может обнаружить состояния вкл/выкл системы 500, относительное расположение компонентов, например, дисплея и кнопочной панели прибора 500, изменение положения системы 500 или одного компонента системы 500, наличие или отсутствие контакта между пользователем и системой 500, а также ориентацию или ускорение/замедление и изменение температуры системы 500. Сенсорный компонент 514 содержит бесконтактный датчик, выполненный с возможностью обнаружения присутствия объекта, находящегося поблизости, когда нет физического контакта. Сенсорный компонент 514 содержит оптический датчик (например, КМОП или ПЗС-датчик изображения) выполненный с возможностью использования в визуализации приложения. В некоторых вариантах сенсорный компонент 514 содержит датчик ускорения, гироскоп, магнитный датчик, датчик давления или датчик температуры.
[00071] Коммуникационный компонент 516 выполнен с возможностью облегчения проводной или беспроводной связи между системой 500 и другими устройствами. Система 500 может получить доступ к беспроводной сети на основе стандарта связи, таких как WiFi, 2G или 3G, или их комбинации. В одном примерном варианте компонент 516 передачи данных получает широковещательный сигнал или трансляцию, связанную с ними информацию из внешней широковещательной системы управления через широковещательный канал. В одном варианте осуществления компонент 516 передачи данных содержит модуль коммуникации ближнего поля (NFC), чтобы облегчить ближнюю связь. Например, модуль NFC может быть основан на технологии радиочастотной идентификации (RFID), технологии ассоциации передачи данных в инфракрасном диапазоне (IrDA), сверхширокополосной (UWB) технологии, Bluetooth (ВТ) технологии и других технологиях.
[00072] В примерном варианте осуществления, память 504 включает инструкции, которые выполняются процессором 518 системы 500 для реализации описанных выше способов измерения расстояния до удаленных объектов. Например, энергонезависимым компьютерно-читаемым носителем может быть ПЗУ, оперативное запоминающее устройство (ОЗУ), компакт-диск, магнитная лента, дискеты, оптические устройства хранения данных и тому подобное.
[00073] Специалист в данной области техники может легко понять другие варианты изобретения из рассмотренного описания. Эта заявка предназначена для того, чтобы покрыть любые варианты использования или применения следующих общих принципов изобретения, и включая такие отклонения от настоящего изобретения, которые появляются в пределах известной или обычной практики в уровне техники. Предполагается, что описание рассматривается только как примерное, с сущностью и объемом настоящего изобретения, обозначенными формулой изобретения.
[00074] Следует принимать во внимание, что настоящее изобретение не ограничивается точными конструкциями, которые были описаны выше и проиллюстрированы на прилагаемых чертежах, и что различные модификации и изменения могут быть сделаны без отхода от области его применения. Предполагается, что объем изобретения ограничен только прилагаемой формулой.
ПРИМЕРЫ РЕАЛИЗАЦИИ ТЕХНИЧЕСКОГО РЕШЕНИЯ
T
T
[00075] Лазерный сканер установлен на автономном складском паллетном погрузчике. Планарный лазерный сканер подключен к вычислительному устройству, установленному на автономном складском паллетном погрузчике. Автономный складской паллетный погрузчик, на котором установлен лазерный сканер, двигается по складу и в зоне видимости планарного лазерного сканера оказывается другое транспортное средство. Вычислительное средство получает два последовательных пакета данных от лазерного сканера по каналу передачи данных RS422, преобразует их в два кадра данных соответственно. Кадр данных в настоящем случае содержит облако точек в системе координат лазерного сканера с меткой времени. Облако точек первого кадра переводится в глобальную систему координат через известные глобальные координаты робота (от системы навигации), как это описано ниже. На Фиг. 7 проиллюстрированы два кадра данных. Глобальная координатная система обозначена заглавными буквами OXY на обоих кадрах. Локальная система координат лазерного сканера обозначена Lxy на первом кадре и L'xy на втором кадре. Одно деление (одна клетка) на Фиг. 7 соответствует одному метру в масштабе. Локальные системы координат отличаются только линейным положением L=[2 12], L'=[3 11]. Угол ориентации локальной системы координат лазерного сканера в обоих случаях составляет 45 градусов или 0.7583 радиана. Таким образом, в поле зрения лазерного сканера в первом и во втором случае попало пять точек. Две точки В являются точками неподвижной сцены (столбом, стеллажом и т.п.), в то же время все три точки А каждого случая соответствуют движущемуся складскому транспорту, который должен быть обнаружен. Причем как видно из Фиг. 7, транспортное средство движется криволинейно. В локальной системе координат на первом кадре получили следующие координаты точек:
1L
T
A=[8.55 5.77]
2L
T
A=[8.55 2.96]
3L
T
A=[11.384 2.96]
1L
T
B=[3.6 3.6]
2L
T
B=[5 2.26]
На втором кадре:
L
L
Как известно, совмещение двух координатных систем происходит путем вращения и перемещения. Таким образом, каждую точку P умножают на матрицу R поворота на 45 градусов и прибавляют координаты центра локальной системы координат L в глобальной системе координат. Т.е. в общем случае:
k
L
L
P=RP+L
1
В частном случае для точки А получают:
1L
T
Аналогичным образом подставляя вместо A=[8.55 5.77] актуальные координаты точек получают координаты всех точек первого кадра в глобальной системе координат OXY:
1
T
A=[12.13001175 10.03000051]
2
T
A=[10.14445591 8.04444467]
3
T
A=[12.14273967 6.04333248]
1
T
B=[7.09116882 12]
2
T
B=[7.13500944 10.06394163]
Пример расчета глобальных координат точки для второго кадра:
Аналогично для остальных точек второго кадра получают глобальные координаты:
[00076] Первое облако точек и второе облако точек сравниваются, после чего выделяются движущиеся точки по критерию гладкости движения. В приведенном примере это означает следующее. Необходимо определить смещение точек на втором кадре относительно точек на первом кадре. Для этого переносят точки первого кадра на второй кадр и определяют имеющиеся точки в окрестности по заданному пороговому значению. Эти окрестности изображены на Фиг. 8 пунктирными окружностями, которые взаимно идентичны на обоих кадрах. По наличию смещения определяют движущиеся точки, т.е.:
P
И если значение Δ больше некоторого заранее заданного порогового значения, например в 15 см, значит, точка сдвинулась, например
2
B2
Аналогично для точки В Δ=0.12. Таким образом, полученные смещения в 11 см и 12 см для точек В1 и В2 соответственно меньше 15 см, поэтому они являются неподвижными. Аналогично для точек А имеем:
A1
Δ=0.54
A2
Δ=0.75
A3
Δ=0.52
Как видно для точек А, смещение составляет больше полуметра, что много больше заданного порога в 15 см, значит все точки А на втором кадре движутся. Затем объединяют полученные точки в кластеры и рассчитывают центроиды для данного примера (центроид - точка пересечения медиан в треугольнике). Результат показан на Фиг. 9. Координаты центроидов и будут искомыми координатами движущегося объекта, а изменение координат по времени между кадрами и будет искомыми скоростями.