路径规划-A*（C++实现）

路径规划A星算法，原理与C++实现

--天行健地势坤--

2610人浏览 · 2023-06-24 15:45:51

--天行健地势坤-- · 2023-06-24 15:45:51 发布

1、简单介绍

A*（念做：A Star）算法是一种很常用的路径查找和图形遍历算法。它有较好的性能和准确度。A*算法最初发表于1968年，由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael发表。它可以被认为是Dijkstra算法的扩展。由于借助启发函数的引导，A*算法通常拥有更好的性能。

2、与Dijkstra算法的异同点

2.1 、Dijkstra算法

Dijkstra算法用来寻找图形中节点之间的最短路径。在Dijkstra算法中，需要计算每一个节点距离起点的总移动代价。同时，还需要一个优先队列结构。对于所有待遍历的节点，放入优先队列中会按照代价进行排序。在算法运行的过程中，每次都从优先队列中选出代价最小的作为下一个遍历的节点。直到到达终点为止。
$f_{(n)} = g_{(n)}$
其中：
$f_{(n)}$ 是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
$g_{(n)}$ 是节点n距离起点的代价。

2.2、A*算法

A*算法原理是从起始节点开始，通过启发函数搜索并选择周围最优节点作为下一个扩展点，不断重复这个操作，直到到达目标点，最终从目标点原路返回到起始点，生成最终路径。
$f_{(n)} = g_{(n)} + h_{(n)}$
其中：
$f_{(n)}$ 是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
$g_{(n)}$ 是节点n距离起点的代价。
$h_{(n)}$ 是节点n距离终点的预计代价，这也就是A*算法的启发函数。

2.3、启发函数

当启发函数 $h_{(n)}$ 始终为0，则将由 $g_{(n)}$ 决定节点的优先级，此时算法就退化成了Dijkstra算法。
如果 $h_{(n)}$ 始终小于等于节点n到终点的代价，则A*算法保证一定能够找到最短路径。但是当h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。
如果 $h_{(n)}$ 完全等于节点n到终点的代价，则A*算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。
如果 $h_{(n)}$ 的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径，不过此时会很快。
在另外一个极端情况下，如果 $h_{(n)}$ 相较于 $g_{(n)}$ 大很多，则此时只有 $h_{(n)}$ 产生效果，这也就变成了最佳优先搜索。

对于网格形式的图，有以下这些启发函数可以使用：

如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。
如果图形中允许朝八个方向移动，则可以使用对角距离。
如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。

2.4、伪代码

* 初始化open_set和close_set；
* 将起点加入open_set中，并设置优先级为0（优先级最高）；
* 如果open_set不为空，则从open_set中选取优先级最高的节点n：
    * 如果节点n为终点，则：
        * 从终点开始逐步追踪parent节点，一直达到起点；
        * 返回找到的结果路径，算法结束；
    * 如果节点n不是终点，则：
        * 将节点n从open_set中删除，并加入close_set中；
        * 遍历节点n所有的邻近节点：
            * 如果邻近节点m在close_set中，则：
                * 跳过，选取下一个邻近节点
            * 如果邻近节点m也不在open_set中，则：
                * 设置节点m的parent为节点n
                * 计算节点m的优先级
                * 将节点m加入open_set中

其中open_set和close_set是用来存储待遍历的节点，与已经遍历过的节点

以上理论参考参考

3、C++实现

些变量设置需要根据情况进行修改,不能直接跑通

#include <assert.h>
#include <float.h>
#include <zmq.h>

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <iostream>
#include <memory>
#include <queue>
#include <unordered_map>
#include <vector>
using namespace std;
// A*路径搜索节点
struct Node
{
  int x, y;
  double cost, heuristic;
  float wx, wy;
  shared_ptr<Node> parent;

  Node(int x_, int y_, float wx_, float wy_, double cost_, double heuristic_,
       shared_ptr<Node> parent_ = nullptr)
      : x(x_),
        y(y_),
        wx(wx_),
        wy(wy_),
        cost(cost_),
        heuristic(heuristic_),
        parent(parent_)
  {
  }
  double f() const { return cost + heuristic; }
  bool operator==(const Node& other) const
  {
    return x == other.x && y == other.y;
  }
};

struct CompareNodes
{
  bool operator()(const shared_ptr<Node>& a, const shared_ptr<Node>& b) const
  {
    return a->f() > b->f();
  }
};

//用来判断网格对应的行列对应位置是否被占据
bool isValid(int x, int y)
{
  int map_width, map_height;                //网格地图的长宽
  std::vector<std::vector<float>> gridMap;  //网格地图
  if (x >= 0 && x < map_width && y >= 0 && y < map_height)
  {
    if (gridMap[y][x] != 1.0)
    {
      return true;
    }
  }
  return false;
}
//获取行列对应的真实世界坐标/可有可无
void getTrueXY(int x, int y, float& truex, float& truey)
{
  float grid_size = 0.04;  // 40cm
  float center_offset_x = 0.1;
  float center_offset_y = 0.2;
  float robotx = 0;
  float roboty = 0;
  float yaw = 0.2;  //弧度
  // 计算相对于中心点的偏移
  float rel_x = (x * grid_size) - center_offset_x;
  float rel_y = (y * grid_size) - center_offset_y;
  // 计算绝对坐标mapStartMessage
  truex = robotx + rel_x * cos(yaw) - rel_y * sin(yaw);
  truey = roboty + rel_x * sin(yaw) + rel_y * cos(yaw);
}

