Кинематическое управление мобильным роботом при движении

КИНЕМАТИЧЕСКОЕ УПРАВЛЕНИЕ МОБИЛЬНЫМ РОБОТОМ
ПРИ ДВИЖЕНИИ ПО ПОЛИГОНУ С ОБХОДОМ ПРЕПЯТСТВИЙ
О.Г. Андрианова, М.А. Королькова, С.А. Кочетков, С.А. Краснова
МГТУ им. Н.Э.Баумана, г. Москва; ИПУ РАН, г. Москва; ИПУ РАН, г. Москва; ИПУ
РАН, г. Москва
karsettt@mail.ru, korolkova_rita@mail.ru, kos@ipu.ru, krasnova@ipu.ru
Введение
В работе рассматривается проблема обеспечения желаемого движения мобильного
робота по полигону с заездом в заданные пункты и обходом препятствий. Подобная задача возникает при использовании роботов для доставки обедов в больницах, разминирования подозрительных предметов, пожаротушения и т.д.
На этапе планирования траектории используется стандартный волновой алгоритм,
позволяющий параметризировать маршрут с помощью кубических сплайнов. Разработан закон кинематического управления платформой, обеспечивающий движение по
сформированной криволинейной траектории в течение заданного времени. Введение
принудительного уравнения связи между линейными и угловыми отклонениями позволило свести проблему синтеза к элементарной задаче, в которой число управляемых и
управляющих переменных совпадает. Приведены результаты численного моделирования, подтверждающие эффективность разработанного подхода.
1. Планирование траектории
На уровне планирования движения робота предполагаются известными цель движения и описание полигона – пункты обхода и препятствия. Для построения непрерывной траектории движения в режиме off-line (когда требуемый объем памяти и скорость
нахождения пути не критичны) целесообразно использовать волновой алгоритм [1], который отличается простотой и надежностью. Данный алгоритм формализует ломаную в
виде последовательности точек координатной плоскости ( xi , yi ) , i  1...n , определяющих движение робота из известного начального в заданное конечное положение с обходом препятствий и появлением робота один раз в каждом из пунктов обхода с заданными параметрами ориентации. Для найденной последовательности точек вводится параметр пути  k , k  1...n ,  k  [0,1] , значения которого находятся из выражений
k 
1 k
 li , li 
L i 1
xi 1  xi 2   yi 1  yi 2 , i  1...n  1,
n 1
L   li .
i 1
На каждом отрезке ломаной на основе наборов данных ( i , xi ) и ( i , yi ) строятся аппроксимации x   ( ) и y   ( ) в виде кубических параметризованных сплайнов, которые задаются значениями функции в узлах и значениями первых или вторых производных на границах отрезков, что обеспечивает гладкость криволинейной траектории.
2. Стабилизация движения робота
Рассматривается двухколесный робот, кинематическая модель которого имеет вид
(1)
x1   cos x3 , x2   sin x3 , x3   ,
где x1, x2 – декартовы координаты центра масс платформы в неподвижной системе координат (см. рис. 1); x3 – угол ее ориентации;  ,  – линейная и угловая скорости соответственно, которые для системы (1) полагаются управляющими воздействиями.
Рисунок 1. Задачно-ориентированные координаты платформы
Ставится задача синтеза кинематического закона управления, обеспечивающего стабилизацию движения робота относительно траектории, заданной параметрически:
(2)
x1r   ( ) , x2r   ( ) ,
где x1r , x2r – координаты желаемой траектории в исходной системе координат.
Для обеспечения заданного времени T движения по трассе из начальной точки в
конечную введем зависимость параметра пути от времени следующим образом:
2
3
4
5
t
t
t
t
t
t
 a2    a3    a4    a5   .
T
T 
T 
T 
T 
T 
Тот факт, что робот должен начать и закончить движение с нулевой скоростью и нулевым ускорением, позволяет однозначно определить коэффициенты ai из условий
 (0)  0 ,  (T )  1 , (0)  0 , (T )  0 , (0)  0 , (T )  0 .
Опорная траектория и закон изменения опорных переменных x1r , x2r во времени
связаны системой дифференциальных уравнений, которую назовем эталонной:
x1r  ( )(t ) , x2r   ( )(t ) , x3r   ( ) ( )   ( ) ( ) (  2 ( )    2 ( )) .
 (t )      a0  a1
