A*算法项目实践之二:基于障碍物避碰的栅格路径生成

2024-02-26 20:30

本文主要是介绍A*算法项目实践之二:基于障碍物避碰的栅格路径生成,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

A*算法项目实践之二:基于障碍物避碰的栅格路径生成

  • 实际问题描述
  • A*算法的实现
  • 增加障碍物避碰的必要性
  • 增加障碍物避碰的思路
  • 实现代码
  • 实验结果

在上文—— A算法项目实践之一:栅格法的使用与障碍物栅格的生成中,我们生成了栅格地图,接下来就需要使用A* 算法找寻路径了,本文就笔者实际项目的一些经验来谈谈相关的方法,若有不对,请在评论区提醒笔者予以斧正 。
本项目基于VS2017,整个项目的代码以及上传到码云: A算法项目实践(正在更新中~)

实际问题描述

在这里插入图片描述问题描述:如上图所示,在一个车库中,蓝色区域表示已经停放的车,搜寻的区域划分成了正方形的格子,大小为0.2m*0.2m,简化搜索区域为 2 维数组。一共91行360列,已经停放的车所覆盖的栅格为不可走 (unwalkable)状态,即该车为障碍物,绿色起点表示目标车初始位置的中心点,红色终点表示目标车终点位置的中心点,现需找到一条从起点到达终点的路径,且该路径能保证目标车不会与障碍物相撞
注意:这里栅格的大小、形状是根据搜寻的区域、障碍物和目标物来确定的,力求整数个栅格能够覆盖区域、障碍物和目标物,本例子中0.2是它们尺寸的最大公约数,具体方法见A*算法项目实践之一:栅格法的使用与障碍物栅格的生成。

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*实现,路径寻找:如上图所示,直接使用A*寻路找到了不与障碍物碰撞的路径,但是只是保证路径上没有障碍物而已,路径完全不符合需求!为什么呢?——因为A*将一个格子认为是目标物,没有大小的概念,且A*寻找的是从起点到终点的最短路径,抄捷径而不是走“光明大道”为了给A增加大小的概念,需要在扩展邻接栅格的时候,以该当前栅格为中心,以目标车的方向和大小构建下一步车的位置,并判断下一步该车是否会覆盖到不可走 (unwalkable)状态的栅格。*

增加障碍物避碰的思路

在这里插入图片描述

注意到A*算法的搜索实质上是对当前栅格相邻的8个中的每一个进行判断,若相邻栅格不满足条件1(栅格点与当前节点重合、超出地图、是障碍物、或者在关闭列表中)则不添加该栅格进入开启列表中,否则将添加其进入开启列表中,在这一步中,我们需要再加一个条件2:“以邻接栅格为中心,以目标车的方向和大小构建下一步车的位置,判断下一步该车是否会覆盖到不可走 (unwalkable)状态的栅格——即与障碍物不能碰撞”,若同时满足条件1、2,再将邻接栅格添加进开启列表,否则不添加。
那么如何实现条件2呢?
具体来说,每一个当前栅格的周围栅格一共有8个,可以到达的栅格相对于当前栅格来说方向为上、下、左、右、上左、上右、下左和下右,以判断右栅格是否满足条件2为例(上、下、左的栅格同理,只是将下图旋转角度即可):
在这里插入图片描述

图中黑色方格为当前栅格,绿色方格为当前栅格的右侧栅格,以绿色方格为目标车的中心栅格(目标车在栅格地图上会覆盖大量栅格,其中的中心栅格)构建目标车的位置如图所示,其中:

  1. 边界1、2、3、4表示目标车的边界栅格(这里为一行或一列栅格),边界1对应车头,边界2对应车左,边界3对应车尾,边界4对应车右;
  2. 边界1、2、3、4与绿色栅格距离分别为head_distance、left_distace、heel_distance和right_distance,这四个值与车的长宽以及中心栅格的选取有关,这是用户设置的参数,见下文的测试代码;
  3. 虚线矩形表示膨胀之后的车,膨胀的意思是在原有车矩形的基础上,长宽都增加CollisionRadius——碰撞安全距离
  4. 判断位于绿色栅格时目标车时否会碰撞到障碍物——判断图中虚线矩形内部的栅格的状态是否都为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,如果越界,跳过//边界1for (int i = boundary_2,j=boundary_1; i <= boundary_4; ++i){//载具越界,没有处理if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}//边界2for (int j = boundary_1,i=boundary_2; j >= boundary_3; --j){//载具越界,没有处理if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}//边界3for (int i = boundary_2, j = boundary_3; i <= boundary_4; ++i){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}//边界4for (int j = boundary_1,i=boundary_4; j >= boundary_3; --j){//载具越界,没有处理if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}};break;

