使用A*算法规划路径

A*算法

//for A-star algorithm
const int kCost1 = 10; //直移一格消耗
const int kCost2 = 14; //斜移一格消耗

struct AStarPoint
{
int x, y; //点坐标，这里为了方便按照C++的数组来计算，x代表横排，y代表竖列
int F, G, H; //F=G+H
AStarPoint *parent; //parent的坐标，这里没有用指针，从而简化代码
AStarPoint(int _x, int _y) :x(_x), y(_y), F(0), G(0), H(0), parent(NULL)  //变量初始化
{
}
};

class Astar
{
public:
void InitAstar(std::vector<std::vector<int>> &_maze);
std::list<AStarPoint *> GetPath(AStarPoint &startPoint, AStarPoint &endPoint, bool isIgnoreCorner);

private:
AStarPoint *findPath(AStarPoint &startPoint, AStarPoint &endPoint, bool isIgnoreCorner);
std::vector<AStarPoint *> getSurroundPoints(const AStarPoint *point, bool isIgnoreCorner) const;
bool isCanreach(const AStarPoint *point, const AStarPoint *target, bool isIgnoreCorner) const; //判断某点是否可以用于下一步判断
AStarPoint *isInList(const std::list<AStarPoint *> &list, const AStarPoint *point) const; //判断开启/关闭列表中是否包含某点
AStarPoint *getLeastFpoint(); //从开启列表中返回F值最小的节点
//计算FGH值
int calcG(AStarPoint *temp_start, AStarPoint *point);
int calcH(AStarPoint *point, AStarPoint *end);
int calcF(AStarPoint *point);
private:
std::vector<std::vector<int>> maze;
std::list<AStarPoint *> openList;  //开启列表
std::list<AStarPoint *> closeList; //关闭列表
};

void Astar::InitAstar(std::vector<std::vector<int>> &_maze)
{
maze = _maze;
}

int Astar::calcG(AStarPoint *temp_start, AStarPoint *point)
{
int extraG = (abs(point->x - temp_start->x) + abs(point->y - temp_start->y)) == 1 ? kCost1 : kCost2;
int parentG = point->parent == NULL ? 0 : point->parent->G; //如果是初始节点，则其父节点是空
return parentG + extraG;
}

int Astar::calcH(AStarPoint *point, AStarPoint *end)
{
//用简单的欧几里得距离计算H，这个H的计算是关键，还有很多算法，没深入研究^_^
return sqrt((double)(end->x - point->x)*(double)(end->x - point->x) + (double)(end->y - point->y)*(double)(end->y - point->y))*kCost1;
}

int Astar::calcF(AStarPoint *point)
{
return point->G + point->H;
}

AStarPoint *Astar::getLeastFpoint()
{
if (!openList.empty())
{
auto resPoint = openList.front();
for (auto &point : openList)
if (point->F<resPoint->F)
resPoint = point;
return resPoint;
}
return NULL;
}

AStarPoint *Astar::findPath(AStarPoint &startPoint, AStarPoint &endPoint, bool isIgnoreCorner)
{
openList.push_back(new AStarPoint(startPoint.x, startPoint.y)); //置入起点,拷贝开辟一个节点，内外隔离
while (!openList.empty())
{
auto curPoint = getLeastFpoint(); //找到F值最小的点
openList.remove(curPoint); //从开启列表中删除
closeList.push_back(curPoint); //放到关闭列表
//1,找到当前周围八个格中可以通过的格子
auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);
for (auto &target : surroundPoints)
{
//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G H
if (!isInList(openList, target))
{
target->parent = curPoint;

target->G = calcG(curPoint, target);
target->H = calcH(target, &endPoint);
target->F = calcF(target);

openList.push_back(target);
}
//3，对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
else
{
int tempG = calcG(curPoint, target);
if (tempG<target->G)
{
target->parent = curPoint;

target->G = tempG;
target->F = calcF(target);
}
}
AStarPoint *resPoint = isInList(openList, &endPoint);
if (resPoint)
return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
}
}

return NULL;
}

std::list<AStarPoint *> Astar::GetPath(AStarPoint &startPoint, AStarPoint &endPoint, bool isIgnoreCorner)
{
AStarPoint *result = findPath(startPoint, endPoint, isIgnoreCorner);
std::list<AStarPoint *> path;
//返回路径，如果没找到路径，返回空链表
while (result)
{
path.push_front(result);
result = result->parent;
}
return path;
}

