A*算法笔记

A*算法与A算法

1. 学习资料来源

文章中关于证明的内容与截图都来自与这个网址，自己看了一遍就整理下来了，大家可以自己到原网址看一下，内容比我摘录的丰富。后面的仿真实验是我自己写的代码，仅供参考哦。

2. 启发性信息

​ 启发性信息是指那种与具体问题求解过程相关的，并可直到搜索过程超最希望方向前景的控制信息。

​ 启发信息的启发能力越强，扩展的无用节点越少。包括一下3种：

1. 有效的帮助确定扩展节点的信息。
2. 有效的帮助决定那些后继节点应被生成的的信息
3. 能决定扩展一个节点时那些节点应从搜索树上删除的信息

3. 估价函数

​ 用来估计节点重要性，定义从初始节点$S_0$出发，约束经过节点$n$到达目标节点$S_g$的所有路径中最小路径代价的估计值。一般形式：
$$f(n)=g(n)+h(n)$$
其中，$g(n)$是从初始节点$S_0$到节点$n$的实际代价；$h(n)$是从节点$n$到目标节点$S_g$的最优路径的估计值

4. A算法

4.1 概念

​ 在状态空间搜索中，如果每一步都利用估价函数$f(n)=g(n)+h(n)$对OPEN列表中的节点进行排序，则称为A算法，这是一种启发式搜索算法。

4.2 类型

1. 全局择优：从OPNE列表的所有节点中选择一个估价函数最小的进行扩展。
2. 局部择优：经从刚生成的子节点中选择一个估计函数最小的进行扩展。

4.3 全局择优搜索A算法描述：

1. 把初始节点$S_0$放入OPEN表中，$f(s_0)=g(S_0)+h(S_0)$
2. 如果OPEN表为空，则问题无解，失败退出；
3. 把OPEN表的第一个节点取出放入CLOSED表中，并标记该节点为$n$;
4. 考察节点$n$是否为目标节点。若是，则找到了问题的解，成功退出；
5. 若节点$n$不可扩展，则转到2步；
6. 扩展节点$n$，生成子节点$n_i(i=1,2,3,4...)$，计算每个子节点的估价值$f(n_i)$，并为每个子节点设置指向父节点的指针，然后将这些子节点放入到OPEN表中；
7. 根据各节点的估价函数值，对OPEN表中的全部节点按从小到大的顺序重新进行排序；
8. 跳转到2步；

5. A*算法

​ A*算法是对A算法的估价函数$f(n)=g(n)+h(n)$加上某种限制后得到的一种启发式搜索算法

​ 假设$f^{\ast }(n)$是从初始节点$S_0$出发，约束经过节点$n$到达目标节点$S_g$的最小代价，估价函数$f(n)$是对$f^{\ast }(n)$的估计值。记：
$$f^{\ast }(n)=g^{\ast }(n)+h^{\ast }(n)$$
其中，$g^{\ast }(n)$是从$S_0$出发到达$S_g$的最小代价，$h^{\ast }(n)$是$n$是$S_g$的最小代价

5.1 A*算法要满足的约束

1. $g(n)$是对最小代价$g^{\ast }(n)$的估计，且$g(n)>0$;
2. $h(n)$是最小代价$h^{\ast }(n)$的下界，即对任意节点$n$均有$h(n)\le h^{\ast }(n)$.

满足上述两条限制的A算法为A*算法。

然而我们不可能知道最优的$h(n)$，实现的时候都是选择一种$h(n)$，但是这种最有是存在的，亦或者说，给定一个$h(n)$那么求的路径就是在这种情况下的最优。

5.2 可纳性

​ 对任意状态空间图，当从初始节点到目标节点的路径存在时，如果搜索算法总能在优先步骤找到一条从初始节点到目标节点的最佳路径，并在此路径上结束，则称该搜索算法是可采纳的。

​ A算法可纳性证明有三个步骤:
1. 对有限图，A算法一定能够成功结束
2. 对无限图，A算法也能成功结束
3. A算法一定能够结束在最佳路径上

5.3 可纳性证明

定理4.1 对有限图，如果从初始节点$S_0$到目标节点$S_g$有路径存在，则A*算法一定成功结束。

引理4.1 对无限图，如果初始节点$S_0$到目标节点$S_0$有路径存在，则A*算法不终止的话，则从OPEN列表中选出的界定啊必具有任意大$f$值

