人工势场法matlab程序

人工势场法（Artificial Potential Field Method）是一种基于势能场的路径规划方法，可以用于机器人的自主导航、避障等任务。下面是一个基于Matlab的简单的人工势场法程序。

程序思路：
1. 初始化机器人位置、目标位置、障碍物位置等参数；
2. 根据机器人与目标之间的距离计算引力，根据机器人与障碍物之间的距离计算斥力；
3. 将引力与斥力叠加，得到总势能场；
4. 计算机器人在总势能场中的梯度，即机器人应该移动的方向；
5. 根据梯度大小和设定的步长，更新机器人位置；
6. 重复2-5步直到机器人到达目标点或者迭代次数达到上限。

程序代码如下：

clear;clc;

% 初始化参数
start = [0,0]; % 机器人起点
goal = [10,10]; % 目标点
obstacle1 = [3,3]; % 障碍物1
obstacle2 = [7,7]; % 障碍物2
k_att = 1; % 引力系数
k_rep = 100; % 斥力系数
max_iter = 1000; % 最大迭代次数
step_size = 0.1; % 步长

% 迭代计算
for i = 1:max_iter
    % 计算机器人与目标的距离和方向
    distance = norm(goal - start);
    direction = (goal - start) / distance;
    
    % 计算机器人与障碍物1的距离和方向
    distance_ob1 = norm(obstacle1 - start);
    direction_ob1 = (start - obstacle1) / distance_ob1;
    
    % 计算机器人与障碍物2的距离和方向
    distance_ob2 = norm(obstacle2 - start);
    direction_ob2 = (start - obstacle2) / distance_ob2;
    
    % 计算引力和斥力
    F_att = k_att * direction;
    F_rep1 = k_rep * (1/distance_ob1 - 1/10) / (distance_ob1^2) * direction_ob1;
    F_rep2 = k_rep * (1/distance_ob2 - 1/10) / (distance_ob2^2) * direction_ob2;
    F_rep = F_rep1 + F_rep2;
    
    % 叠加引力和斥力，得到总势能场
    F = F_att + F_rep;
    
    % 计算机器人的梯度
    gradient = F / norm(F);
    
    % 更新机器人位置
    start = start + step_size * gradient;
    
    % 判断是否到达目标点
    if norm(start - goal) < 0.1
        disp('Reach the goal!');
        break;
    end
end

% 输出最终结果
if i == max_iter
    disp('Cannot reach the goal!');
else
    disp(['Reach the goal in ', num2str(i), ' iterations.']);
    plot([start(1), goal(1)], [start(2), goal(2)], 'g', 'LineWidth', 2);
end

% 画图
hold on;
plot(start(1), start(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
plot(goal(1), goal(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(obstacle1(1), obstacle1(2), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k');
plot(obstacle2(1), obstacle2(2), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'k');
axis equal;

其中，程序中的k_att和k_rep参数可以根据实际情况进行调整，调整这两个参数可以使机器人更快地到达目标点，但是可能会有更多的碰撞风险。