通过以上分析和代码可以知道,判断上、下、左、右这4个邻接栅格是否满足条件2比较容易——因为此时覆盖的栅格要么是全部覆盖,要么是完全不遮盖,且四个边界非常好求。
但是,怎么判断上左、上右、下左和下右这4个邻接栅格呢?——绘图吧,以上右栅格为例(上左、下左和下右同理):
在这里插入图片描述

重点:

  1. 此时目标车边界栅格已经不是一行或者一列,而是倾斜的,但是注意到非常特殊的是,边界栅格倾斜的角度为45的奇数倍,依次可用两个for循环来判断某条边界;
  2. 因为栅格是正方形,因此此时目标车矩阵的中轴线、边界线都是沿着栅格的对角线穿过栅格的;
  3. 利用计算几何的思路计算此时虚线矩形的四个边角点p1、p2、p3和p4——每一个边角点需要画2个辅助的三角形计算,又一次非常幸运的是(如果读者画了的话)这2个三角形是等腰直角三角形!
  4. 这个时候增加CollisionRadius——碰撞安全距离需要格外注意,不能再像之前那么增加,需要结合图具体判断——这里面也有很巧妙的发现哦。

对应的实现代码为(位于Astar::CollisionCheck() ):
:(1)代码中对于目标车越界情况没有处理——即虚线矩形覆盖到了栅格地图的外面没有判定此时目标车不满足条件2;(2)只是根据边界栅格是否有栅格为1来判定条件2;(3)代码中的/ sqrt(2)实际上是*sin(45)或cos(45)。

	case 2://右上{//确定车的边界四个点pair<double, double> 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,如果越界,跳过//边界1for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i <= GetIntBig(p4.first); ++i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界2for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i <= GetIntBig(p2.first); ++i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界3for (int i = GetIntBig(p2.first), j = GetIntBig(p2.second); i <= GetIntBig(p3.first); ++i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界4for (int i = GetIntBig(p4.first),j = GetIntBig(p4.second); i <= GetIntBig(p3.first); ++i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}break;

实现代码

实现的代码位于项目中的AStar.h和AStar.c中,笔者将A*算法实现并封装在类AStar中,实现障碍物避碰的核心代码如下:

