人工势场法（APF）路径规划 MATLAB
- 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/contentcnl/25960.html
四、常见问题与改进
| 现象 | 原因 | 对策 |
|---|---|---|
| 局部极小 | 合力≈0 | ① 随机扰动 ② 虚拟目标 ③ 势场叠加 |
| 路径振荡 | 步长过大 | 减小 α 或加入阻尼 λ |
| 无法到达 | 障碍物包围 | 增加斥力半径 d0 或改 RRT* |
| 机械臂奇异 | Jacobian 奇异 | 伪逆 + 阻尼 λ |