Адаптация фильтра Калмана для использования с локальной и глобальной системами навигации

Размещено на http: //www. allbest. ru/

Институт прикладной математики им. М.В. Келдыша РАН, Москва

Адаптация фильтра Калмана для использования с локальной и глобальной системами навигации

А.Н. Забегаев (andronics@mail.ru)

В.Е. Павловский (vlpavl@keldysh.ru)

Аннотация

фильтр калман навигационный адаптация

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

Введение

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

Где я? Робот должен знать свое положение относительно окружающего пространства. Задача определения положения называется локализацией.

Куда я направляюсь? Для того, чтобы выполнить свою задачу, робот должен знать, куда он направляется.

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

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

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

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

1. Математическая модель

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

где xk есть x в момент времени tk, а щk - белый шум локальных измерений c матожиданием Wk и дисперсией Qk.

Также, робот обладает глобальной навигационной системой, которая в моменты времени tk позволяет получить вектор измерений zk, который линейно связан с xk:

где хk - белый шум глобальных измерений с нулевым смещением и матрицей ковариаций Rk.

2. Построение фильтра

Фильтр будет состоять из двух этапов: этап локальной коррекции и этап глобальной коррекции. Состояние системы в момент времени tk будут характеризовать прогнозируемая и ковариационная матрица ошибки Pk.

Этап локальной коррекции - поступление данных Дx с локальной навигационной системы, Wk - вектор смещения локальных измерений:

Этап глобальной коррекции - поступление данных zk с глобальной навигационной системы:

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

Рис.1 Схема работы фильтра

3. Настройка и адаптивность фильтра

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

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

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

Таким образом, желательно описать алгоритм, позволяющий оценивать вектор Wk.. В качестве характеристики, позволяющей оценивать вектор Wk можно взять разность:

Если считать, что характер движения не сильно меняется со временем, то в качестве оценки смещения вектора измерений локальной системы координат можно взять усредненное значение разностей апостериорной и априорной оценки:

Или

,

причем выборка идет по моментам времени, когда поступали измерения от глобальной навигационной системы, и Ni - количество измерений локальной навигационной системы между моментами времени ti-1 и ti. Ранее отмечалось, что при резком изменении характера движения вектор оценки также может сильно изменить свое состояние, в этом случае следует заново начинать оценивание.

4. Эмуляция поведения системы

Для демонстрации эффективности работы фильтра, покажем поведение модели на эмуляторе. Его блок-схема приведена ниже на рисунке 2.

В качестве примера приведем навигационную систему робота, созданного для соревнований Eurobot 2010. Робот функционирует на поле размером 3x2 м, маяки расположены по треугольнику на сторонах или в углах этого поля. Робот оснащен одометрической локальной навигацией, для которой характерно проскальзывание колес. Глобальная навигация позволяет определять угол на маяк с частотой 2 раза в секунду и с точностью 0,5 градуса. Характерная скорость движения робота в таких задачах составляет около 0,2 м/с. Соотношение реальной и измеренной координаты x, а также ошибку измерений можно увидеть на приведенных ниже графиках (рис.3).

Рис.2 Блок-схема эмуляции поведения системы

Рис.3 Результаты оценивания координаты х в модельной задаче

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

Заключение

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

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

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

Список литературы

1. [Дегтярёв, 2003] Дегтярёв А.А., Тайль Ш. Элементы теории адаптивного расширенного фильтра Калмана // Препринт ИПМ им. М.В. Келдыша РАН. - М., 2003. - №26.

2. [Negenborn, 2003] Rudy Negenborn, Robot Localization and Kalman Filters On ?nding your position in a noisy world / Thesis, utrecht university, 2003

3. [Sung, 2009] W.J. Sung, S.O. Choi, K.H. You, TDoA based UGV Localization using Adaptive Kalman Filter Algorithm / Sungkyunkwan University, 2009.

4. [Kurth, 2004] Derek Kurth, Range-Only Robot Localization and SLAM with Radio // Robotics Institute Carnegie Mellon University, 2004.

5. [Hiebert, 2007] Bradley Hiebert-Treuer, An Introduction to Robot SLAM (Simultaneous Localization And Mapping), 2007.

6. [Rezaei, 2003] Shahram Rezaei, Raja Sengupta, Kalman Filter Based Integration of DGPS and Vehicle Sensors for Localization / Department of Civil Engineering University of California at Berkeley, 2003

7. [Limsoonthrakul, 2008] Somphop Limsoonthrakul, Matthew N. Dailey, Manukid Parnichkun, Intelligent Vehicle Localization Using GPS, Compass, and Machine Vision / Asian Institute of Technology, 2008.

Размещено на Allbest.ru