СИСТЕМА ЛОКАЛИЗАЦИИ ОБЪЕКТОВ В ПОМЕЩЕНИЯХ

Рассматриваемый алгоритм оценки положения можно описать следующими этапами (рисунок 1): 1) cбор данных от датчиков; 2) обработка данных (предварительная оценка положения); 3) оценка положения с использованием специальных алгоритмов.

Рисунок 1. Модель взаимодействия этапов алгоритма локализации

Предлагается использование лидара (частота получения данных 10 Гц) и гироскопа (частота получения данных 100 Гц).

В ходе обработки данных от датчиков формируется вектор измерения, который будет использован для обновления оценки положения. На основании скана лидара формируется оценка о положении (координаты и ориентация в пространстве). Для обработки данных предлагается использование алгоритма KISS-ICP [1]. Данные от гироскопа формируют оценку ориентации в пространстве с использованием фильтра Madgwick [2].

Для оценки положения предлагается использование UKF (Unscented Kalman Filter) [3]. Хотя существуют альтернативные реализации, такие как EKF (Extended Kalman Filter) [4], UKF предпочтителен благодаря простоте разработки и отсутствию необходимости в вычислении якобианов для нелинейных систем.

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

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

При выборе модели состояния учитываются реальные требования предметной области: модель постоянной линейной скорости подходит для статичных или малоподвижных объектов; модель постоянного ускорения ‒ для динамичных, подвижных объектов. Чем сложнее система (чем больше в ней параметров), тем выше вероятность деградации со временем. Деградация может проявляться в виде некорректных оценках положения или формировании отрицательно-определённой ковариационной матрицы. Для отладки предлагается расчёт расстояния Махаланобиса (мера расстояния между векторами случайных величин, обобщающая понятие евклидова расстояния, чем меньше значение, тем лучше работает система).

Предлагается, что в UKF поступает вектор измерений только от лидара. Данные от гироскопа, поступающие с более высокой частотой, не интегрируются напрямую, чтобы избежать некорректного обновления состояния (обновление только ориентации без координат, поскольку измерения лидара реже и будут интерпретированы как шум измерений). Напротив, результаты вычисления ориентации от гироскопа используются для формирования временного состояния между обновлениями лидара. Информация об ориентации используется на этапе обработки скана лидара, для получения более вектора измерений (алгоритм KISS-ICP склонен выдавать некорректный вектор измерений при грубой априорной оценке). Параллельно решается известная проблема дрейфа гироскопа.

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