机器人寻路算法双向A*(Bidirectional A*)算法的实现C++、Python、Matlab语言

机器人寻路算法双向A*(Bidirectional A*)算法的实现C++、Python、Matlab语言

最近好久没更新,在搞华为的软件挑战赛(软挑),好卷只能说。去年还能混进32强,今年就比较迷糊了,这东西对我来说主要还是看运气,毕竟没有实力哈哈哈。

但是,好歹自己吭哧吭哧搞了两周,也和大家分享一下自己的收获吧,希望能为后来有需要的同学提供一些帮助。

我其实不太了解寻路算法的,很多代码也是找的demo自己改的,非常感谢这些大佬的帮助,有些引用可能现在也找不太到了,列不出来,非常感谢万能网友的代码,在此一起感谢啦!部分参考如下:
A*算法路径规划之Matlab实现
A算法路径规划博文附件1.zip
基于matlab的双向A*算法
【路径规划】A*算法方法改进思路简析
等等等

代码和运行资源下载:机器人寻路算法双向A*(Bidirectional A*)算法的实现C++、Python、Matlab语言

目录

  • 机器人寻路算法双向A*(Bidirectional A*)算法的实现C++、Python、Matlab语言
    • 1、基于Matlab的双向A*寻路算法
      • 1.1、基本地图
      • 1.2、非联通区域的地图处理
      • 1.3、双向A*寻路算法Matlab实现
      • 1.4、双向A*寻路算法的加速小TIPS
      • 1.5、现存的问题
    • 2、双向A*寻路算法的C++实现
      • 2.1、在C++得到类似结果
      • 2.2、得到最大联通区域地图
      • 2.3、有限时间的寻路计算
      • 2.4、使用历史路网结构
      • 2.5、中转站机制
      • 2.6、一坨稀烂的机器人防碰撞
      • 2.7、提前规划路径
      • 2.8、运行前修改main.h的日志导出路径,不然报错
    • 3、双向A*寻路算法的python实现

1、基于Matlab的双向A*寻路算法

C++的代码也是用Matlab导出的,因此实现的功能是相近的。

1.1、基本地图

这是我主要改的地方,C++的代码也是用这个Matlab导出去的。首先就是读入地图数据,华为任务书里面写了对应的格式。给出的地图原始格式为txt,有200行200列,代表200*200的地图(第一行代表横坐标为0,纵坐标为0-199,以此类推),海洋和障碍是不能走的地方,泊位、空地都是能够行走的。
在这里插入图片描述
在实际的地图中,我们只考虑实际的空地和障碍,实际的空地包括泊位、空地,实际的障碍包括海洋和障碍,得到的地图如下所示:
在这里插入图片描述
地图的读取主要使用了下面的核心代码:

%% 双向A星24域
% 读入的sign就是对应的格子图
[sign] = readHWmap('map1.txt');
% 定义地图的宽
row=200;
col=200;
% 寻路的起点和终点坐标
start_xy=[44 19];
end_xy=[50 80];

1.2、非联通区域的地图处理

值得注意的是,上面地图中有些空地是被障碍物包围的,因此无法到达。因此我对初始地图进行了处理,获得最大的联通区域(使用搜索算法)。这一步可以在地图初始化的时候进行:

% 得到和坐标【66,66】相连的大联通区域,避免不可达到的空地影响
[sign,area]=getAllOb(sign,66,66);

处理后得到的地图如下所示,可以看到不可抵达的空地全部使用障碍来填充了:
在这里插入图片描述

1.3、双向A*寻路算法Matlab实现

A*算法是一种传统的路径规划算法,相较于Dijkstra算法,其引入了启发式算子,有效的提高了路径的搜索效率。主要步骤包括:
1)设置起始点、目标点以及带障碍物的栅格地图
2)选择当前节点的可行后继节点加入到openlist中
3)从openlist中选择成本最低的节点加入closelist节点
4)重复执行步骤2和步骤3,直到当前节点到达目标点,否则地图中不存在可行路径
5)从closelist中选择从起点到终点的路径,并画图展示

传统A* 算法从起点开始搜索,而双向A*从起点和终点同时开始搜索,所以运算速度会大大加快。以华为的地图为例,按照下面设置起点和终点:

