Реализация алгоритмов ориентации и навигации на четырехколесной мобильной платформе

УДК 629.05

А. С. НОСОВ, А. В МОТОРИН.

(Университет ИТМО, Санкт-Петербург).

РЕАЛИЗАЦИЯ АЛГОРИТМОВ ОРИЕНТАЦИИ И НАВИГАЦИИ НА ЧЕТЫРЕХКОЛЕСНОЙ МОБИЛЬНОЙ ПЛАТФОРМЕ.

Работа посвящена алгоритмам комплексной обработки информации для ориентации и навигации четырехколесной мобильной платформы.

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

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

Введение. Описание мобильной платформы

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

В настоящей работе рассматриваются особенности создания системы комплексной обработки измерения на примере четырехколесной мобильной платформы.

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

Список установленного оборудования:

1	Микроконтроллер	STM32F407
2	Плата расширения с датчиками:
GPS приемник	U BLOX NEO - 6M
Акселерометры	MPU6050
Гироскопы	MPU6050
Магнитометры	HMC5883L
3	Энкодер	PEC12-4220F
4	Батарея	Ni-Mh 2000 mAh

Цель работы.

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

Для решения поставленной задачи предлагается следующая комплексная схема обработки информации:

Рисунок 1. Комплексная навигационная система

Модель движения платформы.

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

При сделанных предположениях можно схематически изобразить мобильную платформу:

Рисунок 2. Система координат

Рисунок 3. Схема движения

Рис 3:- локальная декартова система координат; оси и направлены на восток и север соответственно; - координаты центра масс мобильной платформы в этой системе координат; - угол курса.

Рис 4: - расстояние между осями мобильной платформы; - угол поворота колес; - радиус разворота мобильной платформы.

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

(1)

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

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

Запишем уравнения для истинных значений измерений.

(2)

Где - скорость движения получаемая от одометрического датчика, -задаваемый угол поворота передних колес.

Используя уравнения (2), можно переписать исходную модель в следующем виде:

(3)

Откуда, раскрыв скобки, получим:

(4)

Вводя предположение о малости погрешности углов:

(5)

получим следующую систему уравнений:

(6)

Из которой, пренебрегая членами второго порядка малости, получим линеаризованную модель влияния погрешностей измерений:

(7)

Задача ориентации

Решение задачи ориентации необходимо для выработки курса мобильной платформы. Для нахождения ориентации используются измерения бортовых акселерометров, гироскопов и магнитометров. Комплексирование показаний этих датчиков позволяет избежать негативных эффектов (накопления ошибки ориентации), связанных с погрешностями гироскопов. Это особенно важно при использовании микромеханических гироскопов, так как они подвержены дрейфу порядка нескольких градусов в час. В настоящей работе решение задачи ориентации основывается на работе Себастьяна Магвика [1]. Предложенный им способ позволяет решить задачу Вахбэ по показаниям акселерометров и магнетометров в упрощенном виде и получить оценку коррекции измерений угловой скорости. Затем, с помощью интегрирования уравнений Пуассона в кватернионной форме ищется оценка ориентации.

Комплексная обработка информации.

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

(8)

Рисунок 4. Формирование разностных измерений

Где- гауссовский белый шум

Таким образом, с использованием уравнений (7) задача сводится к оценке вектора состояния

(9)

Где - гауссовский белый шум

по разностным измерениям вида (8).

Для получения такой оценки целесообразно использовать теорию оптимальной фильтрации Калмана. Итоговая схема обработки в этом случае примет вид:

Рисунок 5. Схема комплексной обработки

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

Моделирование.

Было произведено моделирование работы предлагаемого алгоритма. Результаты моделирования приведены на рисунках 7-10.

На рисунке 7 приведено моделировании координат, получаемых от GPS приемника.

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

На рисунке 9 представлена истинная траектория движения.

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

Рисунок 6. Траектория GPS	Рисунок 7. Траектория модели движения
Рисунок 8. Истинная траектория	Рисунок 9. Оценка траектории

Заключение.

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

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

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

[1]	S. Madgwick, A. Harrison and R. Vaidyanathan, "Estimation of IMU and MARG orientation using a gradient descent algorithm," in Rehabilitation Robotics (ICORR), Zurich, 2011.
[2]	, Основы теории оценивания с приложениями к задачам обработки навигационной информации, Санкт-Петербург: ГНЦ РФ ЦНИИ "Электроприбор", 2009.