三维人工势场法：人工势场 APF 方法为实际应用提供了一种简单有效的运动规划方法当前开发的项目是在Matlab中开发的，具有克服局部极小问题的改进算法

最早接触APF的时候觉得这法子挺聪明：给机器人套上“引力场”往目标拽，再套个“斥力场”躲开障碍物，顺着合力的方向挪位置就能走到终点。但真到三维场景里跑才发现坑有多密——比如俩障碍物离得近、目标又卡在中间的时候，引力和斥力刚好抵消，机器人就卡那儿不动了，跟被左右同事夹在工位中间动弹不得一模一样。

这次项目是纯在Matlab里搭的，一开始直接抄了网上的基础三维势场代码，跑简单场景还行，一碰到俩对称障碍物就彻底摆烂：

function [F_total] = basic_apf(current_pos, goal_pos, obstacle_pos, k_att, k_rep, d0)
    % 基础三维人工势场合力计算
    vec_att = goal_pos - current_pos;
    F_att = k_att * vec_att;
    
    F_rep = [0,0,0];
    for i = 1:size(obstacle_pos,1)
        dist = norm(current_pos - obstacle_pos(i,:));
        if dist <= d0
            vec_rep = current_pos - obstacle_pos(i,:);
            F_rep = F_rep + k_rep * (1/dist - 1/d0) * vec_rep / (dist^2 + 1e-6);
        end
    end
    
    F_total = F_att + F_rep;
end

这段代码逻辑直白到一眼能看懂，但问题也藏在这儿：当机器人卡在两个障碍物的引力斥力平衡点时，合力会变成0，欧拉积分更新位置的时候根本挪不动。最开始我还以为是参数调崩了，来回改katt和krep改了俩小时，直到翻论文才反应过来这是经典的局部极小问题。

三维人工势场法：人工势场 APF 方法为实际应用提供了一种简单有效的运动规划方法当前开发的项目是在Matlab中开发的，具有克服局部极小问题的改进算法

后来给自己加了个简单的脱困补丁，就是在主循环里加个卡壳检测：要是机器人连续8步位置变化都小于0.05米，就额外加一个临时斥力，把它往远离最近障碍物的方向推一把。完整的改进代码大概长这样：

% 主循环跑路径
clear;clc;close all
current_pos = [0,0,0];
goal_pos = [10,10,10];
% 造俩对称障碍物，刚好把中间路堵死
obstacle_pos = [[5,5,0];[5,5,10]]; 

k_att = 0.8;   % 引力系数
k_rep = 20;    % 基础斥力系数
k_emerg = 15;  % 临时脱困力系数
d0 = 3;        % 斥力生效范围
pos_history = [];
max_step = 600;

for step = 1:max_step
    % 算基础合力
    F = basic_apf(current_pos, goal_pos, obstacle_pos, k_att, k_rep, d0);
    
    % 脱困检测逻辑
    pos_history = [pos_history; current_pos];
    if size(pos_history,1) > 8
        disp_delta = norm(current_pos - pos_history(end-8,:));
        if disp_delta < 0.05
            % 找到离当前位置最近的障碍物
            dists = sqrt(sum((current_pos - obstacle_pos).^2,2));
            [~,idx] = min(dists);
            nearest_obs = obstacle_pos(idx,:);
            % 生成临时脱困斥力
            vec_emerg = current_pos - nearest_obs;
            F = F + k_emerg * vec_emerg / (norm(vec_emerg) + 1e-6);
            pos_history = []; % 重置历史避免反复触发
        end
    end
    
    % 顺着合力挪位置，欧拉积分简单粗暴够用
    current_pos = current_pos + 0.1 * F / (norm(F) + 1e-6);
    
    % 实时画图看效果
    clf; hold on; axis equal; view(30,30)
    plot3(current_pos(1),current_pos(2),current_pos(3),'ro','MarkerSize',10);
    plot3(goal_pos(1),goal_pos(2),goal_pos(3),'gs','MarkerSize',15);
    plot3(obstacle_pos(:,1),obstacle_pos(:,2),obstacle_pos(:,3),'kx','MarkerSize',10);
    drawnow;
    
    if norm(current_pos - goal_pos) < 0.2
        disp('总算摸到目标点了！'); break;
    end
end

改完之后跑起来就顺多了，再也不会杵在障碍物中间不动。中间踩过的小坑也不少：比如最开始把临时力系数k_emerg设成30，结果机器人一脱困就直接撞去另一个障碍物；还有步数阈值设成5的时候容易误触发，机器人稍微晃一下就强行脱困，后来磨了半天才把参数调到舒服的状态。

其实这个脱困逻辑挺糙的，远不如那些带虚拟目标的改进算法高级，但胜在写起来快、调参简单，对于快速验证三维路径规划场景足够用了。要是你也在Matlab里折腾路径规划，不妨试试这个小补丁，至少能少熬几个夜看机器人摆烂。