---

---

?? 在我之前的文章《ROS導航包Navigation中的 Movebase節點路徑規劃相關流程梳理》中已經介紹過Move_base節點調用局部路徑規劃器插件的接口函數是computeVelocityCommands，接下來，我們就從這個函數入手梳理一下teb_local_planner功能包的工作流程。

?? ☆注：因篇幅較長，本部分內容分成了上和下兩篇文章，閱讀完成本文的內容后，想要閱讀后續內容，請前往下，繼續閱讀。

?? 下篇文章鏈接如下：

?? 《ROS局部路徑規劃器插件teb_local_planner流程梳理（下）》

---

?? 一、規劃前的準備工作

?? 1、判斷插件是否已完成初始化，若沒有則返回錯誤信息

---

if(!initialized_)

?? 2、獲取機器人當前位姿及速度信息

costmap_ros_->getRobotPose(robot_pose);

odom_helper_.getRobotVel(robot_vel_tf);

robot_vel_.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation);

---

?? 3、對走過的全局路徑部分進行修剪

?? 由于機器人是實時運動的，所以需要對已經走過的全局路徑裁減處理，通過調用pruneGlobalPlan函數完成裁剪處理，其實現思路是從全局路徑規劃容器的起點開始一個個遍歷，計算其與機器人當前點的距離差，直到找到距離小于我們設定的閾值的點，將該點之前的全局路徑點全部刪除，這樣就可以將位于機器人后方距離超過一定閾值的點刪除掉。

?? 上述判斷用的閾值使用的是外部參數：global_plan_prune_distance。

  //修改全局路徑規劃，將已經走過的路徑刪除，保留機器人前方的路徑pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);

---

?? 4、 將全局路徑轉換到局部代價地圖的坐標系下，并從中選擇合適的點作為局部路徑點

?? 調用transformGlobalPlan函數，通過tf 樹將全局路徑信息轉換到局部代價地圖的坐標系下，值得注意的是，在這個函數中并不僅僅進行了坐標系的變換，還對全局路徑信息點進行了一些處理，從中選出合適的點作為局部路徑規劃點。

?? 其具體過程是依次對全局路徑進行坐標變換，同時計算其與當前位置的距離，并統計已轉換的全局路徑點之間的距離和（即已轉換的全局路徑的長度），直到超過設定的長度閾值或者與當前點的距離大于設定的距離閾值，則丟棄剩余的路徑點。

?? 其中，長度閾值由外部參數max_global_plan_lookahead_dist決定，距離閾值由局部代價地圖的大小width和height以及系數0.85有關

transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, transformed_plan, &goal_idx, &tf_plan_to_global)

?? 附：距離閾值sq_dist_threshold 計算過程：

double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
double sq_dist_threshold = dist_threshold * dist_threshold;

?? 概括一下，經過本步之后從全局路徑信息中，選出的局部路徑點是在以機器人當前位置為圓心距離閾值sq_dist_threshold 半徑的圓內的路徑點，且累積路徑長度小于路徑長度閾值的點的集合。

---

?? 5、 對得到的局部路徑規劃點進行進一步的篩選

?? 調用updateViaPointsContainer函數，對上一步得到的局部路徑規劃點進行進一步的篩選，其實現思路是，循環遍歷局部路徑規劃點，從中選出距離上一個被選中的點的距離大于設定的閾值的點（局部路徑規劃點中的第一個點為本步篩選中第一個被選中的點）

?? 這樣處理可以將上一步得到的局部路徑點稀疏化，從而減少局部路徑點的個數，閾值由外部參數global_plan_viapoint_sep決定，若該參數設置為負數，則表示跳過本步處理，不進行篩選

updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);

---

---

?? 第3~5步過程動態演示如下：

---

?? 6、 判斷機器人是否到達目標點

?? 得到局部路徑的狀態點后，先檢查是否已經到達了全局目標點，具體要檢查以下內容：

?? （1）當前點距離目標點的距離是否小于由外部參數設定的閾值xy_goal_tolerance

?? （2）當前點的姿態角與目標姿態角是否小于由外部參數設定的閾值yaw_goal_tolerance

?? （3）判斷機器人是否完全到位，由參數complete_global_plan以及via_points_.size()影響，其中complete_global_plan主要作用是防止機器人在越過終點時提前結束路徑