% 寻路的起点和终点坐标
start_xy=[44 19];
end_xy=[50 80];

最终得到的结果如下所示:
在这里插入图片描述

1.4、双向A*寻路算法的加速小TIPS

1、扩展邻域法

扩展邻域法的思想是通过提高单次邻域的搜索范围,从而减少整个过程中的搜索次数,从而降低计算量。此处实现使用的是24邻域。

但是24邻域和此处网格机器人不太一样,网格机器人只能上下左右移动,所以从物理上看只能有四邻域。使用24邻域能够加快搜索速度,但是在实际控制时需要额外的判断。

因此,我在实际实现时是使用双向Astar算法得到关键的路径节点,但是机器人的实际行动是用Astar算法控制的。因为双向Astar算法得到的节点之间的距离都非常近,在此基础上使用A*算法进行二次寻路和避障速度比较快(把其他机器人当成障碍物来避障)


2、有限区域初始化搜索
我们知道地图时非常大的,每次进行Astar算法的搜索都要进行很多的数组的初始化。按照此处的地图大小,是200*200的,一共有40000个数据。

所以在实际搜索时,我先依据起点和终点的位置,并留出一定的余量,从大地图中剥离出一个小地图,在这个小地图中进行搜索,这样速度会提升20-50%左右吧,起点和终点横跨的区域越小,这样做的优势就越明显。

实际实现留出的余量是这个参数:

% 在有些区域内使用算法加快速度,起点终点所含区域向外衍生20格,改为200则使用全局寻路
area_shift=20;

1.5、现存的问题

1、距离为1时进行搜索会抖动
但是我实际使用时发现在特别短的距离下寻路效果不好,会出现抖动的情况(不知道是不是代码问题),虽然长距离运算速度确实不错,例如我起点和终点的距离只差一格:

%% 双向A星24域
% 读入的sign就是对应的格子图
[sign] = readHWmap('map1.txt');
% 定义地图的宽
row=200;
col=200;
% 寻路的起点和终点坐标
start_xy=[44 19];
end_xy=[44 18];

得到的结果抖的不行,这是因为寻路是双向进行的,因此得到的最短的路径长度也是2,非常难受(但是只差1格感觉可以不用寻路了哈哈哈):
在这里插入图片描述


2、奇奇怪怪的点无法搜索得出路径
如果起点和终点弯弯绕绕,那么寻路算法最终会失败。这和路径的长短没有关系,而是看之间寻路的角度,这个现象非常奇怪,照理说Astar算法是100%可以寻到路径的,但是某项情况就是不行。我也试了一些其他的代码,都会寻路失败,C++代码里面也是这样,例如这样的:

% 寻路的起点和终点坐标
start_xy=[44 19];
end_xy=[36 63];

在这里插入图片描述

2、双向A*寻路算法的C++实现

此处我是使用Matlab的Coder Generater产生的C++代码,功能是一致的,简单介绍一下调试的流程和小小的优化。

2.1、在C++得到类似结果

正常来说,调用地图数据是软挑配套的脚本提供的,但是为了调试需要,我直接把地图1的数据存入C文件,这样可以直接debug调用了。

debug时,没有输入和输出内容,因此需要在和官方判题器交互之前打上断点,debug的内容在sys_fun.cpp中实现

