Моделирование парковки автомобиля с прицепом вдоль путей Маркова-Дубинса и Ридса-Шеппа Текст научной статьи по специальности «Математика»

ISSN 2079-3316 ПРОГРАММНЫЕ СИСТЕМЫ: ТЕОРИЯ И ПРИЛОЖЕНИЯ т. 10, №4(43), с. 97-110

ББК В161.83:В192.23 ГРНТИ 27.37.17,28.17.19 УДК 004.94:519.711.2

А. А. Ардентов, И. С. Губанов

Моделирование парковки автомобиля с прицепом вдоль путей Маркова-Дубинса и Ридса-Шеппа

Аннотация. Рассмотрены три классические модельные задачи, описывающие оптимальную парковку автомобиля: задача Маркова, задача Дубинса, задача Ридса-Шеппа. Исследованы кинематические законы поведения прицепа при движении автомобиля вдоль рассматриваемых оптимальных путей. В программной среде Mathematica разработан интерфейс, который по граничным условиям строит соответствующий оптимальный путь и анимацию движения автомобиля (с прицепом) вдоль него.

Ключевые слова и фразы: оптимальное управление, задача быстродействия, Wolfram

Mathematica, кинематика.

Введение

Рассматривается модель автомобиля, двигающегося по однородной плоскости (ровной горизонтальной поверхности без препятствий). Положение автомобиля на плоскости определяется элементом группы движений плоскости ц = (х,у,в) € ЯЕ(2), который соответствует координатам центра автомобиля (х, у) € М2 и углу ориентации автомобиля в € Б1. В работе формулируется задача наискорейшей парковки автомобиля, т.е. задача перевода автомобиля из заданного начального положения цо в некоторое конечное положение Ц1 за минимальное время. При этом предполагается, что автомобиль движется вперёд и назад с единичной скоростью, а угловая скорость ограничена по модулю заданной величиной 1/Д, т.е. автомобиль не может двигаться вдоль окружности с радиусом, меньшим Д. Таким образом, искомое решение ц{Ь),Ь € [0, Т] минимизиреует не только время, но и расстояние на плоскости (х, у).

Исследование выполнено за счет гранта Российского научного фонда (проект № 17-11-01387) в Институте программных систем им. А.К. Айламазяна Российской академии наук.

© А. А. АрдЕнтов, И. С. Губанов, 2019

© Институт программных систем имени А. К. Айламазяна РАН, 2019 © Программные системы: теория и приложения (дизайн), 2019

Решение этой задачи получено Ридсом и Шеппом в 1990 году в работе [?Reeds-Shepp], а соответствующая модель получила впоследствии название машина Ридса-Шеппа. Они показали, что оптимальная траектория может состоять из 48 вариантов последовательного соединения дуг окружностей радиуса R и отрезков прямых линий. Годом позже Суссман и Танг [?Sussmann-Tang] показали, что достаточно рассмотреть 46 вариантов.

Родственная задача исследовалась раньше, в 1957 году Ду-бинс [?Dubins] рассмотрел аналогичную модель (получившую название машина Дубинса), в которой автомобиль может двигаться только вперёд. Оптимальное решение состоит из 6 вариантов последовательного соединения дуг окружностей и отрезков прямых линий. Однако еще раньше в 1889 году Марков [?Markov] рассмотрел аналогичную модель, в которой конечный угол ориентации автомобиля не фиксирован, при этом он показал, что возможно всего два варианта: дуга окружности и отрезок прямой либо две дуги окружностей.

В данной работе представлен разработанный в программной среде Mathematica интерфейс, включающий в себя решения всех трех задач. Кроме того, в интерфейсе имеется опция добавления к автомобилю прицепа. В зависимости от начального положения прицепа вычисляется закон поведения прицепа вдоль рассматриваемых оптимальных путей для автомобиля.

Интерфейс можно разместить на сайте Wolfram Demonstrations Project, на котором из рассматриваемых моделей представлена только модель Дубинса [?Dubins-link].

1. Постановка задач быстродействия

