CC BY
601
УДК 629.7.05.001
А. А. Большаков, Д. Л. Лисицкий УПРАВЛЕНИЕ ДВИЖЕНИЕМ МОБИЛЬНОГО РОБОТА
Введение
В настоящее время бурно развивается раздел робототехники, занимающийся созданием мобильных роботов. Однако создать роботов, уверенно перемещающихся даже по ровной поверхности, на которой имеются непреодолимые для них препятствия, пока не удалось по ряду причин, в том числе и из-за несовершенства алгоритмов управления. Поэтому разработка алгоритмов управления мобильными роботами является актуальной задачей.
Пусть необходимо, чтобы робот переместился из точки А в точку В горизонтальной плоскости (рис. 1) по траектории, минимально отклоняющейся от прямой, соединяющей эти точки (перемещение по прямой линии невозможно из-за препятствий произвольной формы). Для ясности дальнейших рассуждений предположим, что препятствие сплошное и несимметричное относительно прямой АВ (рис. 1). Робот может огибать препятствие, обходя его с разных стороны, однако одно из направлений обхода будет требовать большего отклонения от прямой АВ и, соответственно, должно быть исключено. Кроме того, в точке траектории, наиболее удаленной от прямой АВ (точки С и С' на рис. 1), вектор скорости объекта должен быть ей параллелен и между роботом и препятствием должно оставаться некоторое заданное расстояние Уз.
Уз :
С'
Рис. 1. Иллюстрация постановки задачи
Для обнаружения препятствий местности на корпусе робота устанавливается специальный датчик. Им может являться лазерный дальномер с лучом, сканирующим в горизонтальной плоскости (ЛДС), угол сканирования которого равен 2фтах, причем фтах не меньше максимального угла, который может составлять траектория движения робота с прямой АВ. Предположим, что ось симметрии сектора сканирования луча ЛДС параллельна прямой АВ, а угол отклонения луча ЛДС от оси симметрии сектора - ф изменяется дискретно с интервалом Дф (рис. 1). Дальномер при каждом 7-м измерении определяет угол наклона луча ф7 и расстояние до препятствия 17, что позволяет вычислить координаты видимых дальномером точек препятствия в связанной с объектом системе координат. Всего за полупериод сканирования будет получена информация
о расстоянии до к = (2фтах / Дф) +1 точек препятствия. На корпусе робота установлены также два лазерных дальномера с неподвижными лучами, измеряющие расстояние от робота до препятствий в направлении перпендикулярном прямой АВ.
Положение центра масс робота в неподвижной системе координат АХУ (рис. 1), первые производные этих координат и ориентация робота в этой системе координат определяются с помощью комплексной инерциальной системы навигации и ориентации, корректируемой по показаниям дифференциальных СР^-датчиков.
Для реализации системы воспользуемся структурой (рис. 2), описанной в [1], которая позволяет разделить ее на две последовательно включенные подсистемы - «модель-вычислитель программного управления (ВПУ)» и «объект-регулятор» (рис. 3). Под моделью в этом случае понимается упрощенная математическая модель движения центра масс робота. Подсистема «модель-ВПУ» формирует программную траекторию, а подсистема «объект-регулятор» ее отслеживает.
Рис. 2. Исходная структура САУ Рис. 3. Эквивалентная структура САУ
Предположим, что продольная составляющая скорости центра масс робота стабилизируется специальным регулятором и поэтому постоянна. Тогда упрощенную математическую модель движения центра масс робота можно записать как кинематические дифференциальные уравнения плоского движения материальной точки:
dYM _ у . dVyM _ . dXM _ у _ con4+ (1)
dt _ y’M • dt ~ g"y ■ dt _Уш _• (1)
где YM, XM - координаты упрощенной модели объекта в системе координат АХУ.; УхМ, УуМ -
соответственно продольная и поперечная составляющие скорости упрощенной модели; ny -поперечная перегрузка модели; g - ускорение силы тяжести. В (1) ХМ, YM, VxM, VyM - координаты. ny - управление, которое далее будем называть программным управлением.
На координаты и управления упрощенной модели робота наложены ограничения:
— Уу max £ VyM £ Уу max ■ — ny max £ ny £ ny max. (2)
Первое из неравенств (2) ограничивает угол наклона программной траектории, второе -радиус кривизны траектории. Величины ограничений следует выбирать так, чтобы динамические свойства робота позволяли ему отслеживать «движение» модели.
Необходимо синтезировать алгоритм выбора программного управления ny, который определяет направление обхода препятствия и переводит объект (1), (2) из произвольного начального состояния в точку, расположенную на расстоянии Ys от точки препятствия, наиболее удаленной от оси АХ (рис. 1), с поперечной составляющей вектора скорости, равной нулю (VyM _ 0), таким образом, чтобы минимизировался функционал
¥
I _ jYMdt ® min. (3)
о
Можно показать, что минимизация функционала (3) в рамках модели (1), (2) требует создания системы с максимальным быстродействием. Тогда, согласно принципу максимума Пон-трягина и теореме об n-интервалах [2], оптимальное управление будет релейным и принимает только значения (ny max,0, -ny max).
Для решения задачи будем использовать методику, описанную в [2], согласно которой алгоритм выбора оптимального управления синтезируется исходя из анализа положения изображающей точки относительно фазового портрета оптимального переходного процесса.
Введем новые переменные:
xi = Xпі - xm ; Уі = Ym + Y3sign(Ym) - ym ; vy = -V
yM ■
(4)
где Xш , Yпi - координаты і-й точки препятствия местности в неподвижной системе координат АХУ. Тогда система уравнений (1) примет вид:
dv
= v.
= - gny;
= -VxM = const •
(5)
После введения новых переменных задача формально сводится к синтезу оптимального в смысле функционала (3) алгоритма выбора управления пу = (путах,0, -путах), перемещения
изображающей точки из произвольного положения в фазовом пространстве ОхуУу в начало координат - точку О.
Фазовый портрет завершающего этапа такого переходного процесса приведен на рис. 4 (последние три интервала постоянных значений управления). Несложно видеть, что в начало координат можно попасть по двум траекториям - АО с управлением Пу = —Пу тах и ВО - с управ. На координату vy, согласно обозначениям (4), также наложены ограничения:
лением ny = ny max
где |vy max I Vy max
. v £ v £ v
y max y y max
(6)
поэтому в точке А с координатами (xa, ya,vy =-vymax) траектория АО
переходит в траекторию АС, у которой ny = 0, а vy = -vy max = const. Аналогично траектория ВО в точке В с координатами (xb, Уь , vy = vy max) переходит в траекторию ВD, у которой ny = 0 ,
а vy = vy max = const .
Приблизиться к траектории АО изображающая точка может только по поверхности П1 с управлением Пу = Путах, а к ее продолжению - траектории АC - по поверхности П2 тоже
с управлением Пу = Пу тах . Границей между поверхностями П1 и П2 является траектория АМ.
Приблизиться к траектории ВО изображающая точка может только по поверхности П3 с управлением Пу = -Пу тах, а к ее продолжению - траектории BD - по поверхности П4 также
с управлением Пу = -Пу тах . Границей между поверхностями П3 и П4 является траектория BN.
Для определения уравнений вышеперечисленных траекторий и поверхностей исключим из уравнений (5) время и проинтегрируем их в обратном направлении - от конца фазовых траекторий к началу при соответствующих постоянных значениях управления (Путах,0, -Путах).
Получим уравнения:
- траектории ОА:
- поверхности П1:
УіОА = Уі = ^/(У gny max); (7)
y y
vy ,(-xigny max + VxMVу)
Уі mi = Уі =-^------+----~y--------------—; (8)
2 gny max 4VxM gny max
- граничной траектории АМ:
2 2
yi АМ = у г = (-vy /2 + Vy max) /(gny max ) ; (9)
- поверхности П2:
v2 V V „,v V „V
y y ma^ хм у хм у max ч /1Л\
yi П2 = Уг = --—-—+^—(хг------------------- ------------- —); (10)
- траектории ОВ:
- поверхности П3:
2 gny max VXM gny max gny max
УіОВ = Уі = -vy /(ygny max) ; (11)
vy (X,gny max + VxMVу)У
Уі пз = Уі = -----------7-у-------------------------------------------------—; (iy)
2 gny max 4 VxM gny max
- граничной траектории BN:
yy
УіВМ = Уі = (vy/У - Vymax) /(gnymax); (13)
поверхности П4:
v2 V V ,„v V ,V
y y max / , хм у хм у max ч /Л ЛХ
yi П4 = yi = -------V--(Хг +----------------------( )
2gny max VxM gny max gny max
Программная траектория будет оптимальна в смысле функционала (3), если она совпадает с прямой АВ, за исключением необходимых отклонений по объезду препятствий. Алгоритм должен анализировать все точки препятствия для определения момента начала маневра огибания препятствия, однако сам маневр должен начинаться в момент, когда потенциально опасные точки появляются как в положительной, так и отрицательной частях плоскости АХУ. Причем программная траектория должна огибать сторону препятствия, которая меньше отклоняется от оси АХ.
Любая i-я точка препятствия при Ym > 0 является потенциально опасной для объекта, если изображающая точка попала на поверхности П1, П2, траекторию ОА или пересекла их. Соответственно, при Yni < 0 признаком потенциальной опасности i-й точки препятствия является попадание изображающей точки на поверхности П3, П4, траекторию ОВ или их пересечение.
При попадании изображающей точки на одну из поверхностей или линий переключения, приводящих ее в начало координат (в соответствии со знаком Yni), программное управление должно выбираться равным управлению, для которого получены эта поверхность или линия. Если изображающая точка не попала ни на одну из линий или поверхностей переключения, программное управление должно выбираться таким, чтобы оно переводило модель к траектории АВ за минимальное время.
В соответствии с изложенным подходом сформирован алгоритм выбора управления на каждом шаге интегрирования уравнений (1), однако для стабилизации объекта на прямой АВ использован регулятор с зоной линейности при малых отклонениях.
1. Выбор программного управления, стабилизирующего объект на траектории АВ, осуществляется регулятором с законом управления, учитывающим ограничения (2):
H1 = cl(YM -(Yn + YjSign(Yn))) + c2VyM \ H1, если \H1\ < n,
H 2 = -
y max ’
nyp =
nymaxSign(H1) если |H1| > nymax;
\H2> если (|VyM I < Vy max) V (H2 > 0 л VyM < Vymax) V (H2 < 0 л VyM > Vy max );
I 0, в остальных случаях,
(15)
(16)
(17)
где Сі, с2 - постоянные коэффициенты; Н1, Н2 - промежуточные переменные; Пур - выходной
сигнал регулятора; УП - координата препятствия в системе координат АХУ в месте нахождения объекта.
2. Для каждого положения луча дальномера выбирается программное управление:
Если Ym > 0 , то
H„. =
[упъ если y.- y.AM <0;
1 yіП2, если yi - y.AM > 0
ny1i
“у тах
, если [(y. -Hз. > 0) л (VyM < 0)] V
V [(Уі -H3i > 0) л (VyM > 0) л (yi - y.OA > 0) л (VyM < Vy max )];
0, если [(Уі -H3i < 0) л (VyM < -Vy max)] V
V [(Уі-H3i > 0) л (VyM > 0) л (Уі - УіОА > 0) л (VyM > Vymax)];
yM " ny max,
если [(Уі -Hз. < 0) л (VyM >
Vy max
)] V
V [(Уі -H3i > 0) л (VyM > 0) л (Уі - y.OA < 0)] •
Если Yni < 0, то
HA, =
[Уіго , если Уі - y.BN > 0; 1УіП4 , если Уі - y.BN < 0.
ny2i =
- n
у тах
, если [(Уі -H4і < 0) л (VyM > 0)] V
V [(Уі -H4i < 0) л (VyM < 0) л (Уі - УіОВ < 0) л (V, 0, если [(у. -H4. > 0) л (VyM > Vy max )] V
V [(Уі -H4і < 0) л (VyM < 0) л (у. - УіОВ < 0) л (V,
yM > -у max)];
yM < VVy max)];
у тах
, если [(у.-H4. > 0) л (VyM < Vy max )] V
у max^
V [(Уі -H4і < 0) л (VyM < 0) л (Уі - УіОВ > 0)],
(18)
(19)
(20)
(21)
где 7 = 0, к; Нзг-, Н^ - промежуточные переменные.
Каждому значению 7 соответствует упорядоченная тройка {пуЬ,
где ^17 = (У7 - Н37) , если ¥П ^ 0 , и { пу2,, ¥П, к2, }, гДе к2, = (У; - Н4, ), если ¥П < 0 .
3. Если отсутствуют тройки, в которых ¥П1 > 0, то формируется тройка
{пу1 = путах, 7П1 = 100, Н1 = 1} и пропускаются пп. 5, 7 алгоритма.
4. Если отсутствуют тройки, в которых Ут < 0, то формируется тройка {пу2 = —пу тах, 7П1 = -100, Н1 = -1} и пропускаются пп. 6, 8 алгоритма.
5. Из всех троек, соответствующих ¥П1 > 0 , отбираем те, у которых пу1і > — пу тах . Если таких пар нет, то формируем тройку {Пу1 = — Пу тах, ^П1 = 0, ^1 = — 1} и пропускаем п. 7 алгоритма.
6. Из всех троек, соответствующих 7т < 0, отбираем те, у которых Пу2і < Пу тах . Если таких пар нет, то формируем тройку {Пу2 = Пу тах, Гд2 = 0, ^2 = 1} и пропускаем п. 8 алгоритма.
7. Из оставшихся троек, соответствующих ¥Пі > 0, отбираем ту, у которой ГПі максимально. Обозначим параметры этой группы { Пу 1, Гш, ^}.
8. Из оставшихся троек, соответствующих ¥Пі < 0, отбираем ту, у которой ГПі минимально. Обозначим параметры этой группы {Пу2, ^П2, ^2}.
9. Выбираем программное управление:
Пу =
УР
если h1 < 0 v h2 > 0;
Пу!, если h > 0 л |7Ш | < |Ym |; Пу2, если h2 < 0] л |7Ш | > Yn21-
(22)
В результате интегрирования уравнений (1) с выбранным значением управления получаем численное значение координат модели YM, VyM, которые и определяют значение программной
траектории в каждый момент времени. Координату Хм модели принимаем равной координате ХР центра масс робота.
Работоспособность и эффективность предложенного алгоритма проверялись методом математического моделирования. Для этого разработана программа на языке СИ++. При моделировании использовалась линеаризованная математическая модель плоского движения гипотетического четырехколесного робота шестого порядка [3] с регулятором, обеспечивающим отслеживание программной траектории синтезированным методом АКОР.
Один из результатов моделирования представлен на рис. 5, где показана только та часть препятствия, которую объезжал робот. Программная траектория на этом графике показана сплошной линией, траектория движения центра масс робота - пунктирной.
Рис. 5. Результаты моделирования объезда препятствия
Из результатов моделирования следует, что программная траектория соответствует предъявляемым требованиям. Робот правильно выбирал направление объезда препятствия, отклонение траектории движения центра масс робота от программной траектории несущественно и не превышало 0,2 м.
Заключение
Предложенный подход и алгоритм формирования программной траектории позволяют эффективно управлять движением робота. Несмотря на кажущуюся сложность, алгоритм управления может быть легко реализован в бортовом компьютере мобильного робота и может использоваться в качестве основной части общего алгоритма при создании системы управления. При этом предложенный алгоритм формирования программной траектории может послужить в качестве базового при создании алгоритмов, позволяющих мобильному роботу проезжать между препятствиями местности, объезжать группу препятствий, а также может быть положен в основу гибкой системы управления движением робота.
СПИСОК ЛИТЕРАТУРЫ
1. Лисицкий Д. Л. Выбор структуры системы автоматического управления траекторным движением мобильного робота // Вестн. Сарат. гос. техн. ун-та. - 2009. - № 4 (43). - С. 108-110.
2. Лернер А. Я., Розенман Е. А. Оптимальное управление. - М.: Энергия, 1970. - 360 с.
3. Буданов В. М., Девянин Е. А. О движении колесных роботов // Прикладная математика и механика. -2003. - Т. 67, вып. 2. - С. 244-255.
Статья поступила в редакцию 17.11.2010
MOTION CONTROL OF MOBILE ROBOT
A. A. Bolshakov, D. L. Lisitsky
The article discusses creation of algorithms for systems of automatic control of mobile robot. The problem is solved within the framework of the structure that allows to separate it into two consistently included subsystems. One of the subsystems is named "the model of a computer software control". It generates a program trajectory. The second subsystem is named "the object-regulator". It controls this program trajectory. The article presents a simplified motion model of the center of mass of robot and the constraints imposed on its coordinates and controls. The article presents a synthesized algorithm for choice of program control over the phase portrait of the system by analyzing the position of the image point relatively to the surface and the switching line by the criterion of minimizing the deviation of the program trajectory from the straight line, that is connecting the start and the end points of the route. The efficiency of the proposed approach and the performance of the synthesized algorithm are tested by mathematical modeling.
Key words: robot, obstacles, program trajectory, model, algorithm, control.