引理4.2 在A*算法终止前的任意时刻，OPEN列表中总存在 节点$n^{\prime }$，他是从初始节点$S_0$到目标节点的最优路径上的一个节点，且满足$f(n^{\prime })\le f^{\ast }(S_0)$

定理4.2 对无限图，若从初始节点$S_0$到目标节点$S_g$有路径存在，则A*算法必然会结束。因为不结束$f$越来越大。

5.3.1 A*的可纳性证明

定理4.3 A*算法是可采纳的，即若存在初始界定啊$S_0$到目标节点的$S_g$的路径，则A*算法必能结束在最佳路径上

推论4.2 在A*算法中，对任何被扩展的节点$n$，都有$f(n)\le f^{\ast }(S_0)$

5.3.2 A*算法的最优性

​ A*算法的搜索效率很大程度上取决于估价函数$h(n)$。一般来说，在满足$h(n)\le h^{\ast }(n)$的前提下，$h(n)$的值越大越好。$h(n)$的值越大，说明他携带的启发性信息越多，A*算法搜索时扩展的节点就越少，搜索效率就越高。A*算法的这一特性被称为最优性。

最优性的解释：

5.3.3 $h(n)$的单调限制

在A*算法中，每当扩展一个节点$n$时，都需要检查其子节点是否已经在OPEN表或CLOSED表中

1. 对在OPEN中的子节点，需要决定是否调整指向其父节点的指针
2. 对在CLOSED中的节点，需要决定是否调整指向其父节点的指针，以及其子节点的后续节点的父指针。

如果能保证，每次扩展节点的时候就已经找到通往这个节点的最佳路径，就没有必要在做上述检查，为此需要启发函数$f(n)$单调递增。

一般情况下，选择的$h(n)$是单调递减的，比如欧拉距离、曼哈顿距离。

启发函数单调递增的定义：

定理4.5 如果$h$满足单调条件，则当A*算法扩展节点$n$时，该节点已经找到通往他的最佳路径，即$g(n)=g^{\ast }(n)$.

定理4.6 如果$h(n)$满足单调限制，则A*算法扩展的节点序列的$f$值是非递减的，即$f(n_i)\le f(n_{i+1})$

6. A*算法流程

1. 创建OPEN与CLOSED列表
2. 添加初始节点到OPEN中
3. 判断OPEN是否为空，为空则结束搜索，未找到路径，（循环1）
   1. 取出OPEN中$f$最小的节点，记为current_node，将其从OPEN中删除，并添加到CLOSED中
   2. 获取current_node的周围点(子节点)，并遍历（循环2）
      1. 判断周围点是否在CLOSED中，
         1. 在其中则直接continue，跳过该点周围点的后续判断.
         2. 不在其中则继续下面流程
      2. 判断周围点是否在OPEN中:
         1. 在其中则更新$g$为更小的值($g$原本保留了某条路径的$g$，通过与当前路径的$g$的比较获取更短的路径)，并改变父指针指向
         2. 不在其中，则计算$g、h、f$以及父节点指向（不在其中说明该点第一次被访问）。
      3. 判断周围点是否是终点，
         1. 是，则跳出整个循环1。
         2. 否则，继续流程。
4. 返回带有父节点信息的终点。
5. 从终点开始遍历到起点，保存每个点，最终形成路径点集。

7. 代码实现

地图类的实现：mymap类

#ifndef MYMAP_H_
#define MYMAP_H_

#include <vector>

using std::vector;
namespace mymap {
class MyMap {
 public:
  // 构造函数
  MyMap(){};
  MyMap(const int map_xlength, const int map_ylength);
  // 设置平行与X轴的一排障碍物，[x_begin, x_end]
  void SetObsLineX(const int x_begin, const int x_end, const int y);
  // 设置平行与Y轴的一排障碍物，[y_begin, y_end]
  void SetObsLineY(const int y_begin, const int y_end, const int x);
  // 获取(x, y)处的地图状态，1为有障碍物，0为无障碍物
  bool GetMapPointState(const int x, const int y) const;
  // 获取成员变量接口
  vector<vector<int>> map() const { return map_; };
  int map_xlength() const { return map_xlength_; };
  int map_ylength() const { return map_ylength_; };
 private:
  vector<vector<int>> map_;   // 地图矩阵
  int map_xlength_; // 地图列数，长
  int map_ylength_; // 地图行数，宽
  int unfree = 1;   // 地图点状态
  int free = 0;
};
}  // namespace mymap

#endif  // MYMAP_H

#include "mymap.h"

