接著之前45.在ROS中實(shí)現(xiàn)global planner（1）和46.在ROS中實(shí)現(xiàn)global planner（2）的鋪墊，在ROS中實(shí)現(xiàn)AStar Global Planner

1. planner package

• 照著之前的模板，修改下名稱

git clone -b https://gitee.com/pibot/sample_global_planner.git astar_global_planner

再替換下所有的sample_global_planner–>astar_global_planner

• 基于這個(gè)我們新增之前的astar算法
  拷貝astar_planner.h 和astar_planner.cpp 分別至include和src目錄
• 修改CMakeLists.txt
  • 添加編譯

  add_library(${PROJECT_NAME}
    src/planner_node.cpp
    src/astar_planner.cpp  # 新增
  )
  

  • 添加opencv的依賴

  find_package(catkin REQUIRED
  COMPONENTS
      costmap_2d
      OpenCV)
  

  • c++11支持
    使用了c++11 cmake打開配置

  add_compile_options(-std=c++11) 
  

直接編譯試下
catkin_make -DCATKIN_WHITELIST_PACKAGES=astar_global_planner
頭文件報(bào)錯(cuò)，稍微修改下
#include "astar_global_planner/astar_planner.h"

2. 規(guī)劃實(shí)現(xiàn)

前面加入了編譯，下面我們具體新增其的使用

2.1 添加AStarPlanner

• 把之前實(shí)現(xiàn)好的AStarPlanner 添加為GlobalPlanner的一個(gè)成員

    std::unique_ptr<AStarPlanner> planner_; // 這里我們指定為指針
  

• 初始化接口添加對其的實(shí)例化

    planner_ = std::unique_ptr<AStarPlanner>(new AStarPlanner());
  

2.2 接口調(diào)用

AStarPlanner 主要的接口就是

bool Plan(const Mat& cost_map, const Point& start_point, const Point& end_point, std::vector<Point>& path, PlannerAction planner_action);

主要接收一個(gè)地圖參數(shù)cost_map， 一個(gè)起點(diǎn)start_point，一個(gè)終點(diǎn)end_point， 輸出為路徑path

planner_action為回調(diào)函數(shù) 之前調(diào)試顯示用，這里用不到， 傳入nullptr

• 地圖

地圖參數(shù)在GlobalPlanner的initialize接口有指定，我們只需要在這里保存下來，保存至成員，后續(xù)函數(shù)中使用

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
  costmap_ = costmap_ros->getCostmap();

  int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
  double resolution = costmap_->getResolution();

  cv::Mat costmap_mat = cv::Mat(costmap_->getSizeInCellsY(), costmap_->getSizeInCellsX(), CV_8UC1, costmap_->getCharMap());
  costmap_mat_ = costmap_mat.clone();  // costmap_mat_保存地圖的拷貝
}

• 起點(diǎn)&終點(diǎn)
  規(guī)劃接口中傳入了2個(gè)參數(shù)分別是起點(diǎn)和終點(diǎn)，但需要注意傳入的點(diǎn)的坐標(biāo)系為世界坐標(biāo)系，并非地圖的坐標(biāo)，所以需要做轉(zhuǎn)換，initialize傳入?yún)?shù)costmap_ros帶有轉(zhuǎn)換接口worldToMap直接調(diào)用即可，如下

  unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;

  double wx = start.pose.position.x;
  double wy = start.pose.position.y;
  costmap_->worldToMap(wx, wy, start_x_i, start_y_i);

  wx = goal.pose.position.x;
  wy = goal.pose.position.y;

  costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)

• 執(zhí)行規(guī)劃
  只需要調(diào)用AStarPlanner的Plan接口，傳入相應(yīng)的參數(shù)即可

  std::vector<Point> path;
  bool vaild = planner_->Plan(costmap_mat_, {start_x_i, start_y_i}, {goal_x_i, goal_y_i}, path, nullptr);

