基于人工势场法的MATLAB路径规划实现

基于人工势场法的MATLAB路径规划实现。人工势场法是一种常见的局部路径规划方法，通过虚拟力场引导机器人移动。

1. 基本原理

人工势场法将目标点设置为引力场，障碍物设置为斥力场，机器人受力向目标点移动。

2. MATLAB代码实现

%% 人工势场法路径规划
clear all; close all; clc;

%% 参数设置
start_pos = [0, 0];      % 起点
goal_pos = [10, 10];     % 目标点
obstacles = [            % 障碍物位置 [x, y, radius]
    3, 3, 1;
    5, 6, 1.5;
    7, 4, 0.8;
    8, 8, 1.2
];

% 势场参数
k_att = 1.0;     % 引力增益系数
k_rep = 2.0;     % 斥力增益系数
d0 = 3.0;        % 障碍物影响距离
step_size = 0.1; % 步长
max_iter = 500;  % 最大迭代次数
goal_thresh = 0.5; % 目标点阈值

%% 主循环
current_pos = start_pos;
path = current_pos;

figure;
hold on;
axis equal;
grid on;
xlim([-1, 12]);
ylim([-1, 12]);
xlabel('X');
ylabel('Y');
title('人工势场法路径规划');

% 绘制障碍物
for i = 1:size(obstacles, 1)
    rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
                          obstacles(i,2)-obstacles(i,3), ...
                          obstacles(i,3)*2, obstacles(i,3)*2], ...
              'Curvature', [1,1], 'FaceColor', [0.8, 0.2, 0.2], ...
              'EdgeColor', 'none');
end

% 绘制起点和目标点
plot(start_pos(1), start_pos(2), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal_pos(1), goal_pos(2), 'g*', 'MarkerSize', 15, 'LineWidth', 2);

for iter = 1:max_iter
    % 计算距离目标点的距离
    dist_to_goal = norm(current_pos - goal_pos);
    
    % 检查是否到达目标
    if dist_to_goal < goal_thresh
        disp('到达目标点！');
        break;
    end
    
    % 计算引力
    F_att = k_att * (goal_pos - current_pos);
    
    % 计算斥力
    F_rep = [0, 0];
    for i = 1:size(obstacles, 1)
        obs_pos = obstacles(i, 1:2);
        obs_radius = obstacles(i, 3);
        dist_to_obs = norm(current_pos - obs_pos);
        
        if dist_to_obs < (obs_radius + d0) && dist_to_obs > 0.1
            % 计算障碍物方向
            dir_obs = (current_pos - obs_pos) / dist_to_obs;
            
            % 计算斥力大小
            rep_magnitude = k_rep * (1/(dist_to_obs - obs_radius) - 1/d0) * ...
                          1/((dist_to_obs - obs_radius)^2);
            
            % 添加斥力
            F_rep = F_rep + rep_magnitude * dir_obs;
        end
    end
    
    % 计算合力
    F_total = F_att + F_rep;
    
    % 归一化并计算下一步位置
    if norm(F_total) > 0
        F_total = F_total / norm(F_total);
    end
    
    next_pos = current_pos + step_size * F_total;
    
    % 保存路径
    path = [path; next_pos];
    
    % 更新当前位置
    current_pos = next_pos;
    
    % 绘制当前路径
    plot(path(:,1), path(:,2), 'b-', 'LineWidth', 1.5);
    plot(current_pos(1), current_pos(2), 'ro', 'MarkerSize', 6);
    
    % 绘制力向量（可选）
    quiver(current_pos(1), current_pos(2), ...
           F_att(1)/10, F_att(2)/10, 'g', 'LineWidth', 1.5, 'MaxHeadSize', 0.5);
    quiver(current_pos(1), current_pos(2), ...
           F_rep(1)/10, F_rep(2)/10, 'r', 'LineWidth', 1.5, 'MaxHeadSize', 0.5);
    quiver(current_pos(1), current_pos(2), ...
           F_total(1)/10, F_total(2)/10, 'b', 'LineWidth', 2, 'MaxHeadSize', 1);
    
    drawnow;
    
    % 检查是否陷入局部最小值
    if iter > 10
        recent_path = path(max(1, end-10):end, :);
        if std(recent_path(:,1)) < 0.05 && std(recent_path(:,2)) < 0.05
            disp('可能陷入局部最小值！');
            % 添加随机扰动
            current_pos = current_pos + 0.5 * (rand(1,2)-0.5);
        end
    end
