ИСПОЛЬЗОВАНИЕ АКСЕЛЕРОМЕТРА И ГИРОСКОПА ДЛЯ АВТОНОМНОГО ДВИЖЕНИЯ МОБИЛЬНОГО РОБОТА

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

Ключевые слова: Инерциальная навигация, автономное движение робота.

Введение

В наше время точность для роботов – главное, любые отклонения от заданной траектории необходимо учитывать. Но порой это бывает невозможно из-за сложности предвидения поведения робота при различных природных условиях, таких как: туман, гололёд, ветер. Большинство современных датчиков, устанавливаемых на мобильных роботов, взаимодействуют с окружающей средой. Это могут быть датчики положения колеса (энкодеры), системы оптического позиционирования и т.п. Данные измерительные системы не будут работать при наличии одного или нескольких вышеперечисленных природных условий. Тут в дело могут вступить спутниковые системы навигации, такие как GPS, ГЛОНАСС. Но и они могут дать сбой при наличии электромагнитных помех, например, вблизи ЛЭП, а если речь идёт о космических аппаратах, то о спутниковой навигации говорить не приходится. Существует вариант решения данной проблемы – это применение инерциальной навигации.

Инерциальная навигация – метод навигации (определения координат и параметров движения различных объектов - судов, самолётов, ракет и др.) и управления их движением, основанный на свойствах инерции тел, являющийся автономным.

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

1. Датчик ускорения, акселерометр.
2. Датчик угловой скорости, гироскоп.
3. Вычислительное устройство, например, компьютер или микроконтроллер.

Совокупность вышеперечисленных устройств образует инерциальную систему навигации (далее ИСН).

В данной статье будет описан пример применения инерциальной навигации для автономного движения мобильного робота по заданной траектории. Эта работа является продолжением исследований из статьи [1].

Основная часть

В Данной работе применялся робот с дифференциальным приводом. Его конструкция достаточно простая: два независимых двигателя с колёсами, основная рама, на которой крепятся датчик положения BNO055 и плата Arduino MKR1000 Wi-Fi. Датчик положения включает в себя акселерометр, гироскоп и компас-магнетометр.

Рисунок 1. Схематичное представление робота

На рисунке 1 изображено: ,  – линейные скорости колёс,  – курсовая линейная скорость,  – угол между заданной целью и осью OX,  – угол между курсом робота и осью OX,  – угловая скорость поворота робота. Движения робота можно контролировать при помощи независимого регулирования скорости вращения его колёс. Рассмотрим описание математической модели.

                                                          (1)

Где ,  – скорость изменения соответствующих координат,  - скорость изменения соответствующего угла,  – расстояние между колёсами.

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

                            (2)

Рассмотрим формулу 2 подробнее. Скорость является произведением максимальной линейной скорости  на гиперболический тангенс от оставшегося расстояния до цели (разность между  – длиной от начальной точки до цели и  – пройденной дистанцией). Гиперболический тангенс позволяет плавно снизить скорость вблизи целевой точки, а при достижении её – полостью остановиться. Изменение угла  выполнено на основе ПИ регулятора, компенсирующего разность между требуемым углом  и текущим углом . Коэффициенты P и I рассчитывались экспериментально.

Зная ,  можно определить линейные скорости колёс , :

                                                (3)

Где R - радиус колеса.

Таким образом, для автономного движения робота вычисляются 4 переменные:  – длина от начальной точки до цели по формуле (4), – требуемый угол поворота по формуле (5),  - пройдённая дистанция,  – текущий угол поворота.

                                             (4)

                                                      (5)

Где  – координаты целевой точки,  - координаты начальной точки.

 и  вычисляются исходя из данных от ИСН, об этом поговорим далее.

Рассмотрим алгоритм обработки полученных от ИСН данных. Начнём с данных акселерометра. Акселерометр измеряет ускорение, в данном случае, по трём осям: OX, OY, OZ. При горизонтальном положении (оси OX и OY параллельны горизонту) на данных с оси OZ будет отображаться ускорение свободного падения . При изменении углового положения акселерометра (оси OX и OY не параллельны горизонту) ускорение свободного падения будет проецироваться на оси OX и OY, что негативно сказывается при измерениях линейного перемещения. Возникает необходимость компенсировать гравитационную составляющую (ускорение свободного падения) в данных датчика. Сделать это можно двумя способами:

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

