DWA算法分析

DWA Local Planner这部分是属于Local planner，在Navigation中有两个包：

dwa_local_planner

和

base_local_planner

查看了

dwa_local_planner

，发现其实际是调用的

base_local_planner

中的函数，而

base_local_planner

中包含了两种planner ：

DWA

或者

Trajectory Rollout approach

。所以结论就是，只需要搞清楚

base_local_planner

的执行就OK。

base_local_planner

package 实际是继承于

BaseLocalPlanner

：

class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner

对于基类接口定义：

namespace nav_core {
class BaseLocalPlanner{
public:
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
virtual bool isGoalReached() = 0;
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
virtual ~BaseLocalPlanner(){}
protected:
BaseLocalPlanner(){}
};
};

因此，派生类必须实现基类的接口。类

TrajectoryPlannerROS

的方法：



在成员变量中，重要的几个变量如下：

WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
costmap_2d::Costmap2DROS* costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
costmap_2d::Costmap2D* costmap_; ///< @brief The costmap the controller will use
std::vector<geometry_msgs::PoseStamped> global_plan_;

其实最重要的就是去找到代价最小代价的velocity。通过在三个维度（x，y，theta）的速度，加速度的采样，得到候选velocity，然后对这些velocity做cost 评判，评判的标准是：

if (!heading_scoring_) {
cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
} else {
cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost + 0.3 * heading_diff;
}
traj.cost_ = cost;

那么，这里面的

path_dist，goal_dist，occ_cost，heading_diff

是怎么计算的呢？

occ_cost

的计算很简单，直接通过costmap就能得到这个值:

double footprint_cost = footprintCost(x_i, y_i, theta_i);
occ_cost = std::max( std::max(occ_cost, footprint_cost),
double(costmap_.getCost(cell_x, cell_y)));

计算

path_dist，goal_dist

：

path_dist = path_map_(cell_x, cell_y).target_dist;
goal_dist = goal_map_(cell_x, cell_y).target_dist;

那么如何更新

path_dist，goal_dist

: 需要

costmap_ , global_plan_

path_map_.setTargetCells(costmap_, global_plan_);
goal_map_.setLocalGoal(costmap_, global_plan_);

首先看 成员函数

setTargetCells

：

//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,const std::vector<geometry_msgs::PoseStamped>& global_plan)

这个函数会根据global_plan更新costmap，如何做到的呢？

//将adjusted_global_plan的信息，打包成MapCell类型，塞到path_dist_queue
for (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) {
MapCell& current = getCell(map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push(¤t);
started_path = true;
} else if (started_path) {
break;
}
}
if (!started_path) {
ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}

computeTargetDistance(path_dist_queue, costmap);

于是重点来了，最后一行

computeTargetDistance(path_dist_queue, costmap);

，这个函数的实际算法实现：

void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
MapCell* current_cell;
MapCell* check_cell;
unsigned int last_col = size_x_ - 1;
unsigned int last_row = size_y_ - 1;
while(!dist_queue.empty()){
current_cell = dist_queue.front();
dist_queue.pop();

if(current_cell->cx > 0){
check_cell = current_cell - 1;
if(!check_cell->target_mark){
//mark the cell as visisted
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}

if(current_cell->cx < last_col){
check_cell = current_cell + 1;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}

if(current_cell->cy > 0){
check_cell = current_cell - size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}

if(current_cell->cy < last_row){
check_cell = current_cell + size_x_;
if(!check_cell->target_mark){
check_cell->target_mark = true;
if(updatePathCell(current_cell, check_cell, costmap)) {
dist_queue.push(check_cell);
}
}
}
}
}

上述代码检查

current_cell

的四个邻接cell， 然后不断的扩散，每个cell相对于之前的cell，更新target_dist :

double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist) {
check_cell->target_dist = new_target_dist;
}

  inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap){

//if the cell is an obstacle set the max path distance
unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
(cost == costmap_2d::LETHAL_OBSTACLE ||
cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_2d::NO_INFORMATION)){
check_cell->target_dist = obstacleCosts();
return false;
}

double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist) {
check_cell->target_dist = new_target_dist;
}
return true;
}