A*算法项目实践之一：栅格法的使用与障碍物栅格的生成

栅格形状、大小的确定
栅格地图的生成——障碍物的生成
测试代码
MATLAB绘制栅格

使用路径搜索算法（本文使用A*算法）的第一步就是将地图用程序中的某些逻辑来表示，本文就笔者实际项目的一些经验来谈谈相关的方法，若有不对，请在评论区提醒笔者予以斧正。
本项目基于VS2017，整个项目的代码以及上传到码云： A*算法项目实践（正在更新中~）

栅格形状、大小的确定

百度百科对栅格数据的定义：栅(shān)格数据就是将空间分割成有规律的网格，每一个网格称为一个单元，并在各单元上赋予相应的属性值来表示实体的一种数据形式。每一个单元（像素）的位置由它的行列号定义，所表示的实体位置隐含在栅格行列位置中，数据组织中的每个数据表示地物或现象的非几何属性或指向其属性的指针。
在实际的路径搜索问题，将搜寻的区域（一般为二维地图，形状不定）分割为有规律的栅格，每个栅格称为一个单元，每个栅格的位置由其行和列决定，这样就简化搜索区域为 2 维数组，数组的每一项代表一个栅格，它的状态就是可走 (walkalbe) 和不可走 (unwalkable)，所谓的可走可以理解为该栅格没有障碍物——算法可拓展到该栅格，不可走可以理解为该栅格有障碍物——算法不能拓展到该栅格
那么如何确定栅格的大小和形状呢？
我们先确定几个原则：（1）很自然的，我们可以想到，用栅格的二维数组来表示搜索的区域，最起码应该能够保证栅格地图的大小与搜寻区域的大小是一致的，若栅格地图比搜索区域小——栅格地图没有正确表示实际搜寻区域，搜索算法可能失去某些可行解；若栅格地图比搜索区域大——栅格地图超出了实际搜索区域，搜索算法可能得到某些错误解，同理，我们也需要用栅格集合表示搜索区域的障碍物和移动的目标物，若整数个栅格表示的区域比障碍物要大，那么我们用了多余的区域表示障碍物——相对减少了实际可行的区域，若整数个栅格表示的区域比障碍物要小，那么我们没有正确表示障碍物——相对减少了搜索出的路径的准确度，综上，原则（1）为力求整数个栅格能够覆盖区域、障碍物和目标物。
（2）当搜索的区域确定时，我们要想到——栅格越小，搜索的区域就需要用更多栅格表示，那么搜索算法的问题空间就越大，计算时间就越长，因此不难得出原则（2）栅格的尺寸应尽可能的大。
原则（1）（2）用一句话可以概括为：
栅格的大小、形状是根据搜寻的区域、障碍物和移动的目标物形状来确定的，力求整数个栅格能够覆盖区域、障碍物和目标物，而且栅格的尺寸应尽可能的大。
举例来说，测试中的搜寻区域形状为矩形，大小为72m * 18.2，障碍物两种：形状都为矩形，大小分别为9.8m * 3.4m、8.2m*3.4，移动的目标物大小、形状与障碍物都相同。
在这种情况下，首先，因为搜寻区域、障碍物和移动的目标物的形状都为矩形，那么可以确定栅格的形状为正方形（规则四边形中最好选择正方形），其次，取72、18.2、9.8、3.4、8.2的最大公约数为0.2，因此可以确定正方形的边长为0.2m。
此外，为了满足原则（1），栅格可以为其它多边形而不是正方形，例如可以是六边形，矩形，甚至可以是任意多变形，但是，笔者认为栅格最好为正方形，因为容易计算与实现。

栅格地图的生成——障碍物的生成

栅格地图的生成需要所有障碍物信息，本项目中的障碍物为车（矩形），且能够得到障碍物的信息（位于项目中的Classes.h文件中）为：

//载具信息类
struct car_info
{PPoint car_center;//中心点坐标double car_length;//载具长double car_width;//载具宽double angle;//载具倾斜角
};

将障碍物对应的栅格置位的思路为：

0、计算车中心点坐标
1、根据长、宽和膨胀边界大小计算膨胀之后四个边界点的坐标；
2、取车边界四个点的栅格行列下标[_x_min,_x_max]、[_y_min,_y_max]范围；
2.1获得坐标的x、y的范围；
2.2转换坐标的x、y的范围得到栅格行列下标[_x_min,_x_max]、[_y_min,_y_max]范围；
2.3整理得到的栅格行列下标[_x_min,_x_max]、[_y_min,_y_max]范围范围，使其不越界；
3、在[_x_min,_x_max]、[_y_min,_y_max]范围之内的每一个栅格，判断其栅格中心点与车中心点之间的线段是否与车的边界线段相交；
4、将所有判断为不在外面的栅格置位；

