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

У

правление подвижными объектами и навигация

УДК 004.896;519.6;62-5

МЕТОД ДЕФОРМАЦИИ ПУТИ О ЗАДАЧАХ ПЛАНИРОВАНИЯ ДВИЖЕНИЯ РОБОТОВ ПРИ НАЛИЧИИ ПРЕПЯТСТВИЙ1

Р.Ф. Гилимьянов, Л.Б. Рапопорт

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

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

введение

Задача планирования движения — одна из классических, возникающих при управлении различными робототехническими системами (роботами) [1—4]. Она заключается в следующем: для заданной модели робота в среде с неподвижными препятствиями, заданными начальной и конечной точками в конфигурационном пространстве требуется найти путь между этими точками, исключающий столкновения робота с препятствиями. Данная задача известна также как задача планирования пути.

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

1 Работа выполнена при финансовой поддержке РФФИ (грант № 09-08-00064) и комплексной программы № 15 ОЭММПУ РАН.

му актуальна задача сглаживания спланированного пути при наличии препятствий.

Другая проблема возникает, когда путь уже спланирован, а расположение препятствий изменилось либо появились новые препятствия. Тогда спланированный путь становится непригодным для движения робота без столкновений с препятствиями. В этом случае нужно перепланировать весь путь заново, что является весьма трудоемкой задачей. Либо уже во время движения робота применить один из локальных методов обхода препятствий или реактивное управление [1—5]. Это обеспечит обход препятствия, но не гарантирует, что робот вернется на ранее спланированный путь и достигнет цели. Существует другой подход к решению данной проблемы [3, 6—10], согласно которому ранее спланированный путь деформируется таким образом, что огибает препятствия. При этом полученный путь, как и ранее спланированный, соединяет начальную и конечную точки.

В настоящей работе предлагается метод в рамках данного подхода. Особое внимание уделяется гладкости полученного пути и плавности изменения его кривизны. В основу предлагаемого метода положены покомпонентный метод сглаживания кривизны пути [11] и метод потенциалов, широко применяемый для планирования пути и обхода

препятствий [1—10, 12—18]. Достаточно большой перечень работ, посвященных методу потенциалов, приведен в работе [18].

В работе [11] вариации точек ограничиваются введением в целевой функционал штрафа за большие вариации точек

1. покомпонентный метод сглаживания кривизны пути

-T

р(, ..., га) = 2 Ylj Djlj, D е

(3)

Введем обозначения и кратко повторим основные моменты работы [11]. Пусть есть упорядоченный набор точек

n > 4, r; е Rd,

(1)

1 -3 3 -1 1

M — 1 6 4 0 -6 3 , T(t) — t t2

1 3 3 -3

0 0 0 1 /

г1> Ъ .... гп,

на плоскости (й = 2) или в пространстве (й = 3). Для параметрического и гладкого представления пути используются однородные кубические В-сплайны, каждый из которых строится по четверке контрольных точек следующим образом:

/0(0 = щита), Я, = [г,. - 1, Г., Г,. + 1, Г,. + 2],

t е [0, 1],

где I — параметр сплайна.

В работе [11] предлагается сглаживать кривизну пространственной составной В-сплайновой кривой благодаря вариациям точек в произвольном к кривой направлении. Новое положение г-й контрольной точки г, определяется через вариацию е,. как

Ь(е,) = г,- + Ё = [г,; Р ..., г,; d]T + К Р ..., Ё, d]T,

где первый нижний индекс обозначает номер контрольной точки, второй — номер компоненты.

Сглаживание достигается путем минимизации нормы вектора

