ROS源碼解讀(一)--局部路徑規划

博客轉載自：https://blog.csdn.net/xmy306538517/article/details/78772066

ROS局部路徑導航包括Trajectory Rollout 和 Dynamic Window Approach (DWA)兩種方法，理論上來說分別對應base_local_planner和dwa_local_planner兩個包，但其實dwa的大部分代碼都放在了base_local_planner包里面。

框架

以上接口是局部規划的核心，簡單來說:

1. TrajectorySampleGenerator產生一系列軌跡
2. 然后TrajectoryCostFunction遍歷軌跡打分
3. TrajectorySearch找到最好的軌跡拿來給小車導航
4. 由於小車不是一個質點，worldModel會檢查小車有沒有碰到障礙物

流程

1.初始化

包括參數，地圖，車身數據，比例系數

2.更新局部地圖和代價

• 將全局路徑中第一個離開局部地圖的點作為全局目標（或者使用最后一個在局部地圖內的點作為全局目標）
• 更新局部地圖中每個點的代價，具體是在computeTargetDistance中實現的，實現的方法是用隊列實現了一格格膨脹的效果，eg 終點為0，相鄰的點cost+1 ,同理路徑上的點為0，膨脹+1，注意同一個點取最小值

3.尋找最優路徑

• 然后TrajectoryCostFunction遍歷軌跡打分

• 每條軌跡遍歷代價函數，比例求和

本DWA中主要使用了如下幾種評分函數：

• OscillationCostFunction oscillation_costs_ 震盪代價 ;是否震盪
• ObstacleCostFunction obstacle_costs_ 障礙物代價 ; 看看是否撞到障礙
• base_local_planner::MapGridCostFunction path_costs_ 路徑代價 ;是否靠近全局路徑規划
• base_local_planner::MapGridCostFunction goal_costs_ 目標代價;是不是朝着目標行駛
• base_local_planner::MapGridCostFunction goal_front_costs_ 前向預判的距離針對目標;

• base_local_planner::MapGridCostFunction alignment_costs_ 前向預判的距離針對路徑;

  優點可擴展

  base_local_planner::PreferForwardCostFunction prefer_forward_costs_ 優先選擇前向行駛；