【路径规划】【多种算法比较】基于人工势场 （APF） 算法、涡旋人工势场算法、安全人工势场算法和动态窗口方法的路径规划研究（Matlab代码实现）

 💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

---

💥1 概述

本文研究：【路径规划】【多种算法比较】基于人工势场 （APF） 算法、Vortex APF 算法、Safe APF 算法的路径规划研究

以下局部路径规划算法的比较：

• 人工势场算法 [1]
• 涡旋人工势场算法[2]，
• 安全人工势场算法（论文审稿），
• 动态窗口方法[3]。

有障碍物的环境是随机生成的。

在路径规划的研究领域中，有几种不同的局部路径规划算法被广泛应用，包括基于人工势场（APF）算法、Vortex APF算法和Safe APF算法。这些算法被设计用于在具有障碍物的环境中找到最佳路径。

人工势场算法是一种常见的路径规划算法，它基于对机器人和障碍物之间施加吸引力和排斥力的模拟。机器人受到吸引力来靠近目标点，同时受到障碍物的排斥力来避免碰撞。这种算法简单直观，易于实现，但在某些情况下可能会出现局部最小值问题，导致机器人陷入无法找到全局最优路径的困境。

Vortex APF算法是对人工势场算法的改进，它引入了涡旋势场的概念，以更好地处理局部最小值问题。涡旋势场可以帮助机器人摆脱陷入局部最小值的困境，使其能够找到更优的路径。这种算法在复杂环境中表现出色，但仍然存在一些挑战，例如对参数选择的敏感性以及计算复杂性的增加。

Safe APF算法是一种新的路径规划算法，它综合了安全性和效率的考虑。该算法引入了一种评估安全性的指标，以避免机器人与障碍物之间的碰撞。与传统的人工势场算法相比，Safe APF算法可以更好地在复杂环境中规划安全的路径。然而，该算法仍处于研究阶段，需要进一步的实验和验证。

此外，动态窗口方法是另一种常见的局部路径规划算法，它根据机器人的运动能力和环境的动态变化来调整路径规划策略。该方法通过定义一个能够保证机器人正常运动的窗口，来选择合适的路径。这种算法适用于动态环境和高速移动的机器人，但对机器人运动模型和环境的建模要求较高。

需要注意的是，在以上比较中提到的有障碍物的环境是随机生成的，这是为了模拟真实世界中的复杂环境，并测试各种路径规划算法的性能。这些算法的具体表现取决于算法的实现和参数设置，因此在实际应用中，需要结合具体情况选择最适合的路径规划算法。

📚2 运行结果

部分代码：

while norm([x_goal y_goal] - [x y]) > position_accuracy && t < simTimeMax  
        tic;
    
        % Calculate Attractive Potential
        if norm([x y]-[x_goal y_goal]) <= VAPF.dstar
            nablaU_att =  VAPF.zeta*([x y]-[x_goal y_goal]);
        else 
            nablaU_att = VAPF.dstar/norm([x y]-[x_goal y_goal]) * VAPF.zeta*([x y]-[x_goal y_goal]);
        end
    
        % Find the minimum distance from the obstacle & Calculate Repulsive Potential
        obst_idx = zeros(1,Obstacle_count);
        obst_dist = zeros(1,Obstacle_count);
        nablaU_obst = [0 0];
        for i=1:Obstacle_count
            [obst_idx(i), obst_dist(i)] = dsearchn([obstacle(i,:,1)' obstacle(i,:,2)'], [x y]);
            obst_dist(i) = obst_dist(i) + 0.01*(2*rand()-1); % MEASUREMENT NOISE
            alpha = theta - atan2(obstacle(i,obst_idx(i),2)-y, obstacle(i,obst_idx(i),1)-x);
            alpha = atan2(sin(alpha), cos(alpha));
            if obst_dist(i) <= VAPF.Qstar &&  abs(alpha) < deg2rad(150)
                nablaU_rep_Oi = (VAPF.eta*(1/VAPF.Qstar - 1/obst_dist(i)) * 1/obst_dist(i)^2)*([x y] - [obstacle(i,obst_idx(i),1) obstacle(i,obst_idx(i),2)]);
                
                if isVortex == false 
                    if t > 10
                        if norm( [VAPF.X(t-10) VAPF.Y(t-10)] - [x y]) < 0.02
                            lastSign = lastSign*-1;
                            isVortex = true;
                        end
                    end
                else
                    goalTheta = atan2(y_goal-y, x_goal-x) - theta;
                    if abs(goalTheta) < deg2rad(10)
                        isVortex = false;
                    end
                end

                gamma = 0;
                if isVortex, gamma = lastSign*pi/2; end

                R = [cos(gamma) -sin(gamma)
                     sin(gamma)  cos(gamma) ];

                nablaU_obst = nablaU_obst + (R*nablaU_rep_Oi')';
            end
        end
        
        % Calculate final potential
        nablaU = nablaU_att+nablaU_obst;
    
        % Calculate reference value of linear velocity (v_ref) and orientation (theta_ref)
        theta_ref = atan2(-nablaU(2), -nablaU(1));
    
        error_theta = theta_ref - theta;
        if abs(error_theta) <= VAPF.error_theta_max
            alpha = (VAPF.error_theta_max - abs(error_theta)) / VAPF.error_theta_max;
            v_ref = min( alpha*norm(-nablaU), VAPF.v_max );
        else
            v_ref = 0;
        end
    
        t_i = toc;
        t_max = max(t_max, t_i);
        t_min = min(t_min, t_i);
        t_sum = t_sum + t_i;
        t_count = t_count + 1;
    
        % Simple kinematic mobile robot model
        % Omitted dynamics.
        omega_ref = VAPF.Kp_omega * error_theta;
        omega_ref = min( max(omega_ref, -VAPF.omega_max), VAPF.omega_max);
        theta = theta + omega_ref * dT;
        theta = atan2(sin(theta), cos(theta));
        x = x + v_ref*cos(theta) * dT;
        y = y + v_ref*sin(theta) * dT;
    
        % Archive and plot it
        t = t + 1;
        VAPF.X(t) = x;
        VAPF.Y(t) = y;
        VAPF.Theta(t) = theta;
    end

    VAPF.t = t;
    VAPF.travelTime = (t-1)*dT;
    VAPF.MeanCalculationTime = t_sum/t_count;
    VAPF.MaxCalculationTime = t_max;
    VAPF.MinCalculationTime = t_min;
end

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in Proceedings. 1985 IEEE International Conference on Robotics and Automation, vol. 2. IEEE, 1985, pp. 500–505

[2] X. Yun and K.-C. Tan, “A wall-following method for escaping local minima in potential field based motion planning,” in 1997 8th International Conference on Advanced Robotics. Proceedings. ICAR’97. IEEE, 1997, pp. 421–426

[3]D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to collision avoidance,” IEEE Robotics & Automation Magazine, vol. 4, no. 1, pp. 23–33, 1997.

🌈4 Matlab代码实现