ROS1云课→20迷宫不惑之A*大法（一种虽古老但实用全局路径规划算法）

ROS1云课→19仿真turtlebot（stage）

19提及的机器人如何实现全局路径规划？A*算法是一种可行的选择。 

www.gamedev.net/reference/articles/article2003.asp 

---

A*算法的基本介绍可以查询网络资源。这里，列出一些案例：

蓝桥ROS扩展笔记CppRobotics编译_zhangrelay的博客-CSDN博客

---

第一种：

/*************************************************************************
	> File Name: a_star.cpp
	> Author: TAI Lei
	> Mail: [email protected]
	> Created Time: Sat Jul 20 12:38:43 2019
 ************************************************************************/

#include
#include
#include
#include
#include
#include
#include
#include

using namespace std;


class Node{
public:
  int x;
  int y;
  float sum_cost;
  Node* p_node;

  Node(int x_, int y_, float sum_cost_=0, Node* p_node_=NULL):x(x_), y(y_), sum_cost(sum_cost_), p_node(p_node_){};
};


std::vector > calc_final_path(Node * goal, float reso, cv::Mat& img, float img_reso){
  std::vector rx;
  std::vector ry;
  Node* node = goal;
  while (node->p_node != NULL){
    node = node->p_node;
    rx.push_back(node->x * reso);
    ry.push_back(node->y * reso);
    cv::rectangle(img,
        cv::Point(node->x*img_reso+1, node->y*img_reso+1),
        cv::Point((node->x+1)*img_reso, (node->y+1)*img_reso),
        cv::Scalar(255, 0, 0), -1);
  }
  return {rx, ry};
}


