A*算法项目实践之二：基于障碍物避碰的栅格路径生成 |
您所在的位置：网站首页 › 地图路径生成方法 › A*算法项目实践之二：基于障碍物避碰的栅格路径生成 |
A*算法项目实践之二：基于障碍物避碰的栅格路径生成
|
A*算法项目实践之二：基于障碍物避碰的栅格路径生成
实际问题描述A*算法的实现增加障碍物避碰的必要性增加障碍物避碰的思路实现代码实验结果
在上文——
A算法项目实践之一：栅格法的使用与障碍物栅格的生成中，我们生成了栅格地图，接下来就需要使用A* 算法找寻路径了，本文就笔者实际项目的一些经验来谈谈相关的方法，若有不对，请在评论区提醒笔者予以斧正 。
本项目基于VS2017，整个项目的代码以及上传到码云：
A算法项目实践（正在更新中~）
实际问题描述
首先，需要编码实现传统的A*算法，笔者参考了以下博客（非常优秀的博文）： （1）A算法（C++实现） （2）A算法详解（讲的一级棒 ） 得到了传统A*的算法思路如下： 把起始格添加到 "开启列表" cur_grid初始化为起始格 do { 将cur_grid设为当前格. 把它切换到关闭列表. 对当前格相邻的8格中的每一个 if (它不可通过 || 已经在 "关闭列表" 中) { 什么也不做. } if (它不在开启列表中) { 把它添加进 "开启列表", 把当前格作为这一格的父节点, 并根据终点格，计算这一格的 FGH } if (它已经在开启列表中) { if (用 G 值为参考检查新的路径是否更好, 更低的G值意味着更好的路径) { 把这一格的父节点改成当前格, 并且重新计算这一格的 GF 值. } } . if(终点已经在 "开启列表" ) 找到路径; break; 将cur_grid重置为开启列表的F值最小的栅格 } while( 开启列表不为空) 如果开启列表已经空了, 说明路径不存在. 最后从cur_grid沿父节点回溯到起点，得到最终路径.本博文的重点不在于A*的思路，但是下面的内容要求读者对A*算法有所掌握。 增加障碍物避碰的必要性
注意到A*算法的搜索实质上是对当前栅格相邻的8个中的每一个进行判断，若相邻栅格不满足条件1（栅格点与当前节点重合、超出地图、是障碍物、或者在关闭列表中）则不添加该栅格进入开启列表中，否则将添加其进入开启列表中，在这一步中，我们需要再加一个条件2：“以邻接栅格为中心，以目标车的方向和大小构建下一步车的位置，判断下一步该车是否会覆盖到不可走 (unwalkable)状态的栅格——即与障碍物不能碰撞”，若同时满足条件1、2，再将邻接栅格添加进开启列表，否则不添加。 那么如何实现条件2呢？ 具体来说，每一个当前栅格的周围栅格一共有8个，可以到达的栅格相对于当前栅格来说方向为上、下、左、右、上左、上右、下左和下右，以判断右栅格是否满足条件2为例（上、下、左的栅格同理，只是将下图旋转角度即可）： 图中黑色方格为当前栅格，绿色方格为当前栅格的右侧栅格，以绿色方格为目标车的中心栅格（目标车在栅格地图上会覆盖大量栅格，其中的中心栅格）构建目标车的位置如图所示，其中： 边界1、2、3、4表示目标车的边界栅格（这里为一行或一列栅格），边界1对应车头，边界2对应车左，边界3对应车尾，边界4对应车右；边界1、2、3、4与绿色栅格距离分别为head_distance、left_distace、heel_distance和right_distance，这四个值与车的长宽以及中心栅格的选取有关，这是用户设置的参数，见下文的测试代码；虚线矩形表示膨胀之后的车，膨胀的意思是在原有车矩形的基础上，长宽都增加CollisionRadius——碰撞安全距离判断位于绿色栅格时目标车时否会碰撞到障碍物——判断图中虚线矩形内部的栅格的状态是否都为1，即可走状态。图中对应的代码为（位于Astar::CollisionCheck() ）： 注：（1）代码中对于目标车越界情况没有处理——即虚线矩形覆盖到了栅格地图的外面没有判定此时目标车不满足条件2；（2）只是根据边界栅格是否有栅格为1来判定条件2；这些会在下文进行讨论。 case 4://右 { //确定车的边界,边界1-4为逆时针顺序 int boundary_1, boundary_2, boundary_3, boundary_4; boundary_1 = y + CollisionRadius + head_distance; boundary_2 = x - left_distace - CollisionRadius; boundary_3 = y - heel_distance- CollisionRadius; boundary_4 = x + right_distance + CollisionRadius; //判定各个边界栅格是否有障碍物，是则直接返回true，如果越界，跳过 //边界1 for (int i = boundary_2,j=boundary_1; i //载具越界，没有处理 if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } //边界3 for (int i = boundary_2, j = boundary_3; i //载具越界，没有处理 if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } };break;通过以上分析和代码可以知道，判断上、下、左、右这4个邻接栅格是否满足条件2比较容易——因为此时覆盖的栅格要么是全部覆盖，要么是完全不遮盖，且四个边界非常好求。 但是，怎么判断上左、上右、下左和下右这4个邻接栅格呢？——绘图吧，以上右栅格为例（上左、下左和下右同理）： 重点： 此时目标车边界栅格已经不是一行或者一列，而是倾斜的，但是注意到非常特殊的是，边界栅格倾斜的角度为45的奇数倍，依次可用两个for循环来判断某条边界；因为栅格是正方形，因此此时目标车矩阵的中轴线、边界线都是沿着栅格的对角线穿过栅格的；利用计算几何的思路计算此时虚线矩形的四个边角点p1、p2、p3和p4——每一个边角点需要画2个辅助的三角形计算，又一次非常幸运的是（如果读者画了的话）这2个三角形是等腰直角三角形！这个时候增加CollisionRadius——碰撞安全距离需要格外注意，不能再像之前那么增加，需要结合图具体判断——这里面也有很巧妙的发现哦。对应的实现代码为（位于Astar::CollisionCheck() ）： 注：（1）代码中对于目标车越界情况没有处理——即虚线矩形覆盖到了栅格地图的外面没有判定此时目标车不满足条件2；（2）只是根据边界栅格是否有栅格为1来判定条件2；（3）代码中的/ sqrt(2)实际上是*sin(45)或cos(45)。 case 2://右上 { //确定车的边界四个点 pair p1, p2, p3, p4; p1.first = x - head_distance / sqrt(2) - left_distace / sqrt(2) - CollisionRadius; p1.second = y + head_distance / sqrt(2) - left_distace / sqrt(2); p2.first = x + heel_distance / sqrt(2) - left_distace / sqrt(2); p2.second = y- heel_distance / sqrt(2) - left_distace / sqrt(2) - CollisionRadius; p3.first = x + heel_distance / sqrt(2) + right_distance / sqrt(2) + CollisionRadius; p3.second = y - heel_distance / sqrt(2) + right_distance / sqrt(2); p4.first = x - head_distance / sqrt(2) + right_distance / sqrt(2); p4.second = y + head_distance / sqrt(2) + right_distance / sqrt(2) + CollisionRadius; //使用for循环遍历外围边界，这里需要将边界的四个点向上取整 //判定各个边界栅格是否有障碍物，是则直接返回true，如果越界，跳过 //边界1 for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i if (x != point->x || y != point->y) { //判断条件1、2是否满足 if (isCanreach(point, new Point(x , y), isIgnoreCorner,direction) && !CollisionCheck(x, y, direction_type)) surroundPoints.push_back(new Point(x, y)); direction_type++; } } return surroundPoints; } //************************************ // Method: CollisionCheck // FullName: Astar::CollisionCheck // Access: private // Returns: bool // Qualifier: const 根据当前栅格、车类型和方向判断是否发生碰撞，如果是则返回true，否则返回false // Parameter: int x 栅格x坐标 // Parameter: int y 栅格y坐标 // Parameter: int direction 方向左上方为0，按行列顺序增加：0 1 2 3 4 5 6 7 表示 左上 上 右上 左 右 左下 下 右下 //************************************ bool Astar::CollisionCheck(int x, int y,int direction) const { //修改：将车作为一个正方形，只是以头部边界不碰撞来检测，且头部边界距离中心点为车宽，因此不区分车的类型； bool collision_check = false; //方向左上方为0，按行列顺序增加：0 1 2 3 4 5 6 7 表示 左上 上 右上 左 右 左下 下 右下 //根据方向来判断是否发生碰撞 switch (direction) { case 0://左上 { //确定车的边界四个点 pair p1,p2,p3,p4; p1.first = x - head_distance / sqrt(2) + left_distace / sqrt(2) ; p1.second = y - head_distance / sqrt(2) - left_distace / sqrt(2)- CollisionRadius; p2.first = x + heel_distance / sqrt(2) + left_distace / sqrt(2)+CollisionRadius; p2.second = y + heel_distance / sqrt(2) - left_distace / sqrt(2); p3.first = x + heel_distance / sqrt(2) - right_distance / sqrt(2); p3.second = y + heel_distance / sqrt(2) + right_distance / sqrt(2)+ CollisionRadius; p4.first = x - head_distance / sqrt(2) - right_distance / sqrt(2)- CollisionRadius; p4.second = y - head_distance / sqrt(2) + right_distance / sqrt(2); //使用for循环遍历外围边界，这里需要将边界的四个点向上取整 //判定各个边界栅格是否有障碍物，是则直接返回true，如果越界，跳过 //边界1 for (int i = GetIntBig(p1.first), j = GetIntBig(p1.second); i >= GetIntBig(p4.first); --i, ++j) if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; //边界2 for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i = GetIntBig(p3.first); --i, ++j) if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; //边界4 for (int i = GetIntBig(p4.first),j = GetIntBig(p4.second); i if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } for (int i = boundary_1,j=boundary_2; i if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } for (int i = boundary_1,j=boundary_4; i //确定车的边界四个点 pair p1, p2, p3, p4; p1.first = x - head_distance / sqrt(2) - left_distace / sqrt(2) - CollisionRadius; p1.second = y + head_distance / sqrt(2) - left_distace / sqrt(2); p2.first = x + heel_distance / sqrt(2) - left_distace / sqrt(2); p2.second = y- heel_distance / sqrt(2) - left_distace / sqrt(2) - CollisionRadius; p3.first = x + heel_distance / sqrt(2) + right_distance / sqrt(2) + CollisionRadius; p3.second = y - heel_distance / sqrt(2) + right_distance / sqrt(2); p4.first = x - head_distance / sqrt(2) + right_distance / sqrt(2); p4.second = y + head_distance / sqrt(2) + right_distance / sqrt(2) + CollisionRadius; //使用for循环遍历外围边界，这里需要将边界的四个点向上取整 //判定各个边界栅格是否有障碍物，是则直接返回true，如果越界，跳过 //边界1 for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } for (int j = boundary_1,i=boundary_2; j if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } for (int j = boundary_1,i=boundary_4; j //确定车的边界,边界1-4为逆时针顺序 int boundary_1, boundary_2, boundary_3, boundary_4; boundary_1 = y + CollisionRadius + head_distance; boundary_2 = x - left_distace - CollisionRadius; boundary_3 = y - heel_distance- CollisionRadius; boundary_4 = x + right_distance + CollisionRadius; //判定各个边界栅格是否有障碍物，是则直接返回true，如果越界，跳过 for (int i = boundary_2,j=boundary_1; i if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } for (int i = boundary_2, j = boundary_3; i if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } }break; case 5://左下 { //确定车的边界四个点 pair p1, p2, p3, p4; p1.first = x + head_distance / sqrt(2) + left_distace / sqrt(2) + CollisionRadius ; p1.second = y - head_distance / sqrt(2) + left_distace / sqrt(2); p2.first = x - heel_distance / sqrt(2) + left_distace / sqrt(2); p2.second = y + heel_distance / sqrt(2) + left_distace / sqrt(2) + CollisionRadius; p3.first = x - heel_distance / sqrt(2) - right_distance / sqrt(2) - CollisionRadius; p3.second = y + heel_distance / sqrt(2) - right_distance / sqrt(2); p4.first = x + head_distance / sqrt(2) - right_distance / sqrt(2); p4.second = y - head_distance / sqrt(2) - right_distance / sqrt(2) - CollisionRadius; //使用for循环遍历外围边界，这里需要将边界的四个点向上取整 //判定各个边界栅格是否有障碍物，是则直接返回true，如果越界，跳过 //边界1 for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i >= GetIntBig(p4.first); --i, --j) if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; //边界2 for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i >= GetIntBig(p2.first); --i, ++j) if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; //边界3 for (int i = GetIntBig(p2.first), j = GetIntBig(p2.second); i >= GetIntBig(p3.first); --i, --j) if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; //边界4 for (int i = GetIntBig(p4.first),j = GetIntBig(p4.second); i >= GetIntBig(p3.first); --i, ++j) if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; }break; case 6://下 { //确定车的边界,边界1-4为逆时针顺序 int boundary_1, boundary_2, boundary_3, boundary_4; boundary_1 = x + CollisionRadius + head_distance; boundary_2 = y + left_distace + CollisionRadius; boundary_3 = x - heel_distance - CollisionRadius; boundary_4 = y - right_distance - CollisionRadius; //判定各个边界栅格是否有障碍物，是则直接返回true for (int j = boundary_2,i=boundary_1; j >= boundary_4; --j) { if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } for (int i = boundary_1,j=boundary_2; i >= boundary_3; --i) { if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } for (int j = boundary_2, i = boundary_3; j >= boundary_4; --j) { if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } for (int i = boundary_1,j=boundary_4; i >= boundary_3; --i) { if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; } }break; case 7://右下 { //确定车的边界四个点 pair p1, p2, p3, p4; p1.first = x + head_distance / sqrt(2) - left_distace / sqrt(2) ; p1.second = y + head_distance / sqrt(2) + left_distace / sqrt(2) + CollisionRadius ; p2.first = x - heel_distance / sqrt(2) - left_distace / sqrt(2) - CollisionRadius; p2.second = y - heel_distance / sqrt(2) + left_distace / sqrt(2); p3.first = x - heel_distance / sqrt(2) + right_distance / sqrt(2) ; p3.second = y - heel_distance / sqrt(2) - right_distance / sqrt(2) - CollisionRadius; p4.first = x + head_distance / sqrt(2) + right_distance / sqrt(2) + CollisionRadius; p4.second = y + head_distance / sqrt(2) - right_distance / sqrt(2); //使用for循环遍历外围边界，这里需要将边界的四个点向上取整 //判定各个边界栅格是否有障碍物，是则直接返回false //边界1 for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i = GetIntBig(p2.first); --i, --j) if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; //边界3 for (int i = GetIntBig(p2.first), j = GetIntBig(p2.second); i = GetIntBig(p3.first); --i, --j) if (i = maze.size() || j = maze.front().size()) ; else if (maze[i][j] == 1) return true; }break; } return collision_check; }一些细节讨论： （1） 为何代码中对于目标车越界情况没有处理——即虚线矩形覆盖到了栅格地图的外面没有判定此时目标车不满足条件？ 答：本项目中的车库太小了（可搜索区域），当目标车在边界处转向时（算法，一直先向车库边界拓展，然后发现右方有可行栅格就马上右转向拓展），车头很容易越过车库（即撞墙），因此不判断越界情况（具体情况看实验结果）。 （2）为什么代码只是根据边界栅格是否有栅格状态为1来判定条件2？ 答：根本原因是算法拓展栅格是按照一个一个栅格来搜索的，因此当与障碍物发生碰撞时，首当其冲，边界栅格肯定会先变为1。 实验结果c++测试代码： //障碍物载具信息序列 vector obstacle_info; obstacle_info.reserve(10); //添加障碍物载具 AddCarObastacle(obstacle_info, 3, 3, 0, 1); //AddCarObastacle(obstacle_info, 2, 3, 0, 1); AddCarObastacle(obstacle_info, 4, 3, 350, 1); AddCarObastacle(obstacle_info, 2, 4, 0, 1); AddCarObastacle(obstacle_info, 2, 5, 0, 1); AddCarObastacle(obstacle_info, 4, 5, 0, 1); AddCarObastacle(obstacle_info, 2, 7, 30, 1); AddCarObastacle(obstacle_info, 3, 7, 0, 1); //初始化A星算法对象 //91表示栅格地图的行数，360表示栅格地图的列数，obstacle_info表示障碍物信息（这里是存储载具的左后角坐标、类型（决定载具的长宽）和倾斜角度） Astar astar(91,360, obstacle_info); //输出astar的栅格地图到csv文件 astar.printGraphToCSV(); //设置中心点栅格与边界栅格的距离 astar.SetCarInfo(23, 24, 8, 8); //设置与障碍物的安全栅格距离 astar.SetCollisionRadius(2); //设置起点与终点 Point end = GetEndPointByRow_Col(3, 5, 1); Point start(45, 30); //A星算法对象找寻路径 list path; path = astar.GetPath(start, end, false); //将栅格地图中，路径中的栅格置为1并输出到csv文件中，供matlab代码绘制图像 astar.printPathToGraphInCSV(path);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; % 沿每个坐标轴使用相同的数据单位，保持一致未加障碍物避碰： 增加障碍物避碰： 其它增加障碍物避碰的效果：
|
问题描述：如上图所示，在一个车库中，蓝色区域表示已经停放的车，搜寻的区域划分成了正方形的格子，大小为0.2m*0.2m，简化搜索区域为 2 维数组。一共91行360列，已经停放的车所覆盖的栅格为不可走 (unwalkable)状态，即该车为障碍物，绿色起点表示目标车初始位置的中心点，红色终点表示目标车终点位置的中心点，现需找到一条从起点到达终点的路径，且该路径能保证目标车不会与障碍物相撞。 注意：这里栅格的大小、形状是根据搜寻的区域、障碍物和目标物来确定的，力求整数个栅格能够覆盖区域、障碍物和目标物，本例子中0.2是它们尺寸的最大公约数，具体方法见A*算法项目实践之一：栅格法的使用与障碍物栅格的生成。
A*实现，路径寻找：如上图所示，直接使用A*寻路找到了不与障碍物碰撞的路径，但是只是保证路径上没有障碍物而已，路径完全不符合需求！为什么呢？——因为A*将一个格子认为是目标物，没有大小的概念，且A*寻找的是从起点到终点的最短路径，抄捷径而不是走“光明大道”。为了给A增加大小的概念，需要在扩展邻接栅格的时候，以该当前栅格为中心，以目标车的方向和大小构建下一步车的位置，并判断下一步该车是否会覆盖到不可走 (unwalkable)状态的栅格。*
注：细节讨论（1）——对于目标车越界情况没有处理，这里在下图中标注处可以观察到车的路线是非常贴近车库边界的，在标注处车头其实会越界，但是这条路径的确是我们想要的——因为这只是栅格路径而已，后续的将会对栅格路劲转换为直角坐标系下的路线时会进行平滑处理，倒是该问题将不复存在。 
本博文完，感谢您的阅读，望您不吝点赞之手，再次感谢。 欢迎继续阅读下一博文：A算法项目实践之三：优化A的方法——不只是双向A*【本文地址】