namespace mymap {
// 构造函数，构建指定长宽的地图大小
MyMap::MyMap(const int map_xlength, const int map_ylength)
    : map_xlength_(map_xlength), map_ylength_(map_ylength) {
  map_.resize(map_ylength_);
  for (auto &e : map_) {
    e.resize(map_xlength_);
  }
  int x_scale = map_xlength_ / 5;
  int y_scale = map_ylength_ /5;
  SetObsLineX(0, map_xlength_ - 1, 0);
  SetObsLineX(0, map_xlength_ - 1, map_ylength_ - 1);
  SetObsLineY(0, map_ylength_ - 1, 0);
  SetObsLineY(0, map_ylength_ - 1, map_xlength_ - 1);
  SetObsLineX(x_scale, x_scale * 2, y_scale * 3);
  SetObsLineX(x_scale * 3, x_scale * 4, y_scale * 2);
  SetObsLineX(0, x_scale, y_scale * 2);
  SetObsLineX(x_scale * 4, x_scale * 5, y_scale * 3);
  SetObsLineY(0, y_scale * 3, x_scale * 2);
  SetObsLineY(y_scale * 2, y_scale * 5, x_scale * 3);
}

void MyMap::SetObsLineX(const int x_begin, const int x_end, const int y) {
  for (int i = x_begin; i <= x_end; ++i) {
    map_[i][y] = unfree;
  }
}

void MyMap::SetObsLineY(const int y_begin, const int y_end, const int x) {
  for (int i = y_begin; i <= y_end; ++i) {
    map_[x][i] = unfree;
  }
}

bool MyMap::GetMapPointState(const int x, const int y) const {
  return map_[x][y];
}

}  // namespace mymap

A*算法的代码实现：

#ifndef ASTAR_H_
#define ASTAR_H_

#include <list>
#include <vector>

#include "mymap.h"

struct Point {
  int x_;
  int y_;
  double F_;
  double G_;
  double H_;
  Point* parent_;
  Point(int x, int y)
      : x_(x), y_(y), F_(0.0), G_(0.0), H_(0.0), parent_(nullptr){};
};

class Astar {
 public:
  Astar(){};
  Astar(mymap::MyMap map) : map_(map){};
  // 获取整个路径，也就是搜索完以后的路径点集
  std::list<Point*> get_path(const Point& start_point, const Point& end_point);
  // 获取成员变量
  std::list<Point*> open_list() const { return open_list_; };
  std::list<Point*> close_list() const { return close_list_; };

 private:
  // 从起点开始搜索路径，返回带有父节点信息的终点
  Point* search(const Point& start_point, const Point& end_point);
  // 获取当前点的周围点
  std::list<Point*> get_neighbors(const Point& current_point) const;
  // 获取openlist中f最小的点
  Point* get_minF_point() const;
  // 访问的点在open中
  bool is_in_openlist(const Point& current_point) const;
  // 访问的点在close中
  bool is_in_closelist(const Point& current_point) const;
  // 当前点到下一个点之间是否可以通过
  bool is_collision(const Point* firstPoint, const Point* secondPoint) const;
  // 计算F
  double calculate_F(Point& current_point);
  // 递推G
  double calculate_G(const Point& previous_point, Point& current_point);
  // 计算H
  double calculate_H(Point& current_point, const Point& end_point);

  mymap::MyMap map_;
  std::list<Point*> open_list_;
  std::list<Point*> close_list_;
};

#endif  // ASTAR_H_

#include "astar.h"

#include <cmath>

std::list<Point*> Astar::get_path(const Point& start_point,
                                  const Point& end_point) {
  // 执行搜索，返回带有父节点信息的终点
  Point* end_with_parent = this->search(start_point, end_point);
  std::list<Point*> path;
  // 从终点查找到起点
  while (end_with_parent) {
    path.push_back(end_with_parent);
    end_with_parent = end_with_parent->parent_;
  }
  return path;
}

