DWA局部规划

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

提示：这里可以添加本文要记录的大概内容：
例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。

---

提示：以下是本篇文章正文内容，下面案例可供参考

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
例如：第一章 Python 机器学习入门之pandas的使用

---

提示：写完文章后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

提示：这里可以添加本文要记录的大概内容：
例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、动态窗口算法运动规划(DWA)

1.定义

dynamic window approach算法的定义，根据wiki的定义
在机器人运动规划领域，动态窗方法为三位大佬Dieter Fox, Wolfram Burgard, and Sebastian Thrun 于1997发表的论文，被引用高达2800次。应用于实时碰撞避免策略，和其余碰撞避免算法不同，动态窗口直接考虑到机器人的动力学，特别是设计来用于受到速度和加速度约束的机器人。
其主要包含两部分：首先是生成一个有效的搜索区域，其次在搜索区域中选择一个最优解。其论文中讲到，搜索空间限制在短时间内可到达且无碰撞的安全圆周轨迹。优化目标是选择一个方向和速度，使机器人以最大的距离远离障碍物且能到达目标位置。
DWA算法流程图如下:

2. 伪代码

DWA的主要创新点是使用(v,w)（速度，角速度）空间来表示车辆在平面内的运动；
伪码如下：

//http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html
BEGIN DWA(robotPose,robotGoal,robotModel)
   desiredV = calculateV(robotPose,robotGoal)
   laserscan = readScanner()
   allowable_v = generateWindow(robotV, robotModel)
   allowable_w  = generateWindow(robotW, robotModel)
   for each v in allowable_v
      for each w in allowable_w
      dist = find_dist(v,w,laserscan,robotModel) // 遇到障碍物的距离
      breakDist = calculateBreakingDistance(v)
      if (dist > breakDist)  //can stop in time 能够在障碍物前停下来
         heading = hDiff(robotPose,goalPose, v,w) //计算朝向cost（注意要归一化）
         clearance = (dist-breakDist)/(dmax - breakDist)
         cost = costFunction(heading,clearance, abs(desired_v - v))
         if (cost > optimal)
            best_v = v
            best_w = w
            optimal = cost
    set robot trajectory to best_v, best_w
END

程序实现所需的主要依赖包为:numpy，math，matplotlib.pyplot

程序伪代码如算法\ref{arg:localPlanner}所示。
\begin{algorithm}
\caption{局部规划}\label{arg:localPlanner}
\KwData{Maximum linear velocity: $v_{max}$,Maximum linear acceleration: $\alpha_{max}$,
Linear velocity resolutions: $v_{reso}$,Maximum angular velocity: $\omega_{max}$,
Maximum angular acceleration: $\beta_{max}$,Angular velocity resolutions $\omega_{reso}$,
Time interval:$dT$,Forward predict time:$predict\_time$,Heading weight coefficient: $\alpha$,Safety weight coefficient: $\beta$,Efficiency weight coefficient: $\gamma$}
% Dt = 0.1                #时间间隔
% Predict_Time = 4.0      #模拟轨迹的持续时间$Predict_Time$
% alpha = 1.0             #距离目标点的评价函数的权重系数$alpha$
% Beta = 1.0             #速度评价函数的权重系数$Beta$
%gamma = 1.0             #距离障碍物距离的评价函数的权重系数$gamma$
\KwResult{The robot trajectory to $v_{best}, w_{best}$}
$robotPose \leftarrow (x_{start},y_{start},\theta_{start})$\;
$robotGoal \leftarrow (x_{end},y_{end},\theta_{end})$\;
$robotMode \leftarrow Kinematic\_model$\;
$BEGIN DWA(robotPose,robotGoal,robotModel)$\;
$desiredV \leftarrow calculateV(robotPose,robotGoal)$\;
$laserscan \leftarrow readScanner()$\;
$v_{allowable} \leftarrow generateWindow(robotV, robotModel)$\;
$w_{allowable} \leftarrow generateWindow(robotW, robotModel)$\;
\For{each $v \in v_allowable$} {
    \For {each $w \in w_{allowable}$}{
        $dist \leftarrow find_dist(v,w,laserscan,robotModel)$\; %遇到障碍物的距离
        $breakDist \leftarrow calculateBreakingDistance(v)$\;
        \If {dist > breakDist}{  %can stop in time 能够在障碍物前停下来
            $heading \leftarrow hDiff(robotPose,goalPose, v,w)$\; %计算朝向cost（注意要归一化）
            $clearance \leftarrow (dist-breakDist)/(dmax - breakDist)$\;
            $cost \leftarrow costFunction(heading,clearance, abs(desired_v - v))$\;
            \If {cost > optimal}{
                $ v_best \leftarrow v$\;
                $ w_best \leftarrow w$\;
                $ optimal \leftarrow cost$\;
            }
        }
    set robot trajectory to $v_{best}, w_{best}$
    }
}
\end{algorithm}

