EGO Planner代碼解析bspline_optimizer部分(1)

initControlPoints

從起點開始(或當前比特置開始)對於前2/3的每個控制點構成的線段以step_size為步長檢查線段上每個點是否有障礙物。

1、如果該點有障礙物，上一個點沒障礙物

如果之前已經連續超過兩個點沒有障礙物或為第一段線段，則進障礙物標志比特置true

記錄下有障礙物的軌迹在第幾段控制段(第i段)，相應的進障礙物的前的控制點in_id = i - 1,即為第i-1個控制點;

連續有/無障礙物計數器置0；

障礙物結束標志為置false；

2、如果該點無障礙物，上一個點有障礙物

記錄下出障礙物的軌迹在第幾段控制段(第i段)，相應的出障礙物的第一個控制點out_id=i，即為第i-1個控制點;

連續有/無障礙物計數器置0；

可能出障礙物標志比特置true；

3、如果該點與上一點有/無障礙物情况相同，則連續有/無障礙物計數器加1

如果可能出障礙物標志比特置為true；

並且之前已經連續超過兩個點有障礙物或當前軌迹段為最後一段

則可能出障礙物標志比特置false，出障礙物標志比特置true。

如果同時進和出障礙物標志比特置都為true，則標志比特置false，把障礙物控制點對(in_id,out_id)壓入segment_ids(障礙段)。

