实战|OpenCV结合A*算法实现简单的运动路径规划

学更好的别人，

做更好的自己。

——《微卡智享》

本文长度为3891字，预计阅读10分钟

写在最前

2020年还真是一个不平凡的一年，因为新冠疫情的影响，第一季度就这么不知不觉的过完了，本来年初自己还定了一个计划《展望|2020立个Flag》，里面有部分可以说不用到年底，现在也可以开始打脸了，比如说本来要说学习小程序的，现在我已经不准备再投入精力学习小程序了，因为找到了新的目标-----学习算法。

这篇文章出来，主要原因是一直在想做个什么东西能把公司的产品有结合的，也没怎么想出来，只是觉得商业的话我们做外围的话是不是可以做一起室内的路径规划，可是WMS仓储的绑定对应货位的一个路径规划，于是就在研究怎么去实现这个，就有了下面这个东西出来，先不多说了，直接看效果。

实现效果

关于A*算法

微卡智享

A*的实现算法方式

https://blog.csdn.net/hitwhylz/article/details/23089415

上面这篇就可以很清楚的讲了A*的实现原理，我这里就不再复制粘粘了，直接去原文看一下即可。

 

#	简单流程
1	将起点加入OpenList（开启列表），计算到终点的F值
2	从OpenList（开启列表）中找到F值最小，也就是离终点最近的点为当前点，从OpenList(开启列表)中删除该点，加入到CloseList（关闭列表）中
3	从当前点中搜索邻近的8个点，排除掉地图上的障碍点后和在CLoseList(关闭列表)中的点，计算出每个点到终点的F,G,H值，并把当前点做为每个点的父节点，加入到OpenList(开启列表)中
4	重新2和3直到OpenList(开启列表）中存在终点，跳出寻找。
5	从终点开始通过找到其父节点，一级一级的列表所有的路径点

 

A*实现代码

AStarCalc.h

#include<opencv2/opencv.hpp>
#include<iostream>
using namespace cv;
using namespace std;


const int kCost1 = 10; //直移一个点消耗G 
const int kCost2 = 14; //斜移一个点消耗G 


struct CalcPt
{
  Point pt; //OpenCV中点坐标 
  int F, G, H; //F=G+H 
  CalcPt* parent; //parent的坐标，这里没有用指针，从而简化代码 
  CalcPt(Point _pt) :pt(_pt), F(0), G(0), H(0), parent(NULL)  //变量初始化 
  {
  }
};


class AStarCalc
{
private:


  CalcPt* findPath(CalcPt& startPoint, CalcPt& endPoint, bool isIgnoreCorner);
  //计算临近八个点
  vector<CalcPt*> getSurroundPoints(const CalcPt* point, bool isIgnoreCorner) const;
  //判断某点是否可以用于下一步判断 
  bool isCanreach(const CalcPt* point, const CalcPt* target, bool isIgnoreCorner) const;
  //判断开启/关闭列表中是否包含某点
  CalcPt* isInList(const std::list<CalcPt*>& list, const CalcPt* point) const;
  //检测障碍点
  bool isInSites(const Point* point) const;
  //从开启列表中返回F值最小的节点 
  CalcPt* getLeastFpoint();
  //计算FGH值 
  int calcG(CalcPt* temp_start, CalcPt* point);
  int calcH(CalcPt* point, CalcPt* end);
  int calcF(CalcPt* point);


  vector<vector<int>> sites;  //图片点 0-可通行，1-障碍点
  list<CalcPt*> openList;  //开启列表 
  list<CalcPt*> closeList; //关闭列表 


public:
  void InitSites(vector<vector<int>> _sites); //初始化地图
  //获取到路径
  list<CalcPt*> GetPath(CalcPt& startPoint, CalcPt& endPoint, bool isIgnoreCorner = false);
};

AStarCalc.cpp

#include "AStarCalc.h"




CalcPt* AStarCalc::findPath(CalcPt& startPoint, CalcPt& endPoint, bool isIgnoreCorner)
{
  bool isNearEndPoint = false;
  //首先写入起点,拷贝开启一个节点，内外隔离
  CalcPt* firstpt = new CalcPt(Point(startPoint.pt.x, startPoint.pt.y));
  firstpt->H = calcH(firstpt, &endPoint);
  firstpt->F = calcF(firstpt);
  openList.push_back(firstpt);
  while (!openList.empty()) {
    //找到F值最小的点
    auto curPoint = getLeastFpoint();
    //如果计算的点在关闭列表中，说明无路可走了
    if (!isInList(closeList, curPoint)) {
      //找到后从开启列表中删除
      openList.remove(curPoint);
      //存放到关闭列表中
      closeList.push_back(curPoint);
    }
    else
    {
      return curPoint;
    }


    //距离小于100时开启终点检测,
    //用此方法测试计算比原来减少了800毫秒
    if (curPoint->H < 100) isNearEndPoint = true;


    //1.找到当前点周围八个点中可以通过的点
    auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);
    for (auto& target : surroundPoints) {
      //2.对某一个点，如果不在开启列表中，加入到开启列表中，设置当前格为父节点，计算F,G,H的值
      CalcPt* targetpt = isInList(openList, target);
      if (!targetpt) {
        //计算F,G,H的值
        target->G = calcG(curPoint, target);
        target->H = calcH(target, &endPoint);
        target->F = calcF(target);


        target->parent = curPoint;


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


          targetpt->G = tempG;
          targetpt->F = calcF(targetpt);
        }
      }


      //判断快到终点后才开始计算
      if (isNearEndPoint) {
        CalcPt* resPoint = isInList(openList, &endPoint);
        //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝 
        if (resPoint)
          return resPoint;
      }
    }
  }
  return NULL;
}