end

if iter == max_iter
    disp('达到最大迭代次数！');
end

%% 绘制最终路径
figure;
hold on;
axis equal;
grid on;
xlabel('X');
ylabel('Y');
title('最终路径');

% 绘制障碍物
for i = 1:size(obstacles, 1)
    rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
                          obstacles(i,2)-obstacles(i,3), ...
                          obstacles(i,3)*2, obstacles(i,3)*2], ...
              'Curvature', [1,1], 'FaceColor', [0.8, 0.2, 0.2], ...
              'EdgeColor', 'none');
end

plot(start_pos(1), start_pos(2), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal_pos(1), goal_pos(2), 'g*', 'MarkerSize', 15, 'LineWidth', 2);
plot(path(:,1), path(:,2), 'b-', 'LineWidth', 2);

%% 势场可视化函数
function visualize_potential_field(start_pos, goal_pos, obstacles, k_att, k_rep, d0)
    % 创建网格
    [X, Y] = meshgrid(-2:0.5:12, -2:0.5:12);
    U_att = zeros(size(X));
    U_rep = zeros(size(X));
    
    % 计算每个点的势场值
    for i = 1:size(X,1)
        for j = 1:size(X,2)
            pos = [X(i,j), Y(i,j)];
            
            % 引力势
            U_att(i,j) = 0.5 * k_att * norm(pos - goal_pos)^2;
            
            % 斥力势
            for k = 1:size(obstacles, 1)
                obs_pos = obstacles(k, 1:2);
                obs_radius = obstacles(k, 3);
                dist = norm(pos - obs_pos);
                
                if dist < (obs_radius + d0)
                    U_rep(i,j) = U_rep(i,j) + 0.5 * k_rep * ...
                                (1/(dist - obs_radius) - 1/d0)^2;
                end
            end
        end
    end
    
    % 总势场
    U_total = U_att + U_rep;
    
    % 绘制势场
    figure;
    surf(X, Y, U_total);
    xlabel('X');
    ylabel('Y');
    zlabel('势场值');
    title('人工势场分布');
    colorbar;
    
    figure;
    contourf(X, Y, U_total, 20);
    hold on;
    plot(start_pos(1), start_pos(2), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
    plot(goal_pos(1), goal_pos(2), 'g*', 'MarkerSize', 15, 'LineWidth', 2);
    for i = 1:size(obstacles, 1)
        rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
                              obstacles(i,2)-obstacles(i,3), ...
                              obstacles(i,3)*2, obstacles(i,3)*2], ...
                  'Curvature', [1,1], 'FaceColor', [0.8, 0.2, 0.2]);
    end
    title('势场等高线图');
    colorbar;
end

% 调用势场可视化
visualize_potential_field(start_pos, goal_pos, obstacles, k_att, k_rep, d0);

3. 改进版本：解决局部最小值问题

%% 改进的人工势场法（加入虚拟目标点）
clear all; close all; clc;

%% 参数设置
start_pos = [0, 0];
goal_pos = [10, 10];
obstacles = [
    4, 4, 1;
    6, 6, 1.5;
    5, 8, 1
];

k_att = 1.0;
k_rep = 2.5;
d0 = 2.5;
step_size = 0.08;
max_iter = 1000;

%% 路径规划
current_pos = start_pos;
path = current_pos;
virtual_goal = goal_pos;  % 虚拟目标点

figure;
hold on;
axis equal;
grid on;
xlim([-1, 12]);
ylim([-1, 12]);

% 绘制环境
for i = 1:size(obstacles, 1)
    rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
                          obstacles(i,2)-obstacles(i,3), ...
                          obstacles(i,3)*2, obstacles(i,3)*2], ...
              'Curvature', [1,1], 'FaceColor', [0.8, 0.2, 0.2]);
end

plot(start_pos(1), start_pos(2), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal_pos(1), goal_pos(2), 'g*', 'MarkerSize', 15, 'LineWidth', 2);

% 局部最小值检测参数
stuck_counter = 0;
stuck_threshold = 20;
oscillation_counter = 0;