?? （4）有些版本中還會檢測判斷機器人當前速度是否停止，跟參數theta_stopped_vel、trans_stopped_vel、free_goal_vel有關，其中free_goal_vel由外部參數設定是否允許機器人以最大速度駛向目的地，即是否允許機器人達到目標點的速度不為零，參數heta_stopped_vel和trans_stopped_vel在melodic版本的包中并沒有。

if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance&& fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance&& (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0))

---

?? 7、 判斷是否進入恢復行為模式

?? 調用configureBackupModes函數判斷是否進入恢復模式：

?? （1）短時間內規劃失敗：若機器人規劃失敗時，沒有到達目標點，但是距離上次局部路徑規劃成功的時間間隔很小，算法會縮小規劃范圍，對局部路徑點進行裁減，將遠處的點先裁減掉，以更近的點作為規劃目標，嘗試重新規劃出可行路徑。

?? （2）長時間的規劃失敗：多次短期規劃都失敗后，算法判斷機器人是否長時間在某個地方發生振蕩，其具體思路是：首先將機器人當前速度與角速度歸一化到[0,1]，然后對線速度與角速度進行判斷，若機器人在一段時間內的平均線速度與角速度都小于閾值，并且前后兩次采樣過程中機器人的角速度相反，則認為機器人陷入了振蕩狀態（算法中創建了容器buffer_。用于面存儲了一段時間內的機器人線速度與角速度）

?? 當機器人處于振蕩狀態，同時如果這個時間超過閾值oscillation_recovery_min_duration則機器人會進行旋轉，旋轉方向由機器人當前角速度反方向決定，如果沒有超過閾值則以當前機器人角速度方向進行旋轉。

configureBackupModes(transformed_plan, goal_idx);

---

?? 8、調整局部目標點的角度

?? 根據外部參數 global_plan_overwrite_orientation決定是否調整局部目標點的角度（也就是經過上述處理后的路徑的最后一個點的角度），調整是通過調用estimateLocalGoalOrientation函數來實現的

if (cfg_.trajectory.global_plan_overwrite_orientation)

robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global);

?? 當需要修改時，會判斷當前局部路徑規劃的終點與全局路徑規劃的終點是否接近，如果兩個點比較接近(就是同一個點或者下一個點就是終點)則直接使用當前局部路徑規劃中終點的方向作為規劃的方向。如果當前局部路徑規劃的終點跟全局目標點之間還間隔很多個點的話，會使用當前局部路徑的終點以及向后兩個點的角度進行平滑處理，得到一個新的角度值。

?? 當不需要修改時，采用局部目標點的原有角度。

---

?? 9、將機器人當前的位姿插入到局部路徑點中

?? 上一步完成了局部規劃的目標點的確認，這一步要完成局部規劃的起始點的確認，，前面雖然根據全局路徑規劃中截取了局部路徑規劃的點，但是這個容器的起點不一定是是機器人當前時刻所處的點，因此，需要將機器人當前位姿插入到前面得到的局部路徑點中作為規劃的起點。

  transformed_plan.front() = robot_pose; // update start  更新起始點

---

?? 10、更新障礙物信息

?? 根據外部參數 costmap_converter決定如何更新障礙物信息，若該參數不為空，則調用costmap_converter插件進行地圖信息更新，若為空直接使用costmap的信息進行更新。

?? 分別對應下面那個函數：

updateObstacleContainerWithCostmapConverter

updateObstacleContainerWithCostmap

?? 若使用costmap_converter插件進行更新，則在updateObstacleContainerWithCostmapConverter函數中調用了getObstacles()函數從代價地圖轉換器獲得障礙物，并對障礙物根據其形狀（圓、點、線、多邊形）進行分類，將其轉換成相應的障礙物類型，然后將障礙物坐標存儲到obstacles_容器中，針對移動的障礙物還會設定其速度。

?? 若使用costmap的信息進行更新，根據外部參數 include_costmap_obstacles決定是否考慮costmap中的障礙物，若考慮則遍歷整個代價地圖，對于每一個代價地圖單元格，如果該單元格的代價為LETHAL_OBSTACLE（代價值為100），則將其視為障礙物，并檢測該障礙物是否足夠引人注意(例如，離機器人不遠)，如果障礙物在機器人后面且距離機器人的距離超過了外部參數決定的閾值 costmap_obstacles_behind_robot_dist，則忽略該障礙物。最后將得到的障礙物添加到障礙物容器中

---

