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 <opencv2/opencv.hpp>
        #include <stdio.h>
        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<Point>::iterator it;for(it = v2.begin();it!=v2.end();++it) v1.push_back(*it)
        #define add_o(v1,v2) vector<Info>::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<Point> path_pt;
            vector<Info> target_obj;
            Mat take_look;
        };
  
        class Select_path {
        public:
            bool direct;
            double F;
            vector<Info> path_obj;
            vector<Point> path;
        };
  
        double heuristic(Point spot_pt, Point end_pt);
        double a_star(Obj obj,Params &params);
  
        #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<Info> _dataSet, Info *target) {
        list<Info>::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<Info> &_dataSet, Info current) {
        list<Info>::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 &params) {
        Point start = params.start, dest = params.end;
        Mat show = params.take_look.clone();
        list<Info> openSet;
        list<Info> path;
        list<Info>::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_gen