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

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

Коган Лев Борисович

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

УПРАВЛЕНИЕ МОБИЛЬНЫМ РОБОТОМ, СНАБЖЁННЫМ УЛЬТРАЗВУКОВЫМИ ДАЛЬНОМЕРАМИ

Введение

В данной статье рассматриваются методы управления колёсным мобильным роботом с танковой компоновкой шасси снабжённого ультразвуковыми дальномерами и бесплатформенной инерциальной навигационной системой (БИНС). Фото робота приведено на рисунке 1. Фото модуля трёх УЗ-дальномеров приведено на рис. 2.

Рисунок 1 – колёсный мобильный робот

Рисунок 2 – блок из трёх ультразвуковых дальномеров

Рисунок 3 – Система координат робота (вид сверху)

Данный робот снабжён датчиками колёс (одометрами), модулем из трёх ультразвуковых дальномеров и бесплатформенной инерциальной навигационной системой (БИНС).

Главная цель работы – движение робота в заданную точку по плоской и горизонтальной поверхности. Для достижения этой цели требуется решить следующие задачи:

1. Локализация робота в некоторой системе координат;

2. Построение бинарных карт рабочей зоны;

3. Построение траекторий для движения робота.

Рассмотрим эти задачи последовательно.

Локализация робота.

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

С вертикального ДУС поступает на робота значение угловой скорости w_z. Расчёт курсового угла производится путём интегрирования угловой скорости

A_i+1 = A_i + w_zi Δt

Для упрощения управления траектория робота строится как ломаная линия, состоящая из прямолинейных отрезков. Следовательно, во время поворота курсовой угол A даёт вышеприведённый алгоритм, а линейное перемещение получается из одометров. Исходя из этого, производится счисление координат робота (X, Y, A) следующим образом:

X₀ = X_{начальное};

Y₀ = Y_{начальное};

A₀ = A_{начальное};

X_i= X_i-1+Δx_iCos A_i – Δy_iSin A_i;

Y_i= Y_i-1+Δy_iCos A_i + Δx_iSin A_i;

где (X_{начальное}, Y_{начальное}, A_{начальное}) – начальные координаты робота, в которых робот находился при старте;

A_i - текущий курсовой угол.

Для пояснения рассмотрим систему координат, связанную с роботом – см. рисунок 3.

Кинематическая система робота такова, что он не имеет возможности перемещаться вдоль оси Х, поэтому величина приращения Δx всегда равна нулю.

Для описания решения двух следующих задач надо подробнее рассмотреть блок ультразвуковых дальномеров (см. рис. 2). В этом блоке установлены три одинаковых дальномера под углом друг к другу на 15 угловых градуса – см. рис. 4.

Рисунок 4 – Блок УЗ-дальномеров. 1, 2 и 3 – номера дальномеров. Диаграмма направленности показана для центрального дальномера.

Рисунок 5 – Иллюстрация метода измерения размеров препятствий

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

Построение карт рабочей зоны.

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

Координаты точек А и В можно вычислить, исходя из следующей системы уравнений:

x Cos q - y Sin q = w/2;

x² + y² = d_A²;

Решая эту систему, получаем:

x= w Sin α - Ctg α (Sin²α(d_A² - w²))^1/2;

y = - w Cos α - (Sin²α(d_A² - w²))^1/2;

и аналогично для точки В:

x=w Sin α + Ctg α (Sin²α(d_B² - w²))^1/2;

y = w Cos α - (Sin²α(d_B² - w²))^1/2;

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

Пусть текущее положение робота X, Y и угол ориентации А. Тогда окончательно получаем координаты препятствия:

X_преп = x Cos A – y Sin A + X;

Y_преп = x Sin A – y Cos A + Y;

Теперь эти препятствия можно наносить на карту, как показано в работе [3].

Здесь надо сказать следующее. Построение карт для данного типа роботов осуществляется не точно по трём причинам. Во-первых, ультразвуковая волна, в отличие от оптической (лазерной), сильно чувствительна к плотности воздушного пространства, в котором она перемещается. Любое изменение плотности отклоняет волну от прямолинейной траектории. Так как определить это отклонение невозможно, то диаграмма направленности в данный конкретный момент известна лишь приблизительно. Это приводит к тому, что точки А и В (границы препятствия на рисунке 5) становятся известны неточно. Здесь можно производить измерения многократно и потом вычислить среднее значение. Это в теории. На практике же, робот не имеет времени в разных «узловых» точках производить многократные измерения (одно такое измерение может производиться в течение 20-40 секунд).

Во-вторых, по причине малой дальности измерений УЗ-дальномера (<5 метров, что даже для помещений немного), приходится часто перемеряться. При этом накладываются ошибки локализации на ошибки УЗ-дальномера, что приводит к ещё большим погрешностям определения координат препятствий.

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

Построение траекторий движения.

Общие алгоритмы построения траекторий на бинарной карте хорошо известны. Например, они подробно описаны в статье автора [4]. Но для робота, описываемого в данной статье, эти методы плохо применимы по причинам, перечисленным в разделе построения карты.

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

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

Итак, алгоритм.

1. Вначале проверяется расстояние до цели и, если оно меньше некоторого, то считается, что цель достигнута;

2. Если цель не достигнута, то производится доворот робота на цель;

3. Далее производится движение по прямой на 1 метр, после чего переход к п. 1, либо пока расстояние до непреодолимого препятствия не станет меньше некоторого заданного расстояния;

4. Маневрируем для объезда препятствия: если дальномер слева показывает большее расстояние, то едем влево, если справа – вправо, пока нет возможности проехать, т.е. центральный дальномер не показывает возможность движения больше заданного расстояния, в этом случае переход к п. 3. Если все три дальномера показывают невозможность движения, то переход к п. 5;

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

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

На рисунке 6 показаны две оси: абсцисс и ординат, точки старта и конечная, буквой Т обозначена траектория, цифрами 1 и 2 – препятствия. Как видно, вначале робот едет на препятствие, затем, когда препятствие попадает в зону безопасности, робот начинает объезд препятствия. Затем, объехав препятствие, робот разворачивается в сторону цели и едет прямо. Далее робот доворачивает уже строго на цель и вскоре финиширует. График траектории, по которой робот перемещался во втором примере, приведён на рисунке 7.

Рисунок 6 – траектория движения робота

Рисунок 7 – траектория движения робота

Заключение.

Многочисленные эксперименты, проведённые с описываемым роботом, показали хорошую устойчивость разработанных алгоритмов. Бесплатформенная инерциальная навигационная система работает с малыми уходами (порядка 1 углового градуса за 1 часовую минуту), что вполне допустимая погрешность (за 10 минут уход составит порядка 10 градусов, что на 10 метрах даст погрешность не более 1.7 м).

Алгоритм построения карты устойчив даже к большим уходам БИНС (если недостаточное время калибровать БИНС, то уход может быть спорадическим и очень большим – до 20 градусов в секунду!).

Алгоритм объезда препятствий работает настолько устойчиво, что «загнать» робота в тупик ни разу не получилось.

Единственное неудобство заключается в ненадёжности ультразвуковых дальномеров. Как уже было отмечено выше, робот «не видит» поверхности под большим углом к нормали. Также, УЗ-дальномеры часто имеют ложные срабатывания по причине неровности и шероховатости пола, по которому перемещается робот.

1. П.В.Бромберг. Теория инерциальных систем навигации. М., «Наука» - 1979.

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

3. И.А.Васильев. Построение траекторий движения для колёсного мобильного робота, снабжённого лазерным сканирующим дальномером. // Труды XXI Международной научно-технической конференции «Экстремальная робототехника» - 2010.