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;//visualizationcv::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 hereauto 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 edgesfor(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) { //closefor(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*大法（一种虽古老但实用全局路径规划算法）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。