CC BY
12
ISSN 2079-3316 ПРОГРАММНЫЕ СИСТЕМЫ: ТЕОРИЯ И ПРИЛОЖЕНИЯ № 1(10), 2012, с. 3-29 УДК 517.977
А. П. Маштаков
Алгоритмическое и программное обеспечение решения конструктивной задачи управления неголономными пятимерными системами
Аннотация. В статье рассмотрена задача управления нелинейными пятимерными системами с двумерным линейным управлением. Для приближенного решения задачи в классах кусочно-постоянных и оптимальных управлений разработан итерационный алгоритм, основанный на построении нильпотентной аппроксимации. Подробно излагается реализация алгоритма в виде параллельного программного комплекса, разработанного в среде Wolfram Mathematica. Приведены результаты испытания комплекса на задаче о качении шара по плоскости и задаче управления машиной с двумя прицепами.
Ключевые слова и фразы: Оптимальное управление, двухточечная задача управления, итерационный алгоритм, машина с двумя прицепами, нильпотентная аппроксимация, качение шара по плоскости.
1. Введение
Одной из важнейших задач робототехники и инженерии является задача перевода механической системы в требуемую конфигурацию. Например, это может быть вывод спутника на орбиту, управление мобильным роботом, вращение твердого тела в руке робота-манипулятора и многое другие. В математике подобная задача формализуется как конструктивная задача управления. Имеется динамика механической системы, описываемая системой дифференциальных уравнений с функциональными параметрами (управлениями). Требуется найти управляющие воздействия в заданном классе функций, при применении которых система переходит из начального состояния в заданное целевое состояние.
Работа поддержана Российским фондом функдаментальных исследований, проект 12-01-00913-а.
© А. П. Маштаков, 2012
© Программные системы: теория и приложения, 2012
В работе рассматривается двухточечная задача управления следующего вида:
(1) q = ui(t)Xi(q)+ U2(t)X2(q),
(2) q(0) = q°, q(T)= q\
где пространство состояний Q э q — это связное пятимерное гладкое многообразие, управление принимает значения на двумерной плоскости (U\,U2) € R2, а гладкие векторные поля Х\, X2 удовлетворяют условию полного ранга [1] на многообразии Q (система (1) вполне управляема, то есть для любых состояний q° ,q} € Q существует траектория системы (1), удовлетворяющая условиям (2)). В настоящее время не существует методов явного решения задачи (1), (2) в общем случае. Удовлетворительное решение имеется лишь для некоторых специальных классов систем. Тем не менее такие задачи возникают в инженерии, где достаточно приближенного решения, погрешность которого не превышает наперед заданной. В работе предлагается метод конструирования управлений (ui(t),u2(t)), переводящих систему (1) за время Т > 0 из начального состояния q° в терминальное состояние q1 с любой наперед заданной точностью е > 0, то есть в такое состояние q1, что p(q1,q1) < £, где р — расстояние на многообразии Q.
Например, если Q — область в R5, то р = \j^5-i(?l — q})2.
Системы вида (1) характеризуются тем, что размерность пространства управлений меньше размерности пространства состояний: 2 = dimR2 < dim Q = 5, однако любые точки пространства состояний могут быть соединены траекторией системы. В теории управления такие системы называются вполне неголономными. Нелинейные системы (1), линейно зависящие от управлений, количество которых меньше, чем размерность пространства состояний, характеризуются тем, что возможность перемещения неодинакова по различным направлениям. Величина смещения в направлении полей X} и Х2 за малое время t есть O(t), в направлении коммутатора (см. раздел 2) Х3 = [Х}, Х2] есть 0(t2), в направлении Х4 = [Х}, Х3] и Х5 = \Х2,Х3] есть 0(t3). В силу этой анизотропии пространства состояний, задача управления для таких систем весьма нетривиальна.
Конструктивная задача управления активно изучается в последнее время в нелинейной теории управления. Эта задача имеет удовлетворительное решение в случае систем, находящихся в цепной форме, и приводимых к такой форме обратной связью [2,3], а также для
дифференциально плоских систем [4]. Однако системы общего положения с двумя управлениями имеют вектор роста (2, 3, 5) (см. раздел 2), потому эти результаты неприменимы к таким системам.
В данной работе представлен способ отыскания приближенного решения задачи (1), (2), основанный на построении нильпотентной аппроксимации. Идея метода заключается в том, что исходная система приближается нелинейной системой более простой структуры, для которой точно решается задача управления. Затем найденные управления подставляются в исходную систему. Если состояние, достигнутое после применения найденного управления, отличается от желаемого состояния в пределах допустимой погрешности, то задача считается решенной, иначе процедура повторяется с новыми граничными условиями. Точное решение задачи управления нильпотентной системой дает приближенное решение исходной задачи управления в малой окрестности целевой точки. Метод нильпотентной аппроксимации применим к задачам управления общего вида; существенно только уметь решать задачу управления для нильпотентной аппроксимации. Этот метод предложен в работах [5-9].
В данной работе рассматривается задача управления нелинейными пятимерными системами общего вида, линейно зависящими от двумерного управления. Конкретными примерами систем такого вида, которые представляют самостоятельный интерес из-за своего прикладного значения в робототехнике, являются система, моделирующая качение без прокручиваний и проскальзываний шара по плоскости, и система, моделирующая движение машины с двумя прицепами по плоскости (см. раздел 3).
2. Математические определения
В этом разделе описаны используемые в статье математические понятия. Основным математическим аппаратом, применяемым для решения задачи управления (1), (2), является геометрическая теория управления [1] и субриманова геометрия [10].
Траекторией системы (1), выходящей из точки д° € Q под действием управления (и1(г),и2(г)), Ь € [0,Т], будем называть кривую ^(Ь) : [0,Т] ^ Q, являющуюся решением задачи Коши
'п(г) = х1(г](г))и1(г) + х2(^(г))и2(г), г/(0) = ч0, * € [0,т].
Управления щ(Ь) являются кусочно-непрерывными функциями. Точка д = ц^о) € Q для фиксированного 1о € [0, Т] называется достижимой из д0 за время ¿о. Множеством достижимости из точки д0 за произвольное время Ь € [0, то) называется множество
Линейное пространство Ь называется алгеброй Ли, если на нем определена линейная по каждому аргументу, кососимметричная и удовлетворяющая тождеству Якоби бинарная операция (скобка Ли) [а,,Ь], а,Ь € Ь. Пространство всех гладких векторных полей Уео(^) образует бесконечномерную алгебру Ли, где в качестве скобки Ли выступает коммутатор векторных полей, который в координатах имеет выражение:
Наименьшая подалгебра Ли в Уео(^), содержащая векторные поля Хг, называется алгеброй Ли, порожденной полями Х\,Х2, и обозначается Ые(Х1, Х2).
Теорема Рашевского-Чжоу [1] утверждает, что любые две точки д0, д1 € Q достижимы друг из друга (^^д0, д1) < то), если в любой точке д € Q линейная оболочка элементов алгебры Ли Ые(Х1, Х2), порожденной полями Хъ, совпадает с касательным пространством (условие полного ранга). Предполагается, что система (1) удовлетворяет условию полного ранга, поэтому вполне управляема. Для вполне управляемых систем имеем Уд0 € Q Адо = Q.
Определим субриманову длину траектории ^:
Система (1) полного ранга индуцирует субриманово расстояние в Q, определяемое как
Адо = {д € Q\g достижима из д0 за некоторое время £ > 0}.
[А,В] = — А В, А, В € Уео(д).
(3)
(4)
где инфинум берется по всем траекториям г/, соединяющим д0 с д1.
Зафиксируем q € Q и обозначим через Ls(g) векторное пространство, порождаемое значениями в точке q скобок Ли полей X}, Х2 длины < s, s = 1, 2,... (сами поля Xi — скобки длины 1):
L1(q) = span(X}(q),X2(q)), L2(q)= span(L} (q) + [Xi,X2](q)),
Ls(q) = span(Ls-1(g)+
+ j[Xis, [Xia_!,. . . [Xi2 ,Xh ] . .. ]](q)\n, ...,ia € {1, 2}}).
Условие полного ранга гарантирует, что для любой точки q € Q существует наименьшее целое число г = r(q) такое, что dim Lr (q) = 5.
Обозначим ns(q) = dimLs(g), s = 1, .. . ,r. Вектором роста системы (1) в точке q называется вектор
(ni(q),...,nr (q)).
Точка q называется регулярной, если вектор роста постоянен в окрестности точки q, иначе q называется сингулярной. В статье рассматриваются системы (1) в окрестности регулярных точек. Заметим, что для системы (1) общего положения точка общего положения будет регулярной, то есть вектор роста будет равен (2, 3, 5).
Управляемая система (1) называется нильпотентной, если соответствующая алгебра Ли Lie(X},X2) нильпотентна, то есть для некоторого N € N
[Xi!, [Xi2,..., [XiN ,XiN+1 ]... ]] = 0, yi},..., iN+i € {1, 2}.
3. Неголономные системы в робототехнике
Неголономными системами называются механические системы, стесненные неинтегрируемыми связями, которые налагаются на скорости точек системы, но не на положение системы. С точки зрения теории управления, управляемая система называется неголономной, если размерность множества достижимости A системы из произвольного состояния системы превосходит размерность множества скоростей В в этой точке (dim A > dim В). Система называется вполне неголономной, если размерность пространства состояний Q совпадает с размерностью множества достижимости A, но больше размерности множества скоростей В (dim Q = dim A > dim В). Система называется голономной, если dim В = dim A. Для управляемых систем (1),
линейных по управлению, эти свойства выражаются в терминах алгебры Ли, порожденной исходными полями управляемой системы. Управляемая система голономна, если размерность пространства, порожденного алгеброй Ли, совпадает с размерностью пространства допустимых скоростей: Lie(Xi, X2)(q) = span(Xi, X2)(q), Уд € Q (т.е. существует интегральное многообразие поля скоростей системы). Если же размерность пространства, порожденного алгеброй Ли, больше размерности пространства скоростей, то система неголономна.
В рассматриваемых системах (1) размерность управления меньше размерности состояния 2 = dim R2 < dim Q = 5, однако любые точки пространства состояний могут быть соединены траекторией системы (в предположении условия полного ранга). Это означает, что поставленная задача (1), (2) является задачей управления для вполне неголономных систем. Изучение различных теоретических вопросов неголономных систем представляет большой интерес и является предметом исследований многих ученых как в России, так и за рубежом [1,6,10,11].
Неголономные связи возникают в системах при качении твердых тел по поверхности без скольжения. В связи с развитием робототехники все более актуальной становится задача управления неголоном-ными механическими системами, которые представляют собой базовые модели разнообразных колесных роботов. Необходимым условием функционирования таких роботов является возможность самостоятельного перемещения. Эта задача математически формализуется как задача управления системой, которая моделирует исходную физическую систему с наложенными на нее ограничениями. Отметим, что речь идет о синтезе управления. Используя сведения о своем текущем состоянии, робот вырабатывает стратегию движения и перемещается в желаемое состояние. Для решения таких задач не требуется знания точного аналитического решения задачи управления, а достаточно приближенного решения, найденного с заданной точностью, и на первый план выдвигаются такие критерии, как скорость вычислений и простота реализации найденного закона управления.
В данной работе рассматривается задача управления нелинейными пятимерными системами общего вида, линейно зависящими от двумерного управления. Конкретными примерами систем такого вида, которые представляют самостоятельный интерес из-за своего
прикладного значения в робототехнике, является система, моделирующая качение шара по плоскости без прокручивания и проскальзывания, и система, моделирующая движение машины с двумя прицепами по плоскости. Качение шара по плоскости аппроксимирует вращение предмета в руке робота в случае, когда рука робота плоская. Исследование задачи управления машиной с прицепами стимулируется многочисленными прикладными задачами, возникающими в процессе жизнедеятельности человека. К ним можно отнести, например, задачу автоматизированной транспортировки грузов (аэропорты, вокзалы, гипермаркеты, склады), или работу в опасной среде, или в среде с заранее неизвестными свойствами (доставка в опасную зону для выполнения различных операций, связанных с риском для жизни человека).
Для решения задачи управления такими системами был применен геометрический подход. Состояние системы задается как точка пятимерного пространства. На нем заданы два гладких векторных поля. Система непосредственно может перемещаться только в направлении любой линейной комбинации этих полей. Таким образом, размерность множества скоростей системы равна двум. Рассматриваются вполне управляемые системы, поэтому размерность множества достижимости совпадает с размерностью пространства состояний и равна пяти. Система может перемещаться в направлении коммутаторов исходных векторных полей, совершая при этом значительный маневр. Этот факт иллюстрирует свойство неголономности рассматриваемых систем.
3.1. Качение сферы по плоскости
Конкретным примером систем вида (1) является система, моделирующая качение сферы по плоскости без прокручиваний и проскальзываний. Задача о качении сферы по плоскости имеет большое значение для робототехники при моделировании движения шара в руке робота-манипулятора, а также в задаче управления сферическими роботами [12]. Качение сферы по плоскости является первым приближением при исследовании задач о качении произвольных поверхностей. Они вызывают большой интерес в механике, робототехнике и теории управления [1,2,10,11].
Состояние системы «Сфера на плоскости» описывается точкой контакта сферы с плоскостью и ориентацией сферы в трехмерном пространстве. Требуется перекатить сферу из заданного начального
состояния в заданное терминальное состояние. Управлением является скорость центра сферы. Обозначим через (х,у) € М2 точку контак-
Рис. 1. Задача о качении сферы по плоскости
та сферы с плоскостью, а через q = (д0 ,02,03) Є Б3 — кватернион
единичной длины, задающий ориентацию сферы в пространстве. Тогда управляемая система имеет вид (см. [13]):
(5)
ОС = и\,
Оо = 2 (02и\ - 0\ и2),
< 0\ = 2 (оз и\ + Оо и2),
02 = 2 (—0.0и\ + 03и2):
03 = 1 (~0іиі - 02и2).
Пространство состояний Q = М2 х Б3 является пятимерным гладким многообразием, а векторные поля Х1 = 1 (1, 0, д2, Оз, —0.0, —0.1 )Т и Х2 = 11 (0,1, —01,00, Оз, —02)Т удовлетворяют условию полного ранга на Q.
3.2. Управление машиной с двумя прицепами
Другим важным с точки зрения приложений примером системы вида (1) является система, моделирующая движение по плоскости машины с двумя прицепами. Состояние системы определяется пятью координатами £ = (х,у,6,ф\,ф2). Здесь х,у — координаты центра машины на плоскости, в — угол, задающий ориентацию машины на плоскости, ф\ — угол, который задает положение первого прицепа относительно машины, ф2 задает положение второго прицепа относительно первого прицепа (см. Рис. 2). Управлением («1,^2) является линейная скорость.
Рис. 2. Машина с двумя прицепами
Динамика системы имеет вид (см. [2]):
х = cos ви\, у = sin ви1,
(6) \в = и2,
ф1 = — sin ф\и\ — (1 + cos ф1)и2,
ф2 = (sin(^i — ф2) + sin фі)иі + (cos(^i — ф2) + cos фі)и2.
Это — система вида (1), где поля Х\ и Х2 имеют вид:
Х\ = (cos в, sin в, 0, — sin фі, sin(^i — ф2) + sin фі )т,
Х2 = (0, 0,1, —1 — cos фі, cos(^i — ф2) + cos фі)т.
Заметим, что система (6) не управляема, если фі = я или Ф2 = ^. В дальнейшем будем рассматривать пространство состояний системы R2 х S1 x (S'1 \ j^})2. Вычислив коммутаторы
Хз = [^,Х2І, Х4 = [Х1,Х3], Х5 = [Х2,Х3],
можно проверить, что векторные поля Хі, і = 1,..., 5, линейно независимы всюду, кроме особых поверхностей фі = Ф2. Точки этих поверхностей являются сингулярными. Мы будем рассматривать систему вдали от этих особых поверхностей.
Анимация на рис. 2 вставлена с использованием пакета PDFAnim (см. http://www-user.uni-bremen.de/~skupin/pdfanim/).
4. Алгоритм поиска приближенного решения задачи (1), (2)
Итерационный алгоритм приближенного решения задачи (1), (2) основан на локальном приближении исходной нелинейной системы приближенной нильпотентной системой (см. пункт 4.1), для которой задача управления решается точно на каждой итерации. Итак, имеется исходная система в начальном состоянии q°, требуется найти управление, переводящее систему в целевое состояние q1 с наперед заданной точностью є > 0. Поиск приближенного решения осуществляется следующим образом:
(1) в окрестности целевой точки строится аппроксимирующая система;
(2) точно решается задача управления для приближенной системы;
- траектория исходной системы
Рис. 3. Итерационный алгоритм поиска приближенного управления
(3) найденные управления подставляются в исходную систему, и если достигнутое состояние не попадает в е-окрестность целевого состояния, то нужная точность не достигнута, и шаг (2) повторяется с новым граничным условием — в качестве начального состояния выбирается состояние, достигнутое на предыдущей итерации алгоритма, иначе вычисления прекращаются.
4.1. Нильпотентная аппроксимация
Локальное приближение управляемой системы другой, более простой системой, широко используется в теории управления. Обычно в качестве локальной аппроксимации используется линеаризация управляемой системы. Однако для линейных по управлению систем вида (1) линеаризация дает слишком грубое приближение. Так как размерность управления меньше размерности состояния, то линеаризация не может быть вполне управляемой. Естественную замену линейной аппроксимации в этом случае доставляет нильпотентная аппроксимация — наиболее простая система, сохраняющая структуру управляемости исходной системы (в частности, сохраняется такой важный инвариант как вектор роста).
Понятие нильпотентной аппроксимации управляемых систем введено независимо А. А. Аграчевым и А. В. Сарычевым [5], и Х. Херм-сом [7]. В работе используется алгоритм вычисления нильпотентной аппроксимации, предложенный А. Беллаишем [6], конкретизированный для систем (1) и дополненный переходом в систему координат, в которой нильпотентная аппроксимация имеет канонический вид (7).
Рассмотрим алгоритм построения нильпотентной аппроксимации для систем (1) в окрестности состояния д1:
(1) Вычисление коммутаторов
Хз = [Хі, Х2], Х4 = [Хі, Х3], Х5 = [Х2, Х3].
Веса полей Х\,Х2,Х3,Х4,Х5 соответственно равны (1,1, 2, 3, 3).
(2) Используя исходные координаты д, вычисляются координаты у следующим образом:
где Г — это матрица размерности 5x5, составленная из элементов Г^-, определяемых по формуле:
При такой замене точка д1 переходит в начало координат.
(3) Выполняется переход
аі = Хі(ХіЫ)(Л &2 = Хі(Х2(у4))(д1), аз = Х2(Х2(у4))(д1),
а4 = Хі(Хі(у5))(д1), а5 = Хі(Х2(у5))(д1), а6 = ^(ЗДбЖ?1).
Здесь Хі(/) = (V/, Хі) обозначает производную функции / по направлению векторного поля (оператор (,) обозначает скалярное произведение, а V — взятие градиента).
(4) Используя разложение векторных полей Хі(г) в ряд Маклорена, строится нильпотентная аппроксимация в координатах г:
У = а(д) Г( 1} (д - д1),
в привилегированные координаты г с помощью замены:
где
і = Хі(г)иі + Х2(%)и2, г Є К5, (иі,и2) Є К2,
где
д^дг2 г=0
N д
=о ) дг5
3 = 1, 2.
(5) Вычисляется замена переменных Ф для перехода в систему координат, в которых нильпотентная аппроксимация имеет канонический вид:
Известно [14], что любые две пятимерные нильпотентные системы с вектором роста (2, 3, 5) диффеоморфны. Замена переменных, переводящая одну такую систему в другую, строится следующим образом. Пусть Х\, Х2 — векторные поля первой, а Уі, У2 — векторные поля второй нильпотентной системы с вектором роста (2, 3, 5). Вычислив коммутаторы Х3, Х4, Х$ и У3, І4, І5, можно построить диффеоморфизм, переводящий поля Хг в окрестности точки хо в поля Уі в окрестности уо:
(7)
У\ = «1,
У2 = и2,
< Уз = ¿(у\и2 - у2иі), у Є М5.
У4 = 2 (УІ + У2)u2,
У5 = - 1(У2 + У2)Ul,
Ф: О(хо) ^ О(уо), Ф*(Хі) = У.
Определим отображения F и G как композицию потоков векторных полей Xi и Yi соответственно за время tf.
F (t\,... ,t5) = et5X5 о • • • о etlXl (хо),
G(ti,.. .,t5) = et5V5 о • • • о etlYl (уо).
Тогда искомый диффеоморфизм имеет вид
Ф = G о F-1.
Итак, предложен способ задания диффеоморфизма, переводящего поля Xi исходной системы (1) из окрестности точки q1 в поля системы Yi в окрестности начала координат, нильпотентная аппроксимация которой имеет канонический вид (7).
т = Ф о Л о а.
4.2. Кусочно-постоянные управления
Рассмотрим задачу управления нильпотентной канонической системой (7) с граничными условиями
(8) у(0) = у0, у(1) = (0,0,0,0,0).
Требуется найти управления и\(t) и u2(t) на отрезке [0,1] в классе кусочно-постоянных функций такие, что для произвольной точки у0 € R5 соответствующая траектория удовлетворяет условиям. у(0) = у0, у(1) = (0,0,0, 0, 0).
С использованием системы Wolfram Mathematica было показано, что для решения задачи управления (7), (8) достаточно управлений с тремя точками переключения:
щ, при t € [0, 4],
А, при t € (\, ±], то
4 L где г = 1, 2.
Ui = \
тгт^тт + d ! _ _
( 2 , 4
Ji, при t € (4,1]
Последовательное интегрирование системы (7) дает выражение для конечной точки, в которую переходит система после примененных управлений щ. Получается система из пяти алгебраических уравнений относительно уо и коэффициентов управления. Вначале показывается, что управлений с двумя переключениями Щ = {ГМ,^ ,С{}
недостаточно для решения задачи (7), (8). Для этого требуется проверить, что получившаяся система уравнений неразрешима относительно коэффициентов управления при произвольном у0. Заметим, что система состоит из пяти уравнений с шестью неизвестными {а*,Ь*, с*}, и возникает свобода выбора, относительно каких из неизвестных решать систему. Однако никакой выбор не дает решения при произвольном у0. Далее показывается, что управлений с тремя точками переключения достаточно для разрешимости получившейся системы при любом у0 € М5.
Требуется найти коэффициенты управления а*, $*, 7*, 5*. Для этого решается система из пяти алгебраических уравнений с восемью неизвестными:
yi(v-i,. 0,
V2(ai,. 0,
JL 00 0,
yi(ai,. 0,
СЛ 's 0.
Получается трехпараметрическое семейство решений. Формулы имеют сложный вид, поэтому в статье не приводятся. Для любого начального состояния у0 существует способ зафиксировать свободные параметры так, чтобы получалось решение без особенностей. Кроме того, желательно получить управления такие, чтобы длина соответствующей траектории (в некоторой естественной метрике) была как можно меньше. В зависимости от того, какие коэффициенты выбираются в качестве свободных параметров, получаются различные формулы для оставшихся коэффициентов управления. В качестве свободных параметров предлагается выбирать ^i, ¿i, S2 или ^2, ¿1, S2. Остальные коэффициенты управления выражаются через свободные параметры. Введем показатель Amp = max(|«j|, Iß'l, |7'|, |^®|), характеризующий величину отклонения искомой траектории от постоянной траектории. Свободные параметры предлагается фиксировать в соответствии с критерием Amp ^ min.
4.3. Оптимальные управления
Рассматривается задача оптимального управления системой (7) с граничными условиями
(9) у(0)= у0, у(Т) = (0,0, 0, 0,0),
с естественным интегральным критерием (минимальность функцио-
Эта задача известна в теории управления как обобщенная задача Ди-доны [15], а в субримановой геометрии как нильпотентная субрима-нова задача с вектором роста (2,3,5). Она имеет богатую историю и была детально теоретически изучена Ю. Л. Сачковым [15-18]. Основным результатом теоретических исследований явилось описание структуры экспоненциального отображения и первых времен Максвелла вдоль экстремальных траекторий. На основе этого задача оптимального управления (7), (9), (10) была сведена к задаче решения пяти алгебраических уравнений в неэлементарных функциях с пятью неизвестными. Явно решить эту систему практически не представляется возможным ввиду сложности получившихся формул, поэтому было решено использовать численные методы.
Приведем некоторые результаты из статей [15-18], необходимые для дальнейшего изложения:
(1) Семейство экстремальных траекторий параметризуется экспоненциальным отображением Ехр, переводящим пару (вектор сопряженных переменных, время) в соответствующую точку экстремальной траектории. Задача построения оптимального синтеза состоит в обращении экспоненциального отображения.
(2) Прообраз Ь и образ М экспоненциального отображения разбиваются поверхностями разреза на 4 области (Ь = и4=1Ьі в прообразе и М = У34=1Мі в образе) таких, что Ьі переходит в Мі и экспоненциальное отображение является диффеоморфизмом на этих областях.
(3) Каждая область Ьі разбивается на два непересекающихся множества Сі и С2, в которых экспоненциальное отображение задается разными формулами.
(4) Разбиение Ь на Ьі и М на Мі известно, а разбиение на Cj неизвестно.
(5) В задаче (7), (9), (10) имеется двумерная группа симметрий, состоящая из вращений и растяжений. Факторизация задачи по
нала субримановой длины)
(10)
этой группе уменьшает размерность задачи с пяти до трех. Получается система из трех уравнений с тремя неизвестными:
{Р(и, V, к) = Рх,
Q(u,v,k) = Q1,
К(и, V, к) = К\.
Итак, для построения оптимального синтеза в задаче (7), (9), (10) требуется решить систему (11) относительно (и,у,к) при заданной правой части Р\, Ql, Дх. Известно, что при произвольной правой части (за исключением особых множеств меньшей размерности) система (11) имеет единственный корень. Заметим, что функции Р(и, V, к), Q(u,v,к) и К(и,у,к) выражены в эллиптических интегралах первого и второго рода. Кроме того, они имеют сложную аналитическую запись (занимают несколько страниц текста). В приложении в конце статьи приведены формулы для случая С\.
Предлагается следующий генетический алгоритм решения системы (11) (см. Рис. 5 и Рис. 4):
(1) Определение области Ь* по правой части Р1^1,К\.
(2) Поочередный выбор С\, С2 (или одновременное выполнение на 2-х процессорах).
(3) Создание начальной популяции п случайных приближений (особей популяции) для корня.
(4) Сортировка популяции по приспособленности. Особь обладает тем лучшей приспособленностью, чем меньше невязка
(12) л/Р(и, V, к)2 + д(м, V, к)2 + К(и, V, к)2.
(5) Эволюция. Над особями популяции определены две операции: «мутация особи» (метод Ньютона решения системы уравнений) и скрещивание особей (метод хорд решения системы уравнений). Кроме того, к популяции добавляются случайные особи, во избежание сходимости алгоритма к ложному корню (точка локального минимума невязки (12), не являющаяся корнем системы (11)).
(6) Отбор п наиболее приспособленных особей. Если невязка лидера популяции меньше заданного значения е, то корень найден. Иначе переход к пункту (5).
Выбор такой схемы поиска корня был обусловлен особенностями решаемой системы. Было обнаружено, что невязка (12) имеет много локальных минимумов. В связи с этим, сходимость стандартного метода
Рис. 4. Генетический алгоритм решения системы алгебраических уравнений
Ньютона сильно зависит от того, насколько удачно выбрано начальное приближение. Кроме того, были замечены случаи, когда в малой окрестности корня невязка терпит сильные изменения: внутри области значение невязки мало, а вне этой области значение невязки велико. При этом и внутри, и вне этой области имеются несколько локальных минимумов. В этом случае вероятность удачного выбора начального приближения для метода Ньютона становится крайне мала. Введение популяции начальных приближений позволяет оставаться в описанной окрестности корня (один раз попав в нее) даже если метод Ньютона (мутация) не сошелся к корню. Метод хорд (скрещивание) исследует окрестность найденного лучшего приближения (лидера популяции), помогая выбраться из точки локального минимума.
5. Программный комплекс MotionPlanning
В среде Mathematica 8 (http://www.wolfram.com/mathematica/)
был разработан программный комплекс MotionPlanning (см. Рис. 5) решения задачи (1), (2). Он является дополнительным пакетом для системы Wolfram Mathematica (MotionPlanning.m) и состоит из следующих функциональных модулей:
Начало
xs (символьное обозначение переменных), X1(xs), X2(xs) (векторные поля при управлениях), q0, q1 (начальное и целевое состояния), eps (точность), ^конечный момент времени),
U (класс управлений)
U=CP L Выбран класс кусочно-постоянных управлений
NilpotentApproximation235 (Построение нильпотентной аппроксимации NA)
Switch U
U=OC Выбран класс оптимальных управлений
CPControl235 ^^Выбор класса функцийЛ\__^ в котором ищется OptimalControl235
управление
Вывод результатов: u1(t), u2(t) (найденные управления), q(t) (соответствующая траектория)
Конец
Рис. 5. Структура ПК MotionPlanning
• NilpotentApproximation строит нильпотентную аппроксимацию NA (см. п. 4.1) системы (1) и возвращает замену переменных т = а о А о Ф, в которых N А имеет канонический вид (7).
• CPControl решает задачу (1), (2) в классе кусочно-постоянных управлений (см п.4.2).
• OptimalControl решает задачу (1), (2) в классе оптимальных для нильпотентной системы (7) управлений (см. п. 4.3). Пользователь может легко организовать параллельное вычисление корня на нескольких ядрах процессора. Для этого требуется указать системе Mathematica 8 (функция недоступна в более ранних версиях) количество ядер и изменить, при необходимости, функцию SolveHen (выбрать какие и сколько из алгоритмов решения системы AlgorithmA, AlgorithmB, Algorithme, AlgorithmD будут выполняться одновременно). Перечисленные алгоритмы являются
q=q0
_______________________
OCSolver
(Поиск оптимальных управлений u1(t), u2(t), переводящих нильпотентную аппроксимацию NA
из q в q1 за время t1=topt)
.1 ~ Trajectory
(Вычисление траектории q(t) исходной
системы, соответствующей найденным
u1(t), u2(t))
(t),u2(
q=q(ti)
Конкатенация управлений и перепараметризация времени
Рис. 6. Модуль CPControl и OptimalControl решения задачи (1), (2) в классах кусочно-постоянных и оптимальных управлений
конкретными реализациями генетического алгоритма, описанного в п. 4.3. Они различаются как по количеству особей популяции, так и по закону эволюции. Кроме стандартного метода Ньютона (AlgorithmA) и метода хорд (AlgorithmB), пользователю предлагаются гибридные методы (Algorithme, AlgorithmD). Проведенное обширное тестирование показало, что в большинстве случаев первым результат выдает AlgorithmA или AlgorithmB, хотя и встречаются ситуации, когда гибридные методы справляются с задачей быстрее. Поэтому по умолчанию предлагается делить доступные ядра пополам между AlgorithmA и AlgorithmB. Отметим, что одновременный запуск одного и того же алгоритма на разных ядрах имеет смысл, так как во всех алгоритмах используется генератор случайных начальных приближений. Так как основное время вычисления занимает выбор удачного начального приближения, то использование нескольких ядер дает существенное ускорение.
Рис. 7. Схема работы модуля OptimalControl
• Дополнительные функции ПК МойопПапш^.
Помимо основных модулей комплекса MotionPlanning, решающих задачу (1), (2) (NilpotentApproximation, СРСо^го1 и Орйта1Со^го1), пользователю предоставляются некоторые дополнительные инструменты для представления результатов и слежения за процессом вычислений. Речь идет о следующих функциях:
• Тга]ес1югуМа1Раг[Х1,Х2, {а\,и2,Т},д0,жв] возвращает q(t) траекторию системы (1), где Ь € [0,Т], соответствующую управлениям {и1,и2}, выходящую из точки q0. На рис. 8 приведен пример использования функции Тга]ео1огуМа1Раг для построения (РагашеШсР^) проекций на плоскость семейства оптимальных траекторий в обобщенной задаче Дидоны (7), (9), (10);
Рис. 8. Пример использования функции TrajectoryNatPar
• Р\оЬТга]есЬогу^аЬРаг\рга]ес1огу^0,Т] строит разным цветом графики пяти компонент заданной траектории trajectory(t) за время Ь € [0,Т], а также отмечает на графике заданные состояния q0 и д1. Эта функция может использоваться для визуальной оценки достигнутого состояния и найденных траекторий. Пользователь может включить режим работы ПК, в котором графики найденных траекторий будут выводиться на экран на каждой итерации. На рис. 9 приведен пример использования функции РкЛТга]ес1югуМа1Раг;
• РкЛТга]ес1югу01кШа1Раг[£га7есйогу, q0, q1, Т] строит разным цветом графики отклонений пяти компонент заданной траектории trajectory(t) за время Ь € [0, Т] от состояния ql;
• AmmateCar[trajectory,T,q0,q1, X, {хтЫ,хтах,уты,утаж}] строит анимацию движения машины с двумя прицепами, при движении по траектории trajectory(t) за время Ь € [0, Т] из состояния q0. Область, по которой движется машина с прицепами, ограничивается прямоугольником {{хты,хта ж}, {Утт,Ут а ж}}. В результате на жестком диске создается последовательность из
Рис. 9. Функция PlotTrajectoryNatPar построения компонент траектории
N кадров. Последовательность кадров — это упорядоченный набор изображений в формате РМС, который впоследствии легко может быть собран в видеофайл стандартными утилитами. Размер изображения является параметром, доступным пользователю (по умолчанию 1024*768 пикселей). Помимо положения машины и прицепов, на каждый кадр пунктиром наносится состояние д1. Функция предназначена как для самостоятельного использования (моделирование системы «машина с двумя прицепами» и ее исследование), так и для проверки решений найденных с помощью ПК Мо1юпР\апш^ (см. рис. 2).
• AmmateBa\\[trajectory,T,q0,д1,М] визуализирует качение сферы без прокручиваний и проскальзываний по заданной траектории (аналогично функции Ашта1еСаг) (см. рис. 1).
Приведенный алгоритм поиска приближенного управления (см. раздел 4), основанный на построении нильпотентной аппроксимации, решает локальную задачу управления (т.е. когда начальное д0 и конечное д1 состояния не слишком далеки). Алгоритм реализован в ПК MotionP\aning. В дальнейшем планируется расширить функции ПК для решения глобальной задачи управления (т.е. когда начальное д0 и конечное д1 состояния далеки). Предполагается соединять д0 и д1
непрерывной кривой, и разбивать ее промежуточными точками так, чтобы на каждом отрезке возникала локальная задача управления (см Рис. 10).
Рис. 10. Решение глобальной задачи
6. Пример использования ПК Мо^опР!апп^
Продемонстрируем работу комплекса на задаче о перемещении машины с двумя прицепами из начального состояния
п / Я Я Я
д0 = 0, 0,-,-, - -
V 4 4 4
в целевое состояние
д1 = (-0.252, -0.339,1.085, 0.514, -1.281)
с точностью £ = 10-3. На рис. 11 и рис. 12 приведены графики отклонения от целевого значения д1 найденной траектории, соответствующей найденному кусочно-постоянному и оптимальному управлению. В случае применения кусочно-постоянных управлений, алгоритму потребовалось 6 итераций для достижения требуемой точности, а для оптимальных управлений потребовалось 4 итерации. Заметим, что при использовании кусочно-постоянных управлений система совершила больший маневр (траектория имеет большую амплитуду отклонений от целевого состояния).
7. Заключение
Рассматриваемый в работе метод приближенного решения двухточечной задачи управления (1), (2) был реализован в параллельном программном комплексе MotionP\aning. Комплекс был испытан на двух прикладных задачах (задача о качении шара по плоскости и задача управления машиной с двумя прицепами). В случаях, когда граничные условия не были слишком далеки, комплекс успешно
Рис. 11. График отклонения траектории, соответствующей кусочно-постоянному управлению
Рис. 12. График отклонения траектории, соответствующей оптимальному управлению
решал поставленную задачу управления. В случаях далеких граничных условий алгоритм не сходился, что соответствует теоретическому обоснованию метода (нильпотентная аппроксимация является локальным приближением исходной системы). В будущем планируется
расширить функционал ПК для решения глобальной задачи управления, путем ее сведения к последовательности локальных задач.
В настоящее время ПК MotionPlaning является удобным и надежным средством решения локальной задачи (1), (2).
Список литературы
[1] Аграчев А. А. , Сачков Ю. Л. Геометрическая теория управления. М. : Физматлит.— 391 с. f1, 2, 3, 3.1
[2] Laumond J. P. Robot Motion Planning and Control. Lecture Notes in Control and Information Sciences, Vol. 229, 1998.— 343 p. f1, 3.1, 3.2
[3] Murray R. M. , Sastry S. Steering controllable systems // 29th IEEE Conf.
Dec. and Control. — Honolulu, Hawaii, 1990 f1
[4] Fliess M. , Levine J. , Martin P. , Rouchon P. On differential flat nonlinear systems // IFAC NOLCOS Symposium. — Bordeaux, France, 1992, p. 408-412 f1
[5] Аграчев А. А. , Сарычев А. В. Фильтрация алгебры Ли векторных по-
лей и нильпотентная аппроксимация управляемых систем // ДАН СССР, 1987. Т. 295, c. 777-781 f1, 4.1
[6] Bellaiche A. The tangent space in sub-Riemannian geometry // Sub-Riemannian Geometry / ed. Bellaiche A. , Risler J. J. — Basel, Swizerland : Birkhauser, 1996, p. 1-78 f3, 4.1
[7] Hermes H. Nilpotent and high-order approximations of vector fields systems // SIAM, 1991. Vol. 33, p. 238-264 f4.1
[8] Lafferriere G. , Sussmann H. J. A differential geometric approach to motion
planning // Nonholonomic Motion Planing / ed. Li Z. , Canny J. F. :
Kluwer, 1992 f
[9] Venditelli M. , Oriolo G. , Jea F. , Laumond J. P. Nonhomogeneous nilpotent approximations for nonholonomic systems with singularities // Transactions on Automatic Control, 2004, p. 261-266 f 1
[10] Montgomery R. A Tour of Subriemannian Geometries, Their Geodesics and Applications : American Mathematical Soc., 2006.— 259 p. f2, 3, 3.1
[11] Jurdjevic V. Geometric control theory : Cambridge University Press, 1997. f3, 3.1
[12] Sang S. , Zhao J. , Wu H. , Chen S. , An Q. Modeling and Simulation of a Spherical Mobile Robot // Computer Science and Information Systems, 2010. Vol. 7, no. 1, p. 51-62 f3.1
[13] Маштаков А. П. , Сачков Ю. Л. Экстремальные траектории и асимптотика времени Максвелла в задаче об оптимальном качении сферы по плоскости // Математический сборник, 2011. Т. 202, № 9, c. 261-266 f3.1
[14] Sachkov Yu.L. Symmetries of Flat Rank Two Distributions and Sub-Riemannian Structures // Transactions of the American Mathematical Society, 2004. Vol. 356, p. 457-494 f5
[15] Сачков Ю. Л. Экспоненциальное отображение в обобщенной задаче Ди-доны // Математический сборник, 2003. Т. 194, c. 63-90 f4.3
[16] Сачков Ю. Л. Дискретные симметрии в обобщенной задаче Дидоны // Математический сборник, 2006. Т. 197, № 2, c. 95-116 f
[17] Сачков Ю. Л. Множество Максвелла в обобщенной задаче Дидоны // Математический сборник, 2006. Т. 197, № 4, c. 123-150 f
[18] Сачков Ю. Л. Полное описание стратов Максвелла в обобщенной задаче Дидоны, // Математический сборник, 2006. Т. 197, № 6, c. 111-160 f4.3
Рекомендовал к публикации д.ф.-м.н. Ю . Л. Сачков
Об авторе:
Алексей Павлович Маштаков
После окончания в 2009 г. Университета города Переслав-ля имени А.К. Айламазяна по специальности «Прикладная математика и информатика» успешно завершает обучение в аспирантуре ИПС РАН им. А.К. Айламазяна под руководством д.ф.-м.н. Ю. Л. Сачкова.
e-mail: alexey.mashtakov@gmail.com
Образец ссылки на эту публикацию:
А. П. Маштаков. Алгоритмическое и программное обеспечение решения конструктивной задачи управления неголономными пятимерными системами // Программные системы: теория и приложения : электрон. научн. журн. 2012. T. 3, № 1(10), с. 3-29.
URL: http://psta.psiras.ru/read/psta2012_1_3-29.pdf
A. P. Mashtakov. Algorithms and Software Solving a Motion Planning Problem for Nonholonomic Five-dimensional Control Systems.
Abstract. In this paper we consider a motion planning problem for nonlinear fifth dimensional systems with two-dimensional linear control. An iterative algorithm based on nilpotent approximation finds the approximate solution of the problem in the classes of piecewise constant and optimal controls. We present implementation of the algorithm in a parallel package for Wolfram Mathematica. The package was applied for the plate-ball problem and for the motion planning problem of car with two trailers on a plain.
Key Words and Phrases: Plate-ball problem, car with two trailers, nilpotent approximation, iterative algorithm, optimal control.