• 輸出路徑
  同起點(diǎn)&終點(diǎn)相反，這里輸出路徑保存的是地圖坐標(biāo)，所有需要轉(zhuǎn)換到世界坐標(biāo)返回，同樣使用costmap_ros的下mapToWorld接口即可
  另外AStarPlanner Plan接口輸出的路徑是從終點(diǎn)到起點(diǎn)的，這里需要反序列后輸出

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start,
                             const geometry_msgs::PoseStamped& goal,
                             std::vector<geometry_msgs::PoseStamped>& plan) {
  ...

  geometry_msgs::PoseStamped pose = goal;

  // 使用path反向迭代器循環(huán)
  for (auto it = path.rbegin(); it != path.rend(); it++) {
    costmap_->mapToWorld(it->x, it->y, pose.pose.position.x, pose.pose.position.y);
    plan.push_back(pose);
  }
  
  // plan 即為需要返回和pub的路徑
  ....
}

3. 測試

• 修改move_base_params.yaml中的base_global_planner

  base_global_planner: astar_global_planner/GlobalPlanner
  

• 啟動模擬器
  pibot_simulator

• 查看插件是否加載

  ? rosparam get /move_base/base_global_planner
  astar_global_planner/GlobalPlanner
  

• 打開rviz查看
  pibot_view
  

可以看出來AStar Global Planner已經(jīng)生效了，規(guī)劃路徑也出來了， 很不幸的是local planner一直無法正常運(yùn)行，顯然這個(gè)路徑規(guī)劃的太貼近障礙物。導(dǎo)致規(guī)劃的結(jié)果無法在實(shí)際機(jī)器人場景使用， 后續(xù)我們看看如何做優(yōu)化。

4. 優(yōu)化

4.1 規(guī)劃優(yōu)化

我們的A* 算法沒有考慮到障礙無的距離，導(dǎo)致可能緊挨著障礙物，我們需要在啟發(fā)函數(shù)新增里障礙物的距離值，以使得規(guī)劃路徑盡量遠(yuǎn)離障礙物。

在ROS中的地圖是cost map，所謂cost map也就是每個(gè)grid點(diǎn)有自己的cost， 如官方的這張圖片

簡單的說在ROS中，未知區(qū)域值255， 障礙物為254，其他就是遠(yuǎn)離障礙物就越越小

• 這樣我們在計(jì)算H的函數(shù)新增個(gè)參數(shù)，同時(shí)添加相應(yīng)的weight

  float Point2PointPlanner::CalcH(const Point& current_point, const Point& end_point) {
    ...
    return min_diff * kCornerCost + (max_diff - min_diff) * kLineCost;
  }
  

  改為

  float Point2PointPlanner::CalcH(const Point& current_point, const Point& end_point, float cost) {
    ...
    return min_diff * kCornerCost + (max_diff - min_diff) * kLineCost + cost * kCost;
    }
  

• 計(jì)算時(shí)傳入每個(gè)grid的cost

  ...
    open_list_.emplace(start_point, 0, CalcH(start_point, end_point, cost_map_.at<unsigned char>(start_point.y, start_point.x)));
  
  ...`
  
    all_grid_[index].Update(neighbor, point, G, CalcH(neighbor, end_point, cost_map_.at<unsigned char>(neighbor.y, neighbor.x)));
  ...
  

再次編譯測試，結(jié)果如下圖，可以看到規(guī)劃的路徑在道路中間，并且local planner可以正常工作。`

4.2 路徑優(yōu)化

可以看到，規(guī)劃的路徑是還存在瑕疵，路徑是鋸齒狀的，對local planner的控制有一定影響，我們可以添加平滑處理，使得路徑更為平滑
參考ROS:一種簡單的基于global_planner的全局路徑優(yōu)化方法得到效果如下
文章來源地址http://www.zghlxwxcb.cn/news/detail-659828.html

到了這里，關(guān)于47.在ROS中實(shí)現(xiàn)global planner（3）- 使用A*實(shí)現(xiàn)全局規(guī)劃的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！