AStarPoint *Astar::isInList(const std::list<AStarPoint *> &list, const AStarPoint *point) const
{
//判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标
for (auto p : list)
if (p->x == point->x&&p->y == point->y)
return p;
return NULL;
}

bool Astar::isCanreach(const AStarPoint *point, const AStarPoint *target, bool isIgnoreCorner) const
{
if (target->x<0 || target->x>maze.size() - 1
|| target->y<0 && target->y>maze[0].size() - 1
|| maze[target->x][target->y] == 1
|| target->x == point->x&&target->y == point->y
|| isInList(closeList, target)) //如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回false
return false;
else
{
if (abs(point->x - target->x) + abs(point->y - target->y) == 1) //非斜角可以
return true;
else
{
//斜对角要判断是否绊住
if (maze[point->x][target->y] == 0 && maze[target->x][point->y] == 0)
return true;
else
return isIgnoreCorner;
}
}
}

std::vector<AStarPoint *> Astar::getSurroundPoints(const AStarPoint *point, bool isIgnoreCorner) const
{
std::vector<AStarPoint *> surroundPoints;

for (int x = point->x - 1; x <= point->x + 1; x++)
for (int y = point->y - 1; y <= point->y + 1; y++)
if (isCanreach(point, new AStarPoint(x, y), isIgnoreCorner))
surroundPoints.push_back(new AStarPoint(x, y));

return surroundPoints;
}

void RemoveSmallRegion(Mat& Src, Mat& Dst, int AreaLimit, int CheckMode, int NeihborMode)
{
for (int i = 0; i < Src.rows; ++i)
{
uchar* iData = Src.ptr<uchar>(i);
for (int j = 0; j < Src.cols; ++j)
{
if (iData[j] == 0 || iData[j] == 255) continue;
else if (iData[j] < 10)
{
iData[j] = 0;
//cout<<'#';
}
else if (iData[j] > 10)
{
iData[j] = 255;
//cout<<'!';
}
}
}

int RemoveCount = 0;       //记录除去的个数
//记录每个像素点检验状态的标签，0代表未检查，1代表正在检查,2代表检查不合格（需要反转颜色），3代表检查合格或不需检查
Mat Pointlabel = Mat::zeros(Src.size(), CV_8UC1);

if (CheckMode == 1)
{
for (int i = 0; i < Src.rows; ++i)
{
uchar* iData = Src.ptr<uchar>(i);
uchar* iLabel = Pointlabel.ptr<uchar>(i);
for (int j = 0; j < Src.cols; ++j)
{
if (iData[j] < 10)
{
iLabel[j] = 3;
}
}
}
}
else
{
for (int i = 0; i < Src.rows; ++i)
{
uchar* iData = Src.ptr<uchar>(i);
uchar* iLabel = Pointlabel.ptr<uchar>(i);
for (int j = 0; j < Src.cols; ++j)
{
if (iData[j] > 10)
{
iLabel[j] = 3;
}
}
}
}

vector<Point2i> NeihborPos;  //记录邻域点位置
NeihborPos.push_back(Point2i(-1, 0));
NeihborPos.push_back(Point2i(1, 0));
NeihborPos.push_back(Point2i(0, -1));
NeihborPos.push_back(Point2i(0, 1));
if (NeihborMode == 1)
{
NeihborPos.push_back(Point2i(-1, -1));
NeihborPos.push_back(Point2i(-1, 1));
NeihborPos.push_back(Point2i(1, -1));
NeihborPos.push_back(Point2i(1, 1));
}
else;
int NeihborCount = 4 + 4 * NeihborMode;
int CurrX = 0, CurrY = 0;
//开始检测
for (int i = 0; i < Src.rows; ++i)
{
uchar* iLabel = Pointlabel.ptr<uchar>(i);
for (int j = 0; j < Src.cols; ++j)
{
if (iLabel[j] == 0)
{
//********开始该点处的检查**********
vector<Point2i> GrowBuffer;                                      //堆栈，用于存储生长点
GrowBuffer.push_back(Point2i(j, i));
Pointlabel.at<uchar>(i, j) = 1;
int CheckResult = 0;                                               //用于判断结果（是否超出大小），0为未超出，1为超出

for (int z = 0; z<GrowBuffer.size(); z++)
{

for (int q = 0; q<NeihborCount; q++)                                      //检查四个邻域点
{
CurrX = GrowBuffer.at(z).x + NeihborPos.at(q).x;
CurrY = GrowBuffer.at(z).y + NeihborPos.at(q).y;
if (CurrX >= 0 && CurrX<Src.cols&&CurrY >= 0 && CurrY<Src.rows)  //防止越界
{
if (Pointlabel.at<uchar>(CurrY, CurrX) == 0)
{
GrowBuffer.push_back(Point2i(CurrX, CurrY));  //邻域点加入buffer
Pointlabel.at<uchar>(CurrY, CurrX) = 1;           //更新邻域点的检查标签，避免重复检查
}
}
}

}
if (GrowBuffer.size()>AreaLimit) CheckResult = 2;                 //判断结果（是否超出限定的大小），1为未超出，2为超出
else { CheckResult = 1;   RemoveCount++; }
for (int z = 0; z<GrowBuffer.size(); z++)                         //更新Label记录
{
CurrX = GrowBuffer.at(z).x;
CurrY = GrowBuffer.at(z).y;
Pointlabel.at<uchar>(CurrY, CurrX) += CheckResult;
}
//********结束该点处的检查**********

}
}
}

CheckMode = 255 * (1 - CheckMode);
//开始反转面积过小的区域
for (int i = 0; i < Src.rows; ++i)
{
uchar* iData = Src.ptr<uchar>(i);
uchar* iDstData = Dst.ptr<uchar>(i);
uchar* iLabel = Pointlabel.ptr<uchar>(i);
for (int j = 0; j < Src.cols; ++j)
{
if (iLabel[j] == 2)
{
iDstData[j] = CheckMode;
}
else if (iLabel[j] == 3)
{
iDstData[j] = iData[j];
}
}
}

}

