Материал: 2426
Для каждой точки ig [1; cг] выполняется проверка условия превышения ее вертикальной координаты над соответствующей вертикальной координатой полидистантной поверхности препятствий с теми же координатами xig, zig:
yig≥YЭ(xig, zig). (4.191)
Блок-схема алгоритма проверки выполнения условия непересечения подвижных звеньев ГПК и груза с полидистантной поверхностью препятствий [YЭ] приведена на рис. 4.31. Данный алгоритм многократно используется как составная функциональная часть общего алгоритма ВДК. Выходным параметром алгоритма проверки пересечений является переменная Cross, принимающая значения 0 (отсутствие пересечений) и 1 (присутствие пересечений).
Кроме того, по методике раздела 4.4 выполняется проверка сгенерированной в пространстве конфигураций точки p по ограничению на устойчивость. В зависимости от результатов проверки переменная Opr принимает значение 0 (конфигурация устойчива) либо 1 (конфигурация не устойчива).
При выполнении условий (4.170), (4.173) и (4.191), а также ограничения на устойчивость (Opr=0) значение p увеличивается на 1, в противном случае генерация отдельной точки по (4.183), (4.184) повторяется.
Первая (p=1) и последняя (p=ng) точки траектории будут совпадать с начальной и конечной заданными точками:
s1=sнач=(qn7, qn8, qn9, qn10); sng=sкон=(qk7, qk8, qk9, qk10). (4.192)
7.6. Формируется матрица весов дуг N=[Li1,j1]ing1, j1=1. Выполняется
проверка видимости между текущей точкой si1 {Sr} и всеми прочими точками из множества sj1 {Sr}. Для этого используются два вложенных цикла: внешний i1 [1; ng] и внутренний j1 [1; ng]. Для каждого сочетания значений i1 и j1 для промежуточных точек осуществляется проверка выполнения условий (4.170), (4.173) и (4.191) при помощи рекурсивного алгоритма деления отрезка по методике, изложенной в разделе 3.8. При невыполнении данных условий для любой промежуточной точки вес дуги (si1,sj1) принимается равным бесконечно большому значению:
Li1,j1=∞. (4.193)
246
В случае выполнения данных условий вес дуги Li1,j1 вычисляют по выражениям T (4.4), Ae (4.5) или C (4.6) целевой функции.
7.7. Осуществляется поиск кратчайшего пути между двумя вершинами графа (sнач и sкон) при помощи алгоритма Дейкстры [216]. Результатом поиска является оптимальная траектория S с минимальным значением целевой функции, представляющая собой последователь-
ность из нескольких вершин графа дорожной карты Gr: S={sp}snp=1.
7.8. Осуществляется линейная интерполяция найденной траектории с равномерным ее разбиением на nЛ отрезков для последующей локальной оптимизации. Для этого для заданных значений шагов дискретизации всех управляемых координат ( q7, q8, q9, q10) в цикле p [2; sn] по методу «Манхеттен» [74] подсчитывается количе-
ство отрезков на каждой дуге найденной траектории {sp} snp=1, которое сохраняется в векторе no:
no(p–1)=(|q7(p)–q7(p–1)|)/ q7+(|q8(p)–q8(p–1)|)/ q8+ |
|
+(|q9(p)–q9(p–1)|)/ q9+(|q10(p)–q10(p–1)|)/ q10. |
(4.194) |
Подсчитывается коэффициент масштабирования knл как отношение требуемого количества отрезков траектории nЛ к общему для всей найденной траектории количеству отрезков, равному сумме всех элементов вектора no:
knл = nЛ |
sn |
(4.195) |
å no(p). |
||
|
p=1 |
|
Определяется количество отрезков на каждой дуге траектории, в сумме дающих nЛ отрезков:
no(p) = ëno(p)× knл û, p [1; sn]. |
(4.196) |
Осуществляется линейная интерполяция в no(p) промежуточных точках на каждой дуге p [1; sn] траектории. Под интерполяцией подразумевается вычисление значений каждой из управляемых координат груза в промежутках между узловыми точками найденной траектории, которое выполняется по известному алгоритму [65, 191].
7.9.Выполняется дискретная локальная оптимизация интерполированной траектории S по методике, изложенной в разделе 4.5.
7.10.Определяется уточненное значение целевой функции T, Ae или C оптимизированной траектории S по (4.4), (4.5) или (4.6) соответственно.
247
Пуск |
1 |
|
|
|
2 |
Ввод исходных данных: |
|
|
|
||
|
|
|
|
||
sш=(xш0,yш0,zш0)=(q1,q2,q3);sнач=(xн0,yн0,zн0);sкон=(xк0,yк0,zк0);{ R |
};{ R |
};{ R |
io4 |
}; |
|
|
is |
io3 |
|
|
|
{ Rig };[YПР];ng;δq7; uш; u8;nЛ; uл;δopt;lзап_г;lзап_в; v7кпред;v8,1;v8,2;v9,1;v9,2;v9,3;
v9,4;q9гран;mГР;mГРгран;v10,1;v10,2;v10,3;v10,4;q8min;q8max;q9min;q9max;q10min;q10max; q7; q8; q9; q10;m1;m2;m3;m4;x2,2;x3,31;y3,32;x3,33;x4,41;y3,42;y4,43;x2,54;α0;cГ1;cГ2; k7,1, k7,2, k8,1, k8,2, k8,3, k8,4, k9,1, k9,2, k10,1, k10,2
|
q6 |
|
3 |
break=1 |
4 |
5 |
Нет |
|
|
|
|
|
|
|
|
break=0 |
|
|
6 |
||||
q6=0; q6≤360°; |
|
|
|
|
|
|
|
||||
|
|
|
|
Да |
Вывод сообщения |
||||||
q6=q6+ uш |
|
|
|
|
|
||||||
|
|
|
7 |
|
об отсутствии тра- |
||||||
|
|
|
|
|
|
|
|||||
é– cosq6 |
0 |
−sin q6 |
q1 ù |
|
|
|
ектории |
8 |
|||
|
|
|
|
|
|||||||
ê |
0 |
1 |
0 |
|
q2 ú |
|
|
9 |
|
Останов |
|
T1= ê sin q6 |
0 |
cosq6 |
q3 ú |
|
Определение |
|
|
|
|||
ë 0 |
0 |
0 |
|
1 û |
[qn7]; [qn8В; qn8Н]; [qn9В; qn9Н]; [qn10В; qn10Н] – |
||||||
|
|
|
10 |
|
|
для начальной точки; |
|
|
|||
break1=0 |
|
|
[qk7]; [qk8В; qk8Н]; [qk9В; qk9Н]; [qk10В; qk10Н] – |
||||||||
|
|
|
|||||||||
|
|
|
для конечной точки |
|
|
||||||
|
|
|
11 |
|
|
|
|
||||
|
is |
|
|
|
по методике раздела 4.3 |
|
|
||||
|
|
|
|
|
|
12 |
|||||
is=1; is≤cs; |
|
|
|
|
|
|
|
|
|||
is=is+1 |
|
13 |
|
Построение полидистантной поверхности [YЭ] |
|||||||
|
|
вокруг реальной поверхности препятствий |
|||||||||
|
|
|
|
||||||||
R |
= T × R |
|
|
|
[YПР] по методике раздела 3.3 [94, 102] |
||||||
is,0 |
1 |
is |
|
|
|
|
|
|
|
|
|
|
|
|
14 |
Да |
|
iq 8 max |
= ë(q n 8 B − |
q n 8 N ) |
u |
8 û |
15 |
|
|
|
|
|
|||||||
yis0≥YПР(xis0, zis0) |
|
|
|
iq8 |
|
17 |
|
|
|||
Нет |
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
||
|
|
16 |
|
|
|
iq8=1; iq8≤iq8max |
|
|
|
||
break1=1 |
|
|
|
|
|
|
|||||
|
|
|
|
iq8= iq8+1 |
|
19 |
|
||||
|
|
|
|
|
|
|
|
|
|||
|
|
|
18 |
|
|
|
|
|
|
||
|
|
|
|
qn8=qn8N+(iq8–1)∙ u8 |
|
20 |
|||||
|
is |
|
|
|
|
|
|||||
|
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
Формирование матриц A2,A3,A4 по (4.164), (4.165) |
||||||
|
|
|
|
|
|
||||||
break1=0 |
|
21 |
Нет |
|
T3=A1∙A2∙A3; |
T4=T3∙A4 |
22 |
|
|||
|
|
|
|
|
|||||||
|
|
|
|
|
|
|
24 |
|
|
||
Да |
|
|
23 |
|
|
|
Cross(iq8)=0 |
|
|
||
|
|
|
|
|
|
|
|
||||
break=0 |
|
|
|
|
io3 |
25 |
|
|
|||
|
|
|
|
|
|
|
|||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
26 |
|
|
io3=1; io3≤co3; |
|
|
|
||
|
q6 |
|
|
|
|
|
io3=io3+1 |
27 |
|
|
|
|
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
Rio3,0 = T3 × Rio3 |
|
|
|
|
|
|
|
|
|
|
|
1 |
|
|
|
|
Рис. 4.32. Блок-схема модифицированного алгоритма ВДК поиска траектории перемещения груза в пространстве конфигураций ГПК (начало)
248
iq8 |
28 |
|
|
|
|
|
|
|
|
|
1 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
31 |
|
|
|||
iq8=1; iq8≤iq8max |
|
|
|
|
|
|
|
|
|
|
|
|
Да |
|||
iq8= iq8+1 |
29 |
|
|
|
|
|
|
|
yio3≥YЭ(xio3, zio3) |
|
||||||
|
|
|
|
|
|
|
32 |
|||||||||
qk8=qk8N+(iq8–1)∙ u8 |
|
|
|
|
|
|
30 |
|
|
Нет |
|
|
||||
|
|
|
|
|
|
|
|
|
|
Cross(iq8)=1 |
|
|
|
|||
Формирование матриц A2, A3, A4 по (4.164), (4.165) |
|
|
|
|
|
|
|
|||||||||
|
|
|
|
|
|
|
|
|
||||||||
T3=A1∙A2∙A3; |
T4=T3∙A4 |
33 |
|
|
|
|
|
|
|
|
|
|
35 |
|||
|
|
|
|
|
|
|
|
|
io3 |
|
|
|
|
|||
Cross(iq8)=0 |
34 |
|
|
|
|
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
io3 |
36 |
|
|
|
|
|
|
|
|
|
|
io4 |
|
39 |
||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
||
io3=1; io3≤co3; |
|
|
|
|
|
|
|
|
|
|
io4=1; io4≤co4; |
|
|
|
||
io3=io3+1 |
|
|
38 |
|
|
|
|
|
|
|
io4=io4+1 |
|
|
40 |
||
37 |
|
|
Нет |
|
|
|
|
|
|
|
|
|||||
|
|
41 |
|
Rio4,0 = T4 × Rio4 |
|
|
|
|||||||||
Rio3,0 = T3 × Rio3 |
yio3≥YЭ(xio3, zio3) |
|
|
|
|
|
||||||||||
|
|
|
|
|
|
|
|
|||||||||
42 |
Да |
|
|
Cross(iq8)=1 |
|
|
|
45 |
Да |
|||||||
|
|
|
|
|
|
|
|
|
|
yio4≥YЭ(xio4, zio4) |
|
|||||
|
|
|
|
|
43 |
|
|
|
|
|
|
|||||
io3 |
|
io4 |
|
|
|
|
|
|
|
Нет |
|
46 |
||||
|
|
|
|
|
|
|
|
|
|
|
||||||
|
io4=1; io4≤co4; |
|
|
|
|
|
|
Cross(iq8)=1 |
|
|
|
|||||
|
io4=io4+1 |
|
44 |
|
|
|
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
47 |
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
Rio4,0 = T4 × Rio4 |
|
|
|
|
|
|
|
|
|
|
|
||||
|
|
|
|
|
|
|
|
io4 |
|
|
|
|
||||
|
|
|
|
48 |
|
|
|
|
|
|
|
|
|
|
||
|
|
|
|
|
Да |
|
|
|
|
|
|
|
|
|
||
|
yio4≥YЭ(xio4, zio4) |
|
|
|
|
|
|
|
|
51 |
||||||
|
Нет |
|
|
|
49 |
|
|
|
Cross(iq8)=0 |
|
|
Нет |
||||
|
Cross(iq8)=1 |
|
|
|
|
|
|
Cross(iq8–1)=1 |
|
|
|
|||||
|
|
|
|
|
|
50 |
|
|
|
|
Да |
|
|
|
52 |
|
|
|
io4 |
|
|
|
|
|
|
|
qn8Н=q8(iq8);qn9Н=q9(iq8); |
||||||
|
|
|
|
|
|
|
|
|
|
qn10Н=q10(iq8) |
|
|
|
|||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
53 |
Нет |
|
|
|
|
|
|
|
|
54 |
|
|
Cross(iq8)=0 |
|
|
|
|
iq8 |
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|||||
|
Cross(iq8–1)=1 |
|
|
|
|
|
|
|
|
|
|
|
||||
|
|
|
|
|
|
|
55 |
|
|
|
|
|
|
|
|
56 |
qk8Н=q8(iq8);qk9Н=q9(iq8); |
|
|
|
|
|
|
|
|
||||||||
|
qk10Н=q10(iq8) |
|
|
|
|
|
|
dq7=δq7–(|qk7–qn7|/2) |
||||||||
58 |
|
|
|
|
|
|
= ì[(qn7 - dq7 );(qk 7 + dq7 )] |
|
|
|
57 |
|||||
iq8 |
[q |
|
; q |
|
] |
при qn7 £ qk 7; |
||||||||||
|
nд7 |
|
kд7 |
|
í[(q |
n7 |
+ dq |
);(q |
- dq )] |
при q |
n7 |
|
> q |
|||
|
|
|
|
|
|
|
î |
|
7 |
k 7 |
7 |
|
|
k 7 |
||
|
|
|
|
|
|
|
|
|
|
|
2 |
|
|
|
|
|
Рис. 4.32. Блок-схема модифицированного алгоритма ВДК поиска траектории перемещения груза в пространстве конфигураций ГПК (продолжение)
249