//计算当前点周围八个点
vector<CalcPt*> AStarCalc::getSurroundPoints(const CalcPt* point, bool isIgnoreCorner) const
{
  vector<CalcPt*> surroundPoints;


  for (int x = point->pt.x - 1; x <= point->pt.x + 1; x++) {
    for (int y = point->pt.y - 1; y <= point->pt.y + 1; y++) {
      if (isCanreach(point, new CalcPt(Point(x, y)), isIgnoreCorner)) {
        surroundPoints.push_back(new CalcPt(Point(x, y)));
      }
    }
  }


  return surroundPoints;
}




//判断是否可能进行规划
bool AStarCalc::isCanreach(const CalcPt* point, const CalcPt* target, bool isIgnoreCorner) const
{
  //坐标小于0直接不计算了
  if (target->pt.x < 0 || target->pt.y < 0
    //计算的点与当前一致也不计算
    || target->pt.x == point->pt.x && target->pt.y == point->pt.y
    //判断点在障碍点中不计算
    || isInSites(&target->pt)
    //如果点在关闭列表中也不计算
    || isInList(closeList, target))
    return false;
  else {
    //如果是直线可以计算
    if (abs(point->pt.x - target->pt.x) + abs(point->pt.y - target->pt.y) == 1)
      return true;
    else {
      //斜线要判断当前直线会否被绊住，也是是说直线中是否在障碍点中
      Point tmppt1 = Point(point->pt.x, target->pt.y);
      Point tmppt2 = Point(target->pt.x, point->pt.y);
      if (!isInSites(&tmppt1) && !isInSites(&tmppt2))
        return true;
      else
        return isIgnoreCorner;
    }
  }
}


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


//判断是否是障碍点
bool AStarCalc::isInSites(const Point* point) const
{
  return sites[point->x][point->y] == 1;
}


//从开启列表中返回F值最小的节点 
CalcPt* AStarCalc::getLeastFpoint()
{
  if (!openList.empty())
  {
    auto resPoint = openList.front();
    for (auto& point : openList)
      if (point->F < resPoint->F)
        resPoint = point;
    return resPoint;
  }
  return NULL;
}


//计算G值
int AStarCalc::calcG(CalcPt* temp_start, CalcPt* point)
{
  //判断当前点与计算的原点是直线还是斜线，获取不同的消耗值
  //上下左右的点移动得到的值肯定是1，不为1的是斜线移动
  int tempG = abs(point->pt.x - temp_start->pt.x) + abs(point->pt.y - temp_start->pt.y) == 1 ? kCost1 : kCost2;
  //判断是不是初始的节点，如果是初始节约，则其父节点为空
  int parentG = point->parent == NULL ? 0 : point->parent->G;
  //两个G相加用于判断是走直线和斜线所消耗的总G
  return parentG + tempG;
}


//计算H值
int AStarCalc::calcH(CalcPt* point, CalcPt* end)
{
  //计算终点到当前点的Point差值
  Point tmppoint = end->pt - point->pt;
  //利用欧几里德距离计算H
  return sqrt(pow(tmppoint.x, 2) + pow(tmppoint.y, 2)) * 10;
}


//计算F值
int AStarCalc::calcF(CalcPt* point)
{
  //公式 F=G+H
  return point->F = point->G + point->H;
}


//初始化地图
void AStarCalc::InitSites(vector<vector<int>> _sites)
{
  sites = _sites;
}




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

项目实现思路

微卡智享

01

地图处理

这里就是OpenCV的简单实现，加入地图图片后

1. 实现灰度图

2. 高斯模糊

3. 图像二值化

4. 形态学梯度操作

02

路径规划

1. 将处理后的图像用二维数组存放起来（像素为白色的做为障碍点，黑色的是可通行点）

2. 调用A*算法，先传入地图

3. 寻找路径

实现效果

上面的A*算法也是我根据实现原理后自己摸索的写出来的，也是因为完成了这个后，让自己发现算法真是的很有趣的东西，同时也更了解到了因为自己就不是学计算机专业的，数学底子也有点差，很多东西也要从基础开始学起了，不过话说回来，兴趣就是最好的老师，所以回到开头说的，打自己的脸先，接下来的目标就是学习基础的数据结构和算法。

完

扫描二维码

获取更多精彩

微卡智享

「 往期文章 」

学习|OpenCV匹配相似轮廓

实践|OpenCV4.2使用DNN进行人脸检测二（视频篇）

实践|OpenCV4.2使用DNN进行人脸检测一（图片篇）