精华内容
下载资源
问答
  • DWA轨迹规划
    千次阅读
    更多相关内容
  • dwa轨迹规划,局部路径规划

    千次阅读 2021-05-09 16:14:41
    # 更新机器人状态的函数 u ; u:卡尔曼滤波中表示控制向量 def updateState(state,u,dt): """ :param state [x,y,里程计走的角度,当前的速度,当前的角速度]: :param u 控制向量 [速度,角速度]: ...
    展开全文
  • 动态窗口法,可用于局部轨迹规划。手动设置运动学约束和动力学约束信息。
  • DWA路径规划算法

    千次阅读 2022-06-21 00:43:59
    来源丨古月居1.DWA路径规划基本原理动态窗口法主要是在速度(v,w)空间中采样多组速度,并模拟机器人在这些速度下一定时间(sim_period)内的轨迹。在得到多组轨迹以后,对这些轨迹进行评价,选取最优轨迹所对应的速度来...

    来源丨古月居

    1.DWA路径规划基本原理

    动态窗口法主要是在速度(v,w)空间中采样多组速度,并模拟机器人在这些速度下一定时间(sim_period)内的轨迹。

    在得到多组轨迹以后,对这些轨迹进行评价,选取最优轨迹所对应的速度来驱动机器人运动。

    该算法突出点在于动态窗口这个名词,它的含义是依据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围内。

    2.DWA路径规划流程

    12b27da8cede00d3c30bf5290e2b14ec.png

    fa1841e6a7291521a805cfaf42e38a6c.png

    19d023f7cb03991460cb19b1cd0be5c0.png

    edc6be0f47d38e69f482fc5161d01400.png

    9a64b916be2e2c0e83498a027e3c997f.png

    3.栅格地图上绘制XY图像

    3.1 栅格地图和XY坐标系关系

    可以参考:

    https://blog.csdn.net/qq_42727752/article/details/117339892

    主要区别在于:

    • DWA算法是采用XY的坐标值来表示机器人位置和地图特征信息,其实用行列表示是一样的,只是 求解轨迹时,DWA算法路线很细致随机,用行列表示无法表示如[3.21,4.56]这种位置,因此需要进行行列值向坐标值的相互转换

    • MATLAB的plot函数和scatter函数等,绘制图像都是针对XY坐标系的

    • 在栅格地图上的坐标系X对应于列col,而坐标Y对于与行row

    10ece956a785b330b053c9bee7c76458.png

    3.2 栅格行列位置转坐标系函数sub2coord.m

    function coord = sub2coord(sub)
    %SUB2COORD 将行列式下标装换为坐标格式,此时的坐标格式和原本认知坐标方向也不一致(如下所示)
    %        1 2 3 4 5 6 7 .... X坐标
    %      1|——————————>
    %      2|
    %      3|
    %      4|
    %      5|
    %  Y坐标\/
    
    
        [l,w] = size(sub);
        % 长度l=2表示sub为2*n矩阵
        if l == 2
            coord(1,:) = sub(2,:);
            coord(2,:) = sub(1,:);
        end
        
        if w == 2
            coord(:,1) = sub(:,2);
            coord(:,2) = sub(:,1);
        end
    
    
    end

    3.3 栅格坐标系位置转行列位置函数coord2sub.m

    function sub = coord2sub(coord)
    %COORD2SUB 将坐标转换为矩阵行列格式,坐标格式为下图所示
    %        1 2 3 4 5 6 7 .... X坐标
    %      1|——————————>
    %      2|
    %      3|
    %      4|
    %      5|
    %  Y坐标\/
    
    
        [l,w] = size(coord);
        % 长度l=2表示sub为2*n矩阵
        if l == 2
            sub(1,:) = coord(2,:);
            sub(2,:) = coord(1,:);
        end
        
        if w == 2
            sub(:,1) = coord(:,2);
            sub(:,2) = coord(:,1);
        end
    end

    4.DWA路径规划MATLAB代码

    tips:本例只有动态障碍物,本例对于sub和coord的转换难理解可以见最后的纯坐标系代码
    DWA算法存在陷入局部最优的死循环(可以参考其他文献)

    4.1 MATLAB效果示例

    d2412cbb98da0449a630e10849337602.gif

    4.2 主函数:DWA_sub.m

    clear;close all;
    rows = 15; cols = 20;                           % 地图行列尺寸
    startSub = [15,2];                              % 起点行列位置
    goalSub = [2,20];                               % 终点行列位置
    dy_obsSub = [4,4; 13,5; 9,14; 2,18; 12,16];     % 动态障碍物行列
    step = 0;                                       % 动态障碍物更新运动的频率
    
    
    % 设置地图属性
    field = ones(rows, cols);
    field(startSub(1),startSub(2)) = 4;
    field(goalSub(1),goalSub(2)) = 5;
    dy_obsR = dy_obsSub(:,1);dy_obsC = dy_obsSub(:,2);
    dy_obsIndex = sub2ind([rows,cols],dy_obsR,dy_obsC);
    field(dy_obsIndex) = 3;
    % 颜色表征矩阵
    cmap = [1 1 1; ...       % 1-白色-空地
        0 0 0; ...           % 2-黑色-静态障碍
        1 0 0; ...           % 3-红色-动态障碍
        1 1 0;...            % 4-黄色-起始点
        1 0 1;...            % 5-品红-目标点
        0 1 0; ...           % 6-绿色-到目标点的规划路径
        0 1 1];              % 7-青色-动态规划的路径
    colormap(cmap);
    image(1.5,1.5,field);
    
    
    % 设置栅格属性
    grid on;hold on;
    set(gca,'gridline','-','gridcolor','k','linewidth',0.5,'GridAlpha',0.5);
    set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
    set(gca, 'XAxisLocation','top')
    axis image;
    
    
    % 机器人现在状态
    % 对于DWA算法,结算结果建立在XY坐标系上,先将数据转成XY格式
    % 坐标系只影响位置,对于机器人其他运动学不影响
    robotXY = sub2coord(startSub);
    goalCoord = sub2coord(goalSub);
    dy_obsCoord = sub2coord(dy_obsSub);
    
    
    robotT = pi/2;                      % 机器人当前方向角度(0->2pi)
    robotV = 0;                         % 机器人当前速度
    robotW = 0;                         % 机器人当前角速度
    obstacleR=0.6;                      % 冲突判定用的障碍物半径
    dt = 0.1;                           % 时间[s]
    
    
    % 机器人运动学模型
    maxVel = 1.0;                       % 机器人最大速度m/s
    maxRot = 20.0/180*pi;               % 机器人最大角速度rad/s
    maxVelAcc = 0.2;                    % 机器人最大加速度m/ss
    maxRotAcc = 50.0/180*pi;            % 机器人最大角加速度rad/ss
    
    
    % 评价系数
    alpha = 0.05;                       % 方位角评价系数α
    beta = 0.2;                         % 空隙评价系数β
    gama = 0.1;                         % 速度评价系数γ
    periodTime = 3.0;                   % 预测处理时间,也就是绿色搜寻轨迹的长度
    
    
    path = [];                          % 记录移动路径
    
    
    %% 开始DWA算法求解
    while true
        
        % 是否到达目的地,到达目的地则跳出循环
        if norm(robotXY-goalCoord) < 0.5
            break;
        end
        
        %% 1、求速度区间==============================================
        vel_rangeMin = max(0, robotV-maxVelAcc*dt);
        vel_rangeMax = min(maxVel, robotV+maxVelAcc*dt);
        rot_rangeMin = max(-maxRot, robotW-maxRotAcc*dt);
        rot_rangeMax = min(maxRot, robotW+maxRotAcc*dt);
        
        % 存放当前机器人状态的各个线速度角速度下的评价函数的值
        % evalDB格式为n*6,其中6列为下一状态速度、角速度、方向角函数、距离函数、速度函数、评价函数值
        evalDB = [];
        
        %% 2、计算不同线速度角速度下的评价函数,目的取出最合适的线速度和角速度******************
        for temp_v = vel_rangeMin : 0.01 : vel_rangeMax
            for temp_w = rot_rangeMin : pi/180 : rot_rangeMax
                %% 2.1 最关键内容,不同线速度角速度都是建立在机器人最初始状态下的
                rob_perState = [robotXY(1),robotXY(2),robotT,robotV,robotW]';
                
                %% 2.2 求出在sim_period时间后机器人的状态
                for time = dt:dt:periodTime
                    matE = [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];
                    matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1];
                    % 求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态
                    % 该模型公式参考模型1,其中dt即△t,利用微分思想即dt是一个很小的数这里均取0.1
                    rob_perState = matE*rob_perState+matV*[temp_v;temp_w];
                end
                
                %% 2.3 计算不同条件下的方位角评价函数,此时机器人状态是在预测时间后的假设状态
                % ①方向角评价函数是用来评价机器人在当前设定的采样速度下,
                % ②达到模拟轨迹末端时的朝向和目标之间的角度差距
                goalTheta=atan2(goalCoord(2)-rob_perState(2),goalCoord(1)-rob_perState(1));% 目标点的方位的角度
                evalTheta = abs(rob_perState(3)-goalTheta)/pi*180;
                heading = 180 - evalTheta;
                
                %% 2.4 计算不同条件下的空隙评价函数
                % ①空隙评价函数代表机器人在“当前轨迹上”与最近的障碍物之间的距离
                % ②如果在这条轨迹上没有障碍物,那就将其设定一个常数
                % ③障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重
                dist = inf;
                for i=1:length(dy_obsCoord(:,1))
                    % 利用二范数求距离
                    disttmp=norm(dy_obsCoord(i,:)-rob_perState(1:2)')-obstacleR;
                    % 保证dist是最近的障碍物距离
                    if dist>disttmp
                        dist=disttmp;
                    end
                end
                
                % 限定距离评价函数不能太大,同时对于没有障碍物的距离函数设置为2倍包容半径
                if dist>=2*obstacleR
                    dist=2*obstacleR;
                end
    
    
                
                %% 2.5 速度评价函数
                % 评价当前轨迹的速度值大小。速度越大,评分越高
                velocity = temp_v;
    
    
                %% 2.6 利用制动距离限定速度是在有效的
                % 制动距离的计算,保证所选的速度和加速度不会发生碰撞,参考了博客
                stopDist = temp_v*temp_v/(2*maxVelAcc);
                
                % 将有效的速度和角速度存入评价总的评价函数
                if dist>stopDist
                    evalDB=[evalDB;[temp_v temp_w heading dist velocity 0]];
                end
            end
        end
        
        
        %% 3、对当前状态所有假设的速度加速度组合的评价函数正则化,选取合适的加速度和速度作为下一状态
        % 如果评价函数为空则使得机器人停止,即evalDB全0
        if isempty(evalDB)
            evalDB = [0 0 0 0 0 0];
        end
        
        % 将评价函数进行正则化
        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
        
        % 最终评价函数的计算
        for i=1:length(evalDB(:,1))
            evalDB(i,6)=alpha*evalDB(i,3)+beta*evalDB(i,4)+gama*evalDB(i,5);
        end
        
        [~,ind]=max(evalDB(:,6));         % 选取出最优评价函数的索引
        nextVR=evalDB(ind,1:2)';          % 机器人下一速度和角速度即为该评价函数对应的速度和角速度
        
        %% 4、选择好角速度线速度更新机器人下一状态
        matE = [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];
        matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1];
        robot_NextState = matE*[robotXY(1),robotXY(2),robotT,robotV,robotW]'+matV*nextVR;
        
        % 更新状态开启下一轮DWA算法求解
        robotXY(1) = robot_NextState(1); robotXY(2) = robot_NextState(2);
        robotT = robot_NextState(3); robotV = robot_NextState(4);
        robotW = robot_NextState(5);
        
        % 将路径存放到路径矩阵,主要是为了绘图
        path = [path;[robotXY(1),robotXY(2)]];
        
        %% 5、绘制图像
        field(dy_obsIndex) = 1;
        dy_obsSub = coord2sub(dy_obsCoord);
        dy_obsR = dy_obsSub(:,1);dy_obsC = dy_obsSub(:,2);
        dy_obsIndex = sub2ind([rows,cols],dy_obsR,dy_obsC);
        field(dy_obsIndex) = 3;
        image(1.5,1.5,field);
        scatter(robotXY(1)+0.5,robotXY(2)+0.5,'r','LineWidth',1.5);     % 绘制机器人,红色圆圈表示
        plot(path(:,1)+0.5,path(:,2)+0.5,'-b');                                 % 绘制路径
        drawnow;
        
        %% 6、将障碍物位置更新实现障碍物也在移动,对于静态障碍物删除从此往下代码
        if mod(step,20) == 0
            movpos = [0,1; 0,-1; -1,0; 1,0];                 % 对应上下左右四个方向
            for i = 1:length(dy_obsCoord(:,1))
                temp_obs = dy_obsCoord(i,:);
                temp_pos = randi(4);
                
                % 移动范围限制在地图区间里
                if dy_obsCoord(i,1) + movpos(temp_pos,1) > 0 && dy_obsCoord(i,1) + movpos(temp_pos,1) < cols
                    if dy_obsCoord(i,2) + movpos(temp_pos,2) > 0 && dy_obsCoord(i,2) + movpos(temp_pos,2) < rows
                        dy_obsCoord(i,1) = dy_obsCoord(i,1) + movpos(temp_pos,1);
                        dy_obsCoord(i,2) = dy_obsCoord(i,2) + movpos(temp_pos,2);
                    end
                end
            end
        end
        step = step + 1;
    end

    5. DWA算法Python代码

    5.1 实现效果

    与MATLAB不同,Python代码是绕完所有静态障碍物

    b9ca2d529f6332dce1ffb87590ac2558.png

    5.2 自定义的基本函数PathPlanning.py

    参考(1.4.2节):

    https://blog.csdn.net/qq_42727752/article/details/117339892

    代码复制重命名即可

    5.3 DWA_py.py

    import numpy as np
    import copy
    import matplotlib.pyplot as plt
    import PathPlanning
    
    
    
    
    '''
    # 设置地图信息,其中rows和cols都是从行列的个数,而非最大行列下标值
    # 相比于MATLAB位置从1-->rows,Python的地图信息会相对于数值少1即是0-->rows-1
    # 绘制栅格地图的python和MATLAB的坐标系是一样的需要理解转化关系
    '''
    rows = 15
    cols = 20
    startSub = [14, 1]
    goalSub = [1, 3]
    dy_obsSub = [[3, 3], [12, 4], [8, 13], [1, 17], [11, 15]]
    
    
    field = np.ones([rows, cols])
    field[startSub[0], startSub[1]] = 4
    field[goalSub[0], goalSub[1]] = 5
    for i in range(len(dy_obsSub)):
        field[dy_obsSub[i][0], dy_obsSub[i][1]] = 3
    
    
    
    
    robotXY = PathPlanning.sub2coord(startSub)
    goalCoord = PathPlanning.sub2coord(goalSub)
    
    
    dy_obsCoord = []
    for i in range(len(dy_obsSub)):
        dy_obsCoord.append(PathPlanning.sub2coord(dy_obsSub[i]))
    
    
    '''
    # 机器人现在状态
    # 对于DWA算法,结算结果建立在XY坐标系上,先将数据转成XY格式
    # 坐标系只影响位置,对于机器人其他运动学不影响
    '''
    robotT = np.pi/2
    robotV = 0
    robotW = 0
    obstacleR = 0.6
    dt = 0.1
    
    
    maxVel = 1.0
    maxRot = 20.0/180*np.pi
    maxVelAcc = 0.2
    maxRotAcc = 50.0/180*np.pi
    
    
    alpha = 0.05
    beta = 0.2
    gama = 0.1
    periodTime = 3.0
    
    
    path = []
    pathx = []
    pathy = []
    PathPlanning.drawmap(field)
    
    
    
    
    '''
    # 代码注释同MATLAB代码是一致的
    '''
    while True:
        # 是否到达目的地,到达目的地则跳出循环
        dat = np.sqrt((robotXY[0]-goalCoord[0])*(robotXY[0]-goalCoord[0])+(robotXY[1]-goalCoord[1])*(robotXY[1]-goalCoord[1]))
        
        if dat < 0.5:
            break
    
    
        # 1、求速度区间==============================================
        vel_rangeMin = max(0, robotV-maxVelAcc*dt)
        vel_rangeMax = min(maxVel, robotV+maxVelAcc*dt)
        rot_rangeMin = max(-maxRot, robotW-maxRotAcc*dt)
        rot_rangeMax = min(maxRot, robotW+maxRotAcc*dt)
    
    
        # 存放当前机器人状态的各个线速度角速度下的评价函数的值
        # evalDB格式为n*6,其中6列为下一状态速度、角速度、方向角函数、距离函数、速度函数、评价函数值
        evalDB = []
    
    
        ## 2、计算不同线速度角速度下的评价函数,目的取出最合适的线速度和角速度******************
        for temp_v in np.arange(vel_rangeMin,vel_rangeMax,0.01):
            for temp_w in np.arange(rot_rangeMin,rot_rangeMax,np.pi/180):
                # 2.1 最关键内容,不同线速度角速度都是建立在机器人最初始状态下的
                rob_perState = [robotXY[0],robotXY[1],robotT,robotV,robotW]
    
    
                # 2.2 求出在sim_period时间后机器人的状态
                for time in np.arange(dt,periodTime,dt):
                    matE = np.array([[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]])
                    matV = np.array([[dt*np.cos(rob_perState[2]),0],[dt*np.sin(rob_perState[2]),0],[0,dt],[1,0],[0,1]])
                    # 求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态
                    # 该模型公式参考模型1,其中dt即△t,利用微分思想即dt是一个很小的数这里均取0.1
                    rob_perState = np.dot(matE,rob_perState) + np.dot(matV,np.array([temp_v,temp_w]))
                
                #  2.3 计算不同条件下的方位角评价函数,此时机器人状态是在预测时间后的假设状态
                #  ①方向角评价函数是用来评价机器人在当前设定的采样速度下,
                #  ②达到模拟轨迹末端时的朝向和目标之间的角度差距
                goalTheta = np.arctan2(goalCoord[1]-rob_perState[1],goalCoord[0]-rob_perState[0])
                evalTheta = np.abs(rob_perState[2]-goalTheta)/np.pi*180
                heading = 180 - evalTheta
    
    
                # 2.4 计算不同条件下的空隙评价函数
                #  ①空隙评价函数代表机器人在“当前轨迹上”与最近的障碍物之间的距离
                #  ②如果在这条轨迹上没有障碍物,那就将其设定一个常数
                #  ③障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重
                dist = np.inf
                for i in range(len(dy_obsCoord)):
                    disttmp = np.sqrt((dy_obsCoord[i][0]-rob_perState[0])*(dy_obsCoord[i][0]-rob_perState[0])
                                      + (dy_obsCoord[i][1]-rob_perState[1])*(dy_obsCoord[i][1]-rob_perState[1])) - obstacleR
                    
                    #  保证dist是最近的障碍物距离
                    if dist > disttmp:
                        dist = disttmp
    
    
                # 限定距离评价函数不能太大,同时对于没有障碍物的距离函数设置为2倍包容半径
                if dist > 2*obstacleR:
                    dist = 2*obstacleR
                    
    
    
                #  2.5 速度评价函数
                #  评价当前轨迹的速度值大小。速度越大,评分越高
                velocity = temp_v
    
    
                # 2.6 利用制动距离限定速度是在有效的
                #  制动距离的计算,保证所选的速度和加速度不会发生碰撞,参考了博客
                stopDist = temp_v*temp_v/(2*maxVelAcc)
    
    
                # 将有效的速度和角速度存入评价总的评价函数
                if dist>stopDist:
                    evalDB.append([temp_v,temp_w,heading,dist,velocity,0])
    
    
    
    
        # 3、对当前状态所有假设的速度加速度组合的评价函数正则化,选取合适的加速度和速度作为下一状态
        #  如果评价函数为空则使得机器人停止,即evalDB全0
        if len(evalDB) == 0:
            evalDB = [[0,0,0,0,0,0],[0,0,0,0,0,0]]
    
    
    
    
        # 4、将评价函数进行正则化
        evalDB = np.array(evalDB)
    
    
        sum_h = sum(evalDB[:,2])
        sum_d = sum(evalDB[:,3])
        sum_v = sum(evalDB[:,4])
    
    
    
    
        if sum_h != 0:
            for i in range(len(evalDB)):
                evalDB[i][2] = evalDB[i][2]/sum_h
    
    
        if sum_d != 0:
            for i in range(len(evalDB)):
                evalDB[i][3] = evalDB[i][3]/sum_d
    
    
        if sum_v != 0:
            for i in range(len(evalDB)):
                evalDB[i][4] = evalDB[i][4]/sum_v
        
        for i in range(len(evalDB)):
            evalDB[i][5] = alpha*evalDB[i][2]+beta*evalDB[i][3]+gama*evalDB[i][4]
    
    
        idx = np.argmax(evalDB[:,5])
        nextVR = evalDB[idx,0:2]
    
    
        # 5、选择好角速度线速度更新机器人下一状态
        matE = np.array([[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]])
        matV = np.array([[dt*np.cos(rob_perState[2]),0],[dt*np.sin(rob_perState[2]),0],[0,dt],[1,0],[0,1]])
        robot_NextState = np.dot(matE,[robotXY[0],robotXY[1],robotT,robotV,robotW]) + np.dot(matV,np.array(nextVR))
    
    
        robotXY[0] = robot_NextState[0]
        robotXY[1] = robot_NextState[1]
        robotT = robot_NextState[2]
        robotV = robot_NextState[3]
        robotW = robot_NextState[4]
    
    
        path.append([robotXY[0],robotXY[1]])
        pathx.append(robotXY[0]+0.5)
        pathy.append(robotXY[1]+0.5)
    
    
        plt.plot(pathx,pathy,'b')
        plt.pause(0.01)

    5.4 DWA代码纯坐标系版

    d44969ad0b430f90af039abbe1096d32.png

    clear;
    % 机器人现在状态
    robotX = 1;                         % 机器人当前X位置
    robotY = 1;                         % 机器人当前Y位置
    robotT = pi/2;                      % 机器人当前方向角度(0->2pi)
    robotV = 0;                         % 机器人当前速度
    robotW = 0;                         % 机器人当前角速度
    
    
    % 地图信息
    goal=[9,9];                         % 目标点位置 [x(m),y(m)]
    obstacle=[2,2;4,4;6,6;8,8];         % 障碍物位置列表 [x(m) y(m)]
    obstacleR=0.6;                      % 冲突判定用的障碍物半径
    dt = 0.1;                           % 时间[s]
    
    
    % 机器人运动学模型
    maxVel = 1.0;                       % 机器人最大速度m/s
    maxRot = 20.0/180*pi;               % 机器人最大角速度rad/s
    maxVelAcc = 0.2;                    % 机器人最大加速度m/ss
    maxRotAcc = 50.0/180*pi;            % 机器人最大角加速度rad/ss
    
    
    
    
    % 评价系数
    alpha = 0.05;                       % 方位角评价系数α
    beta = 0.2;                         % 空隙评价系数β
    gama = 0.1;                         % 速度评价系数γ
    periodTime = 3.0;                   % 预测处理时间,也就是绿色搜寻轨迹的长度
    
    
    area=[0 10 0 10];                   % 模拟区域范围 [xmin xmax ymin ymax]
    path = [];                          % 记录移动路径
    
    
    %% 开始DWA算法求解
    while true
        
        % 是否到达目的地,到达目的地则跳出循环
        if norm([robotX,robotY]-goal') < 0.5
            break;
        end
        
        %% 1、求速度区间==============================================
        vel_rangeMin = max(0, robotV-maxVelAcc*dt);
        vel_rangeMax = min(maxVel, robotV+maxVelAcc*dt);
        rot_rangeMin = max(-maxRot, robotW-maxRotAcc*dt);
        rot_rangeMax = min(maxRot, robotW+maxRotAcc*dt);
        
        % 存放当前机器人状态的各个线速度角速度下的评价函数的值
        % evalDB格式为n*6,其中6列为下一状态速度、角速度、方向角函数、距离函数、速度函数、评价函数值
        evalDB = [];
        
        %% 2、计算不同线速度角速度下的评价函数,目的取出最合适的线速度和角速度******************
        for temp_v = vel_rangeMin : 0.01 : vel_rangeMax
            for temp_w = rot_rangeMin : pi/180 : rot_rangeMax
                %% 2.1 最关键内容,不同线速度角速度都是建立在机器人最初始状态下的
                rob_perState = [robotX,robotY,robotT,robotV,robotW]';
                
                %% 2.2 求出在sim_period时间后机器人的状态
                for time = dt:dt:periodTime
                    matE = [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];
                    matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1];
                    % 求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态
                    % 该模型公式参考模型1,其中dt即△t,利用微分思想即dt是一个很小的数这里均取0.1
                    rob_perState = matE*rob_perState+matV*[temp_v;temp_w];
                end
                
                %% 2.3 计算不同条件下的方位角评价函数,此时机器人状态是在预测时间后的假设状态
                % ①方向角评价函数是用来评价机器人在当前设定的采样速度下,
                % ②达到模拟轨迹末端时的朝向和目标之间的角度差距
                goalTheta=atan2(goal(2)-rob_perState(2),goal(1)-rob_perState(1));% 目标点的方位的角度
                evalTheta = abs(rob_perState(3)-goalTheta)/pi*180;
                heading = 180 - evalTheta;
                
                %% 2.4 计算不同条件下的空隙评价函数
                % ①空隙评价函数代表机器人在“当前轨迹上”与最近的障碍物之间的距离
                % ②如果在这条轨迹上没有障碍物,那就将其设定一个常数
                % ③障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重
                dist = inf;
                for i=1:length(obstacle(:,1))
                    % 利用二范数求距离
                    disttmp=norm(obstacle(i,:)-rob_perState(1:2)')-obstacleR;
                    % 保证dist是最近的障碍物距离
                    if dist>disttmp
                        dist=disttmp;
                    end
                end
                
                % 限定距离评价函数不能太大,同时对于没有障碍物的距离函数设置为2倍包容半径
                if dist>=2*obstacleR
                    dist=2*obstacleR;
                end
    
    
                
                %% 2.5 速度评价函数
                % 评价当前轨迹的速度值大小。速度越大,评分越高
                velocity = temp_v;
    
    
                %% 2.6 利用制动距离限定速度是在有效的
                % 制动距离的计算,保证所选的速度和加速度不会发生碰撞,参考了博客
                stopDist = temp_v*temp_v/(2*maxVelAcc);
                
                % 将有效的速度和角速度存入评价总的评价函数
                if dist>stopDist
                    evalDB=[evalDB;[temp_v temp_w heading dist velocity 0]];
                end
            end
        end
        
        
        %% 3、对当前状态所有假设的速度加速度组合的评价函数正则化,选取合适的加速度和速度作为下一状态
        % 如果评价函数为空则使得机器人停止,即evalDB全0
        if isempty(evalDB)
            evalDB = [0 0 0 0 0 0];
        end
        
        % 将评价函数进行正则化
        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
        
        % 最终评价函数的计算
        for i=1:length(evalDB(:,1))
            evalDB(i,6)=alpha*evalDB(i,3)+beta*evalDB(i,4)+gama*evalDB(i,5);
        end
        
        [~,ind]=max(evalDB(:,6));         % 选取出最优评价函数的索引
        nextVR=evalDB(ind,1:2)';          % 机器人下一速度和角速度即为该评价函数对应的速度和角速度
        
        %% 4、选择好角速度线速度更新机器人下一状态
        matE = [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];
        matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1];
        robot_NextState = matE*[robotX,robotY,robotT,robotV,robotW]'+matV*nextVR;
        
        % 更新状态开启下一轮DWA算法求解
        robotX = robot_NextState(1); robotY = robot_NextState(2);
        robotT = robot_NextState(3); robotV = robot_NextState(4);
        robotW = robot_NextState(5);
        
        % 将路径存放到路径矩阵,主要是为了绘图
        path = [path;[robotX,robotY]];
        
        %% 5、绘制图像
        hold off;
        scatter(robotX,robotY,'r','LineWidth',1.5);hold on;     % 绘制机器人,红色圆圈表示
        plot(goal(1),goal(2),'*r');hold on;                     % 绘制地图终点
        scatter(obstacle(:,1),obstacle(:,2),200,'k');hold on;   % 绘制障碍物
        plot(path(:,1),path(:,2),'-b');hold on;                 % 绘制路径
        axis(area);
        grid on;
        drawnow;
        
        %% 6、将障碍物位置更新实现障碍物也在移动,对于静态障碍物删除从此往下代码
        movpos = [0,0.2; 0,-0.2; -0.2,0; 0.2,0];                 % 对应上下左右四个方向
        for i = 1:length(obstacle(:,1))
            temp_obs = obstacle(i,:);
            temp_pos = randi(4);
            
            % 移动范围限制在地图区间里
            if obstacle(i,1) + movpos(temp_pos,1) > 0 && obstacle(i,1) + movpos(temp_pos,1) < 10
                if obstacle(i,2) + movpos(temp_pos,2) > 0 && obstacle(i,2) + movpos(temp_pos,2) < 10
                    obstacle(i,1) = obstacle(i,1) + movpos(temp_pos,1);
                    obstacle(i,2) = obstacle(i,2) + movpos(temp_pos,2);
                end
            end
        end
        
        
    end

    本文仅做学术分享,如有侵权,请联系删文。

    3D视觉工坊精品课程官网:3dcver.com

    1.面向自动驾驶领域的多传感器数据融合技术

    2.面向自动驾驶领域的3D点云目标检测全栈学习路线!(单模态+多模态/数据+代码)
    3.彻底搞透视觉三维重建:原理剖析、代码讲解、及优化改进
    4.国内首个面向工业级实战的点云处理课程
    5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
    6.彻底搞懂视觉-惯性SLAM:基于VINS-Fusion正式开课啦
    7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
    8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

    9.从零搭建一套结构光3D重建系统[理论+源码+实践]

    10.单目深度估计方法:算法梳理与代码实现

    11.自动驾驶中的深度学习模型部署实战

    12.相机模型与标定(单目+双目+鱼眼)

    13.重磅!四旋翼飞行器:算法与实战

    14.ROS2从入门到精通:理论与实战

    15.国内首个3D缺陷检测教程:理论、源码与实战

    重磅!3DCVer-学术论文写作投稿 交流群已成立

    扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

    同时也可申请加入我们的细分方向交流群,目前主要有3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

    一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。

    0f11b99b4921de841b43d660533ae2a6.png

    ▲长按加微信群或投稿

    109d4faecee8d9af9d507201006ba0dc.png

    ▲长按关注公众号

    3D视觉从入门到精通知识星球:针对3D视觉领域的视频课程(三维重建系列三维点云系列结构光系列手眼标定相机标定激光/视觉SLAM自动驾驶等)、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近4000星球成员为创造更好的AI世界共同进步,知识星球入口:

    学习3D视觉核心技术,扫描查看介绍,3天内无条件退款

    9882084f486b5697b8ba099303c93d18.png

     圈里有高质量教程资料、答疑解惑、助你高效解决问题

    觉得有用,麻烦给个赞和在看~  

    展开全文
  • dwa_动态窗口法_轨迹规划_局部规划_DWA.zip
  • dwa_动态窗口法_轨迹规划_局部规划_DWA_源码.zip
  • 机器人局部路径规划DWA

    参考文献

    自动驾驶决策规划算法—DWA 动态窗口法
    机器人局部动态避障算法dwa解析

    简介

    dwa算法全称叫动态窗口法(dynamic window approach),其算法过程主要分为仿真获取机器人的运动轨迹、对轨迹进行评价选择最优轨迹两个主要过程,动态窗口表达的是仿真的运动轨迹数量有限,主要是因为机器人在较短的控制周期内只能达到一定的速度。

    一、机器人如何仿真获取运动轨迹

    1、获取机器人速度样本

    根据机器人当前速度以及运动特性,确定机器人可达的运动速度范围。由于运动的最终目的是到达目标点,因此,在到达目标点附近时,需要降低机器人运动速度,也就是进一步限制机器人的速度范围,以保证机器人能平稳的到达目标点。最后根据人为给定的样本数,在限制的速度范围内获得样本数个离散的速度样本,包括线速度和角速度。

    void SimpleTrajectoryGenerator::initialise(
        const Eigen::Vector3f &pos,                      //机器人的位置
        const Eigen::Vector3f &vel,                      //当前机器人速度
        const Eigen::Vector3f &goal,                     //目标点
        base_local_planner::LocalPlannerLimits *limits,  //运动特性(加速度、最大最小速度…)
        const Eigen::Vector3f &vsamples,                 //样本
        bool discretize_by_time)
    {
        //给定机器人的最大最小运动速度
        double max_vel_th = limits->max_vel_theta;
        double min_vel_th = -1.0 * max_vel_th;
        discretize_by_time_ = discretize_by_time;
        Eigen::Vector3f acc_lim = limits->getAccLimits();
        pos_ = pos;
        vel_ = vel;
        limits_ = limits;
        next_sample_index_ = 0;
        sample_params_.clear();
    
        double min_vel_x = limits->min_vel_x;
        double max_vel_x = limits->max_vel_x;
        double min_vel_y = limits->min_vel_y;
        double max_vel_y = limits->max_vel_y;
    
        // if sampling number is zero in any dimension, we don’t generate samples generically
        if (vsamples[0] * vsamples[1] * vsamples[2] > 0)
        {
            // compute the feasible velocity space based on the rate at which we run
            Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
            Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
            if (!use_dwa_)
            {
                //根据机器人位置到目标点的距离,限制机器人的最大运动速度
                double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
                max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
                max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);
                // 根据控制周期和加速度特性,确定机器人可达的最大最小速度
                // 此处用的是sim_time_仿真时间,确定的是接下来一段时间内机器人可达的运动速度范围,默认是1s
                max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
                max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
                max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);
                min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
                min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
                min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
            }
            else
            {
                // 此处用的sim_period_是控制周期,也就是只确定下一个控制周期机器人的运动速度范围
                max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
                max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
                max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
                min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
                min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
                min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
            }
            //根据给定的速度样本数,在速度空间内等间距的获取速度样本
            Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
            VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
            VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
            VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
            for (; !x_it.isFinished(); x_it++)
            {
                vel_samp[0] = x_it.getVelocity();
                for (; !y_it.isFinished(); y_it++)
                {
                    vel_samp[1] = y_it.getVelocity();
                    for (; !th_it.isFinished(); th_it++)
                    {
                        vel_samp[2] = th_it.getVelocity();
                        // ROS_DEBUG(“Sample %f, %f, %f”, vel_samp[0], vel_samp[1], vel_samp[2]);
                        sample_params_.push_back(vel_samp);
                    }
                    th_it.reset();
                }
                y_it.reset();
            }
        }
    }
    
    

    2、生成运动轨迹

    根据速度样本确定运动轨迹,是简单运行学知识,主要注意的是用的仿真时间产生的样本还是仿真周期产生的样本,其中仿真时间指的是人为设定的一段仿真时间,默认1秒,而仿真周期指的是算法的实际控制周期,默认为0.1秒。

    bool SimpleTrajectoryGenerator::generateTrajectory(
        Eigen::Vector3f pos,               //机器人的位姿
        Eigen::Vector3f vel,               //运动速度
        Eigen::Vector3f sample_target_vel, //样本速度
        base_local_planner::Trajectory &traj)
    { //需要生成的轨迹
        double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);
        double eps = 1e-4;
        traj.cost_ = -1.0; // placed here in case we return early
        // trajectory might be reused so we’ll make sure to reset it
        traj.resetPoints();
    
        //确定样本是否超过设定的最大移动速度
        // make sure that the robot would at least be moving with one of
        // the required minimum velocities for translation and rotation (if set)
        if ((limits_->min_vel_trans >= 0 && vmag + eps < limits_->min_vel_trans) &&
            (limits_->min_vel_theta >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_vel_theta))
        {
            return false;
        }
        // make sure we do not exceed max diagonal (x+y) translational velocity (if set)
        if (limits_->max_vel_trans >= 0 && vmag - eps > limits_->max_vel_trans)
        {
            return false;
        }
    
        //确定仿真使用的控制周期数
        int num_steps;
        if (discretize_by_time_)
        {
            num_steps = ceil(sim_time_ / sim_granularity_);
        }
        else
        {
            // compute the number of steps we must take along this trajectory to be “safe”
            double sim_time_distance = vmag * sim_time_;                    // the distance the robot would travel in sim_time if it did not change velocity
            double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
            num_steps =
                ceil(std::max(sim_time_distance / sim_granularity_,
                              sim_time_angle / angular_sim_granularity_));
        }
    
        if (num_steps == 0)
        {
            return false;
        }
    
        //确定生成轨迹的时间间隔(仅对利用仿真时间进行速度采样的情况)
        // compute a timestep
        double dt = sim_time_ / num_steps;
        traj.time_delta_ = dt;
    
        Eigen::Vector3f loop_vel;
        //连续加速意味着用的是仿真时间进行的速度采样,不是单个控制周期能达到的运动速度。因此,需要根据机器人的运动特性确定接下来的控制周期内机器人能达到的运动速度
        if (continued_acceleration_)
        {
            // assuming the velocity of the first cycle is the one we want to store in the trajectory object
            loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
            traj.xv_ = loop_vel[0];
            traj.yv_ = loop_vel[1];
            traj.thetav_ = loop_vel[2];
        }
        else
        {
            //否则用的就是仿真周期进行的采样,直接将采样速度作为生成轨迹的速度
            // assuming sample_vel is our target velocity within acc limits for one timestep
            loop_vel = sample_target_vel;
            traj.xv_ = sample_target_vel[0];
            traj.yv_ = sample_target_vel[1];
            traj.thetav_ = sample_target_vel[2];
        }
    
        //根据仿真的周期数,生成仿真轨迹
        for (int i = 0; i < num_steps; ++i)
        {
            // add the point to the trajectory so we can draw it later if we want
            traj.addPoint(pos[0], pos[1], pos[2]);
            //如果用的是仿真时间进行的速度采样,在每个仿真控制周期内,速度需要根据加减速特性确定
            if (continued_acceleration_)
            {
                // calculate velocities
                loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
                // ROS_WARN_NAMED(“Generator”, “Flag: %d, Loop_Vel %f, %f, %f”, continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
            }
            //根据速度和时间,确定运动轨迹上的下一个点
            // update the position of the robot using the velocities passed in
            pos = computeNewPositions(pos, loop_vel, dt);
    
        } // end for simulation steps
    
        return true; // trajectory has at least one point
    }
    
    

    二、如何对轨迹进行评价并选取最优轨迹

    1、代价函数

    oscillation_costs_ //振荡代价函数,一旦速度发生振荡,直接丢弃速度样本
    obstacle_costs_ //障碍物代价函数,当轨迹进入障碍物区,直接丢弃当前轨迹样本
    goal_costs_ //目标代价函数,优先选择距离局部目标点近的轨迹
    path_costs_ //路径代价函数,优先选择贴近全局路径的轨迹,横向偏差小
    goal_front_costs_ //保证与路径终点朝向一致
    alignment_costs_ //与path_costs差不多,保证与局部路径朝向一致
    

    注:为了能适应不同场景的需求,可以对这些代价函数配置不同的权重,从而能实现不同场景对这些代价函数的重视程度

    2、主要评价函数的实现

    2.1、障碍物代价函数

    通过仿真轨迹将机器人轮廓映射到全局坐标系下,并对轮廓边上的代价值进行分析,选择最大的代价值作为障碍物代价,从而确定机器人是否会撞到障碍物

    double CostmapModel::footprintCost(const geometry_msgs::Point &position,                 //机器人在全局坐标系下的位置
                                       const std::vector<geometry_msgs::Point> &footprint,   //机器人轮廓
                                       double inscribed_radius, double circumscribed_radius) //内切圆、外接圆半径
    {
        // used to put things into grid coordinates
        unsigned int cell_x, cell_y;
        // get the cell coord of the center point of the robot
        //获得机器人在地图坐标系下的坐标
        if (!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
            return -1.0;
        // if number of points in the footprint is less than 3, we’ll just assume a circular robot
        //当轮廓上的点数少于3时,认为机器人是个圆形机器人,并且只判断机器人中心是否在不可走区域
        if (footprint.size() < 3)
        {
            unsigned char cost = costmap_.getCost(cell_x, cell_y);
            // if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
            if (cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
                return -1.0;
            return cost;
        }
        // now we really have to lay down the footprint in the costmap grid
        unsigned int x0, x1, y0, y1;
        double line_cost = 0.0;
        double footprint_cost = 0.0;
        // footprint是一个多边形,判断该多边形是否与障碍物发生碰撞的方法是:计算多边形的所有边的最大代价值,从而确定是否与障碍物相撞
        // we need to rasterize each line in the footprint
        for (unsigned int i = 0; i < footprint.size() - 1; ++i)
        {
            // get the cell coord of the first point
            //获得地图中机器人轮廓上的一个点的坐标
            if (!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
                return -1.0;
            //获得地图中相邻轮廓点的坐标
            // get the cell coord of the second point
            if (!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
                return -1.0;
            //确定当前轮廓点与相邻轮廓点构成的边的最大代价值
            line_cost = lineCost(x0, x1, y0, y1);
            //选取所有边的最大代价值
            footprint_cost = std::max(line_cost, footprint_cost);
            // if there is an obstacle that hits the line… we know that we can return false right away
            if (line_cost < 0)
                return -1.0;
        }
        //获取第一个轮廓点与最后一个轮廓点构成的边的最大代价值
        // we also need to connect the first point in the footprint to the last point
        // get the cell coord of the last point
        if (!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
            return -1.0;
        // get the cell coord of the first point
        if (!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
            return -1.0;
        line_cost = lineCost(x0, x1, y0, y1);
        //确定所有边的最大代价值
        footprint_cost = std::max(line_cost, footprint_cost);
        if (line_cost < 0)
            return -1.0;
        // if all line costs are legal… then we can return that the footprint is legal
        return footprint_cost;
    }
    
    

    2.2、目标代价函数

    首先根据全局路径与局部代价地图的边界确定局部目标点,然后依据局部目标点生成栅格地图,每个栅格处的值代表当前栅格到目标点的距离,对于障碍物栅格,直接置为到目标点的距离无穷远。最后再根据轨迹末端点处栅格的位置,直接通过索引在地图中获取该位置距目标点的距离,作为距目标点的代价。因此,目标代价函数的主要任务是生成栅格地图。

    // mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
    void MapGrid::setLocalGoal(const costmap_2d::Costmap2D &costmap,                         //局部代价地图
                               const std::vector<geometry_msgs::PoseStamped> &global_plan)   //全局路径
    {
        sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
        int local_goal_x = -1;
        int local_goal_y = -1;
        bool started_path = false;
        //调整全局路径分辨率与地图分辨率一致
        std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
        adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
        // 将全局路径与局部代价地图边界的交点作为局部目标点
        for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i)
        {
            double g_x = adjusted_global_plan[i].pose.position.x;
            double g_y = adjusted_global_plan[i].pose.position.y;
            unsigned int map_x, map_y;
            if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION)
            {
                local_goal_x = map_x;
                local_goal_y = map_y;
                started_path = true;
            }
            else
            {
                if (started_path)
                {
                    break;
                } // else we might have a non pruned path, so we just continue
            }
        }
        if (!started_path)
        {
            ROS_ERROR(“None of the points of the global plan were in the local costmap, global plan points too far from robot”);
            return;
        }
        //构建距离优先队列,并添加局部目标点作为队列的第一个点
        queue<MapCell *> path_dist_queue;
        if (local_goal_x >= 0 && local_goal_y >= 0)
        {
            MapCell &current = getCell(local_goal_x, local_goal_y);
            costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
            current.target_dist = 0.0;
            current.target_mark = true;
            path_dist_queue.push(&current);
        }
        //按优先队列的顺序,从局部目标点开始以单个栅格为步长向外膨胀,从而直接确定出每个栅格距离局部目标点的距离
        computeTargetDistance(path_dist_queue, costmap);
    }
    

    2.3、路径代价函数

    该代价函数的实现原理与目标代价函数类似,只是该函数是以局部路径上的所有点作为起点向外膨胀来构建栅格地图,并且是以仿真轨迹上的所有点的距离值的和的平均值作为轨迹的代价值。

    展开全文
  • 关注同名微信公众号“混沌无形”,阅读更多有趣好文! 原文链接:机器人空间采样...基于运动空间采样的算法则在速度空间等距采样,通过评价函数选择最佳控制指令,驱动机器人运动,主要包括CVM类算法及DWA类算法..
  • 3 移动机器人路径规划5.1 DWA路径规划基本原理5.2 DWA路径规划流程5.3 栅格地图上绘制XY图像5.3.1 栅格地图和XY坐标系关系5.3.2 栅格行列位置转坐标系函数sub2coord.m5.3.3 栅格坐标系位置转行列位置函数coord2sub.m...
  • DWA局部规划

    2021-07-15 10:56:13
    提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 文章目录 前言 系列文章目录 前言 一、动态窗口算法运动规划(DWA) 1.定义 2. 伪代码 二、使用步骤 1.1.读入地图 2.截取本地地图(local_plan) 3...
  • DWA算法(dynamicwindowapproach)是移动机器人在运动模型下推算(v,w)对应的轨迹,确定速度采样空间或者说是动态窗口(三种限制);在速度空间(v,w)中采样多组速度,并模拟这些速度在一定时间内的运动轨迹,通过...
  • 参考Coursera自动驾驶课程-DWA算法 本文为该课程的学习笔记 1.学习目标 了解如何在自行车模型上增加线性/角加速度约束 了解这些约束如何影响我们的规划器 在规划过程中用动态窗口法处理这些约束 2.运动学...
  • DWA算法一般用于局部路径规划,该算法在速度空间内采样线速度和角速度,并根据机器人的运动学模型预测其下一时间间隔的轨迹。 对待评价轨迹进行评分,从而获得更加安全、平滑的最优局部路径。 一、机器人运动学模型 ...
  • 路径规划算法C++实现(三)--DWA

    千次阅读 多人点赞 2021-04-14 14:39:18
    DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟出这些速度在一定时间内的运动轨迹,并通过评价函数对这些轨迹进行评价,选取最优轨迹对应的速度驱动机器人运动。...
  • dwa算法,实现机器人局部路径规划,效果用opencv显示。 终点坐标随机改变 // dwaapp.cpp // #include "pch.h" #include <iostream> #include <math.h> #include<algorithm> #...
  • 关注同名微信公众号“混沌无形”,阅读更多有趣好文! 原文链接:机器人空间采样...基于运动空间采样的算法则在速度空间等距采样,通过评价函数选择最佳控制指令,驱动机器人运动,主要包括CVM类算法及DWA类算法..
  • DWA算法是基于机器人运动学与动力学理论的一种局部避障算法,它将对机器人的位置控制转换为对机器人的速度控制。DWA算法可以概括为三步:一是根据机器人自身的限制以及环境制约将速度的采样空间约束在一定范围内; 二...
  • #本文记录学习过程中的个人理解。...二者在生成轨迹时都会判断是否开启dwa标志位,进而执行不同的轨迹模拟策略。 这里通过以下三个方面比较轨迹模拟过程中开启dwa与否的差异: 速度采样范围 轨迹生成方式 控制命令...
  • 局部路径规划----dwa

    2021-08-07 19:21:51
    dwa_local_planner_params.yaml参数含义 #Robot Configuration Parameters机器人配置参数. acc_lim_x:2.5 #x方向的加速度绝对值. acc_lim_y:2.5 #y方向的加速度绝对值,该值只有全向移动的机器人才需配置. acc_lim_th...
  • ROS运动规划学习六---dwa_local_planner
  • DWA的局部规划方法

    千次阅读 2018-04-09 15:16:34
    动态窗口法(DWA) 通过搜索良好选择的速度空间来考虑机器人的运动学特性 速度空间-&gt;一类构型空间 机器人假设沿弧线运动 保证机器人在撞到障碍物前停下来 使用目标函数o( )选择优化速度 DWA代码 ...
  • 自动驾驶路径规划DWA算法原理解析

    万次阅读 多人点赞 2019-09-25 22:50:36
    这篇文章详细介绍一下最新的ROS给出的DWA算法的结构,以及各个重要的cost function的含义,帮助你们理解DWA算法的构成。
  • ROS局部运动规划器Teb/DWA

    千次阅读 2021-06-07 17:37:00
    TEB与DWA对比: DWA算法 DWA算法是比较普遍使用的,但是这个算法对我们需求来说并不是最合适的算法。 # Differential-drive robot configuration - necessary? 差分机器人配置参数 holonomic_robot: false #是否为...
  • dwa算法简介 动态窗口法(dynamic window approach, dwa),用于实现机器人的局部路径规划 实现原理: 在速度空间(v,w)中不断采样,模拟机器人在采样得到的速度下的运行轨迹,并针对这些轨迹进行评价,从而选取最优的...
  • 1 平滑处理 解读The Dynamic Window Approach to Collision Avoidance DWA(动态窗口)算法是用于局部路径规划的算法,已经在ROS中实现,在move_base堆栈中:http://wiki.ros.org/dwa_local_planner DWA算法第一次...
  • DWA算法在局部路径规划中的应用

    千次阅读 2019-09-01 15:30:32
    ROS的路径规划器分为全局路径和局部路径规划,其中局部路径规划器使用的最广的为dwa,个人理解为: 首先全局路径规划会生成一条大致的全局路径,局部路径规划器会把全局路径给分段,然后根据分段的全局路径的坐标,...
  • 路径规划 --- A*,DWA,D* 理论与代码注1. A*2. DWA3. D* 注 A* 通过损失函数 D的第一次计算和A一样,区别在于:地图障碍物或机器人位置改变后,能利用A计算后的信息,快速更新轨迹,而不需要在重新运行一次A,本质...
  • DWA算法分析

    万次阅读 多人点赞 2019-08-17 13:39:47
    机器人在获得目的地信息后,首先经过全局路径规划规划出一条大致可行的路线,然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略,ROS中主要是使用了DWA算法。在ROS中每当...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 428
精华内容 171
热门标签
关键字:

DWA轨迹规划