路径规划图解A、Dijkstra、GBFS算法的异同(附C++PythonMatlab仿真)

小蜜蜂採花蜜

目录0专栏介绍1栅格地图与邻域2贪婪最佳优先搜索3Dijkstra算法4启发式A*搜索5A*、Dijkstra、GBFS算法的异同6算法仿真与实现6.1算法流程6.2ROSC++实现6.3Python实现6.4Matlab实现0专栏介绍🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。🚀详情：图解自动驾驶中的运动规划(MotionPlanning)，附几十种规划算法1栅格地图与邻域搜索(Search)是指从初始状态(节点)出发寻找一组能达到目标的行动序列(或称问题的解)的过程。在图搜索中，往往将环境简化为栅格地图(GridMap)，易于刻画固定场景，同时也便于计算机控制系统进行信息处理。所谓栅格就是将连续地图用固定大小正方形方格进行离散化的单位。在栅格地图中，常见的邻域(neighbor)模式如下所示，即8邻域24邻域48邻域栅格的邻域表示了从当前位置出发下一次搜索的集合，例如八邻域法中，当前栅格只能和周围的八个栅格相连形成局部路径。下面是一个图搜索问题的例子，可以直观理解什么是搜索问题。例1：在如下的栅格地图中，设绿色栅格为起点，红色栅格为终点，灰色栅格为障碍，白色栅格为可行点，问如何设计一条由栅格组成的连接起点、终点的路径，并尽可能使路径最短？接下来，围绕这个问题展开阐述。2贪婪最佳优先搜索一个朴素的想法是：每一次搜索时就找那些与终点最近的节点，这里衡量最近可以用多种度量方式——曼哈顿距离、欧式距离等。这种方法像一头狼贪婪地望着食物，迫切寻求最近的路径，因此称为贪婪最佳优先搜索(GreedyBestFirstSearch,GBFS)。假设采用八邻域法，在GBFS思想指导下，在起点的八邻域中就会选择最右侧的节点，如下所示。循环地，直到如下所示的节点，因为邻域内有障碍，这些障碍节点不会被候选，所以此时离终点最近的就是下方的方格依次类推直至终点3Dijkstra算法Dijkstra算法走向了另一个极端，它完全不考虑扩展节点与终点的关系，而是定义了一个路径耗散函数g(n)g(n)g(n)，从起点开始，机器人每走一个栅格就会产生一定的代价或耗散，因为Dijkstra算法希望路径最短，所以每次首选那些使路径耗散最小的节点。依照Dijkstra算法的观点，从起点开始，其八个邻域节点都会被依次探索，因为它们离起点最近，接着再探索这些节点的子节点。因此Dijkstra算法会遍历大量的节点，一圈圈地逼近终点4启发式A*搜索A*算法是非常有效且常用的路径规划算法之一，其是结合Dijsktra算法与GBFS各自优势的启发式搜索算法，其搜索代价评估函数为f(n)=g(n)+h(n)f(n)=g(n)+h(n)f(n)=g(n)+h(n)其中g(n)g(n)g(n)代表路径耗散，是Dijsktra算法分量；h(n)h(n)h(n)代表下一个搜索节点与终点的距离，启发式地引导机器人朝着终点拓展，是GBFS算法分量。兼具两个算法特点的A*算法既保持完备性，又在一定条件下体现出最优性，被广泛应用于路径规划中。5A*、Dijkstra、GBFS算法的异同特别地当g(n)=0g\left(n\right)=0g(n)=0时，启发函数影响占据主导，A*算法退化为GBFS算法——完全不考虑状态空间本身的固有属性，不择手段地追求对目标的趋近，此时算法搜索效率将得到提升，但最优性无法保证；当h(n)=0h(n)=0h(n)=0时，路径耗散函数影响占据主导，A*算法退化为Dijsktra算法——无先验信息搜索，此时算法搜索效率下降，但最优性上升。三个算法的直观比较如下所示6算法仿真与实现6.1算法流程6.2ROSC++实现核心代码如下std::tuple>AStar::plan(constunsignedchar*costs,constNode&start,constNode&goal,std::vector&expand){//openliststd::priority_queue,compare_cost>open_list;open_list.push(start);//closedliststd::unordered_setclosed_list;//expandlistexpand.clear();expand.push_back(start);//getallpossiblemotionsconststd::vectormotion=getMotion();//mainloopwhile(!open_list.empty()){//popcurrentnodefromopenlistNodecurrent=open_list.top();open_list.pop();current.id=this->grid2Index(current.x,current.y);//currentnodedonotexistinclosedlistif(closed_list.find(current)!=closed_list.end())continue;//goalfoundif(current==goal){closed_list.insert(current);return{true,this->_convertClosedListToPath(closed_list,start,goal)};}//exploreneighborofcurrentnodefor(constauto&m:motion){Nodenew_point=current+m;//currentnodedonotexistinclosedlistif(closed_list.find(new_point)!=closed_list.end())continue;//exploreanewnodenew_point.id=this->grid2Index(new_point.x,new_point.y);new_point.pid=current.id;//ifusingdijkstraimplementation,donotconsiderheuristicscostif(!this->is_dijkstra_)new_point.h_cost=std::sqrt(std::pow(new_point.x-goal.x,2)+std::pow(new_point.y-goal.y,2));//ifusingGBFSimplementation,onlyconsiderheuristicscostif(this->is_gbfs_)new_point.cost=0;//goalfoundif(new_point==goal){open_list.push(new_point);break;}//bextnodehittheboundaryorobstacleif(new_point.id=this->ns_||costs[new_point.id]>=this->lethal_cost_*this->factor_)continue;open_list.push(new_point);expand.push_back(new_point);}closed_list.insert(current);}return{false,{}};}}123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172736.3Python实现核心代码如下defplan(self):#OPENsetwithpriorityandCLOSEDsetOPEN=[]heapq.heappush(OPEN,self.start)CLOSED=[]whileOPEN:node=heapq.heappop(OPEN)#existsinCLOSEDsetifnodeinCLOSED:continue#goalfoundifnode==self.goal:CLOSED.append(node)returnself.extractPath(CLOSED),CLOSEDfornode_ninself.getNeighbor(node):#existsinCLOSEDsetifnode_ninCLOSED:continuenode_n.parent=node.currentnode_n.h=self.h(node_n,self.goal)#goalfoundifnode_n==self.goal:heapq.heappush(OPEN,node_n)break#updateOPENsetheapq.heappush(OPEN,node_n)CLOSED.append(node)return[],[]1234567891011121314151617181920212223242526272829303132333435366.4Matlab实现核心代码如下while~isempty(OPEN(:,1))%popf=OPEN(:,3)+OPEN(:,4);[~,index]=min(f);cur_node=OPEN(index,;OPEN(index,=[];%existsinCLOSEDsetifloc_list(cur_node,CLOSED,[1,2])continueend%goalfoundifcur_node(1)==goal(1)&cur_node(2)==goal(2)CLOSED=[cur_node;CLOSED];flag=true;cost=cur_node(3);breakend%exploreneighborsfori=1:neighbor_numnode_n=[cur_node(1)+neighbor(i,1),...cur_node(2)+neighbor(i,2),...cur_node(3)+neighbor(i,3),...0,cur_node(1),cur_node(2)];node_n(4)=h(node_n(1:2),goal);%existsinCLOSEDsetifloc_list(cur_node,CLOSED,[1,2])continueend%obstacleifmap(node_n(1),node_n(2))==2continue;end%goalfoundifcur_node(1)==goal(1)&cur_node(2)==goal(2)CLOSED=[cur_node;CLOSED];flag=true;cost=cur_node(3);breakend%updateexpandzoneexpand=[expand;node_n(1:2)];%updateOPENsetOPEN=[OPEN;node_n];endCLOSED=[cur_node;CLOSED];end12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455🔥更多精彩专栏：《ROS从入门到精通》《Pytorch深度学习实战》《机器学习强基计划》《运动规划实战精讲》…👇源码获取·技术交流·抱团学习·咨询分享请联系👇