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

НАУКА и ОБРАЗОВАНИЕ

Эл № ФС77 - 30569. Государственная регистрация №0421100025. ГББМ 1994-0408

Моделирование движения колесного робота по заданному пути 77-30569/239840

# 10, октябрь 2011

О. Г. Андрианова

УДК 519.6

МГТУ им. Н.Э. Баумана [email protected]

1. Введение

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

Требуется, чтобы колесный робот из некоторого начального положения достиг заданной линии движения (заданного пути) и двигался вдоль нее с постоянной скоростью. При движении вдоль заданного пути решается задача стабилизации.

Отметим, что планирование пути является отдельной задачей. Как правило, для колесных роботов задают достаточно гладкую кривую, проходящую через заданную последовательность контрольных точек. Такая кривая может быть задана кубическими параметрическими сплайнами и кубическими В-сплайнами [1].

Поскольку кривая, построенная с использованием В-сплайнов, не всегда проходит через заданные контрольные точки, будем рассматривать кривые, заданные кубическими сплайнами.

Известны различные подходы к решению задачи следования по заданному пути. Так, в [2, 3] соответствующее управление находится с использованием кинематической модели на основе преобразования системы к цепной форме. В

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

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

2. Кинематическая модель колесного робота

Рис. 1. Модель колесного робота

Следуя [4], приведем модель движения колесного робота, учитывающую динамику привода рулевого механизма. Робот представлен абсолютно твердой платформой и колесной системой с четырьмя недеформируемыми колесами. Предполагается, что колеса находятся в точечном контакте с поверхностью и движутся без проскальзывания. Два задних колеса являются ведущими, а два передних отвечают за поворот платформы.

Положение робота на плоскости задается двумя координатами целевой точки 0(х0,у0) и ориентацией продольной оси робота, определяемой углом

между положительным направлением оси Xa и вектором скорости линейной скорости V.

’’Средний” угол поворота передних колес 5 задается соотношением

tg 5 = ul, (1)

где u — мгновенная кривизна пути, l — расстояние между передними и задними колесами.

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

xо = v cos ,

Уо = v sin &t,

4t = vu(5),

5 = V.

На угол поворота передних колес и управление наложены ограничения

|V| < V, |5| < 5.

3. Преобразование системы к нормальной форме

В [4] предъявлена замена по части переменных, приводящая систему к специальному виду. Такой подход представляется не совсем верным, поскольку в этом случае решается задача стабилизации системы на некотором многообразии, что без анализа характера движения по этому многообразию не дает полного представления о поведении системы, замкнутой предложенным управлением. Поэтому представляется целесообразным получить расчетные формулы для стабилизирующего управления на основе теории нормальной формы аффинной системы [5].

Руководствуясь подходом, предложенным в [4], получим формулу для стабилизирующего управления с необходимыми уточнениями и дополнениями.

Напомним, что O точка робота, которая должна следовать по заданному пути. В качестве выхода выберем расстояние от точки O(xo,yo) до ближайшей

г

