穿越稠密障碍物的自适应动态窗口法（Matlab代码实现）

       目录

💥1 概述

📚2 运行结果

🎉3 参考文献

👨‍💻4 Matlab代码

---

💥1 概述

​  针对应用广泛的局部避障算法—–动态窗口法(DWA)穿越稠密障碍物时存在路径不合理、速度和安全性不能兼顾等问题,提出参数自适应的DWA算法,根据机器人与障碍物距离和障碍物的密集度自动调整目标函数中的权值,以自适应环境的动态变化,从而获得移动机器人的最佳运行速度和合理路径.该方法可明显改善机器人穿越稠密障碍物区域时的性能;同时,该方法还可避免机器人从密集障碍物区域外绕行以及轨迹不平滑现象.仿真实验表明:改进的DWA算法在复杂环境中通过逐步优化可使运行轨迹更加合理,能够同时兼顾路径平滑性和安全性;机器人在离稠密障碍物较远处保持高速,通过狭窄通道或者稠密障碍物区域时速度适当降低,安全性更高,实验中总迭代次数和运行时间可缩短20%以上.

📚2 运行结果

主函数部分代码：

close all;

clear all;

disp('Dynamic Window Approach sample program start!!')

% Initial state of the robot [x (m), y (m), yaw (Rad), v (m / s), w (rad / s)]

x=[0 0 pi/2 0 0]';

% Target point position [x (m), y (m)]

goal=[7,8];

% Obstacle position list [x (m) y (m)]

obstacle=[0 2;

          4 2;

          4 4;

          5 4;

          5 5;

          5 6;

          5 9

          8 8

          8 9

          7 9

          6 5

          6 3

          6 8

          6 7

          7 4

          9 8

          9 11

          9 6];

      

% Obstacle radius for collision detection

obstacleR=0.3;

% Time [s]

global dt; dt=0.1;

% Kinematics model

% Maximum speed m / s], maximum rotation speed [rad / s], acceleration [m / ss], rotation acceleration [rad / ss],

% Speed resolution rate [m / s], rotation speed resolution rate [rad / s]]

Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];

% Parameter number reference [heading, dist, velocity, predictDT]

evalParam=[0.05,0.2,0.1,3.0];

% area of the environment

area=[-1 11 -1 11];

% Imitation experiment

result.x=[];

tic;

% movcount=0;

% Main loop

for i=1:5000

    % DWA 

    [u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);

    x=f(x,u); % The robot moves to the next moment

    

    % save the simulation results

    result.x=[result.x; x'];

    

    % when reach the destination

    if norm(x(1:2)-goal')<0.5

        disp('Arrive Goal!!');break;

    end

    

    %====Animation====

    hold off;

    ArrowLength=0.5;% 

    

    % Robot

    quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on;

    plot(result.x(:,1),result.x(:,2),'-b');hold on;

    plot(goal(1),goal(2),'*r');hold on;

    plot(obstacle(:,1),obstacle(:,2),'*k');hold on;

    

    % Explore the track

    if ~isempty(traj)

        for it=1:length(traj(:,1))/5

            ind=1+(it-1)*5;

            plot(traj(ind,:),traj(ind+1,:),'-g');hold on;

        end

🎉3 参考文献

[1]程传奇,郝向阳,李建胜等.融合改进A~*算法和动态窗口法的全局动态路径规划[J].西安交通大学学报,2017,51(11):137-143.