Способ позиционирования наземного транспортного средства
GROUND TRANSPORT VEHICLE POSITIONING METHOD

Изобретение относится к области навигации, а именно способам автономного позиционирования наземных транспортных средств (ТС), в том числе автоматизированных мобильных систем.
В связи с тем, что количество автономных транспортных средств и робототехнических систем растет, задача позиционирования таких систем приобретает большую значимость. Точная оценка собственного положения в пространстве необходима для точного управления движением, планирования маршрута и обеспечения безопасности при эксплуатации автономных транспортных средств, предотвращая возможность возникновения столкновений.
Из данной области техники известны следующие технические решения.
Известен способ (патент RU 2621480), при котором загружают карту с объектами окружающей среды, в которой движется транспортное средство, в блок памяти транспортного средства, в процессе движения на бортовой вычислитель поступают изображения от камер, установленных на транспортном средстве, обрабатывают полученные изображения с выделением объектов окружающей среды (края), инициализируют множество частиц, которые являются предполагаемыми положениями и ориентациями движущегося тела с возможностью их последующих перемещений и преобразуют информацию об объекте окружающей среды из данных карты для каждой частицы в виртуальное изображение, захватываемое из предполагаемого положения и ориентации, сравнивают изображение объекта и виртуальное изображение для каждой частицы, присваивают более высокое значение оценки, если имеется большее количество перекрывающихся краев между изображениями, а также присваивает более высокое значение оценки, если имеется большее количество краев, которые не являются перекрывающимися краями и для которых расстояние от края до края, которое представляет собой расстояние между краем в изображении края и краем в виртуальном изображении, меньше или равно заданному значению, оценивают положение движущегося тела на основе значения вероятности для каждой частицы.
Известен способ оценки положения/ориентации движущегося объекта (патент JP5867176), при котором в память бортового вычислителя загружают трехмерную карту с объектами окружающей среды, в которой движется транспортное средство, в процессе движения на бортовой вычислитель поступают изображения от камер, установленных на транспортном средстве, обрабатывают полученные изображения с выделением объектов окружающей среды, оценивают перемещения и ориентацию в пространстве ТС во времени на основе информации, полученной от бортовых датчиков (например, одометрия, GPS приемник), инициализируют множество частиц-гипотез о пространственном положении транспортном средстве и перемещают каждую из частиц в соответствии с показаниями бортовых датчиков, оценивают вероятность каждой частицы в соответствии с вероятностной моделью на основе сопоставления априорной трехмерной карты с изображениями камер с выделенными объектами окружающей среды.
Известен способ (WO2019092418) определения положения транспортного средства, включающий этапы загрузки и хранения набора данных объектов окружающей среды в бортовой вычислитель ТС, в процессе движения ТС с камер получают данные о объектах окружающей среды, определяют присутствие наблюдаемого объекта в наборе данных окружающей среды с применением искусственной нейроновой сети (ИНС), оценивают перемещения и ориентацию в пространстве ТС во времени на основе информации, полученной от бортовых датчиков (одометрия, инерциальная навигационная система, GNSS), определяют местоположение и ориентацию ТС с применением алгоритма фильтра частиц, при реализации которого инициализируют множество частиц-гипотез о положении транспортного средства в начальном положении и перемещают каждую частицу в соответствии с показаниями бортовых датчиков, оценивают вероятность каждой частицы-гипотезы из инициализированного множества с применением вероятностной модели с учетом результатов обнаружения наблюдаемого объекта-ориентира в наборе данных объектов окружающей среды.
Общими недостатками для представленных способов позиционирования автономных ТС, известных из уровня техники, является то, что реализованные в них модели оценки вероятности для каждой частицы-гипотезы не оценивают достоверность информации о положении ТС в пространстве, которую содержат частицы-гипотезы, на основе выделенных линейных объектов с изображений камер с учетом зоны ограниченного доступа для ТС, отличаются инициализацией большого количества частиц-гипотез о положении ТС, что ведет к увеличению ошибки локализации ТС, снижает надежность и стабильность работы систем позиционирования в рамках, которой реализуется способ, предъявляют более жесткие требования к вычислительным мощностям систем позиционирования.
Способ, раскрытый в источнике WO2019092418, по технической сущности является наиболее близким к заявляемому изобретению и может выступать в качестве прототипа.
Задачей, на решение которой направлено заявленное изобретение, является создание способа позиционирования автономных наземных транспортных средств (ТС) с применением метода фильтра частиц, основанного на формировании множества частиц-гипотез, содержащих вероятностную информацию о положении ТС, с возможностью их последующих перемещений в соответствии с информацией о пройденном пути и угловом положении ТС, определяемой на основе показаний, полученных от бортовых датчиков, и оценке текущего положения ТС на основе определения вероятности каждой частицы-гипотезы о положении ТС в пространстве на основе сопоставления априорно заданной карты с линейными объектами с текущими результатами идентификации объектов окружающей среды, оценивая при этом достоверность информации о положении ТС в пространстве, хранящейся в каждой частице-гипотезе.
Технический результат заявленного изобретения заключается в повышении точности определения положения наземного транспортного средства в пространстве при обеспечении быстродействия и надежности реализации предлагаемого способа для позиционирования ТС.
Технический результат заявленного изобретения достигается при реализации способа позиционирования наземного транспортного средства, при котором формируют карту в виде изображений с линейными объектами, расположенными в плоскости дороги, по которой движется транспортное средство (ТС), загружают указанную карту в память бортового вычислителя ТС, в процессе движения ТС на бортовой вычислитель поступают изображения от камер, размещаемых на ТС, обрабатывают полученные изображения с выделением объектов окружающей среды, оценивают перемещения и ориентацию в пространстве ТС во времени на основе информации, полученной от бортовых датчиков, оценивают текущее положение транспортного средства в пространстве с помощью реализации алгоритма фильтра частиц, основанного на формировании множества частиц-гипотез о положении транспортного средства с возможностью их последующих перемещений, сопоставляют выделенные объекты окружающей среды на изображениях от камер с загруженной в бортовой вычислитель картой с учетом положения каждой частицы-гипотезы, оценивают вероятность каждой частицы-гипотезы, после чего проводят их ресемплинг.
Особенностью предлагаемого способа является то, что карту с линейными объектами формируют с выделением областей непроходимости для ТС, а также при реализации алгоритма фильтра частиц формируют множество частиц-гипотез о положении ТС в начальном положении перед началом движения, от бортовых датчиков получают последовательные значения показаний координат ТС и угла поворота ТС вокруг вертикальной оси с разницей во времени, определяют относительные координаты местоположения ТС и угол поворота ТС вокруг вертикальной оси как разницу между последовательными значениями показаний, поступающих с бортовых датчиков, на основании данных показаний определяют пройденное расстояние ТС и угловое положение с учетом кинематической модели движения ТС с применением аддитивного Гауссового шума, на основании полученных данных формируют информацию о положении ТС перемещением каждой частицы-гипотезы в соответствии с измерениями бортовых датчиков, затем оценивают достоверность информации о положении ТС с учетом карты и изображений, полученных от камер, и оценивают текущее положение ТС, определяя насколько каждая частица-гипотеза о положении ТС соотносится с показаниями от бортовых датчиков в данный момент времени на основе определения вероятности каждой частицы-гипотезы по формуле:
где
– вероятность каждой частицы-гипотезы о положении ТС в пространстве в момент времени t;
= () −измерения бортовых датчиков в момент времени t;
−гипотеза о положении ТС в пространстве в момент времени t, включающая двумерные координаты ТС  и угол поворота вокруг вертикальной оси ;
– данные карты с линейными объектами, расположенными в плоскости дороги и выделенной зоной непроходимости ТС;
−функция для оценки ошибки идентификации объектов окружающей среды с изображений камер с учетом вероятности ложной ошибки идентификации;
− функция для оценки вероятности нахождения ТС в непроходимой зоне, принимающая значение 1, если положение проходимо и 0 − в противном случае;
– оценка вероятности частицы-гипотезы из сформированного множества в момент времени t;
n=1, …, N ‒ порядковый индекс частицы-гипотезы;
N – количество частиц-гипотез о положении ТС,
при этом результирующую оценку положения ТС определяют как среднее арифметическое по всем гипотезам после проведения ресемплинга частиц-гипотез.
Под ресемплингом понимают стохастический процесс, при котором частицы-гипотезы переходят в новую выборку, с вероятностью равной оценке вероятности гипотезы.
В предпочтительном варианте осуществления заявленного способа, в качестве бортовых датчиков выступают энкодеры, размещаемые на колесах ТС, инерциальная навигационная система и GNSS приемник.
В другом предпочтительном варианте осуществления заявленного способа, карту с линейными объектами загружают в бортовой вычислитель в векторном формате, а затем перед началом работы переводят в растровый формат.
Оценка достоверности информации о местоположении ТС (двумерные координаты) и угле поворота ТС вокруг вертикальной оси (угол рыскания) в текущий момент времени, выполняемой для каждой частицы-гипотезы, хранящей данную информацию, с учетом карты и изображений, получаемых от оптических камер при осуществлении их сопоставления с последующей оценкой текущего положения ТС на основе определения вероятности каждой частицы-гипотезы о положении ТС в пространстве из сформированного множества улучшает качество позиционирования автономного ТС в части повышения точности определения местоположения и углового положения ТС при достижении стабильности и надежности реализации предлагаемого способа в составе системы позиционирования на ТС.
К тому же предоставляется возможность анализа чувствительности осуществления предлагаемого способа в части влияния на ошибку локализации ТС таких факторов как, применяемые алгоритмы идентификации объектов окружающей среды с изображений камер, условия обстановки окружающей среды, маневры, выполняемые ТС.
На фиг.1. представлена схема системы позиционирования автономного ТС, на основе которой реализуется предлагаемый способ, где 1 – оптические камеры, 2 – модуль идентификации линейных объектов, 3 – модуль локализации, 4 – цифровая карта, 5 – модуль оценки пройденного пути, 6 – энкодеры, размещенные на колесах ТС, 7 – инерциальная навигационная система, 8 – бортовой вычислитель, 9 – GNSS.
Способ осуществляется следующим образом (фиг.1).
Способ позиционирования наземного транспортного средства предназначен для точного положения в пространстве (на плоскости) автономного наземного транспортного средства, выполняющего движение, предпочтительно, внутри закрытых помещений, таких как, склады, промышленные площадки, и. т. д.
Способ может быть применен для локализации колесных и гусеничных роботов, а также других автономных наземных транспортных средств.
На первом этапе реализации способа формируют цифровую карту (4) в виде многоканального изображения с линейными объектами, расположенными в плоскости дороги, по которой движется транспортное средство (ТС).
Линейные объекты, наносимые на цифровую карту, могут быть представлены в виде дорожной разметки, краев дороги и других линейных стационарных объектов в плоскости дороги.
Кроме того, цифровая карта содержит области, характеризующие зоны непроходимости для ТС, что позволяет при реализации метода фильтра частиц не рассматривать маловероятные гипотезы о положении ТС, тем самым уменьшая их количество и, следовательно, требования к вычислительным ресурсам.
Указанную цифровую карту (4) загружают в память бортового вычислителя ТС (8) в векторном формате, а затем перед началом работы преобразуют в растровый формат, осуществляя хранение указанной карты в бортовом вычислителе ТС (8) в виде растрового многоканального изображения. Такой способ хранения позволяет легко редактировать и модифицировать карту, получать растровую карту произвольной точности (масштаба), а также добавлять новую априорную информацию о линейных объектах в виде дополнительных слоев.
В процессе движения ТС на бортовой вычислитель (8) поступают изображения от оптических камер (1), размещаемых на ТС, при этом обработка полученных изображений осуществляется программным модулем идентификации линейных объектов (2), предназначенного для выделения (идентификации) объектов окружающей среды.
Перемещения и ориентацию в пространстве ТС во времени в процессе движения оценивают на основе информации, полученной от бортовых датчиков, представленных в виде энкодеров (6), размещаемых на колесах ТС, инерциальной навигационной системы (7) и GNSS приемника (9).
Принимаются во внимания условия эксплуатации ТС, так в случае пропадания сигнала от GNSS или его ошибки, которое возможно, например, в тоннелях, лесах, гаражах, закрытых помещениях, данный тип источника навигационной информации не принимается во внимание и, следовательно, GNSS служит только средством поддержки для локализации ТС.
Важно отметить, что данный набор датчиков не может обеспечить точное позиционирование без дополнительной коррекции, так как измерения обоих видов датчиков подвержены накоплению ошибок за время активной работы, по причине чего определяют относительные координаты местоположения ТС и угла поворота ТС вокруг вертикальной оси как разницу между последовательными значениями показаний указанных бортовых датчиков.
Текущее положение транспортного средства в пространстве, выполняемое в модуле локализации (3), оценивают с помощью применения алгоритма фильтра частиц, основанного на формировании множества частиц-гипотез о положении транспортного средства в начальном положении перед движением ТС с возможностью их последующих перемещений в соответствии с информацией о пройденном пути и угловом положении ТС (5), определяемой на основе показаний, полученных от бортовых датчиков (колесные энкодеры, инерциальная навигационная система, GNSS) (6, 7, 9), и последующей оценки вероятности каждой частицы-гипотезы посредством сопоставления априорно заданной цифровой карты с линейными объектами (4), хранящейся в бортовом вычислителе ТС (8), с идентифицированными объектами окружающей среды на изображениях (2), полученных от оптических камер (1).
В применении к задаче позиционирования ТС каждая частица-гипотеза из сформированного множества содержит вероятностную информацию о положении ТС в пространстве, представляющую собой данные о двумерных координатах ТС в системе координат, связанной с цифровой картой и угле поворота вокруг вертикальной оси (угол рыскания) в текущий момент времени.
Информацию о местоположении ТС и его угловом положении каждой частицы-гипотезы из сформированного множества изменяют в процессе движения ТС в соответствии с показаниями, получаемыми от бортовых датчиков движения (6, 7, 9).
От колесных энкодеров (6) получают показания о количестве оборотов колес ТС, по показаниям которых путем интегрирования определяют координаты положения ТС, от инерциальной навигационной системы (7) получают показания о ориентации ТС (углы крена, тангажа и рыскания), от GNSS приемника (9) получают данные о координатах местоположения и скорости ТС.
При этом, от указанных бортовых датчиков получают последовательные значения показаний двумерных координат ТС и угла поворота ТС вокруг вертикальной оси с разницей во времени, определяют относительные координаты местоположения ТС и угла поворота ТС вокруг вертикальной оси как разницу между последовательными значениями показаний указанных бортовых датчиков, на основании полученной информации определяют пройденный путь ТС и изменение угла поворота во времени с применением кинематической модели движения ТС, реализованной в модуле оценки пройденного пути (5).
Аддитивный Гауссов шум с нулевым средним значением применяется к позиции и ориентации каждой частицы-гипотезы для учета шума измерений бортовых датчиков.
Затем, оценивают достоверность вероятностной информации о положении ТС, представленной в каждой частице-гипотезе из сформированного множества, с учетом карты (4), загруженной в бортовой вычислитель (8), и изображений, полученных от оптических камер (1). Указанная оценка достоверности выполняется с учетом ошибки идентификации объектов окружающей среды с изображений, полученных от оптических камер, ложной ошибки указанной идентификации и оценки вероятности нахождения ТС в ограниченной для его передвижения зоны.
Вероятность каждой частицы-гипотезы из сформированного множества оценивают следующим образом:
где
– вероятность каждой частицы-гипотезы о положении ТС в пространстве в момент времени t;
= () −измерения бортовых датчиков в момент времени t;
−гипотеза о положении ТС в пространстве в момент времени t, включающая двумерные координаты ТС  и угол поворота вокруг вертикальной оси ;
– данные карты с линейными объектами, расположенными в плоскости дороги и выделенной зоной непроходимости ТС;
−функция для оценки ошибки идентификации объектов окружающей среды с изображений камер с учетом вероятности ложной ошибки идентификации;
− функция для оценки вероятности нахождения ТС в непроходимой зоне, принимающая значение 1, если положение проходимо и 0 − в противном случае;
– оценка вероятности частицы-гипотезы из сформированного множества в момент времени t;
n=1, …, N ‒ порядковый индекс частицы-гипотезы;
N – количество частиц-гипотез о положении ТС.
Вероятность каждой частицы-гипотезы о положении ТС пропорциональна пересечению линейных объектов, нанесенных на цифровую карту и линейных объектов окружающей среды, распознанных с изображений оптических камер и преобразованных к системе координат цифровой карты с учетом текущего положения частицы.
Оценка достоверности информации о местоположении ТС (двумерные координаты) и угле поворота ТС вокруг вертикальной оси (угол рыскания) в текущий момент времени выполняется для каждой частицы-гипотезы, хранящей данную информацию, с учетом ошибки идентификации объектов окружающей среды с изображений, полученных от оптических камер, ложной ошибки указанной идентификации и оценки вероятности нахождения ТС в ограниченной для его передвижения.
Модель ошибки идентификации напрямую закодирована в цифровую карту на предварительном шаге ее подготовки. Предполагается, что ошибка имеет Гауссово распределение с нулевым математическим ожиданием по двумерным координатам, для учета которой к цифровой карте линейных объектов применяется Гауссово размытие. Среднеквадратичное отклонение  при этом выражает величину ожидаемой ошибки и должно быть экспериментально определено для разных алгоритмов идентификации объектов окружающей среды.
Оценка ложной ошибки идентификации линейных объектов осуществляется для учета возможной ситуаций, когда выполняется идентификация в области изображения, не содержащей образа линейного объекта окружающей среды. Вероятность ошибки ложных срабатываний описывается равномерным распределением.
Таким образом, для определения вероятности частицы-гипотезы о положении ТС в пространстве оценка ошибки идентификации объектов окружающей среды с изображений, получаемых от камер, с учетом совершения ложной идентификации указанных объектов выполняется на основе суммы вероятностной ошибки идентификации объектов окружающей среды, распределенной по нормальному закону и вероятностной ошибки ложной идентификации, распределенной по равномерному закону, при этом координаты из системы координат цифровой карты преобразуют в значение интенсивности пикселя, соответствующего данным координатам на цифровой карте.
Кроме того, нанесенные на цифровую карту участки непроходимости ТС, обеспечивают возможность выделять недостоверную информацию о положении ТС в пространстве (двумерные координаты ТС и угол поворота ТС вокруг вертикальной оси), что позволяет не принимать во внимание маловероятные гипотезы о положении ТС.
С данной целью вводят функцию для оценки вероятности нахождения ТС в непроходимой зоне, которая принимает значение 1, если для ТС местоположение проходимо для движения и 0 – в противном случае.
При этом, в случае выявленной недостоверной информации о положении ТС в пространстве оценка вероятности для данной частицы-гипотезы не проводится.
После оценки вероятности каждой частицы-гипотезы из сформированного множества на текущем этапе производят ресемплинг ‒ стохастический процесс, при котором частицы-гипотезы переходят в новую выборку, с вероятностью равной оценке вероятности гипотезы, при этом общее число частиц остается неизменным, не оцениваются только вероятности частиц-гипотез, попадающих в зону непроходимости ТС.
Данный процесс может осуществлять любым известным способом из уровня техники, например, Resampling wheel (метод «Колесо отсева»),
Результирующую оценку положения ТС определяют как среднее арифметическое по всем гипотезам после проведения ресемплинга частиц-гипотез.
Таким образом, предлагаемое изобретение представляет собой универсальных способ позиционирования автономных наземных транспортных средств, обеспечивающего высокую точность определения местоположения и углового положения в пространстве ТС с повышенной скоростью обработки показаний, получаемых от разнородных источников навигационной информации, при достижении стабильной и надежной реализации способа на ТС без жестких требований к вычислительным ресурсам системы позиционирования.