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

Page 1

ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
83
УДК 62-503.57
В.Н. Герасимов, Б.Б. Михайлов
РЕШЕНИЕ ЗАДАЧИ УПРАВЛЕНИЯ ДВИЖЕНИЕМ
МОБИЛЬНОГО РОБОТА ПРИ НАЛИЧИИ
ДИНАМИЧЕСКИХ ПРЕПЯТСТВИЙ
Рассматривается способ управления движением мобильного робота
по заданной траектории при наличии подвижных и неподвижных
препятствий. Для этого используется известный алгоритм А* (А-
звезда), который был модифицирован авторами для решения постав-
ленной задачи.
E-mail: robot@bmstu.ru
Ключевые слова: мобильный робот, алгоритм управления дви-
жением, статическое препятствие, динамическое препятствие.
В настоящее время все более популярными становятся сервисные
мобильные роботы. К данному типу можно отнести роботы-
экскурсоводы, роботы-пылесосы, подвижные информационные тер-
миналы. Для выполнения своих функций эти роботы должны уметь
перемещаться по определенным траекториям в автоматическом ре-
жиме. Обычно в современных роботах для этой цели используется
навигационная система, которая определяет собственные координаты
робота, планирует траекторию в текущий момент времени и управля-
ет его движением. Поскольку реальная среда, в которой находится
робот, обычно содержит подвижные препятствия (люди, другие мо-
бильные роботы), движение в ней по предварительно заданной траек-
тории практически невозможно.
Постановка задачи. В данной статье рассмотрено решение зада-
чи управления движением мобильного робота при наличии подвиж-
ных (динамических) препятствий. Задача формулируется следующим
образом. Имеется кусочно-линейная траектория от начальной точки
до целевой. Она может корректироваться в реальном времени с ис-
пользованием алгоритма А* (А-звезда). Система навигации мобиль-
ного робота отслеживает его положение и вычисляет координаты те-
кущей опорной точки, к которой должен двигаться робот, избегая
столкновений с препятствиями (статическими и динамическими).
Статические препятствия. Рассмотрим решение задачи в усло-
виях статической среды как более простое. В его основе лежит метод
эффективного пути [1]. В данном случае эффективный путь – это от-
УПРАВЛЕНИЕ ДВИЖЕНИЕМ
РОБОТОВ

---

Page 2

ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
84
резок прямой, соединяющий текущее положение робота с текущей
опорной точкой.
В [1] в качестве опорной
точки использован второй из-
лом кусочно-линейной траек-
тории (рис. 1), поскольку одного
изменения направления траекто-
рии с дискретным шагом в 45°
относительно положения робота
недостаточно для того, чтобы
достоверно определить тенден-
цию дальнейшего изменения тра-
ектории. Вторая смена направле-
ния показывает устойчивость из-
менения заданной траектории
движения. В противном случае
робот продолжает движение в
выбранном направлении.
Расстояние
R
между опорной точкой на траектории и текущим
положением робота определяется его динамикой. Сверху значение
R
ограничено максимальной скоростью перемещения
c
a
v
v t
+ Δ , кото-
рая может быть достигнута роботом через интервал времени
:tΔ
( ) (
)
max
max
,
c
c
a
R
v
v
v t T
=
+ Δ
где
a
v – максимально допустимое ускорение робота:
c
v – текущая
скорость перемещения робота;
max
T
– время, за которое робот до-
стигнет ближайшее препятствие, находящееся на расстоянии
l
s ,
с максимальной скоростью
max
v
:
max
max
,
l
s
T
v
=
т. е. длина эффективно-
го пути зависит от скорости робота.
Если изменение направления траектории обнаружено в непосред-
ственной близости от робота (например, клетка карты, следующая за
текущим положением робота), это изменение является незначитель-
ным для робота. Поэтому расстояние между опорной точкой на тра-
ектории и текущим положением робота ограничено снизу постоян-
ной
min
R , которая отфильтровывает незначительные изменения
направления траектории. Такие изменения замедляют движение ро-
бота, но почти не меняют его положение. Наиболее эффективно
установить значение
min
R
в соответствии с максимальным временем
Рис. 1. Определение опорной точки:
1 – положение робота; 2 – мобильный
робот; 3 – текущий курс робота; 4 – эф-
фективный путь; 5 – опорная точка; 6 –
заданная траектория; 7 – препятствия

---

