深蓝路径规划 A*作业

A*算法基本过程

1. 把起点加入 open list 。

2. 重复如下过程：

a. 遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点。

b. 把这个节点移到 close list 。

c. 对当前方格的 8 个相邻方格的每一个方格？

◆ 如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。

◆ 如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。

◆ 如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。如果你的 open list 是按 F 值排序的话，改变后你可能需要重新排序。

d. 停止，当你

◆ 把终点加入到了 open list 中，此时路径已经找到了，或者

◆ 查找终点失败，并且 open list 是空的，此时没有路径。

3. 保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。

Astar_searcher.cpp文件

//AstarGetSucc函数
inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> & neighborPtrSets, vector<double> & edgeCostSets)
{   neighborPtrSets.clear();edgeCostSets.clear();/**STEP 4: finish AstarPathFinder::AstarGetSucc yourself please write your code below***///if GLX_SIZEGridNodePtr nodePtr;Vector3i center = currentPtr->index;for(int dev_X = -1;dev_X <= 1;dev_X++){if(center[0]+dev_X>=0 && center[0]+dev_X<= GLX_SIZE){for(int dev_Y = -1;dev_Y <= 1;dev_Y++){if(center[1]+dev_Y>=0 && center[1]+dev_Y<= GLY_SIZE){for(int dev_Z = -1;dev_Z <= 1;dev_Z++){if(center[2]+dev_Z>=0 && center[2]+dev_Z<= GLZ_SIZE){nodePtr = GridNodeMap[center[0]+dev_X][center[1]+dev_Y][center[2]+dev_Z];if(isOccupied(nodePtr->index) || nodePtr->id == -1)continue;else{neighborPtrSets.push_back(nodePtr);edgeCostSets.push_back(sqrt((nodePtr->index(0) - currentPtr->index(0)) * (nodePtr->index(0) - currentPtr->index(0)) +(nodePtr->index(1) - currentPtr->index(1)) * (nodePtr->index(1) - currentPtr->index(1)) +(nodePtr->index(2) - currentPtr->index(2)) * (nodePtr->index(2) - currentPtr->index(2))   ) );}}}}}}}
}

//启发函数 getHeu
double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2)
{/* choose possible heuristic function you wantManhattan, Euclidean, Diagonal, or 0 (Dijkstra)Remember tie_breaker learned in lecture, add it here ?***STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic functionplease write your code below***/double Euclidean_dis = 0;Euclidean_dis = sqrt((node1->index(0) - node2->index(0)) * (node1->index[0] - node2->index(0)) +(node1->index(1) - node2->index(1)) * (node1->index(1) - node2->index(1)) +(node1->index(2) - node2->index(2)) * (node1->index(2) - node2->index(2)) );return Euclidean_dis;
}

//寻找路径函数
void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt)
{   ros::Time time_1 = ros::Time::now();    //index of start_point and end_pointVector3i start_idx = coord2gridIndex(start_pt);Vector3i end_idx   = coord2gridIndex(end_pt);goalIdx = end_idx;//position of start_point and end_pointstart_pt = gridIndex2coord(start_idx);end_pt   = gridIndex2coord(end_idx);//Initialize the pointers of struct GridNode which represent start node and goal nodeGridNodePtr startPtr = new GridNode(start_idx, start_pt);GridNodePtr endPtr   = new GridNode(end_idx,   end_pt);//openSet is the open_list implemented through multimap in STL libraryopenSet.clear();closedSet.clear();// currentPtr represents the node with lowest f(n) in the open_listGridNodePtr currentPtr  = NULL;GridNodePtr neighborPtr = NULL;//put start node in open setstartPtr -> gScore = 0;startPtr -> fScore = getHeu(startPtr,endPtr);   //STEP 1: finish the AstarPathFinder::getHeu , which is the heuristic functionstartPtr -> id = 1; startPtr -> coord = start_pt;openSet.insert( make_pair(startPtr -> fScore, startPtr) );/**STEP 2 :  some else preparatory works which should be done before while loopplease write your code below***/vector<GridNodePtr> neighborPtrSets;vector<double> edgeCostSets;int i =0;// this is the main loopwhile ( !openSet.empty() ){/***step 3: Remove the node with lowest cost function from open set to closed setplease write your code belowIMPORTANT NOTE!!!This part you should use the C++ STL: multimap, more details can be find in Homework description***///从openset取出一个代价最小的节点std::cout<<"time1   "<<i<<endl;if(i == 32766)i=0;i++;currentPtr = openSet.begin()->second;//id = -1 表示已经访问过currentPtr->id = -1;//将节点从openset里面删除openSet.erase(openSet.begin());// if the current node is the goal if( currentPtr->index == goalIdx ){ros::Time time_2 = ros::Time::now();terminatePtr = currentPtr;ROS_WARN("[A*]{sucess}  Time in A*  is %f ms, path cost if %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution );            return;}//get the succetionAstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets);  //STEP 4: finish AstarPathFinder::AstarGetSucc yourself     /***STEP 5:  For all unexpanded neigbors "m" of node "n", please finish this for loopplease write your code below*        */         for(int i = 0; i < (int)neighborPtrSets.size(); i++){/***Judge if the neigbors have been expandedplease write your code belowIMPORTANT NOTE!!!neighborPtrSets[i]->id = -1 : expanded, equal to this node is in close setneighborPtrSets[i]->id = 1 : unexpanded, equal to this node is in open set*        */neighborPtr = neighborPtrSets[i];if(neighborPtr -> id == 0){ //discover a new node, which is not in the closed set and open set/***STEP 6:  As for a new node, do what you need do ,and then put neighbor in open set and record itplease write your code below*        */neighborPtr->cameFrom = currentPtr;neighborPtr -> gScore = getHeu(neighborPtr,currentPtr) + currentPtr->gScore;neighborPtr -> fScore = getHeu(neighborPtr,endPtr)+neighborPtr -> gScore;neighborPtr -> id = 1;neighborPtr -> coord = gridIndex2coord(neighborPtr->index);openSet.insert( make_pair(neighborPtr -> fScore, neighborPtr));continue;}else if(neighborPtr -> id == 1){ //this node is in open set and need to judge if it needs to update, the "0" should be deleted when you are coding/***STEP 7:  As for a node in open set, update it , maintain the openset ,and then put neighbor in open set and record itplease write your code below*        */int temp_gSore = getHeu(neighborPtr,currentPtr);if((temp_gSore+currentPtr->gScore)<neighborPtr->gScore){auto pr = openSet.equal_range(neighborPtr->fScore);neighborPtr->fScore = neighborPtr->fScore+temp_gSore+currentPtr->gScore-neighborPtr->gScore;neighborPtr->gScore = temp_gSore+currentPtr->gScore;neighborPtr->cameFrom = currentPtr;if(pr.first != end(openSet)){for(auto iter = pr.first;iter != pr.second;++iter){if(iter->second->index == neighborPtr->index){openSet.erase(iter);openSet.insert( make_pair(neighborPtr -> fScore, neighborPtr));}}}}continue;}else{//this node is in closed set/**please write your code below*        */continue;}}      }//if search failsros::Time time_2 = ros::Time::now();if((time_2 - time_1).toSec() > 0.1)ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec() );
}

