人工势场法matlab代码

人工势场法（Artificial Potential Field，APF）是一种基于规划路径的机器人控制方法。它利用人工势场的概念，将机器人视为一个质点，施加在机器人上的势场有两个部分：吸引力场和斥力场。吸引力场使机器人朝目标点移动，而斥力场则使机器人避开障碍物。这种方法简单易实现，常用于机器人避障、导航等场景。

以下是一个简单的APF算法的MATLAB代码示例，其中机器人的位置为(x,y)，目标点的位置为(tx,ty)，机器人受到的总势场为F_total，k_att和k_rep分别是吸引力场和斥力场的系数，d_safe是障碍物的安全距离，obstacles是包含所有障碍物位置的矩阵。

function APF()
%机器人初始位置
x=0;
y=0;
%目标点位置
tx=10;
ty=10;
%斥力场系数
k_rep=100;
%吸引力场系数
k_att=0.1;
%障碍物安全距离
d_safe=2;
%障碍物位置
obstacles=[3,3;4,4];
%循环次数
for i=1:100
    %计算机器人到目标点的距离和方向
    d=sqrt((tx-x)^2+(ty-y)^2);
    dx=(tx-x)/d;
    dy=(ty-y)/d;
    %计算吸引力场和斥力场
    F_att=k_att*d;
    F_rep=zeros(1,2);
    for j=1:size(obstacles,1)
        d_obstacle=sqrt((obstacles(j,1)-x)^2+(obstacles(j,2)-y)^2);
        if d_obstacle<d_safe
            dx_obstacle=(x-obstacles(j,1))/d_obstacle;
            dy_obstacle=(y-obstacles(j,2))/d_obstacle;
            F_rep=F_rep+k_rep*(1/d_obstacle-1/d_safe)*(1/d_obstacle^2)*[dx_obstacle,dy_obstacle];
        end
    end
    %总势场
    F_total=F_att+F_rep;
    %机器人移动
    x=x+F_total(1);
    y=y+F_total(2);
    %绘制机器人和障碍物位置
    clf;
    hold on;
    plot(x,y,'ro');
    plot(tx,ty,'gx');
    plot(obstacles(:,1),obstacles(:,2),'bs');
    axis([-2,12,-2,12]);
    pause(0.1);
end
end

在示例中，机器人以红色圆点表示，目标点以绿色×表示，障碍物以蓝色方块表示。每次迭代，机器人朝着目标点移动，并避开障碍物，直到到达目标点。

需要注意的是，APF方法并不保证全局最优解，有可能会陷入局部最优解或者无法到达目标点。因此，在实际应用中需要根据场景情况进行调整和改进。