Page 3

ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
85
торможения
max
max
b
b
v
T
v
=
для того, чтобы робот мог остановиться на
расстоянии, равном длине минимального эффективного пути, если
изначально он достиг максимальной скорости:
max
max
2
min
max
1
.
2
b
b b
R
v T
v T
=
−
Отсюда следует, что минимальная скорость движения
min
min
max
R
v
T
=
.
Итак, метод эффективного пути дает координаты опорной точки
на траектории, к которой должен стремиться робот, двигаясь по за-
данному маршруту. При этом траектория становится более гладкой и
робот может столкнуться с препятствием. Столкновение возможно и
в том случае, если некорректно заданы габариты робота в алгоритме
поиска пути. Чтобы избежать подобных ситуаций, метод эффектив-
ного пути был модифицирован. Суть модификации заключается в
том, чтобы искусственно сместить опорную точку из «запрещенной»
зоны, если она туда попадает. Рассмотрим предлагаемый способ рас-
чета скорректированного положения опорной точки.
Пусть имеется карта помещения (рис. 2). На ней показаны траек-
тория движения робота 3 и опорная точка E, координаты которой вы-
числены по методу эффективного пути. В окрестности мобильного
робота находятся препятствия, обозначенные на карте занятыми
ячейками 5. Граница робота задается окружностью, радиус W кото-
рой определяется габаритами робота, зоной безопасности и размером
ячейки карты заполнения.
Сначала задается область, в которой необходимо учитывать пре-
пятствия, например
c
c
N N
×
ячеек. Затем каждая ячейка внутри этой
области проверяется на наличие препятствия. Если препятствие об-
наружено, координаты опорной точки пересчитываются с учетом
следующих условий:
• в новой опорной точке робот не должен сталкиваться с препят-
ствием;
• длина эффективного пути должна сохраняться.
Рассмотрим треугольник OEP (рис. 3, а). Здесь точка O – центр
робота; E – опорная точка; P – центр ячейки с препятствием; OP –
эффективный путь длиной R. Сначала проверяем возможность столк-
новения робота с препятствием в опорной точке. Для этого рассчи-
тываем расстояние d между опорной точкой E и точкой P:
.
d =
=
−
EP
OE OP

---

Page 4

86
Рис. 2. Ре
1 – мобиль
ные клетки
Если
рируется
точки. Р
щая робо
Рис. 3. Ра
а – до кор
ектория; 3
ISSN 0236-393
езультат оп
ьный робот;
и; 5 – заняты
и
,
d W
≥
пр
я. В против
ассмотрим
от, касаетс
асчетная сх
рректировки;
– эффективн
33. Вестник М
пределения
2 – эффектив
е клетки
репятствие
вном случа
м граничны
ся препятст
хема алгори
б – после ко
ный путь; 4 –
МГТУ им. Н.Э.
опорной то
вный путь; 3
находится
ае выбирает
ый случай, к
твия, т. е.
итма объезд
орректировки
– габаритная о
Баумана. Сер.
очки:
3 – заданная т
я далеко от
тся новое п
когда окру
.
d W
=
Из
да препятст
и (1 – препятс
окружность м
“Приборостро
траектория; 4
робота и о
положение
ужность, оп
рис. 3, а о
твий:
ствие; 2 – зад
мобильного р
оение”. 2012
4 – свобод-
оно игно-
е опорной
писываю-
очевидно,
данная тра-
робота)

---

Page 5

ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
87
что новая опорная точка E* должна находиться на пересечении
окружностей (P, W) и (O, R). Координаты новой точки находим, зная
угол ,
ϕ
под которым направлен вектор
OP
(рис. 3, б), и определив
угол
POE
∗
=
α
из теоремы косинусов:
( )
2
2
2
2
cos
.
W
R
R
ρ
ρ
α
=
+
−
⋅
(1)
Отметим, что уравнение (1) имеет два решения, отличающиеся
знаком. Эти решения соответствуют положению опорной точки слева
и справа от препятствия. Положительное решение уравнения (1) име-
ет вид
2
2
2
arccos
.
2
R
W
R
ρ
α
ρ
⎛
⎞
+
−
=
⎜
⎟
⎝
⎠
(2)
Координаты новой опорной точки E* относительно препятствия
зависят от расположения старой опорной точки E. Зная векторы
OE
и
,
OP
определим ориентацию эффективного пути и найдем скоррек-
тированный вектор
:
∗
OE
(
)
(
)
(
)
(
)
(
)
(
)
cos
, sin
,
0,
cos
, sin
,
0.
R
R
R
R
ϕ α
ϕ α
ϕ α
ϕ α
∗
⎧
−
−
×
>
⎪
=
⎨
+
+
×
≤
⎪
⎩
OE OР
OE
OE OР
(3)
По мере движения робота рассчитывается следующая опорная
точка и сравнивается с предыдущей. Из всех точек выбирается та,
которая наиболее удалена от препятствия. Проверка удаленности
проводится отдельно для точек, расположенных слева и справа от
препятствия. Отметим, что робот начнет объезжать препятствие
непосредственно перед столкновением с ним.
Итак, предлагаемая модификация метода эффективного пути поз-
воляет определить координаты текущей опорной точки E* (см. рис. 2)
и обеспечить движение робота по заданной траектории без столкно-
вения с неподвижными препятствиями.
Динамические препятствия. Перейдем к задаче определения
координат опорной точки при наличии движущихся препятствий.
Учет параметров динамических препятствий, с одной стороны, поз-
воляет избежать столкновений с ними, а с другой – оптимизировать
траекторию движения. Рассмотрим простой пример, показанный на
рис. 4. Пусть робот 1 движется по прямой на некотором участке
спланированной траектории. При этом робот 2 (динамическое пре-
пятствие) движется по пересекающему маршруту, являясь помехой
для первого.

