人工势场法Matlab

人工势场法是一种常用的路径规划算法，它利用虚拟势场的概念，将机器人看作一个质点，在二维或三维空间中运动，通过计算机计算虚拟势场，使机器人自动在空间中寻找有效路径，避开障碍物，最终到达目标点。

以下是一个简单的人工势场法的Matlab代码示例：

% 设置起点和终点
start = [0, 0];
goal = [10, 10];

% 设置障碍物
obstacle1 = [4, 4, 2];
obstacle2 = [6, 6, 1];

% 设置常数
k_att = 0.5; % 引力常数
k_rep = 10; % 斥力常数
d_rep = 1; % 斥力作用范围

% 计算机器人当前位置
robot = [2, 2];

% 计算机器人受到的引力和斥力
F_att = -k_att * (robot - goal);
F_rep = [0, 0];
if norm(robot - obstacle1(1:2)) < d_rep
    F_rep = F_rep + k_rep * (1/norm(robot - obstacle1(1:2)) - 1/d_rep) * ((robot - obstacle1(1:2))/norm(robot - obstacle1(1:2)));
end
if norm(robot - obstacle2(1:2)) < d_rep
    F_rep = F_rep + k_rep * (1/norm(robot - obstacle2(1:2)) - 1/d_rep) * ((robot - obstacle2(1:2))/norm(robot - obstacle2(1:2)));
end

% 计算机器人受到的合力
F = F_att + F_rep;

% 更新机器人位置
robot = robot + F;

% 可视化结果
figure;
hold on;
plot(start(1), start(2), 'ro', 'MarkerSize', 10);
plot(goal(1), goal(2), 'rx', 'MarkerSize', 10);
rectangle('Position',[obstacle1(1)-obstacle1(3)/2,obstacle1(2)-obstacle1(3)/2,obstacle1(3),obstacle1(3)],'Curvature',[1,1],'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.2 0.2 0.2]);
rectangle('Position',[obstacle2(1)-obstacle2(3)/2,obstacle2(2)-obstacle2(3)/2,obstacle2(3),obstacle2(3)],'Curvature',[1,1],'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.2 0.2 0.2]);
plot(robot(1), robot(2), 'bo', 'MarkerSize', 10);
axis equal;

该代码模拟了机器人在二维空间中运动，起点为(0,0)，终点为(10,10)，障碍物为两个圆形，机器人初始位置为(2,2)。代码运行后，机器人会自动从起点向终点移动，避开障碍物。运行结果如下图所示：