人工势场法实现无人车避障路径规划

MATLAB的人工势场法无人车避障路径规划实现。该方法通过模拟物理势场（目标点产生吸引力，障碍物产生排斥力）来规划无碰撞路径。

classdef APF_PathPlanner
    %APF_PATHPLANNER 人工势场法路径规划器
    %   实现无人车避障路径规划
    
    properties
        start_pos      % 起点位置 [x, y]
        goal_pos       % 目标位置 [x, y]
        obstacles      % 障碍物列表 [x, y, radius]
        k_att          % 吸引力增益系数
        k_rep          % 排斥力增益系数
        d0             % 障碍物影响距离
        step_size      % 路径规划步长
        max_iter       % 最大迭代次数
        goal_threshold % 目标点到达阈值
        path           % 规划路径
    end
    
    methods
        function obj = APF_PathPlanner(start, goal, obs, varargin)
            % 构造函数
            obj.start_pos = start;
            obj.goal_pos = goal;
            obj.obstacles = obs;
            
            % 设置默认参数
            p = inputParser;
            addParameter(p, 'k_att', 1.0, @isnumeric);
            addParameter(p, 'k_rep', 100.0, @isnumeric);
            addParameter(p, 'd0', 5.0, @isnumeric);
            addParameter(p, 'step_size', 0.5, @isnumeric);
            addParameter(p, 'max_iter', 1000, @isnumeric);
            addParameter(p, 'goal_threshold', 0.5, @isnumeric);
            parse(p, varargin{:});
            
            obj.k_att = p.Results.k_att;
            obj.k_rep = p.Results.k_rep;
            obj.d0 = p.Results.d0;
            obj.step_size = p.Results.step_size;
            obj.max_iter = p.Results.max_iter;
            obj.goal_threshold = p.Results.goal_threshold;
            obj.path = [];
        end
        
        function F_att = attractive_force(obj, pos)
            % 计算吸引力
            % pos: 当前位置 [x, y]
            vec_to_goal = obj.goal_pos - pos;
            dist_to_goal = norm(vec_to_goal);
            
            if dist_to_goal <= obj.goal_threshold
                F_att = [0, 0];  % 已经到达目标点
            else
                F_att = obj.k_att * vec_to_goal;
            end
        end
        
        function F_rep = repulsive_force(obj, pos)
            % 计算排斥力
            % pos: 当前位置 [x, y]
            F_rep = [0, 0];
            
            for i = 1:size(obj.obstacles, 1)
                obs = obj.obstacles(i, :);
                vec_to_obs = pos - obs(1:2);
                dist_to_obs = norm(vec_to_obs) - obs(3);  % 考虑障碍物半径
                
                if dist_to_obs <= 0
                    % 与障碍物碰撞
                    F_rep = [NaN, NaN];
                    return;
                elseif dist_to_obs < obj.d0
                    % 计算排斥力分量
                    rep_mag = obj.k_rep * (1/dist_to_obs - 1/obj.d0) * ...
                              (1/dist_to_obs^2);
                    F_rep = F_rep + rep_mag * (vec_to_obs / norm(vec_to_obs));
                end
            end
        end
        
        function total_force = total_force(obj, pos)
            % 计算合力
            F_att = obj.attractive_force(pos);
            F_rep = obj.repulsive_force(pos);
            
            if any(isnan(F_rep))
                total_force = [NaN, NaN];  % 发生碰撞
            else
                total_force = F_att + F_rep;
            end
        end
        
        function [path, success] = plan_path(obj)
            % 执行路径规划
            current_pos = obj.start_pos;
            path = current_pos;
            iter = 0;
            success = false;
            
            while iter < obj.max_iter
                % 计算合力
                F_total = obj.total_force(current_pos);
                
                % 检查是否发生碰撞
                if any(isnan(F_total))
                    fprintf('与障碍物发生碰撞在迭代 %d\n', iter);
                    break;
                end
                
                % 计算单位方向向量
                F_dir = F_total / norm(F_total);
                
                % 更新位置
                new_pos = current_pos + obj.step_size * F_dir;
                path = [path; new_pos];
                
                % 检查是否到达目标
                if norm(new_pos - obj.goal_pos) <= obj.goal_threshold
                    success = true;
                    fprintf('成功到达目标点在 %d 次迭代\n', iter);
                    break;
                end
                
                current_pos = new_pos;
                iter = iter + 1;
            end
            
            if iter >= obj.max_iter
                fprintf('达到最大迭代次数 %d\n', obj.max_iter);
            end
            
            obj.path = path;
        end
        
        function plot_environment(obj)
            % 绘制环境
            figure;
            hold on;
            grid on;
            axis equal;
            
            % 绘制起点和目标点
            plot(obj.start_pos(1), obj.start_pos(2), 'go', 'MarkerSize', 10, 'LineWidth', 2);
            plot(obj.goal_pos(1), obj.goal_pos(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
            
            % 绘制障碍物
            for i = 1:size(obj.obstacles, 1)
                obs = obj.obstacles(i, :);
                rectangle('Position', [obs(1)-obs(3), obs(2)-obs(3), 2*obs(3), 2*obs(3)], ...
                          'Curvature', [1,1], 'FaceColor', [0.8, 0.2, 0.2], 'EdgeColor', 'none');
                
                % 绘制影响范围
                viscircles([obs(1), obs(2)], obj.d0 + obs(3), 'Color', [0.9, 0.9, 0.9], 'LineStyle', '--');
            end
            
            % 绘制路径
            if ~isempty(obj.path)
                plot(obj.path(:,1), obj.path(:,2), 'b-', 'LineWidth', 1.5);
                plot(obj.path(:,1), obj.path(:,2), 'b.', 'MarkerSize', 10);
            end
            
            title('人工势场法路径规划');
            xlabel('X (m)');
            ylabel('Y (m)');
            legend('起点', '目标', 'Location', 'Best');
            hold off;
        end
    end
end

使用

% 创建路径规划场景
start = [0, 0];          % 起点坐标
goal = [20, 20];         % 目标点坐标

% 定义障碍物 [x, y, 半径]
obstacles = [...
    5, 5, 2; ...
    8, 12, 1.5; ...
    12, 8, 1.5; ...
    15, 15, 2; ...
    10, 15, 1; ...
    15, 10, 1];

% 创建路径规划器对象
planner = APF_PathPlanner(start, goal, obstacles, ...
    'k_att', 1.0, ...    % 吸引力增益
    'k_rep', 100.0, ...  % 排斥力增益
    'd0', 3.0, ...       % 障碍物影响距离
    'step_size', 0.3, ...% 步长
    'max_iter', 500);    % 最大迭代次数

% 执行路径规划
[path, success] = planner.plan_path();

% 可视化结果
planner.plot_environment();

参考代码 使用人工势场法实现避障的无人车路径规划 www.youwenfan.com/contentcnd/97512.html

算法原理说明

1. 人工势场法基本原理

人工势场法模拟物理场中的势能力：

• 吸引势场：目标点产生吸引力，引导车辆向目标移动
• 排斥势场：障碍物产生排斥力，防止车辆与障碍物碰撞

合力公式：

\[\vec{F}_{total} = \vec{F}_{att} + \vec{F}_{rep} \]

2. 吸引力计算

吸引力与当前位置到目标的距离成正比：

\[\vec{F}_{att} = k_{att} \cdot (\vec{x}_{goal} - \vec{x}) \]

3. 排斥力计算

当车辆与障碍物的距离小于影响范围 \(d_0\) 时产生排斥力：

\[\vec{F}_{rep} = \begin{cases} k_{rep} \left( \frac{1}{d} - \frac{1}{d_0} \right) \frac{1}{d^2} \frac{\vec{x} - \vec{x}_{obs}}{d} & d < d_0 \\ 0 & d \geq d_0 \end{cases}\]

其中 \(d\) 是车辆到障碍物边缘的距离（考虑障碍物半径）。

4. 参数调节建议

• k_att：吸引力增益系数（典型值0.5-2.0）
  • 值过小：收敛慢，路径长
  • 值过大：可能震荡或越过障碍物
• k_rep：排斥力增益系数（典型值50-200）
  • 值过小：可能无法避开障碍物
  • 值过大：可能导致目标不可达
• d0：障碍物影响距离（典型值2-5米）
  • 值过小：避障反应迟
  • 值过大：路径可能过于保守
• step_size：路径点步长（典型值0.1-0.5米）
  • 值过小：计算效率低
  • 值过大：可能跳过障碍物