---

Page 6

ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
88
Рис. 4. Постановка задачи обхода динамического препятствия:
1 – мобильный робот; 2 – динамический объект (препятствие); 3 – заданная траек-
тория движения робота; 4 – траектория движения динамического объекта; 5 – ста-
тические препятствия
Пусть имеются параметры движущегося объекта: его положение в
абсолютной системе координат, габаритные размеры, а также ско-
рость. Для решения поставленной задачи был использован модифици-
рованный метод эффективного пути, описанный выше. Только теперь
при наличии подвижных препятствий участок карты заполнения в
окрестности робота меняет свое содержимое в зависимости от времени
наблюдения и параметров движущихся объектов. В дальнейшем такой
участок карты заполнения будет называться динамическим.
Для обработки динамического участка действия должны осу-
ществляться в приводимой ниже последовательности:
1) используя параметры движущегося объекта (препятствия) в
текущий момент времени, вычисляем его параметры в последующие
моменты времени;
2) на динамическом участке карты очищаем клетки, расположен-
ные в зоне текущего положения объекта;
3) рассчитываем точку и время пересечения траектории препят-
ствия с траекторией робота;
4) добавляем на динамическом участке карты клетки, описываю-
щие препятствие в точке пересечения траекторий.
Выполнив перечисленные действия, можно найти опорную точ-
ку модифицированным методом эффективного пути, подставив
вместо исходного участка карты заполнения полученный динамиче-
ский участок.
Сначала рассмотрим получение значений параметров объекта в
произвольный момент времени. В статье [2] предложены две модели
подвижных объектов. Рассмотрим одну из них – модель робота-
препятствия. Согласно рис. 4, вектор состояния подвижного объекта
в текущий момент времени
0
t описывается следующим образом:

---

Page 7

ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
89
(
)
,
T
x y
v
W
ϕ
ω
=
s
(4)
где x, y – положение объекта в абсолютной системе координат;
ϕ
–
угол поворота объекта относительно оси X абсолютной системы ко-
ординат; v,
ω
– скорости перемещения и вращения объекта соответ-
ственно; W – радиус описанной вокруг объекта окружности.
Будем считать, что габаритный размер W объекта не меняется во
времени, т. е. объект является твердым телом с неизменяемой фор-
мой. Кроме того, примем допущение, что скорости ,v
ω
объекта по-
стоянны в окрестности
0
t . Тогда значение оставшихся переменных
вектора (4) в произвольный момент времени
t
можно вычислить с
помощью уравнений кинематики:
0
0
( )
( );
t
t
t
dt
t
ϕ
ω
ϕ
=
+
∫
(5)
0
0
( )
cos ( )
( );
t
t
x t
v
t dt x t
ϕ
=
+
∫
(6)
0
0
( )
sin ( )
( ).
t
t
y t
v
t dt y t
ϕ
=
+
∫
(7)
Отметим, что выражения (5–7) справедливы только в области,
близкой к t
0
(
0
t t≈ ).
Следующий этап – очистка клеток участка карты – является три-
виальной задачей. Однако необходимо уточнить, что с этого момента
и далее алгоритм работает не с картой, сохраненной в памяти, а с ко-
пией участка в окрестности положения робота (с динамическим
участком). На данном этапе всем клеткам, находящимся внутри
окружности с центром в точке
(
)
0
0
( );
( )
x t
y t
и радиусом W присва-
ивается состояние «свободна». Таким образом, если ранее объект был
добавлен на карту как статический, то после данной процедуры эта
область карты считается свободной.
Для расчета точки пересечения используется итерационный под-
ход. Так как интегрирование уравнений (5–7) проводится численным
методом, на каждой итерации имеется отрезок малой длины, описы-
вающий часть траектории движения объекта-препятствия. В качестве
текущей траектории робота принимается кусочно-линейный марш-
рут, данный в условии задачи, с допущением, что отклонение реаль-
ного движения робота от заданной траектории незначительно. Тогда

