局部規(guī)劃器以當(dāng)前速度為參考劈伴，產(chǎn)生一個(gè)合理且可達(dá)的速度采樣范圍，確定下一步的速度握爷。那么如何篩選呢跛璧？它用采樣速度生成相應(yīng)的仿真路徑，借助costmap饼拍，從障礙物赡模、與目標(biāo)的距離田炭、與全局規(guī)劃路徑的距離幾個(gè)方面對(duì)路徑成本進(jìn)行評(píng)估师抄，選擇最優(yōu)成本的路徑，將它對(duì)應(yīng)的采樣速度發(fā)布給機(jī)器人教硫，控制其運(yùn)動(dòng)叨吮。若在循環(huán)生成前向路徑的過(guò)程中，前方遇障瞬矩，無(wú)法得到前向的有效路徑茶鉴，那么進(jìn)入逃逸模式，不斷后退景用、旋轉(zhuǎn)涵叮，離開(kāi)一段距離后再進(jìn)行前向規(guī)劃，向前運(yùn)動(dòng)伞插。在原地自轉(zhuǎn)時(shí)割粮，注意震蕩控制，防止機(jī)器人左右頻繁來(lái)回旋轉(zhuǎn)媚污。

【結(jié)構(gòu)示意圖】

image.png

【相關(guān)文件】

• base_local_planner/src/trajectory_planner_ros.cpp
• base_local_planner/src/trajectory_planner.cpp
• base_local_planner/src/map_grid.cpp
• base_local_planner/src/map_cell.cpp
• base_local_planner/src/costmap_model.cpp
  本篇記錄對(duì)trajectory_planner.cpp中定義的TrajectoryPlanner類的閱讀和理解

【代碼分析】

trajectory_planner.cpp

–目錄–

一舀瓢、TrajectoryPlanner::updatePlan | 傳入全局規(guī)劃路徑
二、TrajectoryPlanner::findBestPath | 局部規(guī)劃器核心函數(shù)
三耗美、TrajectoryPlanner::createTrajectories | 生成給定速度范圍內(nèi)所有路徑
四京髓、TrajectoryPlanner::generateTrajectorie | 生成單條路徑并返回代價(jià)
五、TrajectoryPlanner::checkTrajectorie 與 scoreTrajectorie | 用給定速度生成單條路徑并返回代價(jià)

一商架、TrajectoryPlanner::updatePlan

Movebase調(diào)用全局規(guī)劃器生成全局路徑后堰怨，傳入TrajectoryPlannerROS封裝類，再通過(guò)這個(gè)函數(shù)傳入真正的局部規(guī)劃器TrajectoryPlanner類中蛇摸，并且將全局路徑的最終點(diǎn)最為目標(biāo)點(diǎn)final_goal诚些。

  void TrajectoryPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists){
    global_plan_.resize(new_plan.size());
    for(unsigned int i = 0; i < new_plan.size(); ++i){
      global_plan_[i] = new_plan[i];
    }

    if( global_plan_.size() > 0 ){
      geometry_msgs::PoseStamped& final_goal_pose = global_plan_[ global_plan_.size() - 1 ];
      final_goal_x_ = final_goal_pose.pose.position.x;
      final_goal_y_ = final_goal_pose.pose.position.y;
      final_goal_position_valid_ = true;
    } else {
      final_goal_position_valid_ = false;
    }

compute_dists默認(rèn)為false，即本地規(guī)劃器在更新全局plan時(shí)，不重新計(jì)算path_map_和goal_map_诬烹。

    if (compute_dists) {
      //reset the map for new operations
      path_map_.resetPathDist();
      goal_map_.resetPathDist();

      //make sure that we update our path based on the global plan and compute costs
      path_map_.setTargetCells(costmap_, global_plan_);
      goal_map_.setLocalGoal(costmap_, global_plan_);
      ROS_DEBUG("Path/Goal distance computed");
    }
  }

二砸烦、TrajectoryPlanner::findBestPath

局部規(guī)劃的整個(gè)流程體現(xiàn)在findBestPath函數(shù)中，它能夠在范圍內(nèi)生成下一步的可能路線绞吁，選擇出最優(yōu)路徑幢痘，并返回該路徑對(duì)應(yīng)的下一步的速度。

首先記錄當(dāng)前位姿（global系下）家破、速度颜说，將機(jī)器人當(dāng)前位置的足跡的標(biāo)志位設(shè)置為真。

并且汰聋，對(duì)path_map_和goal_map_的值重置后調(diào)用setTargetCells函數(shù)/setLocalGoal函數(shù)進(jìn)行更新门粪。這兩個(gè)地圖是局部規(guī)劃器中專用的“地圖”，即MapGrid類烹困，和costmap的組織形式一樣玄妈，都以cell為單位，path_map_記錄各cell與全局規(guī)劃路徑上的cell之間的距離髓梅，goal_map_記錄各cell與目標(biāo)cell之間的距離拟蜻，再最終計(jì)算代價(jià)時(shí)，將這兩個(gè)因素納入考慮枯饿，以保證局部規(guī)劃的結(jié)果既貼合全局規(guī)劃路徑酝锅、又不致偏離目標(biāo)。