Point* Astar::search(const Point& start_point, const Point& end_point) {
  // 重新构建新的起始节点与终点，因为传入的是const不能改变内容
  Point* start = new Point(start_point);
  // 起点进入待访问
  open_list_.push_back(start);
  // 开始搜索
  while (!open_list_.empty()) {
    // 获取待访问列表中f最小的点
    Point* current_point = this->get_minF_point();
    // 从待访问中删除，并添加到已访问
    open_list_.remove(current_point);
    close_list_.push_back(current_point);
    // 获取当前点的周围点
    std::list<Point*> neighbors = this->get_neighbors(*current_point);
    // 遍历周围点
    for (auto& neighbor : neighbors) {
      if (this->is_in_closelist(*neighbor)) {
        continue;
      }
      // 当周围点在open列表中，则更新GHF并添加到open列表中
      if (!this->is_in_openlist(*neighbor)) {
        neighbor->parent_ = current_point;
        neighbor->H_ = this->calculate_H(*neighbor, end_point);
        neighbor->G_ = this->calculate_G(*current_point, *neighbor);
        neighbor->F_ = this->calculate_F(*neighbor);
        // 添加到open中
        open_list_.push_back(neighbor);
      } else {
        // 如果已经在open列表中，则计算G值，用最小的G来更新父节点
        if (this->calculate_G(*current_point, *neighbor) < neighbor->G_) {
          neighbor->parent_ = current_point;
          neighbor->G_ = this->calculate_G(*current_point, *neighbor);
          neighbor->F_ = this->calculate_F(*neighbor);
          // 此处不更新H是因为，H只与某点自身与终点有关，而在添加到openl中的时候H就已经计算了
        }
      }
      // 访问的周围点是终点，这句要在neighbor更新并添加到open之后
      if (neighbor->x_ == end_point.x_ && neighbor->y_ == end_point.y_) {
        return neighbor;
      }
    }
  }
  // 查找失败则为空指针
  return nullptr;
}

std::list<Point*> Astar::get_neighbors(const Point& current_point) const {
  std::list<Point*> neighbors;
  int mid_x = current_point.x_;
  int mid_y = current_point.y_;
  // 上下左右四个点
  // // 左边的点
  // if (mid_x - 1 >= 0 and !map_.GetMapPointState(mid_x - 1, mid_y)) {
  //   neighbors.push_back(new Point(mid_x - 1, mid_y));
  // }
  // // 右面的点
  // if (mid_x + 1 <= map_.map_xlength() and
  //     !map_.GetMapPointState(mid_x + 1, mid_y)) {
  //   neighbors.push_back(new Point(mid_x + 1, mid_y));
  // }
  // // 上面的点
  // if (mid_y + 1 <= map_.map_ylength() and
  //     !map_.GetMapPointState(mid_x, mid_y + 1)) {
  //   neighbors.push_back(new Point(mid_x, mid_y + 1));
  // }
  // // 下面的点
  // if (mid_y - 1 >= 0 and !map_.GetMapPointState(mid_x, mid_y - 1)) {
  //   neighbors.push_back(new Point(mid_x, mid_y - 1));
  // }

  // 八个点情况
  // 遍历时点应该在地图范围内，并且中间的那个点，以及对角线路线不能被卡住
  for (int i = mid_x - 1; i <= mid_x + 1; i++) {
    for (int j = mid_y - 1; j <= mid_y + 1; j++) {
      if (i >= 0 and j >= 0 and i <= map_.map_xlength() - 1 and
          j <= map_.map_ylength() - 1 and
          !is_collision(&current_point, new Point(i, j))) {
        neighbors.push_back(new Point(i, j));
      }
    }
  }
  return neighbors;
}

Point* Astar::get_minF_point() const {
  Point* res = open_list_.front();
  // 遍历然后存最小的点
  for (auto& p : open_list_) {
    if (p->F_ < res->F_) {
      res = p;
    }
  }
  return res;
}

bool Astar::is_in_openlist(const Point& current_point) const {
  for (auto p : open_list_) {
    if (p->x_ == current_point.x_ && p->y_ == current_point.y_) {
      return true;
    }
  }
  return false;
}

bool Astar::is_in_closelist(const Point& current_point) const {
  for (auto p : close_list_) {
    if (p->x_ == current_point.x_ && p->y_ == current_point.y_) {
      return true;
    }
  }
  return false;
}

