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

Posted zhangrelay

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了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: ltai@ust.hk
	> Created Time: Sat Jul 20 12:38:43 2019
 ************************************************************************/

#include<iostream>
#include<cmath>
#include<limits>
#include<queue>
#include<vector>
#include<opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>

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<std::vector<float> > calc_final_path(Node * goal, float reso, cv::Mat& img, float img_reso)
  std::vector<float> rx;
  std::vector<float> 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<std::vector<int> > calc_obstacle_map(
    std::vector<int> ox, std::vector<int> 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<std::vector<int> > obmap(ywidth, vector<int>(xwidth, 0));

  for(int i=0; i<xwidth; i++)
    int x = i + min_ox;
    for(int j=0; j<ywidth; j++)
      int y = j + min_oy;
      for(int k=0; k<ox.size(); k++)
        float d = std::sqrt(std::pow((ox[k]-x), 2)+std::pow((oy[k]-y), 2));
        if (d <= vr/reso)
          obmap[i][j] = 1;
          cv::rectangle(img,
                        cv::Point(i*img_reso+1, j*img_reso+1),
                        cv::Point((i+1)*img_reso, (j+1)*img_reso),
                        cv::Scalar(0, 0, 0), -1);
          break;
        
      
    
  
  return obmap;



bool verify_node(Node* node,
                 vector<vector<int> > 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<Node> 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<float> ox_, vector<float> 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<int> ox;
  vector<int> oy;

  int min_ox = std::numeric_limits<int>::max();
  int max_ox = std::numeric_limits<int>::min();
  int min_oy = std::numeric_limits<int>::max();
  int max_oy = std::numeric_limits<int>::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<std::vector<int> > visit_map(xwidth, vector<int>(ywidth, 0));
  
  std::vector<std::vector<float> > path_cost(xwidth, vector<float>(ywidth, std::numeric_limits<float>::max()));

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

  std::vector<std::vector<int> > 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<Node*, std::vector<Node*>, decltype(cmp)> pq(cmp);

  pq.push(nstart);
  std::vector<Node> 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; i<motion.size(); i++)
      Node * new_node = new Node(
        node->x + 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<float> ox;
  vector<float> 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 <stdio.h>
#include<string.h>
#include<math.h>
#include<iostream>
#include<opencv2/highgui.hpp>
#include<opencv2/opencv.hpp>
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<Vec3b>((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<<rgb;
    printf("%d %d \\n",z1,z2);
    //
    if(((z1==x1)&&(z2==x2))||mm>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<image_b.rows;tag_a++)
    
    for(int tag_b=0;tag_b<image_b.cols;tag_b++)
    if(image_b.at<uchar>(tag_a,tag_b)>100)image_b.at<uchar>(tag_a,tag_b)=255;
    else image_b.at<uchar>(tag_a,tag_b)=0;
    
    //
    for(int tag_b=0;tag_b<image_b.cols;tag_b++)
    if(image_b.at<uchar>(tag_b,0)==255)
    
        x1=tag_b/rate-1;
        x2=0;
    

    for(int tag_b=0;tag_b<image_b.cols;tag_b++)
    if(image_b.at<uchar>(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<b.rows;tag_a++)
    
    for(int tag_b=0;tag_b<b.cols;tag_b++)
    if(b.at<uchar>(tag_a,tag_b)>100)b.at<uchar>(tag_a,tag_b)=255;
    else b.at<uchar>(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<m.rows;tag_a++)
    
    for(int tag_b=0;tag_b<m.cols;tag_b++)
    if(m.at<uchar>(tag_a,tag_b)>100)m.at<uchar>(tag_a,tag_b)=255;
    else m.at<uchar>(tag_a,tag_b)=0;
    
    //
    imshow("c",m);
    //
    for(int tag_a=0;tag_a<m.rows;tag_a++)
    for(int tag_b=0;tag_b<m.cols;tag_b++)
    map[tag_a][tag_b]=m.at<uchar>(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<tag_open;tag_a++)
    
        //
        if((close[tag_close-1][0])==open[tag_a][0]&&(close[tag_close-1][1]+1)==open[tag_a][1])
       //
        //open[tag_a][2]=close[tag_close-1][2]+1;
        keyy1=1;
        break;
    
        if(keyy1==0)
    
        map_root[close[tag_close-1][0]][close[tag_close-1][1]+1][0]=close[tag_close-1][0];
        map_root[close[tag_close-1][0]][close[tag_close-1][1]+1][1]=close[tag_close-1][1];
    
    keyy1=0;
    
    
    //
    if(open[com[1]][3]==100000)
    
        open[com[1]][0]=close[tag_close-1][0];
        open[com[1]][1]=close[tag_close-1][1]+1;
        open[com[1]][2]=close[tag_close-1][2]+1;
        open[com[1]][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);

    
    else
    
    //
        open[tag_open][0]=close[tag_close-1][0];
        open[tag_open][1]=close[tag_close-1][1]+1;
        open[tag_open][2]=close[tag_close-1][2]+1;
        open[tag_open][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
        tag_open++;
    
    


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)


        for(int tag_a=0;tag_a<tag_open;tag_a++)
    
        if((close[tag_close-1][0])==open[tag_a][0]&&(close[tag_close-1][1]-1)==open[tag_a][1])
    
        //open[tag_a][2]=close[tag_close-1][2]+1;
        keyy2=1;
        break;
    
        if(keyy2==0)
    
        map_root[close[tag_close-1][0]][close[tag_close-1][1]-1][0]=close[tag_close-1][0];
        map_root[close[tag_close-1][0]][close[tag_close-1][1]-1][1]=close[tag_close-1][1];
    
    keyy2=0;
    
    
        if(open[com[1]][3]==100000)
    
        open[com[1]][0]=close[tag_close-1][0];
        open[com[1]][1]=close[tag_close-1][1]-1;
        open[com[1]][2]=close[tag_close-1][2]+1;
        open[com[1]][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);

    
    else
    
        open[tag_open][0]=close[tag_close-1][0];
        open[tag_open][1]=close[tag_close-1][1]-1;
        open[tag_open][2]=close[tag_close-1][2]+1;
        open[tag_open][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
        tag_open++;
        

    
    


if(map[close[tag_close-1][0]+1][close[tag_close-1][1]]==255&&(close[tag_close-1][0]+1)>=0&&(close[tag_close-1][1])>=0)


    for(int tag_a=0;tag_a<tag_open;tag_a++)
    
        if((close[tag_close-1][0]+1)==open[tag_a][0]&&(close[tag_close-1][1])==open[tag_a][1])
    
        //open[tag_a][2]=close[tag_close-1][2]+1;
        keyy3=1;
        break;
    
        if(keyy3==0)
        
        map_root[close[tag_close-1][0]+1][close[tag_close-1][1]][0]=close[tag_close-1][0];
        map_root[close[tag_close-1][0]+1][close[tag_close-1][1]][1]=close[tag_close-1][1];
        
    keyy3=0;
    
    
            if(open[com[1]][3]==100000)
    
        open[com[1]][0]=close[tag_close-1][0]+1;
        open[com[1]][1]=close[tag_close-1][1];
        open[com[1]][2]=close[tag_close-1][2]+1;
        open[com[1]][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);

    
    else
    
            open[tag_open][0]=close[tag_close-1][0]+1;
            open[tag_open][1]=close[tag_close-1][1];
            open[tag_open][2]=close[tag_close-1][2]+1;
            open[tag_open][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
            tag_open++;
        

    
    


if(map[close[tag_close-1][0]-1][close[tag_close-1][1]]==255&&(close[tag_close-1][0]-1)>=0&&(close[tag_close-1][1])>=0)


    
    for(int tag_a=0;tag_a<tag_open;tag_a++)
    
        if((close[tag_close-1][0]-1)==open[tag_a][0]&&(close[tag_close-1][1])==open[tag_a][1])
    
        //open[tag_a][2]=close[tag_close-1][2]+1;
        keyy4=1;
        break;
    
        if(keyy4==0)
        
        map_root[close[tag_close-1][0]-1][close[tag_close-1][1]][0]=close[tag_close-1][0];
        map_root[close[tag_close-1][0]-1][close[tag_close-1][1]][1]=close[tag_close-1][1];
        
    keyy4=0;
    

            if(open[com[1]][3]==100000)
    
        open[com[1]][0]=close[tag_close-1][0]-1;
        open[com[1]][1]=close[tag_close-1][1];
        open[com[1]][2]=close[tag_close-1][2]+1;
        open[com[1]][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);

    
    else
    
    open[tag_open][0]=close[tag_close-1][0]-1;
    open[tag_open][1]=close[tag_close-1][1];
    open[tag_open][2]=close[tag_close-1][2]+1;
    open[tag_open][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    tag_open++;

    


com[0]=10000;
com[1]=0;
//
for(int tag_a=0;tag_a<tag_open;tag_a++)

if((open[tag_a][2]+open[tag_a][3])<com[0])

    com[1]=tag_a;
    com[0]=open[tag_a][2]+open[tag_a][3];



//
close[tag_close][0]=open[com[1]][0];
close[tag_close][1]=open[com[1]][1];
//map_root[close[tag_close][0]][close[tag_close][1]][0]=close[tag_close-1][0];
//map_root[close[tag_close][0]][close[tag_close][1]][1]=close[tag_close-1][1];
//
map[close[tag_close-1][0]][close[tag_close-1][1]]=0;
//
open[com[1]][3]=100000;
tag_close++;
    //printf("\\n %d %d\\n",close[tag_close-1][0],close[tag_close-1][1]);
//waitKey(5);
if(map[z1][z2]==0)break;






int f(int x1,int x2,int z1,int z2)


return abs(x1-z1)+abs(x2-z2);

Mat pool_max(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)
        
            while(tag1>=0&&tag1<=size)
            
                while(tag2>=0&&tag2<=size)
                
                    if(image_source.at<uchar>(tag_x+tag1,tag_y+tag2)>tag3[1])(tag3[1]=image_source.at<uchar>(tag_x+tag1,tag_y+tag2));
                    tag2++;
                
                tag1++;
                tag2=0; 
            
            image_new.at<uchar>(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<uchar>(tag_x+tag1,tag_y+tag2)<tag3[1])(tag3[1]=image_source.at<uchar>(tag_x+tag1,tag_y+tag2));
                    tag2++;
                
                tag1++;
                tag2=0; 
            
            image_new.at<uchar>(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 分数排序,您可能需要借助该列表来说明更改。
将目标方块添加到封闭列表中,在这种情况下已找到路径(请参见下面的注释),或
找不到目标方格,打开列表为空。在这种情况下,没有路径。
保存路径。从目标方格向后工作,从每个方格到其父方格,直到到达起始方格。那是你的道路。注意:在本文的早期版本中,建议您可以在目标方格(或节点)已添加到打开列表而不是关闭列表时停止。这样做会更快,它几乎总是会给你最短的路径,但并非总是如此。这样做可能会产生影响的情况是,从第二个节点移动到最后一个节点到最后一个(目标)节点的移动成本可能会有很大差异 - 例如,在两个节点之间的河流交叉的情况下。 


以上是关于ROS1云课→20迷宫不惑之A*大法(一种虽古老但实用全局路径规划算法)的主要内容,如果未能解决你的问题,请参考以下文章

ROS1云课→21可视化工具rviz中的A*

ROS1云课→21可视化工具rviz中的A*

迷宫逃离的问题-CoCube

迷宫逃离的问题-CoCube

ROS1云课→29如何借助ROS实现走迷宫机器人

ROS1云课→29如何借助ROS实现走迷宫机器人