机器人路径规划之A*算法(附C++源码)
1. 基本原理
a*算法的本质是广度优先的图搜索。意在寻找一个从起点到目标节点的最短路径。
a*算法在dijkstra的基础上加入了启发式变量，一般用启发式距离（两点的直线距离）表示。
启发式距离
2. 算法伪代码
本伪代码摘取自principles of robot motion
其中o代表优先队列，c存放着已访问过的节点。
3. 关键c++代码剖析
先来看看a*算法运行的最终结果吧
首先先创建一个类代表节点（省略了构造函数等method）。
class node {
private：
int x， y; // 坐标
double sumcost; // f（n）
double heuristic;// 启发值
bool obstacle; // 是否是障碍物
node* backpoint; // 前驱节点
bool isvisited; // 判断是否访问过
};
在main函数中定义起始节点和目标节点
node startnode（40， 10）;// 起始点
node goalnode（10， 40）; // 目标点
初始化地图，这里计算了每个节点的启发式距离
for （int i = 0; i 《 mapsize; ++i） {
vector《node*》 tmp;
for （int j = 0; j 《 mapsize; ++j） {
node* tmpnode = new node（i， j）;
tmpnode-》setheuristic（calheristic（tmpnode， goalnode））;
tmp.push_back（tmpnode）;
}
roadmap.push_back（tmp）;
}
添加障碍物
void defineobstacle（vector《vector《node*》》& roadmap） {
// 先框住四周
for （int i = 0; i 《 mapsize; ++i） {
roadmap［0］［i］-》setobstacle（）;
roadmap［i］［0］-》setobstacle（）;
roadmap［i］［mapsize - 1］-》setobstacle（）;
roadmap［mapsize - 1］［i］-》setobstacle（）;
}
// 再定义一个条形快
for （int i = 1; i 《 mapsize / 2; ++i） {
roadmap［mapsize * 2 / 3］［i］-》setobstacle（）;
roadmap［mapsize * 2 / 3 - 1］［i］-》setobstacle（）;
roadmap［mapsize * 1 / 3］［mapsize - i］-》setobstacle（）;
roadmap［mapsize * 1 / 3 - 1］［mapsize - i］-》setobstacle（）;
}
}
a*算法函数
void astar（const node& startnode， const node& goalnode， vector《vector《node*》》& roadmap， mat& background） {
// 使用lambda表达式定义一个优先队列
auto cmp = ［］（node* left， node* right） { return left-》gn（） 》 right-》gn（）; };
priority_queue《node*， vector《node*》， decltype（cmp）》 o（cmp）;
node* tmp = roadmap［startnode.coordinatex（）］［startnode.coordinatey（）］;
o.push（tmp）;
// algorithm 24 a* algorithm
while （！o.empty（）） {
// pick nbest from o such that f（nbest） 《= f（n）。
node* nbest = o.top（）;
// remove nbest from o and add to c（isvisited）。
o.pop（）;
nbest-》setisvisited（）;
// if nbest == qgoal， exit.
if （*nbest == goalnode）
break;
// 八个方向都可以走
std：：vector《node》 motion = { 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）） };
for （int i = 0; i 《 8; i++） {
node tmpnode = motion［i］;
tmpnode += *nbest;
node* tmpnodepointer = roadmap［tmpnode.coordinatex（）］［tmpnode.coordinatey（）］;
*tmpnodepointer = tmpnode;
if （verifynode（*tmpnodepointer） && ！tmpnodepointer-》returnisvisited（） && ！tmpnodepointer-》isobstacle（）） {
o.push（tmpnodepointer）;
tmpnodepointer-》setisvisited（）;
tmpnodepointer-》setbackpoint（nbest）;
tmpnodepointer-》drawnode（background， imggridsize， scalar（0， 255， 0）， 0）;
imshow（“astar”， background）;
waitkey（5）;
}
}
}
// 画出最终的路径
tmp = roadmap［goalnode.coordinatex（）］［goalnode.coordinatey（）］;
while （！（*tmp == startnode）） {
tmp-》drawnode（background， imggridsize， scalar（255， 0， 0））;
tmp = tmp-》returnbackpoint（）;
imshow（“astar”， background）;
waitkey（5）;
}
}
4. 资源指路
a*算法其中大部分变量和算法过程我都尽量和principles of motion中的说明保持一致，源代码已上传github（非工程文件，需自行配置）
CGHV1J070D L波段放大器
广和通车联网业务全球布局
Qualcomm宣布推出Qualcomm TrueWireless™立体声技术
源创通信-12-G-AT POE Midspan (1U)供电中跨设备介绍
RS232和RS485通信协议的区别
机器人路径规划之A*算法(附C++源码)
人工智能技术设备哪些
智能照明设计/应用现状及发展趋势分析
2010年全球四大区域LED产业特色
工业机器人有哪些典型的应用场景?
AI系统中没有真正的人工智能,目前都无法与生物系统的能力相配
Type-C接口后，iPhone15的充电速度有没有提升？
芯海科技荣获“国产模拟IC行业技术突破卓越奖”
5G建设提速时代,共享建设成新趋势
彩虹塔的制作教程
ARM联合创始人公开致信英国首相
坦桑尼亚国家航空公司计划到2022年将机队规模扩大一倍
OPPO A77和小米6哪个好？没有对比就没有伤害，一眼辨别高性能手机
细数谷歌五笔大收购,亏在了摩托罗拉移动项目上
三星s8最新消息：三星S8价格仅次于华为Mate9保时捷版，性能却更强
a*算法的本质是广度优先的图搜索。意在寻找一个从起点到目标节点的最短路径。
a*算法在dijkstra的基础上加入了启发式变量，一般用启发式距离（两点的直线距离）表示。
启发式距离
2. 算法伪代码
本伪代码摘取自principles of robot motion
其中o代表优先队列，c存放着已访问过的节点。
3. 关键c++代码剖析
先来看看a*算法运行的最终结果吧
首先先创建一个类代表节点（省略了构造函数等method）。
class node {
private：
int x， y; // 坐标
double sumcost; // f（n）
double heuristic;// 启发值
bool obstacle; // 是否是障碍物
node* backpoint; // 前驱节点
bool isvisited; // 判断是否访问过
};
在main函数中定义起始节点和目标节点
node startnode（40， 10）;// 起始点
node goalnode（10， 40）; // 目标点
初始化地图，这里计算了每个节点的启发式距离
for （int i = 0; i 《 mapsize; ++i） {
vector《node*》 tmp;
for （int j = 0; j 《 mapsize; ++j） {
node* tmpnode = new node（i， j）;
tmpnode-》setheuristic（calheristic（tmpnode， goalnode））;
tmp.push_back（tmpnode）;
}
roadmap.push_back（tmp）;
}
添加障碍物
void defineobstacle（vector《vector《node*》》& roadmap） {
// 先框住四周
for （int i = 0; i 《 mapsize; ++i） {
roadmap［0］［i］-》setobstacle（）;
roadmap［i］［0］-》setobstacle（）;
roadmap［i］［mapsize - 1］-》setobstacle（）;
roadmap［mapsize - 1］［i］-》setobstacle（）;
}
// 再定义一个条形快
for （int i = 1; i 《 mapsize / 2; ++i） {
roadmap［mapsize * 2 / 3］［i］-》setobstacle（）;
roadmap［mapsize * 2 / 3 - 1］［i］-》setobstacle（）;
roadmap［mapsize * 1 / 3］［mapsize - i］-》setobstacle（）;
roadmap［mapsize * 1 / 3 - 1］［mapsize - i］-》setobstacle（）;
}
}
a*算法函数
void astar（const node& startnode， const node& goalnode， vector《vector《node*》》& roadmap， mat& background） {
// 使用lambda表达式定义一个优先队列
auto cmp = ［］（node* left， node* right） { return left-》gn（） 》 right-》gn（）; };
priority_queue《node*， vector《node*》， decltype（cmp）》 o（cmp）;
node* tmp = roadmap［startnode.coordinatex（）］［startnode.coordinatey（）］;
o.push（tmp）;
// algorithm 24 a* algorithm
while （！o.empty（）） {
// pick nbest from o such that f（nbest） 《= f（n）。
node* nbest = o.top（）;
// remove nbest from o and add to c（isvisited）。
o.pop（）;
nbest-》setisvisited（）;
// if nbest == qgoal， exit.
if （*nbest == goalnode）
break;
// 八个方向都可以走
std：：vector《node》 motion = { 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）） };
for （int i = 0; i 《 8; i++） {
node tmpnode = motion［i］;
tmpnode += *nbest;
node* tmpnodepointer = roadmap［tmpnode.coordinatex（）］［tmpnode.coordinatey（）］;
*tmpnodepointer = tmpnode;
if （verifynode（*tmpnodepointer） && ！tmpnodepointer-》returnisvisited（） && ！tmpnodepointer-》isobstacle（）） {
o.push（tmpnodepointer）;
tmpnodepointer-》setisvisited（）;
tmpnodepointer-》setbackpoint（nbest）;
tmpnodepointer-》drawnode（background， imggridsize， scalar（0， 255， 0）， 0）;
imshow（“astar”， background）;
waitkey（5）;
}
}
}
// 画出最终的路径
tmp = roadmap［goalnode.coordinatex（）］［goalnode.coordinatey（）］;
while （！（*tmp == startnode）） {
tmp-》drawnode（background， imggridsize， scalar（255， 0， 0））;
tmp = tmp-》returnbackpoint（）;
imshow（“astar”， background）;
waitkey（5）;
}
}
4. 资源指路
a*算法其中大部分变量和算法过程我都尽量和principles of motion中的说明保持一致，源代码已上传github（非工程文件，需自行配置）
CGHV1J070D L波段放大器
广和通车联网业务全球布局
Qualcomm宣布推出Qualcomm TrueWireless™立体声技术
源创通信-12-G-AT POE Midspan (1U)供电中跨设备介绍
RS232和RS485通信协议的区别
机器人路径规划之A*算法(附C++源码)
人工智能技术设备哪些
智能照明设计/应用现状及发展趋势分析
2010年全球四大区域LED产业特色
工业机器人有哪些典型的应用场景?
AI系统中没有真正的人工智能,目前都无法与生物系统的能力相配
Type-C接口后，iPhone15的充电速度有没有提升？
芯海科技荣获“国产模拟IC行业技术突破卓越奖”
5G建设提速时代,共享建设成新趋势
彩虹塔的制作教程
ARM联合创始人公开致信英国首相
坦桑尼亚国家航空公司计划到2022年将机队规模扩大一倍
OPPO A77和小米6哪个好？没有对比就没有伤害，一眼辨别高性能手机
细数谷歌五笔大收购,亏在了摩托罗拉移动项目上
三星s8最新消息：三星S8价格仅次于华为Mate9保时捷版，性能却更强