Васильев Иван Анатольевич

Санкт-Петербург, ЦНИИ РТК, нач. лаб., к.т.н

Локализация МОБИЛЬНОГО РОБОТА с использованием бесплатформенной инерциальной навигационной системы (БИНС)

 

«Инерциальная навигация — метод определения координат и параметров движения различных объектов (судов, самолётов, ракет и др.) и управления их движением, основанный на свойствах инерции тел и являющийся автономным, т. е. не требующим наличия внешних ориентиров или поступающих извне сигналов» [1, 3, 4].

БИНСы, применяемые в робототехнике используют в своём составе, как правило, шесть датчиков. Три – линейного ускорения (датчик линейного ускорения – ДЛУ) и три – угловой скорости (датчик угловой скорости – ДУС).

Для робота, снабжённого качественными одометрами (датчиками колёс) ДЛУ не дадут требуемой точности, поэтому их применение очень ограничено. Но для криволинейного движения требуется применять ДУС. С каждого ДУС поступает на робота значение угловой скорости w. Расчёт угла производится путём интегрирования угловой скорости:

Ai+1 = Ai + wi Δt                                                                         (1)

где                Аi+1 – текущий угол;

      Аi – предыдущий угол;

      wi – отмасштабированное показание ДУС;

      Δt – дискрета времени.

Формула (1) отражает идеальную ситуацию, когда в случае покоящегося робота угловая скорость w, поступающая с ДУС, строго равна нулю. В реальности это совсем не так – см. рисунок 4.2.1

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

Рисунок 4.2.1 – Типичная картина информации, поступающей с ДУС

 

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

Рассмотрим график на рисунке 4.2.2.

Рисунок 4.2.2 – Проинтегрированные показания ДУС

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

Для уравнения прямой , используя МНК (метод наименьших квадратов), можно получить следующие формулы:

где                                                                                                                                                                             (4.2.2)

                

     

      N – количество отсчётов;

      tj – j-тый момент времени;

      ωj – j-тое показание ДУС.

      Для иллюстрации того, что параметры прямой рассчитываются верно, приведём рисунок 4.2.3, на котором тонкая прямая вычисляется по параметрам, найденным по формулам (4.2.2).

Рисунок 4.2.3 – Аппроксимация массива точек прямой

Рисунок 4.2.4 – Демонстрация реальных поворотов робота

На рисунке 4.2.4 продемонстрирован график интегрированных показаний ДУС для поворотов робота. От точки О до точки 1 – робот в покое, от точки 1 до точки 2 робот поворачивается на +90 градусов, далее, от точки 2 до 3 – робот опять покоится, от 3 до 4 – опять поворот на +90 градусов и т.д.

Для того, чтобы учесть смещение нуля при повороте, требуется во время поворотов от вычисленного значения вычесть интегрированное за время стоянки смещение нуля, вычисляемое по формуле , с параметрами, вычисленными по формулам (4.2.2).

 

Итак, алгоритм действия такой:

0.                На каждом такте управления производится:

a.                К счётчику времени st прибавляется период, прошедший с предыдущего такта;

b.               К сумматору ДУС  прибавляется текущее показание угловой скорости;

1.                Робот остановился, то есть, скорости от ненулевых стали нулевыми. Устанавливаются в ноль:

a.                Счётчик времени st;

b.               Счетчик тактов;

c.                Сумматор ДУС ;

2.                Скорости нулевые. Производится вычисление (см. формулы (4.2.2)):

a.                sumx2i;

b.               sumy2i1;

c.                sumx2i1;

d.               sumy2ix2i;

e.                sumsqrx2i;

3.                Скорости стали ненулевыми. Производится вычисление (см. формулы (4.2.2)) параметров a и b;

4.                Скорости ненулевые – робот едет. Производится вычисление текущего угла по формулам:

где         нужно по той причине, что в п. 1 сумматоры и счётчики сбрасываются в ноль;

5.                По прошествии некоторого времени требуется остановить робот для перекалибровки. При этом последовательно будут выполняться пп. 1-3.

 

 

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

 

1. Springer handbook of robotics // под. ред. Bruno Siciliano, Oussama Khatib, «Springer», 2008.

2. Васильев И. А. Управление траекторным движением мобильных роботов разных колёсных схем. // «Экстремальная робототехника». Сборник докладов международной научно-технической конференции, 23-25 ноября 2011

3. Бурдаков С. Ф., Мирошник И. В., Стельмаков Р. Э. Системы управления движением колесных роботов. СПб; «Наука», 2001.

4. http://ru.wikipedia.org/wiki/Инерциальная навигационная система