//************************************
// Method:    getSurroundPoints
// FullName:  Astar::getSurroundPoints
// Access:    private 
// Returns:   std::vector<Point *> 邻接栅格数组
// Qualifier: const 获取当前栅格的邻接栅格
// Parameter: const Point * point	当前栅格
// Parameter: bool isIgnoreCorner	是否忽略边角
// Parameter: bool direction		正向反向A*标志
//************************************
std::vector<Point *> Astar::getSurroundPoints(const Point *point, bool isIgnoreCorner, bool direction) const
{std::vector<Point *> surroundPoints;//修改处//方向左上方为0,按行列顺序增加:0 1 2 3 4 5 6 7 表示 左上 上 右上  左 右 左下 下 右下int direction_type = 0;for (int x = point->x - 1; x <= point->x + 1; x++)for (int y = point->y - 1; y <= point->y + 1; y++){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<double, double> 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,如果越界,跳过//边界1for (int i = GetIntBig(p1.first), j = GetIntBig(p1.second); i >= GetIntBig(p4.first); --i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界2for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i <= GetIntBig(p2.first); ++i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界3for (int i = GetIntBig(p2.first), j = GetIntBig(p2.second); i >= GetIntBig(p3.first); --i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界4for (int i = GetIntBig(p4.first),j = GetIntBig(p4.second); i <= GetIntBig(p3.first); ++i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}break;case 1://{//确定车的边界int boundary_1, boundary_2, boundary_3, boundary_4;//boundary_1 = x - (Car1_length / 2 - 1) - CollisionRadius;boundary_1 = x - head_distance - CollisionRadius;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 < 0 || i >= maze.size() || j < 0 || 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 < 0 || i >= maze.size() || j < 0 || 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 < 0 || i >= maze.size() || j < 0 || 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 < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}	}break;case 2://右上{//确定车的边界四个点pair<double, double> 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,如果越界,跳过//边界1for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i <= GetIntBig(p4.first); ++i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界2for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i <= GetIntBig(p2.first); ++i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界3for (int i = GetIntBig(p2.first), j = GetIntBig(p2.second); i <= GetIntBig(p3.first); ++i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界4for (int i = GetIntBig(p4.first),j = GetIntBig(p4.second); i <= GetIntBig(p3.first); ++i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}break;case 3://{//确定车的边界,边界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 >= boundary_4; --i){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}for (int j = boundary_1,i=boundary_2; j <= boundary_3; ++j){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}for (int i = boundary_2, j = boundary_3; i >= boundary_4; --i){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}for (int j = boundary_1,i=boundary_4; j <= boundary_3; ++j){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}}break;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,如果越界,跳过for (int i = boundary_2,j=boundary_1; i <= boundary_4; ++i){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}for (int j = boundary_1,i=boundary_2; j >= boundary_3; --j){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}for (int i = boundary_2, j = boundary_3; i <= boundary_4; ++i){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}for (int j = boundary_1,i=boundary_4; j >= boundary_3; --j){if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}}break;case 5://左下{//确定车的边界四个点pair<double, double> 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,如果越界,跳过//边界1for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i >= GetIntBig(p4.first); --i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界2for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i >= GetIntBig(p2.first); --i, ++j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界3for (int i = GetIntBig(p2.first), j = GetIntBig(p2.second); i >= GetIntBig(p3.first); --i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界4for (int i = GetIntBig(p4.first),j = GetIntBig(p4.second); i >= GetIntBig(p3.first); --i, ++j)if (i < 0 || i >= maze.size() || j < 0 || 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;//判定各个边界栅格是否有障碍物,是则直接返回truefor (int j = boundary_2,i=boundary_1; j >= boundary_4; --j){if (i < 0 || i >= maze.size() || j < 0 || 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 < 0 || i >= maze.size() || j < 0 || 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 < 0 || i >= maze.size() || j < 0 || 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 < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}}break;case 7://右下{//确定车的边界四个点pair<double, double> 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//边界1for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i <= GetIntBig(p4.first); ++i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界2for (int i = GetIntBig(p1.first),j = GetIntBig(p1.second); i >= GetIntBig(p2.first); --i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界3for (int i = GetIntBig(p2.first), j = GetIntBig(p2.second); i <= GetIntBig(p3.first); ++i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;//边界4for (int i = GetIntBig(p4.first),j = GetIntBig(p4.second); i >= GetIntBig(p3.first); --i, --j)if (i < 0 || i >= maze.size() || j < 0 || j >= maze.front().size());else if (maze[i][j] == 1)return true;}break;}return collision_check;
}

一些细节讨论:
(1) 为何代码中对于目标车越界情况没有处理——即虚线矩形覆盖到了栅格地图的外面没有判定此时目标车不满足条件?
答:本项目中的车库太小了(可搜索区域),当目标车在边界处转向时(算法,一直先向车库边界拓展,然后发现右方有可行栅格就马上右转向拓展),车头很容易越过车库(即撞墙),因此不判断越界情况(具体情况看实验结果)。
(2)为什么代码只是根据边界栅格是否有栅格状态为1来判定条件2?
答:根本原因是算法拓展栅格是按照一个一个栅格来搜索的,因此当与障碍物发生碰撞时,首当其冲,边界栅格肯定会先变为1。

实验结果

c++测试代码:

//障碍物载具信息序列vector<car_info> 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<Point> 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;  % 沿每个坐标轴使用相同的数据单位,保持一致

未加障碍物避碰:
在这里插入图片描述

增加障碍物避碰:
在这里插入图片描述:细节讨论(1)——对于目标车越界情况没有处理,这里在下图中标注处可以观察到车的路线是非常贴近车库边界的,在标注处车头其实会越界,但是这条路径的确是我们想要的——因为这只是栅格路径而已,后续的将会对栅格路劲转换为直角坐标系下的路线时会进行平滑处理,倒是该问题将不复存在。
在这里插入图片描述

其它增加障碍物避碰的效果:

在这里插入图片描述在这里插入图片描述本博文完,感谢您的阅读,望您不吝点赞之手,再次感谢。
欢迎继续阅读下一博文:A算法项目实践之三:优化A的方法——不只是双向A*

