精华内容
下载资源
问答
  • 动态窗口法实现二维路径规划,可以设置圆形动/静态障碍物
  • dwa动态窗口算法代码

    2017-11-15 10:16:07
    dwa动态窗口算法在MATLAB中的仿真代码,有需要的下载。。。。。
  • 动态窗口法,可用于局部轨迹规划。手动设置运动学约束和动力学约束信息。
  • 机器人局部避障的动态窗口法DWA动态障碍物避障效果,障碍物凸显,可以任意改变起点、终点和障碍物位置数量、出现时间
  • 动态窗口法进行机器人避障,直接复制粘贴打开matlab就可以运行。
  • 针对应用广泛的局部避障算法-----动态窗口法(DWA)穿越稠密障碍物时存在路径不合理、速度和安全性不能兼顾等问题,提出参数自适应的DWA算法,根据机器人与障碍物距离和障碍物的密集度自动调整目标函数中的权值,以自适应...
  • 动态窗口算法(DWA)

    千次阅读 多人点赞 2019-10-11 09:40:09
    路径规划算法中动态窗口算法十分灵活好用,但发现在自动驾驶中几乎没有应用,原因大概就是...那么,动态窗口法核心的动态窗口,其实就是将机器人当前状态(即当前速度,航向角)及机器人运动模型(机器人所能达到的...

    路径规划算法中动态窗口算法十分灵活好用,但发现在自动驾驶中几乎没有应用,原因大概就是其难于复杂的环境,日后将对其进行详细的调研,今天先记录一下其原理及过程。
    一、DWA原理
    仔细想想,机器人的运动状态,包括其不停变换的位置及运动方向,实际上是由其当前的运动速度及角速度(转向速度)决定的。那么,动态窗口法核心的动态窗口,其实就是将机器人当前状态(即当前速度,航向角)及机器人运动模型(机器人所能达到的最大速度,最大角速度,加速度,旋转加速度)计算出当前机器人的最大最小速度及角速度,以此作为一个限定范围,这个范围就是窗口。在此范围中计算每个速度及角速度下所能到达到的位置,在对每个位置进行测评(测评内容包括据障碍物的距离,朝向终点的角度等),由此选出当前的最佳位置,然后再由这个最佳位置继续重复以上过程建立新的窗口,这样窗口就动起来了,即所谓的动态窗口。
    二、实现过程
    之后将我找到的动态窗口法的一个用matlab实现的demo贴出来,可以跟据这个小历程来更清晰明了的了解一下动态窗口法,其中很多地方做了解释,然后再将具体流程写于下面。

    % -------------------------------------------------------------------------
    %
    % File : DynamicWindowApproachSample.m
    %
    % Discription : Mobile Robot Motion Planning with Dynamic Window Approach
    %
    % Environment : Matlab
    %
    % Author : Atsushi Sakai
    %
    % Copyright (c): 2014 Atsushi Sakai
    %
    % License : Modified BSD Software License Agreement
    % log: 20181031 增加详细的注释信息 下标的定义
    % 20181101 :增加画出障碍物的大小,更直观的看到障碍物和机器人之间的位置关系
    % -------------------------------------------------------------------------
     
    
    
    
    
    function [] = DynamicWindowApproachSample()
    
    close all;
    clear all;
    
    disp('Dynamic Window Approach sample program start!!') %显示数组
    
    %% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
    % x=[0 0 pi/2 0 0]'; % 5x1矩阵 列矩阵  位置 0,0 航向 pi/2 ,速度、角速度均为0
    x = [0 0 pi/10 0 0]'; 
    
    % 下标宏定义 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
    POSE_X      = 1;  %坐标 X
    POSE_Y      = 2;  %坐标 Y
    YAW_ANGLE   = 3;  %机器人航向角
    V_SPD       = 4;  %机器人速度
    W_ANGLE_SPD = 5;  %机器人角速度 
    
    goal = [10,10];   % 目标点位置 [x(m),y(m)]
    
    % 障碍物位置列表 [x(m) y(m)]
    obstacle=[0 2;
              2 4;
              2 5;      
              4 2;
    %           4 4;
              5 4;
    %           5 5;
              5 6;
              5 9
              8 8
              8 9
              7 9];
    % 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];
          
    obstacleR = 0.5;% 冲突判定用的障碍物半径
    global dt; 
    dt = 0.1;% 时间[s]
    
    % 机器人运动学模型参数
    % 最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],速度分辨率[m/s],转速分辨率[rad/s]]
    Kinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];%toRadian角度转弧度,radian = degree/180*pi;
    %定义Kinematic的下标含义
    MD_MAX_V    = 1; %   最高速度[m/s]
    MD_MAX_W    = 2; %   最高旋转速度[rad/s]
    MD_ACC      = 3; %   加速度[m/ss]
    MD_VW       = 4; %   旋转加速度[rad/ss]
    MD_V_RESOLUTION  = 5; %  速度分辨率[m/s]
    MD_W_RESOLUTION  = 6; %  转速分辨率[rad/s]]
    
    
    % 评价函数参数 [heading,dist,velocity,predictDT]
    % 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间
    evalParam = [0.05, 0.2 ,0.1, 3.0];
    
    area      = [-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax]
    
    % 模拟实验的结果
    result.x=[];   %定义一个数组,累积存储走过的轨迹点的状态值 
    tic;  % 估算程序运行时间开始
    
    % movcount=0;
    %% Main loop   循环运行 5000次 指导达到目的地 或者 5000次运行结束
    for i = 1:5000  %最多5000个循环到达目标点,每个循环作为一个计算阶段,得到一小段路
        %  返回控制量 u = [v(m/s),w(rad/s)] 和 traj 该阶段的一小段轨迹
        [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
        x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度
        
        % 历史轨迹的保存
        result.x = [result.x; x'];  %最新结果 以列的形式 添加到result.x%?????x中只有列???x开始到底是行还是列?????
        
        % 是否到达目的地
        if norm(x(POSE_X:POSE_Y)-goal')<0.5   % norm函数来求得坐标上的两个点之间的距离
            disp('Arrive Goal!!');break;
        end
        
        %====Animation====
        hold off;               % 关闭图形保持功能。 新图出现时,取消原图的显示。
        ArrowLength = 0.5;      % 箭头长度
        
        % 机器人
        % quiver(x,y,u,v) 在 x 和 y 中每个对应元素对组所指定的坐标处将向量绘制为箭头
        quiver(x(POSE_X), x(POSE_Y), ArrowLength*cos(x(YAW_ANGLE)), ArrowLength*sin(x(YAW_ANGLE)), 'ok'); % 绘制机器人当前位置的航向箭头
        hold on;                                                     %启动图形保持功能,当前坐标轴和图形都将保持,从此绘制的图形都将添加在这个图形的基础上,并自动调整坐标轴的范围
        
        plot(result.x(:,POSE_X),result.x(:,POSE_Y),'-b');
        hold on;    % 绘制走过的所有位置 所有历史数据的 X、Y坐标
        plot(goal(1),goal(2),'*r');
        hold on;                          % 绘制目标位置
        
        %plot(obstacle(:,1),obstacle(:,2),'*k');hold on;              % 绘制所有障碍物位置
        DrawObstacle_plot(obstacle,obstacleR);
        
        % 探索轨迹 画出待评价的轨迹
        if ~isempty(traj) %轨迹非空
            for it=1:length(traj(:,1))/5    %计算所有轨迹数  traj 每5行数据 表示一条轨迹点
                ind = 1+(it-1)*5; %第 it 条轨迹对应在traj中的下标 
                plot(traj(ind,:),traj(ind+1,:),'-g');
                hold on;  %根据一条轨迹的点串画出轨迹   traj(ind,:) 表示第ind条轨迹的所有x坐标值  traj(ind+1,:)表示第ind条轨迹的所有y坐标值
            end
        end
        
        axis(area); %根据area设置当前图形的坐标范围,分别为x轴的最小、最大值,y轴的最小最大值
        grid on;
        drawnow;  %刷新屏幕. 当代码执行时间长,需要反复执行plot时,Matlab程序不会马上把图像画到figure上,这时,要想实时看到图像的每一步变化情况,需要使用这个语句。
        %movcount = movcount+1;
        %mov(movcount) = getframe(gcf);%  记录动画帧
    end
    toc  %输出程序运行时间  形式:时间已过 ** 秒。
    %movie2avi(mov,'movie.avi');  %录制过程动画 保存为 movie.avi 文件
    
    %% 绘制所有障碍物位置
    % 输入参数:obstacle 所有障碍物的坐标   obstacleR 障碍物的半径
    function [] = DrawObstacle_plot(obstacle,obstacleR)
    r = obstacleR; 
    theta = 0:pi/20:2*pi;%将圆周分为20个点,下面的循环将20个点依次连接形成圆
    for id=1:length(obstacle(:,1))
     x = r * cos(theta) + obstacle(id,1); 
     y = r  *sin(theta) + obstacle(id,2);
     plot(x,y,'-m');
     hold on; 
    end
    % plot(obstacle(:,1),obstacle(:,2),'*m');
    hold on;              % 绘制所有障碍物位置
        
    
    
    %% DWA算法实现 
    % model  机器人运动学模型  最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], 速度分辨率[m/s],转速分辨率[rad/s]]
    % 输入参数:当前状态、模型参数、目标点、评价函数的参数、障碍物位置、障碍物半径
    % 返回参数:控制量 u = [v(m/s),w(rad/s)] 和 轨迹集合 N * 31  (N:可用的轨迹数)
    % 选取最优参数的物理意义:在局部导航过程中,使得机器人避开障碍物,朝着目标以较快的速度行驶。
    function [u,trajDB] = DynamicWindowApproach(x,model,goal,evalParam,ob,R)
    % Dynamic Window [vmin,vmax,wmin,wmax] 最小速度 最大速度 最小角速度 最大角速度速度
    Vr = CalcDynamicWindow(x,model);  % 根据当前状态 和 运动模型 计算当前的参数允许范围
    
    % 评价函数的计算 evalDB N*5  每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分
    %               trajDB      每5行一条轨迹 每条轨迹都有状态x点串组成
    [evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam);  %evalParam 评价函数参数 [heading,dist,velocity,predictDT]
    
    if isempty(evalDB)
        disp('no path to goal!!');
        u=[0;0];
        return;
    end
    
    % 各评价函数正则化
    evalDB = NormalizeEval(evalDB);%只把距离归一化
    
    % 最终评价函数的计算
    feval=[];
    for id=1:length(evalDB(:,1))
        feval = [feval;evalParam(1:3)*evalDB(id,3:5)']; %返回一个值,根据评价函数参数 前三个参数分配的权重 计算每一组可用的路径参数信息的得分
    end
    evalDB = [evalDB feval]; % 最后一组
     
    [maxv,ind] = max(feval); % 选取评分最高的参数 对应分数返回给 maxv  对应下标返回给 ind
    u = evalDB(ind,1:2)'; % 返回最优参数的速度、角速度  
    
    %% 评价函数 内部负责产生可用轨迹
    % 输入参数 :当前状态、参数允许范围(窗口)、目标点、障碍物位置、障碍物半径、评价函数的参数
    % 返回参数:
    %           evalDB N*5  每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分
    %           trajDB      每5行一条轨迹 每条轨迹包含 前向预测时间/dt + 1 = 31 个轨迹点(见生成轨迹函数)
    function [evalDB,trajDB] = Evaluation(x,Vr,goal,ob,R,model,evalParam)
    evalDB = [];
    trajDB = [];
    for vt = Vr(1):model(5):Vr(2)       %根据速度分辨率遍历所有可用速度: 最小速度和最大速度 之间 速度分辨率 递增 
        for ot=Vr(3):model(6):Vr(4)     %根据角度分辨率遍历所有可用角速度: 最小角速度和最大角速度 之间 角度分辨率 递增  
            % 轨迹推测; 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻 到 预测时刻之间的轨迹(由轨迹点组成)
            [xt,traj] = GenerateTrajectory(x,vt,ot,evalParam(4),model);  %evalParam(4):前向模拟时间;在一个前向模拟时间内生成由30点组成的一条轨迹
            % 各评价函数的计算
            heading = CalcHeadingEval(xt,goal); % 前项预测终点的航向得分  偏差越小分数越高
            dist    = CalcDistEval(xt,ob,R);    % 前项预测终点 距离最近障碍物的间隙得分 距离越远分数越高
            vel     = abs(vt);                  % 速度得分 速度越快分越高
            stopDist = CalcBreakingDist(vel,model); % 返回制动距离
            if dist > stopDist % 如果可能撞到最近的障碍物 则舍弃此路径 (到最近障碍物的距离 大于 刹车距离 才取用)
                evalDB = [evalDB;[vt ot heading dist vel]];
                trajDB = [trajDB;traj];   % 每5行 一条轨迹  
            end
        end
    end
    
    %% 归一化处理 
    % 每一条轨迹的单项得分除以本项所有分数和
    function EvalDB=NormalizeEval(EvalDB)
    % 评价函数正则化
    if sum(EvalDB(:,3))~= 0
        EvalDB(:,3) = EvalDB(:,3)/sum(EvalDB(:,3));  %矩阵的数除  单列矩阵的每元素分别除以本列所有数据的和
    end
    if sum(EvalDB(:,4))~= 0
        EvalDB(:,4) = EvalDB(:,4)/sum(EvalDB(:,4));
    end
    if sum(EvalDB(:,5))~= 0
        EvalDB(:,5) = EvalDB(:,5)/sum(EvalDB(:,5));
    end
    
    %% 单条轨迹生成、轨迹推演函数
    % 输入参数: 当前状态、vt当前速度、ot角速度、evaldt 前向模拟时间、机器人模型参数(没用到)
    % 返回参数; 
    %           x   : 机器人模拟时间内预测的终点状态; 
    %           traj: 当前时刻 到 预测时刻之间 过程中的位姿记录(状态记录) 当前模拟的轨迹  
    %                  轨迹点的个数为 evaldt / dt + 1 = 3.0 / 0.1 + 1 = 31
    %           
    function [x,traj] = GenerateTrajectory(x,vt,ot,evaldt,model)
    global dt;
    time = 0;
    u = [vt;ot];% 输入值
    traj = x;   % 机器人轨迹
    while time <= evaldt   
        time = time+dt; % 时间更新
        x = f(x,u);     % 运动更新 前项模拟时间内 速度、角速度恒定
        traj = [traj x]; % 每一列代表一个轨迹点 一列一列的添加,30个点组成一条轨迹
    end
    
    %% 计算制动距离 
    %根据运动学模型计算制动距离, 也可以考虑成走一段段圆弧的累积 简化可以当一段段小直线的累积
    function stopDist = CalcBreakingDist(vel,model)
    global dt;
    MD_ACC = 3;  % 
    stopDist=0;  % 
    while vel>0  % 给定加速度的条件下 速度减到0所走的距离
        stopDist = stopDist + vel*dt; % 制动距离的计算 
        vel = vel - model(MD_ACC)*dt; % 
    end
    
    %% 障碍物距离评价函数  (机器人在当前轨迹上与最近的障碍物之间的距离,如果没有障碍物则设定一个常数)
    % 输入参数:位姿、所有障碍物位置、障碍物半径
    % 输出参数:当前预测的轨迹终点的位姿距离所有障碍物中最近的障碍物的距离 如果大于设定的最大值则等于最大值
    % 距离障碍物距离越近分数越低
    function dist = CalcDistEval(x,ob,R)
    dist=100;
    for io = 1:length(ob(:,1))
        disttmp = norm(ob(io,:)-x(1:2)')-R;  % 距离 = 到第io个障碍物的距离 - 障碍物半径  !!!有可能出现负值吗??
        if dist > disttmp   % 大于最小值 则选择最小值
            dist = disttmp;
        end
    end
     
    % 障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重
    if dist >= 2*R %最大分数限制
        dist = 2*R;
    end
     
    %% heading的评价函数计算
    % 输入参数:当前位置、目标位置
    % 输出参数:航向参数得分  当前车的航向和相对于目标点的航向 偏离程度越小 分数越高 最大180分
    function heading = CalcHeadingEval(x,goal)
    theta = toDegree(x(3)); % 机器人朝向
    goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1))); % 目标点相对于机器人本身的方位 
    if goalTheta > theta
        targetTheta = goalTheta-theta;% [deg]
    else
        targetTheta = theta-goalTheta;% [deg]
    end
     
    heading = 180 - targetTheta;  
    
    %% 计算动态窗口
    % 返回 最小速度 最大速度 最小角速度 最大角速度速度
    function Vr = CalcDynamicWindow(x,model)
    
    V_SPD       = 4;%机器人速度
    W_ANGLE_SPD = 5;%机器人角速度 
    
    MD_MAX_V = 1;% 
    MD_MAX_W = 2;% 
    MD_ACC   = 3;% 
    MD_VW    = 4;% 
    
    global dt;
    % 机器人速度的最大最小范围 依次为:最小速度 最大速度 最小角速度 最大角速度速度
    Vs=[0 model(MD_MAX_V) -model(MD_MAX_W) model(MD_MAX_W)];
     
    % 根据当前速度以及加速度限制计算的动态窗口  依次为:最小速度 最大速度 最小角速度,最大角速度速度,此处应为速度=model(MD_ACC)/dt
    Vd = [x(V_SPD)-model(MD_ACC)*dt x(V_SPD)+model(MD_ACC)*dt x(W_ANGLE_SPD)-model(MD_VW)*dt x(W_ANGLE_SPD)+model(MD_VW)*dt];
     
    % 最终的Dynamic Window
    Vtmp = [Vs;Vd];  %2 X 4  每一列依次为:最小速度 最大速度 最小角速度 最大角速度速度
    Vr = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
     
    %% Motion Model 根据当前状态推算下一个控制周期(dt)的状态
    % 输入u = [vt; wt];当前时刻的速度、角速度 x = 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
    %输出:下一位置点,x=[[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
    function x = f(x, u)
    global dt;
    F = [1 0 0 0 0
         0 1 0 0 0
         0 0 1 0 0
         0 0 0 0 0
         0 0 0 0 0];
     
    B = [dt*cos(x(3)) 0
        dt*sin(x(3)) 0
        0 dt
        1 0
        0 1];
     
    x= F*x+B*u;  %根据航向角计算下一点的坐标,
    
    %% degree to radian
    function radian = toRadian(degree)
    radian = degree/180*pi;
    
    %% radian to degree
    function degree = toDegree(radian)
    degree = radian/pi*180;
    %% END
    

    三、DWA具体过程(程序直接在Main loop主循环处开始)
    1、输入参数:x,model,goal,evalParam,ob,R:
    function [u,trajDB] =DynamicWindowApproach(x,model,goal,evalParam,ob,R)
    六个参数分别对应

    输入参数对应函数
    xx=[0 0 pi/2 0 0]’%[机器人初始状态。x轴坐标,y轴坐标,航向角 ,速度、角速度]
    modelKinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];%机器人运动学模型=[最高速度[m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],速度分辨率[m/s],转速分辨率[rad/s]]
    goal目标位置坐标
    evalparamevalParam = [0.05, 0.2 ,0.1, 3.0]%[航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间]
    ob所有障碍物坐标
    R障碍物半径,障碍物在图中显示为圆形

    2、输出参数
    u:机器人当前阶段窗口结束时的最佳速度与角速度,作为下一阶段窗口选择的初始速度与角速度。
    trajDB:机器人该阶段的路点坐标集。
    3、过程:

    过程
    1.计算动态窗口:Vr = CalcDynamicWindow(x,model)
    1.1提取动态模型状态范围:速度、角速度范围Vs=[0 model(MD_MAX_V) -model(MD_MAX_W) model(MD_MAX_W)]%[最小速度 最大速度 最小角速度 最大角速度]
    1.2根据当前状态计算速度角速度的限定范围Vd = [x(V_SPD)-model(MD_ACC)*dt x(V_SPD)+model(MD_ACC)*dt x(W_ANGLE_SPD)-model(MD_VW)*dt x(W_ANGLE_SPD)+model(MD_VW)*dt]%最小速度(Vmin=Vinit-a/▲t) 最大速度 最小角速度(Wmin=Winit-W/▲t),最大角速度速度
    1.3选取当前动态窗口Vr = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))]%[max(Vmin),min(Vmax),max(Wmin),min(wmax)]
    2. 计算生成轨迹,并就该段轨迹中的最后一个位姿进行综合测评[evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam)
    2.1生成轨迹[xt,traj] = GenerateTrajectory(x,vt,ot,evalParam(4),model);%生成由3个(time模拟时间)路点组成的一小段轨迹。每个点具体由三角函数计算得出。
    2.2综合评估评估该小段轨迹的最后一点的位姿(1)heading = CalcHeadingEval(xt,goal); % 预测与终点的航向得分,偏差越小分数越高。(2)dist = CalcDistEval(xt,ob,R); % 预测该点距离最近障碍物的距离得分,距离越远分数越高。(3)vel = abs(vt);%速度得分,速度越快分越高。(4)stopDist = CalcBreakingDist(vel,model); % 返回制动距离
    3.将范围内所有的轨迹进行评估,归一化处理evalDB = NormalizeEval(evalDB)
    4.选取分数最高的路径
    5. 继续循环

    4、剩下的为matlab的作图过程,很有意思的小箭头的生成,之后在进行细化。

    展开全文
  • 内容包括C++和MATLAB的DWA算法
  • 动态窗口法的理解和一些细节

    千次阅读 2020-10-31 22:03:53
    动态窗口法(Dynamic Window Approach,DWA)是一类经典的机器人局部路径规划算法。它的过程主要分为两部分: 速度空间(v,ω)(v,\omega)(v,ω)的计算。考虑到实际的运动限制可以得到一个速度范围: V={(v,ω)∣v1...

    机器人局部路径规划—动态窗口法

    动态窗口法(Dynamic Window Approach,DWA)是一类经典的机器人局部路径规划算法。它的过程主要分为两部分:

    1. 速度空间 ( v , ω ) (v,\omega) (v,ω)的计算。考虑到实际的运动限制可以得到一个速度范围:
      V = { ( v , ω ) ∣ v 1 ≤ v ≤ v 2 ,    ω 1 ≤ ω ≤ ω 2 } V = \{ (v,\omega)|v_1\leq v \leq v_2, \; \omega_1\leq \omega \leq \omega_2\} V={(v,ω)v1vv2,ω1ωω2}
      这个范围由多种因素决定,如机器人的速度限制、电机的加速度限制、机器人的最小转动角度等。

    2. 速度空间 ( v , ω ) (v,\omega) (v,ω)的评价。根据给定的评价公式,对速度空间的每一组 ( v i , ω i ) (v_i,\omega_i) (vi,ωi)进行评价,然后选择一组最佳速度作为当前速度。

    值得注意的是,在使用DWA方法进行路径规划时,一般应用于以下场景:

    • 机器人和目的地的坐标已知,但障碍物坐标未知;
    • 有传感器可以检测到障碍物,并且可以得到机器人与障碍物的距离和角度.

    接下来,我会详细介绍DWA的算法流程。

    1.速度空间计算

    在计算速度空间时,为了更加贴近现实,我们一般会考虑一些运动限制。在DWA中,主要考虑了三种运动限制:

    • 机器人移动的最大速度限制;
    • 电机的最大加速度限制;
    • 遇到障碍物时能否在碰撞前停止.

    1.1.移动最大速度限制

    显然,机器人的速度是不可能无限增加的,所以应该考虑其最大运动速度。于是,我们可以得到如下公式:
    v s ∈ [ v m i n , v m a x ] v_s \in [v_{min}, v_{max}] vs[vmin,vmax]

    ω s ∈ [ ω m i n , ω m a x ] \omega_s \in [\omega_{min},\omega_{max}] ωs[ωmin,ωmax]

    一般最小线速度取零,最小角速度和最大角速度互为相反数。但考虑到为了增加评价函数的区分度,可以考虑把最小线速度设置为1。因为当线速度为0时,评价函数就无法发挥作用。从下图的上下限可以看出,其最大速度限制设置为:
    v m i n = 0 ,    v m a x = 90 ,    ω m i n = − 90 ,    ω m a x = 90 v_{min}=0, \; v_{max}=90, \; \omega_{min}=-90, \; \omega_{max}=90 vmin=0,vmax=90,ωmin=90,ωmax=90

    在这里插入图片描述

    如果不考虑其它运动限制,在设置最大速度限制时,上图的坐标系上的所有点都是我们需要考虑的速度,这些速度构成了一个速度空间。

    1.2.移动最大加速度限制

    由于电机的转矩有限,因此存在一个最大的加速度限制。假设机器人当前速度为 ( v c , ω c ) (v_c,\omega_c) (vc,ωc),在一个有限的时间周期 Δ t \Delta t Δt,机器人的速度范围应为:
    v d ∈ [ v c − v ˙ a Δ t ,    v c + v ˙ b Δ t ] v_d \in [v_c-\dot{v}_a \Delta t, \;v_c+\dot{v}_b \Delta t] vd[vcv˙aΔt,vc+v˙bΔt]

    ω d ∈ [ ω c − ω ˙ a Δ t ,    ω c + ω ˙ b Δ t ] \omega_d \in [\omega_c-\dot{\omega}_a \Delta t, \;\omega_c+\dot{\omega}_b \Delta t] ωd[ωcω˙aΔt,ωc+ω˙bΔt]

    上式中, ( v a , v b , ω a , ω b ) (v_a,v_b,\omega_a,\omega_b) (va,vb,ωa,ωb)分别代表线速度最大减速度、线速度最大加速度、角速度最大减速度以及角速度最大加速度。

    在一般情况下,都是默认最大加速度和最大减速度的绝对值相同。下图的白色矩形区域就是考虑加速度限制后可以取到的速度空间,也就是论文提到的动态窗口,该矩形的中心就是机器人当前的运动速度。

    在这里插入图片描述

    1.3.允许速度(Admissible Velocities)

    当机器人检测到障碍物时,应该留有一段刹车距离用于减速。当机器人与最近障碍物的距离小于刹车距离时,机器人就会与障碍物发生碰撞,这是我们不愿看到的情况。因此,我们应该考虑一个允许速度,也就是可以安全停止下来的速度。

    显然,这个安全停止速度应该与机器人与最近障碍物的距离成正比。当距离越小时,这个安全停止速度也应该相应减小。它们应该满足如下不等式:
    v a ≤ 2 ⋅ d i s t ⋅ v ˙ a v_a \leq \sqrt{2 \cdot dist \cdot \dot{v}_a} va2distv˙a

    ω a ≤ 2 ⋅ d i s t ⋅ ω ˙ a \omega_a \leq \sqrt{2 \cdot dist \cdot \dot{\omega}_a} ωa2distω˙a

    计算安全速度的难点在于机器人与障碍物的最短距离如何计算。因为角速度的存在,机器人并不是直线运动的,它的运动轨迹是一条弧线。因此在计算与障碍物的距离时,我们求的应该是弧长,而不是直线距离。
    下图的淡灰色区域就是可允许速度,而深灰色区域则是不可允许速度。

    在这里插入图片描述

    在计算三种速度限制后,我们得到了三个速度范围,它们的交集就是我们在当前状态下可以取到的速度范围,也就是上图中的红色区域。
    V r = V s ⋂ V d ⋂ V a V_r=V_s \bigcap V_d \bigcap V_a Vr=VsVdVa

    2.速度空间评价

    在得到速度空间后,根据评价函数,我们就通过遍历每一组速度 ( v i , ω i ) (v_i,\omega_i) (vi,ωi)对其做出评价,然后得到一组当前最优速度 ( v b e s t , ω b e s t ) (v_{best},\omega_{best}) (vbest,ωbest)。由于在计算机中处理数据是离散的,因此需要设置速度增量,即
    Δ v = 0.01    m / s ,    Δ ω = 1 ∘ / s \Delta v=0.01\;m/s, \;\Delta{\omega}=1^{\circ}/s Δv=0.01m/s,Δω=1/s
    假设计算得到的速度空间为
    v ∈ [ 0 , 3 ] ,    ω ∈ [ − 2 0 ∘ , 2 0 ∘ ] v \in [0, 3],\;\omega \in [-20^{\circ},20^{\circ}] v[0,3],ω[20,20]
    根据设置的速度增量,我们可以得到如下速度序列
    the list of v : [ 0 , 0.01 , . . . , 3.00 ] ,    n = 101 \text{the list of v}:[0,0.01,...,3.00], \; n=101 the list of v:[0,0.01,...,3.00],n=101

    the list of      ω : [ − 2 0 ∘ , − 1 9 ∘ , . . . , 2 0 ∘ ] ,    n = 41 \text{the list of}\;\;\omega:[-20^{\circ},-19^{\circ},...,20^{\circ}], \; n=41 the list ofω:[20,19,...,20],n=41

    经过上述计算后,我们可以得到 101 × 41 101 \times 41 101×41组速度。

    在得到每组速度后,我们还需要做一些预备工作,即对每组速度生成在给定时间周期内的轨迹预测。

    2.1.轨迹预测

    假设有一组速度为 ( v i , ω i ) (v_i,\omega_i) (vi,ωi),预测时间周期为 t t t,我们需要计算出在该周期内的机器人运动轨迹。与上相同,我们需要先设置一个时间增量 Δ t \Delta t Δt,这样我们就有了一个时间序列 [ 0 , Δ t , 2 Δ t , . . . , t ] [0,\Delta t,2\Delta t,...,t] [0,Δt,2Δt,...,t]

    假设机器人当前位姿为 ( x 0 , y 0 , θ 0 ) (x_0,y_0,\theta_0) (x0,y0,θ0),根据下述递推公式就可以得到预测的轨迹点。但要说明的一点是,此处的轨迹预测公式是假设机器人是直线运动的,这是一种近似的运动模型。当距离很小时,我们可以用直线去近似弧线。

    在这里插入图片描述

    除了上述预测公式外,还有更精确的轨迹公式。事实上,当线速度和角速度不变时,机器人的运动轨迹应该是一个半径为 ∣ v ω ∣ |\frac{v}{\omega}| ωv的圆。这也是DWA论文中采用的运动模型。该预测公式如下:
    [ x ′ y ′ θ ′ ] = [ x − v ω sin ⁡ θ + v ω sin ⁡ ( θ + ω Δ t ) y + v ω cos ⁡ θ − v ω cos ⁡ ( θ + ω Δ t ) θ + ω Δ t ]    ( ω ≠ 0 ) \left[ \begin{matrix} x'\\ y'\\ \theta' \end{matrix} \right]= \left[ \begin{matrix} x-\frac{v}{\omega}\sin\theta+\frac{v}{\omega}\sin(\theta+\omega \Delta t)\\ y+\frac{v}{\omega}\cos\theta-\frac{v}{\omega}\cos(\theta+\omega \Delta t)\\ \theta+\omega \Delta t \end{matrix} \right] \; (\omega \neq0) xyθ=xωvsinθ+ωvsin(θ+ωΔt)y+ωvcosθωvcos(θ+ωΔt)θ+ωΔt(ω=0)
    上述公式成立的条件是角速度不为零。当角速度为零时,公式与第一个预测公式相同。

    另外需要注意的是,在整个预测的时间周期 ( t 1 , t n ) (t_1,t_n) (t1,tn),我们默认机器人的速度不变。但在计算安全停止速度时,预测的轨迹点的线速度应该是减少的。

    对时间序列的每个值进行计算,我们就可以得到一系列的轨迹点了。但不是每一个轨迹点都是有用的,我们需要对每个轨迹点进行判断。假设某个轨迹点与障碍物碰撞,则我们应该舍弃这个以及后面的所有轨迹点,选择上一个轨迹点作为最终预测的轨迹点。

    2.2.速度评价

    速度评价函数主要由三个部分组成,分别为方向角评价、障碍物距离评价以及速度评价。

    G ( v i , ω i ) = σ ( α ⋅ h e a d i n g ( v i , ω i ) + β ⋅ d i s t ( v i , ω i ) + γ ⋅ v e l o c i t y ( v i , ω i ) ) G(v_i,\omega_i)=\sigma(\alpha \cdot heading(v_i,\omega_i)+\beta \cdot dist(v_i,\omega_i)+\gamma \cdot velocity(v_i,\omega_i)) G(vi,ωi)=σ(αheading(vi,ωi)+βdist(vi,ωi)+γvelocity(vi,ωi))

    2.2.1.方向角评价

    h e a d i n g ( v i , ω i ) heading(v_i,\omega_i) heading(vi,ωi)用于评价机器人在给定角速度下运动的角度与目标角度之间的差值。显然,根据函数描述,该函数值越小,方向角评价应该越高。
    h e a d i n g ( v i , ω i ) = 18 0 ∘ − ∣ t a r g e t − c u r θ ∣ heading(v_i,\omega_i)=180^{\circ}-|target-cur\theta| heading(vi,ωi)=180targetcurθ

    在这里插入图片描述

    2.2.2.障碍物距离评价

    d i s t ( v i , ω i ) dist(v_i,\omega_i) dist(vi,ωi)用于表示机器人当前位置与最近的障碍物之间的距离。如果轨迹上无障碍物,则设定一个常数。根据函数描述可以得知,当机器人与障碍物的距离越大,则该函数评价应该越高。因此,该函数可以直接用距离作为评价函数。

    另外值得注意的是,在计算距离时还应该考虑机器人的半径。

    当角速度不为零时,我们应该计算的距离是弧线长度,而不是长度 c c c。这个弧长的计算也比较简单,根据运动模型,我们知道:当角速度和线速度不变时,机器人的运动轨迹为一个标准的圆形。该圆的半径为(角速度单位为弧度每秒)
    r = ∣ v ω ∣    ( ω ≠ 0 ) r=|\frac{v}{\omega}| \; (\omega \neq 0) r=ωv(ω=0)
    设机器人当前的位姿为 ( x , y , θ ) (x,y,\theta) (x,y,θ)(这里并不是预测点的位姿),则该圆的圆心坐标为:
    x c = x − v ω sin ⁡ θ y c = y + v ω cos ⁡ θ \begin{aligned} x_c = x - \frac{v}{\omega}\sin\theta \\ y_c = y + \frac{v}{\omega}\cos\theta \end{aligned} xc=xωvsinθyc=y+ωvcosθ
    然后通过余弦公式可以计算出角度 γ \gamma γ,最后应用弧长公式就可以得到弧长:
    s = r ⋅ γ s=r \cdot \gamma s=rγ

    在这里插入图片描述
    补充于2021.5.30: 最近看了CVM算法,发现它和DWA的算法过程十分相似,区别只在于障碍物距离的计算方式不同。CVM论文中的很大篇幅都在描述如何计算障碍物距离,而且它考虑的是弧线长度。值得一提的是,CVM将曲率c(角速度与线速度的比值)作为讨论对象,即在遍历速度时,CVM遍历的是角速度与线速度的比值。)

    2.2.3.速度评价

    v e l o c i t y ( v i , ω i ) velocity(v_i,\omega_i) velocity(vi,ωi)表示当前的机器人速度。对于路径规划而言,显然速度越快越好,因此可直接把当前线速度作为速度评价值,即
    v e l o c i t y ( v i , ω i ) = ∣ v i ∣ velocity(v_i,\omega_i)=|v_i| velocity(vi,ωi)=vi
    在计算三种评价函数后,还需要分别做归一化处理。最后代入上述给出的评价函数,就可以对速度 ( v i , ω i ) (v_i,\omega_i) (vi,ωi)做出评价。

    3.总结

    当机器人运动到某个位置时,首先计算速度空间,然后对每一组速度进行轨迹预测并给出速度评价,最后取评价最高的一组速度作为当前速度。就这样,机器人不断进行速度的计算、评价和选择,就可以越来越接近目标而不碰撞障碍物了。最后给出DMA运行的图像,蓝点表示预测的轨迹点。
    在这里插入图片描述

    在经过实际仿真后,我发现在较为复杂的环境下,DWA方法并不是很有效。特别是当目标位置与当前位置相隔一层障碍物时,此时的评价函数的三个参数很难调,很难同时让角度评价和距离评价的效果都很好。

    在这里插入图片描述

    在上图中,机器人与目标点相隔一层障碍物,只有一个缺口可以通过。此时,如果设置角度评价参数较高,那么机器人很可能会在左侧区域打转;如果设置障碍物距离参数较高,即使机器人很靠近目标时,可能仍然会错过。或许可以根据具体情况自适应地调整参数,而不是让参数固定不变。

    展开全文
  • #资源达人分享计划#
  • #资源达人分享计划#
  • 简要介绍动态窗口算法,具体不加个人评价和理解。可看原文 1. 定义 dynamic window approach算法的定义,根据wiki的定义: 在机器人运动规划领域,动态窗方法为三位大佬Dieter Fox, Wolfram Burgard, and Sebastian ...


    简要介绍动态窗口算法,具体不加个人评价和理解。可看原文

    1. 定义

    dynamic window approach算法的定义,根据wiki的定义:

    在机器人运动规划领域,动态窗方法为三位大佬Dieter Fox, Wolfram Burgard, and Sebastian Thrun 于1997发表的论文,被引用高达2800次。应用于实时碰撞避免策略,和其余碰撞避免算法不同,动态窗口直接考虑到机器人的动力学,特别是设计来用于受到速度和加速度约束的机器人。
    其主要包含两部分:首先是生成一个有效的搜索区域,其次在搜索区域中选择一个最优解。其论文中讲到,搜索空间限制在短时间内可到达且无碰撞的安全圆周轨迹。优化目标是选择一个方向和速度,使机器人以最大的距离远离障碍物且能到达目标位置。

    2. 伪代码

    DWA的主要创新点是使用(v,w)(速度,角速度)空间来表示车辆在平面内的运动;

    论文优化目标如下:
    在这里插入图片描述
    伪码如下:

    //http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html
    BEGIN DWA(robotPose,robotGoal,robotModel)
       desiredV = calculateV(robotPose,robotGoal)
       laserscan = readScanner()
       allowable_v = generateWindow(robotV, robotModel)
       allowable_w  = generateWindow(robotW, robotModel)
       for each v in allowable_v
          for each w in allowable_w
          dist = find_dist(v,w,laserscan,robotModel) // 遇到障碍物的距离
          breakDist = calculateBreakingDistance(v)
          if (dist > breakDist)  //can stop in time 能够在障碍物前停下来
             heading = hDiff(robotPose,goalPose, v,w) //计算朝向cost(注意要归一化)
             clearance = (dist-breakDist)/(dmax - breakDist)
             cost = costFunction(heading,clearance, abs(desired_v - v))
             if (cost > optimal)
                best_v = v
                best_w = w
                optimal = cost
        set robot trajectory to best_v, best_w
    END
    
    展开全文
  • 改进的原有代码,修正错误,只需要输入起点和终点即可,并且任意位置都可成功寻路
  • 机器人路径规划之动态窗口法

    千次阅读 2020-03-19 17:56:56
    动态窗口法(Dynamic Window Approach)概述 DWA是一种基于速度的局部规划器,可计算达到目标所需的机器人的最佳无碰撞速度。 程序实现 DWA算法主要分三步: 计算动态窗口 计算最优[v,ω][v,\omega][v,ω] 更新...

    动态窗口法(Dynamic Window Approach)概述

    DWA是一种基于速度的局部规划器,可计算达到目标所需的机器人的最佳无碰撞速度。

    DWA

    程序实现

    DWA算法主要分三步:

    • 计算动态窗口
    • 计算最优 [ v , ω ] [v,\omega] [v,ω]
    • 更新机器人状态

    流程图如下:

    流程图

    以下代码参考:https://github.com/AtsushiSakai/PythonRobotics

    初始化机器人状态、目标位置、障碍物位置

    # 初始化机器人状态 [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
    # 目标位置 [x(m), y(m)]
    goal = np.array([gx, gy])
    # 障碍物位置 [x(m), y(m)]
    ob = np.array([[-1, -1], ...... , [13.0, 13.0]])
    

    获取动态窗口

    这个动态窗口就是机器人在当前状态下能达到的速度 v v v和转速 ω \omega ω范围,受到自身机械特性以及当前状态的影响。

    def calc_dynamic_window(x, config):
        """
        calculation dynamic window based on current state x
        """
    
        # Dynamic window from robot specification
        Vs = [config.min_speed, config.max_speed,
              -config.max_yawrate, config.max_yawrate]
    
        # Dynamic window from motion model
        Vd = [x[3] - config.max_accel * config.dt,
              x[3] + config.max_accel * config.dt,
              x[4] - config.max_dyawrate * config.dt,
              x[4] + config.max_dyawrate * config.dt]
    
        #  [vmin, vmax, yaw_rate min, yaw_rate max]
        dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
              max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
    
        return dw
    

    计算最优 [ v , ω ] [v,\omega] [v,ω]

    对动态窗口采样,得到 N × M N\times M N×M [ v i j , ω i j ] ∣ i < N , j < M [v_{ij},\omega_{ij}]|i<N,j<M [vij,ωij]i<N,j<M,并计算 v = v i j , ω = ω i j v=v_{ij},\omega=\omega_{ij} v=vij,ω=ωij时机器人的预测轨迹。接下来计算机器人 v = v i j , ω = ω i j v=v_{ij},\omega=\omega_{ij} v=vij,ω=ωij时的评价函数:
    G ( v , ω ) = a 1 ⋅ h e a d i n g ( v , ω ) + a 2 ⋅ d i s t ( v , ω ) + a 3 ⋅ v e l o c i t y ( v , ω ) G(v,\omega)=a_1\cdot heading(v,\omega)+a_2\cdot dist(v,\omega)+a_3\cdot velocity(v,\omega) G(v,ω)=a1heading(v,ω)+a2dist(v,ω)+a3velocity(v,ω)

    • h e a d i n g ( v , ω ) heading(v,\omega) heading(v,ω)表示机器人运动方向与目标方向的偏差 θ \theta θ(如下图)。
    target_heading
    • d i s t ( v , ω ) dist(v,\omega) dist(v,ω)表示与预测轨迹相交并且距离当前机器人位置最近的障碍物的距离,当没有障碍物处于预测轨迹上时,这个函数取一个很大的常数。
    • v e l o c i t y ( v , ω ) velocity(v,\omega) velocity(v,ω)表示机器人处于预测轨迹最后一点时的速度 v v v

    计算机器人处于 v = v i j , ω = ω i j v=v_{ij},\omega=\omega_{ij} v=vij,ω=ωij时的评价函数,得到一系列的代价,代价最小时的 [ v i j , ω i j ] [v_{ij},\omega_{ij}] [vij,ωij]即为最优 [ v , ω ] [v,\omega] [v,ω]

    def calc_control_and_trajectory(x, dw, config, goal, ob):
        """
        calculation final input with dynamic window
        """
    
        x_init = x[:]
        min_cost = float("inf")
        best_u = [0.0, 0.0]
        best_trajectory = np.array([x])
    
        # 计算动态窗口内所有的采样样本的代价函数
        for v in np.arange(dw[0], dw[1], config.v_reso):
            for y in np.arange(dw[2], dw[3], config.yawrate_reso):
    
                trajectory = predict_trajectory(x_init, v, y, config)
    
                # 计算代价函数
                to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
                speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
                ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)
    
                final_cost = to_goal_cost + speed_cost + ob_cost
    
                # 寻找具有最小代价的样本以及它的轨迹
                if min_cost >= final_cost:
                    min_cost = final_cost
                    best_u = [v, y]
                    best_trajectory = trajectory
    
        return best_u, best_trajectory
    

    更新状态

    根据最优 u = [ v , ω ] u=[v,\omega] u=[v,ω]更新机器人状态

    x = motion(x, u, config.dt)  
    

    完整代码参见这里

    展开全文
  • 针对煤矿移动机器人采用动态窗口算法在复杂环境中规划路径时存在路径规划不合理、规划速度慢和实时性较差等问题,提出了一种基于膜计算和粒子群的煤矿移动机器人动态窗口算法。利用粒子群中的随机性和膜计算的分布式...
  • DWA动态窗口法的原理及应用

    千次阅读 多人点赞 2020-04-03 20:56:22
    完全是对学术的不尊重吧,...DWA的核心在于所谓的动态窗口,这个东西在sampling based method 中就是sampling. 对于sampling可以是去sample state,也可以sample action. 百度的lattice planner其实就是在sample sta...
  • 【路径规划】基于改进动态窗口DWA算法机器人静态避障matlab源码
  • 第一眼看这篇文章,以为只是改进了A*算法,没有什么意思的地方。 后来再细看,发现这篇文章有一个比较有意思的创新点——A*生成的全局路径,然后可以在仿真环境下通过DWA算法进行跟踪。DWA算法跟踪路径点,车子运动...
  • 在规划过程中用动态窗口法处理这些约束 2.运动学自行车模型 输入是纵向速度和转向角 运动学方程没有考虑更高阶的加速度,这是导致车内乘客舒适性的主要原因。 3.自行车模型+加速度约束 我们可以通过为...
  • 【路径规划】基于改进动态窗口法DWA实现机器人动态避障matlab源码含 GUI.zip
  • 参考: http://wiki.ros.org/dwa_local_planner 机器人局部避障的动态窗口法(dynamic window approach)
  • 针对目前局部路径规划算法只适用于单车体机器人的问题,提出了一种针对拖车式移动机器人的动态窗口法。首先,利用多车体结构的路径跟踪方程实现对拖车式移动机器人的运动控制;然后,利用评价函数同时对牵引车和拖车...
  • 简介 ...动态窗口法是 Dieter Fox, Wolfram Burgard, 和 Sebastian Thrun在1997年提出的”The dynamic window approach to collision avoidance“,一种在线避障策略,该策略将栅格地图中planner生...
  • 扩展邻域Astar算法 动态窗口法 DWA
  • 机器人局部避障的动态窗口法(dynamic window approach)

    万次阅读 多人点赞 2015-04-10 19:23:54
    机器人局部路径规划方法有很多,ROS中主要采用的是动态窗口法(但是和原论文中的dwa方法不一样,具体见最后)。动态窗口法主要是在速度(v,w)空间中采样多组速度,并模拟机器人在这些速度下一定时间(sim_period)内的...
  • #资源达人分享计划#
  • 耳熟能详的局部避障动态窗口法(Dynamic Window Approach)概述:机器人局比避障规划方法有很多,ROS中主要采用的是动态窗口法(和原来的论文中的dwa方法不一样)。动态窗口法主要是在速度(v,w)空间中采样多组速度,并...
  • #资源达人分享计划#

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 110,698
精华内容 44,279
关键字:

动态窗口法