這兩個(gè)函數(shù)的內(nèi)容和MapGrid類在下一篇記錄奢方。

  Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
      tf::Stamped<tf::Pose>& drive_velocities){

    //將當(dāng)前機(jī)器人位置和方向轉(zhuǎn)變成float形式的vector
    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));

    //重置地圖搔扁，清除所有障礙物信息以及地圖內(nèi)容
    path_map_.resetPathDist();
    goal_map_.resetPathDist();

    //用當(dāng)前位姿獲取機(jī)器人footprint
    std::vector<base_local_planner::Position2DInt> footprint_list =
        footprint_helper_.getFootprintCells(
            pos,
            footprint_spec_,
            costmap_,
            true);

    //將初始footprint內(nèi)的所有cell標(biāo)記 “within_robot = true”，表示在機(jī)器人足跡內(nèi)
    for (unsigned int i = 0; i < footprint_list.size(); ++i) {
      path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
    }

    //確保根據(jù)全局規(guī)劃global plan更新路徑蟋字，并計(jì)算代價(jià)
    //更新哪些地圖上的cell是在全局規(guī)劃路徑上的稿蹲，target_dist設(shè)置為0
    //并且通過(guò)它們和其他點(diǎn)的相對(duì)位置計(jì)算出來(lái)地圖上所有點(diǎn)的target_dist
    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);
    ROS_DEBUG("Path/Goal distance computed");

接下來(lái)，調(diào)用createTrajectories函數(shù)愉老，傳入當(dāng)前位姿场绿、速度、加速度限制嫉入，生成合理速度范圍內(nèi)的軌跡焰盗，并進(jìn)行打分，結(jié)果返回至best咒林。

    //找到代價(jià)最低的軌跡熬拒，輸入分別是目前機(jī)器人位置，速度以及加速度限制
    Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
        vel[0], vel[1], vel[2],
        acc_lim_x_, acc_lim_y_, acc_lim_theta_); //生成諸多可能軌跡垫竞，選取其中打分最高的澎粟。這里也就是最關(guān)鍵的一步蛀序。
    ROS_DEBUG("Trajectories created");

    /*
    //If we want to print a ppm file to draw goal dist
    char buf[4096];
    sprintf(buf, "base_local_planner.ppm");
    FILE *fp = fopen(buf, "w");
    if(fp){
      fprintf(fp, "P3\n");
      fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
      fprintf(fp, "255\n");
      for(int j = map_.size_y_ - 1; j >= 0; --j){
        for(unsigned int i = 0; i < map_.size_x_; ++i){
          int g_dist = 255 - int(map_(i, j).goal_dist);
          int p_dist = 255 - int(map_(i, j).path_dist);
          if(g_dist < 0)
            g_dist = 0;
          if(p_dist < 0)
            p_dist = 0;
          fprintf(fp, "%d 0 %d ", g_dist, 0);
        }
        fprintf(fp, "\n");
      }
      fclose(fp);
    }
    */

對(duì)返回的結(jié)果進(jìn)行判斷，若其代價(jià)為負(fù)活烙，表示說(shuō)明所有的路徑都不可用徐裸；若代價(jià)非負(fù)，表示找到有效路徑啸盏，為drive_velocities填充速度后返回重贺。

    if(best.cost_ < 0){
      drive_velocities.setIdentity(); 
    }
    else{
      tf::Vector3 start(best.xv_, best.yv_, 0);
      drive_velocities.setOrigin(start);
      tf::Matrix3x3 matrix;
      matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
      drive_velocities.setBasis(matrix);
    }

    return best; //返回最優(yōu)軌跡
  }

三、TrajectoryPlanner::createTrajectories

首先回懦，計(jì)算可行的線速度和角速度范圍气笙，這里先對(duì)最大線速度進(jìn)行一個(gè)限制，即保證速度既不超過(guò)預(yù)設(shè)的最大速度限制怯晕，也不超過(guò)“起點(diǎn)與目標(biāo)直線距離/總仿真時(shí)間”潜圃。

  Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
      double vx, double vy, double vtheta,
      double acc_x, double acc_y, double acc_theta) {
          
    //聲明最大/小線速度，最大/小角速度
    double max_vel_x = max_vel_x_, max_vel_theta;
    double min_vel_x, min_vel_theta;

    //如果最終的目標(biāo)是有效的（全局規(guī)劃不為空）
    if( final_goal_position_valid_ ){
      //計(jì)算當(dāng)前位置和目標(biāo)位置之間的距離：final_goal_dist
      double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );
      //最大速度：在預(yù)設(shè)的最大速度和
      max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );
    }

繼續(xù)計(jì)算線速度與角速度的上下限舟茶，使用的限制是由當(dāng)前速度在一段時(shí)間內(nèi)谭期，由最大加減速度達(dá)到的速度范圍，這里進(jìn)行了一個(gè)判斷稚晚，即是否使用dwa法：

① 使用dwa法崇堵，則用的是軌跡前向模擬的周期sim_period_（專用于dwa法計(jì)算速度的一個(gè)時(shí)間間隔）型诚；

② 不使用dwa法客燕，則用的是整段仿真時(shí)間sim_time_

在生成范圍時(shí)注意用預(yù)先給定的速度和角速度范圍參數(shù)進(jìn)行保護(hù)。

    if (dwa_) {
      //使用dwa窗口法狰贯，sim_period_是dwa計(jì)算最大也搓、最小速度用的時(shí)間
      //計(jì)算速度、角速度范圍涵紊，引入加速度限制條件（用sim_period_）
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
    } else {
      //不使用dwa窗口法
      //計(jì)算速度傍妒、角速度范圍，引入加速度限制條件（用sim_time_）
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
    }

接下來(lái)根據(jù)預(yù)設(shè)的線速度與角速度的采樣數(shù)摸柄，和上面計(jì)算得到的范圍颤练，分別計(jì)算出采樣間隔，并把范圍內(nèi)最小的線速度和角速度作為初始采樣速度驱负。不考慮全向機(jī)器人的情況嗦玖，即不能y向移動(dòng)，故暫不考慮vy上采樣跃脊。

    double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);//計(jì)算速度采樣間隔
    double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1);//計(jì)算角速度采樣間隔

    double vx_samp = min_vel_x;//x方向速度采樣點(diǎn)
    double vtheta_samp = min_vel_theta;//角速度采樣點(diǎn)
    double vy_samp = 0.0;

