a star路径规划

created by Dejavu

这里写的c++代码是对process大神的代码进行移植的
大神用JavaScript写的a_star视频,用到p5.js库
[完结]


a_star 简介

  • a star(a*) 是一种启发式路径规划的算法,该算法得到的路径不一定是全局最优的,但是因为路径规划速度较快所以被广泛使用。

  • A算法访问的节点比较少,因此可以缩短搜索时间。他的算法思想是:
    最终路径长度f = 起点到该点的已知长度h + 该点到终点的估计长度g。

      O表(open):
             待处理的节点表。
      C表(close):
             已处理过的节点表。
    
  • 伪代码

        function A*(start, goal)
            // The set of nodes already evaluated
            closedSet := {}

            // The set of currently discovered nodes that are not evaluated yet.
            // Initially, only the start node is known.
            openSet := {start}

            // For each node, which node it can most efficiently be reached from.
            // If a node can be reached from many nodes, cameFrom will eventually contain the
            // most efficient previous step.
            cameFrom := the empty map

            // For each node, the cost of getting from the start node to that node.
            gScore := map with default value of Infinity

            // The cost of going from start to start is zero.
            gScore[start] := 0

            // For each node, the total cost of getting from the start node to the goal
            // by passing by that node. That value is partly known, partly heuristic.
            fScore := map with default value of Infinity

            // For the first node, that value is completely heuristic.
            fScore[start] := heuristic_cost_estimate(start, goal)

            while openSet is not empty
                current := the node in openSet having the lowest fScore[] value
                if current = goal
                    return reconstruct_path(cameFrom, current)

                openSet.Remove(current)
                closedSet.Add(current)

                for each neighbor of current
                    if neighbor in closedSet
                        continue        // Ignore the neighbor which is already evaluated.

                    if neighbor not in openSet  // Discover a new node
                        openSet.Add(neighbor)

                    // The distance from start to a neighbor
                    tentative_gScore := gScore[current] + dist_between(current, neighbor)
                    if tentative_gScore >= gScore[neighbor]
                        continue        // This is not a better path.

                    // This path is the best until now. Record it!
                    cameFrom[neighbor] := current
                    gScore[neighbor] := tentative_gScore
                    fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)

            return failure

        function reconstruct_path(cameFrom, current)
            total_path := [current]
            while current in cameFrom.Keys:
                current := cameFrom[current]
                total_path.append(current)
            return total_path

c++ 实现