重点：
问题1：如何求一个障碍物覆盖的栅格——计算出被覆盖的栅格数组？
答：一个栅格被覆盖的情况有3种：（1）完全遮盖；（2）部分遮盖；（3）完全没有遮盖，毫无疑问，（1）类型的栅格需要被标记为1——不可走，（3）类型的栅格需要被标记为0——可走，然而，对于（2）类型的栅格，直观的办法是确定其被覆盖面积大小的占比是多少，若占比超过50%，则将其标记为1，否则标记为0，但是，计算一个正方形被遮盖的面积显而易见计算量较大，因此我们需要转化一下思路。
具体来说，在本项目中，障碍物为矩形，而一个矩形实质上是四条线段，一个栅格（正方形）可以用其中心点的坐标来表示，在解析几何中，线段与线段是否相交很容易判断，那么我们不妨用这种判定方法a：对某一个栅格，若障碍物矩形的中心点与栅格的中心点之间的线段与矩形的四个边不相交，那么该栅格就需要置为1，这种方式的计算量较少且思路非常清晰，但是，我们得着重考虑一下类型（2）的栅格，这部分的栅格可能满足上述判定方式，也有可能满足不了——这与它们被障碍物矩形遮盖的面积有关，但幸运的是，如果我们只是需要将类型（1）、（2）的栅格全部置为1（这一条不一定成立，请读者自行根据项目需求判断），那么问题就转换为了如何求得所有的（1）和（2）类型的栅格。
我们不妨将障碍物矩阵膨胀一下（长、宽各自加上一个值），然后再用上述判定方法a，如果膨胀的够大，理论上所有的（2）类型栅格都将通过判定方法a筛选出来——代价是“无辜”（本不该置为1）的栅格也被置为1；如果这种代价可以承受的话，那么何乐而不为呢？实际笔者使用的膨胀大小为正方形对角线长度的一半。
问题2：怎么减少求一个障碍物覆盖的栅格的计算量？
答：我们不能对搜索区域中的每一个栅格都进行判定，那样计算量很大，只需要对障碍物附近的栅格进行判断，如何确定附近栅格呢？——取障碍物矩形边界四个角的栅格行列范围：[_x_min,_x_max]、[_y_min,_y_max]，即得到行、列的最大最小值，在这2个范围内使用2个for循环来遍历障碍物附近所有的栅格。

对应的功能函数Caculate_SetGridesOfCar（位于项目的Calculate.h和Calculate.c文件中）：