注：每個障礙段是一個控制點對，即進障礙物前的第一個控制點及出障礙物後的第一個控制點，(代碼中對應為(in_id,out_id))。

 std::vector<std::vector<Eigen::Vector3d>> BsplineOptimizer::initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init /*= true*/)
  {//初始化控制點（控制點，第一次初始化標志）
  //MatrixXd錶示任意大小的元素類型為double的矩陣變量，其大小只有在運行時被賦值之後才能知道

    if (flag_first_init)//第一次初始化
    {
      cps_.clearance = dist0_;// 安全距離
      cps_.resize(init_points.cols());
      //init_points初始化控制點矩陣的列數cols()，即剛開始初始化有幾個控制點
      //cps為控制點動態矩陣，可以通過resize()函數來動態修改矩陣的大小
      //現在其大小為初始化控制點矩陣的列數
      cps_.points = init_points;//第一次初始化時初始化的控制點就為所有控制點
    }


    /*** Segment the initial trajectory according to obstacles ***/
    //根據障礙物分割初始軌迹
    constexpr int ENOUGH_INTERVAL = 2;//持續控制點數(判斷是否持續處於障礙或不處於障礙物中）
    //constexpr常量錶達式
    double step_size = grid_map_->getResolution() / ((init_points.col(0) - init_points.rightCols(1)).norm() / (init_points.cols() - 1)) / 2;
    //col(0)裏面為點的坐標xyz，rightCols(1)為矩陣的最後一列
    //（第一個控制點init_points.col(0)比特置-最後一個控制點init_points.rightCols(1)比特置）的範數
    //即求第一個控制點和最後一個控制的的距離
    //(init_points.cols() - 1))為控制點的數目-1
    //step_size應為步長：分辨率/距離/（控制點數目-1）/2
    //其中距離/（控制點數目-1）即為控制點與控制點間的距離
    //目的：step_size為步長檢查線段上每個點是否有障礙物

    int in_id, out_id;//進障礙物的控制點 ，出障礙物的控制點
    vector<std::pair<int, int>> segment_ids;//控制段(該控制端的開始控制點in_id和結束控制點out_id)
    //segment_ids為std::pair<int, int>(in_id, out_id)
    int same_occ_state_times = ENOUGH_INTERVAL + 1;//連續有/無障礙物計數器
    bool occ, last_occ = false;//該點有障礙物occ，last_occ上一個點障礙物
    bool flag_got_start = false, flag_got_end = false, flag_got_end_maybe = false;
    //進障礙物標志比特置，出障礙物標志比特置，可能出障礙物標志比特置
    int i_end = (int)init_points.cols() - order_ - ((int)init_points.cols() - 2 * order_) / 3; 
    //i_end為控制點總數目的2/3再-1處。
    // only check closed 2/3 points.僅檢查臨近的2/3的控制點。

    for (int i = order_; i <= i_end; ++i)//i為軌迹段
    {
      for (double a = 1.0; a >= 0.0; a -= step_size)
      //對於前2/3的每個控制點構成的線段以step_size為步長檢查線段上每個點是否有障礙物
      {
        occ = grid_map_->getInflateOccupancy(a * init_points.col(i - 1) + (1 - a) * init_points.col(i));
        //占用（障礙）=獲取膨脹障礙物（a*初始化控制點中的第i-1個控制點）+(1 - a)初始化控制點中的第i個控制點
        

        if (occ && !last_occ)//如果該點有障礙物，上一個點沒障礙物
        {
          if (same_occ_state_times > ENOUGH_INTERVAL || i == order_)
          //已經連續超過兩個點（持續ENOUGH_INTERVAL為2）沒有障礙物或(i == order_)為第一段線段
          {
            in_id = i - 1;//記錄下有障礙物的軌迹在第幾段控制段，相應的障礙段的起點(控制點)in_id = i - 1
            flag_got_start = true;//進障礙物標志比特置true
          }
          same_occ_state_times = 0;//連續有/無障礙物計數器置0
          flag_got_end_maybe = false; // terminate in advance，不提前終止
          //障礙物結束標志為置false；
        }
        else if (!occ && last_occ)//該點不在障礙物中，但是上一個是障礙物
        {
          out_id = i;
          //記錄下出障礙物的軌迹在第幾段控制段(第i段)，相應的障礙段的終點out_id=i
          flag_got_end_maybe = true;//可能出障礙物標志比特置true；
          same_occ_state_times = 0;//連續有/無障礙物計數器置0
        }
        else//如果該點與上一點有/無障礙物情况相同
        {
          ++same_occ_state_times;//連續有/無障礙物計數器加1
        }

        if (flag_got_end_maybe && (same_occ_state_times > ENOUGH_INTERVAL || (i == (int)init_points.cols() - order_)))
        //flag_got_end_maybe，如果可能出障礙物標志比特置為true
        //並且之前已經連續超過兩個點有障礙物
        //或當前軌迹段為最後一段(i == (int)init_points.cols() - order_)

        {
          flag_got_end_maybe = false;//則可能出障礙物標志比特置false
          flag_got_end = true;//出障礙物標志比特置true。
        }

        last_occ = occ;

        if (flag_got_start && flag_got_end)//如果同時進和出障礙物標志比特置都為true
        {
          flag_got_start = false;//
          flag_got_end = false;//同時進和出障礙物標志比特置都為false
          segment_ids.push_back(std::pair<int, int>(in_id, out_id));
          //把障礙物控制點對(in_id,out_id)加入segment_ids。
        }
      }
      //對於前1/3的每個控制點構成的線段,以step_size為步長檢查線段上每個點是否有障礙物。
      //如果該點有障礙物上一個點沒障礙物，『如果之前已經連續超過兩個點沒有障礙物或為第一段線段
      //則進障礙物標志比特置true，記錄下有障礙物的軌迹在第幾段控制點in_id』，
      //連續有/無障礙物計數器置0，障礙物結束標志為置false；
      //如果該點無障礙物上一個點有障礙物，記錄下出障礙物的軌迹在第幾段控制點out_id，
      //連續有/無障礙物計數器置0，可能出障礙物標志比特置true；
      //如果該點與上一點有/無障礙物情况相同，則連續有/無障礙物計數器加1
      //如果同時進和出障礙物標志比特置都為true，則標志比特置false，
      //把障礙物控制點對(in_id,out_id)壓入segment_ids。
    }

 

/*** a star search ***/A星搜索

1、對所有段軌迹遍曆完後，如果沒有障礙物，則返回空的vector<std::pair<int,int>>。

2、否則對於每一個障礙物段segment_ids，從進障礙物前的第一個控制點使用A*算法規劃出一條到第一個出障礙物後的控制點的軌迹，得到路徑點a_star_pathes。

/*** a star search ***/
    //A星搜索
    vector<vector<Eigen::Vector3d>> a_star_pathes;//A星路徑點
    //對所有段軌迹遍曆完後，如果沒有障礙物，則返回空的vector<std::pair<int,int>>。
    //否則對於每一個障礙物段segment_ids，從進障礙物前的控制點使用A*算法
    //規劃出一條到第一個出障礙物控制點的軌迹，得到路徑點a_star_pathes。
    for (size_t i = 0; i < segment_ids.size(); ++i)
    //分段的數目
    {
      //cout << "in=" << in.transpose() << " out=" << out.transpose() << endl;
      Eigen::Vector3d in(init_points.col(segment_ids[i].first)), out(init_points.col(segment_ids[i].second));
      //init_points.col(segment_ids[i].first)為進障礙物段的控制點
      //init_points.col(segment_ids[i].second)為第一個出障礙物控制點
      if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out))
      //A星規劃
      {
        a_star_pathes.push_back(a_star_->getPath());//路徑點a_star_pathes
      }
      else
      {
        ROS_ERROR("a star error, force return!");
        return a_star_pathes;
      }
    }

  /*** calculate bounds ***/計算障礙段邊界

