基于Matlab的改进人工势场法路径规划与避障
一、算法原理与改进策略
1.1 传统人工势场法问题
| 问题 | 描述 | 解决方案 |
|---|---|---|
| 局部极小值 | 机器人陷入势能谷点无法逃脱 | 引入虚拟目标点/随机扰动/次梯度法 |
| 目标不可达 | 障碍物附近斥力使机器人无法到达 | 改进斥力函数，目标点附近斥力衰减 |
| 振荡问题 | 狭窄通道中来回振荡 | 引入速度势场/惯性项 |
| 障碍物附近震荡 | 斥力过大导致轨迹抖动 | 自适应调节引力斥力系数 |
1.2 改进势场函数
% 改进后的势场函数形式
U_att(q) = ½ * k_att * ρ_goal^n(q) % 引力势
U_rep(q) = ½ * k_rep * (1/ρ_obs(q) - 1/ρ0)^2 * ρ_goal^m(q) % 斥力势
F_total = -∇U_att(q) - ∇U_rep(q) + F_virtual % 总力
二、MATLAB实现
2.1 主程序（main_improved_apf.m）
%% 改进人工势场法路径规划与避障
clc; clear; close all;
warning('off', 'all');
%% 1. 环境参数设置
env.map_size = [0, 100, 0, 100]; % 地图范围 [x_min, x_max, y_min, y_max]
env.start = [10, 10]; % 起点坐标
env.goal = [90, 90]; % 终点坐标
env.robot_radius = 2; % 机器人半径
env.safety_margin = 1; % 安全裕度
% 障碍物设置（圆形障碍物：[x, y, radius]）
env.obstacles = [
30, 30, 5;
50, 40, 8;
60, 60, 6;
40, 70, 7;
70, 20, 4;
20, 60, 6;
80, 50, 5
];
%% 2. 改进APF参数
apf.k_att = 5.0; % 引力增益系数
apf.k_rep = 100.0; % 斥力增益系数
apf.eta = 0.3; % 斥力衰减系数
apf.rho0 = 20; % 障碍物影响范围
apf.step_size = 1.0; % 步长
apf.max_iter = 3000; % 最大迭代次数
apf.goal_tol = 1.0; % 目标容差
apf.alpha = 0.8; % 动量系数
apf.beta = 0.1; % 随机扰动系数
apf.n = 2; % 引力势幂次
apf.m = 3; % 斥力势幂次
%% 3. 初始化
fprintf('改进人工势场法路径规划开始...\n');
fprintf('起点: (%.1f, %.1f), 终点: (%.1f, %.1f)\n', ...
env.start(1), env.start(2), env.goal(1), env.goal(2));
% 路径记录
path = zeros(apf.max_iter, 2);
path(1, :) = env.start;
vel = [0, 0]; % 速度初始化
% 性能记录
performance.iterations = 0;
performance.path_length = 0;
performance.min_clearance = inf;
performance.collision_free = true;
performance.local_minima = 0;
% 局部极小值检测
local_min_counter = 0;
local_min_threshold = 20; % 连续20次位置变化小于阈值认为陷入局部极小
pos_history = zeros(local_min_threshold, 2);
% 虚拟目标点
virtual_goal_active = false;
virtual_goal = [0, 0];
%% 4. 主循环
figure('Position', [100, 100, 1200, 500]);
for iter = 1:apf.max_iter
current_pos = path(iter, :);
% 检查是否到达目标
dist_to_goal = norm(current_pos - env.goal);
if dist_to_goal < apf.goal_tol
fprintf('成功到达目标点！迭代次数: %d\n', iter);
performance.iterations = iter;
path = path(1:iter, :);
break;
end
% 记录位置历史
pos_history(mod(iter-1, local_min_threshold)+1, :) = current_pos;
% 计算合力
[F_total, F_att, F_rep, min_dist] = compute_total_force(...
current_pos, env.goal, env.obstacles, apf, virtual_goal_active, virtual_goal);
% 更新最小通过距离
performance.min_clearance = min(performance.min_clearance, min_dist);
% 检查碰撞
if check_collision(current_pos, env.obstacles, env.robot_radius, env.safety_margin)
fprintf('发生碰撞！迭代次数: %d\n', iter);
performance.collision_free = false;
break;
end
% 局部极小值检测与处理
[is_local_min, virtual_goal_active, virtual_goal] = ...
handle_local_minima(iter, pos_history, current_pos, env.goal, ...
env.obstacles, apf, virtual_goal_active, virtual_goal, local_min_threshold);
if is_local_min
performance.local_minima = performance.local_minima + 1;
% 添加随机扰动
random_force = apf.beta * (2*rand(1,2)-1);
F_total = F_total + random_force;
% 添加虚拟目标力
if virtual_goal_active
F_virtual = compute_virtual_force(current_pos, virtual_goal, apf);
F_total = F_total + F_virtual;
end
end
% 动量法更新速度
vel = apf.alpha * vel + (1 - apf.alpha) * F_total;
% 归一化并更新位置
vel_norm = norm(vel);
if vel_norm > 0
vel = vel / vel_norm;
end
new_pos = current_pos + apf.step_size * vel;
% 边界约束
new_pos(1) = max(env.map_size(1) + env.robot_radius, ...
min(env.map_size(2) - env.robot_radius, new_pos(1)));
new_pos(2) = max(env.map_size(3) + env.robot_radius, ...
min(env.map_size(4) - env.robot_radius, new_pos(2)));
% 记录新位置
if iter < apf.max_iter
path(iter+1, :) = new_pos;
end
% 实时显示
if mod(iter, 10) == 0
visualize_path(env, path, iter, current_pos, F_att, F_rep, ...
virtual_goal_active, virtual_goal, apf);
end
end
if iter == apf.max_iter
fprintf('达到最大迭代次数！\n');
performance.iterations = apf.max_iter;
path = path(1:iter, :);
end
%% 5. 性能评估
performance.path_length = calculate_path_length(path);
performance = evaluate_performance(env, path, performance);
%% 6. 显示最终结果
plot_final_results(env, path, performance, apf);
2.2 合力计算函数（compute_total_force.m）
function [F_total, F_att, F_rep, min_dist] = compute_total_force(...
pos, goal, obstacles, apf, virtual_goal_active, virtual_goal)
% 计算改进人工势场法的合力
% 输入:
% pos: 当前位置
% goal: 目标位置
% obstacles: 障碍物列表
% apf: 势场参数结构体
% virtual_goal_active: 虚拟目标是否激活
% virtual_goal: 虚拟目标位置
% 输出:
% F_total: 总力
% F_att: 引力
% F_rep: 斥力
% min_dist: 到最近障碍物的距离
% 1. 计算引力
if virtual_goal_active
% 使用虚拟目标计算引力
F_att = compute_attractive_force(pos, virtual_goal, apf);
else
% 使用真实目标计算引力
F_att = compute_attractive_force(pos, goal, apf);
end
% 2. 计算斥力
F_rep = [0, 0];
min_dist = inf;
n_obstacles = size(obstacles, 1);
for i = 1:n_obstacles
obs_center = obstacles(i, 1:2);
obs_radius = obstacles(i, 3);
% 计算到障碍物边缘的距离
dist_to_obs = norm(pos - obs_center) - obs_radius;
if dist_to_obs < min_dist
min_dist = dist_to_obs;
end
% 改进斥力：只在影响范围内计算
if dist_to_obs <= apf.rho0
% 计算斥力方向
if dist_to_obs > 0
dir_rep = (pos - obs_center) / norm(pos - obs_center);
else
% 如果在障碍物内部，取随机方向
dir_rep = (2*rand(1,2)-1);
dir_rep = dir_rep / norm(dir_rep);
end
% 改进的斥力函数
d_goal = norm(pos - goal);
if d_goal > 0
% 斥力随接近目标而衰减
rep_mag = apf.k_rep * (1/dist_to_obs - 1/apf.rho0) * ...
(d_goal^apf.m) / (dist_to_obs^2);
% 增加切向力分量，帮助绕过障碍物
F_rep_obs = rep_mag * dir_rep;
% 添加切向分量
if dist_to_obs < apf.rho0/2
tangent_dir = [dir_rep(2), -dir_rep(1)];
F_rep_obs = F_rep_obs + 0.3 * rep_mag * tangent_dir;
end
F_rep = F_rep + F_rep_obs;
end
end
end
% 3. 计算总力
F_total = F_att + F_rep;
end
function F_att = compute_attractive_force(pos, target, apf)
% 计算引力
% 改进的引力函数：使用指数衰减
dist = norm(pos - target);
if dist > 0
% 改进的引力：在远处使用二次函数，近处使用线性函数
if dist > 10
F_att = apf.k_att * (target - pos);
else
F_att = apf.k_att * (target - pos) / dist;
end
else
F_att = [0, 0];
end
end
2.3 局部极小值处理（handle_local_minima.m）
function [is_local_min, virtual_goal_active, virtual_goal] = ...
handle_local_minima(iter, pos_history, current_pos, goal, obstacles, ...
apf, virtual_goal_active, virtual_goal, threshold)
% 处理局部极小值问题
% 输入:
% iter: 当前迭代次数
% pos_history: 位置历史
% current_pos: 当前位置
% goal: 目标位置
% obstacles: 障碍物
% apf: 势场参数
% virtual_goal_active: 虚拟目标是否激活
% virtual_goal: 虚拟目标
% threshold: 检测阈值
% 输出:
% is_local_min: 是否陷入局部极小
% virtual_goal_active: 更新后的虚拟目标状态
% virtual_goal: 更新后的虚拟目标
is_local_min = false;
% 只在有足够历史数据时检测
if iter >= threshold
% 计算最近threshold次迭代的位置变化
pos_changes = zeros(threshold-1, 1);
for i = 1:threshold-1
pos1 = pos_history(i, :);
pos2 = pos_history(i+1, :);
pos_changes(i) = norm(pos1 - pos2);
end
% 如果最近的位置变化都很小，认为陷入局部极小
if all(pos_changes < 0.1 * apf.step_size)
is_local_min = true;
fprintf('检测到局部极小值！迭代: %d\n', iter);
% 如果虚拟目标未激活，创建虚拟目标
if ~virtual_goal_active
virtual_goal_active = true;
% 方法1：在障碍物稀疏方向设置虚拟目标
[free_direction, is_valid] = find_free_direction(current_pos, obstacles, apf.rho0);
if is_valid
virtual_goal = current_pos + 20 * free_direction;
else
% 方法2：随机方向
random_angle = 2*pi*rand();
virtual_goal = current_pos + 20 * [cos(random_angle), sin(random_angle)];
end
fprintf('设置虚拟目标: (%.1f, %.1f)\n', virtual_goal(1), virtual_goal(2));
end
% 检查是否应该切换到真实目标
if virtual_goal_active
dist_to_virtual = norm(current_pos - virtual_goal);
if dist_to_virtual < 2.0
% 到达虚拟目标，切换回真实目标
virtual_goal_active = false;
fprintf('到达虚拟目标，切换回真实目标\n');
end
% 检查是否有更直接的路径
if can_reach_goal_directly(current_pos, goal, obstacles, apf.rho0)
virtual_goal_active = false;
fprintf('找到直接路径，切换回真实目标\n');
end
end
end
end
end
function [free_dir, is_valid] = find_free_direction(pos, obstacles, rho0)
% 寻找自由方向
% 输入: pos - 当前位置
% obstacles - 障碍物列表
% rho0 - 影响范围
% 输出: free_dir - 自由方向
% is_valid - 是否找到有效方向
n_directions = 36; % 每10度一个方向
angles = linspace(0, 2*pi, n_directions+1);
angles = angles(1:end-1);
best_score = -inf;
best_angle = 0;
is_valid = false;
for i = 1:n_directions
direction = [cos(angles(i)), sin(angles(i))];
score = 0;
% 计算这个方向的得分
for j = 1:size(obstacles, 1)
obs_center = obstacles(j, 1:2);
obs_radius = obstacles(j, 3);
dist_to_obs = norm(pos - obs_center) - obs_radius;
if dist_to_obs < rho0
% 计算障碍物方向
obs_dir = (obs_center - pos) / norm(obs_center - pos);
% 计算与障碍物的夹角
angle_diff = acos(dot(direction, obs_dir));
% 远离障碍物的方向得分高
score = score + exp(-(angle_diff^2) * 10) * (rho0 - dist_to_obs);
end
end
if score < best_score
best_score = score;
best_angle = angles(i);
is_valid = true;
end
end
free_dir = [cos(best_angle), sin(best_angle)];
end
function can_reach = can_reach_goal_directly(pos, goal, obstacles, rho0)
% 检查是否可以直接到达目标
% 输入: pos - 当前位置
% goal - 目标位置
% obstacles - 障碍物
% rho0 - 影响范围
% 输出: can_reach - 是否可以直接到达
% 计算直线路径
path_vector = goal - pos;
path_length = norm(path_vector);
path_dir = path_vector / path_length;
can_reach = true;
% 检查路径上的障碍物
for i = 1:size(obstacles, 1)
obs_center = obstacles(i, 1:2);
obs_radius = obstacles(i, 3);
% 计算点到线段的最小距离
t = max(0, min(1, dot(obs_center - pos, path_vector) / (path_length^2)));
projection = pos + t * path_vector;
dist = norm(obs_center - projection);
if dist < obs_radius + rho0/2
can_reach = false;
break;
end
end
end
2.4 虚拟目标力计算（compute_virtual_force.m）
function F_virtual = compute_virtual_force(pos, virtual_goal, apf)
% 计算虚拟目标力
% 输入: pos - 当前位置
% virtual_goal - 虚拟目标
% apf - 势场参数
% 输出: F_virtual - 虚拟力
dist = norm(pos - virtual_goal);
if dist > 0
% 使用与真实引力相同的计算方式
F_virtual = 0.5 * apf.k_att * (virtual_goal - pos);
% 归一化
F_norm = norm(F_virtual);
if F_norm > 0
F_virtual = F_virtual / F_norm;
end
else
F_virtual = [0, 0];
end
end
2.5 碰撞检测函数（check_collision.m）
function collision = check_collision(pos, obstacles, robot_radius, safety_margin)
% 碰撞检测
% 输入: pos - 当前位置
% obstacles - 障碍物列表
% robot_radius - 机器人半径
% safety_margin - 安全裕度
% 输出: collision - 是否发生碰撞
collision = false;
total_radius = robot_radius + safety_margin;
for i = 1:size(obstacles, 1)
obs_center = obstacles(i, 1:2);
obs_radius = obstacles(i, 3);
dist = norm(pos - obs_center);
if dist < (obs_radius + total_radius)
collision = true;
return;
end
end
end
2.6 路径评估函数
function path_length = calculate_path_length(path)
% 计算路径长度
% 输入: path - 路径点
% 输出: path_length - 路径长度
path_length = 0;
for i = 1:size(path, 1)-1
segment_length = norm(path(i+1, :) - path(i, :));
path_length = path_length + segment_length;
end
end
function performance = evaluate_performance(env, path, performance)
% 评估路径性能
% 输入: env - 环境参数
% path - 路径
% performance - 性能结构体
% 输出: performance - 更新后的性能结构体
% 1. 计算平滑度
performance.smoothness = calculate_path_smoothness(path);
% 2. 计算安全性评分
performance.safety_score = calculate_safety_score(path, env.obstacles, env.robot_radius);
% 3. 计算效率
direct_distance = norm(env.goal - env.start);
performance.efficiency = direct_distance / performance.path_length * 100;
% 4. 计算转弯次数
performance.turn_count = calculate_turn_count(path);
fprintf('\n=== 路径性能评估 ===\n');
fprintf('路径长度: %.2f (直接距离: %.2f)\n', performance.path_length, direct_distance);
fprintf('效率: %.1f%%\n', performance.efficiency);
fprintf('最小通过距离: %.2f\n', performance.min_clearance);
fprintf('安全性评分: %.2f/10\n', performance.safety_score);
fprintf('路径平滑度: %.2f/10\n', performance.smoothness);
fprintf('转弯次数: %d\n', performance.turn_count);
fprintf('局部极小值次数: %d\n', performance.local_minima);
fprintf('是否无碰撞: %s\n', string(performance.collision_free));
end
function smoothness = calculate_path_smoothness(path)
% 计算路径平滑度
% 输入: path - 路径
% 输出: smoothness - 平滑度评分 (0-10)
if size(path, 1) < 3
smoothness = 10;
return;
end
total_angle_change = 0;
for i = 2:size(path, 1)-1
v1 = path(i, :) - path(i-1, :);
v2 = path(i+1, :) - path(i, :);
if norm(v1) > 0 && norm(v2) > 0
v1 = v1 / norm(v1);
v2 = v2 / norm(v2);
angle = acos(dot(v1, v2));
total_angle_change = total_angle_change + abs(angle);
end
end
% 转换为评分 (0-10，越高越平滑)
max_angle = pi * (size(path, 1)-2);
if max_angle > 0
smoothness = 10 * (1 - total_angle_change / max_angle);
smoothness = max(0, min(10, smoothness));
else
smoothness = 10;
end
end
function safety_score = calculate_safety_score(path, obstacles, robot_radius)
% 计算安全性评分
% 输入: path - 路径
% obstacles - 障碍物
% robot_radius - 机器人半径
% 输出: safety_score - 安全性评分 (0-10)
if isempty(path)
safety_score = 0;
return;
end
min_distances = zeros(size(path, 1), 1);
for i = 1:size(path, 1)
pos = path(i, :);
min_dist = inf;
for j = 1:size(obstacles, 1)
obs_center = obstacles(j, 1:2);
obs_radius = obstacles(j, 3);
dist = norm(pos - obs_center) - obs_radius - robot_radius;
if dist < min_dist
min_dist = dist;
end
end
min_distances(i) = min_dist;
end
% 使用平均距离计算安全性评分
avg_safe_dist = mean(min_distances);
safety_score = min(10, avg_safe_dist * 2); % 每0.5单位距离得1分
end
function turn_count = calculate_turn_count(path)
% 计算转弯次数
% 输入: path - 路径
% 输出: turn_count - 转弯次数
if size(path, 1) < 3
turn_count = 0;
return;
end
turn_count = 0;
angle_threshold = pi/6; % 30度阈值
for i = 2:size(path, 1)-1
v1 = path(i, :) - path(i-1, :);
v2 = path(i+1, :) - path(i, :);
if norm(v1) > 0 && norm(v2) > 0
v1 = v1 / norm(v1);
v2 = v2 / norm(v2);
angle = acos(dot(v1, v2));
if abs(angle) > angle_threshold
turn_count = turn_count + 1;
end
end
end
end
2.7 可视化函数
function visualize_path(env, path, iter, current_pos, F_att, F_rep, ...
virtual_goal_active, virtual_goal, apf)
% 实时可视化路径规划过程
% 输入: env - 环境参数
% path - 当前路径
% iter - 当前迭代
% current_pos - 当前位置
% F_att, F_rep - 引力斥力
% virtual_goal_active - 虚拟目标状态
% virtual_goal - 虚拟目标位置
% apf - 势场参数
% 清除当前图形
clf;
% 子图1：路径规划
subplot(1,2,1);
hold on; grid on; axis equal;
axis(env.map_size);
title(sprintf('改进人工势场法 - 迭代: %d', iter));
% 绘制障碍物
for i = 1:size(env.obstacles, 1)
rectangle('Position', [env.obstacles(i,1)-env.obstacles(i,3), ...
env.obstacles(i,2)-env.obstacles(i,3), ...
2*env.obstacles(i,3), 2*env.obstacles(i,3)], ...
'Curvature', [1,1], 'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'k', 'LineWidth', 1.5);
% 绘制障碍物影响范围
viscircles(env.obstacles(i,1:2), env.obstacles(i,3)+apf.rho0, ...
'Color', [0.9, 0.9, 0.2], 'LineWidth', 0.5, 'LineStyle', '--');
end
% 绘制起点和终点
plot(env.start(1), env.start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(env.goal(1), env.goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
% 绘制当前路径
plot(path(1:iter, 1), path(1:iter, 2), 'b-', 'LineWidth', 2);
% 绘制当前位置
plot(current_pos(1), current_pos(2), 'bo', 'MarkerSize', 8, 'MarkerFaceColor', 'b');
% 绘制力向量
scale = 5;
if norm(F_att) > 0
quiver(current_pos(1), current_pos(2), F_att(1)*scale, F_att(2)*scale, ...
'Color', 'g', 'LineWidth', 2, 'MaxHeadSize', 0.5);
end
if norm(F_rep) > 0
quiver(current_pos(1), current_pos(2), F_rep(1)*scale, F_rep(2)*scale, ...
'Color', 'r', 'LineWidth', 2, 'MaxHeadSize', 0.5);
end
% 绘制虚拟目标
if virtual_goal_active
plot(virtual_goal(1), virtual_goal(2), 'm*', 'MarkerSize', 12, 'LineWidth', 2);
plot([current_pos(1), virtual_goal(1)], [current_pos(2), virtual_goal(2)], ...
'm--', 'LineWidth', 1);
end
% 添加图例
legend_items = {'起点', '终点', '路径', '当前位置', '引力', '斥力'};
if virtual_goal_active
legend_items{end+1} = '虚拟目标';
end
legend(legend_items, 'Location', 'northeastoutside');
xlabel('X坐标');
ylabel('Y坐标');
% 子图2：势场可视化
subplot(1,2,2);
[x_grid, y_grid] = meshgrid(linspace(env.map_size(1), env.map_size(2), 50), ...
linspace(env.map_size(3), env.map_size(4), 50));
U_total = zeros(size(x_grid));
% 计算总势场
for i = 1:numel(x_grid)
pos = [x_grid(i), y_grid(i)];
% 计算引力势
dist_goal = norm(pos - env.goal);
U_att = 0.5 * apf.k_att * dist_goal^apf.n;
% 计算斥力势
U_rep = 0;
for j = 1:size(env.obstacles, 1)
obs_center = env.obstacles(j, 1:2);
obs_radius = env.obstacles(j, 3);
dist_obs = norm(pos - obs_center) - obs_radius;
if dist_obs < apf.rho0
U_rep = U_rep + 0.5 * apf.k_rep * (1/dist_obs - 1/apf.rho0)^2 * dist_goal^apf.m;
end
end
U_total(i) = U_att + U_rep;
end
% 绘制势场
contourf(x_grid, y_grid, U_total, 20, 'LineStyle', 'none');
hold on;
colorbar;
title('总势场分布');
% 绘制障碍物
for i = 1:size(env.obstacles, 1)
rectangle('Position', [env.obstacles(i,1)-env.obstacles(i,3), ...
env.obstacles(i,2)-env.obstacles(i,3), ...
2*env.obstacles(i,3), 2*env.obstacles(i,3)], ...
'Curvature', [1,1], 'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'k', 'LineWidth', 1.5);
end
% 绘制路径
plot(path(1:iter, 1), path(1:iter, 2), 'w-', 'LineWidth', 2);
plot(env.start(1), env.start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(env.goal(1), env.goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
axis equal;
axis(env.map_size);
xlabel('X坐标');
ylabel('Y坐标');
drawnow;
end
function plot_final_results(env, path, performance, apf)
% 绘制最终结果
figure('Position', [100, 100, 1400, 600]);
% 子图1：完整路径
subplot(1,3,1);
hold on; grid on; axis equal;
axis(env.map_size);
title('改进人工势场法 - 最终路径');
% 绘制障碍物
for i = 1:size(env.obstacles, 1)
rectangle('Position', [env.obstacles(i,1)-env.obstacles(i,3), ...
env.obstacles(i,2)-env.obstacles(i,3), ...
2*env.obstacles(i,3), 2*env.obstacles(i,3)], ...
'Curvature', [1,1], 'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'k', 'LineWidth', 1.5);
end
% 绘制起点和终点
plot(env.start(1), env.start(2), 'go', 'MarkerSize', 12, 'MarkerFaceColor', 'g');
plot(env.goal(1), env.goal(2), 'ro', 'MarkerSize', 12, 'MarkerFaceColor', 'r');
% 绘制路径
plot(path(:,1), path(:,2), 'b-', 'LineWidth', 2);
plot(path(:,1), path(:,2), 'b.', 'MarkerSize', 8);
% 绘制机器人轨迹
for i = 1:5:size(path,1)
rectangle('Position', [path(i,1)-env.robot_radius, path(i,2)-env.robot_radius, ...
2*env.robot_radius, 2*env.robot_radius], ...
'Curvature', [1,1], 'EdgeColor', 'b', 'LineWidth', 0.5, 'LineStyle', '--');
end
xlabel('X坐标');
ylabel('Y坐标');
legend('起点', '终点', '路径', '机器人位置', 'Location', 'best');
% 子图2：距离障碍物曲线
subplot(1,3,2);
hold on; grid on;
distances = zeros(size(path,1), 1);
for i = 1:size(path,1)
pos = path(i, :);
min_dist = inf;
for j = 1:size(env.obstacles, 1)
obs_center = env.obstacles(j, 1:2);
obs_radius = env.obstacles(j, 3);
dist = norm(pos - obs_center) - obs_radius;
if dist < min_dist
min_dist = dist;
end
end
distances(i) = min_dist;
end
plot(1:size(path,1), distances, 'b-', 'LineWidth', 2);
plot([1, size(path,1)], [env.robot_radius+env.safety_margin, env.robot_radius+env.safety_margin], ...
'r--', 'LineWidth', 1.5);
xlabel('路径点序号');
ylabel('到最近障碍物距离');
title('安全性分析');
legend('实际距离', '安全阈值', 'Location', 'best');
% 子图3：性能指标雷达图
subplot(1,3,3);
metrics = {'路径长度', '安全性', '平滑度', '效率', '转弯次数'};
values = [performance.path_length/100, ...
performance.safety_score/10, ...
performance.smoothness/10, ...
performance.efficiency/100, ...
min(1, performance.turn_count/10)];
% 归一化（越小越好或越大越好调整）
values(1) = 1 - values(1); % 路径长度越小越好
values(5) = 1 - values(5); % 转弯次数越少越好
% 绘制雷达图
angles = linspace(0, 2*pi, length(metrics)+1);
angles = angles(1:end-1);
values = [values, values(1)];
angles = [angles, angles(1)];
polarplot(angles, values, 'b-', 'LineWidth', 2);
hold on;
fill(angles, values, 'b', 'FaceAlpha', 0.2);
thetalim([0, 360]);
rlim([0, 1]);
title('性能指标雷达图');
legend('改进APF', 'Location', 'southoutside');
% 添加参数表
annotation('textbox', [0.02, 0.02, 0.3, 0.15], ...
'String', sprintf('APF参数:\nk_{att}=%.1f, k_{rep}=%.1f\nρ_0=%.1f, α=%.1f\n步长=%.1f', ...
apf.k_att, apf.k_rep, apf.rho0, apf.alpha, apf.step_size), ...
'FitBoxToText', 'on', 'BackgroundColor', 'w');
% 保存结果
saveas(gcf, 'improved_apf_result.png');
end
参考代码 基于Matlab的改进人工势场法实现路径规划与避障 www.youwenfan.com/contentcnu/54690.html
三、高级功能扩展
3.1 动态障碍物处理
function [updated_obstacles, predicted_positions] = handle_dynamic_obstacles(...
obstacles, current_time, velocity, dt)
% 处理动态障碍物
% 输入: obstacles - 障碍物列表
% current_time - 当前时间
% velocity - 速度场
% dt - 时间步长
% 输出: updated_obstacles - 更新后的障碍物
% predicted_positions - 预测位置
updated_obstacles = obstacles;
predicted_positions = cell(size(obstacles, 1), 1);
% 假设前3个障碍物是动态的
for i = 1:min(3, size(obstacles, 1))
% 简谐运动模型
amplitude = 5;
frequency = 0.5;
% 更新位置
dx = amplitude * sin(2*pi*frequency*current_time);
dy = amplitude * cos(2*pi*frequency*current_time);
updated_obstacles(i, 1:2) = obstacles(i, 1:2) + [dx, dy];
% 预测未来位置
prediction_steps = 5;
predicted_positions{i} = zeros(prediction_steps, 2);
for step = 1:prediction_steps
t_pred = current_time + step * dt;
dx_pred = amplitude * sin(2*pi*frequency*t_pred);
dy_pred = amplitude * cos(2*pi*frequency*t_pred);
predicted_positions{i}(step, :) = obstacles(i, 1:2) + [dx_pred, dy_pred];
end
end
end
3.2 多机器人协同
function F_robot = compute_robot_repulsion(current_robot, other_robots, k_robot, rho_robot)
% 计算机器人间的斥力
% 输入: current_robot - 当前机器人位置
% other_robots - 其他机器人位置
% k_robot - 机器人间斥力系数
% rho_robot - 机器人间影响距离
% 输出: F_robot - 机器人间斥力
F_robot = [0, 0];
n_robots = size(other_robots, 1);
for i = 1:n_robots
other_pos = other_robots(i, :);
dist = norm(current_robot - other_pos);
if dist > 0 && dist < rho_robot
dir = (current_robot - other_pos) / dist;
magnitude = k_robot * (1/dist - 1/rho_robot) * (1/dist^2);
F_robot = F_robot + magnitude * dir;
end
end
end
四、参数调优指南
| 参数 | 推荐范围 | 影响 | 调优建议 |
|---|---|---|---|
| k_att | 1-20 | 引力强度 | 增大加快收敛，但可能振荡 |
| k_rep | 50-200 | 斥力强度 | 增大提高安全性，但可能无法到达目标 |
| ρ0 | 10-30 | 障碍物影响范围 | 增大提前避障，但计算量增加 |
| α | 0.5-0.9 | 动量系数 | 增大提高平滑性，降低响应速度 |
| β | 0.05-0.2 | 随机扰动系数 | 增大增强跳出局部极小能力，降低稳定性 |
| 步长 | 0.5-2.0 | 移动步长 | 增大加快规划，但可能振荡或不安全 |
五、应用场景
5.1 移动机器人导航
% 室内环境导航示例
env.map_size = [0, 50, 0, 50]; % 室内环境
env.start = [5, 5];
env.goal = [45, 45];
env.robot_radius = 0.5;
% 墙壁和家具障碍物
env.obstacles = [
% 墙壁
25, 0, 2; 25, 50, 2; 0, 25, 2; 50, 25, 2;
% 家具
10, 20, 3; 30, 15, 2; 40, 30, 4; 15, 40, 3
];
5.2 无人机路径规划
% 三维扩展
function F_total_3D = compute_3d_force(pos_3d, goal_3d, obstacles_3d, params)
% 3D人工势场计算
% 输入: pos_3d - 三维位置
% goal_3d - 三维目标
% obstacles_3d - 三维障碍物
% params - 参数
% 输出: F_total_3D - 三维合力
% 引力
dist_goal = norm(pos_3d - goal_3d);
F_att_3d = params.k_att * (goal_3d - pos_3d);
% 斥力
F_rep_3d = [0, 0, 0];
for i = 1:size(obstacles_3d, 1)
obs_center = obstacles_3d(i, 1:3);
obs_radius = obstacles_3d(i, 4);
dist_obs = norm(pos_3d - obs_center) - obs_radius;
if dist_obs < params.rho0
dir_rep = (pos_3d - obs_center) / norm(pos_3d - obs_center);
magnitude = params.k_rep * (1/dist_obs - 1/params.rho0) * ...
(dist_goal^params.m) / (dist_obs^2);
F_rep_3d = F_rep_3d + magnitude * dir_rep;
end
end
F_total_3D = F_att_3d + F_rep_3d;
end
六、总结
这个改进的人工势场法实现具有以下特点：
局部极小值处理：虚拟目标+随机扰动+切向力
目标可达保证：改进斥力函数，目标点附近斥力衰减
平滑路径生成：动量法+路径平滑度评估
安全性保证：碰撞检测+安全裕度+安全性评分
动态适应性：可扩展支持动态障碍物
全面评估：路径长度、平滑度、安全性、效率等多指标评估
核心改进：
- 改进的斥力函数，避免目标不可达
- 虚拟目标策略跳出局部极小
- 切向力帮助绕过障碍物
- 动量法提高路径平滑性
- 多指标性能评估体系
应用场景：
- 移动机器人导航
- 无人机路径规划
- 自动驾驶局部路径规划
- 工业机器人避障
- 游戏AI路径规划
进一步优化方向：
- 结合A*等全局规划器
- 引入机器学习自适应参数
- 支持非完整约束机器人
- 多智能体协同规划
- 实时性能优化