#include "Calculate.h"
#include "Classes.h"
#include <cmath>
using namespace std;
//************************************
// Method:    CalculateCarCenterPoint
// FullName:  CalculateCarCenterPoint
// Access:    public
// Returns:   void
// Qualifier:根据载具长、宽、左后方点、倾斜角度计算载具中心点坐标
// Parameter: double car_length     车长
// Parameter: double car_width      车宽
// Parameter: PPoint & left_back    左后方点坐标
// Parameter: double angle          倾斜角 非弧度角 [0,360)
// Parameter: PPoint & center       计算得到的中心点坐标
//************************************
void CalculateCarCenterPoint(double car_length, double car_width, PPoint &left_back, double angle, PPoint &center)
{//角度为负值，错误输入，返回if (angle < 0) return;//角度转换angle = angle / 180 * 3.1415926;center.x = left_back.x + cos(angle)*car_length / 2 + sin(angle)*car_width / 2;center.y = left_back.y - cos(angle)*car_width / 2 + sin(angle)*car_length / 2;}//************************************
// Method:    CaculateGridesOfCar
// FullName:  CaculateGridesOfCar
// Access:    public
// Returns:   void
// Qualifier:计算障碍物遮盖的栅格并将对应的栅格置位
// Parameter: double car_length     车长
// Parameter: double car_width      车宽
// Parameter: PPoint & left_back    车左后点的直角坐标
// Parameter: double angle          倾斜角度 非弧度角 [0,360)
// Parameter: double expand_boundary膨胀边界
// Parameter: std::vector<std::vector<int>> & maze  栅格地图
//************************************
void Caculate_SetGridesOfCar(double car_length, double car_width, PPoint left_back, double angle, double expand_boundary, std::vector<std::vector<int>>& maze)
{//0、计算车中心点坐标PPoint car_center;CalculateCarCenterPoint(car_length,car_width,left_back,angle,car_center);//1、根据长、宽和膨胀边界大小计算膨胀之后四个边界点的坐标：vector<PPoint> boundary_points(4);CalculateCarBoundaryPoint(car_length + expand_boundary, car_width + expand_boundary, car_center, angle, boundary_points[0], boundary_points[1], boundary_points[2], boundary_points[3]);//2、取车边界四个点的栅格行列下标[_x_min,_x_max]、[_y_min,_y_max]范围//2.1获得坐标的x、y的范围double max_x=0, min_x= DBL_MAX, max_y=0, min_y= DBL_MAX;for (const auto &point : boundary_points){if (point.x > max_x)max_x = point.x;if (point.y > max_y)max_y = point.y;if (point.x < min_x)min_x = point.x;if (point.y < min_y)min_y = point.y;}// 2.2转换坐标的x、y的范围得到栅格行列下标[_x_min,_x_max]、[_y_min,_y_max]范围范围；int _y_min = static_cast<int>(min_x / 0.2);int _y_max = GetIntBig(max_x / 0.2);//向上取整int _x_min = static_cast<int>((91 * 0.2 - max_y) / 0.2);int _x_max = GetIntBig((91 * 0.2 - min_y) / 0.2);//向上取整// 2.3整理得到的栅格行列下标_[_x_min,_x_max]、[_y_min,_y_max]范围范围，使其不越界；_y_min = _y_min <= 0 ? 0 : _y_min;_y_max = _y_max >= maze.front().size() ? maze.front().size() - 1 : _y_max;_x_min = _x_min <= 0 ? 0 : _x_min;_x_max = _x_max >= maze.size() ? maze.size() - 1 : _x_max;//3、在[_x_min,_x_max]、[_y_min,_y_max]范围之内的每一个栅格，判断其栅格中心点与车中心点之间的线段是否与车的边界线段相交for(int i=_x_min;i<=_x_max;++i)for (int j = _y_min; j <= _y_max; ++j){auto y = 91 * 0.2 - ((i + 1)*0.2 - 0.1);//转换过的y坐标auto x = (j + 1)*0.2 - 0.1;//转换过的x坐标PPoint gride_center(x,y);bool is_outside = false;for (int t = 0; t < 4; t++){//与任意一条边相交，说明点在外面,直接判断下一点if (intersect(gride_center, car_center, boundary_points[t], boundary_points[(t + 1) % 4])){is_outside = true;break;}              }//4、将所有判断为不在外面的栅格置位if (is_outside == false)maze[i][j] = 1;}}
//************************************
// Method:    GenerateCarBoundaryPoint
// FullName:  GenerateCarBoundaryPoint
// Access:    public
// Returns:   void
// Qualifier:根据载具中心坐标生成载具四个边界点的坐标
// Parameter: double car_length     载具长
// Parameter: double car_width      载具宽
// Parameter: PPoint & center       中心点
// Parameter: double angle          倾斜角度 非弧度角 [0,360)
// Parameter: PPoint & front_left   左前点
// Parameter: PPoint & back_left    左后点
// Parameter: PPoint & back_right   右后点
// Parameter: PPoint & front_right  右前点
//************************************
void CalculateCarBoundaryPoint(double car_length, double car_width, PPoint &center, double angle, PPoint &front_left, PPoint &back_left, PPoint &back_right, PPoint &front_right)
{//角度为负值，错误输入，返回if (angle < 0) return;//角度转换angle = angle / 180 * 3.1415926;double X1, Y1, X2, Y2, X3, Y3, X4, Y4;//以(x,y)为中心点，不旋转的情况下四个顶点的坐标back_right.x = (center.x - car_length / 2);back_right.y = (center.y - car_width / 2);back_left.x = (center.x - car_length / 2);back_left.y = (center.y + car_width / 2);front_left.x = (center.x + car_length / 2);front_left.y = (center.y + car_width / 2);front_right.x = (center.x + car_length / 2);front_right.y = (center.y - car_width / 2);if (angle <= 0.00001)return;else{//按逆时针旋转角度center.x后的四个点坐标X1 = (back_right.x - center.x) * cos(angle) - (back_right.y - center.y) * sin(angle) + center.x;Y1 = (back_right.y - center.y) * cos(angle) + (back_right.x - center.x) * sin(angle) + center.y;X2 = (back_left.x - center.x) * cos(angle) - (back_left.y - center.y) * sin(angle) + center.x;Y2 = (back_left.y - center.y) * cos(angle) + (back_left.x - center.x) * sin(angle) + center.y;X3 = (front_left.x - center.x) * cos(angle) - (front_left.y - center.y) * sin(angle) + center.x;Y3 = (front_left.y - center.y) * cos(angle) + (front_left.x - center.x) * sin(angle) + center.y;X4 = (front_right.x - center.x) * cos(angle) - (front_right.y - center.y) * sin(angle) + center.x;Y4 = (front_right.y - center.y) * cos(angle) + (front_right.x - center.x) * sin(angle) + center.y;back_right.x = X1;back_right.y = Y1;back_left.x = X2;back_left.y = Y2;front_left.x = X3;front_left.y = Y3;front_right.x = X4;front_right.y = Y4;}
}

测试代码

笔者将A*算法封装为类Astar（位于项目的Astar.h和Astar.c文件），并将栅格地图的生成部分放在了Astar的构造函数中，对应代码为：

//使用外部障碍物信息构造
Astar::Astar(int rows, int cols, const std::vector<car_info>& obstacle_info)
{//生成地图：行 列maze.reserve(rows);vector<int> row(cols, 0);for (int i = 0; i < rows; i++)maze.push_back(row);//遍历障碍物信息，将对应的栅格置为1for( auto& car_info: obstacle_info){Caculate_SetGridesOfCar(car_info.car_length,car_info.car_width,car_info.back_left,car_info.angle,0.284,maze);}
}

注：实际障碍物的位置与角度是随机的，但是测试时在给整个搜寻区域添加障碍物时，将障碍物的位置确定为划4行7列，即一一共有28个障碍物的位置可以放置障碍物，所有位置如下,图中蓝色区域为障碍物：

测试代码为：

int main()
{//障碍物载具信息序列vector<car_info> obstacle_info;obstacle_info.reserve(10);//添加障碍物载具AddCarObastacle(obstacle_info, 3, 3, 10, 1);AddCarObastacle(obstacle_info, 4, 3, 350, 1);AddCarObastacle(obstacle_info, 2, 4, 0, 1);AddCarObastacle(obstacle_info, 2, 5, 20, 1);AddCarObastacle(obstacle_info, 4, 5, 0, 1);AddCarObastacle(obstacle_info, 2, 7, 30, 1);AddCarObastacle(obstacle_info, 3, 7, 0, 1);//A星搜索对象构造Astar astar(91,360, obstacle_info);//输出A星的栅格地图astar.printGraphToCSV();return 0;
}
//************************************
// Method:    AddCarObastacle
// FullName:  AddCarObastacle
// Access:    public
// Returns:   void
// Qualifier:在对应的行和列上添加载具障碍物
// Parameter: vector<car_info> & obstacle
// Parameter: int row_id 行号
// Parameter: int col_id 列号
// Parameter: double angle 倾斜角度，非弧度，以x轴正向为0度，逆时针旋转
// Parameter: int car_type 载具类型
//************************************
void AddCarObastacle(vector<car_info>& obstacle, int row_id, int col_id, double angle, int car_type)
{car_info cur_info;PPoint car_center;car_center = GenerateCarCenterPoint(row_id, col_id, car_type);cur_info.car_center = car_center;cur_info.angle = angle;if (car_type == 1){cur_info.car_length = 9.6;cur_info.car_width = 3.4;}else{cur_info.car_length = 8.2;cur_info.car_width = 3.4;}obstacle.push_back(cur_info);
}

Astar::printGraphToCSV()函数将Astar对象中的栅格二维数组输出到csv文件，具体代码为：
注：这里函数中用到了fprintf，在VS2017中会报错，解决方法为：点击“调试D”——（最下方）项目属性，在活动（debug下）——c/c++——预处理器——预处理定义中添加“_CRT_SECURE_NO_WARNINGS”即可。（实在不行就百度）。

void Astar::printGraphToCSV() const
{FILE* fd = NULL;if ((fd = fopen("Graph_Astar.csv", "wt+")) != NULL){//存储图信息for (auto row : maze){//栅格0 1 反转 为了使用matlab显示for (auto i : row)fprintf(fd, "%d,", 1 - i);fprintf(fd, "\n");}}fclose(fd);fd = NULL;
}

MATLAB绘制栅格

栅格地图是一个0 1 的二维数组，不直观，因此使用MATLAB将其绘制出来，具体matlab的代码为：
这里需要注意的是：c++存储的栅格二维数组用matlab绘制时需要先将栅格二维数组数矩阵上下翻转才行，具体原因留给读者思考一下。

%创建具有障碍物的栅格地图以及将路径栅格序列显示在地图中
%矩阵中0代表黑色栅格
[graph,txt1,raw1]=xlsread('C:\Users\YW\source\repos\PathSearchOfCars\PathSearchOfCars\Graph_Astar.csv') ;
%上下翻转矩阵
a = flipud(graph);b = a;
% disp(a(end,end));
b(end+1,end+1) = 0;
% disp(b);colormap([0 0 1;1 1 1]);  % 创建颜色,地图的颜色为黑白色
%disp(size(a));
%pcolor(1:size(a,2),0.5:size(a,1),b); % 赋予栅格颜色
pcolor(0.5:size(a,2)+0.5,0.5:size(a,1)+0.5,b); % 赋予栅格颜色
%set(gca,'XTick',1:360,'YTick',1:91);  % 设置坐标
axis image xy;  % 沿每个坐标轴使用相同的数据单位，保持一致

测试代码对应的结果图片，蓝色区域为障碍物：

单个障碍物的添加测试，障碍物位于第4行第3列，蓝色区域为障碍物：
逆时针旋转30度：
逆时针旋转120度：

逆时针旋转210度：

逆时针旋转300度：
本博文完，感谢您的阅读，望您不吝点赞之手，再次感谢。

欢迎继续阅读下一博文：A*算法项目实践之二：基于障碍物避碰的栅格路径生成

A*算法项目实践之一：栅格法的使用与障碍物栅格的生成相关推荐

Python从放弃到入门，公众号历史文章爬取成pdf的项目实践与自主学习法
这篇文章不谈江流所专研的营销与运营,而聊一聊技能学习之路,聊一聊Python这门最简单的编程语言该如何学习,我完成的第一个Python项目,将任意公众号的所有历史文章导出成PDF电子书. 或许我这个P ...
栅格法建立环境地图及MATLAB实现
欢迎来到 < Haoh-Smile > 的博客,觉得受用客官就点个赞评论一下呗! 栅格法建立环境地图 1.在AGV规划路径时首先要获取环境信息,建立环境地图,合理的环境表示有利于建立规划方 ...
SVM算法在项目实践中的应用！
↑↑↑关注后"星标"Datawhale 每日干货 & 每月组队学习,不错过 Datawhale干货作者:苏丽敏,Datawhale优秀学习者,北理工计算机硕士支持向量机 ...
【机器学习基础】SVM算法在项目实践中的应用！
作者:苏丽敏,Datawhale优秀学习者,北理工计算机硕士支持向量机(Support Vector Machine)是Cortes和Vapnik于1995年首先提出的,它在解决小样本.非线性及高维 ...
栅格法路径算法C语言,基于地图栅格与QPSO算法结合的机器人路径规划方法与流程...
本发明属于机器人路径规划领域,提出一种基于地图栅格与QPSO结合的机器人路径规划方法. 背景技术: 移动机器人路径规划是寻找一条无碰撞的可行路径问题的方法.近些年,群智能优化算法逐渐成为移动机器人路径 ...
推荐算法项目征集啦！多重奖品助力开源实践创新！
点击左上方蓝字关注我们打开手机软件,首页总能 "想你所想" ,甚至不断拓展你的需求边界.推荐算法从诞生至今,从规则推荐.协同过滤再至提出隐因子模型.Embedding & ...
java栅格法全局路径规划,基于A*的全局路径规划算法（1）
Rate this post 在现实生活中,我们经常需要找到最短路径.例如,当我们想要从一个地点去往另一个地点的时候,我们希望可以在地图中找到最近的一条路.这个时候我们就需要一些特殊的算法来帮助我们解 ...
【四则运算】个人项目实践
题目要求:像<构建之法>的人物阿超那样,花二十分钟写一个能自动生成小学四则运算题目的命令行 "软件", 分别满足下面的各种需求. 下面这些需求都可以用命令行参数的形式来 ...
深度学习、机器学习方向计算机毕业设计题目大全（算法应用实践类）
(吐血整理) 手动整理了1500多个深度学习及机器学习相关算法在实际应用中的项目,完全可以作为本科生当前较新的毕业设计题目选择方向.讲道理有些题目,比如"用户评分的隐式成分信息的研究&quo ...

A*算法项目实践之一：栅格法的使用与障碍物栅格的生成

A*算法项目实践之一：栅格法的使用与障碍物栅格的生成

栅格形状、大小的确定

栅格地图的生成——障碍物的生成

测试代码

MATLAB绘制栅格

A*算法项目实践之一：栅格法的使用与障碍物栅格的生成相关推荐

最新文章

热门文章