C++ A*算法实现示例

笔记背景

最近在学习自动驾驶规划模块，刚好看到了A*相关算法，查阅资料时发现了大佬写的《路径规划之 A* 算法》文章，写的特别棒，条理清晰，图文并茂。

大佬是用python语言和matplotlib库介绍了示例的详细设计过程，我将示例在C++语言环境下完成，顺便练练手，但是图形化界面就差了很多，只能使用字符打印出来。

大家在开始阅读我的笔记之前，一定要先看一下大佬的文章。大佬博客上的其他文章也很棒，感兴趣的都可以学习一下。

示例说明

示例是在一个二维的网格图形上从起点找寻终点的求解过程。

• 网格大小是50 x 50，起点定义在(0, 0)位置，终点在(49, 49)；
• 在网格图中会生成固定的障碍物一个，在左上角右下角连线的中间位置，2 x 9的规格；
• 随机障碍物6个，随机障碍物是1 x 12的规格，只在水平或者垂直方向上。

在下面算法实现里面会详细介绍上述障碍物的生成过程，生成包含障碍物之后的网格图如下图所示（随机障碍物每次不一样，下图仅供参考，如果障碍物超过网格边界则不再显示）。图中网格中的 “.”表示无障碍物出现，“ X ”表示存在障碍物。

示例通过使用A*算法演示如何从左下角的黄色起点规划一条绕过路径到达右上角红点的最短路径。

算法实现

再次说明，本实例是参考《路径规划之 A* 算法》文章的Python程序完成。

算法的源码可以到我的github上下载：Bitzhangheng/A-Star-Algorithm-Example

源码在Win10和Ubuntu都测试过，没有问题。

坐标点与地图

首先，创建一个简单的类来描述图中的网格（也就是点），相关代码如下：

// Point.h文件
#pragma once

class Point
{
   
public:
    Point(int x, int y):x_(x), y_(y){
   };
    
    int x_;
    int y_;
    double cost_;
    Point *parent_;
};

然后定义一个描述地图结构的类，在这个类中会生成固定障碍物和随机障碍物。

// RandomMap.h文件
#pragma once
#include "Point.h"
#include <vector>
using namespace std;
class RandomMap
{
   
public:
    RandomMap(int size = 50);   // 地图的默认大小是50x50
    vector<Point> GenerateObstacle();   // 生成固定和随机障碍物
    bool IsObstacle(int x, int y);  // 判断某个点是否是障碍物
    void PrintMap();

    int size_;
    int obstacle_;
    vector<Point> obstacle_point_;
};

具体的实现如下：

// RandomMap.cpp文件
#include "RandomMap.h"
#include <stdlib.h>
#include <time.h>
#include <iostream>

using namespace std;

RandomMap::RandomMap(int size)
{
   
    size_ = size;
    obstacle_ = size / 8;   //设置障碍物的数量为地图大小除以8；
    GenerateObstacle(); //调用GenerateObstacle生成随机障碍物；
}

vector<Point> RandomMap::GenerateObstacle()
{
   
    // 添加两个障碍物点
    obstacle_point_.push_back(Point(int(size_/2), int(size_/2)));
    obstacle_point_.push_back(Point(int(size_/2), int(size_/2)-1));

    // 在地图的中间生成一个斜着的障碍物
    for (int i = size_/2 - 4; i < size_/2; i++)
    {
   
        obstacle_point_.push_back(Point(i, size_-i));
        obstacle_point_.push_back(Point(i, size_-i-1));
        obstacle_point_.push_back(Point(size_-i, i));
        obstacle_point_.push_back(Point(size_-i, i-1)