基于人工势场法的移动机器人路径规划MAtlab程序

传统人工势场法程序主程序：clearclcXo=［0 0];％起点位置k=15；％计算引力需要的增益系数m=4；%计算斥力的增益系数，都是自己设定的.Po=2。5；%障碍影响距离，当障碍和车的距离大于这个距离时，斥力为 0，即不受该障碍的影响.也是自己设定.n=7；％障碍个数l=0。2；%步长J=600；％循环迭代次数％假如不能实现预期目标，可能也与初始的增益系数,Po 设置的不合适有关。％end％给出障碍和目标信息Xsum=[10 10;1 1。5；3 2.2；4 4。5；3 6；6 2;5.5 6；8 8.2］；%这个向量是（n+1）＊2 维，其中[10 10]是目标位置，剩下的都是障碍的位置。Xj=Xo；%j=1 循环初始，将车的起始坐标赋给 Xj%＊*＊＊＊***＊＊＊＊＊**初始化结束,开始主体循环＊＊*＊＊*＊***＊＊＊*＊＊＊＊for j=1：J％循环开始Goal（j，1）=Xj(1）；％Goal 是保存车走过的每个点的坐标。刚开始先将起点放进该向量。Goal（j，2）=Xj(2）;％调用计算角度模块Theta=compute_angle(Xj，Xsum，n)；％Theta 是计算出来的车和障碍，和目标之间的与 X轴之间的夹角，统一规定角度为逆时针方向，用这个模块可以计算出来.％调用计算引力模块Angle=Theta（1）；%Theta(1）是车和目标之间的角度，目标对车是引力.angle_at=Theta(1)；％为了后续计算斥力在引力方向的重量赋值给 angle_at［Fatx,Faty］=compute_Attract(Xj，Xsum，k,Angle）;％计算出目标对车的引力在 x，y 方向的两个重量值。for i=1：nangle_re(i)=Theta(i+1)；％计算斥力用的角度，是个向量,因为有 n 个障碍，就有 n 个角度.end%调用计算斥力模块［Yrerxx，Yreryy]=compute_repulsion（Xj，Xsum，m，angle_re，n，Po)；％计算出斥力在 x，y 方向的重量数组。%计算合力和方向,这有问题,应该是数，每个 j 循环的时候合力的大小应该是一个唯一的数,不是数组.应该把斥力的所有重量相加，引力所有重量相加.Fsumyj=Faty+Yreryy;%y 方向的合力Fsumxj=Fatx+Yrerxx；%x 方向的合力Position_angle(j）=atan（Fsumyj/Fsumxj）；％合力与 x 轴方向的夹角向量％计算车的下一步位置if Fsumyj 〈 0 && Fsumxj 〈0 Xnext（1）=Xj（1)-l＊cos（Position_angle(j））； Xnext（2）=Xj（2)-l*sin(Position_angle（j)）;else Xnext（1）=Xj（1）+l*cos(Position_angle（j)); Xnext（2)=Xj（2）+l*sin（Position_angle（j)）；end％保存车的每一个位置在向量中Xj=Xnext;%推断if （(Xj(1）-Xsum（1，1）)>0)＆...