std::vector > calc_obstacle_map(
    std::vector ox, std::vector oy,
    const int min_ox, const int max_ox,
    const int min_oy, const int max_oy,
    float reso, float vr,
    cv::Mat& img, int img_reso){

  int xwidth = max_ox-min_ox;
  int ywidth = max_oy-min_oy;

  std::vector > obmap(ywidth, vector(xwidth, 0));

  for(int i=0; i > obmap,
                 int min_ox, int max_ox,
                 int min_oy, int max_oy){
  if (node->x < min_ox || node->y < min_oy || node->x >= max_ox || node->y >= max_oy){
    return false;
  }

  if (obmap[node->x-min_ox][node->y-min_oy]) return false;

  return true;
}


float calc_heristic(Node* n1, Node* n2, float w=1.0){
  return w * std::sqrt(std::pow(n1->x-n2->x, 2)+std::pow(n1->y-n2->y, 2));
}

std::vector get_motion_model(){
  return {Node(1,   0,  1),
          Node(0,   1,  1),
          Node(-1,   0,  1),
          Node(0,   -1,  1),
          Node(-1,   -1,  std::sqrt(2)),
          Node(-1,   1,  std::sqrt(2)),
          Node(1,   -1,  std::sqrt(2)),
          Node(1,    1,  std::sqrt(2))};
}

void a_star_planning(float sx, float sy,
                     float gx, float gy,
                     vector ox_, vector oy_,
                     float reso, float rr){

  Node* nstart = new Node((int)std::round(sx/reso), (int)std::round(sy/reso), 0.0);
  Node* ngoal = new Node((int)std::round(gx/reso), (int)std::round(gy/reso), 0.0);


  vector ox;
  vector oy;

  int min_ox = std::numeric_limits::max();
  int max_ox = std::numeric_limits::min();
  int min_oy = std::numeric_limits::max();
  int max_oy = std::numeric_limits::min();


  for(float iox:ox_){
      int map_x = (int)std::round(iox*1.0/reso);
      ox.push_back(map_x);
      min_ox = std::min(map_x, min_ox);
      max_ox = std::max(map_x, max_ox);
  }

  for(float ioy:oy_){
      int map_y = (int)std::round(ioy*1.0/reso);
      oy.push_back(map_y);
      min_oy = std::min(map_y, min_oy);
      max_oy = std::max(map_y, max_oy);
  }

  int xwidth = max_ox-min_ox;
  int ywidth = max_oy-min_oy;

  //visualization
  cv::namedWindow("astar", cv::WINDOW_NORMAL);
  int count = 0;
  int img_reso = 5;
  cv::Mat bg(img_reso*xwidth,
             img_reso*ywidth,
             CV_8UC3,
             cv::Scalar(255,255,255));

    cv::rectangle(bg,
                  cv::Point(nstart->x*img_reso+1, nstart->y*img_reso+1),
                  cv::Point((nstart->x+1)*img_reso, (nstart->y+1)*img_reso),
                  cv::Scalar(255, 0, 0), -1);
    cv::rectangle(bg,
                  cv::Point(ngoal->x*img_reso+1, ngoal->y*img_reso+1),
                  cv::Point((ngoal->x+1)*img_reso, (ngoal->y+1)*img_reso),
                  cv::Scalar(0, 0, 255), -1);

  std::vector > visit_map(xwidth, vector(ywidth, 0));
  
  std::vector > path_cost(xwidth, vector(ywidth, std::numeric_limits::max()));

  path_cost[nstart->x][nstart->y] = 0;

  std::vector > obmap = calc_obstacle_map(
                                                  ox, oy,
                                                  min_ox, max_ox,
                                                  min_oy, max_oy,
                                                  reso, rr,
                                                  bg, img_reso);

  // NOTE: d_ary_heap should be a better choice here
  auto cmp = [](const Node* left, const Node* right){return left->sum_cost > right->sum_cost;};
  std::priority_queue, decltype(cmp)> pq(cmp);

  pq.push(nstart);
  std::vector motion = get_motion_model();

  while (true){
    Node * node = pq.top();

    if (visit_map[node->x-min_ox][node->y-min_oy] == 1){
      pq.pop();
      delete node;
      continue;
    }else{
      pq.pop();
      visit_map[node->x-min_ox][node->y-min_oy] = 1;
    }

    if (node->x == ngoal->x && node->y==ngoal->y){
      ngoal->sum_cost = node->sum_cost;
      ngoal->p_node = node;
      break;
    }

    for(int i=0; ix + motion[i].x,
        node->y + motion[i].y,
        path_cost[node->x][node->y] + motion[i].sum_cost + calc_heristic(ngoal, node),
        node);

      if (!verify_node(new_node, obmap, min_ox, max_ox, min_oy, max_oy)){
        delete new_node;
        continue;
      }

      if (visit_map[new_node->x-min_ox][new_node->y-min_oy]){
        delete new_node;
        continue;
      }

      cv::rectangle(bg,
                    cv::Point(new_node->x*img_reso+1, new_node->y*img_reso+1),
                    cv::Point((new_node->x+1)*img_reso, (new_node->y+1)*img_reso),
                    cv::Scalar(0, 255, 0));

      // std::string int_count = std::to_string(count);
      // cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);
      count++;
      cv::imshow("astar", bg);
      cv::waitKey(5);

      if (path_cost[node->x][node->y]+motion[i].sum_cost < path_cost[new_node->x][new_node->y]){
        path_cost[new_node->x][new_node->y]=path_cost[node->x][node->y]+motion[i].sum_cost; 
        pq.push(new_node);
      }
    }
  }

  calc_final_path(ngoal, reso, bg, img_reso);
  delete ngoal;
  delete nstart;

  // std::string int_count = std::to_string(count);
  // cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);
  cv::imshow("astar", bg);
  cv::waitKey(5);
};


int main(){
  float sx = 10.0;
  float sy = 10.0;
  float gx = 50.0;
  float gy = 50.0;

  float grid_size = 1.0;
  float robot_size = 1.0;

  vector ox;
  vector oy;

  // add edges
  for(float i=0; i<60; i++){
    ox.push_back(i);
    oy.push_back(60.0);
  }
  for(float i=0; i<60; i++){
    ox.push_back(60.0);
    oy.push_back(i);
  }
  for(float i=0; i<61; i++){
    ox.push_back(i);
    oy.push_back(60.0);
  }
  for(float i=0; i<61; i++){
    ox.push_back(0.0);
    oy.push_back(i);
  }
  for(float i=0; i<40; i++){
    ox.push_back(20.0);
    oy.push_back(i);
  }
  for(float i=0; i<40; i++){
    ox.push_back(40.0);
    oy.push_back(60.0 - i);
  }

  a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size);
  cin.ignore();
  return 0;
}

效果如下：

 

 但是，需要通过修改代码，定位起点终点和障碍物等。

---

如何，解决下面这种迷宫呢（mazegenerator.net）？

迷宫有中杯，大杯，超大杯。

中杯版本如下：

大杯版本如下：

 

超超大杯版本：

 

咱就不玩超大号的了。 

就用下面这个地图吧：

中：

大：

代码参考： 