?? 除此之外，還要從ROS消息中獲取障礙物信息，然后將這些障礙物添加到當前的障礙物容器中。該過程通過函數updateObstacleContainerWithCustomObstacles完成

  // also consider custom obstacles (must be called after other updates, since the container is not cleared)// 還要考慮custom障礙(必須在其他更新之后調用，因為障礙容器沒有被清除)updateObstacleContainerWithCustomObstacles();

?? 消息來源于：

custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this);

---

?? 至此，規劃前的準備工作完成，下面正式開始規劃進行局部路徑的規劃

---

?? 二、調用核心函數plan進行局部路徑規劃

  bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);

?? 需要注意的是存在三個名為plan的函數，根據傳入的形參及其含義可知此處調用的是如下函數：

bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)

?? 1、根據teb_.isInit()判斷是否初始化從起點到目標點的路徑

?? 若是，則調用initTrajectoryToGoal函數完成該操作，若不是則采用熱啟動的方式，此時需要進一步判斷，如果當前目標與上一個目標的位置差小于閾值force_reinit_new_goal_dist，姿態角度差小于閾值force_reinit_new_goal_angular，則調用updateAndPruneTEB函數清理路徑上已經走過的點，若不滿足上述閾值，則認為當前目標點與上一個目標點之間的距離足夠遠了，需要重新調用initTrajectoryToGoal函數進行路徑的初始化。

?? 其中，updateAndPruneTEB函數的具體實現思路是，首先在局部路徑規劃點中尋找距離當前位置最近的點，然后從局部路徑規劃點的第一個點開始往后搜索直至找到這個最近的點。然后刪除這個最近的點之前的那些點。

?? 調用initTrajectoryToGoal函數的語句如下：

 teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation,cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion);

?? 需要注意的是存在兩個名為initTrajectoryToGoal的函數，根據傳入的形參及其含義可知此處調用的是如下函數：

bool TimedElasticBand::initTrajectoryToGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient, int min_samples, bool guess_backwards_motion)

?? 其具體實現思路如下：

?? （1）檢查是否已經初始化路徑，如果沒有，則添加起始位置處的姿態點(包括機器人的位置和朝向)到局部路徑點的容器中，并設置起點優化中為固定不變的。

?? （2）設定后退標志位，若外部參數 allow_init_with_backwards_motion為真，即否允許在開始時想后退來執行軌跡，且目標點在起始點的后方，則將后退標志位backwards標志設置為true，否則為false。

?? （3）從起始點開始遍歷局部路徑點容器中的每個點，若外部參數 global_plan_overwrite_orientation為真，即否允覆蓋全局路徑中局部路徑點的朝向，若允許，則根據相鄰兩個局部路徑點之間的連線的朝向利用atan2函數重新計算每個局部路徑點的朝向，否則，保持每個局部路徑姿態點的原有朝向不變。

?? 在上述遍歷中，還會利用外部參數 max_vel_x和max_vel_theta，即設定的最大線速度和角速度來預估相鄰兩個局部路徑點之間要花費的時間dt，即兩點間位置差除以max_vel_x，得到線運動時間，兩點間角度差除以max_vel_theta得到角運動時間，然后取這兩個時間中的較大值作為兩點之間小路徑的預估時間dt。

?? （4）判斷是否需要強制添加局部路徑點，若當前的局部路徑點的數量小于由外部參數設定的最小樣本數 min_samples-1，為了保證生成的路徑點數量足夠多，則會強制添加一些局部路徑點，具體方式是不斷在當前點與目標點之間取中點添加到局部路徑點中（當前點會變化），直至局部路徑點的數量等于min_samples-1。

  static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2){return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) );}

?? （5）將目標點添加到局部路徑點的末尾處，并將其設為固定，即在優化中不能被改變。

?? 至此，局部路徑的初始化完成，可以這樣理解以上過程，當我們接收到新的目標點時（新的目標點與上一個目標點的距離或姿態差大于設定的閾值）需要重新調用initTrajectoryToGoal函數進行路徑的初始化，若目標點沒有變化，或者變化小于閾值，則認為是機器人移動過程中針對同一目標點的多次規劃，此時只需要調用updateAndPruneTEB函數對已經走過的部分的路徑點進行裁剪就可以了，不需要重新進行路徑初始化，也就是所謂的熱啟動模式。

---

?? ☆注：因篇幅較長，本部分內容分成了上和下兩篇文章，閱讀完成本文的內容后，想要閱讀后續內容，請前往下，繼續閱讀。

?? 下篇文章鏈接如下：

?? 《ROS局部路徑規劃器插件teb_local_planner流程梳理（下）》

---

---