ros机器人路径远离障碍物的方法

A星或dijkstra规划的路径会贴着障碍物，如果膨胀半径设置过小机器人在跟踪路径运动时会碰到障碍物，特别是在转弯的时候。
这里提供一种路径优化的方法让路径与膨胀层保持一定距离。
步骤：
1、遍历所有的路径点，记录下路径点周围一定范围length(可设置)的障碍物。
2、若所有的障碍物都在路径的同一测则找到距离该路径点A最近的障碍物点B，设最近的距离为d。
3、将路径点A平移，平移的方向为A指向B的方向，平移的距离为length – d，即让A与B的保持距离为length。
效果图如下：

红色的路径为未优化的全局路径，绿色的为优化后的局部路径

代码如下：

void filtePath(std::vector<geometry_msgs::PoseStamped> &plan,double safe_distance)
  {
    if(plan.empty())
    {
      ROS_INFO("PurePlannerROS::filtePath: plan is empty.");
      return;
    }
    int safe_cell = (int)( safe_distance / this->costmap_->getResolution() );
    if(safe_cell < 1)
    {
      ROS_INFO("The safety distance is too small.");
      return;
    }
    size_t point_size = plan.size();
    geometry_msgs::PoseStamped tem_point;
    geometry_msgs::PoseStamped before_point;
    geometry_msgs::PoseStamped next_point;
    geometry_msgs::PoseStamped nearest_obstacle;
    unsigned int mx_min,mx_max,my_min,my_max,mx,my;
    for(size_t i=0;i<point_size;i++)
    {
      tem_point = plan[i];
      before_point = i>0?plan[i-1]:plan[i];
      next_point   = i<point_size-1?plan[i+1]:plan[i];

      this->costmap_->worldToMap(tem_point.pose.position.x,tem_point.pose.position.y,mx,my);
      mx_min = mx>safe_cell?mx-safe_cell:mx;
      mx_max = mx+safe_cell<this->costmap_->getSizeInCellsX()?mx+safe_cell:mx;
      my_min = my>safe_cell?my-safe_cell:my;
      my_max = my+safe_cell<this->costmap_->getSizeInCellsY()?my+safe_cell:my;
      std::vector<geometry_msgs::Point> obstacle_vec;
      geometry_msgs::Point obstacle;
      obstacle_vec.clear();
      for(unsigned int j=mx_min;j<mx_max;j++) //Find all obstacles within a safe distance.
      {
        for(unsigned int k=my_min;k<my_max;k++)
        {
          if(this->costmap_->getCost(j,k) != costmap_2d::FREE_SPACE)
          {
            this->costmap_->mapToWorld(j,k,obstacle.x,obstacle.y);
            obstacle_vec.push_back(obstacle);
          }
        }
      }

      if(obstacle_vec.empty() != true)
      {
         //Check if the points are on the same side.
        bool same_side_flag = false;
        if(next_point.pose.position.x != before_point.pose.position.x)
        {
          double lk = 0,lb = 0,ly = 0,num = 0;
          lk = (next_point.pose.position.y-before_point.pose.position.y) / (next_point.pose.position.x-before_point.pose.position.x);
          lb = next_point.pose.position.y - lk * next_point.pose.position.x;

          for(size_t m=0;m<obstacle_vec.size();m++)
          {
            ly = lk * obstacle_vec[m].x + lb;
            if(ly != 0)
              break;
          }

          for(size_t m=0;m<obstacle_vec.size();m++)
          {
            num = ly*(lk * obstacle_vec[m].x + lb);
            if(num < 0)
            {
              same_side_flag = true;
              break;
            }
          }
        }
        else
        {
          double const_x = next_point.pose.position.x;
          double err = 0,num = 0;
          for(size_t m=0;m<obstacle_vec.size();m++)
          {
            err = const_x - obstacle_vec[m].x;
            if(err != 0)
              break;
          }
          for(size_t m=0;m<obstacle_vec.size();m++)
          {
            num = err*(const_x - obstacle_vec[m].x);
            if(num < 0)
            {
              same_side_flag = true;
              break;
            }
          }
        }

        if(same_side_flag == true)
        {
          ROS_INFO("These points are not on the same side.");
          continue;
        }

        double distance=0,min_distance_obst = 1000.0;
        size_t min_obst_index = 0;
        double diff_x,diff_y;
        for(size_t l=0;l<obstacle_vec.size();l++) //find nearest obstacle
        {
          diff_x = obstacle_vec[l].x - tem_point.pose.position.x;
          diff_y = obstacle_vec[l].y - tem_point.pose.position.y;
          distance = sqrt(diff_x*diff_x+diff_y*diff_y);
          if(min_distance_obst > distance)
          {
            min_distance_obst = distance;
            min_obst_index = l;
          }
        }

        if(safe_distance - min_distance_obst < 0.0)
        {
          continue;
        }

        nearest_obstacle.pose.position.x = obstacle_vec[min_obst_index].x;
        nearest_obstacle.pose.position.y = obstacle_vec[min_obst_index].y;

        distance =  safe_distance - min_distance_obst;
        double err_x,err_y,theta,finally_x,finally_y;
        theta = atan2(tem_point.pose.position.y-nearest_obstacle.pose.position.y,tem_point.pose.position.x-nearest_obstacle.pose.position.x);
        err_x = distance*cos(theta);
        err_y = distance*sin(theta);
        finally_x = tem_point.pose.position.x + err_x;
        finally_y = tem_point.pose.position.y + err_y;
        this->costmap_->worldToMap(finally_x,finally_y,mx,my);
        if(this->costmap_->getCost(mx,my) == costmap_2d::FREE_SPACE)
        {
          plan[i].pose.position.x = finally_x;
          plan[i].pose.position.y = finally_y;
        }
      }
    }
  }