// 输入目标点
if(IsGoToTarget && IsManualTarget && autodrive)
{
global_path.clear();

while (!IsGridMapSaved)
{
ArUtil::sleep(5);
}
IsMapLoading = true;
Mat src = imread(tmpGridMapFile, 0);
xmin = x_min, xmax = x_max, ymin = y_min, ymax = y_max;
IsMapLoading = false;
int width = src.cols, height = src.rows;

//*************************
// walls extraction
//*************************
Mat black;
threshold(src, black, 30, 255, CV_THRESH_BINARY_INV);

Mat dilate_black;
dilate(black, dilate_black, element7);

Mat erode_black;
erode(dilate_black, erode_black, element6);

Mat wall = Mat::zeros(erode_black.size(), CV_8UC1);
RemoveSmallRegion(erode_black, wall, 30, 1, 1);
//namedWindow("wall", 1);
//imshow("wall", wall);

//*************************
// A-Star to get path
//*************************
vector<vector<int>> maze;
width = dilate_black.cols, height = dilate_black.rows;
for (int i = 0; i < width; i += 2)
{
vector<int> tmp;
for (int j = 0; j < height; j += 2)
{
if (dilate_black.at<uchar>(j, i) >= 250)
tmp.push_back(1);
else
tmp.push_back(0);
}
maze.push_back(vector<int>(tmp));
}
Astar astar;
astar.InitAstar(maze);
AStarPoint start((curPose3D.x() - xmin) * 10, (-curPose3D.y() - ymin) * 10);
AStarPoint end((m_targetPoint.x() - xmin) * 10, (-m_targetPoint.y() - ymin) * 10);
std::list<AStarPoint *> path = astar.GetPath(start, end, false);

int path_size = path.size();
if (path_size == 0)
{
sendMessage("cannot reach target!");
IsGoToTarget = false;
continue;
}
else
{
sendMessage("reach goal!");
int num = 1;
for (std::list<AStarPoint *>::iterator it = path.begin(); it != path.end(); it++)
{
AStarPoint * pt = *it;
if (num % 5 == 0)
{
CPoint2D tmppt = CPoint2D(pt->x * 0.1 + xmin, pt->y * -0.1 - ymin);
global_path.push_back(CPoint2D(tmppt));
num = 1;
}
else
{
num++;
}
circle(wall, Point(pt->x * 2, pt->y * 2), 1, Scalar(128));
}
}
sendMessage(QString("target: x: %1, y:%2").arg(m_targetPoint.x()).arg(m_targetPoint.y()));
namedWindow("wall", 1);
imshow("wall", wall);

IsManualTarget = false;
}