路徑規劃與避障算法(三)---規劃層與感知層的接口定義及數據融合

接口概述

1. 規劃層與感知層的數據交互主要體現在以下幾個方面:

• 感知層主要提供可行駛區域信息,車輛前進道路方向上的障礙物信息
• 目前本身程序可以實現靜態障礙物避障功能,車輛識別動態障礙物並停車讓行功能
• 感知層將障礙物信息通過costmap發佈給規劃層
• 發佈的信息包含在一個一維數組當中,感知層與規劃層以統一的收發方式維護彼此相同的costmap

2. 感知層與定位層的數據交互關係主要體現在以下幾個方面:

• 感知層需要將傳感器的座標系轉換到baselink中
• 感知層主要關注costmap的原點在baselink中位置,長,寬以及分辨率大小

3. 由於規劃層同時與定位層以及感知層進行數據交互,定位層與感知層的信息發佈時間往往是不一致的,這樣會對規劃層的避障規劃結果造成比較大的影響.因此建議首先對激光雷達以及慣導在硬件上進行硬同步操作,同時在軟件上利用message_filters對兩層進行軟同步.

代碼詳述

  /**
   * @brief  take the basic properites of the costmap to planner
   * @param  cell_size_x ：the total number of the index in X axis
   * @param  cell_size_y ：the total number of the index in Y axis
   * @param  resolution  ：分辨率
   * @param  origin_x_in_map  ：costmap原點在baselink下的位置
   * @param  origin_y_in_map  ：costmap原點在baselink下的位置
   * @return The associated index
   */
 void dwa_planner_node::rt_costmap_cb(nav_msgs::OccupancyGrid costmap_msg)
{
cell_size_x = costmap_msg.info.height / costmap_msg.info.resolution;
cell_size_y = costmap_msg.info.width / costmap_msg.info.resolution;
resolution = double(costmap_msg.info.resolution);
origin_x_in_map = double(costmap_msg.info.origin.position.x);
origin_y_in_map = double(costmap_msg.info.origin.position.y);

cost_map_.resizeMap(cell_size_x, cell_size_y, resolution, origin_x_in_map, origin_y_in_map);//通過實際數據對costmap進行size,position的設置

cost_map_.setValue(yaw, odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y);//將當前位置與角度傳遞以進行maptoworld的轉換

for (int ii = 0; ii < cell_size_x; ii++)
    {
    for (int jj = 0; jj < cell_size_y; jj++)
    {
      cost_map_.setCost(ii, jj, costmap_msg.data[cost_map_.getIndex(ii, jj)]);
    }
  }//將感知層中costmap的一位數組讀進規劃層的costmap中

}

costmap底層支持函數

  /**
   * @brief  Given two map coordinates... compute the associated index
   * @param mx The x coordinate，index count
   * @param my The y coordinate, index count
   * @return The associated index
   */
 unsigned int getIndex(unsigned int mx, unsigned int my) const//from x start to end then switch to y 
  {
    return my * size_x_ + mx;
  }

  /**
   * @brief  Set the cost of a cell in the costmap
   * @param mx The x coordinate of the cell
   * @param my The y coordinate of the cell
   * @param cost The cost to set the cell to
   */
void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
  costmap_[getIndex(mx, my)] = cost;
}

調試經驗

• costmap座標系的問題,是以車輛前進方向爲X正半軸方向,車輛左側爲Y正半軸方向的右手準則來判定的
• costmap本身的原點設定爲map的右下角
• costmap根據ROS_Navigation庫,其中的值是0_{255,但是此項目中是0}100,且程序中設置爲當柵格數值大於10時就認定爲障礙物
• 乘用車上的激光雷達感知區域穩定,盲區一般較小,障礙物信息往往可以控制在影響較小的固定範圍內
• 在工程項目中,由於工程機械的特定結構遮擋,激光雷達常常存在各種盲區,因此僅僅只靠一個32線或64線激光雷達的話是無法完整的實現避障功能的,還需要感知層完成多傳感器的數據融合工作
• 在開展避障工作前最好先完成傳感器(激光雷達等)的標定工作
• 在工程機械上,當激光雷達等傳感器安裝在可活動的工程機械部件上時,需考慮每次傳感器的位置是否存在誤差與區別,若存在,則推薦做一個動態tf關係來補償傳感器與baselink之間的誤差

總結

• 感知層的問題主要存在於感知的準確性與精度的問題
• 感知的範圍：感知的盲區也是影響避障效果的重要因素
• costmap所用的座標系，原點的位置，一維數組的發送與讀取方式是否相同都是會造成避障偏差的可能原因