Способ оптико-инерциальной навигации с автоматическим подбором масштабного коэффициента Российский патент 2024 года по МПК G01C23/00 

Изобретение относится к области инерциальной навигации и более точно касается способа оптико-инерциальной навигации с автоматическим подбором масштабного коэффициента автономного объекта, в частности, беспилотного летательного аппарата.

Известны способы оптической навигации позволяющие определять местоположение объекта путем получения последовательности изображений с монокулярной визуальной видеокамеры и решения задачи оптимизации расположения камеры в пространстве (см., например J. Zubizarrea, I. Aguinaga, and J. M. M. Montiel, «Direct sparse mapping», IEEE Transactions on Robotics, vol. 36, no. 4, pp. 1363–1370, 2020.)

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

В связи с этим получила распространение идея использования дополнительных датчиков, позволяющих повысить точность системы навигации. В настоящее время известны способы оптико-инерциальной навигации, использующие помимо данных монокулярной видеокамеры, данные инерциальных датчиков, измеряющих ускорение, угловую скорость, направление. В частности известен способ инерциальной навигации с автоматическим подбором масштабного коэффициента, состоящий в восстановлении траектории автономного объекта при помощи системы SLAM (метод одновременной навигации и построения карты, увязывающий два независимых процесса в непрерывный цикл последовательных вычислений, при котором результаты одного процесса участвуют в вычислениях другого) путем получения последовательности изображений с монокулярной видеокамеры и решения задачи оптимизации ее расположения в пространстве, при использовании информации от инерциальных датчиков. При этом благодаря учету данных о предполагаемой траектории движения камеры поступающих от инерциальных датчиков, оказывается возможным точнее расположить камеру в пространстве и одновременно решить проблему с идентификацией масштаба (см., например, Carlos Campos, Richard Elvira, Juan J. Gomez Rodriguez, Jose M. M. Montiel and Juan D. Tardos, «ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM», IEEE Transactions on Robotics 37(6):1874-1890).

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

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

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

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

Технический результат достигается тем, что благодаря объединению информации с существующей системы визуальной навигации с информацией от инерциальных и датчиков абсолютного положения объекта с помощью расширенного фильтра Калмана (EKF) обеспечивается повышение точности определения координат объекта, в данном случае летательного аппарата (ЛА) относительно точки его запуска. Кроме того, предлагаемый способ позволяет без каких-либо существенных усложнений добавлять в систему новые датчики, измеряющие абсолютное положение объекта в пространстве по какой-либо оси для обеспечения повышения скорости сходимости масштабного коэффициента. Например, интеграция данных барометра или высотомера, содержащих информацию

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

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

Устройство, блок-схема которого изображена на прилагаемой фигуре, содержит блок 1 запуска системы, блоки 2,3,4 и 5 получения информации о наличии или отсутствии сигнала соответствующего датчика, блок 6 получения данных от инерциальных датчиков, блок 7 получения данных от датчиков абсолютного положения объекта в пространстве, блок 8 проверки на прерывание сессии SLAM, блок 9 получения данных SLAM, блоки 10, 11 и 12 преобразования в мировую систему координат, блок 13 предсказания смещения внутри сессии, блок 14 уточнения положения точки начала сессии и блок 15 уточнение положения смещения.

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

В данном случае система SLAM рассматривается как отдельный датчик, который получает на вход изображения с видеокамеры, а на выход выдает смешение относительно точки начала очередной сессии SLAM. Примером реализации такого датчика может быть принята система ORB-SLAM2, известная из https://github.com/raulmur/ORB_SLAM2. Сессий SLAM может быть несколько в случае сложных условий записи видеоизображений.

В описываемом примере реализации способа система получает информацию со следующих видов датчиков. Первые данные - данные полученные от какой-либо системы SLAM (система построения карты по монокулярной камере в своей системе координат) фиксируются блоком 5. Вторые - данные, получаемые с инерциальных датчиков фиксируются блоком 2, а третьи с датчиков абсолютного положения фиксируются блоком 3.

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

После типовой системы SLAM на вход блока 9 с соответствующей частотой дискретизации приходят следующие данные:

