直接上干货，今天拆两个版本的人工势场法Matlab实现。先看经典版，再来个魔改版，代码里埋了不少细节彩蛋

人工势场法路径规划matlab代码下图为人工势场法人工势场法改进。 内容包括两种方法编写人工势场法注释挺详细的还有文档对公式方法进行解释。经典版的核心就三块目标点引力、障碍物斥力、合力控制。咱先看引力计算部分% 引力计算二次函数 function F_att attractive_force(q, q_goal, k_att) r norm(q - q_goal); if r 2 % 引力场作用范围阈值 F_att k_att * (q_goal - q); else F_att 2 * k_att * (q_goal - q); % 近目标时增强引力 end end这里有个小trick——当距离目标点小于2米时系数翻倍。实测发现这样能有效防止在目标点附近震荡类似汽车快到终点时轻踩油门。斥力函数就有点讲究了这里用指数衰减% 斥力计算指数型 function F_rep repulsive_force(q, obstacle, k_rep, d0) d norm(q - obstacle(1:2)); if d d0 F_rep [0; 0]; else decay exp(-d/d0*3); % 指数衰减因子 F_rep k_rep * (1/d - 1/d0) * decay * (q - obstacle(1:2))/d^3; end end注意第7行的d^3这个三次方项让斥力方向更合理。之前试过直接用d平方结果机器人在障碍物侧面容易鬼畜走位。改进版主要解决两个痛点局部极小值和路径震荡。看这个速度方向扰动策略% 速度方向扰动 if norm(v) 0.1 % 检测陷入局部极小 theta atan2(v(2), v(1)); theta theta 0.5*randn; % 随机角度偏移 v 0.3*[cos(theta); sin(theta)]; % 强制赋予新方向 end当检测到移动速度过低时给运动方向加个高斯噪声。就像被困住时瞎晃几下实测逃逸成功率提升60%以上。人工势场法路径规划matlab代码下图为人工势场法人工势场法改进。 内容包括两种方法编写人工势场法注释挺详细的还有文档对公式方法进行解释。再放个路径平滑处理的代码片段path [q_start]; while ~reached_goal % ...原有计算逻辑... % 路径回溯检测 if size(path,2) 5 last_5 path(:,end-4:end); std_dev std(last_5,0,2); if max(std_dev) 0.02 % 检测震荡 q q 0.2*(q_goal - q) randn(2,1)*0.1; % 强行突围 end end end这个骚操作通过检测最近5个路径点的标准差判断是否陷入震荡。触发条件时直接往目标方向硬冲配合随机扰动效果拔群。最后说说参数调优的血泪史katt建议从1.0开始调krep别超过0.5否则容易高频抖动。障碍物影响距离d0设置在2-3倍机器人半径比较合适。改进版完整代码里还有动态参数调整模块会根据环境复杂度自动缩放系数这个下回再细聊。注图示结果请参考原文配图改进版路径明显更平滑且能逃出U型陷阱