人工势场算法matlab代码

由于人工势场算法的实现方式较为灵活，不同的问题会有不同的算法实现方法，因此并没有一份通用的人工势场算法的Matlab代码。以下是一个简单的例子代码，以帮助理解人工势场算法的实现过程。

% 假设有一个机器人在二维空间中移动
% 定义机器人的初始位置和目标位置
pos = [1,1];
goal = [10,10];

% 定义障碍物的位置
obs1 = [5,5];
obs2 = [7,2];

% 定义势场函数和导数函数
U_att = @(q) norm(q - goal); % 引力势场函数
U_rep = @(q) 1/norm(q - obs1)^2 + 1/norm(q - obs2)^2; % 斥力势场函数
grad_U_att = @(q) 0.1*(goal - q); % 引力势场的梯度
grad_U_rep = @(q) -2*(q - obs1)/norm(q - obs1)^3 - 2*(q - obs2)/norm(q - obs2)^3; % 斥力势场的梯度

% 定义机器人的运动速度和步长
v_max = 1;
d_q = 0.1;

% 开始迭代
for i = 1:100
    % 计算机器人所处位置的势能和梯度
    U_total = U_att(pos) + U_rep(pos);
    grad_U_total = grad_U_att(pos) + grad_U_rep(pos);
    
    % 计算机器人的运动速度
    v = -grad_U_total/norm(grad_U_total) * min(U_total, v_max);
    
    % 计算机器人的下一个位置
    pos_next = pos + v*d_q;
    
    % 如果下一个位置与障碍物距离太近，则机器人停滞不动
    if norm(pos_next - obs1) < 0.5 || norm(pos_next - obs2) < 0.5
        v = [0,0];
        pos_next = pos;
    end
    
    % 更新机器人位置
    pos = pos_next;
    
    % 如果机器人到达目标位置，则结束迭代
    if norm(pos - goal) < 0.5
        break;
    end
    
    % 绘制机器人和势场
    plot(pos(1), pos(2), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
    hold on;
    plot(goal(1), goal(2), 'gx', 'MarkerSize', 10, 'LineWidth', 2);
    plot(obs1(1), obs1(2), 'rs', 'MarkerSize', 10, 'LineWidth', 2);
    plot(obs2(1), obs2(2), 'rs', 'MarkerSize', 10, 'LineWidth', 2);
    [X,Y] = meshgrid(0:0.1:12);
    Z = U_att([X(:), Y(:)]) + U_rep([X(:), Y(:)]);
    contour(X,Y,reshape(Z,size(X)),10);
    axis equal;
    axis([0,12,0,12]);
    hold off;
    drawnow;
end

上述代码实现了一个简单的二维机器人移动问题。机器人的初始位置为 (1,1)，目标位置为 (10,10)，存在两个障碍物在位置 (5,5) 和 (7,2)。

势场函数中引力项采用欧几里得距离，斥力项采用了一个简单的正比于距离平方的函数。梯度函数则直接根据势场函数的形式计算得到。机器人的运动速度由梯度和势能决定，与机器人当前位置和目标位置有关，同时也受到了一个最大速度限制。

在每次迭代中，计算机器人当前位置的梯度和总势能，更新机器人的位置和速度，如果机器人遇到了障碍物，则停滞不动。绘制机器人和势场的过程中，采用了 Matlab 中的 contour 函数来绘制势场等高线。