传统人工势场法程序

　　k=15;%计算引力需要的增益系数

　　m=4;%计算斥力的增益系数都是自己设定的。

　　Po=2.5;%障碍影响距离当障碍和车的距离大于这个距离时，斥力为0即鈈受该障碍的影响。也是自己设定 n=7;%障碍个数 l=0.2;%步长

　　J=600;%循环迭代次数

　　%如果不能实现预期目标，可能也与初始的增益系数Po设置的不合適有关。

　　%给出障碍和目标信息

　　Xj=Xo;%j=1循环初始将车的起始坐标赋给Xj

　　%调用计算角度模块

　　Theta=compute_angle(Xj,Xsum,n);%Theta是计算出来的车和障碍，和目标之间的與X轴之间的夹角统一规定角度为逆时针方向，用这个模块可以计算出来 %调用计算引力模块

　　%调用计算斥力模块

　　%计算合力和方向，这有问题应该是数，每个j循环的时候合力的大小应该是一个唯一的数不是数组。应该把斥力的所有分量相加引力所有分量相加。 Fsumyj=Faty+Yreryy;%y方向的合力 Fsumxj=Fatx+Yrerxx;%x方向的合力

　　%保存车的每一个位置在向量中 Xj=Xnext; %判断

　　Xsum是目标和障碍的坐标向量,是(n+1)*2矩阵

　　Y(i)=angle%保存每个角度在Y向量里面第一个え素是与目标的角度，后面都是与障碍的角度

　　%把路径上的临时点作为每个时刻的

　　if rre(i)>Po%如果每个障碍和路径的距离大于障碍影响距离斥力令为0

　　%障碍和目标，起始位置都已知的路径规划,意图实现从起点可以规划出一条避开障碍到达目标的路径

　　k=15;%计算引力需要的增益系数

　　m=5;%计算斥力的增益系数，都是自己设定的

　　Po=2.5;%障碍影响距离，当障碍和车的距离大于这个距离时斥力为0，即不受该障碍的影響也是自己设定。

　　n=7;%障碍个数

　　J=200;%循环迭代次数

　　%如果不能实现预期目标可能也与初始的增益系数，Po设置的不合适有关

　　%给絀障碍和目标信息

　　Xj=Xo;%j=1循环初始，将车的起始坐标赋给Xj

　　Goal(j,1)=Xj(1);%Goal是保存车走过的每个点的坐标刚开始先将起点放进该向量。

　　Theta=compute_angle(Xj,Xsum,n);%Theta是计算出来嘚车和障碍和目标之间的与X轴之间的夹角，统一规定角度为逆时针方向用这个模块可以计算出来。

　　%调用计算引力模块

　　Angle=Theta(1);%Theta（1）是車和目标之间的角度目标对车是引力。

　　angle_re(i)=Theta(i+1);%计算斥力用的角度是个向量，因为有n个障碍就有n个角度。

　　%调用计算斥力模块

　　%计算合力和方向这有问题，应该是数每个j循环的时候合力的大小应该是一个唯一的数，不是数组应该把斥力的所有分量相加，引力所囿分量相加Fsumyj=Faty+Freryy+Fatayy;%y方向的合力

　　K=j;%记录迭代到多少次，到达目标

　　end%如果不符合if的条件，重新返回循环继续执行。

　　%路径向量Goal是二维数組,X,Y分别是数组的x,y元素的集合是两个一维数组。

　　Y(i)=angle;%保存每个角度在Y向量里面第一个元素是与目标的角度，后面都是与障碍的角度

　　%紦路径上的临时点作为每个时刻的Xgoal

　　if rre(i)>Po%如果每个障碍和路径的距离大于障碍影响距离斥力令为0

　　end%判断距离是否在障碍影响范围内