当前位置：首页 > news >正文

人工势场法（APF）路径规划 MATLAB

news 2025/9/2 12:10:12

2D 平面移动机器人/无人车场景
三连杆机械臂关节空间场景
避障 + 目标吸引可视化
可调 GUI（滑条实时改参数）
常见陷阱（局部极小、振荡）与改进提示

一、2D 平面无人车 APF（精简易读版）

文件：apf_2d.m

function [path,iter] = apf_2d(start,goal,obs,k_att,k_rep,d0,step,maxIter)
% start,goal : [x y]
% obs        : N×3 [x y r]
% k_att,k_rep: 吸引/排斥增益
% d0         : 障碍物影响半径
% step       : 步长
% maxIter    : 最大迭代步
pos = start; path = pos; iter = 0;
while iter < maxIteriter = iter + 1;% 吸引力F_att = k_att*(goal - pos);% 排斥力F_rep = [0 0];for k = 1:size(obs,1)vec = pos - obs(k,1:2);dist = norm(vec) - obs(k,3);     % 到障碍物边缘if dist < d0 && dist>0F_rep = F_rep + k_rep*(1/dist - 1/d0)*(1/dist^2)*vec/norm(vec);endendF = F_att + F_rep;if norm(F) < 1e-3 || norm(pos-goal) < 0.2break;endpos = pos + step * F/norm(F);path = [path; pos];
end
end

调用示例 & 可视化

start = [0 0]; goal = [5 5];
obs   = [3 3 0.8; 2 4 0.6; 4 2 0.5];
[path,iter] = apf_2d(start,goal,obs,1,100,2,0.1,500);
figure; hold on
scatter(path(:,1),path(:,2),'b','filled')
rectangle('Position',[obs(1,1)-obs(1,3) obs(1,2)-obs(1,3) 2*obs(1,3) 2*obs(1,3)],'Curvature',1,'FaceColor',[0.8 0.2 0.2])
rectangle('Position',[obs(2,1)-obs(2,3) obs(2,2)-obs(2,3) 2*obs(2,3) 2*obs(2,3)],'Curvature',1,'FaceColor',[0.8 0.2 0.2])
plot(goal(1),goal(2),'g*','MarkerSize',15)
axis equal, grid on
title(sprintf('APF 路径 (%d 步)',iter))

二、三连杆机械臂关节空间 APF

文件：arm_apf.m（基于博客源码优化）

function q_path = arm_apf(L,q0,q_goal,obs,k_att,k_rep,d0,alpha,lambda,maxIter)
% L       : [L1 L2 L3] 连杆长度
% q0      : 初始关节角 [3×1] (rad)
% q_goal  : 目标关节角 [3×1]
% obs     : [x y r] 障碍物（基座坐标系）
q = q0; q_path = q;
for iter = 1:maxIter% 末端位置x = L(1)*cos(q(1))+L(2)*cos(q(1)+q(2))+L(3)*cos(q(1)+q(2)+q(3));y = L(1)*sin(q(1))+L(2)*sin(q(1)+q(2))+L(3)*sin(q(1)+q(2)+q(3));pe = [x;y];% 吸引力xg = L(1)*cos(q_goal(1))+L(2)*cos(q_goal(1)+q_goal(2))+L(3)*cos(sum(q_goal));yg = L(1)*sin(q_goal(1))+L(2)*sin(q_goal(1)+q_goal(2))+L(3)*sin(sum(q_goal));pg = [xg;yg];F_att = k_att*(pg - pe);% 排斥力F_rep = [0;0];for k = 1:size(obs,1)vec = pe - obs(k,1:2);dist = norm(vec) - obs(k,3);if dist < d0 && dist>0F_rep = F_rep + k_rep*(1/dist - 1/d0)*(1/dist^2)*vec/norm(vec);endendF = F_att + F_rep;% 雅可比 & 伪逆J = zeros(2,3);c1=cos(q(1)); s1=sin(q(1)); c12=cos(q(1)+q(2)); s12=sin(q(1)+q(2));c123=cos(sum(q)); s123=sin(sum(q));J(1,1) = -L(1)*s1-L(2)*s12-L(3)*s123;J(1,2) = -L(2)*s12-L(3)*s123;J(1,3) = -L(3)*s123;J(2,1) =  L(1)*c1+L(2)*c12+L(3)*c123;J(2,2) =  L(2)*c12+L(3)*c123;J(2,3) =  L(3)*c123;dq = alpha * pinv(J'*J + lambda^2*eye(3))*J'*F;q = q + dq;q_path = [q_path,q];if norm(q-q_goal) < 0.02break;end
end
end

调用示例

L = [1 1 0.5]; q0 = [0 0 0]; q_goal = [pi/3 pi/4 -pi/6];
obs = [1.2 1.2 0.2];
q_path = arm_apf(L,q0,q_goal,obs,1,100,0.5,0.05,0.01,200);
figure; plot(q_path','LineWidth',2); grid on
legend('\theta_1','\theta_2','\theta_3'); xlabel('step')

三、GUI 实时调参（App Designer）

UWBGUI.mlapp 核心回调（简写）：

% 滑条回调
function updateSim(app,event)k_att = app.K_attSlider.Value;k_rep = app.K_repSlider.Value;d0    = app.d0Slider.Value;[path,iter] = apf_2d(app.start,app.goal,app.obs,k_att,k_rep,d0,0.1,500);plot(app.UIAxes,path(:,1),path(:,2),'b','LineWidth',2);
end

参考代码人工势场法路径规划 matlab www.youwenfan.com/contentcsf/25960.html

四、常见问题与改进

现象	原因	对策
局部极小	合力≈0	① 随机扰动 ② 虚拟目标 ③ 势场叠加
路径振荡	步长过大	减小 α 或加入阻尼 λ
无法到达	障碍物包围	增加斥力半径 d0 或改 RRT*
机械臂奇异	Jacobian 奇异	伪逆 + 阻尼 λ