机器人路径规划问题

原理 设: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

for(w=0;w

{

if(*(pCur+h*width+w)==100)

{

d=sqrt((h-h0)*(h-h0)+(w-w0)*(w-w0));

if(d

{

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");

}

运动路线