简单的测试代码如下,寻路结果保存在pointsToVisit[0]中

    std::vector<std::vector<int>> mapdata_matrix_tmp(SIZE_MAP, std::vector<int>(SIZE_MAP, 0));signed char mapdata_vector_tmp[SIZE_MAP*SIZE_MAP];findRouteOKFlag=FLAG_END;// 得到最大联通区域getAllOb(map1_test, 66, 66, mapFindRouteAStat, obstaclesCoords);// mapFindRouteAStat转化为二维地图数组int index111 = 0;for (int i = 0; i < SIZE_MAP; ++i) {for (int j = 0; j < SIZE_MAP; ++j) {Map1_2D_Bit[j][i] = mapFindRouteAStat[index111];Map1_2D_Char[j][i] = mapFindRouteAStat[index111];index111++;}}// 得到地图所有障碍的索引find_nonzero_indices(mapFindRouteAStat, map_Ind1, map_Ind2);// 历史路网数据复位initRouteMemory();//下面一行打开则使用历史的路网结构// loadRouteMemory();//定义起点和终点,寻路结果保存在pointsToVisit[0]int start_xy_int[] = {44,19};int end_xy_int[] = {50,80};RobotTowardResource destination;destination.x = end_xy_int[0];destination.y = end_xy_int[1];robotDestinations[0].push_back(destination);getRoutePath(start_xy_int, end_xy_int,0);if(!myFindRouteTask.empty()){float start_xy[2]={myFindRouteTask[0][0],myFindRouteTask[0][1]};float end_xy[2]={myFindRouteTask[0][2],myFindRouteTask[0][3]};//初始化任务findroute_limit_Init(start_xy,end_xy, 20,  SIZE_MAP,  SIZE_MAP,myFindRouteTask[0][4]);myFindRouteTask.erase(myFindRouteTask.begin());}while(findRouteOKFlag==FLAG_RUNNING){findroute_limit(mapFindRouteAStat, map_Ind1,map_Ind2, route_debug, &dis);}

可以看到得到的结果和Matlab中的一致。
在这里插入图片描述

2.2、得到最大联通区域地图

值得注意的是,上面地图中有些空地是被障碍物包围的,因此无法到达。因此我对初始地图进行了处理,获得最大的联通区域。在C++中,这个实现对应下面的语句:

getAllOb(map1_test, 66, 66, mapFindRouteAStat, obstaclesCoords);

其中map1_test是我预存的地图数据,是1 * 40000的数组,相当于把200 * 200的地图数据展平了,数据中只包含0,1。0表示可以通过,1表示为障碍。mapFindRouteAStat是处理后的1*40000的地图,和map1_test的区别就是不可到达的空地也被视为了障碍。
为了方便使用二维索引,把这个数据转化为了二维数组:

    // mapFindRouteAStat转化为二维地图数组int index111 = 0;for (int i = 0; i < SIZE_MAP; ++i) {for (int j = 0; j < SIZE_MAP; ++j) {Map1_2D_Bit[j][i] = mapFindRouteAStat[index111];Map1_2D_Char[j][i] = mapFindRouteAStat[index111];index111++;}}

下面的索引计算相当于对地图障碍位置预先存入数组了,之后直接调用就行,方便加速计算:

// 得到地图所有障碍的索引find_nonzero_indices(mapFindRouteAStat, map_Ind1, map_Ind2);

2.3、有限时间的寻路计算

在寻路时需要考虑到实时性的要求,寻路的函数被封装为了,其中前两个参数为起点和终点,最后一个参数表示是为第几个机器人寻路的,寻路完成后路径会直接加到目标机器人要走的路径上去

getRoutePath(start_xy_int, end_xy_int,0);

getRoutePath函数并非直接调用了双向Astar寻路算法。函数中,对于较短的路径,一次性使用双向Astar得到路径

 //距离短直接进行运算得到结果// OutputData(LOG_PATH,0,"Direct Found Begin %f %f %f %f\n",start_xy_float[0],start_xy_float[1],end_xy_float[0],end_xy_float[1]);std::vector<std::array<float, 2>>pointstovisit_tmp;findroutevalid=findroute_Direct(start_xy_float,end_xy_float, FIND_ROUTE_AREA_SHIFT, SIZE_MAP, SIZE_MAP, mapFindRouteAStat, map_Ind1,map_Ind2, route_debug, &dis,pointstovisit_tmp,robot_id);if(!findroutevalid){//没有找到路径OutputData(LOG_PATH,0,"ID:%d Direct Found error\n"); }else{pointsToVisit[robot_id].insert(pointsToVisit[robot_id].end(), pointstovisit_tmp.begin(), pointstovisit_tmp.end());OutputData(LOG_PATH,0,"ID:%d %f %f %f %f\n",frame_ID,pointsToVisit[robot_id][0][0],pointsToVisit[robot_id][0][1],pointsToVisit[robot_id][1][0],pointsToVisit[robot_id][1][1]); }

对于较长的路径,如果要进行寻路,则会将创建一个任务列队,每次只运算有限的时间,这主要是考虑实时控制的要求:

myFindRouteTask.push_back({start_xy_float[0],start_xy_float[1],end_xy_float[0],end_xy_float[1],float(robot_id)});

