人工势场法路径规划c语言程序

#include <stdio.h> #include <math.h> #include<stdlib.h> float k = 1; float m = 15; float w = 1; float step = (float)1; int iter = 2000; float n = 8; float r[8] = {7,5,10,8,6,4,5,5}; float Po[8]; float a = 2; float Goal[2000][2]={0}; float Theta[9]; float Fat[2]; float Fatx; float Faty; float Freall[4]; float Frexsum; float Freysum; float Freaxsum; float Freaysum; float Fsumxj; float Fsumyj; float Theta_Fsum[1500]; float pi = (float)3.1415927; float Xsum[9][2] = {{(float)100,(float)100},{(float)25,(float)30},{(float)48,(float)22},{(float)50,(float)29},{(float)45,(float)52},{(float)57,(float)53}, {(float)80,(float)65},{(float)70,(float)70},{(float)93,(float)97}};//终止点和障碍 float Xj[2] = {0,0};//起点 float ZH_angel(float A_x,float A_y,float B_x,float B_y);//计算角度 float ZH_Fat(float A_x,float A_y,float B_x,float B_y,float k);//计算引力 int main ()//主函数 { float Xsum_x1; float Xsum_y1; float Xj_x1; float Xj_y1; float Xsum_x11; float Xsum_y11; float Po_i1; float Theta_i1; float Fre; float Frea; float Frex; float Frey; float Freax; float Freay; float rre; float rat; int j; float Xnext; float Ynext; int K; int Ki; Po[0] = (r[0])*(float)(5);//障碍影响区域 Po[1] = (r[1])*(float)(5); Po[2] = (r[2])*(float)(5); Po[3] = (r[3])*(float)(5); Po[4] = (r[4])*(float)(5); Po[5] = (r[5])*(float)(5); Po[6] = (r[6])*(float)(5); Po[7] = (r[7])*(float)(5); for (j=0;j<iter;j++) { float Xsum_x; float Xsum_y; float Xj_x; float Xj_y; float Theta_i; int n = 8; int i; float Fat_i; float Fat[2] = {0,0}; Goal[j][0] = Xj[0]; Goal[j][1] = Xj[1]; for (i=0;i<9;i++)//计算角度 { Xsum_x = Xsum[i][0]; Xsum_y = Xsum[i][1]; Xj_x = Xj[0]; Xj_y = Xj[1]; Theta_i = ZH_angel(Xsum_x,Xsum_y,Xj_x,Xj_y); Theta[i] = Theta_i; } for (i=0;i<1;i++)//计算引力 { Xsum_x = Xsum[i][0]; Xsum_y = Xsum[i][1]; Xj_x = Xj[0]; Xj_y = Xj[1]; Theta_i = Theta[i]; Fat_i = ZH_Fat(Xsum_x,Xsum_y,Xj_x,Xj_y,k); Fatx = Fat_i * cos(Theta_i); Faty = Fat_i * sin(Theta_i); Fat[0] = Fatx; Fat[1] = Faty; } Frexsum = 0;//对合力进行初始化 Freysum = 0; Freaxsum = 0; Freaysum = 0; for (i=0;i<8;i++)//计算斥力 { Xsum_x1 = Xsum[i+1][0];//障碍中心点 Xsum_y1 = Xsum[i+1][1]; Xj_x1 = Xj[0];//飞行器当前位置 Xj_y1 = Xj[1]; Xsum_x11 = Xsum[0][0];//终止点 Xsum_y11 = Xsum[0][1]; Po_i1 = Po[i];//斥力影响范围 Theta_i1 = Theta[i+1];//起始点与障碍点之间的角度 rat = sqrt((((float)Xsum_x11-(float)Xj_x1)*((float)Xsum_x11-(float)Xj_x1))+((float)Xsum_y11-(float)Xj_y1)*((float)Xsum_y11-(float)Xj_y1)); rre = sqrt(((float)Xsum_x1-(float)Xj_x1)*((float)Xsum_x1-(float)Xj_x1)+((float)Xsum_y1-(float)Xj_y1)*((float)Xsum_y1-(float)Xj_y1)); if(rre <= Po_i1)//在斥力影响范围内 { Fre = m *((float)(1/rre)-(float)(1/Po_i1))*(float)((1/(rre*rre))) * (float)pow((float)rat,(float)a); Frea = (float)(a/2) * m *(((float)(1/rre)-(float)(1/Po_i1))*((float)(1/rre)-(float)(1/Po_i1)))* (float)pow((float)rat,(float)(a-1)); } else { Fre = 0; Frea = 0; } Frex = (float)Fre*cos(Theta_i1+ pi); Frey = (float)Fre*sin(Theta_i1+pi); Freax = (float)Frea*cos(Theta_i1); Freay = (float)Frea*sin(Theta_i1); Frexsum = Frexsum + Frex; Freysum = Freysum + Frey; Freaxsum = Freaxsum + Freax; Freaysum = Freaysum + Freay; } Fsumxj = Fatx + Frexsum + Freaxsum; Fsumyj = Faty + Freysum + Freaysum; if((Fsumxj==0)&&(Fsumyj==0)) { float Fsum; float Fsumxj; float Fsumyj; Fsum = w * sqrt(Fatx*Fatx+Faty*Faty); Fsumxj=Fsum * cos(Theta[0] + pi/2); Fsumyj=Fsum * sin(Theta[0] + pi/2); } Xnext=Xj[0]+((float)step) * (float)(cos((float)atan((float)(Fsumyj/Fsumxj)))); Ynext=Xj[1]+((float)step) * (float)(sin((float)atan((float)(Fsumyj/Fsumxj)))); Ki=j; if (((Xj[0] - Xsum[0][0])>0.0)&&((Xj[1] - Xsum[0][1])>0.0)) { K=j; break; } Xj[0] = Xnext; Xj[1] = Ynext; } Goal[K][0]=Xsum[0][0]; Goal[K][1]=Xsum[0][1]; int i = 0; FILE *fp; fp=fopen("F:\\APF_test.txt","w"); for (i=0;i<K;i++) { fprintf(fp,"%f,%f\n",Goal[i][0],Goal[i][1]); } fclose(fp); return 0; } float ZH_angel(float A_x,float A_y,float B_x,float B_y) { int sig; float L; float Theta_i; float X; float Y; X = A_x - B_x; Y = A_y - B_y; L = sqrt( X*X + Y*Y); if(Y<0) { sig = -1; } else if(Y=0) { sig = 0; } else { sig = 1; } Theta_i=sig*acos(X/L); return Theta_i; } float ZH_Fat(float A_x,float A_y,float B_x,float B_y,float k) { float Fa; Fa = k*sqrt((B_x-A_x)*(B_x-A_x)+(B_y-A_y)*(B_y-A_y)); return Fa; }