A星(A* or A star)演算法C++實現及opencv視覺化
阿新 • • 發佈：2019-02-02
A*演算法，具體原理可參看已有的部落格，下面是我覺得比較好的幾個。
自己在github上找到了一個比較簡單的用C++實現的版本(點選開啟連結)，自己在此基礎上添加了opencv繪製簡單圖塊，將結果可視化了，如下圖。其中，紅色為障礙塊，白色綠邊為自由空間，藍色為起始點，黑色為目標點，規劃的路徑用黃色塊表示，程式設定可斜對角穿行。
#include <iostream> #include <stdlib.h> #include <algorithm> #include <opencv2/opencv.hpp> using namespace std; using namespace cv; class CPoint { public: CPoint(int x,int y):X(x),Y(y),G(0),H(0),F(0),m_parentPoint(NULL){ }; ~CPoint(); int X,Y,G,H,F; CPoint * m_parentPoint; void CalF(){ F=G+H; }; }; class CAStar { private: int m_array[12][12]; static const int STEP = 10; static const int OBLIQUE = 14; typedef std::vector<CPoint*> POINTVEC; POINTVEC m_openVec; POINTVEC m_closeVec; public: CAStar(int array[][12]) { for (int i=0;i<12;i++) for(int j=0;j<12;j++) m_array[i][j]=array[i][j]; } CPoint* GetMinFPoint() { int idx=0,valueF=-9999; for(int i=0; i < m_openVec.size(); ++i) { if(m_openVec[i]->F < valueF) { valueF = m_openVec[i]->F; idx = i; } } return m_openVec[idx]; } bool RemoveFromOpenVec(CPoint* point) { for(POINTVEC::iterator it = m_openVec.begin(); it != m_openVec.end(); ++it) { if((*it)->X == point->X && (*it)->Y == point->Y) { m_openVec.erase(it); return true; } } return false; } bool canReach(int x, int y) { return 0 == m_array[x][y]; } bool IsAccessiblePoint(CPoint* point, int x, int y, bool isIgnoreCorner) { if(!canReach(x, y) || isInCloseVec(x, y)) return false; else { //可到達的點 if(abs(x - point->X) + abs(y - point->Y) == 1) // 左右上下點 return true; else { if(canReach(abs(x - 1), y) && canReach(x, abs(y - 1))) // 對角點 return true; else return isIgnoreCorner; //牆的邊角 } } } std::vector<CPoint*> GetAdjacentPoints(CPoint* point, bool isIgnoreCorner) { POINTVEC adjacentPoints; for(int x = point->X-1; x <= point->X+1; ++x) for(int y = point->Y-1; y <= point->Y+1; ++y) { if(IsAccessiblePoint(point, x, y, isIgnoreCorner)) { CPoint* tmpPoint = new CPoint(x, y); adjacentPoints.push_back(tmpPoint); } } return adjacentPoints; } bool isInOpenVec(int x, int y) { for(POINTVEC::iterator it = m_openVec.begin(); it != m_openVec.end(); ++it) { if((*it)->X == x && (*it)->Y == y) return true; } return false; } bool isInCloseVec(int x, int y) { for(POINTVEC::iterator it = m_closeVec.begin(); it != m_closeVec.end(); ++it) { if((*it)->X == x && (*it)->Y == y) return true; } return false; } void RefreshPoint(CPoint* tmpStart, CPoint* point) { int valueG = CalcG(tmpStart, point); if(valueG < point->G) { point->m_parentPoint = tmpStart; point->G = valueG; point->CalF(); } } void NotFoundPoint(CPoint* tmpStart, CPoint* end, CPoint* point) { point->m_parentPoint = tmpStart; point->G = CalcG(tmpStart, point); point->G = CalcH(end, point); point->CalF(); m_openVec.push_back(point); } int CalcG(CPoint* start, CPoint* point) { int G = (abs(point->X - start->X) + abs(point->Y - start->Y)) == 2 ? STEP : OBLIQUE; int parentG = point->m_parentPoint != NULL ? point->m_parentPoint->G : 0; return G + parentG; } int CalcH(CPoint* end, CPoint* point) { int step = abs(point->X - end->X) + abs(point->Y - end->Y); return STEP * step; } // 搜尋路徑 CPoint* FindPath(CPoint* start, CPoint* end, bool isIgnoreCorner) { m_openVec.push_back(start); while(0 != m_openVec.size()) { CPoint* tmpStart = GetMinFPoint(); // 獲取F值最小的點 RemoveFromOpenVec(tmpStart); m_closeVec.push_back(tmpStart); POINTVEC adjacentPoints = GetAdjacentPoints(tmpStart, isIgnoreCorner); for(POINTVEC::iterator it=adjacentPoints.begin(); it != adjacentPoints.end(); ++it) { CPoint* point = *it; if(isInOpenVec(point->X, point->Y)) // 在開啟列表 RefreshPoint(tmpStart, point); //else if(inCloseVec(point)) //{ // 檢查節點的g值，如果新計算得到的路徑開銷比該g值低，那麼要重新開啟該節點（即重新放入OPEN集） //} else NotFoundPoint(tmpStart, end, point); } if(isInOpenVec(end->X, end->Y)) // 目標點已經在開啟列表中 { for(int i=0; i < m_openVec.size(); ++i) { if(end->X == m_openVec[i]->X && end->Y == m_openVec[i]->Y) return m_openVec[i]; } } } return end; } }; int main() { int start_point_x=1; int start_point_y=1; int goal_point_x=4; int goal_point_y=3; int array[12][12] = { // 0 1 2 3 4 5 6 7 8 9 10 11 { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},// 0 { 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},// 1 { 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},// 2 { 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},// 3 { 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},// 4 { 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},// 5 { 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},// 6 { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1} // 7 }; CAStar* pAStar = new CAStar(array); if(array[start_point_x][start_point_y]||array[goal_point_x][goal_point_y]) { cout<<"start point or goal point set error!!!"<<endl; return 0; } CPoint* start = new CPoint(start_point_x, start_point_y); CPoint* end = new CPoint(goal_point_x, goal_point_y); CPoint* point = pAStar->FindPath(start, end, false); Rect rect; Point left_up,right_bottom; Mat img(600,600,CV_8UC3,Scalar(255,255,255)); namedWindow("Test"); while(point != NULL) { left_up.x = point->Y*50; //儲存陣列的列(point->Y)對應矩形的x軸，一個格大小50畫素 left_up.y = point->X*50; right_bottom.x = left_up.x+50; right_bottom.y = left_up.y+50; rectangle(img,left_up,right_bottom,Scalar(0,255,255),CV_FILLED,8,0);//path yellow(full) std::cout << "(" << point->X << "," << point->Y << ");" << std::endl; point = point->m_parentPoint; } for(int i=0;i<8;i++) { for(int j=0;j<12;j++) { left_up.x = j*50; //儲存陣列的列(j)對應矩形的x軸 left_up.y = i*50; right_bottom.x = left_up.x+50; right_bottom.y = left_up.y+50; if(array[i][j]) { rectangle(img,left_up,right_bottom,Scalar(0,0,255),CV_FILLED,8,0);//obstacles red } else { if(i==start_point_x&&j==start_point_y) rectangle(img,left_up,right_bottom,Scalar(255,0,0),CV_FILLED,8,0);//start point blue(full) else if(i==goal_point_x&&j==goal_point_y) rectangle(img,left_up,right_bottom,Scalar(0,0,0),CV_FILLED,8,0);//goal point black(full) else rectangle(img,left_up,right_bottom,Scalar(0,255,0),2,8,0);//free white content,green edge } } } imshow("Test",img); //視窗中顯示影象 waitKey(0); return 0; }