• X_slam: координаты во внутренней системе координат SLAM;

• q_slam: - кватернион поворота камеры в текущий момент времени относительно первого положения камеры внутри одной сессии SLAM.

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

На вход блока 6 с выхода блока 2 с инерциальных датчиков приходит следующая информация:

• a_imu - ускорения по трем осям в системе отсчета, связанной с телом;

• q_imu: кватернион поворота тела относительно направления на север.

Данные с других датчиков абсолютного положения, в описываемом примере информация о высоте h относительно точки начала полета, поступает на блок 7.

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

где - фиксированный кватернион доворота из системы координат, связанной с камерой, в систему координат, связанную с телом, т.е. с инерциальным датчиком.

Таким образом, можно считать, что мировая система координат и система координат, связанная с сессией SLAM, совпадают с точностью до масштабного коэффициента. В частности, далее под системой отсчета SLAM будем понимать мировую систему координат известную с точностью до масштаба, т.е. где координаты в мировой системе координат, x_slam - координаты в системе отсчета, связанной с сессией SLAM, a s - масштабный коэффициент.

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

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

Уравнение движения, т.е. этап предсказания положения в следующий момент времени (в терминах фильтра Калмана) выполняется с помощью линейного уравнения (II).

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

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

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

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

Кроме того, в силу того, что система координат SLAM совпадает только в пределах одной сессии, необходимо также в рамках уравнения движения отслеживать и обрабатывать смену сессий в системе визуальной навигации SLAM. Для этого разделяют текущие абсолютные координаты на точку начала сессии SLAM x₀ и смещение δх внутри одной сессии SLAM:

Тогда уравнения движения (II) будут обновлять именно δх, a x₀ в рамках уравнения движения остается неизменным.

Однако, если система SLAM начала новую сессию, то добавляется еще один этап предсказания, который сохраняет накопленную координату в x₀:

Далее, данные датчиков, не привязанных к системе SLAM, необходимо соотносить с суммой x₀+δ_х. Например, показания барометра или высотомера нужно соотносить с координатой z вектора суммы:

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

Практическая реализация способа оптико-инерциальной навигации с автоматическим подбором масштабного коэффициента, согласно изобретению, была осуществлена на примере автономного дрона, которому необходимо уметь определять свое местоположение вне зависимости от доступности сигнала спутниковой навигации (слабый спутниковый сигнал, работа в здании). Данный способ был реализован для навигации дрона при специальном игнорировании сигнала от спутниковой навигации. Было установлено, что мощности встраиваемых систем уровня Nvidia Jetson и khadas vim3 достаточно для восстановления траектории прямо на борту дрона. При этом погрешность определения координат не превышала 5% от длины траектории.

Изобретение относится к области измерения навигационных величин, а именно к способу оптико-инерциальной навигации с автоматическим подбором масштабного коэффициента автономного летательного аппарата (ЛА). Способ осуществляют при использовании данных от системы SLAM путем получения последовательности изображений с монокулярной визуальной камеры и использования информации от инерциальных датчиков и датчиков абсолютного положения объекта в пространстве, с последующим восстановлением масштабного коэффициента. Восстановление масштабного коэффициента осуществляют, применяя расширенный фильтр Калмана с добавлением в фазовое пространство масштабного коэффициента, используемого на этапе обновления состояния фильтра Калмана, для соотнесения координат объекта, вычисленных на основе информации от инерциальных датчиков и датчиков абсолютного положения объекта в пространстве с использованием уравнений Ньютона, с координатами камеры, полученными при использовании системы SLAM. В случае недоступности визуальной информации, по которой возможно определить положение камеры в пространстве, осуществляют дополнительный этап обновления фильтра Калмана для поддержания координаты начала сессии SLAM и обновления координат объекта в соответствии с информацией от инерциальных датчиков и датчиков абсолютного положения объекта в пространстве. В качестве последних используют высотомер или барометр. Технический результат – получение достоверной информации о местоположении автономного ЛА относительно точки его запуска. 1 з.п. ф-лы, 1 ил.

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

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