1. 人工势场法原理与基础实现

人工势场法(Artificial Potential Field)是机器人路径规划中的经典算法，由Khatib在1986年首次提出。其核心思想是将机器人运动环境抽象为势能场：目标点产生引力，障碍物产生斥力，机器人沿着势场梯度下降方向移动。

1.1 基本数学模型

引力场函数通常采用二次函数形式：

U_att(q) = 0.5 * ξ * ρ^2(q,q_goal)

其中ξ为引力增益系数，ρ(q,q_goal)表示当前位置q到目标点q_goal的欧式距离。

斥力场函数则表示为：

U_rep(q) = 0.5 * η * (1/ρ(q,q_obs) - 1/ρ0)^2 (当ρ(q,q_obs) ≤ ρ0) = 0 (当ρ(q,q_obs) > ρ0)

η为斥力增益系数，ρ0为障碍物影响半径。

1.2 MATLAB基础实现

% 参数设置 xi = 0.5; % 引力增益 eta = 0.8; % 斥力增益 rho0 = 5; % 障碍物影响半径 % 目标点和障碍物位置 q_goal = [50,50]; q_obs = [30,30]; % 计算势场 [X,Y] = meshgrid(1:0.5:100); U_att = 0.5*xi*((X-q_goal(1)).^2 + (Y-q_goal(2)).^2); U_rep = zeros(size(X)); dist_obs = sqrt((X-q_obs(1)).^2 + (Y-q_obs(2)).^2); U_rep(dist_obs<=rho0) = 0.5*eta*(1./dist_obs(dist_obs<=rho0) - 1/rho0).^2; U_total = U_att + U_rep; % 绘制势场 figure; surf(X,Y,U_total); title('人工势场分布');

注意：增益系数ξ和η的选择对算法性能影响很大，需要根据具体场景调试。一般建议初始值设为0.5-1.0之间。

2. 传统人工势场法的局限性分析

2.1 局部极小值问题

当引力与斥力在某点达到平衡时，机器人会陷入局部极小点而无法到达目标。常见于以下场景：

• U型障碍物环境
• 狭窄通道
• 对称障碍物分布

2.2 振荡现象

在狭窄通道中，机器人可能在两侧障碍物间反复振荡。这是由于：

1. 接近一侧障碍物时受到强斥力
2. 被推向另一侧后同样受到斥力
3. 形成无限循环

2.3 目标不可达问题

当机器人接近目标时，如果附近存在障碍物，斥力可能远大于引力，导致无法精确到达目标点。

3. 改进路径规划方案

3.1 虚拟目标点法

通过在局部极小点附近设置虚拟目标点引导机器人脱离困境：

function [q_new, virtual_goal] = escape_local_min(q, q_goal, obstacles) % 检测是否陷入局部极小（连续5步位移小于阈值） if norm(q - q_prev) < 0.1 % 生成虚拟目标点 virtual_goal = q + 5*randn(1,2); % 临时修改引力场函数 U_att = 0.5*xi*((X-virtual_goal(1)).^2 + (Y-virtual_goal(2)).^2); else virtual_goal = []; end end

3.2 动态窗口法结合

引入速度空间搜索，在势场引导下选择最优速度：

function [v, w] = DWA(q, U_total) % 获取当前速度范围 v_range = [max(0, v_current-a_max*dt), min(v_max, v_current+a_max*dt)]; w_range = [w_current-alpha_max*dt, w_current+alpha_max*dt]; % 评估各速度组合 best_score = -inf; for v = linspace(v_range(1), v_range(2), 20) for w = linspace(w_range(1), w_range(2), 20) % 预测轨迹 traj = predict_trajectory(q, v, w); % 计算势场积分 score = sum(interp2(X,Y,U_total,traj(:,1),traj(:,2))); if score > best_score best_v = v; best_w = w; best_score = score; end end end end

3.3 改进斥力场函数

修改斥力场函数避免目标不可达问题：

U_rep_improved = 0.5*eta*(1./dist_obs - 1/rho0).^2 .* dist_goal^n;

其中dist_goal是到目标的距离，n通常取2-3。

4. MATLAB完整实现案例

4.1 环境设置