---

Page 8

90
для опре
ции реш
траектор
При этом
рации i :
Посл
участок
точке
X
для робо
находитс
торый бу
времени
выражен
наты буд
Чтоб
шения за
чии дина
рис. 5 по
ектории
объект, п
криволин
нают дви
Рис. 5. Ок
Было
зованием
пользова
видно из
ISSN 0236-393
еделения то
шается зада
рии препят
м время пе
0
X
t
t i
= + ⋅
ледним эта
карты. Дл
X
и радиус
ота реальны
ся в своем
удет наход
.
X
t
Иным
ний (2) и (3
дущего пол
бы провери
адачи упра
амических
оказаны два
(по прямой
препятству
нейной тра
игаться одн
кно програ
о проведен
м алгоритм
анием алго
з рис. 6, а,
33. Вестник М
очки перес
ача пересеч
тствия с к
ересечения
,tΔ где
tΔ
пом являет
ля этого кл
сом W при
ым препятс
текущем п
диться в то
ми словами
3) рассчиты
ложения по
ить работос
авления дв
препятств
а робота. П
й). Слева н
ующий дви
аектории. В
новременно
аммы модел
о два вариа
ма объезда с
оритма объ
в первом
МГТУ им. Н.Э.
ечения X =
чения теку
кусочно-ли
X
t опреде
– шаг инт
тся добавл
леткам вну
исваивается
ствием счи
положении
очке пересе
и, метод эф
ывает опор
одвижного
способност
вижением м
вий было п
Правый роб
находится в
ижению пер
В ходе мод
о.
лирования
анта модел
статически
ъезда дин
случае роб
Баумана. Сер.
(
;
X
X
x
y
=
ущего отре
инейной тр
еляется ном
тегрировани
ение клето
утри окруж
я состояни
тается не т
и, а некий м
ечения тра
ффективног
рную точку
объекта (п
ть предлож
мобильного
проведено
бот движет
второй роб
рвого робо
делировани
процесса уп
лирования:
их препятст
амических
бот разогн
“Приборостро
)
X
на каждо
езка интегр
раекторией
мером теку
ия.
ок на динам
жности с ц
ие «занята»
тот объект,
мнимый об
аекторий к
го пути с п
у, учитывая
репятствия
женного ме
о робота п
моделиров
тся по задан
бот – динам
ота. Он дви
ия оба робо
правления
первый – с
твий, второ
препятств
ался до ма
оение”. 2012
ой итера-
рируемой
й робота.
ущей ите-
мический
центром в
». Теперь
, который
бъект, ко-
моменту
помощью
я коорди-
я).
етода ре-
при нали-
вание. На
нной тра-
мический
ижется по
ота начи-
с исполь-
ой – с ис-
вий. Как
аксималь-

---

Page 9

ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
91
ной скорости. Затем на пути возникло препятствие, и робот начал по-
ворачивать, пытаясь его объехать. Однако маневр начался слишком
поздно и произошло столкновение в точке
X
t .
Рис. 6. Результаты моделирования:
а – при работе в статической среде; б – при наличии динамических препятствий
Во втором случае (рис. 6, б) робот начал разгоняться, но как
только появилось мнимое препятствие в точке пересечения траекто-
рий линейная скорость уменьшилась. При этом появились колебания
угловой скорости. Это означает, что опорная точка определялась в
каждый момент времени с разных сторон препятствия, замедляя тем
самым линейную скорость перемещения. После того как помеха ис-
чезла, робот продолжил движение в направлении к цели с макси-
мальной скоростью и достиг ее в момент .
f
t
Итак, поставленная задача управления движением мобильного
робота вдоль заданной траектории при наличии динамических пре-
пятствий решена. В качестве решения предложена модификация ме-
тода эффективного пути. Результаты моделирования подтвердили
работоспособность предложенного метода. Координаты опорных то-
чек, получаемые в реальном времени, можно использовать в системе
управления мобильным роботом для формирования траектории объ-
езда препятствий.

---

Page 10

ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
92
СПИСОК ЛИТЕРАТУРЫ
1. Seder M., M a cek K., Pet r ovi c I. An integrated approach to real-time mo-
bile robot control in partially known indoor environments // Industrial Electronics
Society, 2005. IECON 2005. 31st Annual Conference of IEEE. – 6 p.
2. F e r g u s o n D., D a r m s M., U r m s o n C., K o l s k i S. Detection, Prediction
and Avoidance of Dynamic Obstacles in Urban Environments // Proceedings of
the 2008 IEEE Intelligent Vehicles Symposium. – 2008. – June. – Р. 1149–1154.
Статья поступила в редакцию 28.06.2012