. 6 , . , , : , . .
, :
,
( ) ;
.
, 
, .
. : . :
,
;
.
, 
, , 
.
, 37%. , , .
, .
, , , .
n- .
, . , .
.
( 05-08-01199-, ..).
1. . ( ): / . ., . ., . . .. 2- , . .: , 2001. 319.
|
|
|
2. . . . . : . .: -, 2003.- 188.
3. .., .. (2000) . : . . : - , 2000.
4. .. (2004) []: / .. , .. . - 2- ., . . : - , 2004.- 480: ().
5. .. . .- : 1995.-334.
6. .., .. : . . . / .. , .. .- : , 2002.-124. ().
7. .. . . . . 2004 ( ).
8. . : . . .: ,1988. 334 ., .
9. . . . .- .: , 1973.
10. Barraquand J., Latombe J.-C. Robot Motion Planning: A Distributed Representation Approach // Int. J. of Rob. Res., Vol. 10, No. 6, December 1991. Pp. 628649.
11. S.M.LaValle, Planning Algorithms, 1999-2003. http://msl.cs.uiuc.edu/planning.
12. Lopatin . . Algorithm of a manipulator movement amidst unknown obstacles. Proceedings of the 10th International Conference on Advanced Robotics (ICAR 2001), August 22-25, 2001, Hotel Mercure Buda, Budapest, Hungary. pp. 327-331.
13. N.J.Nilsson, Problem-Solving Methods in Artificial Intelligence. McGraw-Hill Book Company, New York, 1971.
14.
1
..
() . (. 1). q 0=(1.57;0.8)() q T=(3.2;0.8)(). xOy. 60 . : O1, O2 O3. , , :
1) :
(1)
2) :
(2)
3) 
:
(3)
2 , , :
(1.1.3)
(4)
( ).
(5)
. 1 . 1,2,3. q 0=(1.57;0.8)(), q T=(3.2;0.8)().
. . , . delta qi, i=1,2, :
|
|
|
delta = (0+6.28)/ (6)
i- , i- (i=1,2) . delta (. 1).
1
| delta, ˚ | , | ||
| 4.5 | |||
| 1.5 |
Microsoft Visual C++ 6.0 Intel Pentium Celeron 1.3 GHz. , , delta.
2
.
//------------------------------------------------------------------------------
/*
* Finish_optimized.cpp
*
* .
*
* : q0 -
* qT -
* mZap[] -
* z -
* border -
* delta -
* qL qH -
*
*
* : mRez[] -
* n -
*/
//------------------------------------------------------------------------------
#include<iostream.h>
#include <except.h>
#include<stdlib.h>
#include<math.h>
#define NUM 2000
#include<systdate.h>
//------------------------------------------------------------------------------
// .
//------------------------------------------------------------------------------
Word MillisecondsBetween (TDateTime a,TDateTime b)
{
Word A1,B1,C1,D1,E1;
Word A2,B2,C2,D2,E2;
DecodeTime(a,A2,B2,C2,D2);
DecodeTime(b,A1,B1,C1,D1);
E1=D1+C1*1000+B1*60*1000+A1*60*60*1000;
E2=D2+C2*1000+B2*60*1000+A2*60*60*1000;
return E2-E1;
}
TDateTime T1,T2;
class point
{
public:
int x;
int y;
public:
float Distance(point a,point b);
float Normal(point t1,point t2,point t);
point Best_point(point qi,point qT,int delta);
void Liberation(point **data, int m);
void Route(point q0,point qT,
point mZap[],int z,int border,
point *mRez,int &n,
int delta,
point qL,point qH,Word Time);
};
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
float point:: Distance(point a,point b)
{
return sqrt(pow(b.x-a.x,2)+pow(b.y-a.y,2));
}
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
|
|
|
float point:: Normal(point t1,point t2,point t)
{ // t1 -
// t2 -
int A = t2.y - t1.y; // t -
int B = t1.x - t2.x;
int C = -t1.x*A - t1.y*B; // Ax+By+C=0
float M = 1/sqrt(pow(A,2)+pow(B,2));
(C <= 0)? M:-M;
return A*M*t.x + B*M*t.y + C*M;
}
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
point point:: Best_point(point qi,point qT,int delta)
{
point best; // best -
float p=0; // p -
float d=0; // d -
float sum=NUM; // sum = p + d
point neighbor[8]; //
neighbor[0].x = qi.x - delta; // [0] [1] [2]
neighbor[0].y = qi.y + delta;
// [3] qi [4]
neighbor[1].x = qi.x;
neighbor[1].y = qi.y + delta; // [5] [6] [7]
neighbor[2].x = qi.x + delta;
neighbor[2].y = qi.y + delta;
neighbor[3].x = qi.x - delta;
neighbor[3].y = qi.y;
neighbor[4].x = qi.x + delta;
neighbor[4].y = qi.y;
neighbor[5].x = qi.x - delta;
neighbor[5].y = qi.y - delta;
neighbor[6].x = qi.x;
neighbor[6].y = qi.y - delta;
neighbor[7].x = qi.x + delta;
neighbor[7].y = qi.y - delta;
for (int i=0;i<8;i++)
{
d=Distance(neighbor[i],qT); // d > 0
p=Normal(qi,qT,neighbor[i]); // p > < 0
if (fabs(p)+d < sum)
{
best = neighbor[i];
sum = fabs(p)+d;
}
}
return best;
}
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void point:: Liberation (point **data, int e)
{
for (int i = 0; i < e; i++)
delete[] data[i];
delete[] data;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
void point:: Route(point q0, point qT, point mZap[], int z, int border,
point *mRez, int &n, int delta, point qL, point qH,Word Time)
{
T1=Now();
int i=0; // for
int j=0;
int k=0;
int v=0;
int u=0;
int r=0;
// int f=0;
// int g=0;
// int w=0;
bool flag = false;
bool Flag = false;
n=0;
//------------------------------------------------------------------------------
// 1. mPr[]
//------------------------------------------------------------------------------
point qi;
qi=q0;
int s=1; // s -
point *mPr;
mPr = new point [(int)Distance(q0,qT)+10];
if(mPr == NULL)
{
cout << " ";
exit(7);
}
mPr[0]=q0; // mPr[] -
while (qi.x!= qT.x || qi.y!= qT.y) //
{ //
qi=Best_point(qi,qT,delta);
mPr[s++] = qi;
|
|
|
}
//------------------------------------------------------------------------------
// 2.
//
//------------------------------------------------------------------------------
int temp=0;
int a=0; // a -
int aa=0; // a!= aa,
point mTemp[30]; // mTemp[] - mCol[] mEnd[]
point mCol[30]; // mCol[] -
point mEnd[30]; // mEndE[] -
for (i=1;i<s;i++)
for (j=border;j<z;j++)
if (mPr[i].x == mZap[j].x && mPr[i].y == mZap[j].y)
mTemp[temp++]=mPr[i];
for (i=1;i<s;i++)
for (j=0;j<temp;j++)
{
if(mPr[i].x == mTemp[j].x && mPr[i].y == mTemp[j].y &&
(mPr[i-1].x!= mTemp[j-1].x || mPr[i-1].y!= mTemp[j-1].y))
mCol[a++] = mPr[i];
if(mPr[i].x == mTemp[j].x && mPr[i].y == mTemp[j].y &&
(mPr[i+1].x!= mTemp[j+1].x || mPr[i+1].y!= mTemp[j+1].y))
mEnd[aa++] = mPr[i+1];
}
if (a!= 0)
{
//------------------------------------------------------------------------------
// 3.
//------------------------------------------------------------------------------
point qj; // qj -
point **zap; // zap[i][j] -
// i -
zap = new point* [a]; // j -
if(zap == NULL)
{
cout << " ";
exit(1);
}
for(i=0;i<a;i++)
{
zap[i] = new point[(z - border)*2]; // *2
if(zap[i] == NULL)
{
cout << " ";
exit(2);
}
}
for(i=0;i<a;i++)
{
zap[i][0] = mCol[i];
}
//------------------------------------------------------------------------------
int b=1; // b -
int *B; // B[i] -
B = new int [(z - border)*2]; // i -
if(B == NULL)
{
cout << " ";
exit(6);
}
point neighborZap[8]; // ()
flag = false;
for (i=0;i<a;i++)
{
for (v=0;v<b;v++)
{
qj = zap[i][v];
neighborZap[0].x = qj.x - delta; // [0] [1] [2]
neighborZap[0].y = qj.y + delta;
// [3] qj [4]
neighborZap[1].x = qj.x;
neighborZap[1].y = qj.y + delta; // [5] [6] [7]
neighborZap[2].x = qj.x + delta;
neighborZap[2].y = qj.y + delta;
neighborZap[3].x = qj.x - delta;
neighborZap[3].y = qj.y;
neighborZap[4].x = qj.x + delta;
neighborZap[4].y = qj.y;
neighborZap[5].x = qj.x - delta;
neighborZap[5].y = qj.y - delta;
neighborZap[6].x = qj.x;
neighborZap[6].y = qj.y - delta;
neighborZap[7].x = qj.x + delta;
neighborZap[7].y = qj.y - delta;
for (j=0;j<8;j++)
for (k=border;k<z;k++)
{
if (neighborZap[j].x == mZap[k].x && neighborZap[j].y == mZap[k].y)
{
for (u=0;u<b;u++)
if (mZap[k].x == zap[i][u].x && mZap[k].y == zap[i][u].y)
{
flag = true;
break;
}
if (flag == false)
zap[i][b++]=mZap[k];
flag = false;
}
}
}
B[i]=b;
}
// ,
//------------------------------------------------------------------------------
for (i=0;i<a;i++)
for (j=0;j<B[i];j++)
for (k=i+1;k<a;k++)
if (zap[i][j].x == mCol[k].x && zap[i][j].y == mCol[k].y)
{
mEnd[i]=mEnd[k];
}
//------------------------------------------------------------------------------
// 4.
//
//------------------------------------------------------------------------------
point **raz;
raz = new point* [a];
if(raz == NULL)
{
cout << " ";
exit(3);
}
for(i=0;i<a;i++)
{
raz[i] = new point [(z - border)*4];
if(raz[i] == NULL)
{
cout << " ";
exit(4);
}
}
for(i=0;i<a;i++)
{
raz[i][0] = mEnd[i];
}
//------------------------------------------------------------------------------
int c=1; // c -
|
|
|
int *C; // C[i] -
C = new int [(z - border)*4]; // i -
if(C == NULL)
{
cout << " ";
exit(5);
}
point qk;
flag = false;
point neighborRaz[4];
for (i=0;i<a;i++)
{
for (j=0;j<B[i];j++)
{
qk = zap[i][j];
neighborRaz[0].x = qk.x; //... [0]...
neighborRaz[0].y = qk.y + delta;
// [1] qk [2]
neighborRaz[1].x = qk.x - delta;
neighborRaz[1].y = qk.y; //... [3]...
neighborRaz[2].x = qk.x + delta;
neighborRaz[2].y = qk.y;
neighborRaz[3].x = qk.x;
neighborRaz[3].y = qk.y - delta;
for (k=0;k<4;k++)
{
for (v=0;v<z;v++)
if (neighborRaz[k].x == mZap[v].x && neighborRaz[k].y == mZap[v].y)
{
flag = true;
break;
}
for (u=0;u<c;u++)
if (raz[i][u].x == neighborRaz[k].x && raz[i][u].y == neighborRaz[k].y)
{
flag = true;
break;
}
if (flag == false)
raz[i][c++]=neighborRaz[k];
flag = false;
}
}
C[i]=c;
}
//
//------------------------------------------------------------------------------
for (i=0;i<a;i++)
for (j=0;j<B[i];j++)
{
if (raz[i][j].x == qT.x && raz[i][j].y == qT.y)
mEnd[i]=qT;
}
//------------------------------------------------------------------------------
// 5.
// - mRez[]
//------------------------------------------------------------------------------
int number=0; // number - M[L]
int H,h; // H - m
point **m; // h - m
point qim; // m -
// qim -
bool T[NUM]; // T[i] = true, i- mEnd[j]
// T[i] = false, i-
int M[NUM]; // M[i] -
M[0]=0;
int L; // L - m
point neighbor[8]; // neighbor[] -
qi=q0;
for (i=0;i<s;i++)
{
qi= mPr[i];
flag = false;
for (j=0;j<a;j++)
if (qi.x == mCol[j].x && qi.y == mCol[j].y)
{
flag = true;
break;
}
if (flag == true)
{
m = new point* [C[j]*100];
if(m == NULL)
{
cout << " ";
exit(8);
}
H=0;
h=0;
L=0;
for (r=0;r<NUM;r++)
{
T[r]=true;
}
qi=mPr[i-1];
m[H] = new point [C[j]]; // m[0] = new point [C[j]];
if(m[H] == NULL)
{
cout << " ";
exit(9);
}
m[H][h] = qi; // m[0][0] = qi;
qim=qi;
while (1)
{
while (qim.x!= mEnd[j].x || qim.y!= mEnd[j].y)
{
neighbor[0].x = qim.x - delta; // [0] [1] [2]
neighbor[0].y = qim.y + delta;
// [3] qim [4]
neighbor[1].x = qim.x;
neighbor[1].y = qim.y + delta; // [5] [6] [7]
neighbor[2].x = qim.x + delta;
neighbor[2].y = qim.y + delta;
neighbor[3].x = qim.x - delta;
neighbor[3].y = qim.y;
neighbor[4].x = qim.x + delta;
neighbor[4].y = qim.y;
neighbor[5].x = qim.x - delta;
neighbor[5].y = qim.y - delta;
neighbor[6].x = qim.x;
neighbor[6].y = qim.y - delta;
neighbor[7].x = qim.x + delta;
neighbor[7].y = qim.y - delta;
// ?
//------------------------------------------------------------------------------
flag = false;
for (v=0;v<8;v++)
{
for (u=0;u<C[j];u++)
{
if (neighbor[v].x == raz[j][u].x && neighbor[v].y == raz[j][u].y)
flag=true;
for (k=0;k<=M[H];k++)
if (neighbor[v].x == m[H][k].x && neighbor[v].y == m[H][k].y)
{
flag=false;
break;
}
if (flag == true) break;
}
if (flag == true) break;
}
//
//------------------------------------------------------------------------------
if (flag == true)
{
Flag = false;
flag = false;
h++;
for (v=0;v<8;v++)
for (u=0;u<C[j];u++)
{
if (neighbor[v].x == raz[j][u].x && neighbor[v].y == raz[j][u].y)
Flag = true;
for (k=0;k<=M[H];k++)
if (neighbor[v].x == m[H][k].x && neighbor[v].y == m[H][k].y)
{
Flag = false;
break;
}
if (Flag == true)
{
if (flag == true)
{
M[++L]=h;
m[L] = new point [C[j]];
if(m[H] == NULL)
{
cout << " ";
exit(10);
}
for (r=0;r<h;r++)
m[L][r]=m[H][r];
m[L][h] = raz[j][u];
}
if (flag == false)
{
M[H]=h;
m[H][h]=raz[j][u];
qim=raz[j][u];
flag = true;
}
Flag=false;
}
}
}
else
{
T[H]=false;
break;
}
}
if (H < L)
{
H++;
h=M[H];
qim=m[H][h];
}
else break;
}
// M[L]
//------------------------------------------------------------------------------
int min = 9999;
for (k=0;k<=L;k++)
if (T[k]==true && M[k] < min)
{
number = k;
min = M[k];
}
if (min!= 9999)
{
for (k=1;k<=M[number];k++)
mRez[n++]=m[number][k];
}
qi=mEnd[j];
Liberation(m,L+1);
for (k=0;k<s;k++)
if (qi.x == mPr[k].x && qi.y == mPr[k].y) i=k;
} //if close
else
{
mRez[n++] = mPr[i];
qi = mPr[i];
}
} // for s close
Liberation(zap,a);
Liberation(raz,a);
delete [] mPr;
delete [] B;
delete [] C;
}
else
for(i=0;i<s;i++)
mRez[n++] = mPr[i];
T2=Now();
Time=MillisecondsBetween (T1,T2);
}
//------------------------------------------------------------------------------
[1]