这篇关于A*算法项目实践之二:基于障碍物避碰的栅格路径生成的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/750074

相关文章

Oracle查询优化之高效实现仅查询前10条记录的方法与实践

《Oracle查询优化之高效实现仅查询前10条记录的方法与实践》:本文主要介绍Oracle查询优化之高效实现仅查询前10条记录的相关资料,包括使用ROWNUM、ROW_NUMBER()函数、FET... 目录1. 使用 ROWNUM 查询2. 使用 ROW_NUMBER() 函数3. 使用 FETCH FI

在C#中获取端口号与系统信息的高效实践

《在C#中获取端口号与系统信息的高效实践》在现代软件开发中,尤其是系统管理、运维、监控和性能优化等场景中,了解计算机硬件和网络的状态至关重要,C#作为一种广泛应用的编程语言,提供了丰富的API来帮助开... 目录引言1. 获取端口号信息1.1 获取活动的 TCP 和 UDP 连接说明:应用场景:2. 获取硬

MybatisGenerator文件生成不出对应文件的问题

《MybatisGenerator文件生成不出对应文件的问题》本文介绍了使用MybatisGenerator生成文件时遇到的问题及解决方法,主要步骤包括检查目标表是否存在、是否能连接到数据库、配置生成... 目录MyBATisGenerator 文件生成不出对应文件先在项目结构里引入“targetProje

Java内存泄漏问题的排查、优化与最佳实践

《Java内存泄漏问题的排查、优化与最佳实践》在Java开发中,内存泄漏是一个常见且令人头疼的问题,内存泄漏指的是程序在运行过程中,已经不再使用的对象没有被及时释放,从而导致内存占用不断增加,最终... 目录引言1. 什么是内存泄漏?常见的内存泄漏情况2. 如何排查 Java 中的内存泄漏?2.1 使用 J

Python使用qrcode库实现生成二维码的操作指南

《Python使用qrcode库实现生成二维码的操作指南》二维码是一种广泛使用的二维条码,因其高效的数据存储能力和易于扫描的特点,广泛应用于支付、身份验证、营销推广等领域,Pythonqrcode库是... 目录一、安装 python qrcode 库二、基本使用方法1. 生成简单二维码2. 生成带 Log

Python中的随机森林算法与实战

《Python中的随机森林算法与实战》本文详细介绍了随机森林算法,包括其原理、实现步骤、分类和回归案例,并讨论了其优点和缺点,通过面向对象编程实现了一个简单的随机森林模型,并应用于鸢尾花分类和波士顿房... 目录1、随机森林算法概述2、随机森林的原理3、实现步骤4、分类案例:使用随机森林预测鸢尾花品种4.1

Python 中 requests 与 aiohttp 在实际项目中的选择策略详解

《Python中requests与aiohttp在实际项目中的选择策略详解》本文主要介绍了Python爬虫开发中常用的两个库requests和aiohttp的使用方法及其区别,通过实际项目案... 目录一、requests 库二、aiohttp 库三、requests 和 aiohttp 的比较四、requ

SpringBoot项目启动后自动加载系统配置的多种实现方式

《SpringBoot项目启动后自动加载系统配置的多种实现方式》:本文主要介绍SpringBoot项目启动后自动加载系统配置的多种实现方式,并通过代码示例讲解的非常详细,对大家的学习或工作有一定的... 目录1. 使用 CommandLineRunner实现方式:2. 使用 ApplicationRunne

使用IntelliJ IDEA创建简单的Java Web项目完整步骤

《使用IntelliJIDEA创建简单的JavaWeb项目完整步骤》:本文主要介绍如何使用IntelliJIDEA创建一个简单的JavaWeb项目,实现登录、注册和查看用户列表功能,使用Se... 目录前置准备项目功能实现步骤1. 创建项目2. 配置 Tomcat3. 项目文件结构4. 创建数据库和表5.

Python项目打包部署到服务器的实现

《Python项目打包部署到服务器的实现》本文主要介绍了PyCharm和Ubuntu服务器部署Python项目,包括打包、上传、安装和设置自启动服务的步骤,具有一定的参考价值,感兴趣的可以了解一下... 目录一、准备工作二、项目打包三、部署到服务器四、设置服务自启动一、准备工作开发环境:本文以PyChar