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。

相关文章

随想录 Day 69 并查集 107. 寻找存在的路径

随想录 Day 69 并查集 107. 寻找存在的路径

随想录 Day 69 并查集 107. 寻找存在的路径 理论基础 int n = 1005; // n根据题目中节点数量而定，一般比节点数量大一点就好vector<int> father = vector<int> (n, 0); // C++里的一种数组结构// 并查集初始化void init() {for (int i = 0; i < n; ++i) {father[i] = i;}
阅读更多...
用Microsoft.Extensions.Hosting 管理WPF项目.

用Microsoft.Extensions.Hosting 管理WPF项目.

首先引入必要的包: <ItemGroup><PackageReference Include="CommunityToolkit.Mvvm" Version="8.2.2" /><PackageReference Include="Microsoft.Extensions.Hosting" Version="8.0.0" /><PackageReference Include="Serilog
阅读更多...
C++必修：模版的入门到实践

C++必修：模版的入门到实践

✨✨ 欢迎大家来到贝蒂大讲堂✨✨ 🎈🎈养成好习惯，先赞后看哦~🎈🎈 所属专栏：C++学习 贝蒂的主页：Betty’s blog 1. 泛型编程 首先让我们来思考一个问题，如何实现一个交换函数？ void swap(int& x, int& y){int tmp = x;x = y;y = tmp;} 相信大家很快就能写出上面这段代码，但是如果要求这个交换函数支持字符型
阅读更多...
eclipse运行springboot项目，找不到主类

eclipse运行springboot项目，找不到主类

解决办法尝试了很多种，下载sts压缩包行不通。最后解决办法如图： help--->Eclipse Marketplace--->Popular--->找到Spring Tools 3---->Installed。
阅读更多...
亮相WOT全球技术创新大会，揭秘火山引擎边缘容器技术在泛CDN场景的应用与实践

亮相WOT全球技术创新大会，揭秘火山引擎边缘容器技术在泛CDN场景的应用与实践

2024年6月21日-22日，51CTO“WOT全球技术创新大会2024”在北京举办。火山引擎边缘计算架构师李志明受邀参与，以“边缘容器技术在泛CDN场景的应用和实践”为主题，与多位行业资深专家，共同探讨泛CDN行业技术架构以及云原生与边缘计算的发展和展望。 火山引擎边缘计算架构师李志明表示：为更好地解决传统泛CDN类业务运行中的问题，火山引擎边缘容器团队参考行业做法，结合实践经验，打造火山
阅读更多...
vue项目集成CanvasEditor实现Word在线编辑器

vue项目集成CanvasEditor实现Word在线编辑器

CanvasEditor实现Word在线编辑器 官网文档：https://hufe.club/canvas-editor-docs/guide/schema.html 源码地址：https://github.com/Hufe921/canvas-editor 前提声明： 由于CanvasEditor目前不支持vue、react 等框架开箱即用版，所以需要我们去Git下载源码，拿到其中两个主
阅读更多...
React+TS前台项目实战（十七）-- 全局常用组件Dropdown封装

React+TS前台项目实战（十七）-- 全局常用组件Dropdown封装

文章目录 前言Dropdown组件1. 功能分析2. 代码+详细注释3. 使用方式4. 效果展示 总结 前言 今天这篇主要讲全局Dropdown组件封装，可根据UI设计师要求自定义修改。 Dropdown组件 1. 功能分析 （1）通过position属性,可以控制下拉选项的位置 （2）通过传入width属性, 可以自定义下拉选项的宽度 （3）通过传入classN
阅读更多...
代码随想录算法训练营：12/60

代码随想录算法训练营：12/60

非科班学习算法day12 | LeetCode150:逆波兰表达式 ，Leetcode239: 滑动窗口最大值  目录 介绍 一、基础概念补充： 1.c++字符串转为数字 1. std::stoi, std::stol, std::stoll, std::stoul, std::stoull（最常用） 2. std::stringstream 3. std::atoi, std
阅读更多...
android 带与不带logo的二维码生成

android 带与不带logo的二维码生成

该代码基于ZXing项目，这个网上能下载得到。 定义的控件以及属性： public static final int SCAN_CODE = 1;private ImageView iv;private EditText et;private Button qr_btn,add_logo;private Bitmap logo,bitmap,bmp; //logo图标private st
阅读更多...
人工智能机器学习算法总结神经网络算法(前向及反向传播)

人工智能机器学习算法总结神经网络算法(前向及反向传播)

1.定义，意义和优缺点 定义: 神经网络算法是一种模仿人类大脑神经元之间连接方式的机器学习算法。通过多层神经元的组合和激活函数的非线性转换，神经网络能够学习数据的特征和模式，实现对复杂数据的建模和预测。(我们可以借助人类的神经元模型来更好的帮助我们理解该算法的本质，不过这里需要说明的是，虽然名字是神经网络，并且结构等等也是借鉴了神经网络，但其原型以及算法本质上还和生物层面的神经网络运行原理存在
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2