О (л

Чад

А хА

Рис. 2. Расчетная схема

точки заданного пути М(хт,ут). Я(в) мгновенный центр кривизны пути в точке М, ЯМ = г (в) = 1 /с(в), г (в) — радиус кривизны пути в точке М, с(в) — значение кривизны в точке М, $с угол между касательной к пути в точке М и осью Ха, $ = $ — $с. Полагаем, что при движении робота вдоль целевой кривой в положительном направлении выполняется

п

1$1 < 2. (3)

Введем систему координат Огр, ось г направлена ортогонально касательной к заданному пути в точке М, а ось р — по касательной к пути в этой точке в направлении движения.

В качестве независимой переменной возьмем длину пути £, пройденного роботом до текущего момента времени, и заменим производные по времени в уравнениях (2) производными по £.

Первой фазовой переменной выберем г\ — расстояние от точки О до точки М, которое будем вычислять с учетом направления движения робота вдоль пути: г\ = sign(а) |ОМ|, где а = —у'а (хт — Хо) + х'8 (ут — Уо). Построим нормальную форму [5] системы (2) по выходу у = г\.

Так как г\ = V ят $, то г[ = у = ят $. В качестве второй фазовой переменной примем г2 = ят $. Дифференцируя обе части этого равенства по £,

получим

z2 = cos $ $О = cos $ ($£ — $0 = cos $ u —

c cos $

1 + CZi

(4)

Определим третью фазовую переменную формулой

z3 = cos $ u —

c cos $

1 + CZi

(5)

и продифференцируем ее по £

ZО = — sin $ $О u —

c cos $

1 + CZi

+ cos $ u —

c cos $

1 + CZi

= ev — f, (6)

где

f =

Z2Z3

1 — Z22

+

cC cos3 $ CZ2Z3

(1 + CZi)2 1+ CZi

c2(1 — z|)z2 ccCs Z1 cos3 $

(1 + CZi)2 (1 + CZi)3

в =

cos

v

(7)

(8)

Соотношение (7) существенно отличается от аналогичного выражения, полученного в [4]. Отличия связаны с учетом зависимости производной кривизны по пройденному пути.

Заметим, что в = 0, если cos $ = 0. Для выхода y = zi в указанной области определена относительная степень выхода, равная 3. Следовательно, в окрестности каждой точки области замена по части переменных z невырождена [6]. Будем предполагать, что замена переменных справедлива во всей области.

Пусть n = s, тогда П = ic+Cf1.

Таким образом, уравнения (2) в координатах z, n ”в виде заготовки” запишутся так

Zi = Z2, z2 = Z3,

z3 = ev — f,

ПО cos $

(9)

i+czi ■

Последнее уравнение (9) не содержит управления, что существенно облегчает анализ нулевой динамики системы. Нулевая динамика при zi = 0, z2 = 0, Z3 = 0 имеет вид

cos $

n = 1

1 + CZi

так как zi = z2 = sin $ = 0, $ = 0, cos $ = 1. Для интерпретации полученного результата перейдем от производной по £ к производной по t

П = vn° = v. (10)

Выражение (10) описывает движение с постоянной скоростью по желаемому пути.

4. Синтез закона управления

Поскольку коэффициент в при управлении не обращается в ноль в области cos $ = 0, воспользуемся методом линеаризации по части переменных для построения стабилизирующего управления

v = ^, (11)

в

где Y(z) = biZi + &2Z2 + to.

Получаем линейную замкнутую систему z' = A z. Подходящим выбором вектора b = [bi, b2, b3]T матрицу A всегда можно сделать гурвицевой, обеспечивая асимптотическую устойчивость замкнутой системы в переменных z.

При моделировании будем учитывать ограничение на управление |V| < К, примем его в виде

—V ,если V < —V;

V = < ^если |V | < V; (12)

V ,если V > V.

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

Рассмотрим случай, когда заданный путь представляет собой прямую, проходящую через начало координат под углом $с = п/4 рад к оси абсцисс. Примем линейную скорость робота равной 2.00 м/с, длину — 2.45 м. Зададим начальные условия равными

х(0) = 0.00 м, у(0) = 5.00 м,

$г(0) = п/4 рад, 6(0) = 0.00 рад.

Ограничение на управление примем равным V = 0.13 рад/с.

Общее уравнение заданного пути имеет вид Ах + В у + С = 0,

где А = 1, В = —1, С = 0.

Расстояние от текущей точки до заданного пути будем считать по формуле

*1 = ) |ОМ|, где а = — у (хт — Хо) + X (Ут — У о),

Ахо + Вуо + С ^А2 + В2 _ .

Результаты моделирования представлены на рис. 3-6.

у0.“ I

Пуіь рибиіа Заданный путь

Рис. 3. Заданный путь и путь робота

|ОМ | =

Рис. 4. Зависимость угла ориентации робота от времени £

Рис. 5. Зависимость угла поворота колес 6 от времени £

V, рад /с

0.1

-0.05

-0.1

-0.15

—>-

1, С

2 4 6 8 10 12 14 16 18 20

Рис. 6. Зависимость управления V от времени £

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

Пусть дан набор контрольных точек на плоскости, заданных своими координатами

S = {г1, г2,..., тп}, г е К2, п > 4,

где т = гг(хг,уг).

Введем параметр si = Х3=1 3, — длина пройденного пути до г-ой точки;

к = \/^+1 — Xi)2 + (yi+l — уО2, г = 1, (п — 1).

Построим две функции х = ф^) и у = ^^), которые представляют собой нормальные кубические полиномы, проходящие через заданный набор точек.

7. Пример 2

Зададим начальные условия равными

х(0) = —50.00 м, у(0) = —80.00 м,

$г(0) = 0.00 рад, 6(0) = 0.00 рад.

Координаты контрольных точек удовлетворяют параметрическим уравнениям эллипса (а = 200.00 м, Ь = 100.00 м). Зададим путь кубическим параметрическим сплайном, проходящим через эти точки.

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

| х у — у Х | с = (Х2 + у2)3/2

и аппроксимируем кубическим сплайном. Производную кривизны получим непосредственным дифференцированием полученного сплайна.