為了迭代比較不同采樣速度生成的不同路徑的代價(jià)宇挫，這里聲明best_traj和comp_traj并都將其代價(jià)初始化為-1

    Trajectory* best_traj = &traj_one;
    best_traj->cost_ = -1.0;//先初始化一個(gè)最優(yōu)路徑的代價(jià)=-1

    Trajectory* comp_traj = &traj_two; //下面生成的軌跡放到這里，和best_traj作比較酪术，如果生成的軌跡代價(jià)更小器瘪，就選擇它
    comp_traj->cost_ = -1.0;//先初始化一個(gè)當(dāng)前“對(duì)比”路徑的代價(jià)=-1，等會(huì)再用generateTrajectory函數(shù)生成路徑和新的代價(jià)存放在里面

    Trajectory* swap = NULL; //用于交換的指針

在機(jī)器人沒(méi)有處于逃逸狀態(tài)時(shí)，開(kāi)始遍歷所有線速度和角速度橡疼，調(diào)用類內(nèi)generateTrajectory函數(shù)用它們生成軌跡援所。二重迭代時(shí)，外層遍歷線速度（正值）欣除，內(nèi)層遍歷角速度任斋。在遍歷時(shí)，單獨(dú)拎出角速度=0耻涛，即直線前進(jìn)的情況废酷，避免由于采樣間隔的設(shè)置而躍過(guò)了這種特殊情況。

邊迭代生成邊比較抹缕，獲得代價(jià)最小的路徑澈蟆，存放在best_traj。

執(zhí)行完成后卓研，也跳出了“未處于逃逸狀態(tài)”這個(gè)判斷結(jié)構(gòu)趴俘。

    double impossible_cost = path_map_.obstacleCosts();

    //if we're performing an escape we won't allow moving forward
    if (!escaping_) {
      //【循環(huán)所有速度和角速度、打分】
      //外側(cè)循環(huán)所有x速度
      for(int i = 0; i < vx_samples_; ++i) {
        //x速度循環(huán)內(nèi)部遍歷角速度
        //①角速度=0
        vtheta_samp = 0;
        //傳入當(dāng)前位姿奏赘、速度寥闪、加速度限制，采樣起始x向速度磨淌、y向速度疲憋、角速度，代價(jià)賦給comp_traj
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);//【generateTrajectory函數(shù)如何實(shí)現(xiàn)-重點(diǎn)梁只！】參數(shù)：位置缚柳、采樣起始點(diǎn)、加速度

        //對(duì)比生成路徑和當(dāng)前最優(yōu)路徑的分?jǐn)?shù)搪锣，如果生成的路徑分?jǐn)?shù)更小秋忙，就把當(dāng)前路徑和最優(yōu)路徑交換
        //這里會(huì)將第一次生成路徑的代價(jià)賦給best_traj
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }

        //②角速度=下界
        vtheta_samp = min_vel_theta;
        //接下來(lái)迭代循環(huán)生成所有角速度的路徑、打分
        for(int j = 0; j < vtheta_samples_ - 1; ++j){
          generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
              acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

          //同樣地构舟，如果新路徑代價(jià)更小灰追，和best_traj作交換
          if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
            swap = best_traj;
            best_traj = comp_traj;
            comp_traj = swap;
          }
          //遍歷角速度
          vtheta_samp += dvtheta;
        }
        //遍歷x速度
        vx_samp += dvx;
      }

非全向機(jī)器人不考慮y向線速度，會(huì)跳過(guò)這個(gè)判斷結(jié)構(gòu)狗超。

      //只對(duì)holonomic robots迭代循環(huán)y速度弹澎，一般的機(jī)器人沒(méi)有y速度
      if (holonomic_robot_) {
        //explore trajectories that move forward but also strafe slightly
        vx_samp = 0.1;
        vy_samp = 0.1;
        vtheta_samp = 0.0;
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }

        vx_samp = 0.1;
        vy_samp = -0.1;
        vtheta_samp = 0.0;
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }
      }
    } // end if not escaping

接下來(lái)，繼續(xù)考慮線速度=0（原地旋轉(zhuǎn)）的情況抡谐，來(lái)考慮一下這種情況什么時(shí)候會(huì)發(fā)生：

• 當(dāng)TrajectoryPlannerROS中裁奇，位置已經(jīng)到達(dá)目標(biāo)（誤差范圍內(nèi)），姿態(tài)已達(dá)麦撵，則直接發(fā)送0速刽肠；姿態(tài)未達(dá)溃肪，則調(diào)用降速函數(shù)和原地旋轉(zhuǎn)函數(shù)，并調(diào)用checkTrajectory函數(shù)檢查合法性音五，直到旋轉(zhuǎn)至目標(biāo)姿態(tài)惫撰。而checkTrajectory函數(shù)調(diào)用的是scoreTrajectory和generateTrajectory，不會(huì)調(diào)用createTrajectory函數(shù)躺涝，所以厨钻，快要到達(dá)目標(biāo)附近時(shí)的原地旋轉(zhuǎn)，根本不會(huì)進(jìn)入到這個(gè)函數(shù)的這部分來(lái)處理坚嗜。

• 并且夯膀，由于局部規(guī)劃器的路徑打分機(jī)制（后述）是：“與目標(biāo)點(diǎn)的距離”和“與全局路徑的偏離”這兩項(xiàng)打分都只考慮路徑終點(diǎn)的cell，而不是考慮路徑上所有cell的綜合效果苍蔬，機(jī)器人運(yùn)動(dòng)到一個(gè)cell上诱建，哪怕有任何一條能向目標(biāo)再前進(jìn)的無(wú)障礙路徑，它的最終得分一定是要比原地旋轉(zhuǎn)的路徑得分來(lái)得高的碟绑。

