精华内容
下载资源
问答
  • DWA动态窗口法的原理及应用

    千次阅读 多人点赞 2020-04-03 20:56:22
    看了CSDN博客上面的各种...DWA的核心在于所谓的动态窗口,这个东西在sampling based method 中就是sampling. 对于sampling可以是去sample state,也可以sample action. 百度的lattice planner其实就是在sample sta...

    看了CSDN博客上面的各种介绍DWA的博客,就这辣鸡点击都能过万?完全是对学术的不尊重吧,还是我来写一下吧。

    DWA算法的核心:
    DWA的核心在于所谓的动态窗口,这个东西在sampling based method 中就是sampling. 对于sampling可以是去sample state,也可以sample action. 百度的lattice planner其实就是在sample state, 把未来的轨迹candidate一个个sample出来做cost minimization. 而state lattice method其实是在sample action, 对于机器人来说就是sample 不同的角度速度和直线速度,预测出未来轨迹,然后做cost function minimization. 但是这两者区别是,虽然最后都变成了轨迹,但上一个方法时直接生成轨迹,而第二种方案是用动作生成轨迹,所以第二种方法的控制器输出会是直接角速度和直线加速度,而对于第一个方法先生成轨迹的,控制器要做的工作是轨迹跟踪。 DWA这个方案就是第二种方法中的典型。

    车辆运动学:
    原文中就是这么写的,真实的机器人推算也可以按照这个方法做:
    在这里插入图片描述
    对于汽车,可以重新写一下:
    在这里插入图片描述
    在代码中我们把状态估计写成矩阵的形式:

    %% Motion Model 根据当前状态推算下一个控制周期(dt)的状态
    % u = [vt; wt];当前时刻的速度、角速度 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;  
    

    动态窗口:
    动态窗口就是在当前的状态下,在本状态周围进行采样,获得下一时刻的合理的动作变化,这个动作仅包括角速度和线速度。在这个方法中,我们把轨迹认为是一系列不同动作组合而产生的在delta_t内生成的轨迹。我们要做的就是设定一个评价标准,选出我们认为的最合理的动作(也就意味着最合理的轨迹)。

    对于具体的搜索,原文是这么说的:
    在这里插入图片描述
    首先轨迹当成曲线curvature,然后选取合理的速度组合,然后生成动态窗口。

    那么什么是合理的速度?有三个限制。第一:按这个速度行驶,最大减速度停车都不能撞到最近的障碍物:
    在这里插入图片描述
    第二个:加速度限制,当前状态下,加减速变化都是有上下限的,因此下一时刻的状态也是有上下限的:
    在这里插入图片描述
    最后:任何情况下都要遵守车辆本身的动力学限制,意思就是不管怎么变化,角速度和线速度是有上下限标准的。就如图下面框出来的Vs区域。
    在这里插入图片描述
    那么最后动态窗口就是上面三个限制条件的交集:
    在这里插入图片描述
    代价函数(cost function)
    通常来讲我们用cost function来表征不用选择的代价,要做的事情就是cost function minimization. DWA采用了expectation maximization,其实也是一个意思。只是做法和叫法不同。
    原文中采用了三个因素的组合:
    在这里插入图片描述
    在这里插入图片描述
    很好理解:
    1,我希望我的前进方向对准终点
    2,我希望不发生任何碰撞
    3,我希望速度尽量快

    除此之外,还要加上一个:
    4,保证上文提到的最短刹车距离是安全的

    原文还特地解释了说第一个因素是效率问题,如果太大可能就落到局部最优解了,如果太小,虽然可以让你更好的在障碍物之间游走,但是可能效率有点低。剩下两个就是保证你行驶安全加快速。

    值的注意的一点是collison checking, 原文中使用的是软约束的方法,并不对发生碰撞做严格禁止,只是说这一项就没什么得分了。但是要注意,如果不小心设计者选择了一个不太好的weight, 极有可能最后最优路径还是选到了发生碰撞的路径上,因为另外两项可能得分很高。直接抵消了第三项的0分。

    对于cost function, 我先前就写过一个更详细的:
    自动驾驶路径规划DWA算法原理解析
    这个里面直接列出了7项cost function:
    在这里插入图片描述
    这里,我们介绍的时候还是按照最简单的来做,代码实现也仅包含上述的三项基础部分。具体的计算一看代码就知道了:

    %% 计算制动距离 
    %根据运动学模型计算制动距离, 也可以考虑成走一段段圆弧的累积 简化可以当一段段小直线的累积
    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
    
    %% 障碍物距离评价函数  (机器人在当前轨迹上与最近的障碍物之间的距离,如果没有障碍物则设定一个常数)
    % 输入参数:位姿、所有障碍物位置、障碍物半径
    % 输出参数:当前预测的轨迹终点的位姿距离所有障碍物中最近的障碍物的距离 如果大于设定的最大值则等于最大值
    % 距离障碍物距离越近分数越低
    %我们用flag表示是否发生碰撞,如果发生,则这条路径直接不考虑
    function [dist,Flag] = CalcDistEval(x,ob,R)
    dist=100;
    for io = 1:length(ob(:,1))  
        disttmp = norm(ob(io,:)-x(1:2)')-R; %到第io个障碍物的距离 - 障碍物半径  !!!有可能出现负值吗??
        if disttmp <0
            Flag = 1;
            break;
        else
            Flag = 0;
        end
        
        if dist > disttmp   % 大于最小值 则选择最小值
            dist = disttmp;
        end
    end
     
    % 障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重
    if dist >= 3*R %最大分数限制
        dist = 3*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;  
    
    

    其实仿真中我发现,这个第一个weight的选择还是有说法的。不同的选择结果不太一样:
    weight_headiing = 2weight_headiing = 2在这里插入图片描述weight_headiing =0.02

    正则化
    对于这几个单位不同的因素,我们必须进行正则化,把这些量变成无量纲量(百分比),这样才会让比较变得有意义:

    %% 归一化处理 
    % 每一条轨迹的单项得分除以本项所有分数和
    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
    

    值的注意的几个细节:
    1,当前速度的影响:

    在这里插入图片描述
    在这种情况下,我们需要穿过door, 但如果当前速度过高,最后的最优轨迹将会是向前方直行,而不是右转,因为角速度最高限制的原因,无法规划出一条右转通过door的曲线。由下图的expectation function 就可直观看出:
    在这里插入图片描述
    所以,为了应对这种情况,比较好的一种办法就是修正最高的速度限制,保证在这附近机器人速度不会太高。

    同样的情况,对于机器人的加速度也是一样的:
    在这里插入图片描述
    由于小加速度,导致我们右转的行为expectation不高,结果车子也不会向右转。而是选择expectation更高的直行行为。

    最后贴出我修改过的matlab 代码:

    % 直接命令行运行
    dwa_V_1_0
    

    以下为main function

    % -------------------------------------------------------------------------
    %
    % File : DWA
    %
    % Discription : Mobile Robot Motion Planning with Dynamic Window Approach
    %
    % Environment : Matlab
    %
    % Author :Yuncheng Jiang
    %
    %
    % License : Modified BSD Software License Agreement
    % log: collision checking 修改为硬约束
    % -------------------------------------------------------------------------
     
    function [] = dwa_V_1_0()
    
    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矩阵 列矩阵  位置 00 航向 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];
    
     for i =-1
        for j = -1:12
            obstacle = [obstacle; [i,j]];
        end
     end     
    for i =12
        for j = -1:12
            obstacle = [obstacle; [i,j]];
        end
    end 
    for j =-2
        for i = -1:12
            obstacle = [obstacle; [i,j]];
        end
    end 
    for j=13
        for i= -1:12
            obstacle = [obstacle; [i,j]];
        end
    end 
     
    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)];
    %定义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.08, 0.1 ,0.1, 3.0];
    % evalParam = [2, 0.2 ,0.2, 3.0];
    area      = [-3 14 -3 14];% 模拟区域范围 [xmin xmax ymin ymax]
    
    % 模拟实验的结果
    result.x=[];   %累积存储走过的轨迹点的状态值 
    tic; % 估算程序运行时间开始
    
    % movcount=0;
    %% Main loop   循环运行 5000次 指导达到目的地 或者 5000次运行结束
    for i = 1:5000  
        % DWA参数输入 返回控制量 u = [v(m/s),w(rad/s)] 和 轨迹
        [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
        x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度
        
        % 历史轨迹的保存
        result.x = [result.x; x'];  %最新结果 以列的形式 添加到result.x
        
        % 是否到达目的地
        if norm(x(POSE_X:POSE_Y)-goal')<0.25   % 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 limitrate;  %刷新屏幕. 当代码执行时间长,需要反复执行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;
    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));  %evalParam(4),前向模拟时间;
            % 各评价函数的计算
            heading = CalcHeadingEval(xt,goal); % 前项预测终点的航向得分  偏差越小分数越高
            [dist,Flag] = CalcDistEval(xt,ob,R);    % 前项预测终点 距离最近障碍物的间隙得分 距离越远分数越高
            vel     = abs(vt);                  % 速度得分 速度越快分越高
            stopDist = CalcBreakingDist(vel,model); % 制动距离的计算
            if dist > stopDist && Flag == 0 % 如果可能撞到最近的障碍物 则舍弃此路径 (到最近障碍物的距离 大于 刹车距离 才取用)
                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)
    global dt;
    time = 0;
    u = [vt;ot];% 输入值
    traj = x;   % 机器人轨迹
    while time <= evaldt   
        time = time+dt; % 时间更新
        x = f(x,u);     % 运动更新 前项模拟时间内 速度、角速度恒定
        traj = [traj x]; % 每一列代表一个轨迹点 一列一列的添加
    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,Flag] = CalcDistEval(x,ob,R)
    dist=100;
    for io = 1:length(ob(:,1))  
        disttmp = norm(ob(io,:)-x(1:2)')-R; %到第io个障碍物的距离 - 障碍物半径  !!!有可能出现负值吗??
        if disttmp <0
            Flag = 1;
            break;
        else
            Flag = 0;
        end
        
        if dist > disttmp   % 大于最小值 则选择最小值
            dist = disttmp;
        end
    end
     
    % 障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重
    if dist >= 3*R %最大分数限制
        dist = 3*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)];
     
    % 根据当前速度以及加速度限制计算的动态窗口  依次为:最小速度 最大速度 最小角速度 最大角速度速度
    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)]
    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
    
    
    展开全文
  • 动态窗口 1) Admissable Velocities 2) Dynamic Window 3. 评估机制 1) 朝向评价 angle(v, ω) --- 目标的“引力” 2) 障碍物评价 dist(v, ω) --- “斥力” 3) 速度评价 --- 驱动 4) 加权与平滑 4. 特点 1) 决策...

    The Dynamic Window Approach to Collision Avoidance



    1. 航迹推演:

    1) 位置推算

    在这里插入图片描述
    将速度增量=加速度对时间积分、角度增量=角速度对时间积分、角速度增量=角加速度对时间积分带入,得X向位置为
    在这里插入图片描述

    2) 短时间间隔内加速度/角加速度恒定假设

    短时间内机器人所能执行的加速度/角加速度命令数量是有限的,当积分区间足够短时,可以认为这段时间内机器人的加速度和角加速度恒定,同时对上式离散化表示,得
    在这里插入图片描述
    其中
    在这里插入图片描述
    这里认为tit_iti+1t_{i+1}时间段内加速度恒定。

    3) 短时间间隔内速度/角速度恒定假设

    tit_iti+1t_{i+1}的间隔充分小时,下式
    在这里插入图片描述
    可以用tit_iti+1t_{i+1}之间的任意一个速度来近似,这是基于机器人短时间内平滑运动的假设

    类似地,同样可以将角增量
    在这里插入图片描述
    近似为
    在这里插入图片描述
    其中, ωiω_itit_iti+1t_{i+1}之间的任意一个角速度

    于是航迹推演公式可改写为
    在这里插入图片描述
    进一步地,对积分符号及内部式子运算,结果可表示如下
    在这里插入图片描述

    这里我推导的结果和给出的结果有点差别,看别的博主写到这篇文章时也有提到推导结果不一样
    我推导的当角速度≠0时,Fxi(t)F_x^i(t)的值前面有负号而Fyi(t)F_y^i(t)的值前面没有负号
    而当角速度=0时,要将多项式的最后一项t改为(t-ti)

    4) 结果

    经过上述假设,作者将一段机器人的完整轨迹划分成无数段短时间内速度、角速度恒定的轨迹,当角速度=0,轨迹为线段,当角速度≠0,由于恒定假设,机器人运动的轨迹是圆弧,其半径为速度/角速度,于是就可以用无数段圆弧/线段来拟合整段轨迹。

    5) 拟合误差评估

    当角速度为0时,沿轴向的位置偏差最大,由于是选定短时间内的任意一个速度来代替,所以速度的误差一定不超过短时间内的最大速度-最小速度,于是有位置误差

    在这里插入图片描述

    2. 动态窗口

    给定情境如下:可以看出,如果要机器人到达目标位置,可以右转通过两面墙之间的缝隙。
    在这里插入图片描述

    DWA通过以下2个约束,大大缩小了搜索空间,提高了计算效率。

    1) Admissable Velocities

    首先,通过障碍物来限制。当前方有障碍物时,要保证当前的速度不要过大,以至于以最大加速度制停时都无法在障碍物前停下。

    对速度搜索范围做如下限制:
    在这里插入图片描述
    其中dist表示以该速度和角速度拟合出的圆弧上,机器人到障碍物的距离。
    在这里插入图片描述
    上图矩形区域为原本的搜索空间Vs,经过上述约束(Va),可以去除灰色部分,得到剩余部分Vs∩Va

    2) Dynamic Window

    进一步地,限制搜索空间为短时间内可达的速度和角速度
    在这里插入图片描述
    搜索空间将进一步缩小,如下

    在这里插入图片描述
    最终的搜索范围是Vs∩Va∩Vd,Vd也就是图上的小矩形,表示速度的可达范围,即动态窗口。

    3. 评估机制

    经过2,确定了速度的搜索空间,并可以通过1来用线段/圆弧模拟短时间内速度的轨迹,接下来对轨迹进行评价,找到最优的轨迹,它所对应的速度即为DWA规划的结果。

    用于评价的函数object function为:
    在这里插入图片描述
    函数由三部分组成,对应三个考察角度,对应不同的权重

    1) 朝向评价 angle(v, ω) — 目标的“引力”

    轨迹末端机器人的朝向与机器人与目标连线之间的夹角,这个夹角越小越好

    在这里插入图片描述
    对应的angle函数值分布如下,值为0的部分是经过第一个约束Va去除掉的,不考虑。并且,为了更加直观,这里作者给出的搜索空间是整个Vs而不是动态窗口Vd

    在这里插入图片描述
    可以看出,右上角的angle函数值最大,说明这个函数希望机器人以一定的前向速度、右转,到达目标。

    2) 障碍物评价 dist(v, ω) — “斥力”

    这个函数衡量的是当前机器人沿轨迹与最近障碍物之间的距离。如果轨迹上没有障碍物,函数值设置为一个很大的常数。在这里插入图片描述
    这个函数希望机器人选择没有障碍物、或者与障碍物距离较远的轨迹。

    作者给出了距离的计算方法,找到轨迹(圆弧)与障碍物相交的点,获得夹角γ,γ×r即为机器人与障碍物在圆弧上的距离。

    在这里插入图片描述


    3) 速度评价 — 驱动

    当机器人原地不动,自转到一个和目标对齐的朝向时,它很好地满足了前两项评价标准,然而很明显不是想要的结果,为了避免机器人的这种“惰性”,引入这一项评价,速度和角速度的幅度越大,函数值越大。这个函数希望机器人在合理范围内尽可能以更大的速度和角速度移动。
    在这里插入图片描述

    4) 加权与平滑

    作者给出的权重范例为α=γ=0.2,β=2.0,加权值为

    在这里插入图片描述
    对加权值进行平滑(σ函数),得到最终的Objective Function如下

    在这里插入图片描述
    那么搜索空间内对应着Object Function最大值的速度和角速度即为一次DWA规划的结果。

    4. 特点

    1) 决策结果与当前速度的关系

    下图给出了在较大速度Vd1和较小速度Vd2的情况下生成的动态窗口,以及经过评估后所得到的规划结果,用×表示。
    在这里插入图片描述
    联系前面作者给出的情境可以发现,当速度较大时,规划结果并不会使机器人右转穿过缝隙而到达目标,这是因为此时如果紧急右转,由于速度较大,无法保证在撞到墙之前制停,所以算法是希望机器人加速前进在前方绕路的。

    而当速度较小,就可以满足上述条件,所以此时算法希望机器人减速右转,通过缝隙抵达目标。

    在这里插入图片描述

    2) 决策结果与最大加速度的关系

    同理,最大加速度也将限制生成的动态窗口的大小,从而影响决策结果,这里不赘述。
    在这里插入图片描述

    5. 其他说明

    • DWA法同样会有local minima问题,作者提出陷入local minima时,机器人采取原地自转策略,直到找到脱离的轨迹。
    • 算法依赖于传感器获得的障碍物信息,当障碍物有一定高度或者不触地时,可能造成规划结果不准确。
    • 当机器人移动速度过快,需要能够感知更远处信息的传感器满足规划需求,也可以选用不同类型传感器进行信息融合。
    展开全文
  • DWAPlannerROS是封装类,提供了与move_base的接口,而DWAPlanner是具体实现类,它非常依赖costmap(当然不指望让小车动态避障的话就无所谓了),因此我们在使用时需要保证代价地图的膨胀度以及实时更新频率。...

    DWAPlannerROS是封装类,提供了与move_base的接口,而DWAPlanner是具体实现类,它非常依赖costmap(当然不指望让小车动态避障的话就无所谓了),因此我们在使用时需要保证代价地图的膨胀度以及实时更新频率。btw:在此类代码中,基本上下反复使用的变量在函数形参中都是引用,实在放心不下还是看声明吧~

    move_base是规划路径与速度的大类,DWAPlannerROS提供给它的接口有两个,setPlan与computeVelocityCommands。

    一.setPlan负责获取全局路径,DWAPlannerROS::setPlan获取后,转发给DWAPlanner::setPlan,恢复振荡标志位再转发给LocalPlannerUtil::setPlan,这样层层叠叠的调用很有层次感,这样每当产生一个新的全局路径都第一时间提供给全局——局部转化以及剪裁功能使用。

    二.computeVelocityCommands这个函数负责本次循环的下发速度的解算,它一上来就先确定小车当前在全局中位置。

    1.Costmap2DROS::getRobotPose获取小车全局坐标与姿态。

    if ( ! costmap_ros_->getRobotPose(current_pose_))
    {
      ROS_ERROR("Could not get robot pose");
      return false;
    }

     这个错误其实会出现得比较频繁,导致小车不动或者抽搐。具体我们先看这个函数,虽说它位于Costmap2DROS类中,但是和costmap并没有什么关系,负责将小车自身的位姿用tf转化为全局位姿,在这里出错的原因很可能是transform_tolerance_设得太小,小车自身性能跟不上被迫报错。try中抛出的三个错误犯的概率不是太大。

    2.LocalPlannerUtil::getLocalPlan将全局路径转化到局部路径,位于base_local_planner包local_planner_util.cpp中。

    该函数先调用base_local_planner::transformGlobalPlan,这是base_local_planner包中goal_functions.cpp中的函数,它将全局路径转化为base_link下的路径,这个文件包括的是工具性的函数,DWA与trajectoryRollOut方法均会调用该文件。

    if(limits_.prune_plan) 
    {
      base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
    }

     这是在根据小车当前位置裁剪前方一小段路径,依然存放在transformed_plan中。limits_.prune_plan是可配置的参数,默认为true。

    3.调用DWAPlanner::updatePlanAndLocalCosts,这个函数调用了MapGridCostFunction即根据栅格地图产生一系列打分项,它们均调用了各自的接口setTargetPoses:

    void apGridCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {
      target_poses_ = target_poses;
    }

    4.调用LatchedStopRotateController::isPositionReached判断是否到终点了,这也是base_local_planner中的功能包,这只是通过判断终点和当前位置的算术距离,xy_goal_tolerance是可设置的参数,小于这个数就可认为到达了。若到终点了,调用LatchedStopRotateController::computeVelocityCommandsStopRotate函数,使小车旋转至最终姿态;否则继续调用dwaComputeVelocityCommands函数计算下发速度。如果有报错:

    ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");

     这大概率是dwaComputeVelocityCommands中代价地图出现异常,导致函数返回false:

    if(path.cost_ < 0)
    {
      ROS_DEBUG_NAMED("dwa_local_planner",
         "The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
      local_plan.clear();
      publishLocalPlan(local_plan);
      return false;  
    }

    如果该函数没有问题,则move_base将会得到计算后的cmd_vel。

    三.dwaComputeVelocityCommands这个函数很有意思,它的形参是一个tf变换和一个待处理的速度cmd_vel,这个速度是从computeVelocityCommands函数中一脉相承过来的。

    1.用base_local_planner::OdometryHelperRos的对象odom_helper_来读取里程计读取的目前小车位姿global_pose;

    (1)预备知识1:base_local_planner::OdometryHelperRos位于base_local_planner包内,接收base_controller传出的odom话   题,将话题中的twist部分转化为tf变换(robot_vel),这一步是为了在dwa中的findBestPath函数中使用global_vel形参。

    2.dp_是指向DWAPlanner类的shared_ptr,调用DWAPlanner::findBestPath函数,在这个函数里变量后缀是costs_的,都是打分项,例如该函数第一句:

    obstacle_costs_.setFootprint(footprint_spec);
    

    这就是调用了打分项——避障打分,这个函数调用ObstacleCostFunction::setFootprint函数,具体在对base_local_planner的阅读中再分析过,在这里对避障打分类进行初始化。接着是对当前位姿、速度与终点位姿转化为矩阵,并用SimpleTrajectoryGenerator对generator_初始化,创造路径的采样空间。接下来:

    std::vector<base_local_planner::Trajectory> all_explored;
    scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);

    这是用base_local_planner::SimpleScoredSamplingPlanner的对象,对所有可能轨迹进行遍历。dwa算法的实现与base_local_planner包紧密相关,所以两个包需要结合一起阅读~接着在all_explored这个vector中遍历,若找到cost_>=0的轨迹(即我们需要的轨迹),则将pt(base_local_planner::MapGridCostPoint的对象)放入traj_cloud_中。(这是要发布一个由base_local_planner::MapGridCostPoint组成的topic?再发布一个可视化的代价给rviz?)

    随后将drive_velocities设置好(这是关键点,因为实际上cmd_vel的数值来源就是该参数),并返回一个最佳路径传回dwaComputeVelocityCommands。

    3.把drive_cmds(即findBestPath中的drive_velocities)参数赋予cmd_vel,这就是我们需要的下发速度。并且把base_local_planner::Trajectory格式的path转化成nav_msgs/Path,调用publishLocalPlan函数。在此需要注意:

    costmap_ros_->getGlobalFrameID()

    局部轨迹必须与代价地图处于同一frameID下。

    publishLocalPlan函数也是goal_functions.cpp中的函数。

    整个流程非常漫长、繁杂,很令人抓狂。但是把函数调用流程梳理一遍,可以准确定位错误来源,也很方便改写~

    展开全文
  • 1 """ 2 version1.1 3 Mobile robot motion planning sample with Dynamic Window Approach 4 结合https://blog.csdn.net/heyijia0327/article/details/44983551... 5 符号参考《煤矿救援机器人地图构建与路...
      1 """
      2 version1.1
      3 Mobile robot motion planning sample with Dynamic Window Approach
      4 结合https://blog.csdn.net/heyijia0327/article/details/44983551来看,里面含中文注释
      5 符号参考《煤矿救援机器人地图构建与路径规划研究》矿大硕士论文
      6 """
      7 
      8 import math
      9 import numpy as np
     10 import matplotlib.pyplot as plt
     11 
     12 show_animation = True  # 动画
     13 
     14 
     15 class Config(object):
     16     """
     17     用来仿真的参数,
     18     """
     19 
     20     def __init__(self):
     21         # robot parameter
     22         self.max_speed = 1.4  # [m/s]  # 最大速度
     23         # self.min_speed = -0.5  # [m/s]  # 最小速度,设置为可以倒车
     24         self.min_speed = 0  # [m/s]  # 最小速度,设置为不倒车
     25         self.max_yawrate = 40.0 * math.pi / 180.0  # [rad/s]  # 最大角速度
     26         self.max_accel = 0.2  # [m/ss]  # 最大加速度
     27         self.max_dyawrate = 40.0 * math.pi / 180.0  # [rad/ss]  # 最大角加速度
     28         self.v_reso = 0.01  # [m/s],速度分辨率
     29         self.yawrate_reso = 0.1 * math.pi / 180.0  # [rad/s],角速度分辨率
     30         self.dt = 0.1  # [s]  # 采样周期
     31         self.predict_time = 3.0  # [s]  # 向前预估三秒
     32         self.to_goal_cost_gain = 1.0  # 目标代价增益
     33         self.speed_cost_gain = 1.0  # 速度代价增益
     34         self.robot_radius = 1.0  # [m]  # 机器人半径
     35 
     36 
     37 def motion(x, u, dt):
     38     """
     39     :param x: 位置参数,在此叫做位置空间
     40     :param u: 速度和加速度,在此叫做速度空间
     41     :param dt: 采样时间
     42     :return:
     43     """
     44     # 速度更新公式比较简单,在极短时间内,车辆位移也变化较大
     45     # 采用圆弧求解如何?
     46     x[0] += u[0] * math.cos(x[2]) * dt  # x方向位移
     47     x[1] += u[0] * math.sin(x[2]) * dt  # y
     48     x[2] += u[1] * dt  # 航向角
     49     x[3] = u[0]  # 速度v
     50     x[4] = u[1]  # 角速度w
     51     # print(x)
     52 
     53     return x
     54 
     55 
     56 def calc_dynamic_window(x, config):
     57     """
     58     位置空间集合
     59     :param x:当前位置空间,符号参考硕士论文
     60     :param config:
     61     :return:目前是两个速度的交集,还差一个
     62     """
     63 
     64     # 车辆能够达到的最大最小速度
     65     vs = [config.min_speed, config.max_speed,
     66           -config.max_yawrate, config.max_yawrate]
     67 
     68     # 一个采样周期能够变化的最大最小速度
     69     vd = [x[3] - config.max_accel * config.dt,
     70           x[3] + config.max_accel * config.dt,
     71           x[4] - config.max_dyawrate * config.dt,
     72           x[4] + config.max_dyawrate * config.dt]
     73     #  print(Vs, Vd)
     74 
     75     # 求出两个速度集合的交集
     76     vr = [max(vs[0], vd[0]), min(vs[1], vd[1]),
     77           max(vs[2], vd[2]), min(vs[3], vd[3])]
     78 
     79     return vr
     80 
     81 
     82 def calc_trajectory(x_init, v, w, config):
     83     """
     84     预测3秒内的轨迹
     85     :param x_init:位置空间
     86     :param v:速度
     87     :param w:角速度
     88     :param config:
     89     :return: 每一次采样更新的轨迹,位置空间垂直堆叠
     90     """
     91     x = np.array(x_init)
     92     trajectory = np.array(x)
     93     time = 0
     94     while time <= config.predict_time:
     95         x = motion(x, [v, w], config.dt)
     96         trajectory = np.vstack((trajectory, x))  # 垂直堆叠,vertical
     97         time += config.dt
     98 
     99         # print(trajectory)
    100     return trajectory
    101 
    102 
    103 def calc_to_goal_cost(trajectory, goal, config):
    104     """
    105     计算轨迹到目标点的代价
    106     :param trajectory:轨迹搜索空间
    107     :param goal:
    108     :param config:
    109     :return: 轨迹到目标点欧式距离
    110     """
    111     # calc to goal cost. It is 2D norm.
    112 
    113     dx = goal[0] - trajectory[-1, 0]
    114     dy = goal[1] - trajectory[-1, 1]
    115     goal_dis = math.sqrt(dx ** 2 + dy ** 2)
    116     cost = config.to_goal_cost_gain * goal_dis
    117 
    118     return cost
    119 
    120 
    121 def calc_obstacle_cost(traj, ob, config):
    122     """
    123     计算预测轨迹和障碍物的最小距离,dist(v,w)
    124     :param traj:
    125     :param ob:
    126     :param config:
    127     :return:
    128     """
    129     # calc obstacle cost inf: collision, 0:free
    130 
    131     min_r = float("inf")  # 距离初始化为无穷大
    132 
    133     for ii in range(0, len(traj[:, 1])):
    134         for i in range(len(ob[:, 0])):
    135             ox = ob[i, 0]
    136             oy = ob[i, 1]
    137             dx = traj[ii, 0] - ox
    138             dy = traj[ii, 1] - oy
    139 
    140             r = math.sqrt(dx ** 2 + dy ** 2)
    141             if r <= config.robot_radius:
    142                 return float("Inf")  # collision
    143 
    144             if min_r >= r:
    145                 min_r = r
    146 
    147     return 1.0 / min_r  # 越小越好
    148 
    149 
    150 def calc_final_input(x, u, vr, config, goal, ob):
    151     """
    152     计算采样空间的评价函数,选择最合适的那一个作为最终输入
    153     :param x:位置空间
    154     :param u:速度空间
    155     :param vr:速度空间交集
    156     :param config:
    157     :param goal:目标位置
    158     :param ob:障碍物
    159     :return:
    160     """
    161     x_init = x[:]
    162     min_cost = 10000.0
    163     min_u = u
    164 
    165     best_trajectory = np.array([x])
    166 
    167     # evaluate all trajectory with sampled input in dynamic window
    168     # v,生成一系列速度,w,生成一系列角速度
    169     for v in np.arange(vr[0], vr[1], config.v_reso):
    170         for w in np.arange(vr[2], vr[3], config.yawrate_reso):
    171 
    172             trajectory = calc_trajectory(x_init, v, w, config)
    173 
    174             # calc cost
    175             to_goal_cost = calc_to_goal_cost(trajectory, goal, config)
    176             speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
    177             ob_cost = calc_obstacle_cost(trajectory, ob, config)
    178             #  print(ob_cost)
    179 
    180             # 评价函数多种多样,看自己选择
    181             # 本文构造的是越小越好
    182             final_cost = to_goal_cost + speed_cost + ob_cost
    183 
    184             # search minimum trajectory
    185             if min_cost >= final_cost:
    186                 min_cost = final_cost
    187                 min_u = [v, w]
    188                 best_trajectory = trajectory
    189 
    190     # print(min_u)
    191     #  input()
    192 
    193     return min_u, best_trajectory
    194 
    195 
    196 def dwa_control(x, u, config, goal, ob):
    197     """
    198     调用前面的几个函数,生成最合适的速度空间和轨迹搜索空间
    199     :param x:
    200     :param u:
    201     :param config:
    202     :param goal:
    203     :param ob:
    204     :return:
    205     """
    206     # Dynamic Window control
    207 
    208     vr = calc_dynamic_window(x, config)
    209 
    210     u, trajectory = calc_final_input(x, u, vr, config, goal, ob)
    211 
    212     return u, trajectory
    213 
    214 
    215 def plot_arrow(x, y, yaw, length=0.5, width=0.1):
    216     """
    217     arrow函数绘制箭头
    218     :param x:
    219     :param y:
    220     :param yaw:航向角
    221     :param length:
    222     :param width:参数值为浮点数,代表箭头尾部的宽度,默认值为0.001
    223     :return:
    224     length_includes_head:代表箭头整体长度是否包含箭头头部的长度,默认值为False
    225     head_width:代表箭头头部的宽度,默认值为3*width,即尾部宽度的3倍
    226     head_length:代表箭头头部的长度度,默认值为1.5*head_width,即头部宽度的1.5倍
    227     shape:参数值为'full''left''right',表示箭头的形状,默认值为'full'
    228     overhang:代表箭头头部三角形底边与箭头尾部直接的夹角关系,通过该参数可改变箭头的形状。
    229     默认值为0,即头部为三角形,当该值小于0时,头部为菱形,当值大于0时,头部为鱼尾状
    230     """
    231     plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
    232               head_length=1.5 * width, head_width=width)
    233     plt.plot(x, y)
    234 
    235 
    236 def main():
    237     """
    238     主函数
    239     :return:
    240     """
    241     # print(__file__ + " start!!")
    242     # 初始化位置空间
    243     x = np.array([0.0, 0.0, math.pi / 2.0, 0.2, 0.0])
    244 
    245     goal = np.array([10, 10])
    246 
    247     # matrix二维矩阵
    248     # ob = np.matrix([[-1, -1],
    249     #                 [0, 2],
    250     #                 [4.0, 2.0],
    251     #                 [5.0, 4.0],
    252     #                 [5.0, 5.0],
    253     #                 [5.0, 6.0],
    254     #                 [5.0, 9.0],
    255     #                 [8.0, 9.0],
    256     #                 [7.0, 9.0],
    257     #                 [12.0, 12.0]
    258     #                 ])
    259     ob = np.matrix([[0, 2]])
    260     u = np.array([0.2, 0.0])
    261     config = Config()
    262     trajectory = np.array(x)
    263 
    264     for i in range(1000):
    265 
    266         u, best_trajectory = dwa_control(x, u, config, goal, ob)
    267 
    268         x = motion(x, u, config.dt)
    269         print(x)
    270 
    271         trajectory = np.vstack((trajectory, x))  # store state history
    272 
    273         if show_animation:
    274             draw_dynamic_search(best_trajectory, x, goal, ob)
    275 
    276         # check goal
    277         if math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) <= config.robot_radius:
    278             print("Goal!!")
    279 
    280             break
    281 
    282     print("Done")
    283 
    284     draw_path(trajectory, goal, ob, x)
    285 
    286 
    287 def draw_dynamic_search(best_trajectory, x, goal, ob):
    288     """
    289     画出动态搜索过程图
    290     :return:
    291     """
    292     plt.cla()  # 清除上次绘制图像
    293     plt.plot(best_trajectory[:, 0], best_trajectory[:, 1], "-g")
    294     plt.plot(x[0], x[1], "xr")
    295     plt.plot(0, 0, "og")
    296     plt.plot(goal[0], goal[1], "ro")
    297     plt.plot(ob[:, 0], ob[:, 1], "bs")
    298     plot_arrow(x[0], x[1], x[2])
    299     plt.axis("equal")
    300     plt.grid(True)
    301     plt.pause(0.0001)
    302 
    303 
    304 def draw_path(trajectory, goal, ob, x):
    305     """
    306     画图函数
    307     :return:
    308     """
    309     plt.cla()  # 清除上次绘制图像
    310 
    311     plt.plot(x[0], x[1], "xr")
    312     plt.plot(0, 0, "og")
    313     plt.plot(goal[0], goal[1], "ro")
    314     plt.plot(ob[:, 0], ob[:, 1], "bs")
    315     plot_arrow(x[0], x[1], x[2])
    316     plt.axis("equal")
    317     plt.grid(True)
    318     plt.plot(trajectory[:, 0], trajectory[:, 1], 'r')
    319     plt.show()
    320 
    321 
    322 if __name__ == '__main__':
    323     main()

     

    转载于:https://www.cnblogs.com/yangmingustb/p/8928765.html

    展开全文
  • dwa是很成熟的一套适用于差动轮式机器人的局部规划方法,项目需要,对其源码进行解析,看看有那些改进的点 有空把详细的解析发出来,自己理理思路 1.计算速度 2.获取轨迹 3.评价轨迹 4.评价机制 5....
  • 耳熟能详的局部避障动态窗口法(Dynamic Window Approach)概述:机器人局比避障规划方法有很多,ROS中主要采用的是动态窗口法(和原来的论文中的dwa方法不一样)。动态窗口法主要是在速度(v,w)空间中采样多组速度,并...
  • DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟这些速度在一定时间内的运动轨迹,再通过一个评价函数对这些轨迹打分,最优的速度被选择出来发送给下位机。 1 原理分析...
  • 改进的原有代码,修正错误,只需要输入起点和终点即可,并且任意位置都可成功寻路
  • rosparam命令可对ROS参数服务器上的参数进行操作。通过rosparam -h命令,可以看到有下面的一些方法: Commands: rosparam set set parameter 设置参数 rosparam get get parameter 获得参数值 rosparam load load ...
  • % 输入参数 :当前状态、参数允许范围(窗口)、目标点、障碍物位置、障碍物半径、评价函数的参数 % 返回参数: % evalDB N*5 每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分 % trajDB 每5行...
  • 动态窗口算法(DWA

    千次阅读 2019-10-11 09:40:09
    路径规划算法中动态窗口算法十分灵活好用,但发现在自动驾驶中几乎没有应用,原因大概就是...那么,动态窗口法核心的动态窗口,其实就是将机器人当前状态(即当前速度,航向角)及机器人运动模型(机器人所能达到的...
  • rosparam命令可对ROS参数...机器人运动学模型参数 最高速度m/s] 最高旋转速度[rad/s] 加速度[m/ss] 旋转加速度[rad/ss] 速度分辨率[m/s] 转速分辨率[rad/s]] 机器人局部避障的动态窗口法(dynamic window approach) ...
  • 机器人局部避障的动态窗口法(dynamic window approach)

    万次阅读 多人点赞 2015-04-10 19:23:54
    机器人局部路径规划方法有很多,ROS中主要采用的是动态窗口法(但是和原论文中的dwa方法不一样,具体见最后)。动态窗口法主要是在速度(v,w)空间中采样多组速度,并模拟机器人在这些速度下一定时间(sim_period)内的...
  • 动态窗口法(Dynamic Window Approach,DWA)是一类经典的机器人局部路径规划算法。它的过程主要分为两部分: 速度空间(v,ω)(v,\omega)(v,ω)的计算。考虑到实际的运动限制可以得到一个速度范围: V={(v,ω)∣v1...
  • 机器人路径规划之动态窗口法

    千次阅读 2020-03-19 17:56:56
    动态窗口法(Dynamic Window Approach)概述 DWA是一种基于速度的局部规划器,可计算达到目标所需的机器人的最佳无碰撞速度。 程序实现 DWA算法主要分三步: 计算动态窗口 计算最优[v,ω][v,\omega][v,ω] 更新...
  • 针对应用广泛的局部避障算法-----动态窗口法(DWA)穿越稠密障碍物时存在路径不合理、速度和安全性不能兼顾等问题,提出参数自适应的DWA算法,根据机器人与障碍物距离和障碍物的密集度自动调整目标函数中的权值,以自适应...
  • 动态窗口法(Dynamic Windows Approach) 有以下特点: 考虑机器人动力学特性; 考虑机器人惯性; 可以实现高速行驶。 根据运动方程的推导,可以把机器人的轨迹看成是两种形式:直线行走和弧线行走。前者 没有...
  • 参考: http://wiki.ros.org/dwa_local_planner 机器人局部避障的动态窗口法(dynamic window approach)
  • 动态窗的口法(Dynamic Window Approach,DWA)与Matlab中gif...动态窗口法根据机器人模型自身的有限速度和加速度约束,将笛卡尔坐标(x,y)转换成一组机器人速度集合(v是机器人的直线速度,w为机器人的角速度)...

空空如也

空空如也

1 2 3
收藏数 49
精华内容 19
关键字:

dwa动态窗口法