Для нахождения модуля расстояния от текущей точки до заданного пути |ОМ | вычислим расстояние от текущего положения до всех контрольных точек р(0, т^, г = 1, п. Если для трех контрольных точек выполняется условие

р(0, ^—1) > р(0, тi) и р(0, тi) < р(0, т^), то с помощью стандартной функции 1тшЬпОД в Ма^аЬ находим минимальное расстояние между г — 1-ой и г + 1-ой контрольными точками, которое будет равно |ОМ|.

Графики зависимости кривизны и производной кривизны от пройденного пути показаны на рис. 7-8.

Рис. 7. Зависимость кривизны с от пройденного пути s

Рис. 8. Зависимость производной кривизны с, от пройденного пути s Результаты моделирования представлены на рис. 9-13.

Рис. 10. Зависимость углов и $с от времени £

5, рад

О 20 40 60 80 100 120

Рис. 11. Зависимость угла поворота колес 6 от времени £

О 10 20 30 40 50 60 70 80 90 100

Рис. 12. Зависимость управления V от времени Ь

Рис. 13. Расстояние от целевой точки до заданного пути

В настоящей работе проанализирован предложенный в [4] подход к построению управления с точки зрения теории нормальной формы аффинной системы и уточнены расчетные формулы. Показано, что в случае задания пути кубическим параметрическим сплайном, проходящим через все контрольные точки, первая производная кривизны полученной кривой не является непрерывной, и разработан алгоритм сглаживания первой производной. Предложен алгоритм нахождения расстояния до заданного пути. Методами математического моделирования показана работоспособность предложенного метода синтеза управления, обеспечивающего движение колесного транспортного средства по заданому пути с постоянной скоростью. В частности, вычислительные эксперименты подтверждают правильность полученных формул для расчета управления.

Работа выполнена при финансовой поддержке гранта РФФИ 10-07-00468 и проекта 2.1.1/11818 ведомственной целевой программы ’’Развитие научного потенциала высшей школы (2009-2011)”.

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

1. Гилимъянов Р.Ф. Планирование пути колесного робота по зашумленным измерениям в задаче управления движением вдоль криволинейной траектории: Дис. ... канд. техн. наук. Москва, 2010. 125 с.

2. Thuilot B., Cariou C., Martinet P., Berducat M. Automatic Guidance of a Farm Tractor Relying on a Single CP-DGPS // Autonomous Robots. 2002. V. 13. P. 53-71.

3. Fang H, Fan R., Thuilot B., Martinet P. Trajectory tracking control of farm vehicles in presence of sliding // Robotics and Autonomous Systems. 2006. V. 54. №. 10. P. 828-839.

4. Гилимъянов Р. Ф, ПестеревА.В., Рапопорт Л.Б. Управление движением колесного робота в задаче следования вдоль криволинейного пути // Известия РАН. Теория и системы управления. 2008. Т. 47, № 6. С. 158-165.

5. IsidoriA. Nonlinear control systems. London: Springer-Verlag, 1995.

6. Крищенко А.П. Преобразование нелинейных систем и стабилизация программных движений // Труды МВТУ им. Н.Э. Баумана. 1988. № 512. С. 69-87.

SCIENCE and EDUCATION

El № FS77 - 30569. №0421100025. ISSN 1994-0408

Path following simulation of wheeled vehicle 77-30569/239840

# 10, October 2011 O. G. Andrianova

Bauman Moscow State Technical University

[email protected]

Stabilization algorithm of the wheeled vehicle on the planned path is studied by means of numerical modeling. Synthesis of stabilization algorithm is based on theory of normal form of the affine system. Smoothing algorithm of the first derivative of curvature in the case of setting the path by parametric cubic spline is suggested. Methods of finding distance from the wheeled vehicle to the planned path are considered. Efficiency of the method is shown, high sensitivity of control to curvature of the path is established.

References

1. Gilim’janov R.F. P1a.nirova.nie puti kolesnogo robota po zashumlennym izmerenijam v zadache upravlenija dvizheniem vdol’ krivolinejnoj traektorii: Dis. ... kand. tehn. nauk. Moskva, 2010. 125 s.

2. Thuilot B., Cariou C., Martinet P., Berducat M. Automatic Guidance of a Farm Tractor Relying on a Single CP-DGPS // Autonomous Robots. 2002. V. 13. P. 53-71.

3. Fang H, Fan R., Thuilot B., Martinet P. Trajectory tracking control of farm vehicles in presence of sliding // Robotics and Autonomous Systems. 2006. V. 54. №. 10. P. 828-839.

4. 4. Gilim’janov R.F., Pesterev A.V., Rapoport L.B. Upravlenie dvizheniem kolesnogo robota v zadache sledovanija vdol’ krivolinejnogo puti // Izvestija RAN. Teorija i sistemy upravlenija. 2008. T. 47, № 6. S. 158-165.

5. Isidori A. Nonlinear control systems. London: Springer-Verlag, 1995.

6. Krishhenko A.P. Preobrazovanie nelinejnyh sistem i stabilizacija programmnyh dvizhenij // Trudy MVTU im. N.Je. Baumana. 1988. № 512. S. 69-87.