所以俺猿，這里的原地自轉(zhuǎn)，是行進(jìn)過(guò)程中的格仲、未達(dá)目標(biāo)附近時(shí)的原地自轉(zhuǎn)押袍，并且，是機(jī)器人行進(jìn)過(guò)程中遇到障礙凯肋、前方無(wú)路可走只好原地自轉(zhuǎn)谊惭，或是連原地自轉(zhuǎn)都不能滿足，要由逃逸狀態(tài)后退一段距離否过，再原地自轉(zhuǎn)調(diào)整方向午笛，準(zhǔn)備接下來(lái)的行動(dòng)惭蟋。一種可能情況是機(jī)器人行進(jìn)前方遇到了突然出現(xiàn)而不在地圖上的障礙苗桂。

在處理這種情況時(shí)，由于機(jī)器人原地旋轉(zhuǎn)時(shí)的角速度限制范圍要比運(yùn)動(dòng)時(shí)的角速度限制范圍更嚴(yán)格告组，底盤(pán)無(wú)法處理過(guò)小的原地轉(zhuǎn)速煤伟，故要注意處理這層限制。同樣調(diào)用generateTrajectory函數(shù)木缝，生成路徑便锨。

    //接下來(lái)產(chǎn)生機(jī)器人在原地旋轉(zhuǎn)的軌跡
    vtheta_samp = min_vel_theta;
    vx_samp = 0.0;
    vy_samp = 0.0;

    double heading_dist = DBL_MAX;
    //循環(huán)所有角速度
    for(int i = 0; i < vtheta_samples_; ++i) {
      //強(qiáng)制最小原地旋轉(zhuǎn)速度
      double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
        : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
      //產(chǎn)生遍歷角速度的路徑
      generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
          acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

同樣，在迭代過(guò)程中用更好的路徑來(lái)更新best_traj我碟，但處理原地旋轉(zhuǎn)情況時(shí)放案，由于一直在原地cell，所以路徑的代價(jià)一直不變矫俺，所以重點(diǎn)在第二個(gè)if吱殉，它先獲取路徑終點(diǎn)的坐標(biāo)掸冤，實(shí)際上也就是原地的x、y坐標(biāo)友雳，再加上heading_lookahead_×自轉(zhuǎn)后姿態(tài)的三角函數(shù)稿湿，這個(gè)坐標(biāo)跟原地的坐標(biāo)關(guān)系如下：

然后再用計(jì)算結(jié)果的坐標(biāo)，借助goal_map_押赊，取最小值對(duì)應(yīng)的結(jié)果饺藤，用來(lái)更新best_traj。

對(duì)于這個(gè)篩選策略流礁，我的理解是：這個(gè)計(jì)算得到的相當(dāng)于機(jī)器人原地旋轉(zhuǎn)后“面向”的cell涕俗，它和機(jī)器人的距離是heading_lookahead_，由于goal_map_上的障礙物cell值為obstacleCosts（大于正常cell）神帅，所以經(jīng)過(guò)迭代咽袜，必然會(huì)篩選掉計(jì)算結(jié)果是障礙物的情形，也就是機(jī)器人旋轉(zhuǎn)后不會(huì)面向障礙物枕稀；同時(shí)篩選后询刹，也能得到和目標(biāo)點(diǎn)距離最短的計(jì)算結(jié)果，保證機(jī)器人在旋轉(zhuǎn)后向前行進(jìn)萎坷，距離目標(biāo)點(diǎn)的路程更短凹联。

（TrajectoryPlanner的路徑打分策略是以下三者的綜合：①與目標(biāo)點(diǎn)距離、②與全局規(guī)劃的距離哆档、③障礙物蔽挠，在上述原地自轉(zhuǎn)情況下，可能是突發(fā)障礙物瓜浸，此時(shí)①?zèng)]有意義澳淑，而這種篩選策略能夠?qū)ⅱ诤廷奂{入考慮，也比較合理插佛。）

      //如果新路徑代價(jià)更小
      if(comp_traj->cost_ >= 0
          && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
          && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
        //獲取新路徑的終點(diǎn)（原地）
        double x_r, y_r, th_r;
        comp_traj->getEndpoint(x_r, y_r, th_r);
        //坐標(biāo)計(jì)算
        x_r += heading_lookahead_ * cos(th_r);
        y_r += heading_lookahead_ * sin(th_r);
        unsigned int cell_x, cell_y;

        //轉(zhuǎn)換到地圖坐標(biāo)系杠巡，判斷與目標(biāo)點(diǎn)的距離
        if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
          double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
          //取距離最小的，放進(jìn)best_traj
          if (ahead_gdist < heading_dist) {
            if (vtheta_samp < 0 && !stuck_left) {
              swap = best_traj;
              best_traj = comp_traj;
              comp_traj = swap;
              heading_dist = ahead_gdist;
            }
            else if(vtheta_samp > 0 && !stuck_right) {
              swap = best_traj;
              best_traj = comp_traj;
              comp_traj = swap;
              heading_dist = ahead_gdist;
            }
          }
        }
      }
      //角速度遍歷
      vtheta_samp += dvtheta;
    }