% 创建复杂障碍环境 obstacles = [20,20; 30,40; 40,25; 60,60; 70,30; 80,80]; goal = [90,90]; start = [10,10]; % 绘图显示 figure; hold on; plot(obstacles(:,1), obstacles(:,2), 'ro', 'MarkerSize',10); plot(goal(1), goal(2), 'g*', 'MarkerSize',15); plot(start(1), start(2), 'bs', 'MarkerSize',15); grid on; xlim([0 100]); ylim([0 100]);

4.2 路径规划主循环

% 参数初始化 q = start; path = q; step_size = 0.5; max_iter = 1000; for i = 1:max_iter % 计算合力方向 F_att = -xi * (q - goal); F_rep = zeros(1,2); for j = 1:size(obstacles,1) dist = norm(q - obstacles(j,:)); if dist < rho0 F_rep = F_rep + eta*(1/dist - 1/rho0)*(q - obstacles(j,:))/dist^3; end end F_total = F_att + F_rep; % 归一化并移动 if norm(F_total) > 0 q = q + step_size * F_total/norm(F_total); end % 记录路径 path = [path; q]; % 检查是否到达目标 if norm(q - goal) < 2 break; end % 局部极小值处理 if i > 10 && norm(path(end,:)-path(end-5,:)) < 0.5 q = q + 5*randn(1,2); % 随机扰动 end end % 绘制最终路径 plot(path(:,1), path(:,2), 'b-', 'LineWidth',2);

4.3 性能优化技巧

1. 势场预计算：对于静态环境，可以预先计算整个空间的势场值，避免实时计算开销：

[U_total, grad_x, grad_y] = precompute_potential_field(X,Y,goal,obstacles);

2. KD树加速：当障碍物数量多时，使用KD树加速最近邻搜索：

obs_kdtree = KDTreeSearcher(obstacles); [idx, dist] = knnsearch(obs_kdtree, q, 'K', 5);

3. 多分辨率势场：先粗粒度规划大致路径，再局部细化：

% 第一层：低分辨率 [X1,Y1] = meshgrid(1:10:100); U1 = compute_potential(X1,Y1); % 第二层：高分辨率局部优化 [X2,Y2] = meshgrid(max(1,q(1)-20):0.5:min(100,q(1)+20),... max(1,q(2)-20):0.5:min(100,q(2)+20));

5. 实际应用中的挑战与解决方案

5.1 动态障碍物处理

对于移动障碍物，需要引入时间变量：

U_rep_dynamic = eta * exp(-lambda*t) / dist_obs^2;

其中λ为衰减系数，t为预测时间。

5.2 多机器人协调

通过添加机器人间的互斥势场：

for k = 1:num_robots if k ~= current_robot dist_robot = norm(q - robots(k).position); U_rep_robot = mu / dist_robot^2; % 互斥系数μ end end

5.3 传感器噪声应对

采用概率势场方法：

% 障碍物存在概率 p_obs = sensor_model(q); % 概率势场 U_rep_prob = p_obs * eta / dist_obs^2;

关键参数调试心得：

1. 引力增益ξ：太大导致路径震荡，太小则收敛慢
2. 斥力增益η：太大易陷入局部极小，太小则避障不及时
3. 影响半径ρ0：建议设为机器人直径的3-5倍
4. 步长step_size：通常取机器人最大速度的1/2

6. 进阶改进方向

6.1 基于强化学习的参数优化

使用Q-learning自动调整势场参数：

% 状态：距离目标/障碍物的相对位置 % 动作：调整ξ,η,ρ0等参数 % 奖励：路径长度、平滑度、安全性 Q_table = zeros(num_states, num_actions); for episode = 1:1000 [total_reward, path] = run_episode(Q_table); Q_table = update_Q(Q_table, path, total_reward); end

6.2 混合A*算法

在全局规划中使用A*，局部采用势场法：

global_path = A_star(start, goal, map); local_goal = global_path(min(5, length(global_path))); F_att = -xi * (q - local_goal);

6.3 三维扩展

对于无人机等应用，扩展到三维空间：

[X,Y,Z] = meshgrid(1:100, 1:100, 1:50); U_att_3D = 0.5*xi*( (X-goal(1)).^2 + (Y-goal(2)).^2 + (Z-goal(3)).^2 );

在实际机器人项目中，我们通常会将人工势场法与其他算法结合使用。例如在自动泊车系统中，先用RRT*生成全局路径，再用势场法进行局部避障和微调。这种分层方法既保证了全局最优性，又能实时应对动态障碍物。