где  – ускорение по любой из осей в момент времени t,  – переменная для компенсации гравитационной составляющей в момент времени t,   – коэффициент скорости компенсации,

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

                                               (8)

где  – ускорение по оси OX в момент времени t,  – ускорение свободного падения,  – угол поворота акселерометра по оси OX, то есть вокруг оси OY, в радианах.

Данная формула применима и к данным акселерометра по оси OY и OZ.

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

Ускорения по осям OX и OY складываются, и получается полное линейное ускорение:

                                                (9),

где – ускорение в плоскости YOX в момент времени t, – ускорение оси по OX в момент времени t,  – ускорение оси по OY в момент времени t,

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

                  (10) – математическая форма записи.

     (10) – дискретная форма записи.

                  (11) – математическая форма записи.

     (11) – дискретная форма записи.

где  – ускорение в момент времени t,   – скорость в момент времени t,  – пройденная дистанция на момент времени t, - временной промежуток между t и t-1.

Далее рассмотрим обработку полученных данных с гироскопа. Гироскоп измеряет угловую скорость, в данном случае, в трёх плоскостях: YOX, YOZ, XOZ. Для определения угла поворота робота полученные данные необходимо проинтегрировать в соответствии с формулой (12).

                  (12) – математическая форма записи.

     (12) – дискретная форма записи.

где  – угловая скорость в момент времени t,  – угол поворота в плоскости YOX вокруг оси OZ в момент времени t, - временной промежуток между t и t-1.

Данные с гироскопа об углах в плоскостях XOZ и YOZ используются для компенсации гравитационной составляющей в формуле (8). При программной реализации используется дискретная форма записи для формул (10), (11) и (12).

Проверим работоспособность ранее описанной теории на практике. Робот должен проехать траекторию, изображённую на рисунке 2. В его памяти записаны координаты точек A, B и C. В начальный момент времени считается, что робот находится в начале координат, точка 0, и его направление совпадает с направлением оси OX.

Рисунок 2. Требуемая траектория робота

Было проведено несколько тестов, при которых робот проезжал траекторию на разных скоростях и с разными режимами: без остановок от начала до конца и с остановками на каждой точке. На рисунках 3-6 представлены результаты одного из теста.

Рисунок 3. Робот в точке 0           Рисунок 4. Робот в точке A

Рисунок 5. Робот в точке B               Рисунок 6. Робот в точке C

Как видно из рисунков 3-6, существует некоторая погрешность при достижении точек. Робот весьма точно отрабатывал повороты (погрешность составила ). Тесты повторялись с разной скоростью. Наилучший результат получился при максимальной скорости и без остановок, в этом случае ошибка положения получалась минимальной. Связано это с неточностью измерения линейных ускорений и значительным влиянием вибрации на акселерометр. Возникает разность между реальным ускорением и ускорением из данных акселерометра, и при интегрировании по формулам (10) и (11) накапливается ошибка текущей скорости и пройденного пути. Этим обусловлено неточное достижение точек. На максимальной скорости робот преодолевает траекторию быстрее, следовательно, промежуток времени, за который накапливается ошибка, становится меньше, что способствует более точному прохождению траектории.

Минимизировать разность между реальным ускорением и ускорением из данных акселерометра возможно. Это достигается следующими способами:

1. Путем изолирования датчика от воздействия вибраций.
2. Использованием различных фильтров, применение которых описано в статье [1].
3. Введением поправочных коэффициентов, вычисленных исходя из результатов экспериментов.

Заключение

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

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

1. Файздрахманов Д.Г. Применение МЭМС технологий для определения координат мобильного робота в пространстве.
2. Фрайден Дж. Современные датчики. Справочник / Дж. Фрай- ден. – М.: Техносфера, 2005. – 592 с.
3. Певзнер Л.Д. Практикум по теории автоматического управления – М.:Высш.шк, 2006.