bool Astar::is_collision(const Point* firstPoint,
                         const Point* secondPoint) const {
  //如果访问的两个点本身就是障碍
  if (this->map_.GetMapPointState(firstPoint->x_, firstPoint->y_) or
      this->map_.GetMapPointState(secondPoint->x_, secondPoint->y_)) {
    return true;
  }
  //两个点不是同一个点
  if (firstPoint->x_ != secondPoint->x_ and firstPoint->y_ != secondPoint->y_) {
    //如果两个点是反对角线上，则对角线不能有障碍物
    if (secondPoint->x_ - firstPoint->x_ == firstPoint->y_ - secondPoint->y_) {
      //对角线，左下角点
      Point leftDwon(std::min(firstPoint->x_, secondPoint->x_),
                     std::min(firstPoint->y_, secondPoint->y_));
      //对角线，右上角点
      Point rightUp(std::max(firstPoint->x_, secondPoint->x_),
                    std::max(firstPoint->y_, secondPoint->y_));
      //左下角点与右上角点均不能有障碍物
      return this->map_.GetMapPointState(leftDwon.x_, leftDwon.y_) or
             this->map_.GetMapPointState(rightUp.x_, rightUp.y_);
    } else {
      //当两点在对角线上时，反对角线不能有障碍物
      //左上角
      Point leftUp(std::min(firstPoint->x_, secondPoint->x_),
                   std::max(firstPoint->y_, secondPoint->y_));
      //右下角
      Point rightDwon(std::max(firstPoint->x_, secondPoint->x_),
                      std::min(firstPoint->y_, secondPoint->y_));
      //左上角与右下角均不能有障碍物
      return this->map_.GetMapPointState(leftUp.x_, leftUp.y_) or
             this->map_.GetMapPointState(rightDwon.x_, rightDwon.y_);
    }
  }
  //其他状态表示没有障碍物
  return false;
}

// 计算F=H+G
double Astar::calculate_F(Point& current_point) {
  return current_point.H_ + current_point.G_;
}

// 计算G，欧拉距离，当前点的G = 父节点G + 两点之间的代价
double Astar::calculate_G(const Point& previous_point, Point& current_point) {
  double extra_G =
      std::sqrt(std::pow((current_point.x_ - previous_point.x_), 2) +
                std::pow((current_point.y_ - previous_point.y_), 2));
  return previous_point.G_ + extra_G;
}

// 计算H，欧拉距离
double Astar::calculate_H(Point& current_point, const Point& end_point) {
  return std::sqrt(std::pow((current_point.x_ - end_point.x_), 2) +
                   std::pow((current_point.y_ - end_point.y_), 2));
}

main函数的实现：

#include <vector>

#include "astar.h"
#include "matplotlibcpp.h"		//绘图的开源库
#include "mymap.h"

namespace plt = matplotlibcpp;

int main() {
  mymap::MyMap map(51, 51);
  Astar astar(map);
  Point start_point(5, 5);
  Point end_point(45, 45);
  vector<int> start_and_end_point_x{start_point.x_, end_point.x_};
  vector<int> start_and_end_point_y{start_point.y_, end_point.y_};
  vector<int> path_x;
  vector<int> path_y;
  vector<int> open_list_x;
  vector<int> open_list_y;
  vector<int> close_list_x;
  vector<int> close_list_y;

  std::list<Point*> path = astar.get_path(start_point, end_point);
  std::list<Point*> open_list = astar.open_list();
  std::list<Point*> close_list = astar.close_list();

  for (auto p : path) {
    path_x.push_back(p->x_);
    path_y.push_back(p->y_);
  }

  for (auto p : open_list) {
    open_list_x.push_back(p->x_);
    open_list_y.push_back(p->y_);
  }

  for (auto p : close_list) {
    close_list_x.push_back(p->x_);
    close_list_y.push_back(p->y_);
  }

  std::vector<int> free_x, unfree_x, free_y, unfree_y;
  for (int i = 0; i < map.map_xlength(); ++i) {
    for (int j = 0; j < map.map_ylength(); ++j) {
      if (map.GetMapPointState(i, j) == 1) {
        unfree_x.push_back(i);
        unfree_y.push_back(j);
      } else {
        free_x.push_back(i);
        free_y.push_back(j);
      }
    }
  }
  vector<int> x{10, 20};
  vector<int> y{10, 20};
  plt::plot(unfree_x, unfree_y, "ko");
  plt::plot(open_list_x, open_list_y, "co");
  plt::plot(close_list_x, close_list_y, "yo");
  // plt::plot(free_x, free_y, "wo");
  plt::plot(path_x, path_y, "ro");
  plt::plot(start_and_end_point_x, start_and_end_point_y, "gs");
  plt::axis("equal");
  plt::show();
  return 0;
}

8. 仿真结果

1. 绿色代表起始点与终止点
2. 红色代表路径
3. 黄色代表CLOSDE列表
4. 青色代表OPNE列表
5. 黑色为障碍物
6. 白色为没有遍历区域，且没有障碍物。

9. ROS中实现A*算法

目前未完成