人工势场改进避撞

在论坛上搜了一些matlab人工势场法程序，但很多都有一些问题，比如目标不可达，角度计算错误等等。在这些的基础上，我进行了改进，使得能够更好地到达目标点，减少了震荡，但是依旧会出现几次震荡，愿高手继续完善。程序可以用C写出来，做机器人路径规划或者避障的同学，该文档有很高的参考性。

clear all;
%障碍和目标，起始位置都已知的路径规划意图实现从起点可以规划出丿?避开障碍到达目标的路徿

%****************初始化参数*********************************
%APF系数
k=15;%引力系数
m=5;%斥力系数
Po=1;%斥力影响距离
S=0.5;%障碍物安全距离

%起点位置和障碍位罿
%一般情况：受m和Po的影响
Xo=[0 0];%起点位置
Xsum=[10 10;1 2;3 5;4 4.5;3 6;6 2;5.5 5.5;8 8.5];%这个向量n+1）*2维，其中[10 10]是目标位置，剩下的都是障碍的位置?
%局部最小：停滞
% Xo=[5 0];
% Xsum=[5 10;2 5;3 5;4 5;5 5;6 5;7 5;8 5]
%障碍靠近目标：震荡，目标不可达
% Xo=[0 0];
% Xsum=[10 10;8.7 9.2]

n=size（Xsum1）-1;%障碍个数

%其他系数
J=1000;%循环迭代次数
K=0;%用来记录迭代次数
a=0.5;%斥力计算中的一个系数
l=0.1;%步长
pathLength=0;
D=0.005;%结果的精确度

%无人艇位置初始化
Xj=Xo;%j=1循环初始，将车的起始坐标赋给Xj
traj_obs=Xsum（2:）;

%环境绘图
plot（Xsum（2:n+11）Xsum（2:n+12）‘.‘Xsum（11）Xsum（12）‘v‘Xo（1）Xo（2）‘ms‘）;
for i=2:（n+1）
        circle（Xsum（i1）Xsum（i2）S‘‘）;
end
axis（[0 11 0 11]）;