人工势场法matlab程序

人工势场法（Artificial Potential Field Method）是一种常用的机器人路径规划算法，它通过建立一个虚拟的势场来引导机器人沿着规划路径前进，避开障碍物并到达目标点。以下是一个简单的 MATLAB 代码实现：

% 定义机器人起始点和目标点
start = [0, 0];
goal = [10, 10];

% 定义障碍物的位置和半径
obstacles = [4, 4, 1; 6, 6, 1; 8, 8, 2];

% 定义参数
step = 0.1;  % 步长
delta = 0.1;  % 安全距离
epsilon = 0.1;  % 目标点半径

% 定义势场函数
function [Fatt, Frep] = potential_field(q, goal, obstacles, delta)
    q_goal = goal - q;
    Fatt = 0.5 * q_goal * norm(q_goal);
    Frep = [0, 0];
    for i = 1:size(obstacles, 1)
        q_obs = obstacles(i, 1:2) - q;
        d = norm(q_obs);
        if d <= delta
            Frep = Frep - 10 * (1/d - 1/delta) * (1/d^2) * q_obs;
        end
    end
end

% 初始化机器人位置
q = start;

% 开始循环
while norm(q - goal) > epsilon
    % 计算势场
    [Fatt, Frep] = potential_field(q, goal, obstacles, delta);
    F = Fatt + Frep;
    % 更新机器人位置
    q = q + step * F / norm(F);
    % 绘制机器人和障碍物
    clf;
    hold on;
    viscircles(obstacles(:, 1:2), obstacles(:, 3), 'Color', 'b');
    viscircles(q, delta, 'Color', 'r');
    plot(q(1), q(2), 'go', 'MarkerSize', 10, 'LineWidth', 2);
    plot(start(1), start(2), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
    plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
    axis equal;
    xlim([-1, 11]);
    ylim([-1, 11]);
    drawnow;
end

这个程序使用了 `potential_field` 函数来计算机器人当前位置的势场，包括吸引力和斥力两部分。然后根据势场的合力方向和大小来更新机器人的位置，直到到达目标点。在程序中，我们使用 `viscircles` 函数来绘制圆形障碍物和机器人当前位置的安全距离圆。你可以根据需要修改参数来实现更复杂的路径规划任务。