rengong2 师兄的人工势场法避障实验代码

师兄的人工势场法避障实验代码，绝对好用，代码精炼，注释全

function Theta = compute_angle（XXgrrepn）
%X            当前位姿
%Xgr          目标点、障碍点位姿
%repn         障碍数目
%ret          目标点、障碍点与X轴夹角
for i = 1 : repn+1
      deltax = Xgr（i1）-X（1）;
      deltay = Xgr（i2）-X（2）;
    r = sqrt（deltax^2 + deltay^2）; 
    
    if deltax >= 0 & deltay >= 0
    Theta（i） = asin（deltay/r）;
    elseif deltax < 0 & deltay > 0
    Theta（i） = pi-asin（deltay/r）;
    elseif deltax <=0 & deltay <=0
    Theta（i） = pi - asin（deltay/r）;
    elseif deltax>0 &deltay<0
    Theta（i） = 2*pi + asin（deltay/r）;
    
     
     
    end
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件         602  2020-11-27 06:07  rengong2\compute_angle.m
     文件         851  2020-11-27 06:07  rengong2\compute_attract.m
     文件        2485  2020-11-27 06:07  rengong2\compute_repulsion.m
     文件        2085  2020-11-27 06:07  rengong2\main.m