精华内容
下载资源
问答
  • dwa动态窗口算法代码

    2017-11-15 10:16:07
    dwa动态窗口算法在MATLAB中的仿真代码,有需要的下载。。。。。
  • 动态窗口算法(DWA)

    千次阅读 2019-10-11 09:40:09
    路径规划算法中动态窗口算法十分灵活好用,但发现在自动驾驶中几乎没有应用,原因大概就是其难于复杂的环境,日后将对其进行详细的调研,今天先记录一下其原理及过程。 一、DWA原理 仔细想想,机器人的运动状态,...

    路径规划算法中动态窗口算法十分灵活好用,但发现在自动驾驶中几乎没有应用,原因大概就是其难于复杂的环境,日后将对其进行详细的调研,今天先记录一下其原理及过程。
    一、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)
    六个参数分别对应

    输入参数 对应函数
    x x=[0 0 pi/2 0 0]’%[机器人初始状态。x轴坐标,y轴坐标,航向角 ,速度、角速度]
    model Kinematic = [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 目标位置坐标
    evalparam evalParam = [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的作图过程,很有意思的小箭头的生成,之后在进行细化。

    展开全文
  • 简要介绍动态窗口算法,具体不加个人评价和理解。可看原文 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
    
    展开全文
  • 局部避障的动态窗口算法(DWA)

    Dynamic Window Approach(DWA)是重要的局部轨迹规划算法,ROS中使用了DWA算法获得了很好的局部路径规划的效果.如下是一些参考资料

    参考资料:

    [1]. 机器人局部避障的动态窗口法(dynamic window approach)

    [2]. Autonomous Robot Navigation

    [3]. Circular Motion in 2D for graphics and robotics

    [4]. A Tutorial and Elementary Trajectory Model for the Differential Steering System of robot wheel actuators.


    原文链接:http://www.cnblogs.com/cv-pr/p/5800270.html



    展开全文
  • Dynamic Window Approach(DWA)是重要的局部轨迹规划算法,ROS中使用了DWA算法获得了很好的局部路径规划的效果。具体的教程可参考官方的导航调试资料Navigation Tuning Guide。ROS的DWA(dwa_local_planner)应用到新...

    Dynamic Window Approach(DWA)是重要的局部轨迹规划算法,ROS中使用了DWA算法获得了很好的局部路径规划的效果。具体的教程可参考官方的导航调试资料Navigation Tuning Guide。ROS的DWA(dwa_local_planner)应用到新机器人调试时,有一些技巧需要注意的,这里做一些总结。

    1.设置坐标系和话题

    需要订阅的话题:

    里程计话题:/odom

    需要设置的坐标系:包含/map,/odom,/base_footprint

    2.参数调试

    a.加速度的限制

    加速度限制(acc_lim_x,acc_lim_y,acc_lim_th)非常的重要,如果不知道机器人的加速度,可以尽量的往大的设置,因为如果设置太小了,往往会出现机器人往前跑断断续续的,转弯转过头(看似加速度太大了,实际是加速度太小,以至于机器人想把机器人掰回来而掰不及),从而导致反复的震荡(oscillating over and over),例如下面的错误提示:

    Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors

    ~/acc_lim_x (double, default: 2.5)

    The x acceleration limit of the robot in meters/sec^2

    开始调试尽量往大的设置

    ~/acc_lim_y (double, default: 2.5)

    The y acceleration limit of the robot in meters/sec^2

    如果是差分轮,设置为0即可

    ~/acc_lim_th (double, default: 3.2)

    The rotational acceleration limit of the robot in radians/sec^2

    开始调试尽量往大的设置

    b.仿真的时间

    局部路径模拟的时间不用太长也不用太短,一般一秒左右就差不多了,如果太多会容易导致偏离全局的路径,特别启动的时候会转较大的半径,如果想启动的时候基本原地旋转摆正机器人的方向和全局路径的方向一致,那么就把模拟的时间设置短点。如果太小的仿真时间也不好,容易导致频繁的路径规划消耗资源,甚至也会出现震荡的可能。

    ~/sim_time (double, default: 1.7)

    The amount of time to forward-simulate trajectories in seconds

    c.轨迹的规划

    主要是一些设置生成轨迹的参数。其中以下的两个参数比较重要。

    ~/path_distance_bias (double, default: 32.0)

    The weighting for how much the controller should stay close to the path it was given

    刻画局部路径和全局路径的贴合程度,该权重参数越大说明越贴合,越小说明不用那么贴合。

    ~/goal_distance_bias (double, default: 24.0)

    The weighting for how much the controller should attempt to reach its local goal, also controls speed

    达到局部目标点的权重参数,也用来控制速度。权重如果设置为0表示要求完全到达目标点,这会导致机器人走动缓慢和震荡的现象,因为要求达到目标点的精度太高,所对机器人的控制比较苛刻

    3.参考资料

    展开全文
  • Dynamic Window Approach(DWA)是重要的局部轨迹规划算法,ROS中使用了DWA算法获得了很好的局部路径规划的效果。具体的教程可参考官方的导航调试资料Navigation Tuning Guide。ROS的DWA(dwa_local_planner)应用到...
  • 博士论文,关于滑动窗口规模的动态调整算法的有点高深,^_^,滑动窗口是计算机网络中的经典算法,这篇论文是讨论如何对滑动窗口动态调整。
  • 动态窗口避障算法的思想是对可行空间的速度进行采样,并对每一个速度样本进行轨迹预测,然后利用评价函数来对每一条轨迹进行评分,并取最优的速度样本发送给执行器执行。 动态窗口避障算法是一种局部规划算法,它...
  • 一种动态窗口 PCA 运动方向估计算法 (江南大学 物联网工程学院,江苏 无锡 214122)
  • (给算法爱好者加星标,修炼编程内功)来源:山大王wld问题描述给两个整数数组 A 和 B ,返回两个数组中公共的、长度最长的子数组的长度。示例:输入:A: [1,2,3,2,1]B: [3,2,1,4,7]输出:3解释:长度最长的公共子数组...
  • 基于缓存窗口和段补丁预取的流媒体动态调度算法,杨戈,朱晓民,该文提出了一种新的基于缓存窗口和段补丁预取的移动流媒体动态调度算法,采用代理缓存窗口自适应伸缩和分段缓存补丁块方案,实现
  • 提出了基于动态窗口的运动车辆颜色提取算法,其中颜色提取窗口根据车辆中心坐标在交通场景中的位置以及车辆类型的不同进行动态调整;颜色提取算法采用RGB颜色模型,利用颜色的一阶矩和二阶矩作为车辆主体颜色的判定...
  • DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟这些速度在一定时间内的运动轨迹,再通过一个评价函数对这些轨迹打分,最优的速度被选择出来发送给下位机。 1 原理分析...
  • 动态窗口法(Dynamic Windows Approach) 有以下特点: 考虑机器人动力学特性; 考虑机器人惯性; 可以实现高速行驶。 根据运动方程的推导,可以把机器人的轨迹看成是两种形式:直线行走和弧线行走。前者 没有...
  • 此系统实现了数据结构中各算法动态演示 便于初学算法者增加对算法的理解
  • 耳熟能详的局部避障动态窗口法(Dynamic Window Approach)概述:机器人局比避障规划方法有很多,ROS中主要采用的是动态窗口法(和原来的论文中的dwa方法不一样)。...该算法的图书店在于动态窗口这个名词,它的含...
  • 把滚动规划和径向基函数神经网络(RBFNN)预测相结合,提出一种动态不确定环境下移动机器人局部路径规划过程中,针对动态障碍物的新的混合避障算法.利用摄像镜头采集动态障碍物的移动轨迹,提取形心序列,利用RBFNN建立...
  • 第一眼看这篇文章,以为只是改进了A*算法,没有什么意思的地方。 后来再细看,发现这篇文章有一个比较有意思的创新点——A*生成的全局路径,然后可以在仿真环境下通过DWA算法进行跟踪。DWA算法跟踪路径点,车子运动...
  • 在力扣常用解题法中,我们常常会看到这些: 滑动窗口 双指针 快慢指针/ 链表题目 原地链表翻转 区间合并 ...而偏偏是最常用的,也排在首位的滑动窗口算法,却并不为人熟知,什么是滑动窗口算法呢?? .
  • 滑动窗口构成 滑动窗口问题一般3要素和1个求值条件: 要素: 快指针fast 慢指针slow 资源值(一般随着fast右移减少,slow左移增多) ...优化技巧【动态(尽可能消耗)+固定窗口(保持资源为0)】: 因为fast
  • 为了进一步改善高负载状况下服务质量的性能,在原有的IEEE 802.11e 增强型分布式协调功能的基础上,提出了一种基于竞争窗口动态自适应调整的算法。该算法首先采用时隙利用率因子估计网络当前负载状况,然后在不同...
  • 前言科普:什么是滑动窗口算法滑动问题包含一个滑动窗口,它是一个运行在一个大数组上的子列表,该数组是一个底层元素集合。假设有数组 [a b c d e f g h ],一个大小为 3 的 滑动窗口 在其上滑动,则有:[a b c] [b...
  • 机器人局部动态避障算法dwa解析

    千次阅读 多人点赞 2019-04-30 08:55:53
     dwa算法全称叫动态窗口法(dynamic window approach),其算法过程主要分为仿真获取机器人的运动轨迹、对轨迹进行评价选择最优轨迹两个主要过程,动态窗口表达的是仿真的运动轨迹数量有限,主要是因为机器人在较短...
  • 具有时间窗口的基于三维动态请求预测的多目标模因算法
  • 滑动窗口是指一个窗口,在一段区间上从左到右滑动,一直到区间的尾部,滑动窗口的长度是可以动态变化的(也可以固定)。一般情形下可以使用两个指针分别指向窗口的两边,指针 i 指向窗口的左边界,指针 j 指向窗口的右边界...
  • 前言科普:什么是滑动窗口算法滑动问题包含一个滑动窗口,它是一个运行在一个大数组上的子列表,该数组是一个底层元素集合。假设有数组 [a b c d e f g h ],一个大小为 3 的 滑动窗口 在其上滑动,则有:[a b c] [b...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,024
精华内容 409
关键字:

动态窗口算法