4. Определение параметров и переменных модели.

Рассмотрим параметры модели для алгоритма, описывающего движение манипулятора в среде с известными препятствиями.

Все входные и выходные параметры представляют собой точку на двухмерной обобщенной системе координат, поэтому каждый параметр имеет составляющие x и y.

Входные параметры:

1. q⁰(x,y), q^T(x,y) – стартовая и целевая конфигурации;

2. mZap[z](x,y) - массив запрещенных точек;

3. z - количество запрещенных точек;

4. delta - шаг дискретизации;

5. qL(x,y) и qH(x,y) – вектора нижнего и верхнего ограничения на обобщенные координаты.

Выходные данные:

1. mRez[n](x,y) - массив точек конфигураций искомого пути;

2. n - количество точек конфигураций искомого пути.

5. Установление основного содержания модели.

Описание алгоритма двунаправленных графов [S.M. LaValle].

Известные препятствия задаются в виде геометрических фигур с n-вершинами, также известны координаты стартовой q⁰ и целевой q^T конфигураций (рис. 1.5).

Рис. 1.5 Пространство обобщенных координат. Препятствия 1 и 2, стартовая q⁰ и целевая q^T конфигурации.

1. Проводится отрезок из стартовой конфигурации q⁰ в целевую q^T. Если построенный отрезок не накладывается на препятствия, то этот отрезок есть искомый путь, а если отрезок налегает на препятствие, то делаем следующее.

   1. Нумеруем вершины препятствий от 1 до n.

   2. Соединяем точки всех вершин препятствий и точки стартовой и целевой конфигураций m-отрезками по принципу каждая с каждой.

Рис. 1.6 Стартовая точка q⁰ соединена со всеми вершинами препятствий

2. Отрезки, которые, накладываются на препятствия, мы не рассматриваем. Рассматриваются отрезки, которые либо не накладываются на препятствия, либо принадлежат граням препятствия (рис. 1.7).

Рис. 1.7 Жирные линии – рассматриваемые отрезки, пунктирные линии – не рассматриваемые отрезки.

3. Имеем k-путей из q⁰ в q^T.Сравниваем их по длине и выбираем кратчайший, т.е. получаем искомый путь (рис. 1.8).

Рис. 1.8 Искомый путь обозначен штрихпунктирной линией.

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

В описании алгоритма используются понятия:

• окрестная точка – точка, стоящая на расстоянии delta от текущей точки (рис. 1.9)

Рис. 1.9 Текущая точка qⁱ имеет 8 окрестных точек.

• Область запрещенных точек – совокупность точек, каждая из которых имеет хотя бы одну соседнюю точку, отстоящую не более чем на delta.

1. Задаем входные параметры: q⁰(x,y), q^T(x,y), mZap[z](x,y), delta, qL(x,y) и qH(x,y) (рис. 1.10).

Рис. 1.10 Система обобщенных координат, содержащая три запрещенные точки mZap[0], mZap[1], mZap[2].

2. Получаем предварительный маршрут Pr[] – кратчайший путь от q⁰ к q^T без учета запрещенных точек следующим образом:

1. В начальный момент времени текущая конфигурация соответствует стартовой конфигурации qⁱ = q⁰.

2. Через точки qⁱ и q^T проводится прямая вида Ax+By+C=0.

3. Из окрестных точек qⁱ выбирается qⁱ⁺¹ та, которая «максимально приближена» к прямой Ax+By+C=0 и «максимально приближена» к q^T.

Расстояние от точки qⁱ до точки q^T определяется по формуле:

(1)

Расстояние от точки до прямой Ax+By+C=0 определяется следующим образом:

коэффициенты прямой Ax+By+C=0:

A = Y_qT - Y_q0,

B = X_q0 - X_qT,

C = - X_q0*A - Y_q0*B,

если С > 0, то M=-M.

Расстояние от точки qⁱ до прямой Ax+By+C=0 определяется по формуле:

P = A*M* X_qi + B*M* Y_qi + C*M; (2)

«Максимально приближенной» к прямой Ax+By+C=0 и «максимально приближенной» к q^T является та окрестная точка, у которой значение Summa=P+D будет наименьшим.

4. Выбранная точка записывается в массив предварительного маршрута mPr[i+1]= qⁱ⁺¹ и становится текущей точкой qⁱ= qⁱ⁺¹.

5. алгоритм переходит к п. 2.2 если текущая точка qⁱ не достигла целевой q^T.

Для данного случая получим (рис. 1.11).

Рис. 1.11 Система обобщенных координат. Точками 0-5 обозначены точки предварительного маршрута.

3. Сравниваем массив предварительного маршрута с массивом всех запрещенных точек. Если точки предварительного маршрута не налегают на запрещенные точки, т.е. манипулятор при движении из q⁰ в q^T не встречает препятствий, то массив предварительного маршрута mPr[] есть искомый массив mRez[] и алгоритм прекращает работу. В противном случае каждая запрещенная точка, на которую мы натолкнулись (в данном примере - mZap[2] и mZap[1]), считается точкой запрещенной области.

Из (рис. 1.11) видно, что точки 2 и 4 предварительного маршрута совпали с запрещенными точками mZap[2] и mZap[1] соответственно. Ни одна точка предварительного маршрута не натолкнулась на запрещенную точку mZap[0].

4. Формируются массивы областей запрещенных точек (в данном примере – массивы областей точек mZap[2] и mZap[1]). Для каждой из этих запрещенных точек осуществляется проверка окрестности на наличие окрестных запрещенных точек. Если такие точки есть, то они объединяются в массивы областей запрещенных точек.

5. Окружаем (помечаем) все запрещенные области разрешенными точками, т.е. берем каждую найденную запрещенную точку и окружаем ее разрешенными точками так, как показано на (рис. 1.12). Формируем массив разрешенных точек для каждой области запрещенных точек.

Рис. 1.12 Запрещенная точка MZap[2] окружена разрешенными точками 2.1, 2.2, 2.3, 2.4.

На этом этапе подготовка к движению манипулятора закончена.

6. Осуществляем движение из q⁰ в q^T по точкам предварительного маршрута:

1. q⁰ принимается текущей точкой qⁱ = q⁰.

2. движемся по предварительному маршруту, записывая текущие точки в массив искомого пути mRez[i]=qⁱ до столкновения с запрещенной областью или достижения целевой точки q^T.

3. Совершаем обход запрещенной области только по разрешенным точкам этой области.

4. Обход осуществляется из точки прямого пути(где мы “попали в препятствие”) в точку прямого пути (где мы “покинули препятствие”) по двум направлениям (т.к. препятствие можно обойти с двух сторон) из которых выбирается кратчайший или если оба пути равны, то выбирается любой и записывается в mRez[].

5. Возобновляем движение по предварительному маршруту до столкновения со следующей запрещенной областью или достижения целевой точки q^T, т.е. переходим к п.6.2

6. Алгоритм прекращает работу, если текущая конфигурация qⁱ достигла целевой q^T (рис. 1.13).

.6.2м ющей движение по предварительному маршруту.ано нарисапрещенных точек00000000000000000000000000000000000000000000000000000

Рис. 1.13 Система обобщенных координат. Точками 0-5 обозначены точки искомого пути mRez[6].