ПРИМЕНЕНИЕ МЭМС ТЕХНОЛОГИЙ ДЛЯ ОПРЕДЕЛЕНИЯ КООРДИНАТ МОБИЛЬНОГО РОБОТА В ПРОСТРАНСТВЕ

Введение

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

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

Все эти проблемы можно решить с использованием МЭМС технологий. Микроэлектромеханические системы (МЭМС) — устройства, объединяющие в себе микроэлектронные и микромеханические компоненты.

Механическим компонентом может быть миниатюрное зеркальце — элемент системы сканирования, либо примитивный инерциальный датчик, способный определить характерные движения, которые получаются при работе устройства [2].

К МЭМС технологиям относятся такие датчики как акселерометр (датчик ускорения), гироскоп (датчик, отслеживающий угловые перемещения), магнетометр (датчик, определяющий характеристики магнитного поля), электронный барометр (датчик для измерения давления).

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

Конструкция мобильного робота

Система управления

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

Были подобраны соответствующие комплектующие: гироскоп и акселерометр  - MPU6050, программируемый микропроцессор - ATmega 1280. Так же используется микросхема преобразователь интерфейсов FT232RL, стабилизаторы питания, и 2 транзисторных Н-моста для управления коллекторными моторами. Структурная схема представлена на рисунке 1.

Рисунок 1. Структурная схема системы управления

В итоге получилась система управления со следующими возможностями:

1. Подключение к компьютеру и совместимость с популярной средой разработки Arduino IDE (Определяется как Arduino Mega 1280).
2. Наличие акселерометра и гироскопа датчика.
3. 2 программируемых контроллера коллекторных двигателей.
4. Выход шины I²C
5. 2 UATR порта.
6. SPI порт.
7. 11 портов ввода/вывода, 4 из которых можно использовать как аналоговые входы, а остальные как выходы ШИМ сигнала (Широтно-импульсная модуляция).
8. Выход 5 Вольт от импульсного стабилизатора напряжения для подключения внешних нагрузок с током потребления до 3 Ампер.

Шасси

В качестве шасси была приобретена платформа на гусеничном ходу, T-Rex metal robot tank chassis. На данной платформе достаточно легко можно установить всю необходимую электронику и источник питания.

Программное обеспечение

После загрузки в ATmega1280 программы для калибровки и считывания данных ускорения по оси y с MPU6050, на экране отображаются различные численные значения, при этом сам датчик находится в неподвижном состоянии. Такой разброс оправдываются большой погрешностью выходных значений. Поэтому для стабильной работы необходимо фильтровать полученные значения. Фильтр Калмана [3] показался наиболее простым вариантом.

Фильтр Калмана выполняет обработку значений в 2 этапа:

1. Предсказание
1. Предсказание состояния системы:
2. Предсказание ошибки ковариации:
2. Корректировка
1. Вычисление усилия Калмана:
2. Обновление оценки с учётом измерения z_k:
3. Обновление ошибки ковариации:

Пояснение к формулам фильтра Калмана:  – предсказание состояния системы в текущий момент времени;  – состояние системы в предыдущий момент времени; F – матрица перехода между состояниями; B – матрица применения управляющего воздействия;  – управляющее воздействие в прошлый момент времени;  – предсказание ошибки;  – ошибка в прошлый момент времени; Q – ковариация шума процесса;  – усилие Калмана; H – матрица измерений; R – ковариация шума измерения; z_k – измерение в текущий момент времени; I – матрица идентичности.

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

Рисунок 2. Работа фильтра Калмана

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

Чтобы найти скорость по оси y применяется формула 1.

                                                          (1)

Перемещение по оси y определяется формула 2.

                                                 (2)

Пояснение к формулам 1 и 2: a_y – ускорение по оси y; dt - период интегрирования; v_y – текущая скорость по оси y; v_y-1 – скорость по оси y за предыдущее интегрирование; S_y – текущий путь по оси; S_y-1 – путь по оси y за предыдущее интегрирование; t – временной промежуток между предыдущим интегрированием и текущим. Аналогичная формула для оси x и z.

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

Эксперимент

Задача: проехать 1 метр, используя данные с акселерометра.

Алгоритм:

1. В начальный момент времени задать скорость 15% от максимальной.
2. Программно считывать данные с акселерометра, фильтровать и интегрировать в процессе пути.
3. Зафиксировать пройденный путь.
4. Повторить пункты 1-3 ещё раз.
5. Повторить пункты 1-4 на скорости 30% и 50% от максимальной.
6. Сравнить полученные результаты и сделать выводы.

Пройденный путь будет измеряться системой захвата движения Motion Capture. На роботе закреплены световозвращающие маркеры для обнаружения системой.

Рисунок 3. Система Motion Capture и робот.

Робот перемещался вдоль оси y акселерометра. В итоге получилось 6 различных траектории. На рисунке 4 представлены конечные точки.

Рисунок 4. Конечные точки траекторий

Точки 4, 6 были достигнуты на 15 % от максимальной скорости робота; точки 2, 5 – на 30 %; точки 1, 3 – на 50 %.

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

Расчёт погрешности был произведён по формуле (3):

                                                                          (3)

Пояснение к формуле 3:  – погрешность работы акселерометра по оси y;  - средняя дистанция; – i-тая дистанция; p – коэффициент Студента; n – число измерений. Аналогично рассчитывается для погрешности по оси x. Коэффициент Студента взят для 6-ти измерений с доверительной вероятностью 95 %.

Получились следующие доверительные интервалы:

1.  см.
2.  см.

На рисунке 5 изображён доверительный интервал на графике.

Рисунок 5. Доверительный интервал

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

Заключение

Метод интегрирования данных с акселерометра не очень хорошо себя показал. В данной работе был применён достаточно дешёвый МЭМС датчик положения, поэтому его погрешность оправдана. При использовании дорогих и точных акселерометров ошибка будет накапливаться в разы меньше. Так же удобно комбинировать данные датчики с датчиками, которые были описаны в начале статьи для совместной работы. При таком методе датчики будут дополнять друг друга.

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

1. Siegwart and Nourbaksh, “Mobile Robot Localization,” Introd. to Auton. Mob. Robot., 2004.
2. Gang Zhang. Design and Simulation of a CMOS-MEMS Accelerometer, Carnegie Mellon University.
3. Nabil Zhafri Mohd Nasir, Muhammad Aizzat Zakaria, Saifudin Razali, Mohd Yazid bin Abu. Autonomous mobile robot localization using Kalman filter. Universiti Malaysia Pahang.
4. Фрайден Дж. Современные датчики. Справочник / Дж. Фрай- ден. – М.: Техносфера, 2005. – 592 с.