軌跡生成已完成雇寇，接下來(lái)的工作可以概括為氢拥，如果軌跡cost非負(fù)，即找到有效軌跡锨侯，則將其返回嫩海；若找不到有效軌跡，進(jìn)入逃逸狀態(tài)囚痴，后退叁怪、原地自轉(zhuǎn)，若找不到下一步的有效路徑則再后退深滚、自轉(zhuǎn)奕谭，直到后退的距離或轉(zhuǎn)過(guò)的角度達(dá)到一定標(biāo)準(zhǔn)耳璧，才會(huì)退出逃逸狀態(tài)，重新規(guī)劃前向軌跡展箱。其中再加上震蕩抑制旨枯。

    //檢查最優(yōu)軌跡的分?jǐn)?shù)是否大于0
    if (best_traj->cost_ >= 0) {
      //抑制震蕩影響：當(dāng)機(jī)器人在某方向移動(dòng)時(shí)，對(duì)下一個(gè)周期的與其相反方向標(biāo)記為無(wú)效
      //直到機(jī)器人從標(biāo)記震蕩的位置處離開(kāi)一定距離混驰，返回最佳軌跡攀隔。
      
      //若軌跡的采樣速度≤0
      if ( ! (best_traj->xv_ > 0)) {
        //若軌跡的角速度<0
        if (best_traj->thetav_ < 0) {
          if (rotating_right) {
            stuck_right = true;
          }
          rotating_right = true;
        } else if (best_traj->thetav_ > 0) {
          if (rotating_left){
            stuck_left = true;
          }
          rotating_left = true;
        } else if(best_traj->yv_ > 0) {
          if (strafe_right) {
            stuck_right_strafe = true;
          }
          strafe_right = true;
        } else if(best_traj->yv_ < 0){
          if (strafe_left) {
            stuck_left_strafe = true;
          }
          strafe_left = true;
        }

        //set the position we must move a certain distance away from
        prev_x_ = x;
        prev_y_ = y;
      }

      double dist = hypot(x - prev_x_, y - prev_y_);
      if (dist > oscillation_reset_dist_) {
        rotating_left = false;
        rotating_right = false;
        strafe_left = false;
        strafe_right = false;
        stuck_left = false;
        stuck_right = false;
        stuck_left_strafe = false;
        stuck_right_strafe = false;
      }

      dist = hypot(x - escape_x_, y - escape_y_);
      if(dist > escape_reset_dist_ ||
          fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_){
        escaping_ = false;
      }

      return *best_traj;
    }

來(lái)看上面的代碼，當(dāng)軌跡cost非負(fù)栖榨，即軌跡有效昆汹，進(jìn)入下面的過(guò)程，嵌套的幾個(gè)if結(jié)構(gòu)作用是為“震蕩”和“逃逸”做記錄婴栽，不影響軌跡的發(fā)布满粗，只要軌跡有效，都會(huì)執(zhí)行到return *best_traj愚争，返回軌跡映皆。

那么來(lái)看這幾個(gè)if塊，第一大部分是記錄震蕩：“當(dāng)采樣速度v≤0” 轰枝，由于線速度采樣范圍是正的捅彻，當(dāng)出現(xiàn)非正采樣速度時(shí)，只可能是上一次的traj無(wú)效鞍陨，發(fā)布了后退命令步淹，進(jìn)入逃逸模式，然后重新循環(huán)诚撵，跳過(guò)前向軌跡規(guī)劃缭裆，進(jìn)入原地自轉(zhuǎn)模式，導(dǎo)致采樣速度=0

所以可以說(shuō)是寿烟，當(dāng)原地自轉(zhuǎn)時(shí)：

• 若角速度<0澈驼，標(biāo)記rotating_right為真
• 若角速度>0，標(biāo)記rotating_left為真
• 并記錄當(dāng)前自轉(zhuǎn)的位置

跳出這塊代碼韧衣，再跳過(guò)全向機(jī)器人的部分盅藻，發(fā)現(xiàn)一模一樣的代碼又出現(xiàn)了一遍，再執(zhí)行一次會(huì)導(dǎo)致（不過(guò)我不太理解為什么這樣設(shè)計(jì)畅铭，讓一樣的代碼出現(xiàn)兩次…）：

• 若角速度<0，標(biāo)記stuck_right為真
• 若角速度>0硕噩，標(biāo)記stuck_left為真
• 并記錄當(dāng)前自轉(zhuǎn)的位置

直到機(jī)器人離開(kāi)記錄的位置一段距離后缭贡，以上標(biāo)志位才會(huì)恢復(fù)false辉懒。

對(duì)照一下前面原地旋轉(zhuǎn)的代碼：可以發(fā)現(xiàn)，一旦機(jī)器人在某處負(fù)向角速度自轉(zhuǎn)眶俩，stuck_right被置真快鱼，那么在它未離開(kāi)當(dāng)前位置一定距離前颠印，是無(wú)法用正向角速度更新best_traj的，只能用負(fù)向角速度來(lái)更新它抹竹。

