ROS1云课→19仿真turtlebot（stage）
19提及的机器人如何实现全局路径规划？A*算法是一种可行的选择。
www.gamedev.net/reference/articles/article2003.asp
A*算法的基本介绍可以查询网络资源。这里，列出一些案例：
蓝桥ROS扩展笔记CppRobotics编译_zhangrelay的博客-CSDN博客
第一种：
代码语言：javascript复制/*************************************************************************
> 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）？
迷宫有中杯，大杯，超大杯。
中杯版本如下：
大杯版本如下：
超超大杯版本：
咱就不玩超大号的了。
就用下面这个地图吧：
中：
大：
代码参考：
代码语言：javascript复制#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 %dn",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 分数排序，您可能需要借助该列表来说明更改。 将目标方块添加到封闭列表中，在这种情况下已找到路径（请参见下面的注释），或 找不到目标方格，打开列表为空。在这种情况下，没有路径。 保存路径。从目标方格向后工作，从每个方格到其父方格，直到到达起始方格。那是你的道路。注意：在本文的早期版本中，建议您可以在目标方格（或节点）已添加到打开列表而不是关闭列表时停止。这样做会更快，它几乎总是会给你最短的路径，但并非总是如此。这样做可能会产生影响的情况是，从第二个节点移动到最后一个节点到最后一个（目标）节点的移动成本可能会有很大差异 - 例如，在两个节点之间的河流交叉的情况下。