二、使用步骤

1.1.读入地图

代码如下（示例）：

  base_planner_.setPlan(orig_global_traj);
std::vector<geometry_msgs::PoseStamped> global_plan_;//头文件里定义 global_plan_变量

bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {

  global_plan_.clear();//reset the global plan

  global_plan_ = orig_global_plan;

  return true;
}

2.截取本地地图（local_plan）

/****
将参考路径根据costmap大小进行截取,获得适配costmap大小的参考路径地图
****/
base_planner_.getLocalPlan(pose1, transformed_plan);

3.根据获得的地图更新costmap参数

dwa_planner_->updatePlanAndLocalCosts(pose1, transformed_plan);

4.寻找最优轨迹

/**
* @brief  find best trajectory in the generated trajectories
* @pose1  current position of vehicle, including x,y,orientation(yaw/heading)
* @pose2  current velocity of vehicle, including linear velocity; linear accelerated velocity; angular velocity; accelerated angular velocity
* @drive_velocityies  the velocity selected to send to next module
* @footprint_spec     footprint of the vehicle
*/
    dwa_planner_->findBestPath(
      pose1,
      pose2,
      drive_velocities,
      footprint_spec);

##4.1寻找最优轨迹

bool findBestTrajectory(Trajectory &traj, std::vector<Trajectory> *all_explored)//利用此函数寻找最优轨迹
/****
Find Best Trajectory
利用速度采样空间生成轨迹空间,同时利用设定的评分标准对每条轨迹进行评分,选取最小分数的轨迹作为最佳轨迹,发布到循迹控制层
****/
   for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {
     count = 0;
     count_valid = 0;
     TrajectorySampleGenerator* gen_ = *loop_gen;
   
     while (gen_->hasMoreTrajectories()) {

       gen_success = gen_->nextTrajectory(loop_traj);
     
       if (gen_success == false) {
         continue;
       }
 
       loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
     
       if (all_explored != NULL) {
         loop_traj.cost_ = loop_traj_cost;
         all_explored->push_back(loop_traj);
       
       }
     

       if (loop_traj_cost >= 0) {
         count_valid++;
         if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
           best_traj_cost = loop_traj_cost;
           best_traj = loop_traj;
         }
       }
       count++;
       if (max_samples_ > 0 && count >= max_samples_) {
         break;
       }
     }
     if (best_traj_cost >= 0) {
     
       traj.xv_ = best_traj.xv_;
       traj.yv_ = best_traj.yv_;
       traj.thetav_ = best_traj.thetav_;
       traj.cost_ = best_traj_cost;
     
       traj.resetPoints();
       traj_point.traj.clear();
       double px, py, pth;

       for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
      
         best_traj.getPoint(i, px, py, pth);
      
         traj.addPoint(px, py, pth);
         
           dwa_local_planner::traj traj_obj;
           traj_obj.x = float(px);
           traj_obj.y = float(py);
           traj_obj.th = float(pth);
           traj_point.traj.push_back(traj_obj);
       }
     }
     
     ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
     if (best_traj_cost >= 0) {
       break;
     }
   }
   return best_traj_cost >= 0;
 }

2.读入数据

代码如下（示例）：

data = pd.read_csv(
    'https://labfile.oss.aliyuncs.com/courses/1283/adult.data.csv')
print(data.head())

该处使用的url网络请求的数据。

---

总结

提示：这里对文章进行总结：
例如：以上就是今天要讲的内容，本文仅仅简单介绍了pandas的使用，而pandas提供了大量能使我们快速便捷地处理数据的函数和方法。

一、pandas是什么？

示例：pandas 是基于NumPy 的一种工具，该工具是为了解决数据分析任务而创建的。

二、使用步骤

1.引入库

代码如下（示例）：

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import seaborn as sns
import warnings
warnings.filterwarnings('ignore')
import  ssl
ssl._create_default_https_context = ssl._create_unverified_context

2.读入数据

代码如下（示例）：

data = pd.read_csv(
    'https://labfile.oss.aliyuncs.com/courses/1283/adult.data.csv')
print(data.head())

该处使用的url网络请求的数据。

---

总结

提示：这里对文章进行总结：
例如：以上就是今天要讲的内容，本文仅仅简单介绍了pandas的使用，而pandas提供了大量能使我们快速便捷地处理数据的函数和方法。