局部路径规划DWA算法详解

最新推荐文章于 2024-05-30 09:14:43 发布

Martin ZHOU

最新推荐文章于 2024-05-30 09:14:43 发布

阅读量195

点赞数

分类专栏：路径规划文章标签：动态规划

本文链接：https://blog.csdn.net/qq_29320587/article/details/131258252

版权

路径规划专栏收录该内容

6 篇文章 2 订阅

订阅专栏

在这里插入图片描述

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

在这里插入图片描述

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