ния координат груза x, z, γ и ω на дискретной равномерной сетке, а для каждого сочетания указанных координат – последовательно все габаритные точки (Rg)ig , ig [1; cг].
Пуск |
1 |
2 |
|
|
Ввод исходных данных: (xн0, yн0, zн0, сн0, ωн0); (xк0, yк0, zк0, γк0, ωк0); imax; jmax; |
|
kmax; lmax; mmax; { R }; [YПР]; u; lзап_г; lзап_в |
|
|
ig |
l= (xк0 – xн0)/imax; |
γmax=γн0+(0,5∙lmax∙Δu); γmin=γн0–(0,5∙lmax∙Δu); |
ωmax=ωн0+(0,5∙mmax∙Δu); ωmin=ωн0–(0,5∙mmax∙Δu).
Построение полидистантной поверхности [YЭ] вокруг реальной поверхности
препятствий [YПР] по методике раздела 3.3 [94, 102]
3
4
|
|
|
|
|
6 |
|
|
|
j=0; k=1 |
5 |
|
16 |
|
|
|
|
|
|
l=1:lmax |
|
|
7 |
|
i=1:imax |
|
|
|||
|
|
|
|
|
|
|
|
|
|
|||||
|
|
|
|
|
|
|
|
17 |
|
|||||
|
|
|
|
|
|
|
|
|
|
|||||
|
γk =(l–0,5∙lmax)∙Δu |
|
|
|
|
|
k=1:kmax |
|
|
|||||
|
|
|
|
|
|
|
|
|
|
|
|
18 |
|
|
|
|
|
|
m=1:mmax |
|
|
8 |
|
|
|
||||
|
|
|
|
|
|
9 |
|
|
l=1:lmax |
|
|
|||
|
|
|
|
|
|
|
|
|
19 |
|
||||
|
ωk =(m–0,5∙mmax)∙Δu |
|
|
10 |
|
|
m=1:mmax |
|
|
|||||
|
|
|
|
|
|
|
|
|
|
|
20 |
|
|
|
|
Задание матрицы (Aτ)k |
|
|
|
ig=1:cг |
21 |
|
|||||||
|
со значениями углов |
|
|
|
|
|
||||||||
|
|
|
|
γk, ωk |
|
|
|
|
|
|
ix= (i∙ |
l+Ms(1, ig, l, m))/ |
l ; |
|
|
|
|
|
|
11 |
|
|
iz= (k∙ |
l+Ms(3, ig, l, m))/ |
l |
|
|||
|
|
|
|
ig=1:cг |
|
|
|
|
22 |
|||||
|
|
|
|
|
|
|
|
|
|
|
|
|||
|
|
|
|
|
12 |
|
|
Коррекция ix, iz по (3.47) |
23 |
|||||
|
|
|
|
|
|
|
|
|||||||
|
|
|
|
|
|
|
|
|
|
|
||||
|
|
|
|
j=j+1 |
|
|
|
|
|
|||||
|
|
|
|
|
|
13 |
|
Yc(ig)=YЭ(ix,iz)–Ms(2,ig,l,m) |
|
|||||
|
|
|
|
|
|
|
|
|
||||||
|
(Rs) |
j |
= (Aτ ) × (Rg) |
, |
|
|
|
|
24 |
|||||
|
|
|
k |
|
ig |
|
|
|
|
Ymin(i,k,l,m)=min(Yc(ig)) |
|
|||
|
j [1; (cг∙lmax∙mmax)] |
|
|
|
|
|
||||||||
|
|
|
14 |
|
|
|
|
|
||||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Ms(ik, ig, l, m)=(Rs)ik |
; |
|
|
Вывод результатов |
25 |
|
|||||||
|
|
|
|
ik [1; 3] |
|
|
|
|
|
|
|
[Ymin] |
|
|
|
|
|
|
|
|
15 |
|
|
|
Останов 26 |
|
|
||
|
|
|
|
k=k+1 |
|
|
|
|
|
|
||||
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Рис. 3.14. Блок-схема алгоритма построения гиперповерхности минимальных значений вертикальных координат условного центра груза для пяти учитываемых обобщенных координат
Используя массив Ms(ik, ig, l, m) линейных смещений габаритных точек в зависимости от значений угловых координат, определяют значения индексов ix и iz линейных координат x и z текущей габаритной точки на равномерной сетке с учетом смещений:
95
ix= (i∙ l+Ms(1, ig, l, m))/ l ; iz= (k∙ l+Ms(3, ig, l, m))/ l . |
(3.46) |
Значения индексов ix и iz, вычисленные по (3.46), проверяются на выполнение условия невыхода за границы диапазонов заданной сетки ix [1; imax]; iz [1; kmax] соответственно и при необходимости корректируются. Условия коррекции выглядят следующим образом:
ìix |
при |
1 |
≤ ix ≤ imax ; |
ìiz |
при |
1 |
≤ iz ≤ kmax ; |
|
ï |
при |
ix |
<1; |
ï |
при |
iz |
<1; |
(3.47) |
ix = í1 |
iz = í1 |
|||||||
ï |
|
|
ix > imax . |
ï |
|
|
iz > kmax . |
|
îimax при |
îkmax при |
|
||||||
По индексам ix и iz, полученным по (3.46), (3.47), для различных габаритных точек груза при одних и тех же значениях координат формируется одномерный вектор Yc высот точки начала координат груза, элементы которого определяются по зависимости
Yc(ig)=YЭ(ix,iz) – Ms(2, ig, l, m). |
(3.48) |
Отдельный элемент массива гиперповерхности [Ymin] определяется как минимальный элемент вектора Yc:
Ymin(i,k,l,m)=min(Yc(ig)), ig [1; cг]. |
(3.49) |
Блок-схема алгоритма построения гиперповерхности минимальных значений вертикальных координат условного центра груза приведена на рис. 3.14.
3.5. Методика дискретной локальной оптимизации заданной траектории в неоднородном организованном трехмерном пространстве
Локальная оптимизация отдельной заданной траектории S может быть выполнена при соблюдении условия непересечения груза с эквидистантной (полидистантной) поверхностью вокруг препятствий [80, 94, 102]. В случае, если имеется некоторая траектория, в общем случае не являющаяся оптимальной, локальная оптимизация позволяет сравнительно быстро достичь положения ближайшего локального оптимума по целевой функции L путем последовательного изменения положения точек траектории sp, p [1; imax]. Поскольку траектории и полидистантная поверхность [YЭ] заданы дискретно на равномерной
96
сетке, предлагается следующий алгоритм дискретной локальной оптимизации отдельной траектории [80].
Последовательно для каждой из точек sp траектории с координатами
sp=(xp, yp, zp, γp, ωp), |
(3.50) |
где p [1; imax]; xp=p∙Δl,
осуществляется дискретная оптимизация точек из интервала p [2; (imax–1)], т.е. точка sp перемещается в новое положение, минимизирующее целевую функцию L. Поскольку значение целевой функции L (3.19) определяется дискретно в виде суммы, при изменении положения одной точки будут меняться значения только двух слагаемых этой суммы, поэтому вместо значения L при оптимизации может быть использовано значение Lp, вычисление которого занимает в (imax/2) меньше времени по сравнению с L:
çæ |
|
|
|
|
|
+÷ö |
|
||||||||||||||||||
(xp - xp−1 )2 + (y p - y p−1 )2 + (z p - z p−1 )2 |
|
||||||||||||||||||||||||
Lp = ç |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
÷ + |
|
|
|
|
|
|
× |
(γ |
|
-γ |
|
)2 |
+ (ω |
|
|
-ω |
|
|
)2 |
|
|
|
||||||
ç+ c |
|
p |
p−1 |
p |
p−1 |
|
|
÷ |
|
||||||||||||||||
è |
|
|
γω |
|
|
|
|
|
|
|
|
|
|
|
|
ø |
|
||||||||
çæ |
|
|
|
+÷ö |
|
||||||||||||||||||||
(xp - xp+1 )2 + (yp - y p+1 )2 + (z p - z p+1 )2 |
|
||||||||||||||||||||||||
+ ç |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
÷. |
(3.51) |
|
ç |
+ c |
γω |
× |
|
(γ |
p |
-γ |
p+1 |
)2 |
+ (ω |
p |
-ω |
p+1 |
)2 |
|
|
÷ |
|
|||||||
è |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ø |
|
||||||||
При использовании в качестве целевой функции СВД линейных и угловых перемещений появляется возможность дополнительного сокращения времени расчетов. Вместо значения Lp при оптимизации может быть использовано значение Lp1 – СВД от точки искомого оптимума sp с учетом препятствий до предварительно определенной точки sp1, находящейся в пространстве обобщенных координат груза на прямой линии между точками sp–1 и sp+1 без учета препятствий.
При отсутствии препятствий между sp–1 и sp+1 точка sp1 с координатами (xp1, yp1, zp1, γp1, ωp1), находящаяся посредине между точками sp–1 и sp+1, будет являться оптимумом при любом значении весового коэффициента cγω в выражении (3.51). Следовательно, точка sp, определенная с учетом препятствий как ближайшая к sp1, будет являться оптимумом при наличии препятствий. При оптимизации положения отдельной точки sp в качестве оптимизируемой функции может быть использовано значение Lp1, вычисление которого занимает еще в 2 раза меньше времени по сравнению с Lp:
97
|
çæ |
|
|
+÷ö |
|
||||||||||
Lp1 |
(xp - xp1 )2 + (yp - yp1 )2 + (z p - z p1 )2 |
|
|||||||||||||
= ç |
|
|
|
|
|
|
|
|
|
|
|
|
÷. |
(3.52) |
|
+ c × |
(γ |
|
-γ |
|
)2 + (ω |
|
-ω |
|
)2 |
|
|||||
|
ç |
p |
p1 |
p |
p1 |
|
÷ |
|
|||||||
|
è |
|
γω |
|
|
|
|
|
|
ø |
|
||||
Это позволяет использовать для оптимизации положения отдельной точки траектории метод полного перебора на ограниченной об- ласти-гиперкубе с центром в исходном положении точки sp.
Описание методики локальной оптимизации для отдельной точки траектории приведено в пп. 1–9 [80].
1. Для текущего значения p определяются оптимальные значения координат (xp1, yp1, zp1, γp1, ωp1) точки sp без учета препятствий, т.е. на прямой между точками sp–1 и sp+1:
xp1=(xp–1+xp+1)/2; yp1=(yp–1+yp+1)/2; zp1=(zp–1+zp+1)/2; |
|
γp1=(γp–1+γp+1)/2; ωp1=(ωp–1+ωp+1)/2. |
(3.53) |
Соответствующие индексы координат на равномерной дискретной сетке определяются следующим образом:
p= xp1/ l ; jp= yp1/ l ; kp= zp1/ l ; |
|
lp= (γp1–γmin)/ u ; mp= (ωp1– ωmin)/ u . |
(3.54) |
2. В случае, если для индексов, полученных по (3.54), выполняется условие нахождения точки sp над гиперповерхностью [Ymin]
yp≥Ymin(p,kp,lp,mp), |
(3.55) |
оптимизация по текущей точке sp завершается, и начинается оптимизация по следующей точке sp+1. В противном случае выполняется следующий п. 3.
3. Используя вложенные циклы по k, l, m, определяющие z, γ, ω соответственно, для оптимизируемой точки sp с фиксированным p рассматриваются всевозможные сочетания координат груза z, γ и ω на дискретной равномерной сетке для ограниченной области-гиперкуба с центром в исходном положении точки sp. Для этого варьируются индексы k, l, m в следующих диапазонах (области-гиперкубе):
k [(kp–dkp); (kp+dkp)]; l [(lp–dlp); (lp+dlp)]; m [(mp–dmp); (mp+dmp)], (3.56)
где kp, lp, mp – индексы, соответствующие координатам zp, γp, ωp точки sp до оптимизации; dkp, dlp, dmp – положительные целочисленные зна-
98
чения приращений индексов k, l, m соответственно, определяющие область гиперкуба и удовлетворяющие условиям
dkp<<kmax; dlp<<lmax; dmp<<mmax. |
(3.57) |
4. Значения индексов k, l, m, варьируемые по (3.56), проверяются на выполнение условия невыхода за границы диапазонов исходной
сетки k [1; kmax]; l [1; lmax]; m [1; mmax] соответственно и при необходимости корректируются. Условия коррекции выглядят следующим
образом:
ìk |
при |
1 ≤ k ≤ kmax ; |
ìl |
при |
1≤ l ≤ lmax ; |
ï |
при |
k <1; |
ï |
при |
l <1; |
k = í1 |
l = í1 |
||||
ï |
|
|
ï |
|
|
îkmax при k > kmax . |
îlmax при l > lmax . |
||||
ìm при 1≤ m ≤ mmax ; |
|
||
ï |
при |
m <1; |
(3.58) |
m = í1 |
|||
ïm |
max |
при m > m . |
|
î |
max |
|
|
5. Для каждого сочетания значений индексов k, l, m в области гиперкуба определяются текущие значения координат yu, zu, γu, ωu оптимизируемой точки sp с учетом препятствий:
ìy |
p |
при |
y |
p |
³ Y |
(p,k,l,m); |
(3.59) |
||
yu = í |
|
|
min |
y |
|
< Y (p,k,l,m); |
|||
Y |
|
(p,k,l,m) |
при |
p |
|
||||
î min |
|
|
|
|
|
min |
|
||
zu=k∙Δl; |
γu=l∙Δu; |
ωu=m∙Δu, |
|
||||||
где u [1; (dkp∙2+dlp∙2+dmp∙2)] – индекс, соответствующий уникальному сочетанию значений индексов k, l, m и координат yu, zu, γu, ωu. Для каждого значения u из приведенного диапазона в массиве [tgu] запоминаются соответствующие значения координат yu, zu, γu, ωu.
6. Для каждого сочетания значений координат yu, zu, γu, ωu в области гиперкуба определяется расстояние (Lp1)u:
|
çæ |
|
|
+÷ö |
|
||||||||||
(Lp1 ) |
(xp - xp1 )2 + (yu - yp1 )2 + (zu - z p1 )2 |
|
|||||||||||||
= ç |
|
|
|
|
|
|
|
|
|
|
|
|
÷. |
(3.60) |
|
+ c × |
(γ |
|
- γ |
|
)2 + (ω |
|
- ω |
|
)2 |
|
|||||
u |
ç |
u |
p1 |
u |
p1 |
|
÷ |
|
|||||||
|
è |
|
γω |
|
|
|
|
|
|
ø |
|
||||
7. По выходе из циклов по k, l, m определяется значение индекса um, соответствующее минимальному значению (Lp1)u:
99