Д г, = г(,) - г('-11 = г, - 2 - 4г,. - 1 + 6/. - 4/. + 1 + г, + 2,

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

d

S(l, , ..., Ed) = 2 (Ö + -j)TH(r + - ),

(2)

j = 1

rj = [rl, j, ..., rn, j]T,

— [-1, j, ..., l„ j],

где H — C C, H е Rn x n — ленточная симметричная матрица,

C —

1 -4 6 -4100

0 1 -4 6 -4 10

C е

в(и - 4)xn

j = 1

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

Целевой квадратичный функционал имеет вид [11]

Ф( §1, ..., Ё,) = 1 S( ¿1, ..., Ё,) +

+ 2 Р, ..., -d) ^ .min .

2 еЬ -..Cd

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

2. сглаживание пути и его кривизны при наличии препятствий

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

Для того чтобы описанный в § 1 метод можно было применять в среде с препятствиями, предлагается добавить в целевой функционал отталкивающий потенциал U(x), который должен быть неотрицательным, дифференцируемым и при приближении к препятствиям возрастать, а при удалении — убывать. Его наличие не должно позволять точкам в результате вариаций приблизиться к препятствиям. Если расположение препятствий изменилось и точки оказались внутри препятствий, то потенциал должен «вытолкнуть» их наружу, тем самым деформировав путь.

Для сохранения целостности результирующей кривой и уменьшения ее длины и кривизны предлагается также минимизировать сумму квадратов норм векторов r;. — rt - 1 и rt - 1 — 2r; + rt + r

d

nxn

Через вариации сумма квадратов норм данных векторов с учетом суммы (2) записывается как

«У (б! , ) = = X (Г, + 6,)Т(Н + И, + н2)(г, + £,),

) = 1 Т

Н2 - СТ С2, Н2 е

где Н1 - СТ С1, Н1 е Г ленточные симметричные матрицы

С1 =

С2 =

1 -10 0 ... 0 1 -10 ...

1 -2 1 0 0 0 1 -2 10

С1 е

л (п — 1) X п

с2 е И(п - 2) х п.

Необходимо закрепить концы кривой. Это можно сделать наложением большего штрафа на вариации точек в начале и конце пути. Штраф можно выбрать согласно формуле (3) или с учетом близости точек к препятствиям, например, следующим

р(61, ^) - X у£, 1 £,

] = 1

- Шав(Д у (£1)), ..., Д( у (бп))),

(4)

где у — положительный параметр, 1— диагональная матрица, одинаковая для всех у, Д( у (б,)) — дифференцируемая функция, возрастающая по мере удаления точки у (б,) от препятствий, например,

1

Д у (£,)) -или Д(у (£.)) -

в + и(У(£г.))

_1_

в + 1п (1 + и(?,(£,)))

где в — положительный параметр. Таким образом, точкам находящимся внутри или близко к препятствиям назначается меньший штраф. Штраф также может быть некоторой комбинацией функционалов Р( £1, ..., £ё) и Р (£1, ..., £ё).

С учетом дополнительных членов и отталкивающего потенциала целевой функционал

1

1

ф (£1, £d) - 2 «(£1, ^) + 2 Р (£1, £ё) +

+ £ и( Г, (б,)) ^ Ш1П .

, = 1 Е1, ...,Е</

(5)

Данная формула не предполагает использование конкретного отталкивающего потенциала. В следующем параграфе будет рассмотрен отталкивающий потенциал для препятствий в виде выпуклых многоугольников/многогранников.

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

3. отталкивающий потенциал

Пусть препятствия задаются выпуклыми областями и описываются системой линейных уравнений вида g(x) < 0, х е ЯВ двумерном случае это будут выпуклые многоугольники, а в трехмерном — выпуклые многогранники. Для таких препятствий в работе [14] представлена потенциальная функция

Р(х) -

1

5 + /(х)

где 5 — малая положительная константа,

М/аеея

/х) - X №) + к(х)1), , = 1

Щаевз — число сторон многоугольника в двумерном случае или граней многогранника в трехмерном случае. Значение скалярной функции /(х) равно нулю внутри препятствия и возрастает линейно снаружи по мере удаления от него. Преимущество указанного потенциала заключается в том, что не нужно считать точное расстояние до препятствия, что может быть довольно сложной задачей. Недостаток — в том, что функция /(х) и, следовательно, потенциальная функция р(х) не дифференцируемы.

