路徑規劃與避障算法(七)---DWA算法流程之三---碰撞檢測評價函數

版權聲明：本文爲博主原創文章，原創不易, 轉載請聯繫博主。
本篇博客主要介紹DWA算法所採用的評價函數中障礙物相關的評價函數

評價函數:

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

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

由上篇博客(路徑規劃與避障算法(六))描述,無人車依據自行車模型以及當前速度條件及約束可以得到相應的軌跡採樣空間,接下來,將利用以上三條評價標準對採樣空間中的每一條路徑進行評分與比較,選擇綜合分數最小的一條軌跡作爲無人車下一時刻的運動軌跡.

對採樣空間中的所有軌跡都進行遍歷評分是一個十分耗時且浪費計算資源的事情,因此在對所遍歷的路徑進行評分時,比較建議採用優先進行碰撞檢測的方式來減小計算資源的損耗與浪費.同時,車輛避障的實時性也是衡量無人車性能的一個重要標準,在低速場景下如園區物流,礦山卡車,港口物流車等實時性的優劣可能並不會影響無人車避障的性能,但是在高速場景下,無人車的實時性好壞將直接決定無人車避障功能的安全性.

因此,本篇博客將重點介紹與無人車避障路徑中與障礙物相關的評價函數:

• 當採樣路徑上包含障礙物時,直接排除該路徑,不再評分後續的評價函數
• 當採樣路徑上不包含障礙物時,則考慮軌跡上點到障礙物的最短距離

碰撞檢測

通常碰撞檢測主要有以下兩類方法:

• 幾何邊界碰撞檢測(Bounding Space and Hierarchies)
• 柵格空間覆蓋枚舉(Spatial Occupancy Enumeration)

1. 幾何邊界碰撞檢測

常規的幾何邊界碰撞檢測只需要考慮車輛的包絡圖形是否與障礙物存在覆蓋,若存在覆蓋,則判斷出現碰撞現象如圖1.這種檢測方法可以快速的進行碰撞重疊測試,但是因爲包絡圖形過大覆蓋的問題,可能導致檢測結果精度不高,因此後期又提出了改進型(Hirearchies)的措施.

改進型的方法將複雜的凸邊形分解成爲一顆樹,該樹代表了包含多組比原始對象更小子集的邊界空間，這種層次結構允許對對象進行更精確的幾何描述，同時降低了碰撞檢測的計算成本，因爲只有當較大的父對象發生碰撞時，才需要測試父對象內部的子對象。如圖2所示,左圖採用車輛外接圓對車輛進行包絡,無人車碰撞檢測的精度會受到較大影響.右圖則採用改進型方法,只有當無人車外接包絡圓與障礙物碰撞時,才需要檢測子包絡圓與障礙物是否碰撞,既提高了精度也沒有降低檢測的速度.

此篇文獻 [Fast Collision Checking for Intelligent Vehicle Motion Planning] 是該類方法的代表 . 此類方法在檢測過程中依然存在計算成本較大的風險,特別是需要利用判斷子包絡圖形與障礙物是否碰撞的情況.另外,該方法的子包絡圖形的選擇也是一個極大的挑戰.

圖 1 幾何邊界碰撞檢測
圖 2 改進型碰撞檢測

2. 柵格空間覆蓋枚舉

該方法基於柵格地圖,將車輛以及障礙物等周邊環境信息轉換到柵格地圖中,其中,每個格子都可以準確的反應障礙物或者車輛的佔有信息.由此,車輛行駛過程中的free space以及obstacles信息都可以精確的反應出來.具體如圖3所示,車輛的在柵格中的信息(左圖)被轉換到每個格子中(右圖).轉換後,只需要判斷車輛所佔據的柵格原本是否存有障礙物佔據信息.

由於其簡潔性,選用均勻統一大小的柵格是通用的方法,因此,柵格尺寸大小將是影響碰撞檢測的重要性能之一.如果柵格尺寸過於精細,則將影響車輛避障計算的實時性,同時也將降低傳感器在每個柵格信息的精度.如果柵格尺寸過於粗大,將會使得自由空間被低估,碰撞檢測過於保守，同時存在路徑規劃算法無法找到解決方案（即使存在）的風險。

圖 3 柵格空間覆蓋枚舉

3. 基於Costmap的障礙物碰撞檢測

Costmap是無人車感知層收集傳感器信息(例如32線激光雷達)建立和更新的二維或三維信息.Costmap本身可以理解爲自帶分數的柵格地圖.因此此方法屬於柵格空間覆蓋枚舉的改進型方法.如圖4所示,每個柵格的分數都在0~255之間,根據不同的分數,將Costmap劃分爲五層,其中紅色爲無人車或者機器人的外形輪廓(Footprint):

1. Lethal(致命層):分數爲253~255,無人車的幾何中心與該網格的中心重合，此時無人車必然與障礙物碰撞.
2. Inscribed(內切層):分數爲253~255,柵格小於無人車的內切圓,此時無人車也必然與障礙物碰撞.
3. Possibly circumscribed(外接層):分數128~252,柵格與無人車的外接圓外切，此時無人車相當於靠在障礙物附近，不一定碰撞,需要根據車輛朝向等信息具體判斷.
4. Freespace(自由層):分數0~127,無障礙物的自由空間.
5. Unknown(未知層):未知空間.

以上分數規劃是由ROS官方定義的,在編寫代碼時也可以根據自身項目需求進行重新更改.

圖 4 Costmap柵格分類

4. 基於Costmap的障礙物檢測代碼

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

整理碰撞檢測流程如下:

1. 首先判斷採樣路徑上所有采樣點所佔據柵格是否存在障礙物佔據信息
2. 其次在每個採樣點上計算實時車輛footprint對應的各個點所佔據的柵格是否存在障礙物佔據信息
3. 若有障礙物存在信息,則判定爲碰撞並捨棄路徑
4. 若沒有障礙物存在信息,則繼續其餘兩條評價函數分數

/****
@對軌跡上每個點進行評分,並將最終累加的分數作爲obstacle_costs
@if cost < 0, we can determine the trajectory is stucked in collosion
@traj : Class Trajectory 
@Using Trajectory to construct a trajectory which contains:
*@param double xv The x velocity used to seed the trajectory
*@param double yv The y velocity used to seed the trajectory
*@param double thetav The theta velocity used to seed the trajectory
*@param int num_pts The expected number of points for a trajectory
*@param double costs_ The cost of a trjactory, 
*@param double time_delta_ The time gap between points.
****/
double scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }
  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,footprint_spec_,costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }

    if(sum_scores_)//sum_scores_=true
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}

/****
@判斷軌跡上是否存在障礙物及基於距離障礙物遠近產生的分數
@param double x The position of the vehicle in world coordinates
@param double y The position of the vehicle in world coordinates
@param double th The position of the vehicle in world coordinates
@param double scale If we're over a certain speed threshold, we'll scale the robot's footprint to make it either slow down or stay further from walls
@param foot_spec Footprint points of the vehicles
****/
double ObstacleCostFunction::footprintCost (
    const double& x,
    const double& y,
    const double& th,
    std::vector<geometry_msgs::Point> footprint_spec,
    Costmap2D* costmap,
    WorldModel* world_model) {
  double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);
  if (footprint_cost < 0) {
    return -6.0;
  }
  unsigned int cell_x, cell_y;

  if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
    return -7.0;
  }

  double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
  return occ_cost;
}