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。

相关文章

深度解析Java项目中包和包之间的联系

深度解析Java项目中包和包之间的联系

《深度解析Java项目中包和包之间的联系》文章浏览阅读850次，点赞13次，收藏8次。本文详细介绍了Java分层架构中的几个关键包：DTO、Controller、Service和Mapper。_jav... 目录前言一、各大包1.DTO1.1、DTO的核心用途1.2. DTO与实体类(Entity)的区别1
阅读更多...
Java中的雪花算法Snowflake解析与实践技巧

Java中的雪花算法Snowflake解析与实践技巧

《Java中的雪花算法Snowflake解析与实践技巧》本文解析了雪花算法的原理、Java实现及生产实践,涵盖ID结构、位运算技巧、时钟回拨处理、WorkerId分配等关键点,并探讨了百度UidGen... 目录一、雪花算法核心原理1.1 算法起源1.2 ID结构详解1.3 核心特性二、Java实现解析2.
阅读更多...
MySQL 中 ROW_NUMBER() 函数最佳实践

MySQL 中 ROW_NUMBER() 函数最佳实践

《MySQL中ROW_NUMBER()函数最佳实践》MySQL中ROW_NUMBER()函数,作为窗口函数为每行分配唯一连续序号,区别于RANK()和DENSE_RANK(),特别适合分页、去重... 目录mysql 中 ROW_NUMBER() 函数详解一、基础语法二、核心特点三、典型应用场景1. 数据分
阅读更多...
如何在Spring Boot项目中集成MQTT协议

如何在Spring Boot项目中集成MQTT协议

《如何在SpringBoot项目中集成MQTT协议》本文介绍在SpringBoot中集成MQTT的步骤,包括安装Broker、添加EclipsePaho依赖、配置连接参数、实现消息发布订阅、测试接口... 目录1. 准备工作2. 引入依赖3. 配置MQTT连接4. 创建MQTT配置类5. 实现消息发布与订阅
阅读更多...
springboot项目打jar制作成镜像并指定配置文件位置方式

springboot项目打jar制作成镜像并指定配置文件位置方式

《springboot项目打jar制作成镜像并指定配置文件位置方式》：本文主要介绍springboot项目打jar制作成镜像并指定配置文件位置方式,具有很好的参考价值,希望对大家有所帮助,如有错误... 目录一、上传jar到服务器二、编写dockerfile三、新建对应配置文件所存放的数据卷目录四、将配置文
阅读更多...
深度解析Spring AOP @Aspect 原理、实战与最佳实践教程

深度解析Spring AOP @Aspect 原理、实战与最佳实践教程

《深度解析SpringAOP@Aspect原理、实战与最佳实践教程》文章系统讲解了SpringAOP核心概念、实现方式及原理,涵盖横切关注点分离、代理机制（JDK/CGLIB）、切入点类型、性能... 目录1. @ASPect 核心概念1.1 AOP 编程范式1.2 @Aspect 关键特性2. 完整代码实
阅读更多...
怎么用idea创建一个SpringBoot项目

怎么用idea创建一个SpringBoot项目

《怎么用idea创建一个SpringBoot项目》本文介绍了在IDEA中创建SpringBoot项目的步骤,包括环境准备（JDK1.8+、Maven3.2.5+）、使用SpringInitializr... 目录如何在idea中创建一个SpringBoot项目环境准备1.1打开IDEA，点击New新建一个项
阅读更多...
MySQL 用户创建与授权最佳实践

MySQL 用户创建与授权最佳实践

《MySQL用户创建与授权最佳实践》在MySQL中,用户管理和权限控制是数据库安全的重要组成部分,下面详细介绍如何在MySQL中创建用户并授予适当的权限,感兴趣的朋友跟随小编一起看看吧... 目录mysql 用户创建与授权详解一、MySQL用户管理基础1. 用户账户组成2. 查看现有用户二、创建用户1. 基
阅读更多...
Spring Boot 实现 IP 限流的原理、实践与利弊解析

Spring Boot 实现 IP 限流的原理、实践与利弊解析

《SpringBoot实现IP限流的原理、实践与利弊解析》在SpringBoot中实现IP限流是一种简单而有效的方式来保障系统的稳定性和可用性,本文给大家介绍SpringBoot实现IP限... 目录一、引言二、IP 限流原理2.1 令牌桶算法2.2 漏桶算法三、使用场景3.1 防止恶意攻击3.2 控制资源
阅读更多...
springboot项目中整合高德地图的实践

springboot项目中整合高德地图的实践

《springboot项目中整合高德地图的实践》：本文主要介绍springboot项目中整合高德地图的实践,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录一：高德开放平台的使用二：创建数据库（我是用的是mysql）三：Springboot所需的依赖（根据你的需求再
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2