路徑規劃與避障算法(八)---DWA算法流程之四---路徑選擇評價函數

版權聲明：本文爲博主原創文章，原創不易, 轉載請聯繫博主。
本篇博客將重點介紹DWA算法所採用的評價函數中與參考路徑相關的評價函數

評價函數:

軌跡主要依據以下三條準則進行評分,綜合評分後選取分數最小的路徑作爲下一時刻選擇路徑:

• Obstacle_costs 軌跡上是否存在障礙物以及距離障礙物的距離
• Path_costs 軌跡上點距離局部參考路徑最近距離
• Goal_costs 軌跡上點距離局部參考路徑終點最近距離

上篇博客路徑規劃與避障算法(七)—DWA算法流程之三—碰撞檢測評價函數中已經介紹了與障礙物碰撞檢測相關(Collision Detection)的評價函數,本篇博客將重點介紹與參考路徑相關的評價函數。

由於無人車感知層輸出的costmap是基於激光雷達等傳感器的實時數據計算得出，因此costmap的尺寸大小是有一定限制的，此時的路徑規劃需要分成兩種情況來討論：

• 參考路徑全程需要規劃
• 僅避障時需要規劃新路徑

當我們需要利用DWA算法依據全程參考軌跡進行實時平滑擬合時，需要添加一個截圖模塊，這個模塊的功能主要是根據車輛當前位置以及costmap的大小實時的將全局參考軌跡裁剪適配到costmap中。

Path_Costs

Path_Costs主要評價採樣軌跡上的點距離costmap上參考路徑的最近距離，此標準的建立主要是希望選擇運動趨勢儘可能貼近參考軌跡的採樣軌跡作爲無人車下一時刻的運動軌跡。

Goal_Costs

Goal_Costs主要評價採樣軌跡距離costmap上參考路徑終點的最近距離，此標準的建立主要是希望選擇運動朝向儘可能貼近參考路徑的採樣軌跡作爲無人車下一時刻的運動軌跡。

模塊代碼

以下代碼爲基於Costmap的碰撞檢測部分代碼,僅供交流,由於屬於項目開發,不能提供全套代碼,望見諒

利用costmap進行軌跡評價時，需要將軌跡點先轉換至costmap的柵格中，具體轉換方法如下所示：

void AstarSearch::poseToIndex(const geometry_msgs::Pose &pose, int *index_x, int *index_y, int *index_theta)
{
    *index_x = (pose.position.x - costmap_.info.origin.position.x)/costmap_.info.resolution;
    *index_y = (pose.position.y - costmap_.info.origin.position.y)/costmap_.info.resolution;
    
    tf::Quaternion tf_q;
    tf::quaternionMsgToTF(pose.orientation,tf_q);
    double yaw = tf::getYaw(tf_q);
    if(yaw < 0)
    {
        yaw = yaw + 2*M_PI;
    }
    else if (yaw >= 2*M_PI )
    {
        yaw = yaw - 2*M_PI;
    }
    

    static double one_angle_range = 2*M_PI / angle_size_;
    *index_theta = yaw / one_angle_range;
    *index_theta %= angle_size_;
}

對採樣軌跡進行評價時，需要對當前costmap進行預計算：

    /****
    @預計算
    @goal_costs: 計算軌跡路徑計算地圖中所有點到局部地圖末端點的最短距離
    @path_costs: 計算軌跡路徑計算地圖中所有點到整個局部地圖路徑的最短路徑
    ****/
    /****
    預計算,is_local_goal_function=true ==> goal_costs
    is_local_goal_function = false ==> path_costs
    ****/
    bool MapGridCostFunction::prepare() {
      map_.resetPathDist();
      if (is_local_goal_function_) {
        map_.setLocalGoal(*costmap_, target_poses_);
      } else {
        map_.setTargetCells(*costmap_, target_poses_);
      }
      return true;
    }

Path_Costs

/****
@計算地圖中所有點到整個局部地圖路徑的最短距離
****/
 void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,
      const std::vector<geometry_msgs::PoseStamped>& global_plan) {
    sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());

    bool started_path = false;

    queue<MapCell*> path_dist_queue;
    
    std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
    adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
    
    if (adjusted_global_plan.size() != global_plan.size()) {
      ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
    }
    
    unsigned int i;

    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(&current);
        started_path = true;
      } else if (started_path) {
          break;
      }
    }

    computeTargetDistance(path_dist_queue, costmap);
  }

Goal_Costs

/****
@計算地圖中所有點到末端終點的最短距離
@brief Update what cell is considered the next local goal
@Param costmap Information of the costmap
@Param global_plan Input of the reference trajecotry
****/
 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());

    // skip global path points until we reach the border of the local map
    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
      }
    }
    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);
  }

軌跡評分

當依據提出的三條標準：

• Obstacle_costs 軌跡上是否存在障礙物以及距離障礙物的距離
• Path_costs 軌跡上點距離局部參考路徑最近距離
• Goal_costs 軌跡上點距離局部參考路徑終點最近距離

對每條採樣軌跡進行打分後，在對每項標準提供一個權重值，工程師可以根據自身項目以及場景需求對每項權重值進行設置，最終得到的加權乘積值之和即爲該條採樣軌跡的最終評分，對每條軌跡的分數進行比較篩選，其中最小分數的軌跡即爲下一時刻路徑行駛的實時軌跡。公式如下：
$Score=\alpha * Costs_{Obstacle} + \beta * Costs_{Path} + \Gamma * Costs_{Goal}$
其中 $\alpha, \beta, \Gamma$ 分別表示各個評分標準的權重值

---

以上已經完成DWA算法在無人車避障功能中的全部講解，整個系列都是筆者依據自身項目場景以及功能需求得出的一些經驗，若有不完善以及不正確的地方還希望大家指正包涵。也很歡迎大家隨時溝通無人車規劃方向的問題。