需要opencv库,如果自己添加opencv内的类型到该文件也可以不用配置opencv

  • a_star.h 存放数据结构

          #ifndef _a_star_h
          #define _a_star_h
    
          #include 
          #include 
          using namespace std;
          using namespace cv;
    
          #define N 100
          #define M 100
          #define map_info(v) obj.info.map[v.x][v.y]
          #define map_center(v) obj.info.map[v.x][v.y].center
          #define isNeighborExist (current.neighbors[i].x>=0 && current.neighbors[i].y>=0)
          #define copy(v1,v2) v1.assign(v2.begin(), v2.end())
          #define add(v1,v2) vector::iterator it;for(it = v2.begin();it!=v2.end();++it) v1.push_back(*it)
          #define add_o(v1,v2) vector::iterator it_o;for(it_o = v2.begin();it_o!=v2.end();++it_o) v1.push_back(*it_o)
          #define COLS 1024
          #define ROWS 720
          #define DUBUG false
          class Pt {
          public:
              Point pt;
              int r;
          };
    
          class  Info {
          public:
              bool isWall;
              Point index;
              Point pt;
              Point center;
              double F;
              double G;
              double H;
              Point neighbors[8];
              int previous;
              Point previous_pt;
              void init() {
                  F = 0;
                  G = 0;
                  H = 0;
                  previous = -1;
                  previous_pt = Point(-1,-1);
                  for (int i = 0;i < 8;i++) neighbors[i] = Point(-1, -1);
              }
              void addNeighbors() {
                  int x = index.x, y = index.y;
                  if (x > 0 && y > 0)
                      neighbors[0] = Point((x - 1), (y - 1));
                  if (y > 0)
                      neighbors[1] = Point(x, (y - 1));
                  if (x < COLS - 1 && y > 0)
                      neighbors[2] = Point((x + 1), (y - 1));
                  if (x < COLS - 1)
                      neighbors[3] = Point((x + 1), y);
                  if (x < COLS - 1 && y < ROWS - 1)
                      neighbors[4] = Point((x + 1), (y + 1));
                  if (y < ROWS - 1)
                      neighbors[5] = Point(x, (y + 1));
                  if (x > 0 && y < ROWS - 1)
                      neighbors[6] = Point((x - 1), (y + 1));
                  if (x > 0)
                      neighbors[7] = Point((x - 1), y);
              }
              int sq() { return index.y*COLS + index.x; }
              void show(Mat &src, Scalar rectColor) {
                  circle(src,center,25,Scalar(100,200,0),-1);
              }
          };
    
          class GridMapInfo {
          public:
              Mat obj_img;
              Info map[N][M];
              Point target, dest;
          };
    
          class Obj {
          public:
              Mat obj_src;
              Pt target, dest;
              GridMapInfo info;
          };
    
          class Params {
          public:
              Point start,end;
              vector path_pt;
              vector target_obj;
              Mat take_look;
          };
    
          class Select_path {
          public:
              bool direct;
              double F;
              vector path_obj;
              vector path;
          };
    
          double heuristic(Point spot_pt, Point end_pt);
          double a_star(Obj obj,Params ¶ms);
    
          #endif
    
  • a_star.cpp

      #include "a_star.h"
    
      double heuristic(Point spot_pt, Point end_pt) {
          double dx = spot_pt.x - end_pt.x;
          double dy = spot_pt.y - end_pt.y;
          return sqrt(dx*dx*1.0 + dy*dy*1.0);
      }
      bool includes(list _dataSet, Info *target) {
          list::iterator it = _dataSet.begin();
          for (int i = 0; i<_dataSet.size(); i++) {
              if (it->sq() == target->sq()) return true;
              else ++it;
          }
          return false;
      }
      bool includes(Info _dataSet[],int len,Info* target) {
          for (int i = 0;i < len;i++) if (_dataSet[i].sq() == target->sq()) return true;
          return false;
      }
      void removeData(list &_dataSet, Info current) {
          list::iterator it = _dataSet.begin();
          for (int i = 0; i<_dataSet.size(); i++) {
              if (it->sq() == current.sq()) {
                  it = _dataSet.erase(it);
                  break;
              }
              else ++it;
          }
      }
    
      double a_star(Obj obj,Params ¶ms) {
          Point start = params.start, dest = params.end;
          Mat show = params.take_look.clone();
          list openSet;
          list path;
          list::iterator it;
          int closed_len(0);
          Info closedSet[N*M];
          Info current = map_info(start);
          Info begin = current;
          Info end = map_info(dest);
          int winner(0);
          openSet.push_back(current);
    
          params.path_pt.clear();
          params.target_obj.clear();
    
          while (openSet.size() > 0) {
              it = openSet.begin();
              if (winner == openSet.size()) winner--;
              advance(it, winner);
              current = *it;
              double minF = it->F;
    
              int index(0);
              for (it = openSet.begin(); it != openSet.end(); ++it, index++) {
                  double currentF = it->F;
                  if (currentF < minF) {
                      winner = index;
                      minF = currentF;
                      current = *it;
                  }
              }
    
              if (current.sq() == end.sq()) {
                  reverse(params.path_pt.begin(),params.path_pt.end());
                  reverse(params.target_obj.begin(),params.target_obj.end());
                  return current.F;
              }
              params.take_look = show.clone();
    
              removeData(openSet, current);
              closedSet[closed_len++] = current;
    
              int tmpG(0);
              for (int i = 0; i < 8; i++) {
                  if (!isNeighborExist) continue;
                  Info *neighbor = &obj.info.map[current.neighbors[i].x][current.neighbors[i].y];
                  if (!includes(closedSet, closed_len, neighbor) && !neighbor->isWall) {
                      tmpG = current.G + 1;
                      if (includes(openSet, neighbor)) {
                          neighbor->G = tmpG;
                          continue;
                      }
                      neighbor->G = tmpG;
                      neighbor->H = heuristic(neighbor->center, end.center);
                      neighbor->F = neighbor->G + neighbor->H;
                      neighbor->previous = current.sq();
                      neighbor->previous_pt = current.index;
    
                      openSet.push_back(*neighbor);
                  }
              }
    
              path.clear();
              Info tmp = current;
              path.push_back(tmp);
              it = path.begin();
              while(tmp.previous != -1) {
                  tmp = map_info(tmp.previous_pt);
                  path.push_back(tmp);
              }
              params.path_pt.clear();
              params.target_obj.clear();
              params.path_pt.push_back(end.center);
              params.target_obj.push_back(map_info(end.index));
              for( it = path.begin(); it != path.end(); ++it ) {
                  it->show(params.take_look,Scalar(100,200,0));
                  params.path_pt.push_back(it->center);
                  params.target_obj.push_back((*it));
              }
          }
          return 0;
      }
    
a star路径规划_第1张图片
a_star_gen

你可能感兴趣的:(a star路径规划)