for iter = 1:max_iter
    % 检查是否到达目标
    if norm(current_pos - goal_pos) < 0.5
        disp('到达目标点！');
        break;
    end
    
    % 计算受力
    [F_att, F_rep] = calculate_forces(current_pos, virtual_goal, ...
                                      obstacles, k_att, k_rep, d0);
    F_total = F_att + F_rep;
    
    % 处理局部最小值
    if norm(F_total) < 0.1
        stuck_counter = stuck_counter + 1;
        if stuck_counter > stuck_threshold
            % 切换到逃逸模式
            disp('检测到局部最小值，启动逃逸策略');
            
            % 随机扰动
            F_random = 0.3 * (rand(1,2) - 0.5);
            F_total = F_total + F_random;
            
            % 创建虚拟目标点
            virtual_goal = create_virtual_goal(current_pos, goal_pos, obstacles);
            
            stuck_counter = 0;
        end
    else
        stuck_counter = 0;
        virtual_goal = goal_pos;  % 重置虚拟目标
    end
    
    % 更新位置
    if norm(F_total) > 0
        direction = F_total / norm(F_total);
    else
        direction = [0, 0];
    end
    
    next_pos = current_pos + step_size * direction;
    path = [path; next_pos];
    current_pos = next_pos;
    
    % 绘制
    plot(path(:,1), path(:,2), 'b-', 'LineWidth', 1.5);
    plot(current_pos(1), current_pos(2), 'ro', 'MarkerSize', 6);
    
    if mod(iter, 10) == 0
        drawnow;
    end
end

%% 函数定义
function [F_att, F_rep] = calculate_forces(pos, goal, obstacles, k_att, k_rep, d0)
    % 计算引力
    F_att = k_att * (goal - pos);
    
    % 计算斥力
    F_rep = [0, 0];
    for i = 1:size(obstacles, 1)
        obs_pos = obstacles(i, 1:2);
        obs_radius = obstacles(i, 3);
        dist = norm(pos - obs_pos);
        
        if dist < (obs_radius + d0) && dist > obs_radius
            % 斥力方向
            dir_rep = (pos - obs_pos) / dist;
            
            % 斥力大小
            if dist > obs_radius
                rep_mag = k_rep * (1/(dist - obs_radius) - 1/d0) * ...
                         (1/(dist - obs_radius)^2);
                F_rep = F_rep + rep_mag * dir_rep;
            end
        end
    end
end

function virtual_goal = create_virtual_goal(current_pos, real_goal, obstacles)
    % 创建虚拟目标点来逃逸局部最小值
    angle = atan2(real_goal(2)-current_pos(2), real_goal(1)-current_pos(1));
    
    % 尝试不同方向的虚拟目标
    best_angle = angle;
    min_obstacle_dist = 0;
    
    for delta = [-pi/2, -pi/4, 0, pi/4, pi/2]
        test_angle = angle + delta;
        test_point = current_pos + 3 * [cos(test_angle), sin(test_angle)];
        
        % 计算到最近障碍物的距离
        min_dist = inf;
        for i = 1:size(obstacles, 1)
            dist = norm(test_point - obstacles(i,1:2)) - obstacles(i,3);
            if dist < min_dist
                min_dist = dist;
            end
        end
        
        if min_dist > min_obstacle_dist
            min_obstacle_dist = min_dist;
            best_angle = test_angle;
        end
    end
    
    virtual_goal = current_pos + 5 * [cos(best_angle), sin(best_angle)];
end

4. 关键要点

优点：

1. 实时性好：计算简单，适合实时应用
2. 平滑路径：生成的路径自然平滑
3. 实现简单：数学模型清晰，易于实现

缺点：

1. 局部最小值：容易陷入局部最小值
2. 震荡问题：狭窄通道中可能出现震荡
3. 目标不可达：目标点附近有障碍物时可能无法到达

参考代码 基于人工势场法的路径规划问题 www.3dddown.com/cnb/96883.html

参数调优建议：

1. 引力系数k_att：影响收敛速度，过小导致收敛慢，过大导致震荡
2. 斥力系数k_rep：决定避障能力，过小无法避开障碍物，过大可能绕过障碍物
3. 影响距离d0：决定障碍物影响范围
4. 步长step_size：影响路径平滑度和计算速度

5. 应用扩展

可以进一步扩展：

• 结合全局路径规划（如A*算法）
• 加入速度势场考虑动态障碍物
• 三维空间路径规划
• 多机器人协调路径规划