Реализация движения колесного робота по заданной траектории
ПРИКЛАДНАЯ МАТЕМАТИКА И МЕТОДЫ МАТЕМАТИЧЕСКОГО МОДЕЛИРОВАНИЯ
УДК 517.977
С. Б. Ткачев
РЕАЛИЗАЦИЯ ДВИЖЕНИЯ КОЛЕСНОГО РОБОТА ПО ЗАДАННОЙ ТРАЕКТОРИИ
Предложен алгоритм управлений, реализующий движение мобильного робота по линии, вдоль которой задан профиль скорости. Приведена модель движения робота на подвижной колесной платформе с автомобильной компоновкой колес, учитывающая эффект псевдоскольжения, связанный с деформацией периферии колес. В качестве управляемых величин выбраны ускорение центра масс робота и угол поворота передних колес. Получено описание виртуальных выходов, по которым модель преобразуется к нормальной форме. Для заданного программного движения получена нестационарная нормальная форма системы в отклонениях и исследована ее нулевая динамика. Методом нелинейной стабилизации построена нестационарная обратная связь, стабилизирующая программное движение. Приведены результаты численного моделирования.
Задача реализации заданного программного движения является типичной для робототехнической системы на колесной платформе. Такие задачи рассматриваются и для роботизированных сельскохозяйственных машин, использующих GPS-навигацию, и для объектов виртуальной реальности, поведение которых должно выглядеть естественно.
Известны различные подходы [1-4] к реализации заданной программной траектории движения колесной робототехнической системы. Одним из них является подход, развитый в работе [1], где в качестве управляемых величин предложено использовать ускорение центра масс системы и угол поворота управляющих колес. Этот подход был реализован при управлении роботом Nomad 200 с использованием скользящих режимов [1].
Альтернативой скользящим режимам управления являются управления в классе непрерывных функций. Такое управление может быть построено для аффинной (линейной по управлению) динамической системы на основе дифференциально-геометрического подхода [5, 6]. Для аффинных систем с векторным управлением известны условия приводимости к каноническому виду [3], с использованием которого возможно как нахождение программного управления по программной траектории, так и построение нелинейной обратной связи по состоянию, обеспечивающей стабилизацию заданной траектории [4].
Модель движения мобильного робота. Рассмотрим четырехколесный мобильный робот с автомобильной компоновкой колес. Передние колеса управляющие, задние — ведущие.
При построении модели движения предположим, что робот перемещается в горизонтальной плоскости, а эффекты, связанные с динамическим перераспределением нагрузки на колеса при маневрировании, динамикой робота в вертикальной плоскости и рельефом поверхности, не учитываются. Корпус робота рассматривается как абсолютно твердое тело.
Используем модель качения колеса с деформируемой периферией, описывающей только эффект поперечного псевдоскольжения (поперечного увода) колеса (см., например, [2]).
Даже при принятых предположениях модель движения четырехколесного робота оказывается достаточно сложной. Дальнейшее упрощение связано с переходом к так называемой “велосипедной” модели, расчетная схема которой приведена на рис. 1.
Свяжем с роботом строительную систему координат х’О’у’, начало которой совпадает с центром масс робота, а ось х’ направлена вдоль продольной оси симметрии. Положение мобильного робота как твердого тела на плоскости однозначно определяется двумя координатами центра масс О’ в неподвижной системе координат хОу и углом поворота оси О’х’ строительной системы координат относительно оси Ох неподвижной системы координат.
Пусть т — масса мобильного робота; J — момент инерции робота относительно вертикальной оси, проходящей через центр масс; ^ и
.х
Рис. 1. “Велосипедная” модель мобильного робота
lr — расстояния от центра масс до осей переднего и заднего колес соответственно; v — линейная скорость центра масс; в — угол бокового скольжения мобильного робота; ó — угол поворота переднего колеса; в/ и вг — углы бокового скольжения переднего и заднего колес соответственно; ф — угол между осью O’xX строительной и осью Оx неподвижной систем координат; ш — угловая скорость вращения вокруг центра масс; f/ и fr — боковые силы, действующие на переднее и заднее колеса.
Положительное направление отсчета для всех углов — против хода часовой стрелки.
Для углов бокового скольжения переднего и заднего колес из геометрических соображений (см. рис. 1) получены соотношения
V sin в l/ш
в/ = arctg—-ó = а/(V, в,ш) — ó;
v cos в
• в / (1)
v sin в — гш . ,
вг = arctg–— = аг (v, в, ш).
v cos в
Рассмотрим уравнения, определяющие динамику движения системы в горизонтальной плоскости, в проекциях на оси строительной системы координат:
miixi — mvy’ ш = fx’; muyi mvx/ш = fy/. (2)
Уравнение вращательного движения вокруг центра масс имеет вид
Jáj = M. (3)
В уравнениях (2), (3) vxi = v cos в и vyi = v sin в — проекции скорости центра масс на соответствующие координатные оси; fxi и fyi — проекции сил, действующих на робот; M — момент сил относительно центра масс.
Вычислив производные проекций скорости
vxi = v cos в — vв sin в; vyi = v sin в vв cos в, (4)
и подставив (4) в (2), получим
mv(¡3 ш) = —fxi sin в fyi cos в; mv = fxi cos в fy sin в. (5)
Пусть F — движущая сила, создаваемая задними колесами. Тогда выражения для проекций сил, действующих на робот, имеют вид
fxi = —f/sin ó F; fyi = f/cos ó fr, (6)
а для суммарного момента —
Mz = f/1/ cos ó — fr lr. (7)
Подставив (6) в (5) c учетом уравнений связи между строительной и неподвижной системами координат, получим
• ff COS S fr tg P.
/3 =—-ш–v;
mv cos в v
Jüj = ff lf cos S — fr lr;
ф = ш; (8)
x = v cos(e ф);
y = v sin(e ф).
Система уравнений (8) описывает движение мобильного робота при условии, что линейная скорость центра масс отлична от нуля и |в| <п/2.
В системе (8) управляемыми величинами будем считать угол поворота передних колес S и ускорение v. Обозначим S и v через u и u2 соответственно и добавим в систему (8) уравнение v = u2.
Согласно гипотезе Рокара [2], в диапазоне углов Pf, вг от 0 до (5… 8)° поперечные силы реакции ff, fr, действующие на передние и задние колеса, полагают равными
ff = —Cf pf, fr = —crpr (9)
и направленными в сторону, противоположную поперечным составляющим скоростей колес. Здесь Cf и cr — коэффициенты пропорциональности, характеризующие взаимодействие колес с дорогой.
Упростим систему (8) при условии, что угол поворота переднего колеса S и угол бокового скольжения в малы и можно принять sin в ~ в, COS в ~ 1, sin S ~ S, COS S ~ 1.
Подставив в (8) соотношения (9), (1) и упростив первые два уравнения системы (8) с учетом сделанных предположений, получим
• Cf (af — S) Cr ar в.
в =—A—L—-ш–v =
mv v
Cf af cr ar Cf в
= —-——ш –— u1–u2
mv mv v
и
Jüj = —Cf (af — S)lf Cr ar lr = —Cf af lf Cr ar lr Cf lf u1.
Выражения (1) с учетом малости углов можно упростить, приняв
vP lf ш vP — lr ш …. af =–—; ar =-. (10)
С учетом сделанных допущений система (8) примет вид
• сиаи Сг аг си в
¡3 = —^–ш ииг – -щ;
ту ту V
Ь , 1г , lf ш = —- Сf аf — сг аг — Сf иг;
7 7 7
у= «2; (11) тр = ш;
х = V еоэ(в у = у эт(в
Система уравнений (11) — аффинная с двумя управлениями. Исследуем возможность ее преобразования к каноническому или квазиканоническому виду.
Преобразование системы к квазиканоническому виду. Говорят, что аффинная система с двумя управлениями
¿1=2:2,.. -1=гт; ¿т=/г(г, п) дгг(г, пК д^г, п)«2;
¿П1 1 ¿nl 2, . . . , ¿П1 П2 — 1 ¿П1 П2 ;
(12)
¿П1 П2 = f2(¿,r¡) д21 (¿,п)иг д22(2,п)и2;
П = /з(г,п) дзг(г,п)иг дз2(г,п)и2,
где пг П2 < п, г = (¿1,…, ¿щ п )Т, П = (пг , …,Пт)Т, п п т = п, записана в квазиканоническом виде [7] с индексами пг и п2. В случае, если пг п2 = п, вид системы (12) называют каноническим.
Исследуем возможность преобразования аффинной системы (11) к каноническому или квазиканоническому виду с максимально возможными значениями индексов пг и п2.
С системой (11) ассоциированы векторное поле
( си аи сг аг д (1Г и д
А = – ( ш)зв П 7 *а- 7 с> аЧ ди
д д д ш— у еоэ(в — V эш(в — дф дх ду
и векторные поля
„ си д 1иси д „ в д д
Вг = —^— —; В2 = —–1–.
ту дв 7 дш у дв ду
Исследование возможности преобразования системы (11) для некоторой окрестности точки х0 к каноническому виду [5] связано с последовательным анализом инволютивности и регулярности в этой окрест-
ности семейства распределении вида
Нг = зрап(Вь В2, adAB1, adAB2,…, adAB1, adAB2), г = 0,1,…, 5,
где adXУ = [X, У] — коммутатор (скобка Ли) векторных полей X и У, а adX 1Y = [X, adX У]. Если X (£) и У (£) — столбцы координат векторных полей X и У соответственно, то столбец координат векторного поля [X, У](£) вычисляется по формуле
[*,У]К) = ^X«) – ^У(0.
Непосредственными вычислениями можно показать, что [В1, В2]=0, т.е. эти векторные поля коммутируют. Следовательно, порожденное этими векторными полями распределение Н0 = span(B1, В2) инволю-тивно. При V = 0 это распределение имеет постоянный ранг, равный двум, и по теореме Фробениуса интегрируемо. Из соображений размерности для этого распределения существует четыре функционально независимых первых интеграла.
Следовательно, можно утверждать, что система (11) приводится к квазиканоническому виду с индексами п1 = п2 = 2 и неканонической частью, состоящей из двух уравнений.
Для исследования возможности преобразования системы (11) к каноническому виду проведем анализ свойств распределения, порожденного семейством
Н1 = {В1,В2, е^аВЬ adAB2}.
Вычислив векторные поля adAB1, adAB2, получим 17-, д д ¡¿е? д
= др 72 а^ – Т дф
е? siп(в ф) д е/ cos(в ф) д
т дх т ду
д д д adAB2 = 7э — 74д^ – (в ^п(в ф) – ^(в ф)) дх
д
(в^(в ф) – sin(в ф))ду,
где тг, г = 1,…, 4, — некоторые функции.
Определим ранг матрицы и1, столбцы которой — координаты векторных полей В1, В2, adAB1 и adAB2. Выделив в ней минор четвертого порядка, составленный из второй, третьей, пятой и шестой строк,
¡Г е2г
получим, что модуль этого минора равен – = 0. Таким образом,
Тт
dimspan(H1) = 4.
Перейдем к анализу инволютивности. Пополнение семейства Н1 состоит из векторных полей [В1, adAB1], [В1, adAB2], [В2, adAB1],
[В2, еЛаВ2], [adABl, ЕЛАВ2].
Вычислим координаты векторного поля [В1, adAB1]. Рассмотрим матрицу, составленную из матрицы и1 и столбца координат векторного поля [В1, adAB1]. Выделим из нее минор пятого порядка, составленный из строк со второй по шестую. В результате соответствующих
¡2 е4 в
вычислений получим, что модуль этого минора равен ——— и отличен от нуля при в = 0.
Таким образом, ранг рассматриваемой матрицы больше ранга матрицы и1 и распределение, порожденное семейством Н1, неинволю-тивно. Следовательно, система (11) не преобразуется к каноническому виду.
Однако остается возможность преобразования системы (11) к квазиканоническому виду с индексами п1 = 2 и п2 = 3, если размерность инволютивного замыкания распределения, порожденного семейством Н1, равна пяти. Для этого вычислим ранг матрицы С, по столбцам которой записаны координаты векторных полей В1, В2, adAB1, adAB2, [В1, adAB1], [В2, adAB1]; ранг равен шести при в = 0.
Отметим, что определитель матрицы С равен нулю при в = 0, т.е. при прямолинейном движении с отсутствием эффекта поперечного псевдоскольжения (увода), однако при отклонении угла в от нуля он становится отличным от нуля.
Из проведенного анализа следует, что система (11) приводится только к квазиканоническому виду с индексами п1 = п2 = 2.
Отметим, что в силу инволютивности распределения Н0 можно выбрать такую систему координат, в которой два последних уравнения системы квазиканонического вида не содержат управлений. Для этого достаточно использовать в качестве координатных функций функционально независимые первые интегралы распределения Н0. Такой вид системы называют специальным квазиканоническим видом.
Таким образом система шестого порядка с двумя управлениями преобразуется к специальному квазиканоническому виду с индексами п1 = п2 = 2:
¿1 = ^,¿2 = /1(2, п) ди(г,п)и1 912(2, п)«2;
¿з = ¿4, ¿4 = /2(2, п) 921(2, п)и1 922(2, п)«2;
, ч (13)
г}1 = /з(2,п);
, Г]2 = /4(¿,n), где 2 = (¿1, ¿2, ¿3, ¿4)т, п = (п1, П2)Т.
Уравнения для функций ф, которые следует взять в качестве координатных для преобразования системы (11) к квазиканоническому виду (13), имеют следующий вид:
Вхф = 0,В2ф = 0,
т. е.
дф дф =0; тьд/З 3 дш ‘ (14)
_ вдф дф = 0 (14) V дв дь
Поскольку распределение Н0 инволютивно и регулярно при V = 0, то система (14) линейных уравнений в частных производных имеет четыре первых функционально независимых интеграла. Первые три можно выбрать так: ф1 = х, ф2 = у, ф3 = ф, поскольку ни в одном уравнении системы нет производных по этим переменным.
Решив систему (14), получим четвертый первый интеграл в виде
ф4 = ьв–^ш.
т1Г
В результате имеем, что для преобразования систему (11) к виду (13) необходимо использовать замену переменных:
¿1 = х, х2 = ЬАх = V соэ(в ф);
= у, ¿4 = Ьлу = V й1п(в ф), (15)
П1 = Ф, П2 = ьв–
т1Г
Обратная подстановка имеет вид
х = ¿1, у = ¿з, Ф = П1;
ь = V 4 ¿4;
в = агс^ ¿4 _ щ; (16)
¿2
ш = 3 (/А2 ¿4 ^arctg _ _ П2
Отметим, что ограничение г2 = 0 не является существенным, поскольку вместо функции aгctg г можно использовать функцию двух переменных ataп2(z4, ¿2), определенную при всех значениях аргументов.
Для синтеза управлений нет необходимости записывать систему (11) в специальном квазиканоническом виде (13) относительно переменных ¿,п, поскольку эта система предназначена для получения
программного и стабилизирующего управлений, расчетные формулы для которых удобнее выразить сразу в исходной системе координат. Представим систему (13) в виде так называемой “заготовки”:
¿1 = ¿2;
Cf af сТ аТ
¿2 = йт(в ф)-
Cf т
-т Й1п(в ф) • «1 (сОэ(в ф) в Й1п(в ф))«2;
¿э = ¿4;
Cf af сг а. (17)
¿4 = – соэ(в ф) ———
cf т
соэ(в ф) • «1 (вт(в ф) – в соэ(в ф))«2;
т
ГЦ = ш;
сг аг I , ^Т
Т/2 =–1 г- – ^ш.
т V
Правые части системы (17) записаны относительно старых переменных. Далее будем использовать как систему (13), так и ее запись в виде (17).
Построение программного движения. Применительно к проблеме управления мобильным роботом типичной является задача о перемещении по заданной на плоскости траектории из начальной в конечную точку. Такая программная траектория для системы (11) будет траекторией по части переменных, заданных на отрезке [0, Т]. Найдем программную траекторию по всем переменным и програмное управление, т.е. определим программное движение (¿*(^),П*(^),«*(¿)). Отметим, что замена переменных (15) такова, что программная траектория х = х*(£), у = у*(£) при переходе к переменным (¿,п) не преобразуется.
Поэтому ¿1 = х*(£) = ¿*(£), ¿э = у* (£) = ¿*(£). Вычислим ¿*(£) = ¿¿*(£)/^£, ¿4(^) = а также и ¿¿4(£)/^£
и подставим в первые четыре уравнения системы (13). Тогда из второго и четвертого уравнений получим систему линейных алгебраических уравнений относительно и1, «2. Коэффициенты этой системы — функции от ¿, п1 и п2. Для того чтобы она имела единственное решение, достаточно, чтобы матрица С, составленная из коэффициентов при управлениях, была невырожденной при любых значениях ¿, п1 и п2. Условие detG = 0 называют условием регулярности квазиканонического вида.
Кроме того, необходимо убедиться, что траектории и п2(£) не “уйдут на бесконечность” за конечное время.
Используя систему (17), матрицу О удобно записать в исходных переменных:
– — эт(в ф) еоэ(в ф) в й1п(в ф)
О = I —
— еоэ(в ф) й1п(в ф) – в соэ(в ф)
с—
Ее определитель detО =–. Таким образом, условие регулярности
т
выполнено при любых значениях переменных. Это позволяет записать решение указанной линейной системы относительно управлений в виде
и2(ь,п1 ,щ)
V V/(¿1 (^¿Жпиъ)) V¿4(ь))) ,
где ¿ 2(Ь) = х2(Ь), ¿3(Ь) = у2(Ь), ¿2(Ь) = Х2(Ь), ¿2(Ь) = у2(Ь), ¿2(Ь) = Х2(Ь), ;г42 (Ь) = у2(Ь),а
с— а— сг аг
,¿2 = sin(e ф)-
m
– — sin(e ф)и1 (cos(e ф) в sin(e ф))и2
, ,п ..Cf аf Cr ar
Zt = – cos(e Ф)—Л-
m
— cos(e ф)и1 (sin(e ф) – в cos(e ф))и2
(16)
(16)
Найденные управления зависят не только от Ь, но и от п1, п2. Однако, подставив в последние два уравнения системы (13) зависимости ¿2(Ь), г = 1,…, 4, и задав начальные условия п1(0), П2(0), получим для этих уравнений задачу Коши относительно неизвестных п1 и п2. Решая ее, находим п1 (Ь), п2 (Ь). Если полученные решения определены при Ь е [0, Т], то, подставив их в (18), находим программные управления и1(Ь),и2(Ь), которые являются только функциями времени
и1(Ь) = о-1 (- (^^(Ь)) >) (¿2(Ь) , (19)
и2(Ь) ) V К^2(Ь),п2(Ь)) ) Ц*(Ь) )]
где ¿ 2(Ь) = (¿1 ^,¿2(ь), ¿3(t),¿4(Ь))т, п2(Ь) = (п1 (ь),п2(ь))
Таким образом, получено программное движение (¿* (£), п* (£),«*(£)), порожденное программной траекторией по части переменных ¿1, ¿э с заданными начальными условиями п(0).
Аналитическое нахождение программных управлений и программной траектории по переменным п1 и п2 в общем случае затруднительно, поскольку аналитически решить задачу Коши относительно переменных п1 и п2 не удается. Для решения этой задачи воспользуемся численным методом.
Для простоты будем предполагать, что интегрирование выполняется методом Эйлера с шагом Д£. В момент времени £ = 0 для системы (13) известен полный вектор состояния ¿*(0), ¿2 (0), ¿3 (0), ¿4 (0), п1(0), П2 (0), а также ¿2 (0) и ¿4 (0).
По формуле (18) вычислим программные управления «1 и «2 при £ = 0. Подставив значения всех переменных при £ = 0 в правую часть последних двух уравнений системы (13), выполним первый шаг интегрирования и вычислим п* в момент £ = Д£. Отметим, что при £ = Д£ значения ¿*(£), а также ¿2 (£), -¿4 (£) известны, а значения п*(£) вычислены. Следовательно, для последних двух уравнений системы (13) можно выполнить еще один шаг интегрирования и по формуле (18) получить значения программного управления на текущем шаге.
Таким образом, вычислив значения программных управлений и значения переменных п* на к-м шаге интегрирования, можно найти их значения на (к 1)-м шаге, получить численное решение задачи Коши по переменным п и определить программное управление на отрезке [0,Т].
Численный метод нахождения программного движения описан для случая, когда система записана в специальном квазиканоническом виде. Однако он применим и в случае системы произвольного квазиканонического вида, когда управление присутствует в последних уравнениях, поскольку в начале каждого шага интегрирования программные управления известны.
Нелинейная стабилизация программного движения. Рассмотрим задачу стабилизации заданного программного движения ^*(£),п*(£),«*(£)). Пусть Д«1 и Д«2 — дополнительные составляющие управления, обеспечивающие стабилизацию, и «1 = «1(£) Д«1,
«2 = «2(£) Д«2.
Запишем систему в отклонениях от программного движения. Обозначим через Д¿i, г = 1,…, 4, и Дщ, ] = 1, 2, ошибки отработки
(21)
программной траектории. Система (13) примет вид
¿i = ¿2;
-¿2 = fi(¿,n) gii(¿,n)K(t) Aui) gi2(¿,n)(u2(t) Au2);
¿3 = ¿4;
¿4 = f2(¿,n) g2i(¿,n)(u*(í) Aui) g22(¿,n)(u2(t) Au2);
ni = f3(¿,n); k n2 = f4(¿,n).
Пусть ¿¿ = ¿2 A¿i, nj = Vj A%, i = 1,…, 4, j = 1, 2. Вычитая из уравнений (20) тождества
f ¿ 2(t) = ¿2 (t);
¿22 (t) = fi (¿2 (t),n* (t)) gii(¿* (t),n* (t)K(t)
gi2(¿ 2 (t), n 2 (t))u2(t);
¿2 (t) = ¿44 (t);
¿4 (t) = f2 (¿2 (t),n2 (t)) g2i(¿ 2 (t),n2 (t)K(t)
g22(¿ 2 (t), n 2 (t))u2(t);
V 2 = f3(¿ 2 (t),n 2 (t));
l ñl = f4(¿2(t),n2(t)), получим систему в отклонениях от программного движения
Г A¿i = A¿2;
A ¿2 = fi(A¿, An, t) gn(A¿, An,t)Aui gn(A¿, An,t)Au2;
A¿3 = A¿4;
A¿4 = f2(A¿, An, t) gn(A¿, An,t)Aui gn(A¿, An,t)Au2;
k An 2 = g(A¿, An,t), где
fi(¿,t) = fi(¿ 2 A¿,n 2 An) – fi(¿ 2(t), n2(t))
– (gii(¿ 2 A¿, n2 An) – gii(¿ 2(t), n2(t)))u2(t) (gi2(¿2 A¿, n2 An) – gn(¿2(t), n2(t))) u2(t);
f (¿,t) = f2(¿2 A¿,n 2 An) – f2(¿2(t), n2(t))
(gn(¿ 2 A¿, n2 An) – g2i(¿ 2 (t), n2 (t)))u2 (t) (gn(¿2 A¿,n 2 An) – gn(¿ 2(t), n2(t)))u2(t);
(22)
q = (f3(¿ 2 A¿,n2 An) – f3(¿2(t), n2(t)), f4(¿2 A¿, n2 An)-
-f4(¿ 2 (t), n 2 (t)))T. Для решения задачи асимптотической стабилизации положения
равновесия нестационарной системы (22) применим метод нелинейной стабилизации [8] и зададим стабилизирующее управление в виде
Aui
Д«2
= G –
Тогда
/i(Az, An, t) /(Az, An, t) kiiAzi ki2Az2 ki3Az3 kMAz4
k2iAzi ^22 Az2 k23Az3 ^24 AZ4
f AZi = Az2; AZ2 = -kiiAzi – ki2Az2 – ki3AZ3 – kwAz4; AZ3 = AZ4;
„ Az4 = -k2iAzi – ^22AZ2 – k23Az3 – ^24AZ4,
. (23)
или в матричной форме
где матрица K имеет вид
A z = KAz,
(24)
(25)
( О
K=
О
-kii -ki2 -ki3 -k
ь14
(26)
-k21 -k22 -k23 -k24 /
Выбором матрицы коэффициентов К можно обеспечить экспоненциальную устойчивость нулевого решения стационарной системы (24), и построенное стабилизирующее управление обеспечивает стабилизацию программной траектории по переменным г.
Отметим, что используя замену переменных (15), можно записать стабилизирующее управление в исходных переменных.
Условия, при которых нулевое положение равновесия системы (22) при управлении (23) равномерно асимптотически устойчиво, дает следующая теорема, являющаяся обобщением результата, приведенного в работе [11] для систем со скалярным управлением.
Теорема 1. Если нестационарная каскадная система
Az = KAz, An = g(Az, An, t), g(0,0,t) = О,
(27)
(28)
такова, что функция д(Дг, Дп, £) непрерывно дифференцируема в
дд
окрестности точки (Дг, Дп) = (0, 0), матрица Якоби —д”^”
ограничена по норме равномерно по £ в некоторой замкнутой ограниченной окрестности точки (Дг, Дп) = (0,0), система нулевой
1
О
О
О
О
1
динамики ДП = д(0, Дп,£) равномерно асимптотически устойчива в точке Дп = 0, а линейная подсистема (27) асимптотически устойчива в точке Д- = 0, то каскадная динамическая система (27), (28) равномерно асимптотически устойчива в положении равновесия (Д-, Дп) = (0, 0).
Доказательство теоремы 1 практически полностью повторяет доказательство аналогичной теоремы, приведенное в [11] для скалярного случая, поскольку последнее базируется только на предположении об асимптотической устойчивости замкнутой системы (27) и не учитывает число управлений в системе (22).
Для исследования выполнения условий теоремы 1 запишем правые части последних двух уравнений системы (17) в переменных -,п’
п1 = Со(л/4 — – пО – п2);
V ¿4 ^2
П/2 = -с^arctg — – пО
__
г! -Каг^ — – п^ – %)//-Г —2-_ г2 _
(/г2 г2 (arctg г4 – п1) – %) г2 .
Со
z2
mlf lr lf lf lr 1Г2
где Со = ——; ci = Cr–—; С2 = Cr–—.
J mlf mlf
Запишем систему в отклонениях
Д»1 = C0(^(z2 (t) AZ2 )2 (z4 (t) Az4)2 (arctg( ^ (t) AZ4
– nl(t) – Ani) – ^(t) – Дп2)-
-C0(Vz2(t)2 (t)2(arctg Ц) – nl(t)) – П2(t)
/ (t) Az4 Л
ДП2 = -ci( arctg – «Й(t) – V)
C0 (V((z2 (t) AZ2)2 (z4 (t) Az4 )2) (arctg
-ni(t)-Ani)-n2 (t)-A^2
(t) Az2)2 (z4 (t) Az4)2
V (z2 (t) Az2)2 (z4 (t) AZ4)^ -ci ( arctg §§ -ni (t)) -co(Vz2 (t)2 (t)2 ) (arctg Ц) –
-rni(t)) – n2(t)) Г(t)C (t)2 – .
(29)
(t) Az4 (30)
C2
Рассмотрим такую програмную траекторию (г *(£), п*(£)), Для которой г* (£) > М2 > 0 и * (£)| < М1, где М1, М2 — некоторые константы. Отметим, что требование ограниченности программной траектории является достаточно естественным, а условие положительности ¿2 (£) не является жестким, так как связано с использованием функции агс^(-), которую при численных расчетах можно заменить функцией двух переменных а1ап2(а,6).
При сделанных предположениях о программной траектории правая часть системы (30) непрерывно дифференцируема в окрестности точки (Дг, Дп), а матрица Якоби ограничена по норме равномерно по £ в некоторой замкнутой ограниченной окрестности указанной точки.
Система нулевой динамики имеет вид
где v2(t) = ^*2(t)2 (t)2; Yi(t) = ci – cov *(t)ö(t); 72(t) = -coö(t);
нарную систему, для которой условия асимптотической устойчивости известны [12].
Если нулевая динамика является равномерно асимптотически устойчивой, то все условия теоремы 1 выполнены. Следовательно, программное движение стабилизируется управлением (23).
В частном случае, когда у* (£) = у0 постоянна, система (31) будет стационарной. Собственные числа ее матрицы равны
Из положительности со, с1, с2 и у0 вытекает, что ЯеЛ12 < 0 и положение равновесия стационарной системы асимптотически устойчиво.
“Четырехколесная” модель. Рассмотрим “четырехколесную” модель мобильного робота при тех же предположениях, что были сделаны для “велосипедной” модели. Расчетная схема приведена на рис. 2 (а — половина расстояния между колесами, остальные обозначения аналогичны обозначениям рис. 1).
Для углов бокового скольжения передних и задних колес с учетом того, что за положительное направление отсчета углов принято направление против хода часовой стрелки, получены соотношения
Д771 = -cov2(t)A^i – С0ДП2; Д??2 = 7l(t^1 72^)ДП2,
(31)
представляет собой линейную нестацио-
С0С2 ± у7c^ – 4vgcocT 2vo
(32)
ßf 1 = arctg
v sin в lf w
ö = af 1(v, в, w) – ö;
v cos в – aw
Рис. 2. “Четырехколесная” модель мобильного робота
v sin в lf ш Pf 2 = arctg–—f–д = af 2 (v,p,^) — д;
v cos в lf Ш
v sin в — lrШ , ,
Pr1 = arctg—= ar1 (v,p,^);
Pr2 = arctg
v cos P — аш v sin P — lr ш
ar2 (v, P, ш).
v cos в аи
В предположении о малости угла скольжения и угла поворота передних колес получим
_ Cf 1 (af 1 -J) Cf2(а/2_8) Cr1 QV1 ^2ar2 _
mv v Cf 1 а/1 c/2 а/2 cr1 аГ1 cr2 аГ2 с/1 c/2 в _—и –U1—U2;
mv mv v
JUJ _ _(cf 1(а/1 _ 8) cf2(af2 _ 8))1f (cf2(af2 _ 8)-—с/1 (af 1 _ 8))а8 (сГ1 аГ1 сГ2ar2)Zr _
_ _(с/1 а/1 с/2a/2)1/ (с/2a/2 _ с/1 а/1 )au
(cr1 аГ1 сг2аГ2)1r (с/1 с/2)1/U1 (с/1 _ с/2)а^2;
v _ U2;
^ _ u ;
X _ v cos(e Ф); yy _ v sin(e Ф).
(33)
Во втором уравнении системы (33) слагаемым ^ 1 – Cf2)ам2 можно пренебречь. Тогда системе (33) будут сопоставлены векторные поля при управлениях
в = Cf 1 ^ 2 д ^ д ; в = вд д
В1 = -^ 75^—; В2 =–^ ,
шу др дш V др д-
где „ = a(cf2af2 – cf 1) lf (cf 1 ^2)
д 75 3 .
Используя допущение о малости углов скольжения, упростим выражения для аГ1, аГ2, ад, а^ по аналогии с выражением (10). Вычислим коммутатор векторных полей В1 и В2:
[В1,В2] = а
3 ша V – ша
^ш(ав ^) ^ ш(ав – ^ а =
(у – ша)2 (у ша)2 ) дв дв
Таким образом, векторные поля В1 и В2 не коммутируют. Проверим инволютивность распределения эрап(В1, В2), для чего рассмотрим матрицу и, по столбцам которой записаны координаты векторных полей В1, В2 и [В1, В2]. Минор, составленный из элементов трех первых строк матрицы и, равен 7576 и отличен от нуля при 75 = 0 и 7б = 0. Таким образом, распределение эрап(В1,В2) не инволютивно, но инволютивным будет распределение эрап(В1, В2, [В1, В2]).
Действительно, поскольку последние три компоненты порождающих векторных полей В1, В2, [В1, В2], соответствующие координатам д д д
—, — и —, равны нулю, компоненты коммутаторов [В1, [В1, В2]] и дф дх ду
[В2, [В1,В2]], соответствующие указанным координатам, также будут равны нулю. Следовательно, любой минор четвертого порядка матрицы, по столбцам которой записаны координаты векторных полей В1, В2, [В1,В2] и [В1, [В1 ,В2]], содержит нулевую строку и равен нулю. То же положение верно для векторного поля [В2, [В1, В2]].
Для распределения эрап(В1, В2, [В1, В2]) функции ф1 = ф, ф2 = х и ф2 = у являются первыми интегралами. Размерность распределения равна 3 всюду, кроме точек, где 75 = 0 или 76 = 0. Следовательно, только три первых интеграла можно использовать для приведения системы к квазиканоническому виду в отличие от “велосипедной” модели, где таких интегралов было найдено четыре. Это означает, что при любых заменах переменных в преобразованной системе управления не будут входить в лучшем случае только в три уравнения. Поэтому систему (33) нельзя преобразовать к специальному каноническому виду (13).
Для преобразования системы (33) к квазиканоническому виду (12) воспользуемся заменой переменных
¿1 = х, ¿2 = V еоэ(в ф);
¿3 = У, ¿4 = V эш(в ф); (34)
П1 = Ф, П2 =
Функцию п2 можно было выбрать и иначе. Единственное требование здесь — невырожденность и обратимость замены. Однако, как отмечено ранее, найти такую функцию при условии, что управление не будет входить в правую часть соответствующего дифференциального уравнения, невозможно.
Обратная подстановка имеет вид
х = ¿1, у = ¿3, Ф = ш;
V = V ¿2 ¿4;
¿4 (35)
в = аг^–П1;
¿2
Ш = П2.
Запишем систему (33) в новых переменных в виде “заготовки”
¿1 = ¿2;
Cf1 af 1 Cf2af 2 сГ1аГ1 сГ2аГ2
¿2 = sin(e ф)-
m
С/1 С/2 sin(e ф) • М1 (cos(e ф) в sin(e ф))и2
(35)’
(35)
т
¿¿3 = ^4,
¿4 = Г- еов(в ф) аf 1 Cf2af2 ^ ^г2
т
Cf 1 Cf2 еоэ(в ф) • и1 (э1п(в ф) – в соэ(в ф))и2
П1 = Ш|(35);
. _ ((crlarl ^2аГ2)/г – (cf laf 1 Cf2af2)/f П2 = ^ Т
(cf 2af2 – cf 1af 1)а 1 cf2)1f ^ т и1
Т / (35)
(36)
Для “четырехколесной” модели получить программное управление и программные траектории (£), П2 (¿) можно лишь численно. Алгоритм вычисления в целом аналогичен алгоритму, изложенному для “велосипедной” модели. Отличие состоит в том, что на каждом шаге интегрирования из первых четырех уравнений с использовани-
ем заданных зависимостей г* (£), (£) и вычисленных значений п*, п2 получаем и* и Ц, что позволяет проинтегрировать последние два уравнения на следующем шаге.
Отметим, что устойчивость уравнений нулевой динамики системы в отклонениях от заданного программного движения удается исследовать только численно.
Исследование устойчивости нулевой динамики. Пример 1. Рассмотрим “велосипедную” модель со следующими параметрами [2]’ ш = 150 кг, 3 = 82 кг-м2, 1Г = 0,4 м, ^ = 0,6 м. Коэффициенты Cf и ^ примем равными 4480 и 6720 Н/рад соответственно, что соответствует хорошему сцеплению колес с поверхностью.
Рассмотрим программную траекторию по переменным х и у, заданную в виде
х = х* (£) = г* (£) = а вт(ш£); у = у * (£) = -3 (£) = Ь сов(ш£), где а = 4,5м, Ъ = Зм, а; = тг/10, í £ |0,10].
(37)
Рис. 3. Программная траектория для переменных ni, П2
Рис. 4. Графики программных управлений
На рис. 3 приведены графики программных траекторий (£) и П2(£), а на рис.4 — графики программных управлений, расчитан-ные численно при заданном изменении х и у и начальных условиях П1 (0) = 0,055893, п2(0) = 0,11408. Отметим, что выбор начальных условий существенно влияет на вид получаемой программной траектории. Таким образом, получено программное движение ^ *(¿),п *(¿),и*(£)). Рассмотрим задачу его стабилизации.
Как показано ранее, управление вида (23) будет равномерно стабилизировать нулевое положение равновесия нестационарной системы в отклонениях от программного движения, если будут выполнены условия теоремы 1. Для заданной программной траектории (37) эти условия выполняются. Покажем, что система нулевой динамики (31) равномерно асимптотически устойчива.
Построим для системы (31) функцию Ляпунова. Воспользуемся тем, что при V* (£) = Vo система стационарная. При заданных числовых параметрах по формуле (32) получим ЯеЛ12 < -2,5420, и, значит, система нулевой динамики асимптотически устойчива. Решив соответствующее (стационарное) уравнение Ляпунова
Используем функцию V(n) = nTPn в нестационарном случае. При этом производная V(n) есть квадратичная форма с матрицей W(t), и численный анализ показывает, что она отрицательно определена по обобщенному критерию Сильвестра [13], поскольку WM(t) < -1,40 < 0, а det W(t) > 1,26 > 0.
Поскольку функция Ляпунова V(n) не зависит от времени, все условия [14, 15] равномерной асимптотической устойчивости выполняются и положение равновесия системы нулевой динамики будет равномерно асимптотически устойчиво.
На рис. 5 приведены программное и реализующееся изменение переменной n1, а также программная и реализующаяся линии движения на плоскости при наличии отклонений начальных условий от программных значений.
Пример 2. Для “четырехколесной” модели примем а = 0,4, а С/1 = С/2 = С//2, cr1 = cr2 = cr/2. Программную траекторию по ¿1 и ¿3 зададим соотношениями (37). На рис. 6 приведены графики изменения переменных n1 и n2 при различных отклонениях начальных
PA ATP = -Q,
тельно определенную матрицу P
0,60646 -0,00186 -0,00186 0,014024
Рис. 5. Пример стабилизации программной (1) и реализующейся (2) траекторий
Рис. 6. Исследование устойчивости нулевой динамики (1 — программная траектория)
значений от программных значений п * (0) и п2 (0). Из графиков можно видеть, что нулевая динамика “четырехколесной” модели является асимптотически устойчивой.
Для детального исследования свойств нулевой динамики “четырехколесной” модели можно использовать свойства нулевой динамики “велосипедной” модели. В этом случае нулевая динамика “велосипедной” модели, записанная в переменных (34) линейная, стационарная и асимптотически устойчивая при V* (£) = у0. Построив стационарную функцию Ляпунова, ее можно использовать для доказательства равномерной асимптотической устойчивости нулевой динамики “четырехколесной” модели.
Сравнение свойств моделей. На численном примере сравним свойства “велосипедной” и “четырехколесной” моделей. Примем
г* (¿) = 0,Н; -з* (¿) = 0,21
Стартовую точку, лежащую вне программной траектории, зададим одинаковой для обеих моделей. Одинаковые начальные значения присвоим остальным переменным и параметрам моделей.
О 2 4 6 8 10 12 14 16 18 20 -0.5 0 0.5 1 1.5 2
t, С X, м
Рис. 7. Сравнение траекторий движения (1 — траектория “велосипедной” модели, 2 — траектория “четырехколесной” модели, 3 — программная траектория)
На рис. 7 приведены графики изменения переменной Для ДВУХ моделей, а также программная и реализующиеся линии движения на плоскости. Можно видеть, что для “четырехколесной” модели стабилизация по переменной щ = Ф происходит быстрее, что соответствует более естественному выезду на заданную линию движения, в то время как для “велосипедной модели” имеется участок, где реализуется движение вдоль прямой линии, при котором продольная ось симметрии робота имеет небольшой угол с линией движения. Реализующиеся линии движения центров масс обеих моделей совпадают, поскольку имеют место одинаковые отклонения и применяется одинаковая стабилизирующая обратная связь.
Выводы. Разработанная методика позволяет получить программные управления и управления в виде обратных связей, решающие задачу отслеживания программной траектории движения робота. Программную траекторию можно строить на основе пары параметрических сплайнов рх (т), ру (т), которые задают на плоскости линию движения, проходящую через заданные точки, и профиля скорости
т = / (*).
Предположения о малости углов скольжения и углов поворота колес накладывают ограничения на класс траекторий, поскольку кривизна траектории и заданный профиль скорости определяют реализующиеся углы.
Работа выполнена при финансовой поддержке гранта РФФИ № 06-07-89265 и программы Минобрнауки “Развитие научного потенциала высшей школы на 2006-2007гг.”
СПИСОК ЛИТЕРАТУРЫ
1. Stotsky A., Hu X., Egerstedt M. Sliding mode control of a car like mobile robot using single track dynamic model // Proc. of the 14th Word Congress of the IFAC, Beijing, China, 1999. – V. B. – P. 119-124.
2. Бурдаков С. Ф., Мирошник И. В., Стельмаков Р. Э. Системы управления движением колесных роботов. – Спб.: Наука, 2001. – 227 с.
3. Крищенко А. П. Преобразование многомерных аффинных управляемых динамических систем // Управление нелинейными системами: Сб. статей. – М.: ВНИИ системных исследований, 1991.
4. Крищенко А. П., Клинковский М. Г. Преобразование аффинных систем с управлением и задача стабилизации // Дифференциальные уравнения.
– 1992. – Т. 28. – С. 1945-1952.
5. Ж е в н и н А. А., Крищенко А. П. Управляемость нелинейных систем и синтез алгоритмов управления // Доклады АН СССР. – 1981. – Т. 258, № 4.
– С. 805-809.
6. К р и щ е н к о А. П. Стабилизация программных движений нелинейных систем // Известия АН СССР. Техническая кибернетика. – 1985. – № 6. – С. 103112.
7. К р и щ е н к о А. П. Преобразование нелинейных систем и стабилизация программных движений // Труды МВТУ, № 512. – М., 1988. – С. 69-87.
8. К р а с н о щ е ч е н к о В. И., К р и щ е н к о А. П. Нелинейные системы: геометрические методы анализа и синтеза. – М.: Изд-во МГТУ им. Н.Э. Баумана, 2005. – 520 с.
9. Isidori A. Nonlinear control systems. 3rd ed. London, 1995. – 587 p.
10. Андриевский Б. Р., Фрадков А. Л. Избранные главы теории автоматического управления. – Спб.: Наука, 1999. – 494 с.
11. Ткачев С.Б. Стабилизация программного движения, соответствующего заданному изменению выхода // Вестник МГТУ им. Н.Э. Баумана. Сер. “Естественные науки”. – 2006. – № 4. – C. 43-60.
12. Демидович Б. П. Лекции по математической теории устойчивости. – М.: Наука, 1967. -472 с.
13. М е р к и н Д. Р. Введение в теорию устойчивости движения. – М.: Наука, 1987.
– 304 с.
14. K h a l i l H. Nonlinear systems. 3rd ed. Prentice Hall, New Jersey, 2002. – 750 р. 15. Красовский Н. Н. Некоторые задачи теории устойчивости движения. -М.: Физматгиз, 1959. – 211 с.
Статья поступила в редакцию 29.06.2007
Сергей Борисович Ткачев родился в 1961 г., окончил в 1984 г. МВТУ им. Н.Э. Баумана. Канд. физ.-мат. наук, доцент кафедры “Математическое моделирование” МГТУ им. Н.Э. Баумана. Лауреат премии Правительства РФ в области науки и техники. Автор 16 научных работ в области математической теории управления.
S.B. Tkachov (b. 1961) graduated from the Bauman Moscow Higher Technical School in 1984. Ph. D. (Phys.-Math.), assoc. professor of “Mathematical Simulation” department of the Bauman Moscow State Technical University. Winner of the Prize of the Russian Federation Government in the field of Science and Technology. Author of 16 publications in the field of mathematical theory of control.