机器人路径规划问题

原理 设:U(X)为总引力场，()att U x 为目的地引力场，()rep U x 为障碍物排斥场；F(X)为总引力，()att F x 为引力，()rep F x 为斥力；,k η是正比例位置增益系数，0,,g X X X 分别代表机器人，目标和障碍物在空间中的位置。

(,)||g g X X X X ρ=-表示机器人与目标之间的距离。

00(,)||X X X X ρ=-为机器人在空间的位置与障碍物之间的距离。

常数0ρ代表障碍物的影响距离，应根据障碍物和目标点的具体情况而定。

引力势场函数为： 21()(,)2
att g U X k X X ρ= 斥力势场函数为：
2000000111(,)()2(,)0
rep X X U X X X X X ηρρρρρρ⎧⎡⎤⎪-≤⎢⎥=⎨⎣⎦⎪>⎩ 总势场函数为：
()()()att rep U X U X U X =+
力函数F(X)是势场函数U(X)的负梯度。

机器人所受的引力为：
()()att g F X k X X =-
斥力为：
00200000111 (,)()(,)(,)0 (,) rep X X F X X X X X X X ηρρρρρρρ⎧⎡⎤-≤⎪⎢⎥=⎨⎣⎦⎪>⎩
合力为：
()()()att rep F X F X F X =+
实验步骤 根据上述原理进行做实验，力求确定主要参数影响距离0ρ，引力参数k ，斥力系数η，以及机器人运动的步长l 。

步骤：
(1) 简历地图，确定机器人目标和障碍的位置，并确定矢量势场模型的矢量初始参数；
(2) 计算机器人到球的距离,计算吸引力矢量；
(3) 计算球场上障碍物对机器人的位置斥力,判断是否需要避障，计算斥力矢量；
(4) 计算引力矢量和斥力矢量的和,并将该和矢量分解到x 和y 轴上，继而确定机器人下一步的位置点；
（5）然后回到步骤（2），直到该位置点为终点。

核心代码：
void find_Attract(double *Yatx,double *Yaty,int h0,int w0)//求引力
{
double angle,r;
r=sqrt((100-h0)*(100-h0)+(1100-w0)*(1100-w0));
angle=acos((1100-w0)/r);
*Yatx=k*r*cos(angle);
*Yaty=k*r*sin(angle);
return;
}
void find_repulsion(BYTE *pImg,int width,int height,double *Yrerx,double *Yrery,int h0,int w0)//求斥力{
BYTE *pCur=pImg;
int h,w;
double Yrer=0.0,sumx=0.0,sumy=0.0,angle,d;
for(h=0;h<height;h++)
for(w=0;w<width;w++)
{
if(*(pCur+h*width+w)==100)
{
d=sqrt((h-h0)*(h-h0)+(w-w0)*(w-w0));
if(d<p)
{
if(w>w0)
angle=acos((w-w0)/d);
else
angle=pi-acos((w0-w)/d);
Yrer=-m*(1/d-1/p)/d/d;
sumx+=Yrer*cos(angle);
sumy+=Yrer*sin(angle);
}
}
}
*Yrerx=sumx;
*Yrery=sumy;
return;
}
void main()
{
BYTE *pImg,*pCur;
int width,height;
pImg=Read8BitBmpFileImg("map.bmp",&width,&height);
pCur=pImg;
int h0=1100,w0=100;
double Yatx=0.0,Yaty=0.0,Yrerx=0.0,Yrery=0.0,Fsumx=0.0,Fsumy=0.0;
double theta;
do{
find_Attract(&Yatx,&Yaty,h0,w0);
find_repulsion(pImg,width,height,&Yrerx,&Yrery,h0,w0);
Fsumx=Yatx+Yrerx;
Fsumy=Yaty+Yrery;
theta=atan(Fsumy/Fsumx);
w0+=l*cos(theta);
h0-=l*sin(theta);
*(pImg+width*h0+w0)=255;
}while(h0!=100 && w0!=1100);
Write8BitImgBmpFile(pImg,width,height,"route.bmp");
}
运动路线。