//计算欧几里得距离
double euclideanDistance(int x1, int y1, int x2, int y2)
{
  int dx_cg = x1 - x2;
  int dy_cy = y1 - y2;
  return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));
}
void getNeighbors(std::vector<Node>& neighbors, const Node& current,
                  const Node& goal)
{
  //代价地图需要自己修改
  std::vector<std::vector<float>> costMap;
  neighbors.clear();
  //八个方向搜索
  vector<pair<int, int>> directions = {{1, 0}, {-1, 0}, {0, -1},  {0, 1},
                                       {1, 1}, {1, -1}, {-1, -1}, {-1, 1}};
  // vector<pair<int, int>> directions = {{0, 1}, {1, 0}, {-1, 0}, {0, -1}};
  for (const auto& dir : directions)
  {
    int x = current.x + dir.first;
    int y = current.y + dir.second;
    if (isValid(x, y))
    {
      float truex, truey;
      getTrueXY(x, y, truex, truey);
      double cost = current.cost + 1;
   
      double heuristic =
          20.01 * euclideanDistance(x, y, goal.x, goal.y) + costMap[y][x];
      neighbors.emplace_back(x, y, truex, truey, cost, heuristic,
                             make_shared<Node>(current));
    }
  }
}
//到达目标点后，获取从起点到目标点的路径
void reconstructPath(shared_ptr<Node> current,
                     std::vector<std::vector<float>>& path_)
{
  //以下五个变量是在机器人坐标系下生成网格地图时对应的参数，包括网格大小，机器人相对网格地图的偏移量（控制机器人在网管中心或者在网格一头的中心或者在网管00位置）
  //创建网格地图时机器人在世界坐标系下的位置以及偏行角
  //其作用把网管的行列坐标转换到世界坐标系中
  float grid_size = 0.04;  // 40cm
  float center_offset_x = 0.1;
  float center_offset_y = 0.2;
  float robotx = 0;
  float roboty = 0;
  float yaw = 0.2;  //弧度
  vector<Node> path;
  while (current != nullptr)
  {
    path.push_back(*current);
    current = current->parent;
  }

  reverse(path.begin(), path.end());
  std::cout << "path start: ";
  for (const auto& pa : path)
  {
    // 计算相对于中心点的偏移
    float rel_x = (pa.x * grid_size) - center_offset_x;
    float rel_y = (pa.y * grid_size) - center_offset_y;
    // 计算绝对坐标mapStartMessage
    float x_pos = robotx + rel_x * cos(yaw) - rel_y * sin(yaw);
    float y_pos = roboty + rel_x * sin(yaw) + rel_y * cos(yaw);
    path_.push_back(std::vector<float>{x_pos, y_pos});
    std::cout << "(" << pa.x << " , " << pa.y << " , " << x_pos << " , "
              << y_pos << " : " << pa.wx << " , " << pa.wy << ") -->";
  }
  std::cout << std::endl;
}

bool aStar(std::vector<std::vector<float>>& path, const Node& start,
           const Node& goal, int map_width)
{
  path.clear();
  priority_queue<shared_ptr<Node>, vector<shared_ptr<Node>>, CompareNodes>
      open_list;
  unordered_map<int, shared_ptr<Node>> closed_list;
  shared_ptr<Node> start_ptr = make_shared<Node>(start);
  open_list.push(start_ptr);

  while (!open_list.empty())
  {
    shared_ptr<Node> current = open_list.top();
    open_list.pop();
    if ((current->x == goal.x && current->y == goal.y))
    {
      reconstructPath(current, path);
      return true;
    }
    int current_key = current->y * map_width + current->x;
    if (closed_list.count(current_key) == 0 ||
        current->f() < closed_list[current_key]->f())
    {
      closed_list[current_key] = current;
      std::vector<Node> neighbors;
      getNeighbors(neighbors, *current, goal);
      for (const Node& neighbor : neighbors)
      {
        shared_ptr<Node> neighbor_ptr = make_shared<Node>(neighbor);
        int neighbor_key = neighbor.y * map_width + neighbor.x;
        if (closed_list.count(neighbor_key) == 0 ||
            neighbor.f() < closed_list[neighbor_key]->f())
        {
          open_list.push(neighbor_ptr);
        }
      }
    }
  }
  return false;
}

int main(int argc, char const* argv[])
{
  //   Node start(starmapx, starmapy, robotPos[0], robotPos[1], 0.0, 0.0,
  //   nullptr); Node goal(goalmapx, goalmapy, goalPose[0], goalPose[1], 0.0,
  //   0.0, nullptr); return aStar(path, start, goal);
  return 0;
}