可見(jiàn)窃判，震蕩抑制能夠避免機(jī)器人在一個(gè)小范圍內(nèi)左右來(lái)回亂轉(zhuǎn)。

            if (vtheta_samp < 0 && !stuck_left) {
             swap = best_traj;
             best_traj = comp_traj;
             comp_traj = swap;
             heading_dist = ahead_gdist;
           }
           else if(vtheta_samp > 0 && !stuck_right) {
             swap = best_traj;
             best_traj = comp_traj;
             comp_traj = swap;
             heading_dist = ahead_gdist;

軌跡有效的部分結(jié)束询件，當(dāng)軌跡cost為負(fù)即無(wú)效時(shí)雳殊，執(zhí)行接下來(lái)的部分窗轩，設(shè)置一個(gè)負(fù)向速度痢艺，產(chǎn)生讓機(jī)器人緩慢退后的軌跡。此處也還是判斷一下震蕩距離色建。

    vtheta_samp = 0.0;
    vx_samp = backup_vel_;//后退速度
    vy_samp = 0.0;
    generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
        acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

    //if the new trajectory is better... let's take it
    /*
       if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
       swap = best_traj;
       best_traj = comp_traj;
       comp_traj = swap;
       }
       */
       
    swap = best_traj;
    best_traj = comp_traj;
    comp_traj = swap;

    double dist = hypot(x - prev_x_, y - prev_y_);
    if (dist > oscillation_reset_dist_) {
      rotating_left = false;
      rotating_right = false;
      strafe_left = false;
      strafe_right = false;
      stuck_left = false;
      stuck_right = false;
      stuck_left_strafe = false;
      stuck_right_strafe = false;
    }

若后退速度生成的軌跡的終點(diǎn)有效（> -2.0）箕戳，進(jìn)入逃逸狀態(tài)国撵，循環(huán)后退、自轉(zhuǎn)介牙，并且記錄下的逃逸位置和姿態(tài)，只有當(dāng)離開(kāi)逃逸位置一定距離或轉(zhuǎn)過(guò)一定角度囚似，才能退出逃逸狀態(tài)，再次規(guī)劃前向速度徐伐。逃逸判斷和震蕩判斷一樣办素，也已在上面多次執(zhí)行熬尺，只要發(fā)布best_traj前就執(zhí)行一次判斷粱哼。

若終點(diǎn)無(wú)效，返回負(fù)cost胯舷，本次局部規(guī)劃失敗桑嘶。

    //僅當(dāng)存在有效終點(diǎn)時(shí)才進(jìn)入逃逸模式
    if (!escaping_ && best_traj->cost_ > -2.0) {
      escape_x_ = x;
      escape_y_ = y;
      escape_theta_ = theta;
      escaping_ = true;
    }

    dist = hypot(x - escape_x_, y - escape_y_);

    if (dist > escape_reset_dist_ ||
        fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
      escaping_ = false;
    }

若后退軌跡遇障逃顶，還是繼續(xù)后退充甚，因?yàn)楹笸艘稽c(diǎn)后立刻就會(huì)進(jìn)入原地自轉(zhuǎn)模式伴找。

    //if the trajectory failed because the footprint hits something, we're still going to back up
    if(best_traj->cost_ == -1.0)
      best_traj->cost_ = 1.0;

    return *best_traj;

  }

這個(gè)函數(shù)內(nèi)容比較多技矮，畫(huà)張圖總結(jié)一下主體部分：

進(jìn)入逃逸狀態(tài)后衰倦，如下（白色圓點(diǎn)代表檢測(cè)退出條件）：

四耿币、TrajectoryPlanner::generateTrajectorie

