机器人路径规划之A*算法(附C++源码)

来源丨 古月居

点击进入—>3D视觉工坊学习交流群

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(非工程文件,需自行配置),有不明白的可以在评论区留言,我会尽量回复

https://link.zhihu.com/?target=https%3A//github.com/kushuaiming/PathPalnning

本文仅做学术分享，如有侵权，请联系删文。

点击进入—>3D视觉工坊学习交流群

干货下载与学习

后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件

后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf

后台回复：3D视觉课程，即可学习3D视觉领域精品课程

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

8.从零搭建一套结构光3D重建系统[理论+源码+实践]

9.单目深度估计方法：算法梳理与代码实现

10.自动驾驶中的深度学习模型部署实战

11.相机模型与标定(单目+双目+鱼眼）

12.重磅！四旋翼飞行器：算法与实战

13.ROS2从入门到精通：理论与实战

14.国内首个3D缺陷检测教程：理论、源码与实战

15.基于Open3D的点云处理入门与实战教程

16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进

重磅！粉丝学习交流群已成立

交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。

扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿，微信号：dddvisiona

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看，3天内无条件退款

高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~