對於每一段障礙物段

1、如果是第一段線段並且障礙物個數大於1，則id_low_bound=第一個控制點，id_up_bound=第一個障礙物的結束點與第二個障礙物開始點的中點；

如果障礙物個數為1，則id_up_bound=最後一個控制點。

2、如果障礙物段是最後一段線段，則id_low_bound=最後一個障礙物的開始點與倒數第二個障礙物的結束點的中點，id_up_bound=最後一個控制點

3、如果障礙物段是中間段，id_low_bound=前一個障礙物的結束點與當前障礙物的開始點的中點，id_up_bound=當前障礙物的結束點與下一個障礙物的開始點的中點。

最後將每段障礙物控制點對的邊界(id_low_bound,id_up_bound)存入bounds。

/*** calculate bounds ***/
    //計算每一個障礙段的邊界，即每個障礙段的上界和下届對應的控制點
    int id_low_bound, id_up_bound;//
    vector<std::pair<int, int>> bounds(segment_ids.size());
    for (size_t i = 0; i < segment_ids.size(); i++)
    //遍曆每個障礙物段
    {

      if (i == 0) // first segment
      //如果障礙物段是第一段
      {
        id_low_bound = order_;//id_low_bound為第一個控制點
        if (segment_ids.size() > 1)//障礙物的個數>1
        {
          id_up_bound = (int)(((segment_ids[0].second + segment_ids[1].first) - 1.0f) / 2); // id_up_bound : -1.0f fix()
          //id_up_bound=第一個障礙物的結束點與第二個障礙物開始點的中點
        }
        else//如果障礙物個數為1
        {
          id_up_bound = init_points.cols() - order_ - 1;
          //則id_up_bound=最後一個控制點
        }
      }
      else if (i == segment_ids.size() - 1) // last segment, i != 0 here
      //如果是最後一段障礙物
      {
        id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil()
        //則id_low_bound=最後一個障礙物的開始點與倒數第二個障礙物的結束點的中點
        id_up_bound = init_points.cols() - order_ - 1;
        //id_up_bound=最後一個控制點
      }
      else
      //如果是中間段
      {
        id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil()
        //id_low_bound=前一個障礙物的結束點與當前障礙物的開始點的中點
        id_up_bound = (int)(((segment_ids[i].second + segment_ids[i + 1].first) - 1.0f) / 2);  // id_up_bound : -1.0f fix()
        //id_up_bound=當前障礙物的結束點與下一個障礙物的開始點的中點。
      }
      bounds[i] = std::pair<int, int>(id_low_bound, id_up_bound);
      //將每段障礙物段的邊界(id_low_bound,id_up_bound)存入bounds
    }

 /*** Adjust segment 0 ***/調整障礙段