В работе [19] используется схожая с /(х) функция, лишенная указанного недостатка,

Щаеез

Г(х) - X

где

у/х) - (g2 (х) + и2)1/2 + ?.(х), 0 < и << 1.

п X п

1

п

Функция v;(x) имеет следующий порядок малости

v.(x)

2gi + O(и2), gi > 0, и, g = 0,

IO (и 2),

',< 0.

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

и ,(*) = Кв-а¥(х\

4. градиент целевого функционала

Решать задачу минимизации функционала (5) можно различными методами оптимизации. Для некоторых из них требуется знание градиента целевого функционала. Поэтому выпишем градиент целевого функционала от йп переменных

дФ ( Ё 1, . . . , Ё о) дФ ( Ё 1 , .. . , Ё о) . дЁ1 5Ёо _

V(b (Ej, ..., ld) =

где

(6)

где K и а — некоторые положительные константы. При удалении от препятствия значение потенциала убывает экспоненциально, а внутри препятствия близко к K. Такой потенциал не позволяет точкам пути приближаться близко к препятствию. Однако если препятствие лежит на пути и точки оказались внутри него, то необходимо, чтобы потенциал внутри препятствия не был таким пологим, как потенциал (6), и выталкивал точки за пределы границы препятствия. Для этого используем потенциал, предложенный в работе [13]:

Un(x) = Umax (1 - + Uoffset,

Rmax

где Umax — максимальное значение потенциала без сдвига на значение Uofet, Rin(x) — расстояние от те-

дФ ( E 1, . . . , E d) = BE,

дФ(Ei, ..., Ed) ВФ(Ei,..., Ed)

Be

i, j

BEn, j

ВФ ( E i, . . . , E d) = BS ( e i, . . . , E d) + BP ( Ei , .. . , E d) +

BE-

+

be,-

'BU(?i (Ei)) BUfo(En))'

BE,

be>

BEn

'i, j j Первое слагаемое имеет простой вид

BS ( E ь . . . , E d) = (h + H + H2)( r, + E,).

i 2 j j

BEj

Если штраф выбран согласно формуле (3), то

BP ( Ei , .. . , E d) = yD. E.,

BEj

кущей точки x = [xi, ..., xd] до центроида препятс- a если согласно формуле (4), то

T

твия с координатами c = [сь, ..., cd] :

Rin(X) = |Z (Xj - С,)2 ,

Rmax — расстояние от центроида до самой дальней вершины препятствия. Потенциал U0fsei можно взять равным потенциалу Uob(x), тогда потенциал препятствия под номером k можно записать в следующем виде:

Uk(x) = U0b (X) + Umax(1 - & (x)/Rmax ),

где второе слагаемое считается только для точек внутри препятствия, т. е. для которых выполнено f(x) = 0. Если число препятствий равно Nob, то итоговый потенциал будет складываться из потенциалов каждого препятствия:

Nob

U(x) = Z Uk

k = i

BP(Eb ..., Ed) ,чч , 2 BF(ri(Ei))

BE . , = 2Y Ei, jF(ri (Ei)) + Y Z E2, m BE' . , '

■4 j

m = i

4 j

Выпишем частные производные итогового потенциала от Nob препятствий:

BU(ri(E'.)) _ Nob BUk(r(Ei))

BEi, j

Z

k = i

BEi

', j

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

дУк ( Г, ( ё , ) ) =

диоЬ( Г)(Ё))) _ ( ./ + Ё. 1 - С- /(?.(ё0) = 0,

BEi, j BUob(r(Ei))

Rm

Rin(r{)

BEi

f(ri(Ei))> 0,

i, j

d

где

= —aKe

-aV(x)

Nfaces

' I

l = 1

Частные производные

dPQbfr (e,)) =

de j

g i (r/( e/) ) л/g/ (r / (e/)) + u

dg i (r i (e j))

+ 1

dg i (^j- ( e j) )

de

находятся

j

просто, так как gl( (ei)) — линейная функция.

4. численные примеры

В описываемых далее примерах задавались препятствия на плоскости и в трехмерном пространстве, начальное и конечное положения точечного свободнолетающе-го робота. Модели препятствий создавались с помощью программы «Google SketchUp» [20]. Затем с помощью широко применяемого метода планирования пути [21], реализованного в библиотеке «Open Motion Planning Library» [22], планировался путь, соединяющий начальную и конечную точки в обход препятствий. Спланированный путь представлял собой набор упорядоченных точек.

На рис. 1 штриховой линией отрезками прямых изображен спланированный путь на плоскости с препятствиями. Кривизна спланированного пути, представленного с помощью однородных кубических B-сплайнов, в зависимости от длины пути изображена на рис. 2 штриховой линией.

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

Рис. 2. Зависимость кривизны ранее спланированного пути (штриховая линия) и кривизны модифицированного пути на плоскости от длины пути

Рис. 1. Ранее спланированный путь (штриховая линия)

Рис. 3. Увеличенный фрагмент ранее спланированного пути (штриховая линия) и модифицированного пути на плоскости

Путь, построенный по этому набору точек с помощью однородных кубических В-сплайнов, показан на рис. 1 сплошной линией, а его кривизна в зависимости от длины пути на рис. 2. Как видно из рис. 2 и 3, где показан увеличенный фрагмент рис. 1, построенный по новому набору путь и его кривизна имеют более плавный вид.

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

В результате был получен новый набор точек. Модифицированный путь, построенный по этому набору то-

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

Следующий пример иллюстрирует применение предложенного метода для деформации пути в трехмерном пространстве с препятствиями. Спланированный

Рис. 7. Увеличенный фрагмент ранее спланированного пути (штриховая линия) и модифицированного пути в трехмерном пространстве

Рис. 4. Новое местоположение препятствий и модифицированный путь (сплошная линия)

Рис. 5. Отталкивающий потенциал для препятствий на плоскости

Рис. 8. Новое расположение препятствий и модифицированный путь

путь в этом пространстве изображен на рис. 6 сплошной линией и на рис. 7 штриховой линией. Аналогично предыдущему примеру расположение некоторых препятствий было изменено. Затем с помощью предложенного метода был получен модифицированный путь, показанный на рис. 8. Увеличенный фрагмент модифицированного пути показан на рис. 7 сплошной линией. Из рисунков видно, что, как и в предыдущем примере, модифицированный путь плавно огибает препятствия.

заключение

Рис. 6. Спланированный путь в трехмерном пространстве с препятствиями

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

литература

1. Choset H., et al. Principles of Robot Motion: Theory, Algorithms, and Implementations. — Boston: MIT Press, 2005.

2. LaValle S.M. Planning Algorithms. — Cambridge, UK: Cambridge University Press, 2006.

3. Siciliano B., Khatib O. Handbook of Robotics. — Berlin: Springer, 2008.

4. Siciliano B, Sciavicco L., Villani L, Oriolo G. Robotics: Modelling, Planning and Control. — London: Springer, 2009.

5. Khatib O. Real-time obstacle avoidance for manipulators and mobile robots // In Proc. of the IEEE Int. Conf. on Robotics and Automation. — 1985. — Vol. 2. — P. 500—505.

6. Quinlan S, Khatib O. Elastic bands: connecting path planning and control // In Proc. of the IEEE Int. Conf. on Robotics and Automation. — 1993. — P. 802—807.

7. Quinlan S. Real-Time Modification of Collision-Free Paths: Ph.D. thesis. — Stanford University, 1995.

8. Khatib M., Jaouni H, Chatila R, Laumond J.P. Dynamic path modification for car-like nonholonomic mobile robots // In Proc. of the IEEE Intern. Conf. on Robotics and Automation. — 1997. — P. 2920—2925.

9. Lamiranx F, Bonnafous D, Lefebvre O. Reactive path deformation for nonholonomic mobile robots // IEEE Trans. on Robotics and Automation. — 2004. — Vol. 20, N 6. — P. 967—977.

10. Brock O, Khatib O. Real-time re-planning in high-dimensional configuration spaces using sets of homotopic paths // In Proc. of the IEEE Int. Conf. on Robotics and Automation. — 2000. — Vol. 1. — P. 550—555.

11. Гилимьянов Р.Ф. Покомпонентный метод сглаживания кривизны пространственного пути, построенного по за-шумленным измерениям в задачах планирования движения роботов // Проблемы управления. — 2011. — № 6. — С. 66—72.

12. Khosla P., Volpe R. Superquadric artificial potentials for obstacle avoidance and approach // In Proc. of the IEEE Intern. Conf. on Robotics and Automation. — 1988. — Vol. 3. — P. 1778—1784.

13. Warren C.W. Global path planning using artificial potential fields // In Proc. of the IEEE Intern. Conf. on Robotics and Automation. — 1989. — Vol. 1. — P. 316—321.

14. Hwang Y.K., Ahuja N. A Potential Field Approach to Path Planning // IEEE Trans. on Robotics and Automation. — 1992. — Vol. 8, N 1. — P. 23—32.

15. Hashimoto H, Kunii Y., Harashima F, Utkin V.I, Drakunov S.V. Obstacle Avoidance Control In Multi-dimensional Space Using Sliding Mode // In Proc. of the IEEE/RSJ Intern. Conf. on Intelligent Robots and Systems. — 1992. — Vol. 1. — P. 697—702.

16. Kitamura Y., Tanaka T., Kishino F, Yachida M. 3-D path planning in a dynamic environment using an octree and an artificial potential field // In Proc. of the IEEE/RSJ Intern. Conf. on Intelligent Robots and Systems. — 1995. — Vol. 2. — P. 474—48.

17. Chuang J.-H. Potential-based modeling of three-dimensional workspace for obstacle avoidance // IEEE Trans. on Robotics and Automation. — 1998. — Vol. 14, N 5. — P. 778—785.

18. Masoud S.A., Masoud A.A. Motion planning in the presence of directional and regional avoidance constraints using nonlinear, anisotropic, harmonic potential fields: a physical metaphor // IEEE Trans. on Systems, Man and Cybernetics, Part A: Systems and Humans. — 2002. — Vol. 32, N 6. — P. 705—723.

19. Comba P.G. A Procedure for Detecting Intersections of Three-Dimensional Objects // Journal of the ACM. — 1968. — Vol. 15, N 3. — P. 354—366.

20. SD-моделирование для всех и каждого. — URL: http:// sketchup.google.com/ (дата обращения (22.11.2011).

21. Lavalle S.M. Rapidly-exploring random trees: A new tool for path planning: Tech. Rep. TR 98-11. — Computer Science Department, Iowa State University, 1998.

22. The Open Motion Planning Library. — URL: http://ompl.kav-rakilab.org/ (дата обращения (22.11.2011).

Статья представлена к публикации членом редколлегии Б.В. Павловым.

Гилимьянов Руслан Фаильевич — канд. техн. наук, ст. науч. сотрудник, S (495) 334-93-69, И [email protected],

Рапопорт Лев Борисович — д-р физ.-маг. наук, зав. лабораторией, S (495) 334-86-60, И [email protected],

Институт проблем управления им. В.А. Трапезникова РАН, г. Москва.

"овал книга

Архипова Н.И., Кульба В.В., Микрин В.Е. Модели и методы репланирования сельскохозяйственного производства в условиях чрезвычайных ситуаций с использованием аэрокосмической информации. — М.: Экономика, 2012. — 228 с.

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

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