#include 
#include
#include
#include
#include
#include
using namespace cv;
using namespace std;
//
Mat pool_max(Mat image_source, int size);
Mat pool_min(Mat image_source, int size);
int f(int x1,int x2,int z1,int z2);

class A
{
    private:
    int rate=6;
    int map[300][300];
    Mat m;
    //
    int tag_open=0,tag_close=0;
    //
    int map_root[300][300][2];
    //
    int x1;
    int x2;
    //
    int z1;
    int z2;
    //
    int way[1000][2];
    //
    //
    int close[24400][3];
    //
    int open[24400][4];
    public:
    //
    Mat image_source;
    //
    void deal_image();
    //
    void find_ori();
    //
    void deal_A();
    //
    void draw_road();

};
void A::draw_road()
{
    Mat rgb;
    rgb=imread("maze.png",1);
    VideoWriter witer = VideoWriter("maze.avi",CV_FOURCC('M','P','4','2'),40,Size(image_source.cols,image_source.cols),1);//
    int mm=0;
    //
    while(1)
    {
        mm++;
        //
        for(int tag_a=4;tag_a<7;tag_a++)
        for(int tag_b=4;tag_b<7;tag_b++)
        for(int tag_c=0;tag_c<2;tag_c++)
        rgb.at((map_root[z1][z2][0]*rate+tag_a),(map_root[z1][z2][1]*rate+tag_b))[tag_c%3]=0;
        imshow("a",rgb);
        waitKey(1);
    z1=map_root[z1][z2][0];
    z2=map_root[z1][z2][1];
    //
    witer<1000)break;
    }
    //
    imshow("a",rgb);

    witer.release();
}
void A::find_ori()
{
    Mat image_b;
        GaussianBlur(image_source,image_b,Size(3,3),5);
    //
    for(int tag_a=0;tag_a(tag_a,tag_b)>100)image_b.at(tag_a,tag_b)=255;
    else image_b.at(tag_a,tag_b)=0;
    }
    //
    for(int tag_b=0;tag_b(tag_b,0)==255)
    {
        x1=tag_b/rate-1;
        x2=0;
    }

    for(int tag_b=0;tag_b(tag_b,image_b.rows-1)==255)
    {
        z1=tag_b/rate-1;
        z2=(image_b.rows)/rate-1;
    }

}
void A::deal_image()
{   //
    image_source = imread("maze.png",0);
    //
    Mat b;
    //
    GaussianBlur(image_source,b,Size(3,3),5);
    //
    for(int tag_a=0;tag_a(tag_a,tag_b)>100)b.at(tag_a,tag_b)=255;
    else b.at(tag_a,tag_b)=0;
    }
    //
    //m=pool_min(pool_max(b,3),3);
    m=pool_min(b,6);
    //
    for(int tag_a=0;tag_a(tag_a,tag_b)>100)m.at(tag_a,tag_b)=255;
    else m.at(tag_a,tag_b)=0;
    }
    //
    imshow("c",m);
    //
    for(int tag_a=0;tag_a(tag_a,tag_b);
}
void A::deal_A()
{
    int keyy1=0,keyy2=0,keyy3=0,keyy4=0;
    map_root[z1][z2][0]=5;
    //
    close[0][0]=x1;
    close[0][1]=x2;
    tag_close++;
    //
    int com[2]={10000,0};
while(1)
{
//
if(map[close[tag_close-1][0]][close[tag_close-1][1]+1]==255&&close[tag_close-1][0]>=0&&(close[tag_close-1][1]+1)>=0)
{   //close

        for(int tag_a=0;tag_a=0&&(close[tag_close-1][1]-1)>=0)
{

        for(int tag_a=0;tag_a=0&&(close[tag_close-1][1])>=0)
{

    for(int tag_a=0;tag_a=0&&(close[tag_close-1][1])>=0)
{

    
    for(int tag_a=0;tag_a=0&&tag_x<=rows-size)//
    {
        while(tag_y>=0&&tag_y<=cols-size)
        {
            while(tag1>=0&&tag1<=size)
            {
                while(tag2>=0&&tag2<=size)
                {
                    if(image_source.at(tag_x+tag1,tag_y+tag2)>tag3[1])(tag3[1]=image_source.at(tag_x+tag1,tag_y+tag2));
                    tag2++;
                }
                tag1++;
                tag2=0; 
            }
            image_new.at(tag_x/size,tag_y/size)=tag3[1];
            tag3[1]=0;
            tag_y+=size;tag1=0;tag2=0;
        }
        tag_x+=size;
        tag_y=0;tag1=0;tag2=0;
    }
    return image_new;

}

Mat pool_min(Mat image_source, int size)
{
    int rows = image_source.rows;
    int cols = image_source.cols;
    int tag_x=0,tag_y=0,tag1=0,tag2=0;
    int tag3[3]={0,0,0};
    Mat image_new(image_source.rows/size,image_source.cols/size,CV_8UC1);
    while(tag_x>=0&&tag_x<=rows-size)//
    {
        while(tag_y>=0&&tag_y<=cols-size-1)
        {
            while(tag1>=0&&tag1<=size)
            {
                while(tag2>=0&&tag2<=size)
                {
                    if(image_source.at(tag_x+tag1,tag_y+tag2)(tag_x+tag1,tag_y+tag2));
                    tag2++;
                }
                tag1++;
                tag2=0; 
            }
            image_new.at(tag_x/size,tag_y/size)=tag3[1];
            tag3[1]=255;
            tag_y+=size;tag1=0;tag2=0;
        }
        tag_x+=size;
        tag_y=0;tag1=0;tag2=0;
    }
    return image_new;

}


int main()
{
    A a;
    a.deal_image();
    a.find_ori();
    a.deal_A();
    a.draw_road();
    cin.ignore();
}

---

www.gamedev.net/reference/articles/article2003.asp 

A*（发音为 A-star）算法对于初学者来说可能很复杂。虽然网上有很多解释 A* 的文章，但大多数都是为已经了解基础知识的人编写的。这篇文章是为真正的初学者准备的。

本文并不试图成为该主题的权威著作。相反，它描述了基础知识，并让准备好出去阅读所有其他材料并理解他们在说什么。本文末尾的进一步阅读下提供了一些最佳链接。

最后，本文不是特定于程序的。应该能够使这里的内容适应任何计算机语言。然而，正如所料，我在本文末尾提供了一个示例程序的链接。示例包包含两个版本：一个是 C++ 版本，一个是 Blitz Basic 版本。如果只想查看 A* 的运行情况，它还包含可执行文件。

将其从打开列表中删除并将其添加到关闭列表中。
检查所有相邻的方块。忽略那些在封闭列表中或无法行走（有墙壁、水或其他非法地形的地形）的那些，如果它们已经不在开放列表中，则将它们添加到开放列表中。使选定的方块成为新方块的“父级”。
如果相邻的方格已经在打开列表中，请检查通往该方格的这条路径是否更好。换句话说，如果我们使用当前方格到达那里，请检查该方格的 G 分数是否较低。如果没有，不要做任何事情。
另一方面，如果新路径的 G 成本较低，则将相邻方格的父方更改为选定方格（在上图中，将指针的方向更改为指向选定方格）。最后，重新计算该方格的 F 和 G 分数。如果这看起来令人困惑，您将在下面看到它。
将起始方块（或节点）添加到打开列表中。
重复以下操作：
a) 在开放列表中寻找最低的 F 成本方。我们将其称为当前方格。
b) 将其切换到关闭列表。
c) 对于与当前方格相邻的 8 个方格中的每一个 ...
如果它不可步行或在封闭列表中，请忽略它。否则请执行以下操作。
如果它不在打开列表中，请将其添加到打开列表中。使当前方格成为该方格的父方。记录正方形的 F、G 和 H 成本。
如果它已经在开放列表中，请使用 G 成本作为衡量标准，检查通往该方格的这条路径是否更好。较低的 G 成本意味着这是一条更好的路径。如果是，则将方块的父级更改为当前方块，并重新计算该方块的 G 和 F 分数。如果您保持您的开放列表按 F 分数排序，您可能需要借助该列表来说明更改。
将目标方块添加到封闭列表中，在这种情况下已找到路径（请参见下面的注释），或
找不到目标方格，打开列表为空。在这种情况下，没有路径。
保存路径。从目标方格向后工作，从每个方格到其父方格，直到到达起始方格。那是你的道路。注意：在本文的早期版本中，建议您可以在目标方格（或节点）已添加到打开列表而不是关闭列表时停止。这样做会更快，它几乎总是会给你最短的路径，但并非总是如此。这样做可能会产生影响的情况是，从第二个节点移动到最后一个节点到最后一个（目标）节点的移动成本可能会有很大差异 - 例如，在两个节点之间的河流交叉的情况下。 

---