【路径规划】基于人工势场（APF）算法、Vortex APF算法、Safe APF算法和动态 Windows 方法的比较

摘要

本文对比了人工势场（APF）算法、Vortex APF（VAPF）算法、Safe APF（SAPF）算法以及动态窗口（DWA）方法在机器人路径规划中的性能。通过实验，评估了各个算法在不同障碍环境中的表现，尤其是在避障和路径优化方面的差异。实验结果表明，VAPF和SAPF在解决传统APF算法的局部极小值问题上表现优异，而DWA在实时路径规划中具有较好的综合性能。

理论

• 人工势场（APF）算法：基于人工势场算法，机器人在目标点生成吸引力，在障碍物处生成排斥力。通过力的合成引导机器人向目标移动。然而，该算法容易陷入局部极小值，即机器人被困在非目标点的区域。

• Vortex APF（VAPF）算法：该算法通过引入旋涡场，改进了传统APF的局部极小值问题。机器人不仅受到吸引力和排斥力的影响，还在合适的情况下受到旋转力场的引导，使其更容易逃脱局部极小值区域。

• Safe APF（SAPF）算法：SAPF算法通过动态调整障碍物周围的排斥力强度，确保机器人在复杂障碍环境中能够安全通行，减少了机器人和障碍物碰撞的风险。

• 动态窗口（DWA）方法：DWA是一种基于速度和加速度的实时路径规划方法。该算法通过评估不同速度和角速度的可行性，找到最优路径，从而实现实时避障和路径规划。DWA方法适用于动态环境，且在计算资源有限的情况下表现优异。

实验结果

在多个复杂环境下，对比了各算法的表现。下图展示了四种算法的路径规划结果：

• DWA（橙色）：DWA路径较为平滑，避障效果明显，尤其适合实时路径规划。

• APF（蓝色）：APF算法在简单环境中效果较好，但在局部极小值问题上仍有缺陷。

• VAPF（绿色）：VAPF通过旋涡场改进了局部极小值问题，在复杂环境中表现优异。

• SAPF（红色）：SAPF算法在障碍物密集的情况下有更好的避障效果，确保了路径的安全性。

部分代码

1. 人工势场算法（APF）:

function apf_path = APF(start, goal, obstacles)
    % 初始化参数
    k_att = 1.0;  % 吸引力系数
    k_rep = 100.0; % 排斥力系数
    d0 = 2.0;     % 排斥力的作用范围
    max_step = 1000;
    apf_path = start;
    
    for i = 1:max_step
        pos = apf_path(end, :);
        f_att = k_att * (goal - pos);  % 吸引力
        f_rep = [0, 0]; % 初始化排斥力
        
        for j = 1:size(obstacles, 1)
            dist = norm(pos - obstacles(j, :));
            if dist < d0
                f_rep = f_rep + k_rep * (1/dist - 1/d0) * (pos - obstacles(j, :)) / dist^3;
            end
        end
        
        f_total = f_att + f_rep;  % 合力
        new_pos = pos + f_total;  % 更新位置
        apf_path = [apf_path; new_pos];
        
        if norm(goal - new_pos) < 0.1  % 如果接近目标点，结束
            break;
        end
    end
end

1. Vortex APF算法（VAPF）：

function vapf_path = VAPF(start, goal, obstacles)
    % 初始化参数
    k_att = 1.0;  % 吸引力系数
    k_rep = 100.0; % 排斥力系数
    k_vortex = 10.0; % 旋涡力系数
    d0 = 2.0;     % 排斥力的作用范围
    max_step = 1000;
    vapf_path = start;
    
    for i = 1:max_step
        pos = vapf_path(end, :);
        f_att = k_att * (goal - pos);  % 吸引力
        f_rep = [0, 0]; % 初始化排斥力
        f_vortex = [0, 0]; % 初始化旋涡力
        
        for j = 1:size(obstacles, 1)
            dist = norm(pos - obstacles(j, :));
            if dist < d0
                f_rep = f_rep + k_rep * (1/dist - 1/d0) * (pos - obstacles(j, :)) / dist^3;
                f_vortex = f_vortex + k_vortex * [-(pos(2) - obstacles(j, 2)), pos(1) - obstacles(j, 1)] / dist^2;
            end
        end
        
        f_total = f_att + f_rep + f_vortex;  % 合力
        new_pos = pos + f_total;  % 更新位置
        vapf_path = [vapf_path; new_pos];
        
        if norm(goal - new_pos) < 0.1  % 如果接近目标点，结束
            break;
        end
    end
end

1. Safe APF算法（SAPF）：

function sapf_path = SAPF(start, goal, obstacles)
    % 初始化参数
    k_att = 1.0;  % 吸引力系数
    k_rep = 100.0; % 排斥力系数
    d0 = 2.0;     % 排斥力的作用范围
    max_step = 1000;
    sapf_path = start;
    
    for i = 1:max_step
        pos = sapf_path(end, :);
        f_att = k_att * (goal - pos);  % 吸引力
        f_rep = [0, 0]; % 初始化排斥力
        
        for j = 1:size(obstacles, 1)
            dist = norm(pos - obstacles(j, :));
            if dist < d0
                % 动态调整排斥力
                safe_dist = max(0.1, dist);
                f_rep = f_rep + k_rep * (1/safe_dist - 1/d0) * (pos - obstacles(j, :)) / safe_dist^3;
            end
        end
        
        f_total = f_att + f_rep;  % 合力
        new_pos = pos + f_total;  % 更新位置
        sapf_path = [sapf_path; new_pos];
        
        if norm(goal - new_pos) < 0.1  % 如果接近目标点，结束
            break;
        end
    end
end

1. 动态窗口（DWA）算法的核心伪代码：

function dwa_path = DWA(start, goal, obstacles, max_v, max_w)
    % 初始化
    max_step = 1000;
    dwa_path = start;
    pos = start;
    v = 0; % 初始速度
    w = 0; % 初始角速度
    
    for i = 1:max_step
        [v, w] = calculate_velocity(v, w, pos, goal, obstacles, max_v, max_w);
        pos = update_position(pos, v, w);
        dwa_path = [dwa_path; pos];
        
        if norm(goal - pos(1:2)) < 0.1  % 如果接近目标点，结束
            break;
        end
    end
end

参考文献

❝

1. Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotics Research, 5(1), 90-98.

2. Fox, D., Burgard, W., & Thrun, S. (1997). The dynamic window approach to collision avoidance. IEEE Robotics & Automation Magazine, 4(1), 23-33.

3. Park, J. H., & Oh, J. H. (2003). Vortex field method for local path planning of mobile robots. Journal of Intelligent and Robotic Systems, 37(1), 1-16.

4. Tan, H. S., Zhang, H., & Zhu, Q. (2011). A safe artificial potential field approach for robot path planning. Proceedings of the 2011 International Conference on Information and Automation, 1121-1126.