模拟处理寻路任务时,其中while(findRouteOKFlag==FLAG_RUNNING)是模拟每帧不断处理的情况,事实上,每帧运行一次findroute_limit函数即可,因为函数中使用了std::chrono来限制每次运算的时间:

 if(!myFindRouteTask.empty()){float start_xy[2]={myFindRouteTask[0][0],myFindRouteTask[0][1]};float end_xy[2]={myFindRouteTask[0][2],myFindRouteTask[0][3]};//初始化任务findroute_limit_Init(start_xy,end_xy, 20,  SIZE_MAP,  SIZE_MAP,myFindRouteTask[0][4]);myFindRouteTask.erase(myFindRouteTask.begin());}while(findRouteOKFlag==FLAG_RUNNING){findroute_limit(mapFindRouteAStat, map_Ind1,map_Ind2, route_debug, &dis);}

实际运行处理时的函数是这样的:

void findLongRouteTask(void)
{//如果正在计算if(findRouteOKFlag==FLAG_RUNNING){findroute_limit(mapFindRouteAStat, map_Ind1,map_Ind2, route_debug, &dis);findLongRouteTask_Time++;//计算超时,不再计算了to do导出计算超时的点路径,超时导致不会再计算?if(findLongRouteTask_Time>500){findRouteOKFlag=FLAG_END;findLongRouteTask_Time=0;//一定删除目标点!!!!!!!!!!!!!!!!!!!!!!其实就是最后一个OutputData(LOG_LONG_PATH,0,"ID:%d robotID:%d findLongRouteTask END OVERTIME\n",frame_ID,robotFindRouteID);robotDestinations[robotFindRouteID].erase(robotDestinations[robotFindRouteID].end());}if(frame_ID%100==0){// OutputData(LOG_PATH,0,"ID:%d findLongRouteTask ing\n",frame_ID);}}else{// OutputData(LOG_PATH,0,"ID:%d findLongRouteTask FREE\n",frame_ID);//空闲则判断是否有任务没有完成//有任务没有执行完成if(!myFindRouteTask.empty()){findLongRouteTask_Time=0;float start_xy[2]={myFindRouteTask[0][0],myFindRouteTask[0][1]};float end_xy[2]={myFindRouteTask[0][2],myFindRouteTask[0][3]};//初始化任务findroute_limit_Init(start_xy,end_xy, 20,  SIZE_MAP,  SIZE_MAP,myFindRouteTask[0][4]);myFindRouteTask.erase(myFindRouteTask.begin());OutputData(LOG_LONG_PATH,0,"ID:%d robotID:%d findLongRouteTask Begin sx%f sy%f ex%f ey%f\n",frame_ID,robotFindRouteID,start_xy[0],start_xy[1],end_xy[0],end_xy[1]);}}
}

2.4、使用历史路网结构

loadRouteMemory();函数会加载历史走过的路径数据,因此曾经走过的路径不再需要寻路了。getRoutePath(start_xy_int, end_xy_int,0);函数会判断这条寻路能否使用历史的路网数据

主要加载的数据有这三个,在main.h中设置导出路径,会把历史的寻路数据直接导出来txt,加到Cpp文件就行了,非常方便:
在这里插入图片描述
简单介绍这三个的含义,routeMemoryBufTmp是路径数据,所有路径数据都存在这里面
routeMemoryLengthBufTmp是每条路径的长度,知道了了长度就能把routeMemoryBufTmp的全部路径的数据进行恢复了
unReachablePointTmp是寻路失败的目标点,如果这些点产生了货物,那么就会忽略这些货物。

float routeMemoryBufTmp[]={};
std::vector<int> routeMemoryLengthBufTmp={};
std::vector<int> unReachablePointTmp={};

加载的代码如下:

unsigned int index_tmp=0;
//赋值给要走的路径
for(int j=0;j<routeMemoryLengthBufTmp.size();j++)
{for(int i=0;i<routeMemoryLengthBufTmp[j];i++){routeMemoryBuf[j].push_back({routeMemoryBufTmp[0+2*index_tmp], routeMemoryBufTmp[1+2*index_tmp]});//涂黑路径边沿,方便进行路径重新调用setBitsAroundPosition(routeMemoryIndex[routeMemoryBufSize], routeMemoryBuf[routeMemoryBufSize][i][0], routeMemoryBuf[routeMemoryBufSize][i][1], ROUTE_MEMORYBUF_DIFFUSION);index_tmp++;}routeMemoryBufSize=routeMemoryBufSize+1;
}
for(int j=0;j<unReachablePointTmp.size();j=j+2)
{deletedTargets.push_back({unReachablePointTmp[j],unReachablePointTmp[j+1]});
}

我设置最多可以存储20000条历史路网数据,路网数据的索引使用了一点小技巧来加速。对于每个搜索得到的路网数据,我都创建了一个200x200的bit数组routeMemoryIndex,将路径节点和节点周围N格的数据都设置为1,其余为0。这样我得到一个寻路任务时,我只需要判断起点和终点在这个200x200的bit数组中的位置是否为1,就能判断能否使用这条路径了。

//先判断能否使用现有的路网结构
for(int i=0;i<routeMemoryBufSize;++i)
{if(routeMemoryIndex[i][start_xy[0]][start_xy[1]]==1&&routeMemoryIndex[i][end_xy[0]][end_xy[1]]==1){routememoryindex_tmp=i;//找到了现有路网,跳出For循环break;}
}

例如,红色为历史的路网数据。粉红色为将路径节点和节点周围N格的数据都设置为1的示意,在进行搜索时,如果起点和终点都位于一条路径的粉红色区域内,则判断为能使用这条历史路径,如蓝色圆圈所示的起点和终点。这样就会搜索起点到最短路径节点的距离,和终点到最短节点的距离,其余使用路网数据,由此可以省下许多时间。
在这里插入图片描述

2.5、中转站机制

奇奇怪怪的点无法搜索得出路径,那么我就设置了一个中转站机制,就是起点和终点位于给定区域的话,就先走到中转站,在前往目标点。但是这种方法逻辑复杂,唉,全书败笔,如果起点和终点分别位于station_area1、station_area2,那么就先前往中转区域station_area:

    TransferStation station;station.station_area1.clear();station.station_area2.clear();station.area1 = {{0, 0}, {100, 100}};   station.area2 = {{0, 100}, {100, 200}};station.station_area1.push_back({{32, 94}, {35, 96}});station.station_area1.push_back({{64, 93}, {66, 95}});station.station_area1.push_back({{80, 92}, {82, 95}});station.station_area2.push_back({{32, 105}, {35, 107}});station.station_area2.push_back({{64, 105}, {66, 107}});station.station_area2.push_back({{80, 105}, {82, 107}});map1TransferStation.push_back(station);

使用getRoutePathByTransfer(start_xy_int, end_xy_int,0);在寻路时调用中转站,实际上是对getRoutePath的二次封装。

2.6、一坨稀烂的机器人防碰撞

我用双向Astar得到关键节点,机器人前往节点的具体行动使用Astar算法(因为这段非常好寻路的,距离很短且没有障碍)行动时将其他机器人当作障碍来避障,效果很差,主要是狭窄通道有问题,多个机器人堵起来也有问题。

寻路没问题,就是要撞起来!!!

2.7、提前规划路径

会提前为机器人规划路径加入缓存,比如说我正在去拿货物,会提前规划从货物到码头的路径节点等等。

2.8、运行前修改main.h的日志导出路径,不然报错

3、双向A*寻路算法的python实现

参考:https://blog.csdn.net/m0_56662453/article/details/126426863

大佬现成的代码,学习的。双向Astar相比Astar节省了50%的时间。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.hqwc.cn/news/573038.html

如若内容造成侵权/违法违规/事实不符,请联系编程知识网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【安全用电管理系统的应用如何保证用电安全】Acrel-6000安科瑞智慧安全用电解决方案

政策背景 国家部委 ※2017年5月3日国务院安委会召开电气火灾综合治理工作视频会议&#xff0c;决定在全国范围内组织开展为期3年的电气火灾综合治理工作。 公安部领导 ※公安部副部长李伟强调&#xff1a;向科技要战斗力&#xff0c;加快推进“智慧消防”建设不断提升火灾防控…

夜莺浏览日志、filebeat采集日志(四)

文章目录 一、elasticsearch二、filebeat三、日志分析 一、elasticsearch docker启动 docker run -d -p 9200:9200 -p 9300:9300 --restartalways -e ES_JAVA_OPTS"-Xms512m -Xmx512m" \ -e discovery.typesingle-node -e xpack.security.enabledtrue -e ELASTIC_P…

01 | 深入理解 Swoole 的底层加载原理

首发原文链接&#xff1a;深入理解 Swoole 的底层加载原理 PHP 扩展加载 我们从 php-src/sapi/cli/php_cli.c:1159 文件的入口函数 int main(int argc, char *argv[]) 开始分析。 大家可以先看下面这张图&#xff0c;描述了整个关键函数的加载、调用流程。从模块的初始化&…

C++初学者:像C#一样优雅写程序

C语言的强大&#xff0c;在于自由自在&#xff0c;你可以随便怎么写程序&#xff0c;不讲格式&#xff0c;可以很整齐&#xff0c;也可以鬼画符&#xff0c;只要能运行就可以了&#xff0c;程序员可以自由发挥。 如图&#xff1a; 国际C语言大赛中的代码 图2 C的语言更强大&…

Django auth模块

【一】命令行创建用户 【1】语法 python manage.py createsuper【2】示例 用户名 默认是是电脑名称 邮箱 可以填也可以不填 密码 terminal中&#xff1a;输入密码不显示出来manage.py中&#xff1a;明文输入输入密码太简单会提示 Username (leave blank to use administra…

【爬虫基础】第6讲 opener的使用

在爬虫中&#xff0c;opener是一个用来发送HTTP请求的对象。它可以用来模拟浏览器发送请求&#xff0c;包括设置请求头、处理Cookie等操作。使用opener可以实现一些高级功能&#xff0c;如模拟登录、处理验证码等。 方法1&#xff1a; from urllib.request import Request,bu…

自动驾驶传感器:惯性导航IMU原理

自动驾驶传感器&#xff1a;惯性导航IMU原理 附赠自动驾驶学习资料和量产经验&#xff1a;链接 组合导航里包含了GNSS卫星导航模块与IMU惯性导航模块&#xff0c;前一篇文章写了GNSS模块&#xff0c;本章写IMU惯导&#xff0c;也是本系列最后一篇文章。 1. 惯性测量单元&…

python学习13:python中的字符串格式化

python中的字符串格式化另外一种方式 我们前面是使用的%d,%f,%s占位符来实现字符串的格式化的&#xff1b;这次我们通过f"{}"来快速实现格式化&#xff0c;代码示例如下&#xff1a;

vue 预览excel文件的又一伟大实践 —— vue-office

实际上&#xff0c;预览excel这个功能&#xff0c;我之前已经写过一个文章了。如下&#xff0c;使用的是 luckysheet/luckyExcel 实现的。 vue 实现在线预览Excel-LuckyExcel/LuckySheet实现方案_excel在线预览的方案-CSDN博客 但是最近客户使用发现一个bug&#xff0c;就是某…

WPF —— DockPanel、ProgressBar 控件详解

ProgressBar 控件详解 1Progress bar简介 ProgressBar&#xff1a;进度条控件。 WPF带有一个方便的控件用于显示进度&#xff0c;称ProgressBar。它的工作原理就是设置最小值和最大值然后通过递增一个值&#xff0c;这样就可以直观的显示当前进度情况。 2 Progress bar常用的…

web——xml,xxe

xml的格式 &#xff0c;我们可以通过这个读取文件 改成这个 这样就可以知道&#xff0c;文件里的信息&#xff0c;但这是有回显的&#xff0c;因为&#xff0c;有prinetf 带外测试 将数据包改成这个 就发现漏洞存在 这是有回显的情况 数据包这么写 然后在自己的服务器…

如何用Flask中的Blueprints构建大型Web应用

本文分享自华为云社区《构建大型Web应用Flask中的Blueprints指南》&#xff0c;作者&#xff1a; 柠檬味拥抱。 什么是Blueprints&#xff1f; 什么是Blueprints&#xff1f; Blueprints是Flask中的一种模式&#xff0c;用于将应用程序分解为可重用的模块。每个蓝图实际上是…