這個(gè)函數(shù)根據(jù)給定的速度和角速度采樣生成單條路徑和其代價(jià)。

  void TrajectoryPlanner::generateTrajectory(
      double x, double y, double theta,
      double vx, double vy, double vtheta,
      double vx_samp, double vy_samp, double vtheta_samp,
      double acc_x, double acc_y, double acc_theta,
      double impossible_cost,
      Trajectory& traj) {

    boost::mutex::scoped_lock l(configuration_mutex_); //局部范圍鎖 

    // 記錄初始時(shí)刻的位姿十性、速度劲适、角速度
    double x_i = x; 
    double y_i = y;
    double theta_i = theta;
    double vx_i, vy_i, vtheta_i;
    vx_i = vx;
    vy_i = vy;
    vtheta_i = vtheta;

    //非全向機(jī)器人即線速度
    double vmag = hypot(vx_samp, vy_samp);

計(jì)算仿真步數(shù)和每一步對(duì)應(yīng)的時(shí)間厢蒜，朝向打分與否對(duì)應(yīng)的步數(shù)計(jì)算方法略有不同斑鸦。

    int num_steps;
    if(!heading_scoring_) {
      //sim_granularity_：仿真點(diǎn)之間的距離間隔
      //步數(shù) = max(速度南镉欤×總仿真時(shí)間/距離間隔，角速度/角速度間隔)憨琳，四舍五入
      num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
    } else {
      //步數(shù) = 總仿真時(shí)間/距離間隔篙螟，四舍五入
      num_steps = int(sim_time_ / sim_granularity_ + 0.5);
    }

    //至少一步
    if(num_steps == 0) {
      num_steps = 1;
    }

    //每一步的時(shí)間
    double dt = sim_time_ / num_steps;
    double time = 0.0;

    //聲明軌跡
    traj.resetPoints();
    traj.xv_ = vx_samp;//線速度
    traj.yv_ = vy_samp;
    traj.thetav_ = vtheta_samp;//角速度
    traj.cost_ = -1.0;//默認(rèn)代價(jià)-1.0

    //初始化軌跡的代價(jià)
    double path_dist = 0.0;//路徑距離
    double goal_dist = 0.0;//目標(biāo)距離
    double occ_cost = 0.0;//障礙物代價(jià)
    double heading_diff = 0.0;//航向角

接下來(lái)循環(huán)生成軌跡遍略，并計(jì)算軌跡對(duì)應(yīng)的代價(jià)值骤坐，先將當(dāng)前點(diǎn)從global系轉(zhuǎn)換到地圖系，若無(wú)法轉(zhuǎn)換到地圖上寞忿，直接結(jié)束軌跡生成腔彰，并返回代價(jià)-1.0辖佣。

    for(int i = 0; i < num_steps; ++i){
      //存儲(chǔ)點(diǎn)的地圖上的坐標(biāo)
      unsigned int cell_x, cell_y;

      //不希望路徑跑出已知地圖
      //從當(dāng)前位置開(kāi)始卷谈，依次循環(huán)全局路徑上的點(diǎn)
      //把當(dāng)前位置(x_i,y_i)轉(zhuǎn)換到地圖上，如果無(wú)法轉(zhuǎn)換端逼，說(shuō)明該路徑點(diǎn)不在地圖上顶滩，將其代價(jià)設(shè)置為-1.0礁鲁，并return
      if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
        traj.cost_ = -1.0;
        return;
      }

轉(zhuǎn)換到地圖后，開(kāi)始考慮機(jī)器人的大小冗美，把該點(diǎn)擴(kuò)張到機(jī)器人在該點(diǎn)的足跡范圍粉洼，并調(diào)用footprintCost函數(shù)漆改，獲得機(jī)器人在該點(diǎn)時(shí)它的足跡所對(duì)應(yīng)的代價(jià)准谚，若返回-1柱衔，說(shuō)明足跡遇障费什，直接返回-1琐旁。這個(gè)函數(shù)在WorldModel類中聲明枢赔，在它的派生類CostmapModel中定義脆炎，將機(jī)器人所在的點(diǎn)擴(kuò)張到該點(diǎn)足跡氓辣，并結(jié)合局部規(guī)劃器的costmap的值返回足跡對(duì)應(yīng)的代價(jià)钞啸，具體內(nèi)容下一篇記錄。

將路徑上點(diǎn)的最大cost記錄在occ_cost中梭稚，這一步完成了碰撞檢測(cè)的工作哨毁。

      //檢查當(dāng)前路徑上的點(diǎn)的合法性扼褪，每一個(gè)路徑點(diǎn)→機(jī)器人的足跡粱栖，檢測(cè)足跡的多邊形里是否碰到障礙物闹究，若遇障則返回負(fù)值
      double footprint_cost = footprintCost(x_i, y_i, theta_i);

      //若返回一個(gè)負(fù)值渣淤，說(shuō)明機(jī)器人在路徑上遇障价认，直接return
      if(footprint_cost < 0){
        traj.cost_ = -1.0;
        return;
        //TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits,
        //it can actually cause the robot to hit obstacles. There may be something to be done to fix, but I'll have to
        //come back to it when I have time. Right now, pulling it out as it'll just make the robot a bit more conservative,
        //but safe.
        /*
        double max_vel_x, max_vel_y, max_vel_th;
        //we want to compute the max allowable speeds to be able to stop
        //to be safe... we'll make sure we can stop some time before we actually hit
        getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th);

        //check if we can stop in time
        if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){
          ROS_ERROR("v: (%.2f, %.2f, %.2f), m: (%.2f, %.2f, %.2f) t:%.2f, st: %.2f, dt: %.2f", vx_samp, vy_samp, vtheta_samp, max_vel_x, max_vel_y, max_vel_th, time, stop_time_buffer_, dt);
          //if we can stop... we'll just break out of the loop here.. no point in checking future points
          break;
        }
        else{
          traj.cost_ = -1.0;
          return;
        }
        */
      }

      //更新occ_cost：max(max(occ_cost用踩，路徑足跡代價(jià))脐彩，當(dāng)前位置的代價(jià))
      //也就是把所有路徑點(diǎn)的最大障礙物代價(jià)設(shè)置為路徑的occ_cost
      occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));

接下來(lái)考慮與全局路徑及目標(biāo)點(diǎn)的距離：

• 如果simple_attractor_開(kāi)啟惠奸，采用比較簡(jiǎn)單的追蹤策略，只考慮與目標(biāo)點(diǎn)之間的直線距離^2
• 如果simple_attractor_未開(kāi)啟梗掰，則借助goal_map_和path_map_獲取該點(diǎn)與目標(biāo)點(diǎn)及全局規(guī)劃路徑之間的距離愧怜，若超過(guò)范圍拥坛，說(shuō)明沒(méi)有明確的目標(biāo)路徑，則該路徑無(wú)效丸氛，直接返回-2.0

經(jīng)過(guò)迭代缓窜，goal_dist和path_dist儲(chǔ)存的都是路徑上最后一個(gè)點(diǎn)的對(duì)應(yīng)代價(jià)禾锤，也就是用這種方法評(píng)價(jià)一條路徑時(shí)摹察，若路徑有效供嚎，【全局路徑及目標(biāo)點(diǎn)的距離】只與路徑末點(diǎn)有關(guān)克滴。

如果開(kāi)啟heading_scoring_即為朝向打分，只有當(dāng)從開(kāi)始生成該條路徑計(jì)時(shí)誓焦，到達(dá)特定時(shí)刻時(shí)罩阵，才會(huì)進(jìn)行唯一一次打分稿壁，headingDiff函數(shù)的具體過(guò)程是：

• 從全局路徑終點(diǎn)（目標(biāo)點(diǎn)）開(kāi)始歉备，當(dāng)前點(diǎn)與全局路徑上的各點(diǎn)依次連線獲得cost蕾羊，cost為正（無(wú)障礙）則立即計(jì)算：當(dāng)前點(diǎn)與迭代到的點(diǎn)間的連線方向與當(dāng)前點(diǎn)的姿態(tài)之差龟再，返回給heading_diff并停止繼續(xù)連線利凑；
• 若所有連線cost都為負(fù)嫌术，返回極大值度气。