調整障礙物段

對於每一段障礙物控制點對(進障礙物前的第一個控制點，出障礙物後的第一個控制點)

原因：保證每個障礙物段有足够的點來產生足够的推力(將該段推離障礙物)

如果控制點對之間的控制點數小於所要求的數量(即最小點數目=round(初始控制點總數目*最小百分比))

則在每段障礙物段的邊界以內向兩邊擴展,向障礙物段的兩邊添加點的數目=(最小點數-當前點數）/2

1. 最終障礙段的第一個控制點=障礙段的第一個控制點-添加的點數>=障礙段邊界的第一個點？是則返回最終障礙段的第一個起始控制點為障礙段初始的控制點，如果小於則為邊界的第一個點
2. 最終障礙段的出控制點=出障礙段的第一個控制點+添加的點數<=障礙段邊界的最後一個點？是則返回出最終障礙段的最後一個控制點為出障礙段的控制點，如果大於則為邊界的最後一個點

如果有足够的控制點則保持原來的障礙物控制點對

/*** Adjust segment 0 ***/
    vector<std::pair<int, int>> final_segment_ids(segment_ids.size());
    //最終的障礙段
    constexpr double MINIMUM_PERCENT = 0.0; //最小百分比
    // Each segment is guaranteed to have sufficient points to generate sufficient thrust
    //保證每個障礙物段有足够的點來產生足够的推力(將該段推離障礙物)
    int minimum_points = round(init_points.cols() * MINIMUM_PERCENT), num_points;
    //定義最小點數目=round(初始控制點總數目*最小百分比)，四舍五入
    //點數目為int型
    for (size_t i = 0; i < segment_ids.size(); i++)
    //遍曆每個障礙物段
    {
      /*** Adjust segment length ***/
      //調整障礙物段長度
      num_points = segment_ids[i].second - segment_ids[i].first + 1;
      //計算點的數目=障礙物段結束的控制點-障礙物段開始的控制點
      //(點的數目：如第8個控制點-第3個控制點就為8-3=5個控制點)
      //cout << "i = " << i << " first = " << segment_ids[i].first << " second = " << segment_ids[i].second << endl;
      if (num_points < minimum_points)
      //如果障礙物段內點的數目小於最小點數目
      {
        //在每段障礙物段的控制點對的邊界以內向兩邊擴展
        double add_points_each_side = (int)(((minimum_points - num_points) + 1.0f) / 2);
        //向障礙物段的兩邊添加點的數目=(最小點數-當前點數）/2

        final_segment_ids[i].first = segment_ids[i].first - add_points_each_side >= bounds[i].first ? segment_ids[i].first - add_points_each_side : bounds[i].first;
        //最終障礙段的第一個控制點=障礙段的第一個控制點-添加的點數>=障礙段邊界的第一個點？是則返回最終障礙段的第一個起始控制點為障礙段初始的控制點，如果小於則為邊界的第一個點
        final_segment_ids[i].second = segment_ids[i].second + add_points_each_side <= bounds[i].second ? segment_ids[i].second + add_points_each_side : bounds[i].second;
        //最終障礙段的出控制點=出障礙段的第一個控制點+添加的點數<=障礙段邊界的最後一個點？是則返回出最終障礙段的最後一個控制點為出障礙段的控制點，如果大於則為邊界的最後一個點
      }
      else//否則保持原來的障礙物控制點對
      {
        final_segment_ids[i].first = segment_ids[i].first;
        final_segment_ids[i].second = segment_ids[i].second;
      }

      //cout << "final:" << "i = " << i << " first = " << final_segment_ids[i].first << " second = " << final_segment_ids[i].second << endl;
    }