CC BY
111
УДК 621.87:621.865.8
МЕТОДИКА ПЛАНИРОВАНИЯ ТРАЕКТОРИИ ОБЪЕКТА В СРЕДЕ С ПРЕПЯТСТВИЯМИ НА ОСНОВЕ МОДИФИЦИРОВАННОГО АЛГОРИТМА ВЕРОЯТНОСТНОЙ ДОРОЖНОЙ КАРТЫ
В.С. Щербаков, М.С. Корытов
ГОУ ВПО «Сибирская государственная автомобильно-дорожная академия», г. Омск E-mail: [email protected]
Описывается модификация алгоритма вероятностной дорожной карты, позволяющая осуществлять поиск оптимальной траектории перемещения грузоподъемной машиной груза произвольной формы в трехмерном пространстве с произвольными препятствиями, заданными в дискретном виде, с учетом угловой ориентации груза.
Ключевые слова:
Алгоритм вероятностной дорожной карты, планирование оптимальной траектории, угловые координаты. Key words:
Algorithm for probabilistic roadmap, optimal trajectory planning, angular coordinates.
Проблема оптимизации траектории перемещения объекта в трехмерном пространстве с препятствиями является актуальной. В качестве такого объекта может рассматриваться груз, перемещаемый грузоподъемными машинами.
Алгоритм вероятностной дорожной карты PRM (Probabilistic Road Map) относится к современным подходам в области планирования траекторий. Этот подход считается одним из ведущих при планировании движения, в первую очередь для механических систем со многими степенями свободы в среде с препятствиями. Вероятностный метод PRM является высокоэффективным, простым в реализации, и применимым для различных видов задач, связанных с планированием траектории движения [1, 2].
Представляется целесообразным использование преимуществ алгоритма вероятностной дорожной карты (ВДК) при решении задачи оптимизации траектории перемещения объемного объекта-груза в среде с препятствиями с учетом координат угловой ориентации.
Постановка задачи. Заданы линейные и угловые координаты груза в начальной sHm и в конечной sm точках траектории груза в пространстве:
Sнач=(xн<),Ун<),Zн<),Гнойно); Swн=(xк0>,Ук0>,Zк0>,Хк0>ск0)>
где хн0, ун0, z^ - линейные координаты точки начала локальной прямоугольной системы координат груза OXYZ& в неподвижной прямоугольной системе координат OfX0YiZ0, связанной с рабочей областью перемещений, соответствующие начальному положению груза; хк0, ук0, zK0 - линейные координаты, соответствующие конечному положению груза; ун0, сон0 - координаты углов поворота груза вокруг осей Хр Y в начальной точке; уа, сок0 - угловые координаты, соответствующие конечному положению груза в пространстве.
Ось X0 неподвижной системы координат направлена параллельно линии, соединяющей две точки в пространстве: начальную sum и конечную sKm точки траектории. Это позволяет упростить расчет и уменьшить объем вычислений.
Линейные и угловые координаты груза заданы на равномерной сетке: /б[1;/тах]; /'e^/J;
ке [1;^]; /е[1;/тах]; т е [1;тшах]. Индексы ¡,}, к соответствуют линейным перемещениям точки начала локальной системы координат груза соответственно вдоль осей Х0, 70,2{„ а индексы I, т - двум углам поворота груза у0, щ вокруг собственных осей соответственно. Задан Аитар - шаг дискретности угловых координат у и а>.
В локальной системе координат груза ОХУ^^ заданы координаты габаритных точек г£е[1;сг] на поверхности объемного тела груза, определяющие его форму. Координаты точек заданы векторами вида (Яя\=[Хц,у ¡^ 1]г, где х, у, ^ - координаты точки I в локальной системе координат груза.
В неподвижной системе координат О0Х0У{^0 задана дискретная матрица высот препятствий УПР(¡,к), где ¡, к - индексы координат х0, у0 соответственно: ге[1;гшах]; ке[1;кшах].
Для дискретного описания исходных данных задачи формируется граф Ог=(6г,Ег), где - множество вершин графа, Nr={(s¡ьsn)}:¡lsn=1 - множество дуг (ребер). Общее количество вершин графа щ определяется заданным количеством рассматриваемых точек в пространстве конфигураций, свободном от препятствий.
Необходимо найти оптимальную траекторию с минимальным значением целевой функции Ь" произвольного вида из начальной вершины sнач в конечную вершину sKон, представляющую собой последовательность из нескольких вершин графа дорожной карты Ог: 5"={лр}-р-1. Каждой вершине графа соответствует определенное пространственное линейно-угловое положение груза в свободном пространстве дорожной карты.
Структура графа дорожной карты определяется квадратной матрицей весов дуг ^[Ь^]:^. Значения весов Ьа п определяются по зависимости целевой функции.
В качестве целевой функции может использоваться любой интегральный критерий оптимальности на основе координат груза в точках его траектории. В качестве примера использовалось среднее взвешенное значения длин линейных и угловых перемещений Ь. В случае дискретного представле-
ния траектории среднее взвешенное Хумежду двумя точками траектории I и у может быть представлено в виде [3]:
+СГШ>] (/¡-У; )2 + (Щ-Щ )2 .
Полное выражение целевой функции отдельной траектории 3 для дискретного представления имеет вид [3]:
( П- ~ " ~ " ч ? Л
x - x^f + (y, - ytj2 + - z-i)2 + +crj Ori-L-12 + (Ц-Ц-1)2
(1)
где 1шах - число точек отдельной дискретной траектории.
Если две вершины не соединены между собой, например, по причине наличия препятствий между ними, вес соответствующей дуги (ребра) графа принимается равным бесконечно большому значению:
(2)
Существует несколько реализаций алгоритма ВДК с различными подходами к формированию дуг между вершинами и матрицы весов графа соответственно.
Традиционный подход, применяемый для соединения точек дугами в 2-х и 3-мерном Евклидовом пространстве, заключается в использовании быстрого и простого алгоритма соединении дугами каждой точки с ближайшими соседями, например, при помощи метода триангуляции [1, 2].
Достоинством такого подхода является его простота и эффективность, поскольку при соединении соседних точек в свободном пространстве, находящихся на минимальном расстоянии друг от друга, можно пренебречь проверкой на пересечение с препятствиями промежуточных положений объекта.
Для решения поставленной задачи данный подход является неполным, т. к. в «-мерном случае, т. е. при рассмотрении не только трех линейных, но и нескольких угловых координат, проверка на пересечение с препятствиями промежуточных положений объекта между двумя точками становится необходимой, без нее возможно соединение вершин с ближайшими соседями только с учетом линейных координат, в результате чего могут быть потеряны глобально оптимальные пути перемещения.
Предложен и реализован другой подход, обладающий полнотой. Полученные случайным образом вершины графа соединяются между собой дугами с учетом видимости, т. е. выполнением условия непересечения с препятствиями при движении из вершины в вершину по прямой в пространстве конфигураций. Выполняется проверка видимости между текущей вершиной £яе{$-} и каждой из подмножества вершин ¿дб{&} с большими или равными значениями линейной координаты груза х. Подмножество {3х} формируется из множества {3г} по условию:
Щ^^^х}); Хп > (х-^т), (3)
где {3х}с{Яг}.
После того, как сформирована матрица весов графа [#], осуществляется поиск кратчайшего пути между двумя вершинами графа (sHa4 и sm) при помощи традиционных алгоритмов поиска на графе.
Далее выполняется интерполяция и локальная оптимизация найденной траектории. После локальной оптимизации найденная траектория представляет собой последовательность из смежных вершин, заданных на равномерной решетке S={sp}ip=x.
Описание модифицированного алгоритма ВДК перемещения груза, положение которого определяют 3 линейные и 2 угловые обобщенные координаты.
1. Задание численных значений исходных данных: 5нач=(хн0,ун0,^н0,)'н0,®н0); 5^н=(Хк0,ук0,^к0,Хк0,®к0); {Д?}; 'max; jmax; kmax; lmax; mmax; [ ng; Cym Au; Opt^x; 1зап_г; 1зап_в.
Параметры соответствуют описанным при постановке задачи. Собственный параметр алгоритма ВДК, задаваемый эмпирически: ng - количество вершин графа дорожной карты.
2. C использованием методики построения полидистантных поверхностей вокруг реальных поверхностей препятствий, заданных дискретно в трехмерном пространстве [6], формируется массив [ Ymjn] гиперповерхности минимальных значений вертикальных координат условного центра груза с учетом его угловых координат: YmM(i,k,l,m), ¿e[1;imx]; ke[1;kmJ; le^U; me[1;mmJ. Гиперповерхность представляет собой функцию минимально возможных (при выполнении условия непересечения с препятствиями) значений вертикальной координаты y точки начала локальной системы координат груза OgXgYgZg, принятой за условный центр груза, в неподвижной системе координат O0X0Y0Z0.
3. Генерируется случайным образом множество вершин Sr={s2,...,sng-1} графа дорожной карты, представляющих собой точки в пространстве конфигураций, т. е. возможные положения груза в пределах заданной сетки координат, в которых он не пересекается с препятствиями.
Для создания дорожной карты при помощи генератора случайных чисел создается ng точек в пространстве конфигураций с координатами: sp=(xp,yp,zp,yp, щ), pe[2;ng-1],
где
Xp=ipAl; y„=jAl; z„=kpAl; 7p=(lp-0,5lmJAu;
®i=(mi-0,5mmax)Au; ;=[Rand ¿max];i=[Rand¿J; L=[Rand kJ;
(4)
/p=[Rand /max]; mp=[Rand mj. (5)
где Rand - случайное число с равномерным законом распределения в диапазоне [0; 1].
Значения (xp,yp,zp,rp,o>p), полученные для каждого значения p, должны удовлетворять условию непересечения груза с полидистантной (эквидистантной) поверхностью [Y3], что через массив гиперповерхности Ynn описывается следующим условием:
yp>Ymn(ip,kp,/p,mp). (6)
При выполнении этого условия значение p увеличивается на 1, в противном случае генерация отдельной точки по (4), (5) повторяется.
¡=2
Первая (p=1) и последняя (p=ng) точки траектории будут совпадать с начальной и конечной заданными точками:
S1=S»M=(x»0>y»0>Z»0> Х»0> ®»0); Sng=SKOH=(XKS),yKS),ZK<),Yic<),^ic<))-
4. Формируется матрица весов дуг N=[Li1J1]^j1=1. Выполняется проверка видимости между текущей точкой sa е {Sr} и каждой из подмножества точек sj1 е {Sx} с большими или равными значениями линейной координаты груза х. Подмножество {Sx} формируется из множества {Sr} с выполнением условия (3). Для этого, используется два вложенных цикла: внешний /1e[1;ng] и внутренний j1e[1;ng]. Для каждого сочетания значений /1 и j1 осуществляется проверка выполнения условия(З). При невыполнении данного условия вес дуги (saj принимается равным бесконечно большому значению по (2).
Если условие выполняется, то проводится дополнительная проверка выполнения условия непересечения с препятствиями всех точек, лежащих на прямой в пространстве конфигураций между точками sa и j Находят применение два способа осуществления подобной проверки [1, 4]: 1) при помощи инкрементного алгоритма (алгоритма последовательных малых приращений); 2) при помощи рекурсивного алгоритма деления отрезка пополам (используя метод дихотомии).
Проведенные рядом зарубежных авторов исследования показали, что рекурсивный алгоритм деления отрезка работает в общем случае быстрее ин-крементного [1, 4]. Причина в том, что в среднем положении любого отрезка груз имеет самую высокую вероятность пересечения с препятствием, если таковое имеет место.
Осуществлялась проверка выполнения условия непересечения с препятствиями всех точек, лежащих на прямой в пространстве конфигураций между точками s/1 и sj1, используя второй способ, по следующей модификации рекурсивного алгоритма.
4.1. Переменная break, определяющая наличие (break=1) либо отсутствие (break=0) пересечения груза с препятствиями при движении по прямой в пространстве конфигураций между двумя точками s/1 и sj1, принимается равной нулю:
break=0. (7)
4.2. В цикле с идентификатором /2 происходит изменение значения индекса /2 от 1 до некоторого максимального значения /2max, определяемого условием
AL^ALmin, где ALmin - постоянная величина;
AL = 7 (Ax2)2 + (Ay 2)2 + (Az 2)2 + „V (A/2)2
+c,„
-(Ает2)2
где Ах2=(хя-Хд)//2; Ау2=(уя-уд)//2; Аг2=Сгя-гд)//2; Аг2=(^1-^1)//2; Аю2=(ю,'1-а>л)/г'2.
Изменение индекса ¡'2 на каждой итерации цикла ¡2 происходит по закону геометрической прогрессии (увеличение в 2 раза):
i2=й-2.
Значение i2 определяет количество равноотстоящих друг от друга точек на прямой между точками sa и sj1, в которых на текущей итерации цикла i2 происходит проверка пересечения груза с препятствиями, рис. 1.
AL
Sil '—*—> Si
^-1-1-1-1-1-ЪЧ!
¿2=1; j'2e[1;2] ¿2=2; j'2e[1;4]
¿2=4; j'2e[1;8]
Рис. 1. Гграфическая интерпретация модификации рекурсивного алгоритма деления отрезка пополам (пример на плоскости, дерево рекурсии)
4.3. Кроме того, на каждой итерации цикла
i2 во вложенном цикле j2 с интервалом в 1 варьируется вспомогательный индекс j2:
j2e[1; /2.2].
Максимальное значение j2=i2.2 определяет суммарное количество точек, проверенных на пересечение с препятствиями с начала цикла /2. Причем, на каждой итерации цикла i2 во вложенном цикле j2 проверяются новые точки, не совпадающие с проверенными на предыдущей итерации i2, рис. 1. Для этого на каждой итерации алгоритма /2 определяются собственные значения координат точки, с которой начинается проверка:
x2Ha4=Xii+(Xii-Xji)/(i2.2); y2„a4=уя+(уя-удУ(й?2);
z2um=Zii+(Zn-Zji)/( /2.2); у2НаЧ=Гй+(Гй-Гп)/(/2.2); „2ш=„я+(„я-„>;1У(/2.2).
Затем во вложенном цикле j2 вычисляются координаты текущей проверяемой точки:
x2=x2„a,+Ax2(/2-1); y2=y2„,+Ay2(/2-1); z2=z2„a,+Az2 С/2-1); y2=y2Ha+Ay2(j2-1); „2=„2Hl+A„2(j2-X).
Соответствующие индексы координат нарав-номерной дискретной сетке определяются следующим образом:
i=[x2/A/]; j=[y2/A/]; k=[z2/A/]; H(/-ymn)/Au];
m=[(©2-„min)/Au]. (8)
4.4. Далее во вложенном цикле j2 для текущей проверяемой точки с индексами координат (8) выполняется собственно проверка условия непересечения груза с препятствиями (6). В случае, если данное условие не выполняется, т. е. имеет место пересечение груза с препятствиями, переменная break принимается равной 1, и одновременно циклы j2 и i2 прерываются:
break=1.
4.5. Данный пункт выполняется после завершения либо прерывания вложенных циклов j2 и i2. Выполняется проверка условия (7). Если данное условие не выполняется, т. е. имеет место пересечение груза с препятствиями при движении из точки
Рис. 2. Блок-схема модифицированного алгоритма ВДК для 5-и учитываемых обобщенных координат груза
s¡1 в s^1, вес соответствующей дуги (ребра) графа принимается равным бесконечно большому значению по (2). В случае выполнения условия (7) вес дуги рассчитывается как значение целевой функции по (1).
5. Осуществляется поиск кратчайшего пути между двумя вершинами графа ^нач и sкон) при помощи алгоритма Дейкстры. Результатом поиска является оптимальная траектория 3* с минимальным значением целевой функции Ь*, представляющая собой последовательность из нескольких вершин графа дорожной карты 3*=^}^.
6. Осуществляется линейная интерполяция найденной траектории на равномерной сетке обобщенной координаты ^¡(¡'е [1;/шах]) для вычисления обобщенных координат у¡, г„ у, с..
В результате интерполяции получается траектория, представляющая собой последовательность из смежных вершин, заданных на равномерной решетке 3=^}™!
7. Выполняется локальная оптимизация интерполированной траектории 3*.
8. Определяется уточненное значение целевой функции Ь* оптимизированной лучшей траектории 3* по (1).
9. Вывод результатов: Ь*, 3*. Окончание работы алгоритма.
Блок-схема модифицированного алгоритма ВДК приведена на рис. 3.
Примеры найденной траектории 3* (точки начала координат системы груза) и этой же траектории после интерполяции и локальной оптимизации с указанием положений осей груза в форме цилиндра приведены на рис. 2, а и б, соответственно. Для неоптимизированной траектории положения осей цилиндра не показаны, чтобы не затемнять рисунок. Груз в форме цилиндра в рассматриваемом примере имел габаритный диаметр 0,5 условных единиц, и высоту 2 условные единицы. Траектория после локальной оптимизации на рис. 2, б, совпадает с глобальным минимумом за-
данной целевой функции. Время поиска приведенной в качестве примера траектории составило менее 2 с на ЭВМ средней производительности (AMD Athlon 64 X2 Dual Core Processor 5600+2,90 ГГц) при количестве узлов графа ng=800.
Рис. 3. Примеры найденной траектории модифицированным алгоритмом: а) до и б) после локальной оптимизации
Выводы
Одним из эффективных методов оптимизации траектории движения объекта в трехмерном пространстве с препятствиями является модифицированный метод вероятностной дорожной карты.
Предложенный в работе модифицированный алгоритм реализован в средах Microsoft Visual C++ и MATLAB. Эффективность алгоритма подтверждена конкретными расчетами, которые показали, что траектория движения объекта после локальной оптимизации совпадает с глобальным минимумом целевой функции. Алгоритм обладает сравнительно высоким быстродействием по сравнению с аналогичными, его применение возможно в системах автоматического управления грузоподъемными машинами при перемещении грузов в трехмерном пространстве с препятствиями.
СПИСОК ЛИТЕРАТУРЫ
1. Geraerts R., Overmars M.H. A comparative study of probabilistic ro-admap planners // Proc. Workshop on the algorithmic foundations of robotics (15-17 December, 2002). - Nice, France: WAFR, 2002. - P. 43-57.
2. Kavraki L.E., Latombe J.-C. Randomized preprocessing of configuration space for fast path planning // IEEE Intern. Conf. Robotics and Automation (8-13 May, 1994). - San Diego, CA, USA: IEEE Press, 1994. - P. 2138-2145.
3. Джини К. Средние величины. - М.: Статистика, 1970. - 447 с.
4. Sanchez G., Latombe J.-C. A single-query bi-directional probabilistic roadmap planner with lazy collision checking // Intern. Symp. robotics research (9-12 November, 2001). - Lorne, Victoria, Australia: ISRR, 2001. - P. 403-417.
5. Калиткин Н.Н. Численные методы. - М.: Наука, 1978. - 512 с.
6. Корытов М.С. Использование полидистантных поверхностей в задаче поиска пути перемещения груза в среде с препятствиями // Матер. 64-й научно-техн. конф. ГОУ «СибАДИ». -Омск: СибАДИ, 2010. - Кн. 1. - С. 302-306.
Поступила 21.01.2011 г.
