路径规划_路径规划_小车_

%第一步： %计算机器人与障碍物、目标点的夹角. function Y=compute_angle(X,Xsum,n)%输入参数依次为:机器人坐标向量、目标和障碍物坐标向量、障碍物数目. for i=1:n+1 deltaXi=Xsum(i,1)-X(1) deltaYi=Xsum(i,2)-X(2) ri=sqrt(deltaXi^2+deltaYi^2) if deltaYi>=0 theta=acos(deltaXi/ri) else theta=pi+acos(-deltaXi/ri) end r(i)=ri Z(i)=theta end Y(1)=Z(1) for i=2:n+1 if((Z(i)-Z(1))==0)&(r(i)<r(1)) angle=Z(i)+pi/2 elseif((Z(i)-Z(1))==0)&(r(i)>r(1)) angle=Z(i) elseif((Z(i)-Z(1))>0)&((Z(i)-Z(1))<pi) angle=Z(i)+3/2*pi elseif(Z(i)-Z(1))==pi angle=Z(i)-pi elseif((Z(i)-Z(1))>pi)&((Z(i)-Z(1))<2*pi) angle=Z(i)+pi/2 elseif((Z(i)-Z(1))<0)&((Z(i)-Z(1))>-pi) angle=Z(i)+pi/2 elseif(Z(i)-Z(1))==pi angle=Z(i)+pi elseif((Z(i)-Z(1))<-pi)&((Z(i)-Z(1))>-2*pi) angle=Z(i)+3/2*pi end Y(i)=angle end end %第二步： %引力计算. function [Fatx,Faty]=compute_gravitation(X,Xsum,k,angle) %输入参数依次为:机器人坐标向量、目标和障碍物坐标向量、引力势场常数、Fatt与x轴正方向的夹角. R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路径点和目标的距离平方. r=sqrt(R); Fatx=k*r*cos(angle); Faty=k*r*sin(angle); end %第三步： %斥力计算. function [Frerxx,Freryy,Fataxx,Fatayy]=compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,u,r) %输入参数依次为:机器人坐标向量、目标和障碍物坐标向量、斥力势场常数、Fatt(Frep2)与x轴的夹角、Frep1与x轴的夹角、障碍物数目、障碍物影响范围、大于零的实数、障碍物“膨胀化”半径. Rat=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2; rat=sqrt(Rat); for i=1:n Rrei(i)=(X(1)-Xsum(i+l,1))^2+(X(2)-Xsum(i+1,2))^2; rre(i)=sqrt(Rrei(i)); if rre(i)>Po Frerx(i)=0 Frery(i)=0 Fatax(i)=0 Fatay(i)=0 else Frer(i)=m*(1/(rre(i)-r)-1/Po)*1/((rre(i)-r)^2)*((abs(X(1)-Xsum(1,1)))^u+(abs(X(2)-Xsum(1,2)))^u) Fata(i)=m*u/2*((1/(rre(i)-r)-1/Po)^2)*((abs(X(1)-Xsum(1,1)))^(u-1)+(abs(X(2)-Xsum(1,2)))^(u-1)) Frerx(i)=Frer(i)*cos(angle_re(i)) Frery(i)=Frer(i)*sin(angle_re(i)) Fatax(i)=Fata(i)*cos(angle_at) Fatay(i)=Fata(i)*sin(angle_at) end Frerxx=sum(Frerx) Freryy=sum(Frery) Fataxx=sum(Fatax) Fatayy=sum(Fatay) end end %第四步： %机器人路径规划. clear all clc ep=0.05;%当机器人与目标点的距离小于ep时，退出算法. i=0.4;%障碍物“膨胀化”半径. u=1; %u是大于零的任意实数. Xo=[0 0];%起点位置. k=200;%引力势场常量. K=0;%迭代次数初值. m=50;%斥力势场常量. Po=1;%障碍物影响范围半径，当机器人和障碍物的距离大于这个距离时，斥力为0. l=0.1;%步长. J=300;%循环迭代次数. Xsum=[2.4 0;3 0];%这个向量是(n+1)*2维，其中Xsum(1,1),Xsum(1,2)是目标位置，剩下的都是障碍的位置. n=size(Xsum,1)-1;%障碍物个数. for t=l:n sita=O:pi/30:2*pi; x2=r*cos(sita)+Xsum(t+1,1); y2=r*sin(sita)+Xsum(t+1,2); plot(x2,y2) hold on end for t=1:n sita=0:pi/30:2*pi; x1=Po*cos(sita)+Xsum(t+1,1); y1=Po*sin(sita)+Xsum(t+1,2); plot(x1,y1,'r-.') hold on end Xj=Xo; for j=1:J Goal(j,1)=Xj(1); Goal(j,2)=Xj(2); Theta=compute_angle(Xj,Xsum,n); angle_at=Theta(1); [Fatx,Faty]=compute_gravitation(Xj,Xsum,k,angle_at); if n==0 Frerxx=O; Freryy=0; Fataxx=0; Fatayy=0; else for i=l:n angle_re(i)=Theta(i+1); end [Frerxx,Freryy,Fataxx,Fatayy]=compute_repulsion(Xj,Xsum,m,angle_at,angle_re,n,Po,u,r); end Fsumyj=Faty+Freryy+Fatayy; Fsumxj=Fatx+Frerxx+Fataxx; Position_angle(j)=atan(Fsumyj/Fsumxj); Xnext(1)=Xj(1)+1*cos(Position_angle(j)); Xnext(2)=Xj(2)+1*sin(Position_angle(j)); Xj=Xnext; K=j if((Xj(1)-Xsum(1,1)).^2+(Xj(2)-Xsum(1,2)).^2)<ep^2 break end end X=Goal(:,1); Y=Goal(:,2); x=Xsum(2:n+1,1); y=Xsum(2:n+1,2); plot(x,y,'o',Xsum(1,1),Xsum(1,2),'v',0,0,'ms',X,Y,'*r')