為朝向打分的輪次將不更新goal_dist和path_dist磷籍。

      /*********************這里負(fù)責(zé)更新路徑和目標(biāo)距離**********************/
      //如果只是想blindly follow的話院领，如下（求與目標(biāo)間的三角距離）栅盲，否則見(jiàn)后
      if (simple_attractor_) {
        //目標(biāo)距離
        goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *
          (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
          (y_i - global_plan_[global_plan_.size() -1].pose.position.y) *
          (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
      } else {
        //不只是blindly follow废恋，“更新路徑距離和目標(biāo)距離”
        bool update_path_and_goal_distances = true;
        //如果有heading scoring鱼鼓，我們要求出航向角迄本，并且為路徑某點(diǎn)的路徑距離和目標(biāo)距離打分
        //如果為朝向打分
        if (heading_scoring_) { 
          /**********這里求出本次時(shí)間是否是要給航向打分的時(shí)間**********/
          //heading_scoring_timestep_是給朝向打分時(shí)在時(shí)間上要看多遠(yuǎn)
          //也就是在路徑上走過(guò)一個(gè)特定時(shí)刻后嘉赎，才為朝向打分一次
          //如果heading_scoring_timestep_≤時(shí)間<heading_scoring_timestep_+一次的時(shí)間間隔
          if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt) {
            //求出航向角
            heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
          } else {
            //不更新路徑和目標(biāo)距離
            update_path_and_goal_distances = false;
          }
        }

        if (update_path_and_goal_distances) { 
          //更新路徑距離與目標(biāo)距離（只考慮終點(diǎn)cell）
          path_dist = path_map_(cell_x, cell_y).target_dist;
          goal_dist = goal_map_(cell_x, cell_y).target_dist;

          //如果目標(biāo)距離或路徑距離≥impossible_cost（地圖尺寸）公条，代價(jià)設(shè)置為-2.0
          if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
          //            ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
          //                goal_dist, path_dist, impossible_cost);
            traj.cost_ = -2.0;
            return;
          }
        }
      }
      /*****************************************************************/

經(jīng)過(guò)檢測(cè)靶橱，若該點(diǎn)足跡不遇障关霸，且該點(diǎn)的goal_dist與path_dist存在队寇，加入軌跡章姓。

并通過(guò)函數(shù)計(jì)算該點(diǎn)的速度，速度計(jì)算函數(shù)使當(dāng)前速度在dt時(shí)間內(nèi)以加速度acc_x向采樣速度靠近惭聂，到達(dá)采樣速度后將不再改變辜纲。所以實(shí)際上每條軌跡都是一個(gè)由當(dāng)前速度趨向并穩(wěn)定在采樣速度的過(guò)程耕腾。

接下來(lái)通過(guò)航跡推演公式計(jì)算下一個(gè)路徑點(diǎn)的坐標(biāo)扫俺，并將幾個(gè)打分的項(xiàng)按比例加和固翰，得到當(dāng)前路徑的cost骂际，進(jìn)入下一次迭代歉铝，循環(huán)往復(fù)填充路徑坐標(biāo)，并更新路徑cost柠贤。

需要注意的是臼勉，這里對(duì)速度的計(jì)算與我們發(fā)布給機(jī)器人的速度無(wú)關(guān)坚俗，這里的速度只是為了推算下一個(gè)點(diǎn)猖败，獲得路徑降允，而我們真正發(fā)布給機(jī)器人的速度是采樣速度剧董。真實(shí)世界里機(jī)器人由當(dāng)前速度–>采樣速度的過(guò)程對(duì)應(yīng)我們地圖上本次仿真的軌跡。

      //這個(gè)點(diǎn)有效真慢，加入軌跡
      traj.addPoint(x_i, y_i, theta_i);

      //計(jì)算速度黑界、角速度
      //computeNewVelocity函數(shù)作用：不管速度大于還是小于vx_samp朗鸠，都讓其以加速度acc_x接近vx_samp后穩(wěn)定在vx_samp
      vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
      vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
      vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);

      //通過(guò)計(jì)算出的速度計(jì)算下一個(gè)位置烛占、姿態(tài)
      x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
      y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
      theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);

      //增加時(shí)間
      time += dt;
    } // end for i < numsteps

    //ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
    //計(jì)算生成路徑的代價(jià)
    double cost = -1.0; 
    if (!heading_scoring_) {
      //代價(jià)=路徑距離+目標(biāo)距離+障礙物代價(jià)（乘以各自的比例）
      cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
    } else {
      //代價(jià)=路徑距離+目標(biāo)距離+障礙物代價(jià)+航向角（如果有航偏則會(huì)增大代價(jià)）
      cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
    }
    //設(shè)置稱當(dāng)前路徑的代價(jià)
    traj.cost_ = cost;
  }

五忆家、TrajectoryPlanner::checkTrajectorie 與 scoreTrajectorie

這兩個(gè)函數(shù)并未進(jìn)行實(shí)際工作弦赖，checkTrajectory調(diào)用scoreTrajectory，scoreTrajectory調(diào)用generateTrajectory流酬，生成單條路徑并返回代價(jià)芽腾。它們是在足夠接近目標(biāo)時(shí)摊滔，局部規(guī)劃器產(chǎn)生降速和自轉(zhuǎn)時(shí)生成的對(duì)應(yīng)速度的路徑艰躺。

  bool TrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy,
      double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
    Trajectory t;

    double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);

    //if the trajectory is a legal one... the check passes
    if(cost >= 0) {
      return true;
    }
    ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost);

    //otherwise the check fails
    return false;
  }

  double TrajectoryPlanner::scoreTrajectory(double x, double y, double theta, double vx, double vy,
      double vtheta, double vx_samp, double vy_samp, double vtheta_samp) {
    Trajectory t;
    double impossible_cost = path_map_.obstacleCosts();
    generateTrajectory(x, y, theta,
                       vx, vy, vtheta,
                       vx_samp, vy_samp, vtheta_samp,
                       acc_lim_x_, acc_lim_y_, acc_lim_theta_,
                       impossible_cost, t);

    // return the cost.
    return double( t.cost_ );
  }

轉(zhuǎn)載：https://blog.csdn.net/Neo11111/article/details/104713086#commentBox