Опишем модель машины Ридса-Шеппа как следующую задачу оптимального управления. Задана дифференциальная система уравнений:

ÍX(t) = u\(t) cos 0(t), y(t) = ui(t)sin 0(t), 0(t) = u2(t),

где ui(t),u2(t) есть функции линейной и угловой скоростей автомобиля. Также зафиксирован набор граничных условий

(2) x(0) = xo, y(0) = yo, 0(0) = во,

(3) x(T )= xi, y(T ) = yi, 0(T ) = 0i,

а оптимальность в задаче быстродействия соответствует минимизации времени

(4) T ^ min .

Также имеются следующие ограничения на искомые оптимальные управления:

(5) |ui(t)| = 1, U(t) < 1/R,

где R > 0 некоторая заданная константа.

Отметим, что в задаче Дубинса условие на управление (??) следует заменить на следующее:

(6) ui(t) = 1, u2(t) < 1/R.

При этом в задаче Маркова условие на конечную точку (??) следует заменить на следующее:

(7) x(T )= xi, y(T ) = yi.

Таким образом, рассматриваются следующие задачи:

(1) Маркова (??)-(??), (??), (??)-(??);

(2) Дубинса (??)-(??), (??);

(3) Ридса-Шеппа (??)-(??).

Мы не будем детально описывать решение каждой задачи, которые подробно описаны в первоисточниках [?Reeds-Shepp, ?Dubins, ?Markov]. Отметим, что при реализации интерфейса использовалось компактное описание решений, представленное в книге [?LaValle, раздел 15.3]. Далее опишем расширение рассматриваемых моделей автомобиля, учитывающее поведение прицепа, добавленного к автомобилю.

2. Поведение прицепа вдоль оптимальных путей

Расширенная модель автомобиля с прицепом описывается дополнительным дифференциальным уравнением на угол ориентации прицепа относительно автомобиля:

. .sin w(t) . . (lr cos w(t) \

(8) y>(t) = -ui (t) - U2 (t) r sФ() + 1J ,

где lr, lt суть расстояния от центра робота до точки связки робота с прицепом и от точки связки до центра колесной пары прицепа соответственно.

При движении вдоль прямой линии (ui(t) = ±1,u2(t) = 0) уравнение (??) легко интегрируется:

(9) ^(t) = 2 arcctg(e±í/ít cot у),

где фо есть начальный угол прицепа.

Далее рассмотрим случай движения автомобиля вдоль окружности (ui(t) = 1,u2(t) = ±1/R). Введём обозначение lR := l^ — lt2 + R2. При Ir > 0,lr = lt уравнение (??) также можно проинтегрировать, получив следующее выражение для угла:

R — VTR th ( + arcth -/^tg ^ )

(10) *,(t) = ±2 arctg-V2'tR l l-^-J-,

lr — lt

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

3. Решение задач в среде Mathematica 3.1. Работа интерфейса

На рисунке ?? представлено рабочее поле разработанного интерфейса. В верхней части расположена панель управления, а в нижней части активная часть плоскости (x,y), в которой анимируются решения поставленных в разделе ?? задач (Маркова, Дубинса, Ридса-Шеппа).

Для работы интерфейса используются следующие основные элементы управления:

• Таймер Time позволяет воспроизводить движения автомобиля (в том числе с прицепом) вдоль найденных путей.

• Ползунок Radius дает возможность изменять максимальный угол поворота передних колес автомобиля.

• Существует возможность переключения между задачей Маркова и задачей Дубинса с помощью клавиши FinalAngle, что позволяет включать и отключать конечное направление.

• При включенном конечном направлении есть возможность выбора, какую задачу необходимо решить, задачу Дубинса или задачу Ридса-Шеппа, выбрав стиль передвижения MoveStyle: ^ или ^ соответственно.

• Поскольку при некоторых конфигурациях существует несколько оптимальных решений, была реализована возможность переключения отображения одного или всех возможных путей, с помощью кнопки AllWays. Так же при включенном отображении нескольких путей можно переключаться между ними,

Time ¡►III |K| Radius .—0-

Travel Time:4.83

Export

Auto Scale

All Ways Q Way Trailer CriticalAngle

Optimal

FinalAngle

MoveStyle

a «

Scale

Рисунок 1. Интерфейс программы

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

• Кнопка Trailer добавляет к автомобилю прицеп. При включении этой кнопки есть возможность задать угол критического отклонения, который называется CriticalAngle. Если прицеп выходит за границы заданного угла, то возникает сообщение об ошибке.

• Изменение граничных условий (xo,yo,#o), (xi,yi), в\ (если вклю-

чено конечное направление FinalAngle), а также начальное положение прицепа фо (при включенной кнопке Trailer) осуществляются путем передвижения локатора по видимой части плоскости.

Для удобства работы с программой дополнительно добавлена возможность масштабирования (см. рисунок ??):

1. Изменение масштаба прямоугольника активной части плоскости (х, у).

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

3. Автомасштабирование для отображения найденных решений.

Рисунок 2. Масштаб

3.2. Работа методов

Для работы интерфейса были разработаны три основные процедуры Markov, Dubins и Reeds.

Основная логика работы у всех методов одинаковая. Методы отличаются только количеством путей и способом передвижения автомобиля. Рассмотрим все возможные способы передвижения для трех задач.

Для задачи Маркова существует всего два возможных типа движения [?Markov]: по стартовой окружности (Ci) и прямой (S) либо по стартовой окружности и вспомогательной окружности (Ch). Автомобиль может начинать свое движение как влево (L), так и вправо (Д), итого имеем всего 4 возможных пути (см. таблицу ??). Для задачи Дубинса, помимо стартовой окружности есть еще конечная окружность (C2), и как следствие существует 6 возможных путей (см. таблицу ??).

Таблица 1. Классификация путей

тип пути

CiS LS, RS

Ci Ch LR, RL

пути

Qf^-t LSL, RSR, ClSC2 LSR, RSL

Ci Ch C2 LRL, RLR

(a) Маркова (b) Дубинса

тип

В задаче Ридса-Шеппа автомобиль может двигаться в обе стороны, поэтому для описания всех маршрутов воспользуемся следующими обозначениями: вперед + , назад - и смена направления В таблице ?? приведено описание всех возможных путей, согласно работе [?Reeds-Shepp].

Таблица 2. Классификация путей Ридса-Шеппа

тип пути

Сх| Сн |С2 я+ь-я+, я- ь+я-

СхСн |С2 Ь+Д+Ь-, я+ь+я-, я- ь-я+

Сх|Сн С2 я+ь-я-, я- ь+я+

Сх£С2 Ь+5+Ь+, ь-я-Ь-, Ь+ я+я+, ь-я-я-, я+я+я+, я-я+я+ь+, я- я-я-,

СхСн1 |Сн2 С2 ь-я-ь+я+, я-ь-я+ь+

Сх|Сн1 Сн21С2 ь-я+ь+я-, я-ь+я+ь-

Сх|Сн 5С2 ь+я-я-Ь-, ь+ я-я -я-, я+ь-я -я-, я+ь-я-ь-, ь-я+я+ь+, ь-я+я+я+, я-ь+я+я+, я-ь+я+ь+

СхЯСн |С2 ь+я+я+ь-, ь+я+ь+я-, я+я+ ь+я-, я+я+я+ь-, ь-я-я-ь+, ь-я-ь-я+, я-я-ь-я+, я-я-я-ь+

Сх|Сн1 ЯСн21С2 ь+я-я-ь-я+, я+ь-я-я-ь+, ь-я+я+ь+я+, я-ь+я+я+ь-

В таблицах ??,?? представлены логические и оптимальные ограничения. В таблицах используются следующие обозначения: Ос1, Ос2, ОСн суть центры окружностей С\, С2, Сн соответственно, Ьа1 ,ЬС2, ¿Сн (¿сН1, ^сН1) суть нормированные длины дуг окружностей, выраженные в угловых координатах, т.е. 2п соответствует длине всей окружности.

Прежде чем перейти к объяснению метода реализации процедур, необходимо уточнить, что существует 3 состояния пути:

1. Путь не существует (не выполняются условия из таблицы ??).

2. Путь существует, но заведомо не оптимален (справедливы условия таблицы ??, но не выполнены условия таблицы ??).

3. Путь существует и возможно оптимален (выполнены условия обеих таблиц ?? и ??).

Таблица 3. Описание условий существования путей

тип пути Логические условия

Марков:

CiS все d (Oct, (xi, yi)) > Radius

CICh все d (OCl, (xi,yi)) < 3Radius

Дубинс: C1SC2 LSL, RSR LSR, RSL True d(Ocl ,Oc2 ) > 2Radius

CICh C2 все d(Od ,Oc2 ) < 4Radius

Ридс-Шепп:

CICh C2 все d(Od ,Oc2 ) < 4Radius

Ci S C2 LSL, RSR LSR, RSL True d(Od ,Oc2 ) > 2Radius

CiChI \Ch2 C2 все d(Oa ,Oc2 ) < 6Radius

Ci\ChI Ch2 \C2 все d(Oc1 ,Oc2 ) G [2Radius, 6Radius]

Ci\Ch SC2 LRSL, RLSR LRSR, RLSL d(Oci ,Oc2 ) > 3Radius d(Ocl ,Oc2 ) > 2Radius

Ci SCh \ C2 LSLR, RSRL d(Ocl ,Oc2 ) > 2Radius

LSRL, RSLR d(Ocl ,Oc2 ) > 3Radius

Ci\ChiSCh2 \C2 все d(Oc\ ,Oc2 ) > 4.5Radius

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

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

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

Построение маршрута осуществляется по одному принципу, отличаются только точки касания, рассмотрим на примере задачи Маркова более подробно.

Для движения C\S необходима касательная, то есть два отрезка пути разделяет одна точка касания, следовательно, зная её, программа

Таблица 4. Описание условий оптимальности путей

тип пути

Условия оптимальности

Марков:

CiCH

d(OCl, (xi,yi)) < Radius

Дубинс:

Ci CH C2

Ридс-Шепп:

CI|CH |C2 Ci|ChC2, CICHIC2 C1SC2

CICH1 |CH2C2, CiSCH|C2 CI|CHSC2, CISCH|C2 CI|CH1 SCH2 IC2

n < icH < 2n

2 € [0, n] tcH € [0, 2]; tc1 ,tc2 € [0,tcH] tc1, tc2 € [0, П] icHl = tcH2 € [0, П]; tci ,tc2 € [0,icHl ]

tc1, tc2 € [0, 2], tcH = п

iCi , iC2 € [0, 2], icHl = tcH2 = п

рисует дугу с центром Oc1 от стартовой точки до точки касания, далее прямую от точки касания до конечной точки.

Однако для движения по C\Ch используется вспомогательная окружность. Два отрезка пути также разделяет точка касания, но необходимо знать еще и центр Och . В этом случае программа рисует дугу с центром Oc1 от стартовой точки до точки касания, а далее дугу с центром Och от точки касания до конечной точки.

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

Дальнейшие вычисления происходят по аналогичному алгоритму. Рассчитываются два времени: tc1 и ts или же tcH, сумма которых дает общее время to. Движение осуществляется по первому отрезку пока время не примет значение tc1 и по второму отрезку от tc1 до to.

Для остальных методов может варьироваться количество отрезков пути. Ниже представлена таблица процедур, определяющих точки касания (P) для всех возможных методов.

Рассмотрим принцип работы процедур Marcov, Dubins и Reeds на задаче Дубинса.

При начальном положении Time = 0 вычисляются все 6 времен и состояния путей, далее находится минимальное время из всех возможных, и определяется, какой из путей соответствует этому времени.

Таблица 5. Описание процедур определения точек касаний в методах

о

а

Вход

Тип пути

Название

Выход

Марков:

Point,Center, Radius, Side

CiS CiCh

Дубине:

Center1,Center2, Radius, Side

C1SC2 (LSL или RSR) C1SC2 (LSR или RSL)

C1CH C2

Ридс-Шепп:

Center1,Center2, Radius, Side

CiCH C2 C1SC2 (LSL или RSR) C1SC2 (LSR или RSL) Center1,Center2,Radius,MoveStyle, Side

C1CH11CH2C2 или C^C^ Ch2 IC2

Center1, Center2,

Radius, Side, TangentType

C1|Ch SC2, C1 SCH IC2 Center1,Center2, Radius, Side

C1|Chj SCH2 IC2

TangentToCircle CircleToCircle

OuterTangent InnerTangent CircleToDoubleCircle

CircleToDoubleCircle OuterTangent InnerTangent

DoubleCircle

CircleLine CircleLine

P1

och , p1

P1, P2 P1, P2 och , P1, P2

och , P1, P2 P1, P2 P1, P2

Och, , och2 , P1, Ph , P2

Och ,P1, Ph ,P2

.

А

LineDoubleCircle OCh , OCh , P1, PH, P2

Этот путь определяется как оптимальный и отрисовывается. При включенном значении AllWays, пути, состояние которых оценивается как возможно оптимальное, отрисовываются полупрозрачным цветом. Далее вызывается процедура движения автомобиля по оптимальному пути. При изменениях значения Time отрисовывается новое положение автомобиля.

Соответственно при указании конкретного значения Way, сравнения времен проводиться не будет, и оптимальное время примет значение выбранного пути.

Заключение

В данной работе представлена версия интерфейса для моделирования оптимальной парковки автомобиля. Программа разработана в системе Mathematica и в дальнейшем планируется выложить её на сайт Wolfram Demonstrations Project. Кроме рассмотренных путей Маркова-Дубинса и Ридса-Шеппа в интерфейс будут включены новые классы оптимальных траекторий: эластики Эйлера [?euler], субримановы кратчайшие на группе SE(2) [?SR-SE2], а также обобщение путей Ридса-Шеппа на субфинслеровы кратчайшие. Будет добавлена опция перепарковки прицепа из заданного начального положения фо в заданное конечное фч на основе алгоритма нильпотентной аппроксимации [?nilpotent].

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

[1] J. A. Reeds III, L. A. Shepp. "Optimal paths for a car that goes both forwards and backwards", Pacific J. Math., 145:2 (1990), pp. 367-393.

[2] H. J. Sussmann, G. Tang. Shortest paths for the Reeds-Shepp car: a worked out example of the use of geometric techniques in nonlinear optimal control, Report SYCON-91-lO, SYCON, 1991, 72 pp. urC

[3] L. E. Dubins. "On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents", American Journal of Math., 79:3 (1957), pp. 497-516. t

[4] А. А. Марков. «Несколько примеров решения особого рода задач о наибольших и наименьших величинах», Сообщ. Харьков, матем. общ. 2-я сер., 1:2 (1889), с. 250-276. gl t

[5] S. M. LaValle. Planning Algorithms, 1 ed., Cambridge University Press, 2006, ISBN 978-0521862059, 844 pp. t

[6] A. T. Becker, Shiva Shahrokhi. Shortest path for the dubins car, December 2017. url;

[7] A. A. Ardentov, Yu. L. Karavaev, K. S. Yefremov. "Euler elasticas for optimal control of the motion of mobile wheeled robots: the problem of

experimental realization", Regular and Chaotic Dynamics, 24:3 (2019), pp. 312-328. d

[8] Yu. L. Sachkov. "Cut locus and optimal synthesis in the sub-Riemannian

problem on the group of motions of a plane", ES AIM: COCV, 17:2 (2011), pp. 293-321. t

[9] A. A. Ardentov. "Controlling of a mobile robot with a trailer and its nilpotent approximation", Regular and Chaotic Dynamics, 21:7-8 (2016), pp. 775-791. d ¡>f

Поступила в редакцию 03.12.2019 Переработана 06.12.2019

Опубликована 11.12.2019

Рекомендовал к публикации

д.ф.-м.н. Ю.Л. Сачков

Пример ссылки на эту публикацию:

А. А. Ардентов, И. С. Губанов. «Моделирование парковки автомобиля с прицепом вдоль путей Маркова-Дубинса и Ридса-Шеппа». Программные системы: теория и приложения, 2019, 10:4(43), с. 97-110.

10.25209/2079-3316-2019-10-4-97-110 @ http://psta.psiras.ru/read/psta2019_4_97-110.pdf

Об авторах:

Андрей Андреевич Ардентов

Старший научный сотрудник Исследовательского Центра Процессов Управления Института Программных Систем имени А. К. Айламазяна РАН

i|[ 0000-0001-7808-7597

e-mail: aaa@pereslavl.ru

Иван Сергеевич Губанов

Инженер-исследователь Исследовательского Центра Процессов Управления Института Программных Систем имени А. К. Айламазяна РАН

МИ 0000-0003-3393-8313 e-mail: igubanov95@gmail.com

CSCSTI 27.37.17,28.17.19 UDC 004.94:519.711.2

Andrei A. Ardentov, Ivan S. Gubanov. Modeling of car parking along Markov-Dubins and Reeds-Shepp paths.

Abstract. We examine three classical model problems to describe optimal car parking: the Markov problem, the Dubins problem, the Reeds-Shepp problem. We investigate the kinematic laws for the trailer when the car is moving along the examined optimal paths. An interface is developed as Mathematica software. It constructs the desired optimal paths and the animation of the car (with a trailer) movement.

Key words and phrases: optimal control, time-minimizating problem, Wolfram Mathematica, kinematic.

2010 Mathematics Subject Classification: 49J21; 97P50, 70B15

References

[1] J. A. Reeds III, L. A. Shepp. "Optimal paths for a car that goes both forwards and backwards", Pacific J. Math., 145:2 (1990), pp. 367-393.

[2] H. J. Sussmann, G. Tang. Shortest paths for the Reeds-Shepp car: a worked out example of the use of geometric techniques in nonlinear optimal control, Report SYCON-91-lO, SYGON, 1991, 72 pp. (uRÙf

[3] L. E. Dubins. "On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents", American Journal of Math., 79:3 (1957), pp. 497-516. f

[4] A. A. Markov. "Neskol'ko primerov resheniya osobogo roda zadach o naibol'shikh i naimen'shikh velichinakh", Soobshch. Khar'kov. matem. obshch. 2-ya ser., 1:2 (1889), pp. 250-276. g] f

[5] S. M. LaValle. Planning Algorithms, 1 ed., Cambridge University Press, 2006, ISBN 978-0521862059, 844 pp.f

[6] A. T. Becker, Shiva. Sha.hrokhi. Shortest path for the dubins car, December 2017. >url t

[7] A. A. Ardentov, Yu. L. Karavaev, K. S. Yefremov. "Euler elasticas for optimal control of the motion of mobile wheeled robots: the problem of experimental realization", Regular and, Chaotic Dynamics, 24:3 (2019), pp. 312—328.

[8] Yu. L. Sachkov. "Cut locus and optimal synthesis in the sub-Riemannian problem on the group of motions of a plane", ESAIM: GOGV, 17:2 (2011), pp. 293-321.

[9] A. A. Ardentov. "Controlling of a mobile robot with a trailer and its nilpotent approximation", Regular and Chaotic Dynamics, 21:7-8 (2016), pp. 775-791.

This research was performed under Financial support from Russian Science Foundation (Project No. 17-11-01387) in Ailamazyan Program Systems Institute of Russian Academy of Sciences.

© A. A. Ardentov, I. S. Gubanov, 2019 © Ailamazyan Program Systems Institute of RAS, 2019 © Program Systems: Theory and Applications (design), 2019

Sample citation of this publication:

Andrei A. Ardentov, Ivan S. Gubanov. "Modeling of car parking along Markov-Dubins and Reeds-Shepp paths". Program Systems: Theory and Applications, 2019, 10:4(43), pp. 97-110. (In Russian).

10.25209/2079-3316-2019-10-4-97-110 url; http : //psta.psiras . ru/read/psta2019_4_97- 110 .pdf