路径规划-A*（C++实现）_c++实现a*算法

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中
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

其中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;
}


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200