Представление уравнений эталонной системы в виде, аналогичном (1), а именно
x1r  r cos x3r , x2r  r sin x3r , x3r  r ,
(3)
позволяет формализовать выражения для эталонных скоростей r , r , обеспечивающих движение по заданной траектории (2) с заданными показателями:
r  (t )   2 ( )    2 ( ) , r   ( ) ( )   ( ) ( ) (  2 ( )    2 ( )) .
~X~ , связанной с эталонной моделью (см. рис. 1)
Перейдем к системе координат X~1O
2
и с учетом (1), (3) составим систему дифференциальных уравнений относительно лиx  (~
x ,~
x ,~
x )T , где H – невырожденнейных и углового отклонений ~
x  H (x  x ) , ~
r
1
2
3
ная матрица поворота на угол x3r по часовой стрелке:
~
(4)
x1   cos ~
x3  r  r ~
x2 , ~
x2   sin ~
x3  r ~
x1 , ~
x3    r .
Таким образом, задача управления сводится к задаче стабилизации системы (4).
Основная проблема заключается в том, что число управляющих воздействий  ,  в системе (4) меньше числа стабилизируемых переменных. Чтобы обойти эту проблему,
введем новые переменные – полярные координаты S  ( ~
x2  ~
x 2 ) / 2 , tg  ~
x /~
x , tgx~ ,
1
2
2
1
3
относительно которых с учетом (4) получим систему дифференциальных уравнений:
S  2S ( cos( ~
x3   )  r cos ) ,
d


(5)
tg 
(1  tg 2 ) sin( ~
x3   )  r (1  tg 2 )  r tg 1  tg 2 ,
dt
2S
2S
d ~
tgx3  (1  tg 2 ~
x3 )(  r ) .
dt
При независимом синтезе управляющих воздействий  ,  возможно вырождение
x3   )  0 . Следуя алгоритму, описанному в работе [2],
первого уравнения (5) при cos( ~
введем принудительную связь между линейными и угловыми отклонениями в виде

tg~
x  tg  ,   const  0 ,   const  0 ,    .
(6)
3

что позволит, во-первых, обеспечить связь между управляющими воздействиями  , 
и исключить вырождение в первом уравнении (5) управления  ; во-вторых, редуцировать порядок системы и свести задачу синтеза к решению двух элементарных задач.
Первая задача состоит в формировании закона управления  , обеспечивающего

x  tg между желаемыми и реальными значениями
стабилизацию ошибки e  tg~
3

tgx~3 (6), динамика которой в силу (5) имеет вид
e  (1  tg 2 ~
x3 )(  r )  f ( ,  ,  , ~
x3 ) ,



f      sin( ~
x3   ) 2S  r (1  tg 2 )  r tg 1  tg 2 2S  .


Выбор управления  , связывающего скорости вращения и движения с направлением
движения робота, в виде
    ( f  l e) /(1  tg 2 ~
x ) , l  0,
(7)
r
1
3
1
обеспечивает асимптотическую стабилизацию замкнутой подсистемы e  l1e .
Вторая задача состоит в выборе управления  , обеспечивающего стабилизацию
расстояния S до центра движущейся системы координат. Закон управления
(8)
  (l2 S  r cos  ) / cos( ~
x3   ) , l2  0
обеспечит асимптотическую сходимость переменной S к нулю: S   2l S .
2
Синтезированные законы кинематического управления (7)–(8) обеспечивают движение робота по заданной трассе (2) за заданное время T . Существенно, что соотношения (7)–(8) можно трактовать как задающие воздействия для отработки приводами, что
позволит осуществить декомпозицию верхнего уровня при синтезе системы управления
на основе динамической модели мобильного робота.
3. Пример
Эффективность разработанных алгоритмов подтверждается результатами моделирования. На рис. 2 (слева) показаны ломаная и соответствующая ей криволинейная траектории робота, который должен добраться из пункта A в пункт B за 100 сек., обогнув
препятствия и побывав в трех пунктах обхода. Также на рис. 2 (справа) приведены зависимости параметра пути и опорных координат от времени. На рис. 3 представлены
результаты моделирования при    2 , l1  200 , l2  15 , которые демонстрируют достаточно высокую точность решения траекторной задачи.
Рисунок 2. Траектория робота (слева) и опорные переменные (справа)
Рисунок 3. Опорная траектория (слева) и результаты моделирования (справа)
Литература
1. Frank Rubin. The Lee path connection algorithm // IEEE Transactions on Computers.
1974. Vol. C-23. № 9. PP. 907–914.
2. Kochetkov S.A., Utkin V.A. A trajectory stabilization algorithm for mobile robot // The
Proceedings of 11-th International Workshop on Variable Structure systems (VSS2010).
Mexico, 2010. PP. 121–127.