//反向追溯得到路径
vector<Vector3d> AstarPathFinder::getPath()
{   vector<Vector3d> path;vector<GridNodePtr> gridPath;/***STEP 8:  trace back from the curretnt nodePtr to get all nodes along the pathplease write your code below*      */GridNodePtr tempPtr = terminatePtr;while(tempPtr != NULL){gridPath.push_back(tempPtr);tempPtr = tempPtr->cameFrom;}for (auto ptr: gridPath)path.push_back(ptr->coord);reverse(path.begin(),path.end());return path;
}

深蓝路径规划 A*作业相关推荐

MATLAB实现遗传算法、模拟退火遗传算法求解避障路径规划问题——计算智能作业
文章目录一.问题描述二.遗传算法设计 2.1 算法原理 2.2 编码 2.2 适应度函数 2.3 混合遗传算法三.实验结果及分析四.总结参考文献 MATLAB代码主程序相关函数一.问题 ...
【大作业】基于MATLAB的PRM算法的路径规划设计（随机地图+GUI+源码+报告）
基于MATLAB的PRM算法的路径规划设计下载链接: [Matlab期末大作业]基于MATLAB的PRM算法的路径规划设计(大报告+源代码+注释) 课题内容和要求学会运用MATLAB 解决实际优化 ...
【路径规划】Astart算法——图文直观解析
Astart算法详解 1. Astart算法路径搜索原理 2. 栅格网络的Astart算法 3. Astart算法优缺点 1. Astart算法路径搜索原理 Dijkstra算法已经是非常经典的求 ...
中国物流供应链“零的突破”！阿里路径规划算法入围运筹学“奥斯卡”
鱼羊发自凹非寺量子位报道 | 公众号 QbitAI 最新消息,中国物流力量,刚在全球运筹和管理科学界的最高荣誉中,实现零的突破! 1月15日,国际运筹学与管理科学学会(INFORMS)公布了2 ...
matlab 角度转四元数_基于Matlab的机械臂路径规划
什么是 trajectory(路径)规划中文路径在英语中可能有两种翻译: 1. path 2. trajectory 首先告诉大家,我们所说的"路径"是后者--trajector ...
菜鸟车辆路径规划创造26项世界纪录实际可降低10.3%配送成本
9日,菜鸟方面透露,菜鸟已经在全球权威车辆路径规划(VRP)问题评测系统中创造了26项世界记录.目前,菜鸟是国内首个问鼎该评测系统的研究机构.这意味着在26个物流场景中,菜鸟的算法可以使用最少的车辆, ...
基于图搜索的路径规划方法
ps:本文的相关图片来自与深蓝学院的课件. 图搜索的基本概念 Workspace:现实空间. 配置空间:机器人表示为一个点,障碍物表示为无法达到的点. 下面是不同的图的形式:抽象图.无向图.带权重的图 ...
基于dijsktra算法的最短路径求解_基于dijkstra算法的AGV路径规划(含C++代码)
文字&代码来源: @Wasabi喵喵喵基于dijkstra算法的AGV路径规划 dijkstra算法的路径规划经典Dijkstra算法是一种贪心算法,根据路径长度递增次序找到最短路径,通常 ...
中国物流领域首次！菜鸟路径规划算法入围全球最高工业奖项
12月10日,美国运筹学与管理科学学会信息显示,以菜鸟人工智能团队为核心研发的物流路径规划算法,已经入围2021年Franz Edelman杰出成就奖.这是全球运筹和管理科学界的最高工业应用奖,被称为 ...

深蓝路径规划 A*作业

深蓝路径规划 A*作业相关推荐

最新文章

热门文章