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

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

     

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

                          <li class="tool-item tool-active is-like "><a href="javascript:;"><svg class="icon" aria-hidden="true">
                              <use xlink:href="#csdnc-thumbsup"></use>
                          </svg><span class="name">点赞</span>
                          <span class="count"></span>
                          </a></li>
                          <li class="tool-item tool-active is-collection "><a href="javascript:;" data-report-click="{&quot;mod&quot;:&quot;popu_824&quot;}"><svg class="icon" aria-hidden="true">
                              <use xlink:href="#icon-csdnc-Collection-G"></use>
                          </svg><span class="name">收藏</span></a></li>
                          <li class="tool-item tool-active is-share"><a href="javascript:;"><svg class="icon" aria-hidden="true">
                              <use xlink:href="#icon-csdnc-fenxiang"></use>
                          </svg>分享</a></li>
                          <!--打赏开始-->
                                                  <!--打赏结束-->
                                                  <li class="tool-item tool-more">
                              <a>
                              <svg t="1575545411852" class="icon" viewBox="0 0 1024 1024" version="1.1" xmlns="http://www.w3.org/2000/svg" p-id="5717" xmlns:xlink="http://www.w3.org/1999/xlink" width="200" height="200"><defs><style type="text/css"></style></defs><path d="M179.176 499.222m-113.245 0a113.245 113.245 0 1 0 226.49 0 113.245 113.245 0 1 0-226.49 0Z" p-id="5718"></path><path d="M509.684 499.222m-113.245 0a113.245 113.245 0 1 0 226.49 0 113.245 113.245 0 1 0-226.49 0Z" p-id="5719"></path><path d="M846.175 499.222m-113.245 0a113.245 113.245 0 1 0 226.49 0 113.245 113.245 0 1 0-226.49 0Z" p-id="5720"></path></svg>
                              </a>
                              <ul class="more-box">
                                  <li class="item"><a class="article-report">文章举报</a></li>
                              </ul>
                          </li>
                                              </ul>
                  </div>
                              </div>
              <div class="person-messagebox">
                  <div class="left-message"><a href="https://blog.csdn.net/aihuo7077">
                      <img src="https://profile.csdnimg.cn/3/9/0/3_aihuo7077" class="avatar_pic" username="aihuo7077">
                                              <img src="https://g.csdnimg.cn/static/user-reg-year/2x/4.png" class="user-years">
                                      </a></div>
                  <div class="middle-message">
                                          <div class="title"><span class="tit"><a href="https://blog.csdn.net/aihuo7077" data-report-click="{&quot;mod&quot;:&quot;popu_379&quot;}" target="_blank">aihuo7077</a></span>
                                              </div>
                      <div class="text"><span>发布了0 篇原创文章</span> · <span>获赞 0</span> · <span>访问量 495</span></div>
                  </div>
                                  <div class="right-message">
                                              <a href="https://im.csdn.net/im/main.html?userName=aihuo7077" target="_blank" class="btn btn-sm btn-red-hollow bt-button personal-letter">私信
                          </a>
                                                              <a class="btn btn-sm  bt-button personal-watch" data-report-click="{&quot;mod&quot;:&quot;popu_379&quot;}">关注</a>
                                      </div>
                              </div>
          </div>
      
    展开全文
  • 机器人局部避障的动态窗口法(dynamic window approach)

    万次阅读 多人点赞 2015-04-10 19:23:54
    机器人局部路径规划方法有很多,ROS中主要采用的是动态窗口法(但是和原论文中的dwa方法不一样,具体见最后)。动态窗口法主要是在速度(v,w)空间中采样多组速度,并模拟机器人在这些速度下一定时间(sim_period)内的...





    首先在V_m∩V_d的范围内采样速度:
    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)  //如果能够及时刹车,该对速度可接收
    	如果这组速度可接受,接下来利用评价函数对其评价,找到最优的速度组
    

    来源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html
    BEGIN DWA(robotPose,robotGoal,robotModel)
       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) 
              //clearance与原论文稍不一样
             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
    




    (转载请注明作者和出处:http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途)



    参考:

    dwa:

    1.Fox.《The Dynamic Window Approach To CollisionAvoidance》

    2.MarijaSeder. 《dynamic window based approach tomobile robot motion control in the presence of moving obstacles》

    3.http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html

     

    运动模型:

    4. http://adrianboeing.blogspot.com.au/2010/09/circular-motion-in-2d-for-graphics-and.html

    5.https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture5-Odometry.pdf

    6.http://rossum.sourceforge.net/papers/DiffSteer/


    最后贴出matlab仿真代码

    % -------------------------------------------------------------------------
    %
    % 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
    % -------------------------------------------------------------------------
    
    function [] = DynamicWindowApproachSample()
     
    close all;
    clear all;
     
    disp('Dynamic Window Approach sample program start!!')
    
    x=[0 0 pi/2 0 0]';% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
    goal=[10,10];% 目标点位置 [x(m),y(m)]
    % 障碍物位置列表 [x(m) y(m)]
    % obstacle=[0 2;
    %           4 2;
    %           4 4;
    %           5 4;
    %           5 5;
    %           5 6;
    %           5 9
    %           8 8
    %           8 9
    %           7 9];
    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)];
    
    % 评价函数参数 [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
    for i=1:5000
        % DWA参数输入
        [u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
        x=f(x,u);% 机器人移动到下一个时刻
        
        % 模拟结果的保存
        result.x=[result.x; x'];
        
        % 是否到达目的地
        if norm(x(1:2)-goal')<0.5
            disp('Arrive Goal!!');break;
        end
        
        %====Animation====
        hold off;
        ArrowLength=0.5;% 
        % 机器人
        quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on;
        plot(result.x(:,1),result.x(:,2),'-b');hold on;
        plot(goal(1),goal(2),'*r');hold on;
        plot(obstacle(:,1),obstacle(:,2),'*k');hold on;
        % 探索轨迹
        if ~isempty(traj)
            for it=1:length(traj(:,1))/5
                ind=1+(it-1)*5;
                plot(traj(ind,:),traj(ind+1,:),'-g');hold on;
            end
        end
        axis(area);
        grid on;
        drawnow;
        %movcount=movcount+1;
        %mov(movcount) = getframe(gcf);% 
    end
    toc
    %movie2avi(mov,'movie.avi');
     
    
    function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)
    
    % Dynamic Window [vmin,vmax,wmin,wmax]
    Vr=CalcDynamicWindow(x,model);
    
    % 评价函数的计算
    [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam);
    
    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);% 最优评价函数
    u=evalDB(ind,1:2)';% 
    
    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),前向模拟时间;
            % 各评价函数的计算
            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];
            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
    
    function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model)
    % 轨迹生成函数
    % evaldt:前向模拟时间; vt、ot当前速度和角速度; 
    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;
    stopDist=0;
    while vel>0
        stopDist=stopDist+vel*dt;% 制动距离的计算
        vel=vel-model(3)*dt;% 
    end
    
    function dist=CalcDistEval(x,ob,R)
    % 障碍物距离评价函数
    
    dist=100;
    for io=1:length(ob(:,1))
        disttmp=norm(ob(io,:)-x(1:2)')-R;%僷僗偺埵抲偲忈奞暔偲偺僲儖儉岆嵎傪寁嶼
        if dist>disttmp% 离障碍物最小的距离
            dist=disttmp;
        end
    end
    
    % 障碍物距离评价限定一个最大值,如果不设定,一旦一条轨迹没有障碍物,将太占比重
    if dist>=2*R
        dist=2*R;
    end
    
    function heading=CalcHeadingEval(x,goal)
    % heading的评价函数计算
    
    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)
    %
    global dt;
    % 车子速度的最大最小范围
    Vs=[0 model(1) -model(2) model(2)];
    
    % 根据当前速度以及加速度限制计算的动态窗口
    Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt];
    
    % 最终的Dynamic Window
    Vtmp=[Vs;Vd];
    Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
    
    function x = f(x, u)
    % Motion Model
    % u = [vt; wt];当前时刻的速度、角速度
    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;
    
    function radian = toRadian(degree)
    % degree to radian
    radian = degree/180*pi;
    
    function degree = toDegree(radian)
    % radian to degree
    degree = radian/pi*180;



    展开全文
  • 针对应用广泛的局部避障算法-----动态窗口法(DWA)穿越稠密障碍物时存在路径不合理、速度和安全性不能兼顾等问题,提出参数自适应的DWA算法,根据机器人与障碍物距离和障碍物的密集度自动调整目标函数中的权值,以自适应...
  • 机器人局部路径规划—动态窗口法 参考博客:机器人局部避障的动态窗口法(dynamic window approach) 动态窗口法(Dynamic Window Approach,DWA)是一类经典的机器人局部路径规划算法。它的过程主要分为两部分: ...

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

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

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

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

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

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

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

    1.速度空间计算

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

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

    1.1.移动最大速度限制

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

    ωs[ωmin,ωmax] \omega_s \in [\omega_{min},\omega_{max}]

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

    在这里插入图片描述

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

    1.2.移动最大加速度限制

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

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

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

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

    在这里插入图片描述

    1.3.允许速度(Admissible Velocities)

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

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

    ωa2distω˙a \omega_a \leq \sqrt{2 \cdot dist \cdot \dot{\omega}_a}

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

    在这里插入图片描述

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

    2.速度空间评价

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

    the list of    ω:[20,19,...,20],  n=41 \text{the list of}\;\;\omega:[-20^{\circ},-19^{\circ},...,20^{\circ}], \; n=41

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

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

    2.1.轨迹预测

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

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

    在这里插入图片描述

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

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

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

    2.2.速度评价

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

    G(vi,ωi)=σ(αheading(vi,ωi)+βdist(vi,ωi)+γvelocity(vi,ωi)) G(v_i,\omega_i)=\sigma(\alpha \cdot heading(v_i,\omega_i)+\beta \cdot dist(v_i,\omega_i)+\gamma \cdot velocity(v_i,\omega_i))

    2.2.1.方向角评价

    heading(vi,ωi)heading(v_i,\omega_i)用于评价机器人在给定角速度下运动的角度与目标角度之间的差值。显然,根据函数描述,该函数值越小,方向角评价应该越高。
    heading(vi,ωi)=180targetcurθ heading(v_i,\omega_i)=180^{\circ}-|target-cur\theta|

    在这里插入图片描述

    2.2.2.障碍物距离评价

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

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

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

    在这里插入图片描述

    2.2.3.速度评价

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

    3.总结

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

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

    在这里插入图片描述

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

    展开全文
  • 机器人路径规划之动态窗口法

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

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

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

    DWA

    程序实现

    DWA算法主要分三步:

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

    流程图如下:

    流程图

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

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

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

    获取动态窗口

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

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

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

    对动态窗口采样,得到N×MN\times M[vij,ωij]i<N,j<M[v_{ij},\omega_{ij}]|i<N,j<M,并计算v=vij,ω=ωijv=v_{ij},\omega=\omega_{ij}时机器人的预测轨迹。接下来计算机器人v=vij,ω=ωijv=v_{ij},\omega=\omega_{ij}时的评价函数:
    G(v,ω)=a1heading(v,ω)+a2dist(v,ω)+a3velocity(v,ω) G(v,\omega)=a_1\cdot heading(v,\omega)+a_2\cdot dist(v,\omega)+a_3\cdot velocity(v,\omega)

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

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

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

    更新状态

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

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

    完整代码参见这里

    展开全文
  • 路径规划: 局部路径规划 - 动态窗口法(dynamic window approach) 简介 在ROS中有很多种局部路径的规划方法,这里来介绍动态窗口法(dynamic window approach)。动态窗口法是 Dieter Fox, Wolfram Burgard, 和 ...
  • 耳熟能详的局部避障动态窗口法(Dynamic Window Approach)概述:机器人局比避障规划方法有很多,ROS中主要采用的是动态窗口法(和原来的论文中的dwa方法不一样)。动态窗口法主要是在速度(v,w)空间中采样多组速度,并...
  • 参考: http://wiki.ros.org/dwa_local_planner 机器人局部避障的动态窗口法(dynamic window approach)
  • DWA动态窗口法的原理及应用

    千次阅读 多人点赞 2020-04-03 20:56:22
    完全是对学术的不尊重吧,...DWA的核心在于所谓的动态窗口,这个东西在sampling based method 中就是sampling. 对于sampling可以是去sample state,也可以sample action. 百度的lattice planner其实就是在sample sta...
  • 动态窗口法_version1.3

    2019-09-25 13:52:37
    1 """ 2 version1.3 3 Mobile robot motion planning sample with Dynamic Window Approach 4 结合https://blog.csdn.net/heyijia0327/article/details/44983551... 5 符号参考《煤矿救援机器人地图构建与路...
  • 源:机器人局部避障的动态窗口法(dynamic window approach) 首先在V_m∩V_d的范围内采样速度: allowable_v = generateWindow(robotV, robotModel) allowable_w = generateWindow(robotW, robotModel...
  • 这个方法很简单,网上资料也很多,这里就不介绍了,具体参考论文为Mobile robot motion planning sample with Dynamic Window Approach,我使用Matlab复现了该方法,该代码可在我...机器人局部避障的动态窗口法 ...
  • 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 3 4 5 ... 20
收藏数 471
精华内容 188
关键字:

动态窗口法